褚金奎,尉 達(dá),張鐘元,于 灝,宋 濤
(大連理工大學(xué)機(jī)械工程學(xué)院,遼寧大連 116024)
隨著科技的發(fā)展,智能移動(dòng)機(jī)器人在服務(wù)業(yè)[1]、制造業(yè)[2]以及星際探索等領(lǐng)域已經(jīng)獲得了廣泛的應(yīng)用。傳統(tǒng)的輪式機(jī)器人在實(shí)際應(yīng)用過程中,越障能力差、地形適應(yīng)能力差、轉(zhuǎn)彎效率低,容易打滑,不夠平穩(wěn),面對復(fù)雜地形時(shí)不能正常工作[3-5]。足式機(jī)器人的使用場景廣泛與復(fù)雜,幾乎可以適應(yīng)各種地形,能夠跨越障礙,有著良好的自由度,同時(shí)動(dòng)作靈活、自如、穩(wěn)定,在環(huán)境探索、災(zāi)害搜救中起到至關(guān)重要的作用[6-7]。然而目前絕大多數(shù)的四足仿生機(jī)器人都不攜帶自主導(dǎo)航系統(tǒng),完全由人工實(shí)現(xiàn)遠(yuǎn)程操控,但隨著機(jī)器人工作多樣化,操作復(fù)雜化,解決這一問題變得重要起來。航姿參考系統(tǒng)(attitude and heading reference system,AHRS)是以電子羅盤修正陀螺儀角速度積分從而得到航向角,然而電子羅盤易受到磁場干擾,同時(shí)陀螺儀的零漂誤差會(huì)隨時(shí)間積累[8],所以在衛(wèi)星拒止,電場干擾以及需要長時(shí)間運(yùn)行的復(fù)雜環(huán)境中難以獲得精確穩(wěn)定的航向信息,魯棒性較差[9]。
偏振光導(dǎo)航[10]是一種新興導(dǎo)航方式,借鑒昆蟲利用天空偏振光進(jìn)行導(dǎo)航[11]的思路發(fā)展而來,具有無誤差積累、實(shí)時(shí)性好的特點(diǎn)。因此,將偏振光傳感器與其他傳感器相結(jié)合來提高導(dǎo)航性能,受到國內(nèi)外學(xué)者的關(guān)注。2009年,王洪青[12]搭建了輪式自主移動(dòng)機(jī)器人平臺,并且采用偏振光傳感器實(shí)現(xiàn)其室外導(dǎo)航功能;2011年,Chahl等[13]根據(jù)蜻蜓復(fù)眼結(jié)構(gòu)設(shè)計(jì)了天空偏振羅盤,并在無人機(jī)飛行測試中利用羅盤保持航向;2015年,支煒等[14]提出了一種偏振光及紅外傳感器輔助慣導(dǎo)定姿方法,利用偏振光傳感器與紅外傳感器在慣性導(dǎo)航系統(tǒng)中進(jìn)行修正位置與姿態(tài)信息解算,并且依據(jù)常見的慣導(dǎo)系統(tǒng)參數(shù)進(jìn)行了動(dòng)態(tài)仿真;2019年,Dupeyroux等[15]基于紫外光線性偏振設(shè)計(jì)了天體羅盤,并利用羅盤在六足步行機(jī)器人的直線行走實(shí)驗(yàn)上保持航向恒定;2020年,褚金奎等[16]實(shí)現(xiàn)了偏振光傳感器在實(shí)際三維空間中的導(dǎo)航應(yīng)用,并利用傳感器完成了四旋翼飛行器的導(dǎo)航控制;同年褚金奎等[17]設(shè)計(jì)了一種偏振光傳感器,微慣性測量單元及全球定位系統(tǒng)(global positioning system,GPS)的組合導(dǎo)航系統(tǒng),并且實(shí)現(xiàn)無人船的自主導(dǎo)航與編隊(duì)航行。
上述研究中,其導(dǎo)航方法利用純偏振光獲得航向信息,或采用松耦合方式,分別處理偏振光傳感器與其他傳感器數(shù)據(jù)后再做置信融合,并未將其優(yōu)勢完全發(fā)揮出來。偏振光傳感器具有瞬時(shí)性,不存在累計(jì)誤差,但探測范圍存在2個(gè)半?yún)^(qū),設(shè)備啟動(dòng)工作時(shí)不能靠自身判斷出當(dāng)前所在半?yún)^(qū),而存在于慣性系統(tǒng)中的磁力計(jì)可以解決這個(gè)問題,同時(shí)偏振光傳感器要正對天頂使用,對于近距離掠過其上方的飛行物感應(yīng)較強(qiáng)烈,魯棒性較差,易被環(huán)境干擾。2種導(dǎo)航系統(tǒng)各有一定的優(yōu)缺點(diǎn),利用各自傳感器的原始信息數(shù)據(jù),將兩者緊耦合,以偏振光信息為主,慣性測量單元(inertial measurement unit,IMU)作為輔助,可以彌補(bǔ)各自的不足并發(fā)揮自身的優(yōu)越性。
本文為了實(shí)現(xiàn)四足機(jī)器人在衛(wèi)星拒止環(huán)境中的自主導(dǎo)航,并提升導(dǎo)航系統(tǒng)的抗干擾性。首先設(shè)計(jì)了偏振光傳感器與九軸AHRS的緊耦合導(dǎo)航算法;然后,設(shè)計(jì)實(shí)驗(yàn)驗(yàn)證了該導(dǎo)航算法的抗環(huán)境干擾能力;最后,基于上述組合導(dǎo)航算法,與傳統(tǒng)地磁慣性和偏振光導(dǎo)航方式進(jìn)行了四足機(jī)器人的循跡對比實(shí)驗(yàn),實(shí)驗(yàn)表明組合導(dǎo)航算法在光干擾和電磁環(huán)境干擾的情況下可以正常工作,并且循跡路徑精度得到有效提升,驗(yàn)證了組合導(dǎo)航系統(tǒng)可以實(shí)現(xiàn)衛(wèi)星拒止環(huán)境中的四足機(jī)器人自主導(dǎo)航。
太陽光在傳播過程中遇到大氣中的各種粒子時(shí)會(huì)發(fā)生散射現(xiàn)象,在天空形成穩(wěn)定的偏振分布模式,利用大氣瑞利散射理論模型,可以建立全天空在晴朗無云條件下的理想大氣偏振模式。在某一時(shí)刻天空中任意一點(diǎn)光偏振方向即E矢量總是垂直于由太陽S、觀測點(diǎn)O和被觀測點(diǎn)P組成的平面。倘若處于地面O點(diǎn)的觀察者可以探測出天空某點(diǎn)的E矢量方向與自身方向的幾何關(guān)系,就可以確定出自身的方向。
本文選用東北天坐標(biāo)系為導(dǎo)航坐標(biāo)系,偏振光傳感器固聯(lián)于載體坐標(biāo)系為右前上坐標(biāo)系(m),圖1中Hs為太陽高度角,As為太陽方位角,E矢量為傳感器觀測點(diǎn)處光偏振方向,α為觀測點(diǎn)處光偏振方向與傳感器體軸夾角。
圖1 偏振光導(dǎo)航原理圖
(1)
(2)
太陽視線在導(dǎo)航坐標(biāo)系下的投影為
(3)
(4)
多源緊耦合算法設(shè)計(jì)如圖2所示,包括偏振光傳感器、陀螺儀、加速度計(jì)和磁力計(jì)。由于路徑導(dǎo)航主要依賴于航向角以及位置信息,偏振光傳感器提供的航向角魯棒性差,對環(huán)境干擾敏感,基于卡爾曼濾波,提取各傳感器原始數(shù)據(jù),利用陀螺儀輸出角速度進(jìn)行角度預(yù)測與補(bǔ)償,能夠大幅提高其魯棒性,在實(shí)際使用過程中,如果不能正對天頂會(huì)影響偏振光傳感器的測量精度。根據(jù)Sabatelli等[18]提出的基于擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)的IMU姿態(tài)角解算方法,計(jì)算出當(dāng)前的翻滾角r、俯仰角p和磁航角y,然后根據(jù)Zhi等[19]提出的一種偏振光傳感器姿態(tài)補(bǔ)償算法進(jìn)行航向修正,該方法可有效提高傳感器的角度輸出精度。磁航角可以在設(shè)備開機(jī)時(shí)幫助修正偏振光傳感器的二義性:當(dāng)輸出值為0~1時(shí),置偏振光傳感器為上半?yún)^(qū)0~180°;輸出值為-1~0時(shí),置偏振光傳感器為下半?yún)^(qū)180°~360°。
圖2 多源緊耦合導(dǎo)航算法模型流程
在進(jìn)行耦合時(shí)要解決偏振光傳感器的半?yún)^(qū)映射問題:偏振光傳感器只能輸出0~180°的數(shù)據(jù),所以在對應(yīng)全方向上存在2個(gè)半?yún)^(qū);相反的,陀螺儀的Z軸輸出是全向變化的。所以需要將偏振光傳感器數(shù)據(jù)對應(yīng)映射到陀螺儀中。并且卡爾曼濾波的主要作用是去除跳變點(diǎn),但是在傳感器進(jìn)行半?yún)^(qū)轉(zhuǎn)換時(shí),存在正常的跳變點(diǎn),這種跳變點(diǎn)并不是由于環(huán)境干擾而產(chǎn)生的,所以相應(yīng)的卡爾曼濾波不能將這種跳變點(diǎn)進(jìn)行過濾。為了解決這些問題,首先設(shè)置了半?yún)^(qū)符號用來識別當(dāng)前半?yún)^(qū),接下來規(guī)定了有效卡爾曼濾波區(qū)間,其中濾波范圍為10°~170°和190°~350°,其余范圍是入跳變檢測區(qū)域,當(dāng)進(jìn)入跳變檢測區(qū)域時(shí),關(guān)閉卡爾曼濾波,直接采用測量值,濾波范圍可以隨著采樣頻率的增加而擴(kuò)大。映射流程如圖3所示,過程中初始化卡爾曼濾波參數(shù)的目的在于半?yún)^(qū)轉(zhuǎn)換后重新進(jìn)入濾波范圍時(shí),濾波器的中間參數(shù)還停留在離開濾波范圍的時(shí)刻,初始化可以防止中間參數(shù)對于新的濾波結(jié)果造成影響。
圖3 半?yún)^(qū)映射流程
在實(shí)際應(yīng)用過程中,偏振光傳感器通常與載體固聯(lián)安裝,并不會(huì)始終保持水平,利用AHRS解算的姿態(tài)角進(jìn)行偏振角補(bǔ)償,之后利用外部的UTC時(shí)鐘以及經(jīng)緯度信息得到太陽高度角以及太陽方位角進(jìn)行航向角求解。首先計(jì)算太陽信息:
sinHs=sinφ·sinδ+cosφ·cosδ·cost
(5)
cosAs=(sinHs·sinφ-sinδ)/(cosHs·cosφ)
(6)
式中:φ為地理緯度;δ為太陽赤緯;t為時(shí)角。
之后進(jìn)行姿態(tài)補(bǔ)償,求得航向角為
Ψ=arcsinx+arcsiny
(7)
x,y表達(dá)式分別為:
(8)
(9)
式中:α為前一步緊耦合計(jì)算出的偏振角;r和p分別為解算后的翻滾角與俯仰角。
偏振光傳感器雖然沒有累計(jì)誤差,但其對環(huán)境干擾非常靈敏,環(huán)境中的干擾會(huì)使解算數(shù)據(jù)有很大的波動(dòng),魯棒性較差,因此在融合過程中,基于卡爾曼濾波,利用陀螺儀測量的Z軸角速度作為修正值,可以很大程度上解決環(huán)境噪音干擾的問題,有效提高了偏振光傳感器的魯棒性??柭鼮V波過程如圖4所示。
圖4 卡爾曼濾波過程
(10)
(11)
(12)
式中:Qangle為陀螺儀噪聲協(xié)方差;Qbias為偏移噪聲協(xié)方差。
系統(tǒng)的測量方程為Zt=HXt+Vt,其中測量值即為偏振光傳感器解算值,由于偏振光傳感器的噪聲已經(jīng)包含進(jìn)輸出值中,所以Vt=0,可以得出系統(tǒng)測量參數(shù)H=[1 0]T,根據(jù)卡爾曼增益計(jì)算公式,可以求得增益為
(13)
式中Rangle為偏振光傳感器的角度測量噪聲協(xié)方差。
根據(jù)最優(yōu)值計(jì)算公式,可求得當(dāng)前角度與誤差最優(yōu)估計(jì)值為:
(14)
(15)
式中measure為偏振光傳感器解算值。
根據(jù)傳感器融合方法,首先設(shè)計(jì)半?yún)^(qū)映射采樣實(shí)驗(yàn),將偏振光傳感器放在室外平臺上,手動(dòng)旋轉(zhuǎn)平臺多圈,利用機(jī)器人操作系統(tǒng)(robot operating system,ROS)進(jìn)行數(shù)據(jù)采集并繪制映射前與映射后的偏振角,結(jié)果如圖5所示。圖中虛線為偏振光測量角度,實(shí)線為映射后的輸出角度,可以看出2個(gè)半?yún)^(qū)被映射到了0~360°的范圍上,并且跳變點(diǎn)也沒有被卡爾曼濾波處理掉。
圖5 半?yún)^(qū)映射結(jié)果
接下來測試組合傳感器的抗環(huán)境光干擾能力,實(shí)驗(yàn)時(shí)將傳感器靜止平放置于室外,然后人為的在傳感器上方多次制造光干擾或者遮擋,模擬真實(shí)環(huán)境中掠過傳感器上方的物體造成的影響,實(shí)驗(yàn)中卡爾曼濾波器設(shè)置陀螺儀噪聲協(xié)方差Qangle為0.001(°)/h,偏移噪聲協(xié)方差Qbias為0.003(°)/h,卡爾曼增益中,根據(jù)偏振光傳感器參數(shù)設(shè)置角度測量噪聲協(xié)方差Rangle為0.03(°)/h。利用ROS進(jìn)行數(shù)據(jù)采集并繪制融合前后的角度值,如圖6所示。其中虛線為融合前偏振光傳感器輸出角度值,突變點(diǎn)為人為干擾點(diǎn),可見融合后的突變大幅減小,角度值趨于穩(wěn)定,標(biāo)準(zhǔn)差較融合前減少了64.8%。融合前后的誤差結(jié)果對比如表1所示。
表1 環(huán)境光干擾誤差對比 (°)
圖6 環(huán)境光干擾輸出對比
接下來測試磁干擾環(huán)境中組合傳感器的魯棒性,靜置組合傳感器并在其周圍設(shè)置移動(dòng)的干擾磁鐵來模擬環(huán)境中的電磁干擾,利用ROS采集并輸出AHRS解算磁航角以及融合后輸出航向角,將其映射到相同量程中繪制曲線,輸出如圖7所示。由于AHRS對環(huán)境中電磁信息極為敏感,所以在擾動(dòng)處產(chǎn)生了很大波動(dòng),但是多源融合算法以偏振信息為主,磁力計(jì)只起到了開機(jī)判斷二義性的作用,因此電磁干擾對于輸出基本沒有影響。誤差對比如表2所示。
表2 磁干擾誤差對比 (°)
圖7 磁干擾輸出對比
實(shí)驗(yàn)可見,多源融合算法,基于卡爾曼濾波利用陀螺儀進(jìn)行了修正,因此環(huán)境中的光遮擋而產(chǎn)生的突變點(diǎn)能夠被平滑處理,并且由于融合方法中以偏振信息為主導(dǎo),所以能夠抵抗環(huán)境的電磁干擾。實(shí)驗(yàn)表明,組合傳感器在正常進(jìn)行半?yún)^(qū)轉(zhuǎn)換的同時(shí)也有效提高了魯棒性。
四足機(jī)器人實(shí)驗(yàn)平臺采用Unitree A1型號四足機(jī)器人,能夠滿足崎嶇路面行走的條件,四足機(jī)器人搭載了Nvidia-TX2控制板,提供外部開發(fā)者接口。在機(jī)器狗頂部搭載了Xsens-MTi-1慣性傳感器(包含三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì))進(jìn)行慣性位姿解算;搭載了偏振光傳感器進(jìn)行偏振方向解算;搭載無線模塊,與上位機(jī)進(jìn)行遠(yuǎn)程連接與信息交互;遙控器用來切換程序控制與手動(dòng)控制。四足機(jī)器人實(shí)驗(yàn)平臺如圖8所示,組成部分各具體參數(shù)如表3所示。
表3 組合導(dǎo)航系統(tǒng)傳感器與平臺參數(shù)信息
圖8 實(shí)驗(yàn)平臺
實(shí)驗(yàn)地點(diǎn)為大連理工大學(xué)西部校區(qū)知方樓正門口廣場(121°30′37″E,38°52′49″N)。實(shí)驗(yàn)時(shí)間為2022年10月18日16時(shí)30分。實(shí)驗(yàn)預(yù)設(shè)軌跡為10 m的正方形,四足機(jī)器人前進(jìn)速度設(shè)定為0.6 m/s,轉(zhuǎn)向速度設(shè)定為0.3 rad/s。實(shí)驗(yàn)如圖9所示。
圖9 循跡實(shí)驗(yàn)
本次實(shí)驗(yàn)共設(shè)置了3個(gè)實(shí)驗(yàn)組,沿預(yù)設(shè)軌跡連續(xù)行走2圈,采用的平面導(dǎo)航方法是基于路徑的航跡推算,其中路徑策略分為直線行進(jìn)與直角轉(zhuǎn)彎兩部分,直線行進(jìn)部分采用內(nèi)部路徑積分的方式,規(guī)定了機(jī)器人的前進(jìn)速度與前進(jìn)時(shí)間,用來限定前進(jìn)距離,在前進(jìn)過程中,利用PID控制實(shí)時(shí)修正前進(jìn)航向,保證直線前進(jìn);直角轉(zhuǎn)彎的部分采用航向檢測的方式,檢測到當(dāng)前航向與轉(zhuǎn)向前航向差值大于90°時(shí)完成轉(zhuǎn)彎。實(shí)驗(yàn)中組一使用九軸AHRS解算磁航角作為航向信息,組二使用偏振光傳感器解算的偏振角作為航向信息,組三使用“偏振光+慣性+地磁”多源融合方式?jīng)Q定航向。
本文使用基于視覺的方法進(jìn)行運(yùn)動(dòng)檢測,并繪制可量化的機(jī)器人運(yùn)動(dòng)軌跡。實(shí)驗(yàn)過程中采用大疆四旋翼無人機(jī),型號為經(jīng)緯M300RTK,進(jìn)行高空定點(diǎn)航拍。首先對航拍視頻進(jìn)行預(yù)處理,包括調(diào)整分辨率與邊界范圍;之后利用第三方庫openCV識別視頻中運(yùn)動(dòng)物體,生成并且顯示該運(yùn)動(dòng)物體在視頻當(dāng)前幀中的坐標(biāo)與軌跡,視頻運(yùn)動(dòng)物體捕捉如圖10所示,該坐標(biāo)點(diǎn)即為四足機(jī)器人相對于視頻邊框的坐標(biāo);由于無人機(jī)在航拍時(shí)存在抖動(dòng)與鏡頭漂移,所以需要將該坐標(biāo)轉(zhuǎn)換為相對于地面的絕對坐標(biāo),利用實(shí)驗(yàn)過程中擺放在場地3個(gè)角的綠色飛盤,使用openCV提取當(dāng)前幀的飛盤坐標(biāo),聯(lián)合記錄的最初幀飛盤坐標(biāo)計(jì)算當(dāng)前幀的旋轉(zhuǎn)矩陣,然后利用旋轉(zhuǎn)矩陣反算出當(dāng)前幀中機(jī)器人相對地面的絕對坐標(biāo);最后利用生成的坐標(biāo)繪制路徑軌跡,如圖11所示。該方法不僅解決了小環(huán)境下GPS定位誤差大的問題,還相較滴水或者漏沙的方法,量化了路徑軌跡,為誤差分析提供了有效數(shù)據(jù)。
圖10 在航拍中生成的路徑軌跡
(a)磁航角參考算法
誤差計(jì)算采用即時(shí)定位與地圖構(gòu)建中絕對軌跡誤差(absolute trajectory error,ATE)的計(jì)算方法,計(jì)算公式如式(16)所示。
(16)
式中:trans為矩陣的求跡計(jì)算;N為輸出點(diǎn)個(gè)數(shù);Tgt,i為實(shí)際軌跡坐標(biāo)向量,通過坐標(biāo)輸出文件獲得,每個(gè)向量包括時(shí)間、x軸坐標(biāo)與y軸坐標(biāo);Tet,i為預(yù)設(shè)軌跡坐標(biāo)向量。
首先根據(jù)預(yù)設(shè)方形軌跡,按照實(shí)際采集坐標(biāo)點(diǎn)數(shù),均勻等距離生成正方形軌跡,作為ATE中的估計(jì)軌跡,然后將實(shí)際軌跡的坐標(biāo)點(diǎn)依據(jù)情況分為2圈,最后求解每圈ATE,結(jié)果如表4所示。
表4 不同導(dǎo)航方式的誤差對比 m
通過圖11與表4可以得到,傳統(tǒng)慣性+地磁組的兩圈平均誤差為0.547 m,偏振光組為0.421 m,以偏振信息為主的多源融合組為0.273 m。融合組行走預(yù)設(shè)路徑的ATE較磁航角參考方式減少了50.1%,較偏振角參考方式減少了35.2%。對于磁航角參考組,由于在試驗(yàn)場地周圍存在軟磁干擾,磁場分布不是均一的,所以在缺少先驗(yàn)信息的情況下,航向估計(jì)會(huì)出現(xiàn)非線性誤差,引起運(yùn)動(dòng)偏移預(yù)設(shè)軌跡;對于偏振角參考組,雖然偏振光定向方法是無累計(jì)誤差的,但是偏振角計(jì)算復(fù)雜,傳感器解算速率較低,不能修正短時(shí)間內(nèi)的方位漂移,因此引起運(yùn)動(dòng)偏移預(yù)設(shè)軌跡;對于融合組,以偏振角為估計(jì)量時(shí),航向參考本身更準(zhǔn)確,與陀螺儀緊耦合后,提高了偏振光定向的動(dòng)態(tài)響應(yīng),修正了短時(shí)間內(nèi)的方位漂移,同時(shí)通過慣性參考對偏振光傳感器進(jìn)行姿態(tài)的補(bǔ)償,提高了由于四足機(jī)器人運(yùn)動(dòng)顛簸造成的估計(jì)量誤差,路徑精度較前2組都有了提升。由以上實(shí)驗(yàn)可以看出,組合導(dǎo)航系統(tǒng)具有較高的魯棒性,且誤差不隨時(shí)間累積、實(shí)時(shí)性較好。
為了實(shí)現(xiàn)復(fù)雜環(huán)境中四足機(jī)器人的自主導(dǎo)航,提升導(dǎo)航系統(tǒng)的抗干擾性,本文首先設(shè)計(jì)了偏振光傳感器與九軸AHRS的緊耦合算法,然后設(shè)計(jì)了偏振光傳感器與陀螺儀的卡爾曼濾波方法,接下來設(shè)計(jì)了融合實(shí)驗(yàn)驗(yàn)證了組合傳感器抗干擾的效果,最后搭建了四足機(jī)器人導(dǎo)航平臺完成了循跡實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明:多源緊耦合算法在光干擾下標(biāo)準(zhǔn)差為0.19°,較融合前減小了64.8%,并且不受電磁干擾;四足機(jī)器人循跡實(shí)驗(yàn)中組合傳感器組的ATE為0.273 m,ATE較磁航角參考算法減少了50.1%,較偏振角參考算法減少了35.2%。實(shí)驗(yàn)表明該導(dǎo)航算法可用于四足機(jī)器人在衛(wèi)星拒止中的自主導(dǎo)航,并且擁有較好的魯棒性。
目前,四足機(jī)器人導(dǎo)航平臺僅應(yīng)用于平坦地面,下一步的工作是對組合傳感器寫入波動(dòng)補(bǔ)償算法以提升四足機(jī)器人在復(fù)雜路面環(huán)境下自主導(dǎo)航的適應(yīng)性,并在山地環(huán)境下進(jìn)行相關(guān)實(shí)驗(yàn)。