基于慣性/偏振光/光流的六足步行機器人自主導(dǎo)航方法研究

曾云豪，杜 濤，星 艷

(北方工業(yè)大學信息學院，北京 110144)

0 引言

近年來，導(dǎo)航技術(shù)在航空、航天、航海、地面交通、測繪等領(lǐng)域已經(jīng)得到廣泛應(yīng)用。目前，應(yīng)用最廣泛的組合導(dǎo)航系統(tǒng)為慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)與全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System，GNSS)的組合[1]，即INS/GNSS組合導(dǎo)航系統(tǒng)。在INS/GNSS組合導(dǎo)航系統(tǒng)中，其導(dǎo)航自主性受限于GNSS信號的通信傳輸，在面臨城市峽谷等遮擋或干擾環(huán)境時，GNSS信號可能發(fā)生丟失或多徑效應(yīng)，造成導(dǎo)航系統(tǒng)精度下降，甚至無法正常工作[1]。

近年來，學者發(fā)現(xiàn)自然界中某些生物具備高超且精準的導(dǎo)航定位能力。例如，沙蟻擁有利用天空偏振光完成長達數(shù)百米的隨機覓食與直線歸巢的能力[3]；帝王蝶可以借助天空偏振光實現(xiàn)長達數(shù)千千米的遷徙導(dǎo)航[4]。以上研究表明,沙蟻和帝王蝶具備精準感知大氣偏振光的能力，可以借助天空中穩(wěn)定的偏振分布模式獲取導(dǎo)航信息，實現(xiàn)高精度、長時間的導(dǎo)航與定位[5-6]。

受生物導(dǎo)航原理的啟發(fā)，國內(nèi)外學者開展了仿生偏振光導(dǎo)航方法研究。2007年，盧鴻謙等[7]提出了偏振光/GPS/SINS組合導(dǎo)航方法，利用偏振光實現(xiàn)了對GPS/SINS組合導(dǎo)航的輔助，提高了組合導(dǎo)航系統(tǒng)的可觀測度和精度。2015年，胡小平等[8]對偏振光定向算法進行了誤差分析，結(jié)果表明偏振光定向誤差主要來自仿生偏振光傳感器誤差和載體的水平角誤差。2019年，何曉峰等[9]針對地面無人平臺，提出了仿生偏振光定向算法，實現(xiàn)了地面無人平臺航向角確定。2019年，Dupeyroux等[10-11]對沙蟻的導(dǎo)航策略開展了研究，研制了仿沙蟻機器人，復(fù)現(xiàn)了沙蟻的運動模式和導(dǎo)航策略。2020年，褚金奎等[12]基于動態(tài)步長搜索機制，提出了基于天空光偏振模式的導(dǎo)航姿態(tài)最優(yōu)化解算方法，進一步提高了偏振光導(dǎo)航系統(tǒng)的穩(wěn)定性與可靠性。

本文從提高組合導(dǎo)航系統(tǒng)的自主性出發(fā)，引入仿生偏振光傳感器和光流傳感器，以輔助慣性導(dǎo)航系統(tǒng)，設(shè)計了無跡Kalman濾波(Unscented Kalman Filter, UKF)方法，并應(yīng)用在六足步行機器人上。

1 六足機器人組合導(dǎo)航系統(tǒng)建模

為了實現(xiàn)六足步行機器人的組合導(dǎo)航，首先，分別建立了基于機器人運動學的狀態(tài)模型和基于仿生偏振光、光流傳感器的量測模型；其次，建立了慣性/偏振光/光流的組合導(dǎo)航系統(tǒng)模型。

1.1 六足步行機器人運動學建模

文中涉及的坐標系為機器人載體坐標系(以載體質(zhì)心為中心，載體運動方向右方為x軸，前方為y軸，上方為z軸，構(gòu)成右手坐標系)，即b系；全局坐標系(以初始時刻載體質(zhì)心為中心，初始時刻載體運動方向右方為x軸，前方為y軸，上方為z軸，構(gòu)成右手坐標系)，即w系。機器人位于起始位置時，假定w系和b系重合;同時假定偏振光傳感器和光流傳感器與六足步行機器人的安裝誤差和桿臂效應(yīng)等已經(jīng)被標定補償，偏振光傳感器和光流傳感器安裝后的測量坐標系與載體坐標系重合。

Xk=f(Xk-1,uk-1)+wk-1

(1)

其中

1.2 基于仿生偏振光傳感器量測方程建模

天氣晴朗時，天空偏振光具有穩(wěn)定的分布模式：不同時間，不同地點，偏振角和偏振度各不相同；而在同一時間，同一地點，同一觀測方向，偏振角和偏振度具有可重復(fù)性[14]。基于偏振敏感(Polarization Sensitive, POL)神經(jīng)元模型[15]的六通道光電偏振光傳感器(圖1所示)，其中每2個通道構(gòu)成一個對立的偏振單元，類似于POL神經(jīng)元，可以獲取偏振角和偏振度等信息。單個通道偏振傳感器量測方程如下

Ii(φ)=KIt(1+dcos(φ-αi)),i=1, 2,…, 6

(2)

其中，Ii(φ) 是第i通道光電二極管的輸出強度；K是未知尺度因子，需要標定；It是總光強度；d為偏振度；φ為偏振方位角；αi是傳感器的偏振方向和參考方向之間的角度，由式(3)給出

(3)

圖1 六通道光電偏振光傳感器Fig.1 Six-channel photoelectric polarized light sensor

在不同的天氣條件下，It具有不同的值，It值的變化可能會限制其在不同環(huán)境條件下的通用性。實際上，許多昆蟲復(fù)眼具有偏振對立結(jié)構(gòu)，可以消除不同環(huán)境下由于總光強不同帶來的影響。受偏振對立單元結(jié)構(gòu)的啟發(fā)，將式(2)與式(3)結(jié)合，得到

(4)

(5)

(6)

其中，p1、p2和p3為偏振對立單元調(diào)整到0、π/3和2π/3這3個方向的輸出。

(7)

(8)

式(7)和式(8)即為獲取的偏振度和偏振方位角信息。針對二維平面運動，根據(jù)文獻[16]，太陽方位角αs與偏振光傳感器測量的偏振方位角φ、航向角θ的關(guān)系如下

(9)

其中，太陽方位角αs由文獻[17]方法得到：當αs≥π/2時，β=1；αs<π/2時，β=-1。

k時刻偏振光傳感器的量測方程為

zp, k=θk+vp, k

(10)

其中，量測zp,k=φk+β(αs-π/2)∈R；θk為k時刻航向角，可以由姿態(tài)四元數(shù)轉(zhuǎn)換得到；vp,k為服從零均值，方差為Rp,k的高斯噪聲。

1.3 基于光流傳感器的量測方程建模

光流傳感器基于像素運動場與載體速度之間的關(guān)系，獲得載體運動速度，具體原理過程如下[18]。

記相機參考坐標系點Pc=[Xc,Yc,Zc]T，其在像素平面的投影puv為

(11)

其中，puv=[xuv,yuv,f]T，f為相機焦距；相機和Pc之間的相對運動為

Vc=-T-ωc×Pc

(12)

其中，ωc∈R3為角速度，T∈R3為運動的平移分量，對式(11)求導(dǎo)得

(13)

其中，Vc,z為Vc的z軸分量。式(13)中，將vuv根據(jù)焦距和傳感器到場景距離進行縮放，得到平移速度vm[18]

(14)

其中，D可以由Pix4flow光流傳感器獲得，vm中包含對地垂直方向速度。由此，光流傳感器的量測方程為

zof,k=CVk+vof,k

(15)

其中，量測zof,k=vm∈R3；C為k時刻全局參考系到載體系的轉(zhuǎn)換；Vk為k時刻全局參考系中機器人運動速度；vof, k為服從零均值，方差為Rof, k的量測噪聲。

綜上所述，慣性/偏振光/光流組合導(dǎo)航系量測方程為

zk=h(Xk)+vk

(16)

由式(1) 和式(16) 可得慣性/偏振光/光流組合導(dǎo)航系統(tǒng)方程。

2 UKF設(shè)計

針對慣性/偏振光/光流組合導(dǎo)航系統(tǒng)方程，設(shè)計了UKF估計六足步行機器人的位姿。

UKF如式(17)～式(24)所示，主要流程分為以下2個步驟[13]。

(1)狀態(tài)預(yù)測：

1)構(gòu)造2n+1，n=10的sigma點集

2)將sigma點集通過狀態(tài)轉(zhuǎn)移函數(shù)映射到新的點集上

(17)

3)將新的sigma點集加權(quán)后用于狀態(tài)的一步預(yù)測和協(xié)方差預(yù)測

(18)

(19)

(2)量測更新：

1)生成總數(shù)為2n+1的sigma點集

2)將新的sigma點集通過量測函數(shù)映射到新的點集上

3)將新的sigma點集加權(quán)后用于量測的估計值和協(xié)方差預(yù)測

(20)

4)計算UKF增益

(21)

(22)

5)更新系統(tǒng)狀態(tài)和協(xié)方差矩陣

(23)

(24)

其中

3 仿真驗證

為驗證慣性/偏振光/光流組合導(dǎo)航算法的有效性，以六足步行機器人的位姿估計為對象，模擬了變加速度和變角速度仿真運動數(shù)據(jù)，且仿真數(shù)據(jù)中慣性測量單元(Inertial Measurement Unit, IMU)數(shù)據(jù)頻率為100Hz，光流傳感器數(shù)據(jù)頻率為50Hz，偏振光傳感器數(shù)據(jù)頻率為20Hz，設(shè)計UKF，并與慣性/光流組合導(dǎo)航系統(tǒng)位姿估計進行對比。

在UKF過程中，系統(tǒng)噪聲方差矩陣設(shè)置為

Qk-1=diag([0.1 0.1 0.1 0.06 0.06 0.06

0.03 0.03 0.03 0.03])2

速度量測噪聲的協(xié)方差為

航向角量測噪聲的協(xié)方差為

Rp,k=0.062

六足步行機器人的位置、速度和航向角估計以及估計誤差如圖2～圖7所示。

圖2 位置估計Fig.2 Position estimation

圖3 速度估計Fig.3 Velocity estimation

圖4 航向角估計Fig.4 Heading angle estimation

圖5 位置估計誤差Fig.5 Position estimation errors

圖6 速度估計誤差Fig.6 Velocity estimation errors

圖7 航向角估計誤差Fig.7 Heading angle estimation error

為說明引入偏振光量測信息后組合導(dǎo)航系統(tǒng)位姿估計的改進效果，本文對比了慣性/偏振光/光流組合導(dǎo)航系統(tǒng)和慣性/光流組合導(dǎo)航系統(tǒng)兩種組合導(dǎo)航方法。為了消除隨機噪聲的影響，開展了50次蒙特卡羅仿真，結(jié)果如表1所示。

表1 50次實驗平均均方根誤差

仿真結(jié)果表明，在使用UKF算法的情況下，與慣性/光流組合導(dǎo)航系統(tǒng)相比，慣性/偏振光/光流組合導(dǎo)航系統(tǒng)在位置、速度和航向角等方面的估計值有明顯改進，且更接近基準值，而慣性/光流組合導(dǎo)航系統(tǒng)僅速度估計接近基準值。從表1中可以得到，基于慣性/偏振光/光流的組合導(dǎo)航系統(tǒng)定位均方根誤差在10cm左右，航向角均方根誤差在0.05rad左右；而無偏振光量測的基于慣性/光流的組合導(dǎo)航系統(tǒng)定位均方根誤差在40cm左右，航向角均方根誤差在0.2rad左右。

4 六足機器人設(shè)計與實驗

4.1 六足步行機器人設(shè)計

為了進一步驗證基于慣性/偏振光/光流的六足步行機器人的自主導(dǎo)航方法，本文搭建了低成本六足機器人實驗平臺(圖8所示)。機器人共有六足，每條腿均有3個關(guān)節(jié)，由18個MG996舵機驅(qū)動，安裝在計算機數(shù)控(Computer Numerical Control，CNC)加工的結(jié)構(gòu)件中。每個舵機的動作由Jetson Nano發(fā)送指令到18通道舵機控制板產(chǎn)生。六足機器人由 NIVIDIA Jetson Nano 微型計算機控制，所有傳感器的信息都在此處理，其總體架構(gòu)如圖9所示。

圖8 六足步行機器人Fig.8 Hexapod robot

圖9 硬件架構(gòu)Fig.9 Hardware architecture

4.2 室外實驗

室外實驗環(huán)境地點為北方工業(yè)大學運動場入口道路，在長為2.4m，寬為2.4m，地面紋理清晰的區(qū)域中進行，實驗時間為15∶00，天氣條件晴朗無云，實驗場地無遮擋。組合導(dǎo)航系統(tǒng)中采用消費級IMU，型號ICM-20602,其性能參數(shù)為：陀螺零偏 1(°)/s，加速計零偏40mg。機器人運動路徑如圖10所示，沿2.4m×2.4m正方形路徑運動，并返回起點。

從圖11和圖12可以看出，六足機器人的位姿估計效果較好。在無直接位置量測的情況下，其估計路徑與預(yù)設(shè)運動路徑接近。實測實驗結(jié)果表明，基于慣性/偏振光/光流的六足步行機器人的自主導(dǎo)航方法性能優(yōu)異，可實際應(yīng)用。

圖10 預(yù)設(shè)機器人運動路徑Fig.10 Preset robot motion path

圖11 實時顯示濾波后的路徑Fig.11 Real time display of filtered path

圖12 路徑估計Fig.12 Estimated paths

5 結(jié)論

本文針對常見的組合導(dǎo)航系統(tǒng)依賴無線電信號，系統(tǒng)自主性不足的問題，提出了一種基于慣性/偏振光/光流的六足步行機器人自主導(dǎo)航方法。分別建立了基于偏振光傳感器和光流傳感器的量測方程，并開展了仿真與實驗，結(jié)果表明：

1)引入誤差不隨時間累積的偏振光傳感器量

測信息可提升導(dǎo)航系統(tǒng)的航向角估計精度，并間接提升位置與速度估計精度。

2)本文利用無需外部信號源的傳感器，提出了基于慣性/偏振光/光流的六足步行機器人自主導(dǎo)航方法，可有效降低位姿估計的累計誤差，并具備高實時性和強自主性。

在未來研究中，將進一步考慮六足步行機器人的動力學模型，研究傳感器誤差等因素，以適應(yīng)高精度導(dǎo)航的要求，并開展不同天氣、不同光照、障礙物遮擋的情況對導(dǎo)航精度影響的研究，以及如何降低環(huán)境變化對偏振光導(dǎo)航精度的影響。