[發(fā)明專利]一種無人機(jī)路徑規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201811627334.6
• 申請(qǐng)日: 2018-12-28
• 公開（公告）號(hào): CN109582038B
• 公開（公告）日: 2021-08-27
• 發(fā)明（設(shè)計(jì)）人: 趙小川;劉培志;胡雄文;徐凱;宋剛;劉華鵬
• 申請(qǐng)（專利權(quán)）人: 中國(guó)兵器工業(yè)計(jì)算機(jī)應(yīng)用技術(shù)研究所
• 主分類號(hào): G05D1/10
• 分類號(hào): G05D1/10;G05D1/08
• 代理公司: 北京君泊知識(shí)產(chǎn)權(quán)代理有限公司 11496
• 代理人: 王程遠(yuǎn);胡玉章

本發(fā)明公開了一種無人機(jī)路徑規(guī)劃方法，包括：步驟1，設(shè)定目標(biāo)點(diǎn)1，計(jì)算無人機(jī)到目標(biāo)點(diǎn)1的目標(biāo)偏航角θ，使無人機(jī)沿指定目標(biāo)偏航角θ向前運(yùn)動(dòng)；步驟2，判斷前方有無障礙物：若有則無人機(jī)避障后沿當(dāng)前方向前行；若無則判斷實(shí)際偏航角與目標(biāo)偏航角θ的角度差；步驟3，判斷是否達(dá)到目標(biāo)點(diǎn)1：若未達(dá)目標(biāo)點(diǎn)1，則回步驟1；否則到步驟4；步驟4，利用激光識(shí)別門后判斷是否達(dá)到目標(biāo)點(diǎn)2：若到達(dá)則無人機(jī)向正前方前行；否則判斷是否識(shí)別到門：若識(shí)別到門，則重復(fù)前述步驟直到到達(dá)目標(biāo)點(diǎn)2；否則在當(dāng)前置位搜索門直至識(shí)別到門。本發(fā)明的有益效果：在航線上探測(cè)到障礙物后重新規(guī)劃新航線，并避開障礙物區(qū)域。

技術(shù)領(lǐng)域

本發(fā)明涉及無人機(jī)導(dǎo)航技術(shù)領(lǐng)域，具體而言，涉及一種無人機(jī)路徑規(guī)劃方法。

背景技術(shù)

目前，在無人機(jī)技術(shù)領(lǐng)域，多半采用的是全球衛(wèi)星導(dǎo)航系統(tǒng)，只能實(shí)現(xiàn)在室外開闊區(qū)域的導(dǎo)航及路徑規(guī)劃，完成室外自主導(dǎo)航，進(jìn)而完成一些簡(jiǎn)單的飛行任務(wù)。而在室內(nèi)環(huán)境中，在遇到突發(fā)威脅、隨遇目標(biāo)等情況下，無法實(shí)時(shí)進(jìn)行航線調(diào)整，尤其在雜亂的建筑物內(nèi)和障礙遍布的環(huán)境下無法實(shí)現(xiàn)自主導(dǎo)航飛行，進(jìn)而無法實(shí)現(xiàn)高級(jí)別的飛行任務(wù)。

發(fā)明內(nèi)容

為解決上述問題，本發(fā)明的目的在于提供一種無人機(jī)路徑規(guī)劃方法，能在遇到突發(fā)威脅、隨遇目標(biāo)等情況下實(shí)時(shí)航路重規(guī)劃方法，航線上探測(cè)到障礙物，重新規(guī)劃新航線，避開障礙物區(qū)域。

本發(fā)明提供了一種無人機(jī)路徑規(guī)劃方法，包括：

步驟1，無人機(jī)啟動(dòng)，設(shè)定目標(biāo)點(diǎn)1，計(jì)算無人機(jī)到目標(biāo)點(diǎn)1的目標(biāo)偏航角θ，調(diào)整實(shí)際偏航角使無人機(jī)沿指定的目標(biāo)偏航角θ后向前運(yùn)動(dòng)；

步驟2，判斷前方有無障礙物：

若有障礙物，無人機(jī)避障后沿當(dāng)前方向前行T秒；

若無障礙物，判斷實(shí)際偏航角與目標(biāo)偏航角θ的角度差，當(dāng)角度差a°時(shí)，無人機(jī)沿當(dāng)前方向前行T秒，當(dāng)角度差不滿足a°時(shí)，調(diào)整實(shí)際偏航角使角度差a°，無人機(jī)沿當(dāng)前方向前行T秒；

步驟3，判斷是否達(dá)到目標(biāo)點(diǎn)1：

若未達(dá)目標(biāo)點(diǎn)1，則回到步驟1；

若到達(dá)目標(biāo)點(diǎn)1，則進(jìn)入步驟4；

步驟4，利用激光識(shí)別門后判斷是否達(dá)到目標(biāo)點(diǎn)2：

若到達(dá)目標(biāo)點(diǎn)2，無人機(jī)向正前方繼續(xù)前行N米，無人機(jī)停止運(yùn)動(dòng)；

若未到達(dá)目標(biāo)點(diǎn)2，判斷是否識(shí)別到門：若識(shí)別到門，則返回步驟1重復(fù)前述步驟直到無人機(jī)到達(dá)目標(biāo)點(diǎn)2；若未識(shí)別到門，則在當(dāng)前置位搜索門直至識(shí)別到門。

作為本發(fā)明進(jìn)一步的改進(jìn)，在判斷有無障礙物時(shí)，利用二維激光束照射到障礙物后的返回值判斷其方位，然后通過控制指令驅(qū)動(dòng)無人機(jī)調(diào)整偏航角從而完成障礙，具體方法如下：

根據(jù)二維激光束照射在障礙物上返回的信息，得到照射點(diǎn)的極坐標(biāo)(θ,r)，其中，r表示激光中心點(diǎn)到照射點(diǎn)的距離，θ表示激光束與Y軸的夾角，定義θ向左為正，θ∈[-35°,35°]，d＝2m表示安全距離；

當(dāng)r＞d時(shí)，無人機(jī)前方無障礙物，無人機(jī)向前運(yùn)動(dòng)；

當(dāng)r≤d時(shí)，根據(jù)θ值的大小控制無人機(jī)避障：若θ＞0，無人機(jī)向左轉(zhuǎn)動(dòng)直到避開障礙物；若θ≤0，無人機(jī)向右轉(zhuǎn)動(dòng)直到避開障礙物。

作為本發(fā)明進(jìn)一步的改進(jìn)，在識(shí)別門時(shí)，根據(jù)二維激光束的返回信息，判斷并識(shí)別門的邊緣后計(jì)算出門寬并與設(shè)定條件做匹配，通過成功匹配的門邊緣信息計(jì)算出門的中心點(diǎn)位置信息，具體方法如下：

當(dāng)激光照射到門的邊緣時(shí)，記照射點(diǎn)A1為門的左邊緣，表示激光信號(hào)上升沿狀態(tài)到下降沿狀態(tài)；記照射點(diǎn)A2為門的右邊緣，表示激光信號(hào)下降沿狀態(tài)到上升沿狀態(tài)；