曾云豪，杜 濤，星 艷

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

0 引言

近年來，導(dǎo)航技術(shù)在航空、航天、航海、地面交通、測(cè)繪等領(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信號(hào)的通信傳輸，在面臨城市峽谷等遮擋或干擾環(huán)境時(shí)，GNSS信號(hào)可能發(fā)生丟失或多徑效應(yīng)，造成導(dǎo)航系統(tǒng)精度下降，甚至無法正常工作[1]。

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

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

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

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

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

1.1 六足步行機(jī)器人運(yùn)動(dòng)學(xué)建模

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

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

(1)

其中

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

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

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

(2)

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

(3)

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

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

(4)

(5)

(6)

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

(7)

(8)

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

(9)

其中，太陽方位角αs由文獻(xiàn)[17]方法得到：當(dāng)αs≥π/2時(shí)，β=1；αs<π/2時(shí)，β=-1。

k時(shí)刻偏振光傳感器的量測(cè)方程為

zp, k=θk+vp, k

(10)

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

1.3 基于光流傳感器的量測(cè)方程建模

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

記相機(jī)參考坐標(biāo)系點(diǎn)Pc=[Xc,Yc,Zc]T，其在像素平面的投影puv為

(11)

其中，puv=[xuv,yuv,f]T，f為相機(jī)焦距；相機(jī)和Pc之間的相對(duì)運(yùn)動(dòng)為

Vc=-T-ωc×Pc

(12)

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

(13)

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

(14)

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

zof,k=CVk+vof,k

(15)

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

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

zk=h(Xk)+vk

(16)

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

2 UKF設(shè)計(jì)

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

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

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

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

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

(17)

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

(18)

(19)

(2)量測(cè)更新：

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

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

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

(20)

4)計(jì)算UKF增益

(21)

(22)

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

(23)

(24)

其中

3 仿真驗(yàn)證

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

在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

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

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

Rp,k=0.062

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

圖2 位置估計(jì)Fig.2 Position estimation

圖3 速度估計(jì)Fig.3 Velocity estimation

圖4 航向角估計(jì)Fig.4 Heading angle estimation

圖5 位置估計(jì)誤差Fig.5 Position estimation errors

圖6 速度估計(jì)誤差Fig.6 Velocity estimation errors

圖7 航向角估計(jì)誤差Fig.7 Heading angle estimation error

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

表1 50次實(shí)驗(yàn)平均均方根誤差

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

4 六足機(jī)器人設(shè)計(jì)與實(shí)驗(yàn)

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

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

圖8 六足步行機(jī)器人Fig.8 Hexapod robot

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

4.2 室外實(shí)驗(yàn)

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

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

圖10 預(yù)設(shè)機(jī)器人運(yùn)動(dòng)路徑Fig.10 Preset robot motion path

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

圖12 路徑估計(jì)Fig.12 Estimated paths

5 結(jié)論

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

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

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

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

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