[发明专利]一种倾斜RTK航向初始化方法

权利要求书

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轨迹上相同历元下的对应点，组成多对向量，求出多对向量的夹角后，赋予多对向量的夹角不同的权重，确定出一个初始航向角误差的角度值。