基于多源信息緊耦合的四足機器人平面導航方法

褚金奎,尉 達,張鐘元,于 灝,宋 濤

(大連理工大學機械工程學院,遼寧大連 116024)

0 引言

隨著科技的發(fā)展,智能移動機器人在服務(wù)業(yè)[1]、制造業(yè)[2]以及星際探索等領(lǐng)域已經(jīng)獲得了廣泛的應用。傳統(tǒng)的輪式機器人在實際應用過程中,越障能力差、地形適應能力差、轉(zhuǎn)彎效率低,容易打滑,不夠平穩(wěn),面對復雜地形時不能正常工作[3-5]。足式機器人的使用場景廣泛與復雜,幾乎可以適應各種地形,能夠跨越障礙,有著良好的自由度,同時動作靈活、自如、穩(wěn)定,在環(huán)境探索、災害搜救中起到至關(guān)重要的作用[6-7]。然而目前絕大多數(shù)的四足仿生機器人都不攜帶自主導航系統(tǒng),完全由人工實現(xiàn)遠程操控,但隨著機器人工作多樣化,操作復雜化,解決這一問題變得重要起來。航姿參考系統(tǒng)(attitude and heading reference system,AHRS)是以電子羅盤修正陀螺儀角速度積分從而得到航向角,然而電子羅盤易受到磁場干擾,同時陀螺儀的零漂誤差會隨時間積累[8],所以在衛(wèi)星拒止,電場干擾以及需要長時間運行的復雜環(huán)境中難以獲得精確穩(wěn)定的航向信息,魯棒性較差[9]。

偏振光導航[10]是一種新興導航方式,借鑒昆蟲利用天空偏振光進行導航[11]的思路發(fā)展而來,具有無誤差積累、實時性好的特點。因此,將偏振光傳感器與其他傳感器相結(jié)合來提高導航性能,受到國內(nèi)外學者的關(guān)注。2009年,王洪青[12]搭建了輪式自主移動機器人平臺,并且采用偏振光傳感器實現(xiàn)其室外導航功能;2011年,Chahl等[13]根據(jù)蜻蜓復眼結(jié)構(gòu)設(shè)計了天空偏振羅盤,并在無人機飛行測試中利用羅盤保持航向;2015年,支煒等[14]提出了一種偏振光及紅外傳感器輔助慣導定姿方法,利用偏振光傳感器與紅外傳感器在慣性導航系統(tǒng)中進行修正位置與姿態(tài)信息解算,并且依據(jù)常見的慣導系統(tǒng)參數(shù)進行了動態(tài)仿真;2019年,Dupeyroux等[15]基于紫外光線性偏振設(shè)計了天體羅盤,并利用羅盤在六足步行機器人的直線行走實驗上保持航向恒定;2020年,褚金奎等[16]實現(xiàn)了偏振光傳感器在實際三維空間中的導航應用,并利用傳感器完成了四旋翼飛行器的導航控制;同年褚金奎等[17]設(shè)計了一種偏振光傳感器,微慣性測量單元及全球定位系統(tǒng)(global positioning system,GPS)的組合導航系統(tǒng),并且實現(xiàn)無人船的自主導航與編隊航行。

上述研究中,其導航方法利用純偏振光獲得航向信息,或采用松耦合方式,分別處理偏振光傳感器與其他傳感器數(shù)據(jù)后再做置信融合,并未將其優(yōu)勢完全發(fā)揮出來。偏振光傳感器具有瞬時性,不存在累計誤差,但探測范圍存在2個半?yún)^(qū),設(shè)備啟動工作時不能靠自身判斷出當前所在半?yún)^(qū),而存在于慣性系統(tǒng)中的磁力計可以解決這個問題,同時偏振光傳感器要正對天頂使用,對于近距離掠過其上方的飛行物感應較強烈,魯棒性較差,易被環(huán)境干擾。2種導航系統(tǒng)各有一定的優(yōu)缺點,利用各自傳感器的原始信息數(shù)據(jù),將兩者緊耦合,以偏振光信息為主,慣性測量單元(inertial measurement unit,IMU)作為輔助,可以彌補各自的不足并發(fā)揮自身的優(yōu)越性。

本文為了實現(xiàn)四足機器人在衛(wèi)星拒止環(huán)境中的自主導航,并提升導航系統(tǒng)的抗干擾性。首先設(shè)計了偏振光傳感器與九軸AHRS的緊耦合導航算法;然后,設(shè)計實驗驗證了該導航算法的抗環(huán)境干擾能力;最后,基于上述組合導航算法,與傳統(tǒng)地磁慣性和偏振光導航方式進行了四足機器人的循跡對比實驗,實驗表明組合導航算法在光干擾和電磁環(huán)境干擾的情況下可以正常工作,并且循跡路徑精度得到有效提升,驗證了組合導航系統(tǒng)可以實現(xiàn)衛(wèi)星拒止環(huán)境中的四足機器人自主導航。

1 偏振光導航原理與多源信息緊耦合方法

1.1 偏振光導航原理

太陽光在傳播過程中遇到大氣中的各種粒子時會發(fā)生散射現(xiàn)象,在天空形成穩(wěn)定的偏振分布模式,利用大氣瑞利散射理論模型,可以建立全天空在晴朗無云條件下的理想大氣偏振模式。在某一時刻天空中任意一點光偏振方向即E矢量總是垂直于由太陽S、觀測點O和被觀測點P組成的平面。倘若處于地面O點的觀察者可以探測出天空某點的E矢量方向與自身方向的幾何關(guān)系,就可以確定出自身的方向。

本文選用東北天坐標系為導航坐標系,偏振光傳感器固聯(lián)于載體坐標系為右前上坐標系(m),圖1中Hs為太陽高度角,As為太陽方位角,E矢量為傳感器觀測點處光偏振方向,α為觀測點處光偏振方向與傳感器體軸夾角。

圖1 偏振光導航原理圖

(1)

(2)

太陽視線在導航坐標系下的投影為

(3)

(4)

1.2 偏振光傳感器與九軸AHRS緊耦合算法

多源緊耦合算法設(shè)計如圖2所示,包括偏振光傳感器、陀螺儀、加速度計和磁力計。由于路徑導航主要依賴于航向角以及位置信息,偏振光傳感器提供的航向角魯棒性差,對環(huán)境干擾敏感,基于卡爾曼濾波,提取各傳感器原始數(shù)據(jù),利用陀螺儀輸出角速度進行角度預測與補償,能夠大幅提高其魯棒性,在實際使用過程中,如果不能正對天頂會影響偏振光傳感器的測量精度。根據(jù)Sabatelli等[18]提出的基于擴展卡爾曼濾波(extended Kalman filter,EKF)的IMU姿態(tài)角解算方法,計算出當前的翻滾角r、俯仰角p和磁航角y,然后根據(jù)Zhi等[19]提出的一種偏振光傳感器姿態(tài)補償算法進行航向修正,該方法可有效提高傳感器的角度輸出精度。磁航角可以在設(shè)備開機時幫助修正偏振光傳感器的二義性:當輸出值為0～1時,置偏振光傳感器為上半?yún)^(qū)0～180°;輸出值為-1～0時,置偏振光傳感器為下半?yún)^(qū)180°～360°。

圖2 多源緊耦合導航算法模型流程

在進行耦合時要解決偏振光傳感器的半?yún)^(qū)映射問題:偏振光傳感器只能輸出0～180°的數(shù)據(jù),所以在對應全方向上存在2個半?yún)^(qū);相反的,陀螺儀的Z軸輸出是全向變化的。所以需要將偏振光傳感器數(shù)據(jù)對應映射到陀螺儀中。并且卡爾曼濾波的主要作用是去除跳變點,但是在傳感器進行半?yún)^(qū)轉(zhuǎn)換時,存在正常的跳變點,這種跳變點并不是由于環(huán)境干擾而產(chǎn)生的,所以相應的卡爾曼濾波不能將這種跳變點進行過濾。為了解決這些問題,首先設(shè)置了半?yún)^(qū)符號用來識別當前半?yún)^(qū),接下來規(guī)定了有效卡爾曼濾波區(qū)間,其中濾波范圍為10°～170°和190°～350°,其余范圍是入跳變檢測區(qū)域,當進入跳變檢測區(qū)域時,關(guān)閉卡爾曼濾波,直接采用測量值,濾波范圍可以隨著采樣頻率的增加而擴大。映射流程如圖3所示,過程中初始化卡爾曼濾波參數(shù)的目的在于半?yún)^(qū)轉(zhuǎn)換后重新進入濾波范圍時,濾波器的中間參數(shù)還停留在離開濾波范圍的時刻,初始化可以防止中間參數(shù)對于新的濾波結(jié)果造成影響。

圖3 半?yún)^(qū)映射流程

在實際應用過程中,偏振光傳感器通常與載體固聯(lián)安裝,并不會始終保持水平,利用AHRS解算的姿態(tài)角進行偏振角補償,之后利用外部的UTC時鐘以及經(jīng)緯度信息得到太陽高度角以及太陽方位角進行航向角求解。首先計算太陽信息:

sinHs=sinφ·sinδ+cosφ·cosδ·cost

(5)

cosAs=(sinHs·sinφ-sinδ)/(cosHs·cosφ)

(6)

式中:φ為地理緯度;δ為太陽赤緯;t為時角。

之后進行姿態(tài)補償,求得航向角為

Ψ=arcsinx+arcsiny

(7)

x,y表達式分別為:

(8)

(9)

式中:α為前一步緊耦合計算出的偏振角;r和p分別為解算后的翻滾角與俯仰角。

1.3 偏振光傳感器與陀螺儀的卡爾曼濾波方法

偏振光傳感器雖然沒有累計誤差,但其對環(huán)境干擾非常靈敏,環(huán)境中的干擾會使解算數(shù)據(jù)有很大的波動,魯棒性較差,因此在融合過程中,基于卡爾曼濾波,利用陀螺儀測量的Z軸角速度作為修正值,可以很大程度上解決環(huán)境噪音干擾的問題,有效提高了偏振光傳感器的魯棒性。卡爾曼濾波過程如圖4所示。

圖4 卡爾曼濾波過程

(10)

(11)

(12)

式中:Qangle為陀螺儀噪聲協(xié)方差;Qbias為偏移噪聲協(xié)方差。

系統(tǒng)的測量方程為Zt=HXt+Vt,其中測量值即為偏振光傳感器解算值,由于偏振光傳感器的噪聲已經(jīng)包含進輸出值中,所以Vt=0,可以得出系統(tǒng)測量參數(shù)H=[1 0]T,根據(jù)卡爾曼增益計算公式,可以求得增益為

(13)

式中Rangle為偏振光傳感器的角度測量噪聲協(xié)方差。

根據(jù)最優(yōu)值計算公式,可求得當前角度與誤差最優(yōu)估計值為:

(14)

(15)

式中measure為偏振光傳感器解算值。

2 多源融合與四足機器人循跡實驗

2.1 多源融合實驗

根據(jù)傳感器融合方法,首先設(shè)計半?yún)^(qū)映射采樣實驗,將偏振光傳感器放在室外平臺上,手動旋轉(zhuǎn)平臺多圈,利用機器人操作系統(tǒng)(robot operating system,ROS)進行數(shù)據(jù)采集并繪制映射前與映射后的偏振角,結(jié)果如圖5所示。圖中虛線為偏振光測量角度,實線為映射后的輸出角度,可以看出2個半?yún)^(qū)被映射到了0～360°的范圍上,并且跳變點也沒有被卡爾曼濾波處理掉。

圖5 半?yún)^(qū)映射結(jié)果

接下來測試組合傳感器的抗環(huán)境光干擾能力,實驗時將傳感器靜止平放置于室外,然后人為的在傳感器上方多次制造光干擾或者遮擋,模擬真實環(huán)境中掠過傳感器上方的物體造成的影響,實驗中卡爾曼濾波器設(shè)置陀螺儀噪聲協(xié)方差Qangle為0.001(°)/h,偏移噪聲協(xié)方差Qbias為0.003(°)/h,卡爾曼增益中,根據(jù)偏振光傳感器參數(shù)設(shè)置角度測量噪聲協(xié)方差Rangle為0.03(°)/h。利用ROS進行數(shù)據(jù)采集并繪制融合前后的角度值,如圖6所示。其中虛線為融合前偏振光傳感器輸出角度值,突變點為人為干擾點,可見融合后的突變大幅減小,角度值趨于穩(wěn)定,標準差較融合前減少了64.8%。融合前后的誤差結(jié)果對比如表1所示。

表1 環(huán)境光干擾誤差對比 (°)

圖6 環(huán)境光干擾輸出對比

接下來測試磁干擾環(huán)境中組合傳感器的魯棒性,靜置組合傳感器并在其周圍設(shè)置移動的干擾磁鐵來模擬環(huán)境中的電磁干擾,利用ROS采集并輸出AHRS解算磁航角以及融合后輸出航向角,將其映射到相同量程中繪制曲線,輸出如圖7所示。由于AHRS對環(huán)境中電磁信息極為敏感,所以在擾動處產(chǎn)生了很大波動,但是多源融合算法以偏振信息為主,磁力計只起到了開機判斷二義性的作用,因此電磁干擾對于輸出基本沒有影響。誤差對比如表2所示。

表2 磁干擾誤差對比 (°)

圖7 磁干擾輸出對比

實驗可見,多源融合算法,基于卡爾曼濾波利用陀螺儀進行了修正,因此環(huán)境中的光遮擋而產(chǎn)生的突變點能夠被平滑處理,并且由于融合方法中以偏振信息為主導,所以能夠抵抗環(huán)境的電磁干擾。實驗表明,組合傳感器在正常進行半?yún)^(qū)轉(zhuǎn)換的同時也有效提高了魯棒性。

2.2 四足機器人循跡實驗

四足機器人實驗平臺采用Unitree A1型號四足機器人,能夠滿足崎嶇路面行走的條件,四足機器人搭載了Nvidia-TX2控制板,提供外部開發(fā)者接口。在機器狗頂部搭載了Xsens-MTi-1慣性傳感器(包含三軸陀螺儀、三軸加速度計、三軸磁力計)進行慣性位姿解算;搭載了偏振光傳感器進行偏振方向解算;搭載無線模塊,與上位機進行遠程連接與信息交互;遙控器用來切換程序控制與手動控制。四足機器人實驗平臺如圖8所示,組成部分各具體參數(shù)如表3所示。

表3 組合導航系統(tǒng)傳感器與平臺參數(shù)信息

圖8 實驗平臺

實驗地點為大連理工大學西部校區(qū)知方樓正門口廣場(121°30′37″E,38°52′49″N)。實驗時間為2022年10月18日16時30分。實驗預設(shè)軌跡為10 m的正方形,四足機器人前進速度設(shè)定為0.6 m/s,轉(zhuǎn)向速度設(shè)定為0.3 rad/s。實驗如圖9所示。

圖9 循跡實驗

本次實驗共設(shè)置了3個實驗組,沿預設(shè)軌跡連續(xù)行走2圈,采用的平面導航方法是基于路徑的航跡推算,其中路徑策略分為直線行進與直角轉(zhuǎn)彎兩部分,直線行進部分采用內(nèi)部路徑積分的方式,規(guī)定了機器人的前進速度與前進時間,用來限定前進距離,在前進過程中,利用PID控制實時修正前進航向,保證直線前進;直角轉(zhuǎn)彎的部分采用航向檢測的方式,檢測到當前航向與轉(zhuǎn)向前航向差值大于90°時完成轉(zhuǎn)彎。實驗中組一使用九軸AHRS解算磁航角作為航向信息,組二使用偏振光傳感器解算的偏振角作為航向信息,組三使用“偏振光+慣性+地磁”多源融合方式?jīng)Q定航向。

本文使用基于視覺的方法進行運動檢測,并繪制可量化的機器人運動軌跡。實驗過程中采用大疆四旋翼無人機,型號為經(jīng)緯M300RTK,進行高空定點航拍。首先對航拍視頻進行預處理,包括調(diào)整分辨率與邊界范圍;之后利用第三方庫openCV識別視頻中運動物體,生成并且顯示該運動物體在視頻當前幀中的坐標與軌跡,視頻運動物體捕捉如圖10所示,該坐標點即為四足機器人相對于視頻邊框的坐標;由于無人機在航拍時存在抖動與鏡頭漂移,所以需要將該坐標轉(zhuǎn)換為相對于地面的絕對坐標,利用實驗過程中擺放在場地3個角的綠色飛盤,使用openCV提取當前幀的飛盤坐標,聯(lián)合記錄的最初幀飛盤坐標計算當前幀的旋轉(zhuǎn)矩陣,然后利用旋轉(zhuǎn)矩陣反算出當前幀中機器人相對地面的絕對坐標;最后利用生成的坐標繪制路徑軌跡,如圖11所示。該方法不僅解決了小環(huán)境下GPS定位誤差大的問題,還相較滴水或者漏沙的方法,量化了路徑軌跡,為誤差分析提供了有效數(shù)據(jù)。

圖10 在航拍中生成的路徑軌跡

(a)磁航角參考算法

誤差計算采用即時定位與地圖構(gòu)建中絕對軌跡誤差(absolute trajectory error,ATE)的計算方法,計算公式如式(16)所示。

(16)

式中:trans為矩陣的求跡計算;N為輸出點個數(shù);Tgt,i為實際軌跡坐標向量,通過坐標輸出文件獲得,每個向量包括時間、x軸坐標與y軸坐標;Tet,i為預設(shè)軌跡坐標向量。

首先根據(jù)預設(shè)方形軌跡,按照實際采集坐標點數(shù),均勻等距離生成正方形軌跡,作為ATE中的估計軌跡,然后將實際軌跡的坐標點依據(jù)情況分為2圈,最后求解每圈ATE,結(jié)果如表4所示。

表4 不同導航方式的誤差對比 m

通過圖11與表4可以得到,傳統(tǒng)慣性+地磁組的兩圈平均誤差為0.547 m,偏振光組為0.421 m,以偏振信息為主的多源融合組為0.273 m。融合組行走預設(shè)路徑的ATE較磁航角參考方式減少了50.1%,較偏振角參考方式減少了35.2%。對于磁航角參考組,由于在試驗場地周圍存在軟磁干擾,磁場分布不是均一的,所以在缺少先驗信息的情況下,航向估計會出現(xiàn)非線性誤差,引起運動偏移預設(shè)軌跡;對于偏振角參考組,雖然偏振光定向方法是無累計誤差的,但是偏振角計算復雜,傳感器解算速率較低,不能修正短時間內(nèi)的方位漂移,因此引起運動偏移預設(shè)軌跡;對于融合組,以偏振角為估計量時,航向參考本身更準確,與陀螺儀緊耦合后,提高了偏振光定向的動態(tài)響應,修正了短時間內(nèi)的方位漂移,同時通過慣性參考對偏振光傳感器進行姿態(tài)的補償,提高了由于四足機器人運動顛簸造成的估計量誤差,路徑精度較前2組都有了提升。由以上實驗可以看出,組合導航系統(tǒng)具有較高的魯棒性,且誤差不隨時間累積、實時性較好。

3 結(jié)束語

為了實現(xiàn)復雜環(huán)境中四足機器人的自主導航,提升導航系統(tǒng)的抗干擾性,本文首先設(shè)計了偏振光傳感器與九軸AHRS的緊耦合算法,然后設(shè)計了偏振光傳感器與陀螺儀的卡爾曼濾波方法,接下來設(shè)計了融合實驗驗證了組合傳感器抗干擾的效果,最后搭建了四足機器人導航平臺完成了循跡實驗。實驗結(jié)果表明:多源緊耦合算法在光干擾下標準差為0.19°,較融合前減小了64.8%,并且不受電磁干擾;四足機器人循跡實驗中組合傳感器組的ATE為0.273 m,ATE較磁航角參考算法減少了50.1%,較偏振角參考算法減少了35.2%。實驗表明該導航算法可用于四足機器人在衛(wèi)星拒止中的自主導航,并且擁有較好的魯棒性。

目前,四足機器人導航平臺僅應用于平坦地面,下一步的工作是對組合傳感器寫入波動補償算法以提升四足機器人在復雜路面環(huán)境下自主導航的適應性,并在山地環(huán)境下進行相關(guān)實驗。