分體式飛行汽車全自主對接導(dǎo)引系統(tǒng)設(shè)計(jì)與驗(yàn)證

王琛，林威，胡良鵬，張駿銘

(1.長安大學(xué) 工程機(jī)械學(xué)院，陜西 西安 710064；2.中國兵器工業(yè)試驗(yàn)測試研究院技術(shù)中心，陜西 西安 710043)

分體式飛行汽車憑借其模塊化設(shè)計(jì)、便捷式運(yùn)行模態(tài)切換的特點(diǎn)成為未來城市間立體化交通的重要組成部分，將在緩解城市交通運(yùn)輸壓力、減少出行時間方面發(fā)揮巨大作用[1].分體式飛行汽車共包含3 個部分：飛行模塊、乘客艙、地面行駛模塊，其中飛行模塊與乘客艙結(jié)合組成飛行模態(tài)，地面行駛模塊與乘客艙結(jié)合組成地面行駛模態(tài).飛行模態(tài)與地面行駛模態(tài)之間的全自主轉(zhuǎn)換依靠對接導(dǎo)引系統(tǒng)（分體式飛行汽車的關(guān)鍵技術(shù)之一），全自主對接導(dǎo)引系統(tǒng)的主要技術(shù)難點(diǎn)在于全自主、全天候、高精度和高可靠性的要求.

對接導(dǎo)引系統(tǒng)廣泛應(yīng)用于航天器交會對接、水下自主機(jī)器人(autonomous underwater vehicle,AUV）回收、無人機(jī)精準(zhǔn)導(dǎo)引降落等任務(wù)領(lǐng)域.基于差分衛(wèi)星定位導(dǎo)航、視覺導(dǎo)引以及雷達(dá)測距的航天器對接導(dǎo)引系統(tǒng)[2-5]在自主交會對接中表現(xiàn)出良好的性能，電磁對接[6]能夠在不消耗航天器推進(jìn)劑的情況下實(shí)現(xiàn)自主對接.水下AUV 對接導(dǎo)引系統(tǒng)常用的定位傳感器主要包含聲學(xué)、光學(xué)、電磁等類型[7-11].在無人機(jī)精準(zhǔn)導(dǎo)引降落技術(shù)領(lǐng)域，常用的技術(shù)方案包括全球定位系統(tǒng)(global positioning system, GPS )[12-13]、視覺[14-18]、激光雷達(dá)[19-20]、超寬帶(ultra wide band，UWB)[21-22]等.Cui 等[14]利用快速特征提取結(jié)合基于網(wǎng)格的運(yùn)動統(tǒng)計(jì)策略提出改進(jìn)的雙目定位與建圖系統(tǒng)，在仿真試驗(yàn)中成功完成4.8 m×1.2 m 跑道上的降落任務(wù).Cheng等[22]設(shè)計(jì)基于UWB 視覺傳感器的自主動態(tài)導(dǎo)航方法，提出在GPS 失效環(huán)境下實(shí)現(xiàn)無人機(jī)對接移動目標(biāo)的三階段方法，但是該方法通信距離短且設(shè)備成本較高.Marcon 等[23]成功進(jìn)行多次基于實(shí)時動態(tài)差分全球定位系統(tǒng)(real time kinematic global positioning system, RTK-GPS)模塊的飛行器導(dǎo)引降落試驗(yàn)，降落偏差均值為12.4 cm.Krogius 等[24]提出靶標(biāo)系統(tǒng)識別雙重嵌套的AprilTag 標(biāo)識，實(shí)現(xiàn)0.08～16.15 m 高度范圍的無人機(jī)導(dǎo)引降落，由于其標(biāo)識尺寸為90.2 cm×90.2 cm，無法適用于窄小平臺的自主導(dǎo)引降落任務(wù).Kim 等[25]在垂直方向上通過評估每個分割段的空間接近度對激光雷達(dá)的點(diǎn)云數(shù)據(jù)進(jìn)行聚類，成功定位到無人機(jī)點(diǎn)云集群，完成無人機(jī)在移動平臺上的導(dǎo)引降落；但是激光雷達(dá)通常占用的空間較大且成本較高，對機(jī)載設(shè)備的處理性能也有一定要求.

由于對接環(huán)境、容許位姿誤差以及可能存在的外界干擾等因素影響，導(dǎo)致航天器交會以及水下AUV 回收工程無法直接應(yīng)用.現(xiàn)有的無人機(jī)導(dǎo)引降落技術(shù)存在如下問題，不能直接適應(yīng)分體式飛行汽車模態(tài)轉(zhuǎn)換任務(wù)：1)位置精度不足，均超過10 cm；2)合作標(biāo)識尺寸大；3)導(dǎo)引方式魯棒性弱；4)缺乏考慮環(huán)境擾動，如光照環(huán)境、陣風(fēng)帶來的無人機(jī)姿態(tài)對視覺傳感器測量精度的影響；5)導(dǎo)引降落目標(biāo)為平面.針對上述分體式飛行汽車模塊間導(dǎo)引的技術(shù)挑戰(zhàn)，本研究圍繞系統(tǒng)流程設(shè)計(jì)、軟硬件系統(tǒng)架構(gòu)、核心導(dǎo)引算法以及試驗(yàn)驗(yàn)證開展.

1 對接導(dǎo)引系統(tǒng)設(shè)計(jì)

1.1 設(shè)計(jì)要求

如圖1 所示，分體式飛行汽車通過對接導(dǎo)引系統(tǒng)完成飛行模態(tài)與行駛模態(tài)之間的轉(zhuǎn)換，其模態(tài)轉(zhuǎn)換過程主要包含飛行模塊與乘客艙頂部和乘客艙底部與地面行駛模塊之間的對接導(dǎo)引和鎖定與分離.

圖1 分體式飛行汽車的組成Fig.1 Composition of modular flying vehicle

根據(jù)分體式飛行汽車的運(yùn)行場景和功能，設(shè)計(jì)滿足以下3 項(xiàng)要求的對接導(dǎo)引系統(tǒng).1）環(huán)境適應(yīng)性：模態(tài)轉(zhuǎn)換時，在光照變化、風(fēng)力引起的機(jī)體姿態(tài)以及GPS 信號擾動等外界因素綜合作用下，對接導(dǎo)引系統(tǒng)能夠準(zhǔn)確融合出光順的模塊間的相對位姿偏差，引導(dǎo)飛控系統(tǒng)進(jìn)行位姿校正.2）導(dǎo)引精度：導(dǎo)引系統(tǒng)末段的位置偏差不超過1 cm，角度偏差不超過1°；導(dǎo)引與飛行控制系統(tǒng)結(jié)合后的位置控制偏差不超過6 cm，角度偏差不超過5°.3）應(yīng)用性：受限于系統(tǒng)空間，對接導(dǎo)引系統(tǒng)的定位傳感器應(yīng)具有體積小、輕量化的特點(diǎn)，在不干涉其他對接機(jī)構(gòu)正常運(yùn)作的條件下，定位傳感器及其合作標(biāo)識的布置不影響整體的工業(yè)美觀性.

1.2 對接導(dǎo)引系統(tǒng)流程設(shè)計(jì)

分體式飛行汽車的導(dǎo)引系統(tǒng)分為2 個部分，分別完成飛行單元-座艙以及座艙-地面行駛模塊之間的相對導(dǎo)引任務(wù).由于這2 個部分的流程和功能相似，采用相同的導(dǎo)引方案.整個對接導(dǎo)引任務(wù)按不同的導(dǎo)引方式可以劃分為3 個導(dǎo)引階段：遠(yuǎn)程、中程、近程.根據(jù)不同導(dǎo)引階段所用的定位方式的有效檢測范圍進(jìn)行導(dǎo)引高度的劃分，其中相對高度超過10 m 的為遠(yuǎn)程導(dǎo)引階段，3～10 m的為中程導(dǎo)引階段，不超過3 m 的為近程導(dǎo)引階段，導(dǎo)引流程如圖2 所示.圖中，r1為地面行駛模塊初始偏航角，r2為飛行模塊偏航初始角，r1′為地面行駛模塊最終偏航角，r2′為飛行模塊最終偏航角，x、y為橫、縱向偏差，z為高度偏差，θ 為航向角偏差.采用RTK-GPS 以及視覺傳感器融合方式完成分體式飛行汽車模塊間的相對位姿測量.其中RTK-GPS作用于全流程的對接導(dǎo)引，提供分米級的模塊間的定位信息.在視覺中、近程導(dǎo)引階段，以視覺傳感器的目標(biāo)識別定位信息為主，RTK-GPS 的測量定位信息為輔，在視覺中程導(dǎo)引階段使用YOLOv4-tiny 目標(biāo)檢測算法識別待對接目標(biāo)的車體外形輪廓信息，實(shí)現(xiàn)待對接目標(biāo)在像素平面內(nèi)的二維定位；在視覺近程導(dǎo)引階段，多級視覺基準(zhǔn)庫AprilTag算法提供厘米級的模塊間的相對定位結(jié)果.導(dǎo)引方式切換時，使用數(shù)據(jù)融合的方式，將RTK-GPS數(shù)據(jù)與視覺數(shù)據(jù)融合，防止出現(xiàn)因?qū)б绞角袚Q產(chǎn)生的數(shù)據(jù)不平滑的現(xiàn)象，光順平滑的融合數(shù)據(jù)也避免了瞬時的異常數(shù)據(jù)沖擊以及飛行姿態(tài)突變的問題產(chǎn)生.

圖2 飛行模塊導(dǎo)引降落流程Fig.2 Flight module guidance landing process

1.3 硬件系統(tǒng)架構(gòu)

為了使分體式飛行汽車對接導(dǎo)引硬件系統(tǒng)在滿足高精確度、高可靠、小體積、輕量化的設(shè)計(jì)需求同時，適用于分體式飛行汽車全流程模態(tài)轉(zhuǎn)換的對接導(dǎo)引過程，設(shè)計(jì)該硬件系統(tǒng)為3 個部分，如圖3 所示.1）通信子系統(tǒng).采用雙中央處理器Wi-Fi通信方案實(shí)現(xiàn)對接流程中各個模塊處理器之間的互相通信，數(shù)據(jù)主要包括：飛行模塊與地面行駛模塊的RTK-GPS 位姿測量數(shù)據(jù)、視覺目標(biāo)檢測定位數(shù)據(jù)、對接導(dǎo)引系統(tǒng)的當(dāng)前狀態(tài)等.2）數(shù)據(jù)處理子系統(tǒng).為了保證對接導(dǎo)引系統(tǒng)硬件的高速處理能力和可靠性，滿足體積小、輕質(zhì)量的要求，數(shù)據(jù)處理系統(tǒng)選用NVIDIA Jetson Xavier NX 模塊來處理視覺信息，并將信息通過串口通信及Wi-Fi通信方式傳輸至特定組件.3）信息采集子系統(tǒng).主要包括2 個部分：RTK-GPS 傳感器和視覺傳感器.RTK-GPS 天空端與地面端分別安裝在飛行模塊與地面行駛模塊上，用于獲取模塊間的RTKGPS 定位定向信息.視覺傳感器選擇全局快門單目相機(jī)，采用經(jīng)過適當(dāng)減震處理后與機(jī)身固連的方式安裝在飛行模塊和乘客艙底部.全局快門單目相機(jī)可以避免飛行狀態(tài)下機(jī)身高頻振動對圖像數(shù)據(jù)獲取的影響.視覺標(biāo)志物在乘客艙、地面行駛模塊頂部分別布置.照明單元分別布置在飛行模塊與乘客艙底部，當(dāng)環(huán)境光過低時會自主打開提高場景照度，輔助視覺模塊進(jìn)行圖像信息采集.

圖3 分體式飛行汽車的對接導(dǎo)引硬件系統(tǒng)Fig.3 Docking guidance hardware system of modular flying vehicle

2 對接導(dǎo)引系統(tǒng)核心算法

飛行汽車的對接導(dǎo)引系統(tǒng)在工程實(shí)際應(yīng)用中面臨的挑戰(zhàn)主要有以下4 個方面.1)深度學(xué)習(xí)目標(biāo)檢測框架YOLOv4-tiny 在實(shí)際使用時會產(chǎn)生錯誤檢測幀，包含誤檢和漏檢的情況；與飛行模塊固定連接的相機(jī)在檢測標(biāo)志AprilTag 時，位姿解算精度會被飛行模塊姿態(tài)影響，即隨著觀察角度增大而降低.2)進(jìn)行全天候?qū)б鞒虝r，夜晚低照度環(huán)境會影響視覺模塊的識別精度；3)視覺模塊識別位姿的數(shù)據(jù)流連續(xù)性稍差，無法輸出穩(wěn)定的數(shù)據(jù)流，只依賴RTK-GPS 導(dǎo)引不僅存在定位精度不足問題，而且信號容易受到環(huán)境因素的影響，須融合2 個傳感器的測量數(shù)據(jù).4)在近程導(dǎo)引階段，AprilTag 測量精度會隨著高度逐漸降低而提高，但是在高度較高時誤差較大，須進(jìn)行融合方式的算法修正.針對上述挑戰(zhàn)，本研究文提出以下算法.

2.1 基于密度聚類和核相關(guān)濾波算法的錯、漏檢補(bǔ)償算法及姿態(tài)補(bǔ)償算法

在中程導(dǎo)引階段，利用YOLOv4-tiny 算法識別出待對接目標(biāo)的車體外形輪廓信息，以返回待對接目標(biāo)在像素平面內(nèi)的坐標(biāo)信息.針對YOLOv4-tiny 算法在目標(biāo)檢測過程中極容易出現(xiàn)目標(biāo)誤檢、漏檢的現(xiàn)象，采用基于滑動窗口的目標(biāo)檢測點(diǎn)密度聚類方法篩查出YOLOv4-tiny 算法出現(xiàn)的目標(biāo)誤檢或漏檢幀，再使用核相關(guān)濾波算法(kernel correlation filter，KCF)對目標(biāo)誤檢或漏檢幀進(jìn)行修正，最終輸出待對接目標(biāo)在像素坐標(biāo)系下連續(xù)穩(wěn)定、準(zhǔn)確的檢測信息流.設(shè)YOLOv4-tiny 算法識別出的目標(biāo)中心點(diǎn)坐標(biāo)為 (xc,yc) ，在本研究的自主導(dǎo)引降落場景中，共有i類物體類別，定義YOLOv4-tiny算法正確檢測到目標(biāo)、將其他物體檢測成目標(biāo)的點(diǎn)坐標(biāo)為 {X|X=(xc,yc,1)} ，將目標(biāo)檢測成其他物體的點(diǎn)坐標(biāo)為 {X|X=(xc,yc,i),(i∈N,i≠1)} ，未檢測出目標(biāo)或者同時檢測出多個目標(biāo)的點(diǎn)坐標(biāo)為{X|X=(xc,yc,0)}.如圖4 所示，若連續(xù)檢測到待對接目標(biāo)的幀數(shù)大于設(shè)置的閾值時，則將新進(jìn)入的n幀目標(biāo)檢測點(diǎn)坐標(biāo)組成幀群，并輸入大小為m×n的滑動窗口中，再使用密度聚類算法對滑動窗口內(nèi)的目標(biāo)檢測點(diǎn)進(jìn)行聚類.若聚類后返回的簇?cái)?shù)大于1，則該輸入幀群存在誤檢或者漏檢幀，將新進(jìn)幀群的前一幀作為KCF 算法的初始化幀，對該幀群內(nèi)的所有目標(biāo)檢測點(diǎn)執(zhí)行KCF 更新步驟.若連續(xù)丟失目標(biāo)的幀數(shù)大于設(shè)置的閾值，則不再繼續(xù)執(zhí)行KCF 算法的初始化及更新，直至重新連續(xù)檢測到待對接目標(biāo)的幀數(shù)大于設(shè)置的閾值時，繼續(xù)執(zhí)行滑動窗口目標(biāo)檢測點(diǎn)密度聚類以及KCF 初始化、更新步驟.

圖4 基于滑動窗口的目標(biāo)檢測點(diǎn)密度聚類方法Fig.4 Target detection point density clustering method based on sliding window

在近程導(dǎo)引階段，選用AprilTag 算法以提供模塊間的相對位姿定位結(jié)果.為了降低導(dǎo)引降落系統(tǒng)的復(fù)雜程度，在本研究中，相機(jī)與機(jī)身底部固連安裝，并利用機(jī)身姿態(tài)角對AprilTag 解算出來的位置信息進(jìn)行補(bǔ)償修正，以降低機(jī)身姿態(tài)角變化時對AprilTag 算法定位解算結(jié)果的影響.定義相機(jī)光心所在軸線垂直于地面時標(biāo)簽相對于相機(jī)的位置信息為X′=[x′,y′,z′]T，相機(jī)在存在滾轉(zhuǎn)、俯仰角時AprilTag 算法解算到的位置信息為X=[x,y,z]T，則X′與X存在關(guān)系式

式中：Rx為由相機(jī)滾轉(zhuǎn)角確定的旋轉(zhuǎn)矩陣，Ry為由相機(jī)俯仰角確定的旋轉(zhuǎn)矩陣.

2.2 低照度環(huán)境下的圖像增強(qiáng)算法

針對低照度環(huán)境下，視覺圖像亮度低、對比度小、噪聲大、細(xì)節(jié)可視性差等因素導(dǎo)致的視覺目標(biāo)檢測的精度下降問題，將輕量化的自適應(yīng)伽馬校正暗光圖像增強(qiáng)算法(adaptive gamma correction for image enhancement, AGCIE)[26]引入，對攝像頭輸入的視頻流信息進(jìn)行預(yù)處理，伽馬校正過程表示為

式中：Iout、Iin分別為輸出和輸入圖像；c為灰度縮放系數(shù)；γ 為伽馬因子，控制校正的縮放程度.AGCIE 可以根據(jù)輸入圖像的對比度和亮度信息，自適應(yīng)調(diào)整c、γ，以在不同的照度下實(shí)現(xiàn)最優(yōu)的圖像增強(qiáng)，如圖5 所示.當(dāng)照度過低時，在沒有任何外界光源的情況下，增強(qiáng)后的圖像存在大量的塊狀噪聲.此時可通過布置在飛行模塊或乘客艙下方的照明單元來輔助照明，以提高場景的照度，再進(jìn)行圖像增強(qiáng).在低照度環(huán)境中，通過圖像增強(qiáng)算法可以極大程度上提高目標(biāo)模塊以及標(biāo)簽的可識別性，從而保證視覺模塊的識別精度.

圖5 圖像增強(qiáng)前后對比Fig.5 Comparison of image before and after enhancement

2.3 多傳感器數(shù)據(jù)融合算法

盡管視覺傳感器能夠在近距離擁有厘米級的定位精度，飛行模塊在調(diào)整姿態(tài)的過程中難以保證視覺算法姿態(tài)解算數(shù)據(jù)的連續(xù)性，本研究將RTK-GPS 及機(jī)載慣性測量單元（inertial measurement unit，IMU）與視覺定位數(shù)據(jù)融合以獲得更加精準(zhǔn)、穩(wěn)定的模塊間相對位姿檢測信息流.對傳感器進(jìn)行內(nèi)、外參標(biāo)定后，統(tǒng)一測量坐標(biāo)系以獲得待對接目標(biāo)在各定位傳感器中的一致性描述，再使用卡爾曼濾波算法對2 種傳感器數(shù)據(jù)進(jìn)行融合.

定義RTK-GPS 傳感器測量飛行模塊坐標(biāo)系下待對接目標(biāo)的位姿為 [xr,yr,vxr,vyr,zr,θr]T，其中θr為飛行模塊的偏航角.在中程導(dǎo)引階段，視覺傳感器測量得到像素坐標(biāo)系下的待對接目標(biāo)坐標(biāo)為[us,vs]T，在近程導(dǎo)引階段，視覺傳感器測量得到相機(jī)坐標(biāo)系下待對接目標(biāo)上視覺標(biāo)識物的位姿為[xt,yt,zt,θr]T.卡爾曼濾波(Kalman filter, KF)數(shù)據(jù)融合過程如圖6 所示，以IMU 加速度測量值[αx,αy]T作為系統(tǒng)的輸入ut，以飛行模塊坐標(biāo)系下的待對接目標(biāo)位姿 [xr,yr,vxr,vyr,zr,θr]T作為系統(tǒng)的狀態(tài)變量.飛行模塊的運(yùn)動方程為

圖6 基于卡爾曼濾波的多傳感器數(shù)據(jù)融合Fig.6 Multi-sensor data fusion based on Kalman filter

當(dāng)觀測值為 RTK-GPS 或 AprilTag 數(shù)據(jù)時，采用KF 算法，則狀態(tài)預(yù)測步表示為

其中系統(tǒng)的輸入ut=[ax,ay]T，協(xié)方差矩陣的初始值P為6×6 階單位矩陣，F(xiàn)、B分別為

更新步表示為

當(dāng)觀測值為RTK-GPS 數(shù)據(jù)時，Hr為6×6 階單位矩陣；當(dāng)觀測值為 AprilTag 數(shù)據(jù)時，

R為對應(yīng)傳感器的測量不確定度矩陣.當(dāng)使用AprilTag 測量數(shù)據(jù)進(jìn)行更新時，使用線性修正因子修正R以獲得更加準(zhǔn)確的融合定位結(jié)果.當(dāng)觀測數(shù)據(jù)為YOLOv4-tiny+KCF 數(shù)據(jù)時，采用擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)算法對狀態(tài)變量進(jìn)行更新，預(yù)測步狀態(tài)方程為

其中狀態(tài)轉(zhuǎn)移矩陣F′為

更新步表示為

其中Hj為雅克比矩陣，

式中：fx、fy為相機(jī)水平方向和豎直方向上的歸一化焦距；zr為此時的RTK-GPS 測量數(shù)據(jù)中飛行模塊與待對接目標(biāo)之間的相對高度；h(Xt) 為飛行模塊坐標(biāo)系下的點(diǎn)到像素坐標(biāo)系的非線性映射，計(jì)算式為

式中：Tcf為相機(jī)坐標(biāo)系到飛行模塊坐標(biāo)系的轉(zhuǎn)換矩陣.

2.4 基于修正因子的近程導(dǎo)引數(shù)據(jù)融合方法

為了更好地描述近距離AprilTag 算法定位解算的誤差規(guī)律，基于修正因子的近程導(dǎo)引數(shù)據(jù)融合方法來提升多傳感器融合的最終精度.相機(jī)與標(biāo)簽之間的相對位姿影響AprilTag 的定位精度，在本研究的自主導(dǎo)引降落過程中，位置、航向方向上的偏差校正是位姿微調(diào)的過程，在位姿校正過程中的每個時刻，相機(jī)的俯仰、滾轉(zhuǎn)角變化不大，因此本研究重點(diǎn)考慮相機(jī)與標(biāo)簽的三軸距離對AprilTag 定位解算結(jié)果的影響.

在Gazebo 仿真軟件中進(jìn)行取點(diǎn)采樣，設(shè)置位于X軸上的取樣點(diǎn)為 { 0,0.5,1.0,1.5,2.0,2.5,3.0,3.5} ，位于Y軸上的取樣點(diǎn)位 { 0,0.5,1.0,1.5,2.0,2.5,3.0,3.5}，位于Z軸上的取樣點(diǎn)位為 { 1.5,2.5,3.5,4.5} ，總計(jì)256 個采樣點(diǎn)位，將AprilTag 解算出的數(shù)據(jù)與真實(shí)值進(jìn)行對比得到定位誤差如圖7 所示，其中Xe、Ye、Ze和 θe分別為橫向、縱向、高度和偏航角誤差，Xb和Yb為橫向和縱向的偏移量.可以得出，AprilTag 解算出來的X、Y方向位置精度隨著高度以及各自方向的偏移量的增大而增大，且誤差值近似趨于線性增長，在相同偏移量、不同高度條件下的X、Y方向定位誤差的變化較小.還可以得出，在不同Y方向偏移量下的X方向定位誤差基本呈水平線狀，波動不大，其影響程度遠(yuǎn)小于不同X方向偏移量以及高度z帶來的影響.同樣的，在不同Y方向偏移量下的Y方向定位誤差變化也不明顯.從采樣數(shù)據(jù)中發(fā)現(xiàn)，不同X、Y方向的偏移量對Ze、 θe的影響較小，取各個高度層Z方向定位誤差平均值，得到圖7(e)，從中得出Z方向的定位誤差隨著高度的增大而增大，且誤差值也近似趨于線性增長.取各個不同高度層數(shù)據(jù)的偏航定位誤差平均值，得到圖7(f)，可以看出偏航誤差基本不隨高度的增大而變化.

圖7 不同取樣點(diǎn)處的視覺標(biāo)識碼定位誤差分析Fig.7 Analysis of visual identification code location error at different sampling points

忽略對X、Y方向定位誤差的弱影響項(xiàng)，即認(rèn)為X、Y方向的定位誤差僅與對應(yīng)方向的偏移量有關(guān)，本研究提出修正因子對近程卡爾曼濾波融合算法中AprilTag 測量更新步的R矩陣進(jìn)行修正，修正因子

式 中：A、A0和B、B0以 及C、C0分 別 為X、Y和Z方向的修正常數(shù)，x、y和z分別取上一時刻融合結(jié)果的對應(yīng)值，經(jīng)過修正后的矩陣表示為

3 仿真與試驗(yàn)驗(yàn)證

3.1 試驗(yàn)設(shè)置

為了驗(yàn)證本研究算法的魯棒性及有效性，分別在仿真平臺與工程應(yīng)用平臺上進(jìn)行試驗(yàn).工程應(yīng)用平臺與分體式飛行汽車的對應(yīng)關(guān)系如圖8 所示，其中對接機(jī)構(gòu)允許的位置偏差為6 cm，航向偏差為5°.仿真部分在Gazebo 仿真軟件中基于PX4 ROS 環(huán)境進(jìn)行無人機(jī)仿真試驗(yàn)，所有算法均基于OpenCV 庫實(shí)現(xiàn)編程.

圖8 工程應(yīng)用平臺與分體式飛行汽車的對應(yīng)關(guān)系Fig.8 Correspondence between engineering application platform and modular flying vehicle

在工程應(yīng)用平臺上，選擇最大起飛重量為50 kg的六旋翼飛行器作為飛行模塊，其底部安裝對接機(jī)構(gòu)、視覺傳感器和處理器.視覺傳感器選擇型號為KS2A543 的全局快門單目相機(jī)，其焦距為3.6 mm，采用640×480 的分辨率和30 Hz 的采樣率.選用的RTK-GPS 傳感器精度為厘米級（定位精度受距離、電磁干擾和環(huán)境遮蔽等因素影響），最大有效距離為10 km.乘客艙部分使用碳纖維結(jié)構(gòu)，并置有定位傳感器、中央處理器以及對接鎖定機(jī)構(gòu).地面行駛模塊框架由亞克力板搭建，其上置有對接機(jī)構(gòu)和視覺標(biāo)識，增加車輛構(gòu)型因素來模擬車體特征.其中工程平臺的對接機(jī)構(gòu)為1∶2 縮比原理樣機(jī)，對接機(jī)構(gòu)要求的對接精度誤差為6 cm.

3.2 仿真試驗(yàn)結(jié)果與分析

3.2.1 中程視覺導(dǎo)引算法對比分析 為了驗(yàn)證中程視覺導(dǎo)引算法的有效性，在仿真平臺中搭建2 個不同場景下的中程導(dǎo)引階段待對接目標(biāo)識別環(huán)境，定義待對接目標(biāo)（suv）、干擾項(xiàng)（pika）為YOLOv4-tiny 算法的目標(biāo)識別類別.在目標(biāo)的錯誤檢測幀中，圖9(a)、(c)分別為待對接目標(biāo)在場景一、二中被誤檢成物體的修正過程，當(dāng)聚類簇?cái)?shù)大于1 時，采用KCF 算法更新策略，組合算法輸出待對接目標(biāo)的正確定位信息；圖9(b)、(d)分別為待對接目標(biāo)在場景一、二中漏檢情況下的修正過程，在YOLOv4-tiny 算法丟失待對接目標(biāo)后的一段時間內(nèi)，組合算法仍能準(zhǔn)確輸出待對接目標(biāo)的定位信息.

圖9 錯誤檢測幀目標(biāo)修正過程Fig.9 Target correction process of error detection frame

對比本研究使用的中程視覺目標(biāo)檢測算法與連續(xù)自適應(yīng)均值偏移(Camshift)算法、KCF 算法、YOLOv4-tiny 原算法的目標(biāo)檢測效果，結(jié)果如表1所示，其中FNR 為誤檢率，F(xiàn)PR 為漏檢率，ACC 為正確率，v為檢測速度.可以得出，Camshift 算法對目標(biāo)背景顏色的要求較高，檢測框難以鎖定目標(biāo)，誤檢率較高；當(dāng)目標(biāo)在視野中緩慢移動時，KCF 算法跟蹤效果較好，但是當(dāng)丟失目標(biāo)后，算法難以再正確檢測到目標(biāo)；本研究的組合算法在選取的目標(biāo)識別正確率較低的場景中，以較高的正確率識別出待對接目標(biāo)并輸出其定位信息，完成錯誤幀的修正，但在丟失目標(biāo)時長超出閾值后，組合算法不再對目標(biāo)進(jìn)行漏檢幀修正，存在漏檢的情況.

表1 不同算法在2 種場景中的中程視覺目標(biāo)檢測結(jié)果Tab.1 Target detection results of different algorithms of medium range in two scenarios

3.2.2 近程視覺姿態(tài)補(bǔ)償效果分析 為了驗(yàn)證近程視覺導(dǎo)引算法的有效性，在仿真平臺上設(shè)計(jì)X軸水平方向上具有不同風(fēng)速干擾的自主導(dǎo)引降落場景，對比在有無姿態(tài)補(bǔ)償影響下的無人機(jī)導(dǎo)引降落最終位置誤差，如圖10 所示，其中φ為無人機(jī)的橫滾角.隨著風(fēng)速的提升，φ變大，無姿態(tài)補(bǔ)償下的導(dǎo)引降落位置誤差逐步增大，在最大風(fēng)速（即滾轉(zhuǎn)角最大）條件下，無姿態(tài)補(bǔ)償下X方向的位置誤差已經(jīng)超出0.1 m，姿態(tài)補(bǔ)償條件下的X方向位置誤差仍不超過0.1 m.上述試驗(yàn)結(jié)果驗(yàn)證了姿態(tài)補(bǔ)償能夠提高AprilTag 算法在風(fēng)擾條件下的適應(yīng)性，保證了對接導(dǎo)引過程的最終降落精度.

圖10 不同滾轉(zhuǎn)角下，對接導(dǎo)引降落的水平方向位置誤差Fig.10 Position error in horizontal direction of docking guidance under different roll angles

3.2.3 近程導(dǎo)引數(shù)據(jù)融合修正 為了驗(yàn)證近程數(shù)據(jù)融合定位修正算法的有效性.在近程導(dǎo)引階段中，先規(guī)劃飛行模塊的航跡，再分別使用經(jīng)過修正因子修正后的R矩陣與初始矩R矩陣進(jìn)行多傳感器數(shù)據(jù)融合.經(jīng)過融合后的位姿估計(jì)結(jié)果如圖11 所示.可以得出，當(dāng)AprilTag 算法的位姿解算結(jié)果出現(xiàn)跳變時，融合算法能夠很好地抑制噪聲數(shù)據(jù)，實(shí)現(xiàn)穩(wěn)定的相對定位測量.還可以得出，使用經(jīng)過修正的R矩陣進(jìn)行數(shù)據(jù)融合的位置、高度估計(jì)偏差小于使用初始R矩陣的結(jié)果，在飛往最后一個航點(diǎn)時，飛行模塊位于AprilTag 正上方，X、Y方向偏移量和高度在整個航跡飛行過程中都比較小，屬于在較近距離對AprilTag 標(biāo)簽檢測的情況，定位精度較高.此時使用修正R矩陣的數(shù)據(jù)融合方法在利用AprilTag 的定位數(shù)據(jù)更新飛行模塊狀態(tài)時，其結(jié)果將更加偏向AprilTag 的測量，因此位置估計(jì)精度更高.如圖12所示為飛行模塊在飛航點(diǎn)任務(wù)時使用修正R矩陣與初始R矩陣的三維軌跡對比.

圖11 修正不確定度矩陣與初始不確定度矩陣融合后的相關(guān)參數(shù)對比Fig.11 Comparison of related parameters after fusion of modified uncertainty matrix and initial uncertainty matrix

圖12 修正不確定度矩陣與初始不確定度矩陣融合后的三維飛行軌跡Fig.12 3D flight path after fusion of modified uncertainty matrix and initial uncertainty matrix

如表2 所示為使用修正R矩陣與初始R矩陣的多傳感器數(shù)據(jù)融合位姿估計(jì)誤差對比，其中Xe、Ze和 θe在使用修正R矩陣的近程多傳感器數(shù)據(jù)融合過程中，平均位置估計(jì)偏差將比使用初始R矩陣的結(jié)果降低約28%，能夠輸出更加穩(wěn)定、準(zhǔn)確的模塊間相對位姿檢測信息流.

表2 多傳感數(shù)據(jù)融合的仿真試驗(yàn)結(jié)果Tab.2 Simulation test results of multi-sensor data fusion

3.3 全流程工程應(yīng)用試驗(yàn)結(jié)果與分析

為了驗(yàn)證本研究算法的實(shí)際工程應(yīng)用效果，在工程驗(yàn)證平臺下進(jìn)行自主導(dǎo)引降落試驗(yàn).將基于多傳感器數(shù)據(jù)融合的自主導(dǎo)引降落試驗(yàn)分2 個部分進(jìn)行.

3.3.1 飛行-座艙模塊對接驗(yàn)證試驗(yàn) 如圖13 所示，在遠(yuǎn)程階段，飛行模塊通過RTK-GPS 定位導(dǎo)引至地面行駛模塊及乘客艙的上方區(qū)域；隨后中程YOLOv4 開始識別地面行駛模塊及乘客艙組合體，此時高度不斷下降.一旦相機(jī)捕捉到對接機(jī)構(gòu)內(nèi)的AprilTag 信息，立刻將AprilTag 數(shù)據(jù)優(yōu)先作為導(dǎo)引數(shù)據(jù)轉(zhuǎn)入近程階段，最終使其精準(zhǔn)落至對接機(jī)構(gòu)中.在上對接驗(yàn)證試驗(yàn)過程中，飛行模塊與待對接模塊的相對位姿偏差變化如圖14 所示.

圖13 上對接自主導(dǎo)引降落試驗(yàn)流程Fig.13 Autonomous guidance landing process in up-docking test

圖14 上對接驗(yàn)證試驗(yàn)相對位姿偏差變化Fig.14 Change of relative pose deviation in up-docking test

3.3.2 座艙-地面行駛模塊對接試驗(yàn) 如圖15 所示，試驗(yàn)流程與上對接類似，遠(yuǎn)程階段，飛行模塊攜帶乘客艙通過RTK-GPS 定位導(dǎo)引至地面行駛模塊上方，隨后中程YOLOv4 開始識別地面行駛模塊的整體特征，此時飛機(jī)不斷下降并修正水平以及航向位姿.一旦相機(jī)捕捉到位于行駛模塊中心機(jī)構(gòu)的AprilTag 信息，立刻將AprilTag 數(shù)據(jù)優(yōu)先作為導(dǎo)引數(shù)據(jù)，使其精準(zhǔn)落至對接機(jī)構(gòu)中.下對接驗(yàn)證試驗(yàn)過程中，飛行模塊與待對接模塊的相對位姿偏差變化如圖16 所示.

圖15 下對接自主導(dǎo)引降落試驗(yàn)流程Fig.15 Autonomous guidance landing process in lower-docking test

圖16 下對接驗(yàn)證試驗(yàn)相對位姿偏差變化Fig.16 Change of relative pose deviation of lower-docking test

盡管在中程和近程導(dǎo)引階段中，難以保證在每幀圖像中都能對目標(biāo)進(jìn)行識別并解算，會出現(xiàn)定位數(shù)據(jù)丟失的情況，但融合算法依然能夠輸出光順的相對位姿檢測數(shù)據(jù)，這保證了控制系統(tǒng)輸出的平順性.在上述2 個導(dǎo)引階段中，融合算法選擇更加信任在近距離定位誤差更小的AprilTag測量數(shù)據(jù)，使定位結(jié)果更加精確.融合算法精準(zhǔn)、穩(wěn)定的相對位姿檢測不僅有效避免了因視覺定位數(shù)據(jù)丟失導(dǎo)致對接導(dǎo)引過程失敗的情況，也解決了只使用RTK-GPS 導(dǎo)引存在的定位精度不足問題.在對接流程邏輯設(shè)計(jì)中，本對接系統(tǒng)同樣考慮了安全性措施，當(dāng)水平位置偏差或航向偏差不滿足當(dāng)前閾值要求時，導(dǎo)引系統(tǒng)會指示飛行模塊向上爬升到安全高度后重新對偏差進(jìn)行修正，直到滿足閾值要求后繼續(xù)降落，因此圖14(c)和圖16(c)的末端圖像起伏.在對接機(jī)械機(jī)構(gòu)圓錐面的作用下，位置偏差、航向偏差得到完全修正，飛行模塊精準(zhǔn)降落在待對接模塊上，對接導(dǎo)引任務(wù)完成.

4 結(jié) 語

針對分體式飛行汽車實(shí)現(xiàn)模態(tài)轉(zhuǎn)換時需要獲得精準(zhǔn)、穩(wěn)定的模態(tài)間相對位姿測量的問題，本研究1）設(shè)計(jì)基于RTK-GPS 以及視覺傳感器數(shù)據(jù)融合的無人機(jī)自主導(dǎo)引降落技術(shù)方案.2）通過利用KCF 核相關(guān)濾波算法和基于滑動窗口的目標(biāo)檢測點(diǎn)密度聚類方法，修正了YOLOv4-tiny 目標(biāo)檢測的誤檢漏檢情況.3）通過姿態(tài)補(bǔ)償算法減小相機(jī)與機(jī)身固連時機(jī)身姿態(tài)角對AprilTag 算法定位解算結(jié)果的影響.4）采用暗光圖像增強(qiáng)技術(shù)提高在低照度環(huán)境下視覺導(dǎo)引算法的魯棒性.5）通過基于卡爾曼濾波的多傳感器數(shù)據(jù)融合方法將RTK-GPS 與視覺定位數(shù)據(jù)融合，獲得精準(zhǔn)、穩(wěn)定的模塊間相對位姿檢測信息流.6）在搭建的仿真平臺和工程應(yīng)用平臺上進(jìn)行了全流程對接試驗(yàn).試驗(yàn)結(jié)果表明，本研究提出的導(dǎo)引方案可以滿足僅允許6 cm 降落對接偏差的要求，驗(yàn)證了技術(shù)的高精度和可靠性.隨著項(xiàng)目進(jìn)度的進(jìn)行，計(jì)劃在未來的工作中進(jìn)一步擴(kuò)展試驗(yàn)場景和試驗(yàn)組數(shù)，以更充分地驗(yàn)證全自主對接導(dǎo)引技術(shù)在不同場景下的表現(xiàn).同時，將考慮不同飛行控制和天氣條件對本研究的影響.