多傳感器數據融合下機器人路徑分段規劃仿真

王紅梅，陳 冬

(1.山西工程技術學院大數據與信息工程科學系，山西 陽泉 045000；2.山西大同大學數學與計算機科學學院 山西 大同 037009)

1 引言

隨著軍事、工業、航空航天領域對使用機器人的需求日益增多，大力研發機器人技術[1]成為了熱門課題。因機器人憑借較好的靈活性與獨立性等優勢，還被廣泛應用于服務業、醫療業、日常生活等各個領域，使人類在生產生活等諸多方面都得到極大程度的改善與革新。為滿足當今的應用需求，機器人既要能夠很好地適應環境，還需要有理想的智能控制系統，而機器人的路徑規劃作為該研究方向的一個關鍵課題，受到眾多相關學者的深入探討。

比如，文獻[2]張杰等人在研究水下群機器人路徑規劃算法形式化的過程中，構建出一種基于定理證明器HOL4的遺傳算法形式化模型，該模型具有合理性與有效性；李靖等人[3]在灰狼優化算法中引入Logistic混沌映射進行優化，將其與執行大任務量監測任務的機器人相結合，實現大規模任務量的有效路徑規劃；李娟等人[4]針對蟻群系統的種群多樣性與收斂速度問題，利用動態混沌算子構建動態混沌蟻群系統，應用于機器人路徑規劃中，加快規劃效率。

影響機器人路徑規劃效果因素較多，上述文獻方法極易造成時間與空間信息上的缺失與冗余，因此，本文引用多傳感器數據融合技術，提出一種機器人路徑分段規劃方法。從多級別、多層次、多方面處理多個傳感器數據的技術就叫做多傳感器信息融合技術[5]，其應用領域有工業機器人、金融系統、船舶避碰、空中交通管制、軍事戰略等。該項技術可有效補充缺失信息，優化冗余數據，使環境得到極大程度認知，強化傳感器性能，既提升機器人路徑規劃的適用范圍，也降低傳感器元件生成的累積誤差，且在機器人移動定位、障礙物躲避、路徑規劃等重要環節中得到了理想的仿真效果；通過平滑處理規劃路徑，消除因路徑生成較多折線與拐點而產生的鋸齒效應，解決機器人碰撞問題；利用轉彎策略，抑制機器人本體寬度影響，防止其突然變向、發生碰撞；在簡化運算方面，分別采取了做正切圓、將機器人車輪寬度算入整體寬度中等策略。

2 機器人路徑狀態變量估計值分析

為提升機器人路徑規劃的適用范圍，降低傳感器元件生成的累積誤差[6]，融合機器人超聲波測距儀等多個傳感器的測量數據。

(1)

假設機器人在[k，k+1]時段內未發生軌跡，測距量i的權重為wi，則利用加權最小二乘法[8]構建下列表達式

(2)

(3)

其中

(4)

(5)

(6)

(7)

(8)

(9)

式中，θ為機器人正前方方向與順時針旋轉90°方向所呈夾角，b為機器人履帶寬度，ΔSl、ΔSr分別為左右兩條履帶的移動距離。

(10)

(11)

得到機器人運行軌道偏方差為

S′(k+1)=H(k+1)P(k+1|k)H(k+1)T+R(k+1)

(12)

(13)

P(k+1|k+1)=P(k+1|k)-

K(k+1)S′(k+1)K(k+1)-1

(14)

K(k+1)=p(k+1|k)H(k+1)TS′(k+1)-1

(15)

通過更新機器人運行路徑狀態，結合方差，對路徑數據進行卡爾曼濾波[9]，得到濾波結果表達式為

(16)

(17)

(18)

給定輸入量與測量值的協方差數據信息后，通過融合多傳感器數據，得到機器人不同采樣時刻的狀態，獲取其路徑規劃結果。

3 機器人路徑分段規劃

機器人在需要躲避路徑障礙物時，會存在較大路徑曲率的路段，應先減速慢行，確保平穩過彎，待經過該路段需加速移動一段距離，之后再勻速通行。因此，以路徑軌跡曲率極值點[11]為依據，分段處理各段路徑上的機器人移動速度。

3.1 平滑處理

為消除因路徑生成較多折線與拐點而產生的鋸齒效應，解決機器人碰撞問題，需平滑處理規劃路徑。根據規劃路徑各分段點，連接當前分段點與下一個相鄰分段點，逐段展開平滑處理至全路徑結束。具體執行流程描述如下：

1)根據多傳感器數據融合過程中得到的機器人定位點，分析最終路徑規劃結果上各分段點；

2)假設當前分段點是下一個路徑起始點，若存在當前分段點的子分段點，則進入下一流程；反之，則停止平滑處理；

3)遍歷起始點與下一個當前分段點，若獲取的返回值是真值，則進入下一流程；反之，則跳至第(5)步；

4)用下一個當前分段點賦值當前分段點，與此同時，去除之前的各當前分段點，返回第(2)步；

5)用當前分段點賦值起始點，用下一個當前分段點賦值當前分段點，返回第(2)步；

6)終止路徑平滑處理。

平滑處理工程中，需按照固定間隔進行遍歷、采樣，依序處理各分段路徑。

3.2 轉彎策略

為抑制機器人本體寬度影響，防止突然變向、發生碰撞，設計出一種合理的轉彎策略。假設機器人轉彎半徑是R，圓心是O，以起始點為參照的圓心方向是DO，起始點方向是Dstart，x、y表示各點對應橫縱坐標，則當機器人右轉時，用下列表達式描述圓心O的轉動方向與位置

(19)

(20)

已知點Q是加速區域的結束點，則基于三角定理可得該點與下一分段點的間距d′、距離LQ與半徑R所呈夾角θ′，計算公式如下所示

(21)

由此推導出下列點Q的轉動方向與位置計算公式

(22)

當機器人向左轉，將圓心O的方向加90°，點Q坐標求解過程中把φQ+θ′改為φQ-θ′。

當機器人在起始點P′1與目標點P′2的轉彎方向均為順時針方向時，則兩點間距的長度LP′1P′2與斜率k′P′1P′2等于以兩點為圓心的圓外切線AB長度LAB與斜率k′AB，即

(23)

式中，離開第一段弧的角度是θ″arc1，抵達第二段弧的角度是θ″arc2。

若機器人在起始點P′1與目標點P′2的轉彎方向是相對的，離開時為順時針，抵達是逆時針，則對以目標點為圓心的圓做正切圓，以降低運算復雜度，設正切圓圓心為點P′3，因此，基于三角形P′1P′2P′3，推導出下列方程組

(24)

式中，兩目標點間距為LP′2P′3，∠P′1P′2P′3度數是θ′P′1P′2P′3，切線AB角度是θ′AB。根據得到的各分段路徑離開與到達弧度，實現機器人移動路徑的分段規劃，避免發生碰撞，跌倒。

4 機器人路徑分段規劃仿真

4.1 機器人

選取由驅動控制模塊、傳感器模塊、GPS(Global Positioning System，全球定位系統)定位模塊以及軟件控制模塊組成的機器人作為實驗對象，規劃其移動路徑。機器人具體參數信息如表1所示。

表1 實驗用機器人參數統計表

開啟軟件程序，進入仿真界面，初始化處理機器人與路徑規劃環境，將其存儲于數據庫中，利用編程軟件執行本文分段規劃方法，待機器人抵達指定目標點后，終止路徑規劃，退出程序。方法運行時，各傳感器持續讀取環境信息，不停往數據庫中存儲機器人位置信息。

4.2 機器人定位精度分析

超聲波傳感器的采樣時間是0.1秒，分別采用文獻[2]、[3]、[4]方法以及本文方法，融合一定時間里多個傳感器采集到的數據信息，整理得到表2。

表2 多傳感器數據融合測量值統計表

結合上表2中數據可知，本文方法經過預測、觀測以及估計等一系列步驟，不斷更新狀態變量與對應協方差，獲取機器人不同采樣時刻狀態與最優位置估計，所以，對比文獻方法的定位準確度具有顯著優越性，誤差均值不超過1厘米，機器人移動路線與理想的預測軌跡較為趨近。因此，引入多傳感器數據融合為機器人路徑規劃奠定了良好的數據基礎。

4.3 路徑分段規劃仿真

為簡化運算復雜度，驗證方法有效性，將機器人車輪寬度算入整體寬度中，并分別在簡單障礙物與復雜障礙物兩種環境里展開仿真。按照所得融合數據，規劃出機器人移動路徑，分段規劃的最終路徑結果如圖1所示。

圖1 不同環境下路徑分段規劃仿真效果

根據上圖中得到的路徑結果可以看出，機器人不論是在簡單障礙物還是更接近實際環境的復雜障礙物環境中，都規劃出了較為理想的路徑，不僅有效躲避了環境中的障礙物，而且規劃的路徑也比較合理；從機器人移動狀態而言，未出現任何晃動、跌倒趨勢。這是因為本文方法根據路徑軌跡曲率極值點，分段處理了各段路徑上的機器人移動速度，通過平滑處理與轉彎策略，有效實現了機器人移動路徑的分段規劃，防止了碰撞與跌倒的發生。

5 結論

基于給定的環境信息程度，一般將路徑規劃方法劃分成兩種：全局、局部。前者常用于障礙物信息已知、固定、可預見等情況下，而后者則通過傳感器信息，在未知、動態的環境中，展開路徑規劃。將進一步提升機器人障礙物躲避與環境識別能力、實時性與靈活性作為下一個研究重點；應嘗試引用AS-R機器人視覺系統，增加其在三維空間中自由運動的能力；應在完全未知的環境中仿真規劃方法，拓展方法適用性；僅以單機器人為研究對象具有一定的局限性，需在今后的工作中，深入探索多機器人的路徑分段規劃策略；路徑規劃應用領域多種多樣，應通過分析環境信息，采取對應的規劃方法。