CN111089587B - 一种倾斜rtk航向初始化方法 - Google Patents

Abstract

本发明提出了一种倾斜RTK应用场景下通过匹配RTK轨迹与INS轨迹计算INS初始航向角误差的方法，能够在2s内实现精度为1deg的航向角初始化。本发明提出的INS轨迹推算方法不使用加速度计，并且在测量开始阶段通过静止扣除大的零偏初值，保证了INS解算轨迹的精度。同时，本发明的方法操作简单、易于实现，只需原地来回晃动测量杆即可，超快速的初始化时间也大大地提高了测量效率。与一般倾斜RTK航向初始化方法相比，本发明的方法不需使用磁力计，不会受到磁场干扰，在复杂环境中具有更强的适应性。

Description

一种倾斜RTK航向初始化方法

技术领域

本发明属于MEMS INS/GNSS组合导航***航向初始化领域，特别涉及一种针对倾斜RTK的快速精确的航向初始化方法。

背景技术

惯性导航***(inertial navigation system,INS)是推算导航***的范例，具有自主性完全、可靠性高、动态性能好等优点，然而由于其误差会随时间不断积累，常常需要其他的导航手段进行辅助与校正。与INS相比，全球卫星导航定位***(global navigationsatellite system,GNSS)可以实现长时间较高精度的定位，误差不随时间积累；但也具有输出频率低、无法进行连续定位、无法输出姿态信息等缺点。将二者结合在一起，组成INS/GNSS组合导航***，可以提供连续、高带宽、长时和短时精度均较高的、完整的导航参数。

INS/GNSS组合导航***可应用于众多领域，以倾斜RTK应用场景为例，加入低成本的微机械(micro-electro-mechanical system，MEMS)惯导器件，即可组成MEMS INS/GNSS组合导航***；利用MEMS INS输出的姿态信息进行倾斜补偿，能够打破传统的RTK测量模式，大大提高作业效率。

传统GNSS RTK(real-time kinematic)能够达到厘米级的定位精度，可用于高精度位置测绘。但是在传统RTK测量过程中，需要严格对中并扶直测量杆，这不仅使得有些点(墙根点、地下管道管口内侧点等)无法进行测绘，还会导致作业效率低下。近两年，通过在RTK设备中集成惯性测量单元形成的“倾斜RTK”，可依靠INS或GNSS/INS组合给出的姿态角将天线相位中心的位置坐标补偿至杆尖，从而确定困难点的坐标，实现了可倾斜的RTK测量。倾斜RTK在测量过程中测量杆无需保持竖直，也无需长时间静止，在拓展RTK应用范围的同时也大大地提高了测量效率。

包括倾斜RTK在内的GNSS/INS组合导航***的应用过程中，航向初始化是一个关键点。由于INS的导航解算需要利用前一次的导航结果作为初始值，在首个历元解算时就必须进行初始化处理。对于INS而言，位置和速度的初始化较为简单，可以直接由外部信息源(如GNSS)提供；而姿态初始化较为复杂，一般由外部信息提供或者通过自对准获得。载体静止时，除了精度极低的惯导***外，其他类型的INS均可通过自对准实现横滚角与俯仰角的初始化，其本质是利用加速度计感知重力矢量。在姿态初始化过程中，最为困难的是航向角初始化。自对准通过陀螺感知地球自转矢量来确定北方向，此种方法对陀螺精度要求极高，陀螺零偏必须小于地球自转角速度15deg/h，尤其不适用于低成本低精度的MEMS INS。同时，由外部信息源获取的航向角精度并不理想，比如磁力计极易受到外部磁场的电磁干扰，而多天线GNSS定姿结果也常常包含着很大的噪声。

当然，对于某些特定的应用场景，可以使用特定的航向初始化方法：比如对绝大多数的车载导航而言，可以通过动对准进行航向初始化，此方法要求在车辆不转弯时按一定速度行驶且前进方向与载体x轴对齐。而在低速运动(如农用拖拉机)和手持设备的应用过程中，航向初始化仍然是一个关键点和亟需解决的问题。以倾斜RTK应用场景为例，思拓力的最新一代倾斜RTK测量仪S6Ⅱ通过磁校准确定北方向，容易受到磁场的干扰和影响；华测的惯导RTK需要手持设备前进10米，利用GNSS的位置与速度完成寻北。现有方案存在两个明显的缺点：一是使用磁力计寻北容易受到磁场干扰，可靠性较差；二是初始化时间较长，需要数秒至十几秒，且需要运动数米至十几米后才能使航向初始化精度达到可用水平，效率有待提高。

综上，在倾斜RTK应用场景中，通过INS或GNSS/INS提供测量杆精确的三轴姿态至关重要。由于MEMS INS无法进行自寻北(即航向初始化)，因此如何快速，精准地获得INS的初始姿态是需要解决的关键问题。本发明以倾斜RTK实例，包括但不限于此应用场景，提出一种简单有效的航向初始化方法。

发明内容

针对低速运动和手持设备的应用过程中INS航向快速准确初始化的需求以及现有技术所存在的不足，本发明提出了一种倾斜RTK应用场景下基于轨迹匹配原理确定INS初始航向误差的方法，实现精度为1deg的航向角初始化，可在2s之内完成航向初始化。

本发明采用如下的技术方案：一种基于轨迹匹配原理确定INS初始航向误差的方法，主要是通过晃动测量杆形成一段IMU的运动轨迹线，计算该运动轨迹的RTK轨迹与INS轨迹的角度偏差，得到INS的初始航向误差；所述的技术方案包括以下步骤：

步骤1，将测量杆平放于平坦整洁的地面上，静止m分钟，用于确定大的陀螺零偏初值；

步骤2，扶起测量杆，杆尖触地，倾斜测量杆准备开始晃动，晃动开始前保持静止m秒，用于确定初始的横滚角和俯仰角；

步骤3，测量杆触地，杆尖保持不动，顶端晃动，形成一段运动轨迹线，称之为IMU轨迹线，然后通过如下步骤解算获得INS初始航向误差；

31)通过RTK解算得到天线相位中心的位置，并将上述位置投影到IMU中心，形成一段轨迹线，称之为RTK轨迹；

32)利用定位传感器的测量值解算IMU中心的位置，形成一段轨迹线，称之为INS轨迹；

33)通过匹配RTK轨迹与INS轨迹，计算出INS的初始航向角误差。

进一步的，步骤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分别是当前时刻和上一时刻的陀螺角增量输出。

进一步的，步骤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)

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

进一步的，最小二乘法的初值通过如下方式获得，

通过选取RTK轨迹和INS轨迹上相同历元下的对应点，组成多对向量，求出多对向量的夹角后，赋予多对向量的夹角不同的权重，确定出一个初始航向角误差的角度值。

进一步的，所述m的取值为1。

本发明具有以下有益效果：

(1)本发明通过匹配RTK轨迹与INS轨迹计算INS的初始航向误差。RTK轨迹的精度高，其误差认为可忽略，因此航向角精度主要受到INS轨迹精度的影响。本发明提出的INS轨迹推算方法不使用加速度计，从而避免了加速度计误差对定位精度的影响，并使得INS轨迹的误差主要来源于常值陀螺零偏，而大的陀螺零偏初值，可以通过开始阶段的静止确定并扣除。因此本发明能够实现精度为1deg的航向角初始化，对于上述应用场景而言已经达到相当可观的精度。

(2)本发明的方法操作简单、易于实现。原地晃动测量杆时，理论上晃动幅度(指顶端IMU的运动幅度)越大越好；实际操作时由于测量杆长度为2米，晃动幅度能够很容易达到1米，已经能够达到较好的初始化效果。同时，本发明的方法对晃动速度没有要求，晃动时间也会根据***的实时解算结果反馈到终端，一般两秒左右即可完成初始化；因此操作人员不需要花费额外的精力，只需根据提示完成操作即可。

(3)本发明的方法初始化时间短，能够大大提高测量效率。现有的倾斜RTK航向初始化方法需要数秒至十几秒的时间，且需要运动数米至十几米后才能使航向初始化精度达到可用水平；而本发明的方法只需两秒即能达到1deg的初始化精度，使得作业效率得到了大幅提高。

(4)本发明的方法与一般倾斜RTK航向初始化方法相比，不需使用磁力计，不会受到磁场干扰，在复杂环境中具有更强的适应性。

附图说明

图1为倾斜RTK初始化过程中IMU的轨迹线示意图。

图2为INS轨迹与GPS RTK轨迹的相似性示意图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步说明。

在倾斜RTK的应用场景下，用户开机后，依据本发明的方法进行航向初始化。具体步骤如下：

1)设备开机后，将测量杆平放于平坦整洁的地面上，静止1分钟，用于确定大的陀螺零偏初值。

2)扶起测量杆，杆尖触地，倾斜测量杆准备开始晃动；晃动开始前保持静止1秒，用于确定初始的横滚角和俯仰角。

3)测量杆触地，杆尖保持不动，顶端(IMU端)晃动，形成一段运动轨迹线(称之为IMU轨迹线)，如图1所示。此时***进行实时解算，当航向精度可靠时给出提示，用户可停止晃动。一般只需晃动2秒左右即可确定INS初始航向。

所述的***实时解算进一步包括以下子步骤：

31)通过RTK解算得到天线相位中心的位置，并将上述位置投影到IMU中心，形成一段轨迹线(称之为RTK轨迹)。

32)利用定位传感器的测量值解算IMU中心的位置，形成一段轨迹线(称之为INS轨迹)；本发明提出如下解算方法确定INS轨迹：

倾斜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中心指向触地点的向量，在IMU坐标系(b系)下的投影。其中位置采用北东地形式(平面坐标与高程)。

在初始对准设计的运动中，

不随时间变化，给定一个粗略的位置初值即可，式中杆臂l^b是可精确量测的，因此，IMU的位置推算公式可改写为

式中右边只有姿态矩阵

随时间变化，因此只要求解出每个时刻的姿态即可完成IMU位置的推算。姿态更新算法描述如下：

(3)式通过姿态四元数q更新姿态。其中，k-1表示上一时刻，k表示当前时刻，则当前时刻b系到n系的四元数

可进一步分解为上式所示的3个四元数：

和

分别表示n系和b系的姿态变化，

是上一时刻的四元数。

b系的姿态变化可以简化为：

其中，Δθ_k和Δθ_k-1分别是当前时刻和上一时刻的陀螺角增量输出。

由于倾斜RTK的初始化运动特点是：速度小，位置变化有限，因此

的更新可以简化，甚至忽略。这样影响INS轨迹的主要误差只剩下常值陀螺零偏，大的陀螺零偏初值，可以通过开始阶段的静止算初值确定出来。

33)假设INS轨迹只受初始航向误差的影响，忽略其他传感器误差，则INS轨迹与RTK轨迹具有高度相似性，表现为两条轨迹形状基本一致，只是旋转了一个角度，如图2所示。这个夹角即为INS的初始航向角误差。因此，通过匹配RTK轨迹与INS轨迹，计算出INS的初始航向误差，所述的轨迹匹配算法包括但不限于向量匹配法、最小二乘法等，具体实现方式如下，

将RTK轨迹所确定的坐标系记为参考坐标系r系，将INS轨迹所确定的坐标系记为计算坐标系c系，r系与c系可简化为两个共面的二维坐标系，两个坐标系之间的夹角即为初始航向角误差。选择三参数(初始航向角误差θ、北向平移距离N、东向平移距离E)转化模型实现坐标系的转换。可构建如下坐标转换矩阵：

其中

表示c系到r系的坐标转换矩阵，依据坐标转换原理，可建立如下观测方程：

式中，s_i,r，s_i,c分别是RTK轨迹与INS轨迹坐标序列的平面投影，i(i＝1,2,3,4…n)表示第i个点；Δ是观测误差。需要注意的是，由于坐标转换矩阵

包含了平移信息，因此原本是二维的s_i,c扩展为三维向量。

虽然坐标转换矩阵

有6个参数，但独立的参数只有初始航向角误差θ、北向平移距离N、东向平移距离E这三个参数，因此将这三个参数作为估计参数：

式中，上标k表示迭代次数。

将观测方程按泰勒公式展开，舍弃二阶项和高阶项，可得：

令

上式可简化为：

写成残差方程的形式，则为：

通过最小二乘法即可解算结果：

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

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

其中，最小二乘法的初值可以但不限于通过向量匹配法给出，具体的做法是：通过选取RTK轨迹和INS轨迹相同历元下的对应点，组成多对向量；例如，假设初始对准的开始时间9:00，,晃动2秒钟，则可以得到9:00，9:01，9:02三个时间点的坐标，相同时间点下RTK轨迹和INS轨迹上的点为对应点，任一轨迹上的这三个点一共可以组成3个向量，不同轨迹上的向量组成3对向量；理论上讲，每对向量的夹角即为两个轨迹的夹角；求出多对向量的夹角后，依据一定的准则给予权重，可以确定出一个初始航向角误差的角度值；上述准则包括但不限于向量长度、时间先后顺序等。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (4)

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

Images