狹小檢測空間下多傳感融合四足機器人自主導航策略

摘 要：針對狹小檢測空間的機器人巡檢任務需求，提出多傳感適時融合的小型四足機器人自主導航策略。采用慣性測量單元和激光測距傳感器進行卡爾曼適時融合濾波獲取機器人位姿；引入模型預測控制實現對期望路徑的高精度魯棒跟蹤，完成機器人狹小空間內的自主導航目標。試驗驗證表明：機器人實際行進路徑與期望路徑的吻合度較好，基于該策略可以實現小型四足機器人在狹小空間的自主導航目標。

關鍵詞：四足機器人；自主導航；狹小空間；多傳感適時融合算法；模型預測控制；局部路徑跟蹤

中圖分類號：TP242 文獻標志碼：A 文章編號：1671-5276（2024）04-0214-05

Autonomous Navigation Strategy for Multi-sensor Fusion Quadruped Robots in Confined Spaces

XING Yuhang¹，ZHANG Zhourong¹，YUAN Jiwei¹，DUAN Jinjun²， XUE Jingfeng²， DAI Zhendong¹

（1. College of Mechanical and Electrical Engineering， Nanjing University of Aeronautics and Astronautics， Nanjing 210016， China; 2. Chinese Aeronautical Establishment， Beijing 225111， China）

Abstract：For the demand of robot inspection tasks in confined space， this paper proposes an autonomous navigation strategy for small quadruped robots with multi-sensor timely fusion. Inertial measurement units and laser range sensors are used to obtain the robot's position by timely Kalman fusion filtering， and model predictive control is introduced to achieve high-precision robust tracking of the desired path to complete the robot's autonomous navigation goal in a confined space. The experimental verification shows that the actual path of the robot travels well with the desired path， and the autonomous navigation task of the small quadruped robot in the confined space can be achieved based on this strategy.

Keywords：quadruped robots；autonomous navigationl；confined space; multi-sensor fusion algorithm; model predictive control; local path tracking

0 引言

在航空工業中，諸如機翼艙道、飛機進氣道等對于飛機的安全可靠運行有著至關重要的作用，因此針對這些部件的定期維護檢修需求很大^[1-2]。此類部件人工難以進入內部檢修，使用機器人代替人工進行檢修能大大提高效率，而小型四足機器人在此類非結構復雜空間中表現出較強的越障能力和一定的負載能力。

為實現四足機器人在狹小空間內的檢測任務，需要四足機器人具備在環境中的精確定位并跟蹤軌跡的能力^[3]。在機器人導航問題中狀態估計和路徑跟蹤是導航框架中兩個重要環節，浙江大學的ZHANG等^[4]通過三維激光雷達融合IMU及運動學里程計實現機器人的狀態感知，并通過動力學建模實現對機器人軌跡的滾動時域控制。中國北方車輛研究所的劉宇飛等^[5]采用基于雷達點云信息的全局SLAM定位系統，提出了一種基于拓撲度量相結合的定位框架，實現了在室外環境的地形建模和定位，并利用Dijkstra算法和人工勢場理論實現了全局及局部的路徑規劃和自主避障。ZHANG等^[6]提出了一套輕量化的四足機器人導航系統FSR-SLAM，該系統利用深度相機和二維激光雷達完成機器人狀態感知。MIT的KIM等^[7]在Mini-Cheetah上通過RealSense深度相機實現了機器人對地形的高度感知。ETH ANYmal團隊的TRANZATTO等^[8]利用互補的多模態傳感器融合方法（CompSLAM）完成機器的自主導航，其融合了視覺和熱成像、激光雷達深度數據、慣性和運動學位姿估計，以分層方式提供針對傳感器數據的融合處理。

不同于一般的四足機器人自主導航問題，狹小空間中的導航問題有一定的特殊性，在諸如空間結構尺寸、表面材質、信號屏蔽、障礙物處理等問題上都有獨特的限制條件和邊界情況，由此也帶來了機器人導航方式和傳感器選取上的限制。針對上述問題，本文提出采用高精度激光測距傳感器和IMU數據融合實現機器人的狀態估計，在此基礎上結合機器人的失穩邊界，對期望路徑進行基于模型預測控制算法的閉環控制，為狹小復雜空間下的四足機器人自主導航提供了一種可行的思路。

1 四足機器人自主導航策略

在狹小空間的檢測任務中，為實現機器人在指定位置的準確定點啟停，以更好地完成檢測任務，本文提出在已知地圖的指定位置及機器人狀態切換節點設立信標點，從而將全局地圖轉換為依次連續的以各信標點為起點的局部地圖。

根據所在局部地圖特性，設計機器人的運行狀態以及期望路徑，并對機器人在該局部地圖內的位姿狀態進行估計，為使機器人獲得更準確、魯棒的狀態估計，采用激光測距傳感器和慣性測量單元進行卡爾曼濾波最優融合，并以融合后的估計值與期望路徑作為輸入，通過模型預測控制輸出機器人的期望速度，以實現機器人在每個局部內的高精度路徑跟蹤（圖1）。

2 多傳感適時融合位姿估計

機器人對自身位置的準確估計是實現路徑跟蹤的前提，機器人在環境中行走時存在以下問題：

1）地面情況不平整，由于自身重力作用、足底與界面間的打滑及傳動誤差等因素，自身的運動學里程計會產生較大偏移；

2）使用IMU進行積分可以估計機器人自身狀態，但無法長時間準確估計機器人在環境中的精確位姿；

3）激光測距傳感器會受到環境中干擾因素的影響，使數據產生毛刺。

為解決上述問題，本文提出一種基于卡爾曼濾波的多傳感適時融合算法，利用激光測距傳感器的高精度特性及IMU基于機器人自身狀態不受環境干擾的特性，對激光測距傳感器和IMU數據進行融合濾波。

2.1 傳感器數據預處理

IMU固連安裝于機器人本體構成捷聯系統，因此基于IMU的觀測都在機器人本體坐標系下，對陀螺儀的觀測進行積分可以得到機器人本體姿態；對加速度計的觀測進行積分，并通過陀螺儀獲取的姿態進行變換轉換到大地坐標系下，消除重力加速度影響后，對觀測進行二次積分，最終可以得到機器人本體相對于大地坐標系的位置。

激光測距傳感器（LRS）可以適用于多種不同工況，不受光照條件、表面材質的影響，可直接獲取與環境間的高精度相對位置信息，因此不存在累計誤差，但同時在受環境因素干擾時，會產生數據失真現象。

在本方案中，機器人機身周圍共設置6個激光測距傳感器，分別位于機器人左右兩側及后方。

在圖2所示局部地圖中，通過位于機器人左側的兩個激光測距傳感器進行對機器人側邊距和偏航角的估計，令大地坐標系原點與環境立面的側邊距為e，可以得到機器人本體坐標系對于大地坐標系的狀態估計。

2.2 基于卡爾曼濾波的傳感器適時融合算法

由于卡爾曼濾波算法僅需儲存前一時刻的狀態量，因此適用于機器人搭載的嵌入式系統，并具有很好的實時性。

考慮機器人的運動動態系統模型為

式中：X_k+1=[P_yΨ P_y·Ψ·]為系統下一時刻的系統狀態變量；X_k為當前時刻的系統狀態變量；A為狀態轉移矩陣；U_k為控制輸入；B為控制增益矩陣；W_k為運動模型的噪聲；Z_k為傳感器的觀測量；H為傳感器觀測矩陣；V_k為傳感器觀測噪聲。該適時融合方法的主要過程（圖3）如下。

1）傳感器初始化，以機器人局部地圖坐標原點作為起點，將機器人靜止狀態時狀態變量作為起始狀態。

2）狀態預測，對機器人狀態進行預測。

3）傳感器測量，通過IMU和LRS分別得到傳感器的測量值，記作Z_IMU和Z_LRS。為消除累計誤差對估計值的影響，IMU僅更新速度的變化量。

4）閾值判斷，當LRS檢測到突變并判斷其大于設定閾值時，僅利用IMU在此時融合結果基礎上進行估計，并在恢復后回到融合濾波模式。

5）狀態更新，對卡爾曼增益K′進行更新，X︿′_k為對應傳感器的本次預測更新的系統狀態向量的最優估計值，同時進行對應協方差矩陣P′_k的更新。

圖3 卡爾曼適時融合濾波算法流程圖

基于該融合濾波方法，既可以利用激光測距傳感器實現機器人在局部環境中的高精度、無累計誤差的狀態估計，又可以利用IMU短時運動估計不受環境變化影響的特點，有效增加了系統應對環境干擾時的魯棒性和長期狀態估計的準確無偏。

3 基于模型預測控制的局部路徑跟蹤算法

在本自主導航策略中，采用模型預測控制實現機器人對于局部地圖中的期望路徑進行跟蹤。將融合濾波后的狀態估計作為輸入，通過模型預測控制輸出機器人速度，并作為期望值輸入機器人底層運動控制環節。

模型預測控制算法由運動學模型建立、模型預測、構建誤差目標函數、優化求解這幾部分構成。

記控制步長為N，結合前文得到的機器人運動模型便可以得到預測時域內的機器人狀態預測模型為X=A_qpx_k+B_qpU。

取ΔT為時間間隔，離散得到一系列機器人位姿的期望值X_k+i，ref（i=1，2，3，…，N）。

構建目標函數時，希望跟蹤誤差及控制量都盡可能小，因此以此建立目標函數：

構造二次規劃問題，以實現問題的最優化。其中第一項與位置的跟蹤精度對應，第二項控制機器人速度，這在四足機器人的平衡穩定運動中起著相當的作用。

在此基礎上，根據實際情況，同時需要對輸入輸出做一些限制，由于本文研究的環境為狹小環境，同時要求一定的作業精度，因此將最大允許的側向工作范圍作為其中一個約束，通過c_min和c_max表示，另外由機器人速度失穩邊界產生對控制量的約束，通過U_min和U_max表示，將模型預測控制問題描述為：

式中C為參數矩陣。

采用二次規劃對此問題進行求解。

4 狹小檢測環境下機器人自主導航試驗驗證

4.1 仿真測試

在Webots軟件中搭建仿真環境進行試驗，選取其中一個局部地圖對基于卡爾曼濾波的多傳感適時融合算法進行測試（圖4）。

從圖4中可以看出，該濾波算法在側向距離和偏航角兩個維度上都可以有效過濾突變數據，增強了機器人在環境存在干擾工況下的魯棒性。

設定期望偏航角及側向距離并選取采樣區間對機器人位姿進行測量，得出結果如圖5所示。

由此可知，在機器人行進時，通過模型預測控制進行對期望軌跡的跟蹤有顯著效果，對側向距離的控制可以精確到±0.01m，同時對偏航角的控制可以精確到±0.02rad。

4.2 實物測試與驗證

實物試驗主要基于上述仿真試驗的結果，對實物機器人在機翼艙道內軌跡跟蹤進行測試。機器人的各項參數及實物試驗中的各項參數選取如表1所示。

根據本文提出的機器人自主導航策略，首先對機器人的路徑跟蹤能力進行驗證，由于機器人在局部地圖連接時存在一定的誤差，因此分別驗證機器人在不同初始位姿偏置情況下的路徑跟蹤情況（圖6）。

在圖6中4種不同初始偏置情況下，分別記錄機器人側向位移與偏航角的變化情況（圖7）。

從圖7數據可以分析得出，機器人樣機在不同初始偏置情況下的位姿變化情況，在初始均無偏置情況下，機器人側向距離穩定在期望距離±15mm以內，偏航角穩定在±0.1rad內。當存在初始側向偏置時，機器人均可在6s內穩定在期望位置與姿態，由此說明機器人樣機可以在不同偏置情況下快速收斂到局部地圖的期望路徑上，完成機器人在艙道內的高精度路徑跟蹤。在此基礎上，根據上文基于信標點的全局地圖離散方法，通過狀態機的判斷和切換進行局部地圖間的連接，以此實現機器人在全局地圖上的自主導航（圖8）。

5 結語

本文針對狹小檢測空間的機器人巡檢任務需求，進行了四足機器人自主導航策略的研究，提出了多傳感適時融合位姿估計算法以及模型預測控制路徑跟蹤算法。實驗結果表明：采用基于卡爾曼融合濾波的多傳感適時融合算法，機器人在艙道內行進時可以較好地過濾角鐵帶來的傳感器失真，在復雜環境中為機器人提供穩定準確的高精度狀態估計。在此基礎上，基于模型預測控制，在機器人失穩邊界內得出多目標下的最優期望速度，實現機器人反饋控制，精確穩定地對機器人期望路徑進行實時跟蹤，通過任務狀態機控制聯接各局部，實現機器人在狹小復雜空間下四足機器人的自主導航。

參考文獻：

[1] NEGRI S P， BASILE V， VALORI M， et al. A modular mobile robotic architecture for defects detection and repair in narrow tunnels of CFRP aeronautic components[J]. Robotics and Computer-Integrated Manufacturing， 2019， 55： 109-128.

[2] JENA S，GUPTA A. Embedded sensors for health monitoring of an aircraft[M]//Energy，Environment，and Sustainability. Singapore：Springer Singapore，2018：77-91.

[3] FERREIRA J，MOREIRA A P，SILVA M，et al. A survey on localization，mapping，and trajectory planning for quadruped robots in vineyards[C]//2022 IEEE International Conference on Autonomous Robot Systems and Competitions （ICARSC）. Santa Maria da Feira，Portugal：IEEE，2022：237-242.

[4] ZHANG Z，YAN J Q，KONG X，et al. Efficient motion planning based on kinodynamic model for quadruped robots following persons in confined spaces[J]. IEEE/ASME Transactions on Mechatronics，2021，26（4）：1997-2006.

[5] 劉宇飛，江磊，邢伯陽，等. 基于全局SLAM的四足機器人路徑規劃研究[J]. 無人系統技術，2021，4（6）： 57-64.

[6] ZHANG C，YANG Z，FANG Q H，et al. FRL-SLAM：a fast，robust and lightweight SLAM system for quadruped robot navigation[C]//2021 IEEE International Conference on Robotics and Biomimetics （ROBIO）. Sanya，China：IEEE，2022：1165-1170.

[7] KIM D，CARBALLO D，DI CARLO J，et al. Vision aided dynamic exploration of unstructured terrain with a small-scale quadruped robot[C]//2020 IEEE International Conference on Robotics and Automation （ICRA）. Paris，France：IEEE，2020：2464-2470.

[8] TRANZATTO M，MIKI T，DHARMADHIKARI M，et al. CERBERUS in the DARPA subterranean challenge[J]. Science Robotics，2022，7（66）：eabp9742.

收稿日期：2023-02-15