基于互補濾波算法的傾斜RTK姿態解算

劉坤之，唐詩華*，張炎，李灝楊，呂富強

(1.桂林理工大學測繪地理信息學院，桂林 541004;2.廣西空間信息與測繪重點實驗室，桂林 541004)

RTK(real time kinematic)是一種利用GPS載波相位觀測值進行實時動態相對定位的技術，可以實時提供指定坐標系下觀測點的三維坐標。傾斜RTK是通過內置慣性測量單元(inertial measurement unit,IMU)，根據內部慣性器件計算的姿態角生成方向余弦矩陣，從而實現空間矢量的坐標轉換，將天線相位中心位置補償到地面測量點，得到待測點坐標。解決了傳統RTK測量過程中在特殊作業情況下不能保持對中桿豎直，出現定位誤差較大的問題。

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

1 基于互補濾波的姿態解算原理

1.1 空間姿態描述

(1)

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

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

(2)

(3)

θ=arcsin2(q0q2-q1q3)

(4)

(5)

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

1.2 四元數姿態求解

傳感器姿態的變化即為四元數的變化，需要構建四元數關于時間的微分方程研究此類問題。由于計算機中的計算是離散的，需要采用一階龍格庫塔法對微分方程離散化處理[7]，從而實現四元數的更新計算。計算公式為

(6)

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

1.3 傳感器數據融合

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

圖1 誤差補償原理圖Fig.1 Schematic diagram of error compensation

四元數和理論上的重力加速度分量，是可以相互推導，具體的變換關系式為

(7)

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

加速度計實測重力加速度與理論重力加速度二者之間出現的偏差，很大程度是因為陀螺儀數據產生的角速度誤差引起的，產生的偏差可以通過向量外積補償解算陀螺儀數據的誤差[9]，可表示為

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

(8)

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

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

|ρ|≈α=error

(9)

式(9)中：error為誤差補償值，該補償值不能直接應用到陀螺儀的數據中，因此構建比例積分補償器來控制補償值的大小和精度。控制器表達式為

(10)

式(10)中：δ為陀螺儀誤差補償量；kP為比例控制項；kI為積分控制項；dt為對時間的積分。

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

2 基于DE算法的PI控制參數尋優

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

2.1 DE算法基本原理

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

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

j=1,2，…,D}

(11)

(12)

步驟2實現個體變異。設迭代次數為G，在第g代迭代中，隨即從群里中選取一定數量的個體，把所選個體作為目標變量進行矢量變異，同時為了加強DE算法的搜索能力，避免陷入局部最優解，對搜索步長的控制參數縮放變異因子合理取值，更新傳統的差分進化算法，其表達式為

(13)

(14)

步驟4選擇操作。將父代個體與子代個體的一一對應選擇，選取適應度值較小的個體進行下一代搜索，組成新一代種群，實現選擇操作，其表達式為

(15)

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

2.2 適應度函數選取

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

(16)

式(16)中：A為姿態角；xobs為解算的姿態角；xref為參考姿態角；m為采樣點數。

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

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

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

圖3 適應度函數值Fig.3 Fitness function value

3 實驗驗證及結果分析

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

圖4 模塊搭載平臺Fig.4 Module carrying platform

3.1 靜態對比實驗

通過全站儀氣泡居中的方法將模塊水平安裝放置，利用模塊內置安裝角誤差矯正功能補償校準，則系統輸出的俯仰角和橫滾角的參考值均為0°，航向角在系統初始時刻參考值也為0°。在靜止狀態下，圖5為3種算法的三軸姿態解算對比結果，mahony算法解算姿態角在參考角度附近上下浮動，相較于EKF算法稍穩定些；EKF算法解算姿態角浮動較大，穩定性較差，過程噪聲影響較大；DE-mahony算法解算姿態角在參考角度和0°附近浮動，浮動范圍小，相對較穩定。

圖5 靜態三軸角度Fig.5 Static triaxial angle

為了更直觀的比較3種算法的解算精度，靜態情況下使用均值比較法對比分析估算精度，結果如表1所示。可以看出，使用所提出的DE-mahony算法的解算的三軸精度優于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種算法解算的三軸靜態姿態角度精度均小于0.1°，可以滿足傾斜RTK日常工作需求。

表1 靜態解算結果Table 1 Static solution results

3.2 動態對比試驗

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

圖6 動態三軸角度Fig.6 Dynamic triaxial angle

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

表2 動態解算結果Table 2 Dynamic solution results

4 結論

針對低成本六軸慣性傳感器解算姿態的問題，提出了一種基于差分進化智能尋參的互補濾波算法，將慣導模塊中的加速度計與陀螺儀利用PI補償器進行信息融合，通過四元數計算出3個姿態角的變化，為傾斜RTK復雜環境的運動提供狀態信息。得出如下結論。

(1)在靜態測試中，3個姿態角的精度可以控制在0.1°以內，相比于EKF與傳統的mahony互補濾波，所提算法穩定性能更好，魯棒性更強。

(2)在動態測試中，3種算法均是利用陀螺儀動態性能好但隨時間會有積分漂移的特點，使用加速度計對陀螺儀進行實時修正，所提算法所得橫滾角與俯仰角中誤差在0.5°以內精度優于EKF與傳統mahony互補濾波；由于加速度計對航向角沒有修正作用，航向角會隨時間積分導致誤差累計，誤差在1.6°，相較于EKF與傳統mahony互補濾波精度提高了17.8%和21.5%。

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