[發明專利]一種傾斜RTK航向初始化方法

有效

• 申請號: 201911380356.1
• 申請日: 2019-12-27
• 公開（公告）號: CN111089587B
• 公開（公告）日: 2021-05-18
• 發明（設計）人: 陳起金;牛小驥;林歡;郭若南;賴昌鑫
• 申請（專利權）人: 武漢大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01C25/00;G01S19/49
• 代理公司: 武漢科皓知識產權代理事務所(特殊普通合伙) 42222
• 代理人: 王琪

1.一種傾斜RTK航向初始化方法，其特征在于，包括如下步驟：

步驟1，將測量桿平放于平坦整潔的地面上，靜止m分鐘，用于確定大的陀螺零偏初值；

步驟2，扶起測量桿，桿尖觸地，傾斜測量桿準備開始晃動，晃動開始前保持靜止m秒，用于確定初始的橫滾角和俯仰角；

步驟3，測量桿觸地，桿尖保持不動，頂端晃動，形成一段運動軌跡線，稱之為IMU軌跡線，然后通過如下步驟解算獲得INS初始航向誤差；

31)通過RTK解算得到天線相位中心的位置，并將上述位置投影到IMU中心，形成一段軌跡線，稱之為RTK軌跡；

32)利用定位傳感器的測量值解算IMU中心的位置，形成一段軌跡線，稱之為INS軌跡；

33)通過匹配RTK軌跡與INS軌跡，計算出INS的初始航向角誤差。

2.如權利要求1所述的一種傾斜RTK航向初始化方法，其特征在于：步驟32)得到具體實現方式如下，

傾斜RTK的IMU位置與測量桿底端觸底部位的位置關系描述如下：

式中，n表示當地坐標系n系，n系是以IMU相位中心為原點，x軸平行于當地水平面指向正北，y軸平行于當地水平面指向正東，z軸垂直于當地水平面向下，三者構成右手系；b表示載體坐標系b系，b系是以IMU相位中心為原點，x軸指向載體前進方向，y軸垂直于x軸指向載體右側，z軸與x軸和y軸垂直并構成右手系；表示測量桿觸地點的位置向量，投影在n系，T表示桿尖；表示IMU位置向量在n系下的投影；為姿態矩陣；l^b為IMU中心指向觸地點的向量，在b系下的投影；

進一步將IMU的位置推算公式改寫為

式中右邊只有姿態矩陣隨時間變化，因此只要求解出每個時刻的姿態即完成IMU位置的推算，姿態更新算法描述如下：

(3)式通過姿態四元數q更新姿態，其中，k-1表示上一時刻，k表示當前時刻，則當前時刻b系到n系的四元數進一步分解為上式所示的3個四元數：和分別表示n系和b系的姿態變化，是上一時刻的四元數；

其中，的更新忽略不計，b系的姿態變化簡化為：

其中，Δθ_k和Δθ_k-1分別是當前時刻和上一時刻的陀螺角增量輸出。

3.如權利要求1所述的一種傾斜RTK航向初始化方法，其特征在于：步驟33)的具體實現方式如下，

將RTK軌跡所確定的坐標系記為參考坐標系r系，將INS軌跡所確定的坐標系記為計算坐標系c系，r系與c系簡化為兩個共面的二維坐標系，兩個坐標系之間的夾角即為初始航向角誤差，選擇三參數：初始航向角誤差θ、北向平移距離N、東向平移距離E，構建如下坐標轉換矩陣：

其中表示c系到r系的坐標轉換矩陣，依據坐標轉換原理，建立如下觀測方程：

式中，s_i,r，s_i,c分別是RTK軌跡與INS軌跡坐標序列的平面投影，i表示第i個點，i＝1,2,3,4…n；Δ是觀測誤差；

雖然坐標轉換矩陣有6個參數，但獨立的參數只有航向角誤差θ、北向平移距離N、東向平移距離E這三個參數，因此將這三個參數作為估計參數：

式中，上標k表示迭代次數；

將觀測方程按泰勒公式展開，舍棄二階項和高階項得：

令

上式簡化為：

寫成殘差方程的形式，則為：

通過最小二乘法得到解算結果：

x＝(H^TH)^-1H^Tz (12)

X^k＝X^k-1+x^k (13)

其中，最小二乘法的初值通過向量匹配法獲得。

4.如權利要求3所述的一種傾斜RTK航向初始化方法，其特征在于：最小二乘法的初值通過如下方式獲得，

通過選取RTK軌跡和INS軌跡上相同歷元下的對應點，組成多對向量，求出多對向量的夾角后，賦予多對向量的夾角不同的權重，確定出一個初始航向角誤差的角度值。