本發(fā)明屬于無人機(jī)避障技術(shù)領(lǐng)域，特別涉及一種基于最大內(nèi)切圓圓心移動(dòng)矢量的無人機(jī)自動(dòng)避障方法。

背景技術(shù)：

多旋翼無人機(jī)具有體積小、重量輕、開發(fā)成本低、安全性好等優(yōu)點(diǎn)，得到了許多機(jī)構(gòu)和公司企業(yè)的廣泛青睞。目前，多旋翼無人機(jī)被廣泛應(yīng)用在安防巡邏、電力巡視、植保、快遞運(yùn)送、災(zāi)害搜救等作業(yè)中。但是無人機(jī)在手動(dòng)或者自動(dòng)巡邏的過程中，隨時(shí)會(huì)遇到障礙物，為了保證無人機(jī)和人員的安全，自主避障系統(tǒng)顯得尤為重要。

目前主要采用的避障傳感器為超聲波或者視覺傳感器，容易受環(huán)境的影響。相對(duì)于超聲波和視覺，雷達(dá)可靠性和探測(cè)范圍比較高，并且可實(shí)現(xiàn)360°檢測(cè)障礙。人工勢(shì)場(chǎng)法為目前最常用的局部路徑規(guī)劃算法，該算法將障礙物當(dāng)作質(zhì)點(diǎn)，忽略其環(huán)境信息，當(dāng)環(huán)境較為復(fù)雜時(shí)，計(jì)算復(fù)雜度增加。

技術(shù)實(shí)現(xiàn)要素：

為了解決上述問題，本發(fā)明提供了一種基于最大內(nèi)切圓圓心移動(dòng)矢量的無人機(jī)自動(dòng)避障方法，該方法能夠快速實(shí)現(xiàn)無人機(jī)的自主避障，滿足無人機(jī)控制的實(shí)時(shí)性要求。

實(shí)現(xiàn)本發(fā)明的技術(shù)方案如下：

一種基于最大內(nèi)切圓圓心移動(dòng)矢量的無人機(jī)自動(dòng)避障方法，具體過程為：

以無人機(jī)所處位置為原點(diǎn)，獲取當(dāng)前時(shí)刻無人機(jī)所處的無故障區(qū)域的內(nèi)切圓的圓心；

根據(jù)內(nèi)切圓圓心位置，計(jì)算無人機(jī)斥力方向；

根據(jù)圓心移動(dòng)距離和斥力方向計(jì)算避障控制量，無人機(jī)飛控系統(tǒng)根據(jù)所述控制量進(jìn)行控制。

進(jìn)一步地，本發(fā)明所述內(nèi)切圓的圓心的獲取過程為：

步驟一：無人機(jī)上雷達(dá)掃描無人機(jī)防御區(qū)域，獲得區(qū)域中障礙物的角度和距離數(shù)據(jù)；

步驟二：將雷達(dá)獲取的數(shù)據(jù)轉(zhuǎn)換到柵格坐標(biāo)系下，獲得障礙物柵格地圖；

步驟三：對(duì)障礙物柵格地圖進(jìn)行膨脹-腐蝕-膨脹處理，提取無人機(jī)周圍無障礙物區(qū)域；

步驟四：計(jì)算所述無故障區(qū)域最大內(nèi)切圓圓心。

進(jìn)一步地，本發(fā)明所述步驟四的具體過程為：

(1)依據(jù)所提取區(qū)域邊緣，計(jì)算初始圓心o′(x0,y0)；

xj,yj為所提取區(qū)域邊緣數(shù)據(jù)點(diǎn)坐標(biāo)

(2)尋找區(qū)域邊緣上距離初始圓心o′最近的點(diǎn)ps1，做內(nèi)切圓；以o′和點(diǎn)ps1連線，遠(yuǎn)離ps1方向移動(dòng)圓心，直至內(nèi)切圓和區(qū)域邊緣上第二個(gè)點(diǎn)ps2相切；

(3)令內(nèi)切圓的圓心沿著點(diǎn)ps1、ps2連線的垂直平分線的兩邊分別移動(dòng)，尋找內(nèi)切圓與區(qū)域邊緣的接觸點(diǎn)，選取圓心移動(dòng)距離較大的一邊所對(duì)應(yīng)的接觸點(diǎn)為點(diǎn)ps3；

(4)將點(diǎn)ps1、ps2、ps3三點(diǎn)所確定的圓的圓心作為區(qū)域內(nèi)最大內(nèi)切圓圓心(xi,yi)。

進(jìn)一步地，本發(fā)明所述根據(jù)內(nèi)切圓圓心位置，計(jì)算斥力方向；

斥力方向?yàn)?/p>

進(jìn)一步地，本發(fā)明根據(jù)所述斥力方向和圓心移動(dòng)距離計(jì)算出避障控制量為：

x，y方向控制量u(x)，u(y)：

u(x)＝δu(l)*cosθ

u(y)＝δu(l)*sinθ

δu(l)＝kp(err(l)-err(l-1))+kierr(l)+kd(err(l-1)-err(l-2))

其中，err(l)為第l次圓心移動(dòng)距離，l為變量，kp為比例系數(shù)，ki為積分系數(shù)，kd為微分系數(shù)。

有益效果

本發(fā)明提供了一種高效、快速的無人機(jī)避障方法，包括局部地圖的創(chuàng)建和基于最大內(nèi)切圓圓心移動(dòng)矢量的路徑規(guī)劃，該路徑規(guī)劃方法通過雷達(dá)建圖，可描述360°的局部環(huán)境，將所建地圖中的所有障礙物的作用用最大內(nèi)切圓圓心移動(dòng)矢量來表示，可得到精確的避障控制矢量。該方法可完成快速檢測(cè)障礙和自主避障，計(jì)算速度滿足無人機(jī)控制的實(shí)時(shí)性要求。

附圖說明

圖1為本發(fā)明避障流程圖；

圖2為本發(fā)明避障系統(tǒng)框圖；

圖3為求解最大內(nèi)切圓迭代過程。

具體實(shí)施方式

下面結(jié)合附圖和具體實(shí)例對(duì)本發(fā)明進(jìn)行詳細(xì)說明。

本發(fā)明的設(shè)計(jì)思路為：首先利用二維激光雷達(dá)所測(cè)角度和距離數(shù)據(jù)建立以無人機(jī)為中心的局部環(huán)境信息，并將此時(shí)的環(huán)境信息用柵格圖表示；根據(jù)無人機(jī)和水平障礙物的具體位置關(guān)系，使用基于最大內(nèi)切圓圓心移動(dòng)矢量的路徑規(guī)劃算法進(jìn)行自主避障。

本發(fā)明一種基于最大內(nèi)切圓圓心移動(dòng)矢量的無人機(jī)自動(dòng)避障方法，具體過程為：

步驟一：無人機(jī)上雷達(dá)掃描無人機(jī)防御區(qū)域，獲取區(qū)域中故障物的角度和距離數(shù)據(jù)，具體包括：

雷達(dá)測(cè)出的是距離和角度數(shù)據(jù)，為了避免距離數(shù)據(jù)發(fā)生跳變而產(chǎn)生的誤操作，采用中值濾波對(duì)距離數(shù)據(jù)進(jìn)行處理，處理過的數(shù)據(jù)可以用來地圖構(gòu)建。

步驟二：將雷達(dá)數(shù)據(jù)(ρ,θ)(ρ為角度，θ所對(duì)應(yīng)的距離)，轉(zhuǎn)換到柵格坐標(biāo)系(xg,yg)(xg,yg為整數(shù))，具體包括：

s21.建立柵格度量信息

取單個(gè)柵格大小為g*g(g為柵格相對(duì)于實(shí)際空間大小的度量值)。然后以無人機(jī)中心為原點(diǎn)，設(shè)定局部防御區(qū)域大小為2w*2w(每個(gè)柵格初始化為0)，則：-w<x<w,-w<y<w。其中0表示自由柵格，1表示障礙柵格。

s22.將激光雷達(dá)數(shù)據(jù)轉(zhuǎn)換成柵格坐標(biāo)

相當(dāng)于把激光雷達(dá)數(shù)據(jù)先轉(zhuǎn)換到機(jī)體坐標(biāo)系(x,y)，然后把該數(shù)據(jù)在機(jī)體坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到柵格坐標(biāo)系(xg,yg)。轉(zhuǎn)換公式如下：

其中，ceil為取整函數(shù)。

s23.將在無人機(jī)防御區(qū)域的障礙點(diǎn)所對(duì)應(yīng)的柵格的f(i,j)值置1，其余柵格的值置0。

步驟三：對(duì)障礙柵格地圖進(jìn)行膨脹-腐蝕-膨脹操作，提取無人機(jī)周圍無故障區(qū)域的邊緣；

障礙物的膨脹運(yùn)算首先可以使障礙物與無人機(jī)行駛路徑之間有一定的緩沖距離，這段距離可以讓無人機(jī)與障礙物保持在安全的防撞區(qū)域內(nèi)；另一方面，也可以使一些沒有連續(xù)采集到的同一個(gè)障礙物的信息隨著膨脹的過程在地圖上變成連接的一體障礙，提高了離散數(shù)據(jù)在柵格地圖上對(duì)障礙物的表達(dá)準(zhǔn)確度。障礙物的腐蝕運(yùn)算，可以避免對(duì)障礙的誤判。

s31.對(duì)當(dāng)前柵格地圖進(jìn)行膨脹-腐蝕-膨脹處理

假設(shè)輸入的柵格地圖為f(i,j)，輸出的柵格地圖為g(i,j)，其中i，j為柵格坐標(biāo)，則膨脹和腐蝕過程可以表述為：

膨脹：

腐蝕：

s32.邊緣提取

對(duì)于二值柵格圖,只需要判斷柵格值是否為1并且4領(lǐng)域內(nèi)是否同時(shí)存在0和1。如果存在這樣的情況，那么鄰域中間點(diǎn)就是邊緣點(diǎn)，否則邊緣點(diǎn)就是無人機(jī)防御區(qū)域邊緣，得到無人機(jī)防御區(qū)域內(nèi)不包含障礙物的區(qū)域。

步驟四：針對(duì)步驟三中提取出的區(qū)域，計(jì)算其最大內(nèi)切圓圓心

面對(duì)復(fù)雜的環(huán)境情況時(shí)，人工勢(shì)場(chǎng)中斥力場(chǎng)的計(jì)算會(huì)比較復(fù)雜。本發(fā)明基于人工勢(shì)場(chǎng)法，本申請(qǐng)基于圓心移動(dòng)矢量獲取無人機(jī)的控制輸入，實(shí)現(xiàn)路徑規(guī)劃。

將雷達(dá)所處的位置(即無人機(jī)防御區(qū)域的中心)定義為原點(diǎn)(0,0)，針對(duì)步驟三中所提取出的區(qū)域，尋找區(qū)域內(nèi)的最大內(nèi)切圓，并求出圓心坐標(biāo)(xi,yi)。

其中，最大內(nèi)切圓圓心求解步驟如下：

(1)計(jì)算最小二乘圓心為初始圓心

xj,yj為所提取區(qū)域邊緣數(shù)據(jù)點(diǎn)坐標(biāo)

(2)以輪廓上距離初始圓心最近的點(diǎn)ps1半徑為ρ(ps1,o')做圓，以o'和ps1連線，遠(yuǎn)離ps1方向移動(dòng)圓心，直至內(nèi)切圓和輪廓上第二個(gè)點(diǎn)ps2相切，見附圖3。

(3)獲得兩接觸點(diǎn)ps1、ps2后，沿兩接觸點(diǎn)連線的垂直平分線向兩邊尋找第三個(gè)接觸點(diǎn)ps3。圓心的移動(dòng)距離分別為t1，t2，取較大的距離對(duì)應(yīng)的點(diǎn)為ps3。根據(jù)三個(gè)點(diǎn)坐標(biāo)：ps1、ps2、ps3，帶入圓公式

(x-xi)²+(y-yi)²＝r²

求得圓心坐標(biāo)(xi,yi)和半徑r。

步驟五：根據(jù)最大內(nèi)切圓圓心，計(jì)算斥力方向和斥力大小；

斥力方向?yàn)椋?/p>

斥力大小為：

η為比例系數(shù)。

步驟六：計(jì)算出避障控制量傳給無人機(jī)的飛控系統(tǒng)實(shí)現(xiàn)對(duì)無人機(jī)的控制利用傳統(tǒng)pid算法，離散化增量式pid表示公式如下：

δu(l)＝kp(err(l)-err(l-1))+kierr(l)+kd(err(l-1)-err(l-2))

其中，err(l)為第l次圓心移動(dòng)距離，l為變量，kp為比例系數(shù)，ki為積分系數(shù)，kd為微分系數(shù)，分別計(jì)算出x和y方向控制量u(x),u(y):斥力的大小和err()大小有關(guān)，計(jì)算出δu(l)，然后根據(jù)斥力方向θ和δu(l)，計(jì)算出x，y方向控制量。

u(x)＝δu(l)*cosθ

u(y)＝δu(l)*sinθ

利用x和y方向控制量u(x),u(y)對(duì)無人機(jī)進(jìn)行控制。

綜上所述，以上僅為本發(fā)明的較佳實(shí)施例而已，并非用于限定本發(fā)明的保護(hù)范圍。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。