基于互補(bǔ)濾波算法的傾斜RTK姿態(tài)解算

劉坤之，唐詩(shī)華*，張炎，李灝楊，呂富強(qiáng)

(1.桂林理工大學(xué)測(cè)繪地理信息學(xué)院，桂林 541004;2.廣西空間信息與測(cè)繪重點(diǎn)實(shí)驗(yàn)室，桂林 541004)

RTK(real time kinematic)是一種利用GPS載波相位觀測(cè)值進(jìn)行實(shí)時(shí)動(dòng)態(tài)相對(duì)定位的技術(shù)，可以實(shí)時(shí)提供指定坐標(biāo)系下觀測(cè)點(diǎn)的三維坐標(biāo)。傾斜RTK是通過內(nèi)置慣性測(cè)量單元(inertial measurement unit,IMU)，根據(jù)內(nèi)部慣性器件計(jì)算的姿態(tài)角生成方向余弦矩陣，從而實(shí)現(xiàn)空間矢量的坐標(biāo)轉(zhuǎn)換，將天線相位中心位置補(bǔ)償?shù)降孛鏈y(cè)量點(diǎn)，得到待測(cè)點(diǎn)坐標(biāo)。解決了傳統(tǒng)RTK測(cè)量過程中在特殊作業(yè)情況下不能保持對(duì)中桿豎直，出現(xiàn)定位誤差較大的問題。

慣性測(cè)量器件具有體積小、成本低、重量輕和精度高等優(yōu)點(diǎn)，適用于傾斜RTK的姿態(tài)檢測(cè)。但是在運(yùn)動(dòng)過程中，微機(jī)電系統(tǒng)(micro electro mechanical system，MEMS)慣性傳感器，無法實(shí)時(shí)的獲取準(zhǔn)確的姿態(tài)信息，導(dǎo)致解算出的姿態(tài)精度較差，無法直接使用。通過利用不同傳感器的自身特性，文獻(xiàn)[1]提出了一種基于加速度修正模型的姿態(tài)解算法，搭建卡爾曼濾波算法，減弱非重力加速度對(duì)姿態(tài)解算的干擾，提高了姿態(tài)解算的精度和抗干擾能力。文獻(xiàn)[2]利用混合濾波的算法對(duì)加速度計(jì)和磁力計(jì)數(shù)據(jù)進(jìn)行四元數(shù)的最優(yōu)估計(jì)，通過設(shè)計(jì)自適應(yīng)濾波器調(diào)參，有效的降低磁干擾，減少加速度對(duì)姿態(tài)解算的影響。文獻(xiàn)[3]采用一種自適應(yīng)增益互補(bǔ)濾波算法，融合九軸傳感器數(shù)據(jù)，設(shè)計(jì)互補(bǔ)卡爾曼濾波器，實(shí)現(xiàn)姿態(tài)估計(jì)，但該算法計(jì)算量較大且容易產(chǎn)生奇異值。文獻(xiàn)[4]設(shè)計(jì)一種自適應(yīng)無跡卡爾曼濾波算法，基本消除了噪聲干擾和信號(hào)野點(diǎn)值，有效的抑制了陀螺儀漂移引起的姿態(tài)角發(fā)散問題。文獻(xiàn)[5]對(duì)粒子濾波的重采樣過程進(jìn)行了改進(jìn)，應(yīng)用拓展卡爾曼濾波(extended Kalman filter,EKF)對(duì)系統(tǒng)狀態(tài)預(yù)測(cè)，根據(jù)不同環(huán)境自適應(yīng)調(diào)參，優(yōu)于傳統(tǒng)的粒子濾波算法，但計(jì)算量太大，實(shí)時(shí)性仍有不足。因此，設(shè)計(jì)了一種基于互補(bǔ)濾波的融合算法，設(shè)置PI控制器，用DE算法對(duì)參數(shù)尋優(yōu)，實(shí)時(shí)解算出最優(yōu)姿態(tài)角。通過慣導(dǎo)模塊搭載全站儀實(shí)時(shí)采集數(shù)據(jù)，對(duì)本算法進(jìn)行仿真驗(yàn)證，為傾斜RTK確定姿態(tài)提供一種可行方案。

1 基于互補(bǔ)濾波的姿態(tài)解算原理

1.1 空間姿態(tài)描述

(1)

式(1)中：φ為橫滾角；θ為俯仰角；ψ為航向角；C表示cos；S表示sin。

歐拉角在空間姿態(tài)的描述是非常直觀理解的，但是存在的問題也很嚴(yán)重。函數(shù)的自變量和因變量是相同的，無法直接求解；含有大量的三角運(yùn)算，會(huì)拖慢主控芯片的運(yùn)行效率；存有萬向節(jié)死鎖等問題。因此，需要一種方便計(jì)算的描述方式——四元數(shù)。

(2)

(3)

θ=arcsin2(q0q2-q1q3)

(4)

(5)

式中：q0、q1、q2、q3為四元數(shù)的基本元素。

1.2 四元數(shù)姿態(tài)求解

傳感器姿態(tài)的變化即為四元數(shù)的變化，需要構(gòu)建四元數(shù)關(guān)于時(shí)間的微分方程研究此類問題。由于計(jì)算機(jī)中的計(jì)算是離散的，需要采用一階龍格庫(kù)塔法對(duì)微分方程離散化處理[7]，從而實(shí)現(xiàn)四元數(shù)的更新計(jì)算。計(jì)算公式為

(6)

式(6)中：ωx、ωy、ωz分別為陀螺儀輸出的x、y、z軸角速度；T為時(shí)刻；Δt為數(shù)據(jù)更新的頻率。

1.3 傳感器數(shù)據(jù)融合

六軸傳感器有兩種計(jì)算角度的方法：一是對(duì)角速度直接積分得到角度，如果角速度積分時(shí)在某個(gè)瞬間引入誤差，隨著積分的過程，誤差將會(huì)一直累加，形成低頻誤差；二是對(duì)加速度正交分解計(jì)算角度，由于加速度計(jì)是比較敏感的傳感器，載體在運(yùn)動(dòng)的過程中，微小的運(yùn)動(dòng)都將會(huì)對(duì)加速度的數(shù)據(jù)造成影響，產(chǎn)生高頻誤差[8]。可以將兩種傳感器融合起來，利用一種向量外積補(bǔ)償?shù)幕パa(bǔ)濾波算法，更新四元數(shù)，解決上述問題，如圖1所示。

圖1 誤差補(bǔ)償原理圖Fig.1 Schematic diagram of error compensation

四元數(shù)和理論上的重力加速度分量，是可以相互推導(dǎo)，具體的變換關(guān)系式為

(7)

式(7)中：vb為理論重力加速度；vbx、vby、vbz為b系下x、y、z軸理論重力加速度；gn為n系實(shí)際重力加速度。

加速度計(jì)實(shí)測(cè)重力加速度與理論重力加速度二者之間出現(xiàn)的偏差，很大程度是因?yàn)橥勇輧x數(shù)據(jù)產(chǎn)生的角速度誤差引起的，產(chǎn)生的偏差可以通過向量外積補(bǔ)償解算陀螺儀數(shù)據(jù)的誤差[9]，可表示為

|ρ|=|gb||vb|sinα

(8)

式(8)中：ρ為外積向量；α為向量夾角;gb為b系實(shí)際重力加速度。

叉乘運(yùn)算前對(duì)理論向量和實(shí)際向量歸一化處理，且由于載體瞬時(shí)運(yùn)行角度不會(huì)超過45°，可以對(duì)sinα小角近似處理，可表示為

|ρ|≈α=error

(9)

式(9)中：error為誤差補(bǔ)償值，該補(bǔ)償值不能直接應(yīng)用到陀螺儀的數(shù)據(jù)中，因此構(gòu)建比例積分補(bǔ)償器來控制補(bǔ)償值的大小和精度。控制器表達(dá)式為

(10)

式(10)中：δ為陀螺儀誤差補(bǔ)償量；kP為比例控制項(xiàng)；kI為積分控制項(xiàng)；dt為對(duì)時(shí)間的積分。

補(bǔ)償量加到角速度上后，即可得到可信度較高的陀螺儀數(shù)據(jù)，將所求陀螺儀補(bǔ)償量代入式(6)更新陀螺儀數(shù)據(jù)即可得到當(dāng)前四元數(shù)。

2 基于DE算法的PI控制參數(shù)尋優(yōu)

加速度計(jì)測(cè)量的是物體在外力影響下造成的一種微小形變，在靜止不動(dòng)時(shí)Z軸輸出的是1g(g為重力加速度)的加速度，但是傳感器在三維運(yùn)動(dòng)時(shí)，它不能分辨重力加速度和外力加速度，因此，計(jì)算的誤差補(bǔ)償不能全部用于更正角速度。PI參數(shù)作為控制系統(tǒng)精度的主要參數(shù)之一，其比例項(xiàng)kP用于控制傳感器的“可信度”，積分項(xiàng)kI用于消除靜態(tài)誤差，為滿足快速性和精確性的要求，對(duì)其參數(shù)整定至關(guān)重要。傳統(tǒng)PI參數(shù)整定一般選擇手動(dòng)調(diào)參，存在工作量大、效率低和參數(shù)不滿足要求等缺點(diǎn)。利用差分進(jìn)化(differential evolution,DE)算法對(duì)PI參數(shù)進(jìn)行參數(shù)整定，可以快速尋找滿足要求的參數(shù)，提高系統(tǒng)的控制精度和魯棒性。

2.1 DE算法基本原理

差分進(jìn)化算法是基于現(xiàn)代智能理論的自適應(yīng)全局優(yōu)化算法，與其他基于群體的進(jìn)化算法相比，它是以達(dá)爾文生物進(jìn)化論“優(yōu)勝劣汰，適者生存”的理論為原則，模擬生物界生物進(jìn)化的群體差異的啟發(fā)式隨機(jī)搜索算法[10]。DE算法尋優(yōu)過程分為：①種群初始化;②通過差分策略實(shí)現(xiàn)個(gè)體變異;③以概率的形式交叉操作隨機(jī)生成新的個(gè)體;④采用貪婪選擇的策略選擇較優(yōu)的個(gè)體作為新的個(gè)體。

對(duì)于無約束優(yōu)化問題f(Xi)min，Xi=[xi,1,xi,2，…，xi,D]為問題的解，D為優(yōu)化的維度，i=1,2,…，n，DE算法的實(shí)現(xiàn)步驟如下。

j=1,2，…,D}

(11)

(12)

步驟2實(shí)現(xiàn)個(gè)體變異。設(shè)迭代次數(shù)為G，在第g代迭代中，隨即從群里中選取一定數(shù)量的個(gè)體，把所選個(gè)體作為目標(biāo)變量進(jìn)行矢量變異，同時(shí)為了加強(qiáng)DE算法的搜索能力，避免陷入局部最優(yōu)解，對(duì)搜索步長(zhǎng)的控制參數(shù)縮放變異因子合理取值，更新傳統(tǒng)的差分進(jìn)化算法，其表達(dá)式為

(13)

(14)

步驟4選擇操作。將父代個(gè)體與子代個(gè)體的一一對(duì)應(yīng)選擇，選取適應(yīng)度值較小的個(gè)體進(jìn)行下一代搜索，組成新一代種群，實(shí)現(xiàn)選擇操作，其表達(dá)式為

(15)

通過不斷重復(fù)進(jìn)行式(12)～式(15)操作，直至滿足條件終止，方可得出最優(yōu)參數(shù)估值。

2.2 適應(yīng)度函數(shù)選取

PI參數(shù)是PI控制器中重要的影響因子，DE算法是對(duì)PI控制器的參數(shù)進(jìn)行合理選取，實(shí)現(xiàn)最優(yōu)解的工作。核心是根據(jù)目標(biāo)函數(shù)構(gòu)建合適的適值函數(shù)，通過選取合適的光滑因子求出最佳的目標(biāo)值，可將目標(biāo)值與參考值比較得到的均方根誤差(root mean square error,RMSE)作為目標(biāo)函數(shù)和評(píng)價(jià)指標(biāo)，當(dāng)均方根誤差值最小時(shí)，說明所求目標(biāo)值達(dá)到最優(yōu)效果，所選的光滑因子為最佳[13]。目標(biāo)函數(shù)RMSE表達(dá)式為

(16)

式(16)中：A為姿態(tài)角；xobs為解算的姿態(tài)角；xref為參考姿態(tài)角；m為采樣點(diǎn)數(shù)。

考慮DE算法的適應(yīng)度值越小越好的要求，仿真傾斜RTK工作時(shí)對(duì)中桿傾斜對(duì)載體姿態(tài)的影響，為滿足瞬態(tài)響應(yīng)振蕩小，具備快速性和穩(wěn)定性的性能，選用均方根誤差作為DE算法的適應(yīng)度函數(shù)。根據(jù)上訴步驟編寫DE算法，建立優(yōu)化算法與聯(lián)合仿真模型的數(shù)據(jù)交互，修改PI控制器中的不確定參數(shù)，得到姿態(tài)解算的最優(yōu)值。完整的流程圖如圖2所示。

vx、vy、vx為x、y、z軸理論重力加速度；Ax、Ay、Az為x、y、z軸實(shí)際重力加速度；ex、ey、ez為三軸外積誤差圖2 處理流程圖Fig.2 Processing flow chart

為了驗(yàn)證DE算法對(duì)PI參數(shù)模型整定的優(yōu)越性，同時(shí)使用粒子群優(yōu)化(particle swarm optimization,PSO)算法對(duì)適應(yīng)度函數(shù)進(jìn)行仿真對(duì)比分析。各優(yōu)化算法中控制參數(shù)的選擇會(huì)直接影響算法快速準(zhǔn)確運(yùn)行的結(jié)果，DE算法有3個(gè)基本控制參數(shù)：種群大小NP取20；縮放因子F取0.4～1；交叉概率常數(shù)CR取0.5～1；kP取值范圍為1～7；kI取值范圍為0.01～0.8。PSO基本參數(shù)與DE算法一致。兩個(gè)算法經(jīng)過50次迭代進(jìn)化后目標(biāo)函數(shù)值的變化如圖3所示，兩種算法計(jì)算的最優(yōu)函數(shù)值均為0.388 78，DE算法在迭代第7次便得到函數(shù)最優(yōu)解，PSO算法在第32次才得到，對(duì)比分析可得DE算法比較穩(wěn)定，收斂速度較快；是一種概率性全局尋優(yōu)算法，不容易陷入局部最優(yōu)解；具有較強(qiáng)的環(huán)境交互能力；可進(jìn)行大規(guī)模并行性運(yùn)算。由此可見使用DE算法對(duì)PI控制參數(shù)整定，提高系統(tǒng)的魯棒性與收斂速度具有明顯的優(yōu)勢(shì)。

圖3 適應(yīng)度函數(shù)值Fig.3 Fitness function value

3 實(shí)驗(yàn)驗(yàn)證及結(jié)果分析

圖4為慣導(dǎo)模塊搭載SOUTH全站儀(NTS-362L)作為實(shí)驗(yàn)平臺(tái)模擬傾斜RTK的工作環(huán)境，該慣導(dǎo)模塊具有穩(wěn)定的角度輸出，均方根(root mean square,RMS)作為傳感器的精度的評(píng)定標(biāo)準(zhǔn)，其中航向角0.5°RMS，橫滾與俯仰靜態(tài)0.05°RMS，動(dòng)態(tài)0.1°RMS，根據(jù)協(xié)議要求使用50 Hz的IMU數(shù)據(jù)輸出頻率，波特率為921 600，加速度計(jì)量程為±16g，分辨率為0.5 mg，零偏穩(wěn)定性為0.04 mg，陀螺儀量程為±2 000(°)/s，零偏穩(wěn)定性為10(°)/h，分辨率為0.02(°)/s。為驗(yàn)證算法的可行性和有效性，通過上位機(jī)對(duì)3軸加速度計(jì)，3軸陀螺儀進(jìn)行原始數(shù)據(jù)的讀取，將模塊輸出姿態(tài)角作為參考對(duì)比對(duì)象，使用MATALB對(duì)融合算法仿真實(shí)驗(yàn)，其中基于差分進(jìn)化的互補(bǔ)濾波算法(DE-mahony)為智能尋參，傳統(tǒng)的mahony互補(bǔ)濾波算法為手動(dòng)調(diào)參。將DE-mahony與mahony互補(bǔ)濾波算法和拓展卡爾曼濾波算法進(jìn)行動(dòng)態(tài)和靜態(tài)兩部分姿態(tài)解算結(jié)果對(duì)比分析。

圖4 模塊搭載平臺(tái)Fig.4 Module carrying platform

3.1 靜態(tài)對(duì)比實(shí)驗(yàn)

通過全站儀氣泡居中的方法將模塊水平安裝放置，利用模塊內(nèi)置安裝角誤差矯正功能補(bǔ)償校準(zhǔn)，則系統(tǒng)輸出的俯仰角和橫滾角的參考值均為0°，航向角在系統(tǒng)初始時(shí)刻參考值也為0°。在靜止?fàn)顟B(tài)下，圖5為3種算法的三軸姿態(tài)解算對(duì)比結(jié)果，mahony算法解算姿態(tài)角在參考角度附近上下浮動(dòng)，相較于EKF算法稍穩(wěn)定些；EKF算法解算姿態(tài)角浮動(dòng)較大，穩(wěn)定性較差，過程噪聲影響較大；DE-mahony算法解算姿態(tài)角在參考角度和0°附近浮動(dòng)，浮動(dòng)范圍小，相對(duì)較穩(wěn)定。

圖5 靜態(tài)三軸角度Fig.5 Static triaxial angle

為了更直觀的比較3種算法的解算精度，靜態(tài)情況下使用均值比較法對(duì)比分析估算精度，結(jié)果如表1所示?？梢钥闯觯褂盟岢龅腄E-mahony算法的解算的三軸精度優(yōu)于mahony算法和拓展卡爾曼濾波算法；DE-mahony解算的橫滾角均值為-0.004 6°，與其他兩種算法相比分別降低了36.1%，33.3%；俯仰角均值為-0.000 71°，與其他兩種算法相比較降低了94.6%，4.1%；航向角均值為0.024 5°，與其他兩種算法相比降低22.7%，18.6%。3種算法解算的三軸靜態(tài)姿態(tài)角度精度均小于0.1°，可以滿足傾斜RTK日常工作需求。

表1 靜態(tài)解算結(jié)果Table 1 Static solution results

3.2 動(dòng)態(tài)對(duì)比試驗(yàn)

傾斜RTK的工作環(huán)境中，橫滾角與俯仰角的傾角需小于60°才能保證解算坐標(biāo)的精度，仿真實(shí)驗(yàn)時(shí)以模塊輸出角度作為參考角度，將模塊在平臺(tái)上60°以內(nèi)無規(guī)則轉(zhuǎn)動(dòng)約100 s，圖6為動(dòng)態(tài)三軸姿態(tài)解算對(duì)比結(jié)果，從波形上可以看出，3種算法與參考姿態(tài)角基本吻合，通過放大圖對(duì)比分析可得，EKF與mahony互補(bǔ)濾波誤差相差不大，DE-mahony算法則震蕩明顯減小，收斂速度更快，更靠近參考角度曲線。由于采用六軸解算，僅使用加速度計(jì)修正陀螺儀，航向角在重力方向無修正作用。在角度遞增或遞減時(shí)誤差較小，而轉(zhuǎn)折時(shí)陀螺儀引入積分誤差，且隨著時(shí)間的推移，陀螺儀積分漂移現(xiàn)象嚴(yán)重，最終會(huì)導(dǎo)致航向角出現(xiàn)較大誤差。

圖6 動(dòng)態(tài)三軸角度Fig.6 Dynamic triaxial angle

動(dòng)態(tài)情況下使用中誤差作為算法性能的評(píng)價(jià)標(biāo)準(zhǔn)，由表2對(duì)比分析可知，3種算法的精度，DE-mahony解算的橫滾角均方根誤差為0.430 8°，相比于傳統(tǒng)mahony互補(bǔ)濾波和EKF分別降低了28.7%和24.5%；DE-mahony解算的俯仰角均方根誤差為0.391 2°，相比于傳統(tǒng)mahony互補(bǔ)濾波和EKF分別降低了15.6%，10.4%；mahony解算的航向角均方根誤差為1.597 9°，相比于傳統(tǒng)mahony互補(bǔ)濾波和EKF分別降低了17.8%，21.5%。動(dòng)態(tài)結(jié)果分析中三軸橫滾角與俯仰角解算精度較高，誤差范圍在0.4°～0.6°；航向角由于陀螺儀累計(jì)誤差，精度較差，誤差范圍在1.5°～2°。

表2 動(dòng)態(tài)解算結(jié)果Table 2 Dynamic solution results

4 結(jié)論

針對(duì)低成本六軸慣性傳感器解算姿態(tài)的問題，提出了一種基于差分進(jìn)化智能尋參的互補(bǔ)濾波算法，將慣導(dǎo)模塊中的加速度計(jì)與陀螺儀利用PI補(bǔ)償器進(jìn)行信息融合，通過四元數(shù)計(jì)算出3個(gè)姿態(tài)角的變化，為傾斜RTK復(fù)雜環(huán)境的運(yùn)動(dòng)提供狀態(tài)信息。得出如下結(jié)論。

(1)在靜態(tài)測(cè)試中，3個(gè)姿態(tài)角的精度可以控制在0.1°以內(nèi)，相比于EKF與傳統(tǒng)的mahony互補(bǔ)濾波，所提算法穩(wěn)定性能更好，魯棒性更強(qiáng)。

(2)在動(dòng)態(tài)測(cè)試中，3種算法均是利用陀螺儀動(dòng)態(tài)性能好但隨時(shí)間會(huì)有積分漂移的特點(diǎn)，使用加速度計(jì)對(duì)陀螺儀進(jìn)行實(shí)時(shí)修正，所提算法所得橫滾角與俯仰角中誤差在0.5°以內(nèi)精度優(yōu)于EKF與傳統(tǒng)mahony互補(bǔ)濾波；由于加速度計(jì)對(duì)航向角沒有修正作用，航向角會(huì)隨時(shí)間積分導(dǎo)致誤差累計(jì)，誤差在1.6°，相較于EKF與傳統(tǒng)mahony互補(bǔ)濾波精度提高了17.8%和21.5%。

該六軸姿態(tài)解算算法不但可以避開萬向鎖死結(jié)的問題，而且對(duì)數(shù)據(jù)的融合補(bǔ)償達(dá)到最大化的利用，同時(shí)相比拓展卡爾曼濾波具有較小的計(jì)算量，表現(xiàn)的精度效果更好，可以滿足傾斜RTK功能的使用。而針對(duì)加速度計(jì)和陀螺儀的高低頻特征需對(duì)其濾波處理，以及使用磁力計(jì)矯正航向角誤差和抗磁干擾內(nèi)容，將在后續(xù)的研究中進(jìn)一步改進(jìn)。