一種慣導裝置初始對準方法
【技術領域】
[0001] 本發明針對農業機械和工程機械領域傳統初始對準算法在較低成本和性能的慣 導裝置不適用的問題，設計發明了一種針對上述領域主要是農業機械的初始對準方法。
【背景技術】
[0002] 隨著MEMS(Micro-Electro-Mechanical-System)傳感器、導航和控制技術的發展 以及國家對農業扶持力度的進一步加大，精準農業正在快速變成一種趨勢，而在農業機械 輔助駕駛控制過程中，車體的姿態（包括俯仰角、翻滾角和航向角）、速度和位置信息能夠 實時反映出車體的運動和位置信息，這些信息能夠為高精度的組合導航和控制算法提供重 要的數據輸入。
[0003] 捷聯慣性導航（Strapdown-Inertial-Navigation-System，SINS)具有自主導航、 保密性好、抗干擾能力強、導航參數豐富和短時間內精度高等特點被廣泛應用，但是由于慣 性傳感器固有誤差的存在，使得導航誤差隨著時間積累長時間導航精度較差，需要其他誤 差穩定的導航系統輔助，例如高精度GPS-RTK。慣性導航系統是根據測得的車體加速度，經 過積分運算求得速度和位置。為此，必須知道初始速度和位置。此外，以地理坐標系為導航 坐標系的慣性系統中，物理平臺和數學平臺都是測量加速度的基準，而且平臺必須準確地 對準和跟蹤地理坐標系，以避免平臺誤差引起加速度測量誤差。初始對準的精度直接關系 到導航系統的工作精度，也是重要關鍵技術之一。
[0004] 目前常用的對準方法有粗對準和精對準兩個步驟，但是這兩種對準方法主要針對 的是高精度的陀螺儀能夠感應到地球自轉角速度，明顯在低成本和低性能的慣導裝置中， 這兩種方法并不適用。
【發明內容】
[0005] 本發明針對農業機械和工程機械領域傳統初始對準算法在較低成本和性能的慣 導裝置不適用的問題，設計發明了一種針對上述領域主要是農業機械的初始對準方法。
[0006] 本發明的技術方案為，一種慣導裝置初始對準方法，包括如下步驟：
[0007] 提供一裝載有傳感器的器械，并對傳感器進行預處理；
[0008] 進行相對對準處理，以求得傳感器的安裝誤差角；
[0009] 進行絕對對準處理，以求得傳感器的安裝姿態角誤差，進而提高相對對準計算出 的誤差姿態角精度。
[0010] 上述的方法，其中，對傳感器進行預處理的步驟包括：
[0011] 對傳感器進行初始化設置；
[0012] 對傳感器進行濾波處理，以減小由于器械震動對傳感器的影響。
[0013] 上述的方法，其中，進行相對對準的步驟包括：
[0014] 將器械朝一個方向靜止，采集三軸加速度數據，然后將器械掉頭180°，采集三軸 加速度數據；
[0015] 進行傳感器的安裝誤差角度計算，求得傳感器在不同方向上的安裝誤差角度。
[0016] 上述的方法，其中，求得安裝誤差角度的公式為：
[0017]
[0018]
[0019]
[0020]
[0021] 其中，A、B、C為模型系數，力、#、.及分別為傳感器中加速度計輸出值，Y、0、 夢分別為傳感器的安裝誤差角度，g為地球重力加速度，a為器械平面與水平面的夾角。
[0022] 上述的方法，其中，進行最小二乘法數據擬合，以得到可計算出模型系數A、B、C的 公式：
[0023]
[0024] 其中，方、#、及分別為傳感器中加速度計輸出值，n為總的采樣個數，i為采 樣點，i= 1，2.....，n，n為正整數。
[0025] 上述的方法，其中，器械平面與水平面的夾角a計算方式為：
[0026]假設第一次器械停放的航向角為0，利用停放角度為0時的三軸加速度數據和 (0 + 31)時的三軸加速度數據來求得器械平面與水平面的夾角a，a的求解公式為：
[0027]
a
[0028] 上述的方法，其中，進行絕對對準的步驟包括：
[0029] 絕對對準卡爾曼濾波模型建立：
[0030] 卡爾曼濾波狀態向量
[0031] 其中，SX誤差系統向量，Sp、SV、S!tnb分別為位置誤差、速度誤差和姿態角誤 差，域和^{分別為加速度和陀螺儀零偏，T為轉置；
[0032] 卡爾曼濾波觀測向量：y= [pTvT];
[0033] 其中，p為GPS位置信息，v為GPS速度信息；
[0034] 卡爾曼濾波系統更新方程為
[0035]
[0036]
[0037]
[0038] 其中，多1、彡[、分別對應為Sp、Sv、Sil)nb的導數為器械坐標系到導 航坐標系旋轉矩陣，^分別為傳感器輸出的加速度和角速度；
[0039] 由相對對準中計算的初始姿態角a作為卡爾曼濾波狀態向量中S也nb的初始值， 然后按卡爾曼濾波系統更新方程來更新系統向量以及進行卡爾曼濾波，當卡爾曼濾波收斂 后，計算出傳感器的安裝姿態角誤差S
[0040] 上述的方法，其中，所述器械為農用器械；
[0041] 所述農用器械為拖拉機。
[0042] 本發明通過相對對準和絕對對準，相對對準主要是解決MEMS-頂U傳感器坐標系 與拖拉機坐標系的對準，絕對對準解決的是拖拉機坐標系與導航坐標系的對準。相對對準 過程計算出相對的誤差姿態角，相對誤差姿態角作為絕對對準過程中狀態向量中姿態誤差 的初始值從而可以加快卡爾曼濾波收斂速度，通過絕對對準過程可以進一步提高對準精 度。
【附圖說明】
[0043] 通過閱讀參照以下附圖對非限制性實施例所作的詳細描述，本發明及其特征、外 形和優點將會變得更明顯。在全部附圖中相同的標記指示相同的部分。并未刻意按照比例 繪制附圖，重點在于示出本發明的主旨。
[0044] 圖1為本發明公開的一種慣導裝置初始對準方法的示意圖；
[0045] 圖2為本發明進行相對對準處理的流程圖；
[0046] 圖3為本發明進行絕對對準處理的流程圖。
【具體實施方式】
[0047] 在下文的描述中，給出了大量具體的細節以便提供對本發明更為徹底的理解。然 而，對于本領域技術人員而言顯而易見的是，本發明可以無需一個或多個這些細節而得以 實施。在其他的例子中，為了避免與本發明發生混淆，對于本領域公知的一些技術特征未進 行描述。
[0048] 為了徹底理解本發明，將在下列的描述中提出詳細的步驟以及詳細的結構，以便 闡釋本發明的技術方案。本發明的較佳實施例詳細描述如下，然而除了這些詳細描述外，本 發明還可以具有其他實施方式。
[0049] 本發明提供了一種慣導裝置初始對準方法，參照圖1所示，主要包括如下步驟：
[0050] 步驟S1 :提供一裝載有傳感器的器械，并對傳感器進行預處理。
[0051] 在發明一可選的實施例中，對傳感器進行預處理的步驟包括：步驟Sla:對傳感器 進行初始化設置；步驟Sib:對傳感器進行濾波處理，也即圖示的特殊處理，以減小由于器 械震動對傳感器的影響。
[0052] 步驟S2:進行相對對準處理，以求得傳感器的安裝誤差角。
[0053] 在本發明一可選的實施例中，結合圖2所示，進行相對對準處理的步驟包括：首先 將器械朝一個方向靜止，采集三軸加速度數據；然后將器械掉頭180°，采集三軸加速度數 據；進行傳感器的安裝誤差角度計算，求得傳感器在不同方向上的安裝誤差角度