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

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

中圖分類號：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 引言

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

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

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

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

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

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

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

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

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

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

3）激光測距傳感器會受到環(huán)境中干擾因素的影響，使數(shù)據(jù)產(chǎn)生毛刺。

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

2.1 傳感器數(shù)據(jù)預(yù)處理

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

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

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

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

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

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

考慮機器人的運動動態(tài)系統(tǒng)模型為

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

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

2）狀態(tài)預(yù)測，對機器人狀態(tài)進行預(yù)測。

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

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

5）狀態(tài)更新，對卡爾曼增益K′進行更新，X︿′_k為對應(yīng)傳感器的本次預(yù)測更新的系統(tǒng)狀態(tài)向量的最優(yōu)估計值，同時進行對應(yīng)協(xié)方差矩陣P′_k的更新。

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

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

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

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

模型預(yù)測控制算法由運動學模型建立、模型預(yù)測、構(gòu)建誤差目標函數(shù)、優(yōu)化求解這幾部分構(gòu)成。

記控制步長為N，結(jié)合前文得到的機器人運動模型便可以得到預(yù)測時域內(nèi)的機器人狀態(tài)預(yù)測模型為X=A_qpx_k+B_qpU。

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

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

構(gòu)造二次規(guī)劃問題，以實現(xiàn)問題的最優(yōu)化。其中第一項與位置的跟蹤精度對應(yīng)，第二項控制機器人速度，這在四足機器人的平衡穩(wěn)定運動中起著相當?shù)淖饔谩?/p>

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

式中C為參數(shù)矩陣。

采用二次規(guī)劃對此問題進行求解。

4 狹小檢測環(huán)境下機器人自主導航試驗驗證

4.1 仿真測試

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

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

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

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

4.2 實物測試與驗證

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

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

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

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

5 結(jié)語

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

參考文獻：

[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的四足機器人路徑規(guī)劃研究[J]. 無人系統(tǒng)技術(shù)，2021，4（6）： 57-64.

[6] ZHANG C，YANG Z，F(xiàn)ANG 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，F(xiàn)rance：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