【技術實現步驟摘要】
本專利技術屬于無人機無人車空地協同系統領域，具體涉及一種兼具避障功能的空地協同軌跡優化算法。
技術介紹
1、現代智能
中，空地協同系統的應用前景日益廣闊，特別是在災害應急響應、環境監測、城市發展規劃以及安全巡邏等方面。無人機(uav)和無人車(ugv)技術的迅猛進步，更多技術可以被用于二者協同作戰，發揮一加一大于二的功效。為了實現各個智能體的自主導航，地圖感知與動態、非結構環境下的路徑規劃方法十分重要。
2、近些年關于機器人自主導航的相關研究中，主要集中于單一機器人的環境感知與自主導航，但是將環境感知與自主導航都集中在單個無人車上，可能會造成環境信息的缺失，對后續機器人的導航與路徑規劃造成影響，而且將傳感器都集中于無人機或無人車，會導致其分配于運輸或其他任務的載重減少；部分多機器人協助系統，如一些集中式空地協同系統，通過基站來協調不同智能體之間的工作，但如果基站遭到干擾，則會對系統的穩定性產生影響。本專利技術將感知環境與自主導航模塊分別放在無人機與無人車上，減少了二者的負載，分布式協調方案也可以減輕在收到干擾后對系統穩定性的影響。
技術實現思路
1、本專利技術的目的在于提供一種兼具避障功能的空地協調軌跡優化算法。
2、實現本專利技術目的的技術解決方案為：一種兼具避障功能的空地協同軌跡優化算法，無人機配備激光雷達和相對位姿估計傳感器、且搭載aloam算法、dbscan聚類算法，無人車配備車輪里程計和相對位姿估計傳感器、搭載mpc-d-cbf算法、kuhn
3、步驟(1)：無人機通過激光雷達接收點云信息，通過dbscan聚類算法對點云信息進行預處理；通過aloam算法計算得到無人機自身的里程計數據，通過raycast算法計算更新局部的占據柵格地圖；
4、步驟(2)：根據無人機傳來的里程計數據，及相對位姿估計傳感器數據，無人車計算得到相對位姿，通過無人車搭載的擴展卡爾曼濾波器得到無人車的精確位姿；同時，通過mbe算法計算每組點云簇的最小邊界橢圓，通過擴展卡爾曼濾波器得到的精確位姿估計無人車對應的最小邊界橢圓并剔除；
5、步驟(3)：將步驟(1)無人機的占據柵格地圖發送給無人車，無人車還原地圖，使用搭載的ego-planner規劃器，在占據柵格地圖上進行路徑規劃，得到無人車的參考路徑；
6、步驟(4)：無人車接受步驟(2)的最小邊界橢圓信息，通過kuhn-munkres算法對前后兩幀橢圓進行匹配，根據步驟(3)規劃的參考路徑，通過mpc-d-cbf算法進行路徑跟蹤與優化，得到最終的無人車無碰撞路徑。
7、進一步的，步驟(1)中“通過dbscan聚類算法對點云信息進行預處理”具體為：
8、設置dbscan聚類算法的參數領域大小eps和形成簇所需最少的點數量minpts，dbscan從輸入的每一幀的點云集中的任意點p開始，從p檢索所有密度可達的點，如果p是一個核心點、即p的領域內點數量大于minpts，則形成一個簇；否則，將該點標記為噪聲點，繼續遍歷下一個點，直到遍歷完所有點，整合得到一組n個簇aobs＝{o1，o2，...，on}與若干噪聲點。
9、進一步的，步驟(2)中的“通過無人車搭載的擴展卡爾曼濾波器得到無人車的精確位姿”具體為：
10、在預測階段，擴展卡爾曼濾波器ekf利用無人車車輪里程計提供的角速度和線速度作為控制輸入；初始化協方差矩陣r和高斯噪聲矩陣q，并計算先驗協方差矩陣pk|k-1和位姿信息預測值；
11、在更新階段，擴展卡爾曼濾波器ekf融合無人機里程計數據和相對位姿估計傳感器數據，構建包含位姿信息測量值的測量矩陣z；
12、通過計算狀態轉移矩陣k，擴展卡爾曼濾波器ekf結合測量值和預測值之間的誤差，最終得到經過濾波的后驗狀態估計和協方差矩陣pk|k；
13、迭代持續計算無人車的精確位姿。
14、進一步的，步驟(2)中的“通過mbe算法計算每組點云簇的最小邊界橢圓，通過擴展卡爾曼濾波器得到的精確位姿估計無人車對應的最小邊界橢圓并剔除”具體為：
15、對于步驟(1)每一組即每一幀點云信息的n個簇，分別對每個簇計算最小邊界橢圓：對于給定的點集、即一個簇，遍歷簇中的點，將選中的點p加入到初始為空的點集p中，計算除點p以外點的最小邊界橢圓di，如果點p在最小邊界橢圓di的邊界上，那么最小邊界橢圓di也是包括了p在內的最小邊界橢圓，將點p添加到邊界集r中；如果不是，通過調用遞歸邊界函數b_minidisk更新最小邊界橢圓di以包圍點p；具體表達式如下：
16、
17、遞歸邊界函數b_minidisk中的遞歸關系為：
18、
19、其中，t(n)是處理n個點的期望步驟數，c是計算最小圓所需的常數時間；
20、當點集中的點數量減少到了0，或者邊界集r中的點數量達到了(d+3)d/2時，即滿足遍歷的終止條件，此時的di即為最小邊界橢圓d；
21、計算一組點云信息所有簇對應的最小邊界橢圓d的集合；
22、結合擴展卡爾曼濾波器得到的無人車精確位姿，剔除無人車對應的最小邊界橢圓。
23、進一步的，步驟(3)具體包括如下步驟：
24、步驟(31)：生成一條只滿足終端約束、不考慮避障的b樣條曲線ф、即軌跡；
25、步驟(32)：迭代每個碰撞段，使用a*算法生成一條無碰撞路徑г；
26、步驟(33)：對于軌跡ф上每個控制點qi的切向量ri，過控制點qi切向量ri的垂直平面ψ與無碰撞路徑г相交形成線i，線i與障礙物表面j相交于點pi，j，同時獲得斥力方向向量vi，j，v是q指向點pi，j的單位向量；
27、步驟(34)：根據(p,v)對定義距離場di，j進行計算：
28、di，j＝(qi-pi，j)·vi，j
29、如果di，j大于0，則表示點pi，j不在障礙物內，路徑規劃成功。
30、進一步的，步驟(4)中的“通過kuhn-munkres算法對前后兩幀橢圓進行匹配”具體為：
31、針對步驟(2)中前后兩幀獲得的兩組最小邊界橢圓，使用kuhn–munkres算法，將前后兩幀最小邊界橢圓的數據轉化為狀態向量，求解對應的分配矩陣rij，對于給定正整數的n×n矩陣r＝(rij)，找到變換j1，...，jn在整數1，...，n使最大；具體如下：
32、argmin?u1+...+un+v1+...+νn
33、ui+νj≧rij
34、其中u1，...，un與v1，...，vn為非負整數，找到對應的匹配關系后，對對應的邊界橢圓編上編號，對前后兩幀的邊界橢圓進行匹配。
35、進一步的，步驟(4)中“通過mpc-d-cbf算法進行路徑跟蹤與優化，得到最終本文檔來自技高網...
【技術保護點】
1.一種兼具避障功能的空地協同軌跡優化算法，其特征在于，無人機配備激光雷達和相對位姿估計傳感器、且搭載ALOAM算法、DBSCAN聚類算法，無人車配備車輪里程計和相對位姿估計傳感器、搭載MPC-D-CBF算法、Kuhn-Munkres算法、擴展卡爾曼濾波器、ego-planner規劃器；算法包括如下步驟：
2.根據權利要求1所述的算法，其特征在于，步驟(1)中“通過DBSCAN聚類算法對點云信息進行預處理”具體為：
3.根據權利要求2所述的算法，其特征在于，步驟(2)中的“通過無人車搭載的擴展卡爾曼濾波器得到無人車的精確位姿”具體為：
4.根據權利要求3所述的算法，其特征在于，步驟(2)中的“通過MBE算法計算每組點云簇的最小邊界橢圓，通過擴展卡爾曼濾波器得到的精確位姿估計無人車對應的最小邊界橢圓并剔除”具體為：
5.根據權利要求4所述的算法，其特征在于，步驟(3)具體包括如下步驟：
6.根據權利要求5所述的算法，其特征在于，步驟(4)中的“通過Kuhn-Munkres算法對前后兩幀橢圓進行匹配”具體為：
7
8.根據權利要求7所述的算法，其特征在于，步驟(4)中“通過MPC-D-CBF算法進行路徑跟蹤與優化，得到最終的無人車無碰撞路徑”具體步驟如下：
...【技術特征摘要】
1.一種兼具避障功能的空地協同軌跡優化算法，其特征在于，無人機配備激光雷達和相對位姿估計傳感器、且搭載aloam算法、dbscan聚類算法，無人車配備車輪里程計和相對位姿估計傳感器、搭載mpc-d-cbf算法、kuhn-munkres算法、擴展卡爾曼濾波器、ego-planner規劃器；算法包括如下步驟：
2.根據權利要求1所述的算法，其特征在于，步驟(1)中“通過dbscan聚類算法對點云信息進行預處理”具體為：
3.根據權利要求2所述的算法，其特征在于，步驟(2)中的“通過無人車搭載的擴展卡爾曼濾波器得到無人車的精確位姿”具體為：
4.根據權利要求3所述的算法，其特征在于，步驟(2)中的“通過mb...
還沒有人留言評論。發表了對其他瀏覽者有用的留言會獲得科技券。