[發(fā)明專利]一種地鐵隧道內(nèi)四旋翼飛行器自主導(dǎo)航方法

有效

• 申請?zhí)枺?/td>: 202010764270.5
• 申請日: 2020-08-02
• 公開（公告）號: CN112000125B
• 公開（公告）日: 2022-09-27
• 發(fā)明（設(shè)計）人: 周燕;蔡晨曉;鄭標(biāo);梁立東;歐凱;唐元軍;李仕游
• 申請（專利權(quán)）人: 南京理工大學(xué);廣州地鐵集團(tuán)有限公司
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 南京理工大學(xué)專利中心 32203
• 代理人: 陳鵬

1.一種地鐵隧道內(nèi)四旋翼飛行器自主導(dǎo)航方法，其特征在于，包括如下步驟：

步驟1，利用激光雷達(dá)及HectorSLAM算法構(gòu)建離線的隧道地圖，并對四旋翼進(jìn)行實時定位；

步驟2，通過引入回歸濾波機制及主干約束的改進(jìn)快速探索隨機樹基于離線地圖實現(xiàn)航跡規(guī)劃；具體如下：

首先對RRT*引入回歸濾波機制，該機制設(shè)計了參數(shù)：最小探索區(qū)域半徑ε_threshold，當(dāng)新生長節(jié)點x_new與隨機樹T上最近節(jié)點x_nearest1的距離小于ε_threshold，則認(rèn)為該節(jié)點位于已探索區(qū)域，屬于過度搜索節(jié)點，對其進(jìn)行回歸處理不予生長，通過設(shè)定不同ε_threshold值可調(diào)整隨機樹節(jié)點的分布密度；

其次，對RRT*引入主干約束，設(shè)定路徑起點到終點為主干方向，并限制搜索空間為主干的周圍區(qū)域，壓制無效的采樣搜索空間，具體為：

定義地圖坐標(biāo)系為O(X，Y)，路徑起點為S(x₀，y₀)，路徑終點為E(x₁，y₁)，其中，即為主干方向；定義縮小后搜索空間的坐標(biāo)系為O’(X’，Y’)，該坐標(biāo)系以點S為原點，以為X’軸，Y’軸是平面上經(jīng)過原點并與X’軸垂直的任一向量；坐標(biāo)系O(X，Y)到O’(X’，Y’)的變換矩陣分為平移變換矩陣

及旋轉(zhuǎn)變換矩陣

其中，α為O(X，Y)旋轉(zhuǎn)至O’(X’，Y’)的夾角，順勢針為正，逆時針為負(fù)；由此，可將O(X，Y)中一點(x，y)^T通過變換矩陣H、R轉(zhuǎn)換至O’(X’，Y’)中一點(x’，y’)^T；

通過限制(x’，y’)^T橫坐標(biāo)與縱坐標(biāo)范圍，即可限制搜索空間范圍；

步驟3，利用三次B樣條曲線優(yōu)化離線航跡，包括平滑處理和曲率約束；其具體過程如下：ReT-RRT*算法所規(guī)劃的航跡由若干直線段連接而成，采用三次B樣條曲線對其進(jìn)行擬合平滑處理；基于三次B樣條的局部可重建特性，通過控制點調(diào)整策略進(jìn)行曲率約束，所述控制點調(diào)整策略為：若某點曲率值超出上限值，則對其所屬曲線段對應(yīng)的控制點進(jìn)行位置調(diào)整，限制曲率；

步驟4，根據(jù)離線規(guī)劃的航跡點及四旋翼實時位置設(shè)計PID位置控制器求解引力加速度，計算方法為：

e(k)＝||q(k)-p_refer(k)||

其中，ξ為引力系數(shù)，K_p、K_i、K_d為PID控制系數(shù)，T_s為采樣控制周期，q為四旋翼的當(dāng)前位置，p_refer為參考航跡點，為引力加速度的作用方向，為單位矢量；

步驟5，根據(jù)人工勢場法求解障礙物的斥力加速度；

步驟6，計算引力及斥力加速度的矢量和控制四旋翼的姿態(tài)角偏轉(zhuǎn)及位置，使四旋翼完成起始點至終點的自主導(dǎo)航飛行。

2.根據(jù)權(quán)利要求1所述的地鐵隧道內(nèi)四旋翼飛行器自主導(dǎo)航方法，其特征在于，步驟1中，建圖及定位傳感器采用激光雷達(dá)HOKUYO UTM-30LX。

3.根據(jù)權(quán)利要求1所述的地鐵隧道內(nèi)四旋翼飛行器自主導(dǎo)航方法，其特征在于，步驟5中，根據(jù)人工勢場法求解障礙物的斥力加速度，計算方法為：

其中，η為斥力系數(shù)，ρ_obs(q)為四旋翼到障礙物的最短距離，d為障礙物的作用范圍閾值，為斥力加速度的作用方向，為單位矢量，始終垂直于四旋翼前進(jìn)方向，且背離障礙物。

4.根據(jù)權(quán)利要求1所述的地鐵隧道內(nèi)四旋翼飛行器自主導(dǎo)航方法，其特征在于，步驟6中，計算引力及斥力加速度的矢量和作為參考姿態(tài)角以控制四旋翼的位姿偏移，使其完成自主導(dǎo)航飛行。