一种基于轮速惯导联合预积分并与RTK紧组合的定位方法与流程

一种基于轮速惯导联合预积分并与rtk紧组合的定位方法
技术领域
1.本发明涉及导航技术领域，具体涉及一种基于轮速惯导联合预积分并与rtk紧组合的定位方法。


背景技术：

2.车辆自动驾驶时，获得无人车的位置坐标、速度、姿态等状态是必不可少的，且为能够精准控制无人车自动驾驶，无人车的定位精度有较高的要求。卫星导航通过测量卫星到接收机之间伪距进行定位，但卫星定位通常多种误差因素，如：接收机时钟误差、卫星时钟误差、多径效应误差、通道误差、星历误差、内部噪声误差等等，均会影响卫星的定位精度。
3.常规提高精度是通过结合rtk技术，以更好的消除卫星导航中的误差，提高定位精度。但rtk的定位方法原理是通过测量多个卫星到gnss接收机之间的距离来进行定位，抵消单个卫星定位出现的误差，但是当接收机和卫星之间存在遮挡时，rtk的测距精度会受到影响，从而导致整体的定位精度有所降低。且rtk定位技术是建立在流动站与基准站误差非常类似的基础上的，它利用gnss载波相位观测值进行实时动态定位，随着基准站和流动站距离增大，位精度也胡会越来越低。
4.为克服上述问题，提出了将卫星定位技术和惯性导航结合使用，通过将卫星定位与惯性定位组合使以提高卫星定位的精度。但惯性导航使用惯性测量单元(imu)测量自身的角速度和加速度，然后通过惯导算法(ins)实现短距离精准定位，通过将卫星定位导航技术与惯性导航技术相互结合(度诶卫星定位误差进行修正)，以提高定位精度。
5.但ins算法非常快的会发散，车辆移动的距离越远，惯性导航的误差越大，导致ins的可信时间比较短。但若使用高精度的imu，其价格比较昂贵，低成本的imu精度较差，在进行长时间的导航定位时会出现漂移现象，导致误差积累越来越大，ins的可信时间较短，提高定位精度的效率越低。


技术实现要素：

6.有鉴于此，本发明要解决的问题是提供一种基于轮速惯导联合预积分并与rtk紧组合的定位方法，能够将轮速计与imu的预积分进行紧组合，以持续对imu积累的误差进行补偿，提升预积分算法的性能，实现长效高精度定位。
7.为解决上述技术问题，本发明采用的技术方案是：
8.一种基于轮速惯导联合预积分并与rtk紧组合的定位方法，步骤一，接收卫星导航数据并构建表示卫星导航误差量的观测残差；
9.步骤二，通过轮速计以单独获取每个时段内的里程增量，并结合惯性传感器进行联合预积分，以生成包含传感器和轮速计整体误差量的预积分残差；
10.步骤三，预积分残差与观测残差紧组合以生成矫正因子，依据矫正因子矫矫正卫星定位。
11.进一步的，所述联合预积分包括有速度增量β，位移增量α、姿态变化量q和里程增量s，并分别构建β、α、q和s的传播方程，所述里程增量s的传播方程为：
12.其中：表示在惯性导航坐标系b下，k-1时刻到i时刻之间的位移量，表示在惯性导航坐标系b下，k-1时刻到i-1时刻之间的位移量，所述表示在惯性导航坐标系b下，i-1时刻到i时刻之间的位移量，以单独获取每个时段内的里程增量，
13.所述里程增量的传播方程为：所述里程增量的传播方程为：
14.其中和均为旋转矩阵，用于将设备坐标系v转到惯性导航坐标系b下，表示在设备坐标系v下，i-1时刻到i时刻之间的位移量，1时刻到i时刻之间的位移量，表示在惯性导航坐标系b下，矫正轮速计与惯性传感器其位置误差的杆臂矫误差量。
15.进一步的，所述观测残差包括有双差伪距观测残差和双差相位观测残差依据所述联合预积分构建在卫星坐标系下imu/轮速计联合预积分残差所述矫正因子为：
16.进一步的，所述imu/轮速计联合预积分残差进一步的，所述imu/轮速计联合预积分残差其中表示在大地坐标系下imu/轮速计联合预积分观测因子的协方差矩阵，表示包括有位置变化残差、速度变化残差、姿态变化残差、加速度计的零偏残差、陀螺仪的零偏残差和轮速计的位移残差的残差矩阵。
17.进一步的，所述步骤一之前增设有初始化策略，所述初始化策略包括有获取设备比初始坐标和初始姿态；
18.所述初始坐标包括有获取若干组卫星导航的定位坐标数据并取其平均值，获取加速度计数据以计算出重力矢量，获取出重力矢量与设备坐标系的z轴和oxy平面之间的夹角，以分别生成包含横滚角和俯仰角的初始状态。
19.本发明具有的优点和积极效果是：
20.通过在惯性导航的联合预积分内增加轮速计获取的里程增量传播公式，且里程增量传播公式通过单独获取每个时段内的里程增量，并可有效的抵消该时段内惯性导航数据叠加产生的叠加误差，以持续对imu积累的误差进行补偿，延长ins算法发散时间，进而提升预积分算法的性能，实现长期有效的高精度定位。
附图说明
21.附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
22.图1是本发明的一种基于轮速惯导联合预积分并与rtk紧组合的定位方法的整体流程框图。
具体实施方式
23.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
24.需要说明的是，当组件被称为“固定于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件，它可以是直接连接到另一个组件或者可能同时存在居中组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。
25.除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。
26.本发明提供一种基于轮速惯导联合预积分并与rtk紧组合的定位方法，如图1所示，设备上安装有用于卫星定位的卫星传感器(gnss接收机)，并构建双差观测方程(双差伪距观测方程)，双差观测方程用于计算移动设备在卫星坐标系下的位置坐标，后依据双差伪距观测方程构建双差伪距观测残差双差伪距观测残差用于表示通过伪距定位时会产生的位置误差量(有也就是卫星定位时产生的误差)。
27.为提高卫星的定位精度，结合rtk技术进行定位，包括构建有双差载波相位观测方程，在依据双差载波相位观测方程构建双差相位观测残差相同的，双差相位观测残差用于表示定位过程中产生的误差量。
28.定位时，gnss接收机接收定位数据，定位数据代入双差伪距观测残差和双差相位观测残差内计算出定位的误差量(也可以叫做第一矫正量)，并将定位数据带入双差伪距观测方程生和双差载波相位观测方程以生成定位坐标，并通过误差量对定位坐标进行矫正，
提高卫星定位精度。
29.当设备移动到有遮挡的位置时，第一矫正量的准起度降低，影响定位准精度。为进一步提高设备的定位精度，设备上还安装有惯性传感器(陀螺仪和加速度计)和轮速计，通过陀螺仪用于获取设备的倾斜情况(以确定设备的姿态)，加速度计用于获取设备的移动的加速度全款(以确定设备的移动距离或设备的位置向量)，轮速计获取设备的行驶路程。并共同构建imu/轮速计联合预积分公式：
[0030][0031][0032][0033][0034][0035]
其中有：表示速度增量的β，位移增量的α、姿态变化量的q和里程增量的s的预积分方程，其中：表示在惯性坐标系下，k-1时刻到i时刻之间速度的变化量(由加速度计的参数获取)，表示在惯性坐标系下，k-1时刻到i时刻之间位置的变化量(由加速度计的参数获取)，表示在惯性坐标系下，k-1时刻到i时刻之间姿态的变化量(由陀螺仪的参数获取)。
[0036]
联合预积分内增加有计算路程位移量的位移变化量公式，示在惯性坐标系下，k-1时刻到i时刻之间路程的位移量(由轮速计的参数获取)，可单独计算两个时刻之间的位移变化量(并非计算整体的位移量，避免的数据叠加产生的误差积累)。通过在联合预积分内增加轮速计记录的设备的位移变化量，可抵消惯性定位中的误差增量，延长惯性定位的发散时间，可有效的提高预积分精度。
[0037]
的传播方程为其中表示在惯性导航坐标系b下，k-1时刻到i-1时刻之间的位移量，表示在惯性导航坐标系b下，i-1时刻到i时刻之间的位移量，以单独获取每个k-1时刻到i－1之间和i－1时刻到i时刻之间的位移量并叠加，通过单独获取两个相邻时段之间的位移量(k-1时刻到i-1时刻和i-1时刻到i时刻，设备单独的位移量)，并非单独获取k-1时刻设备的位移量和i时刻设备的位移量，来计算设备k-1时刻到i时刻之间的位移量，大大降低了轮速计由于路程叠加产生的叠加误差，进一步提高imu/轮速计联合预积分的精度(抵消部分惯性导航的叠加误差)。
[0038][0039]
其中和均为旋转矩阵，用于将设备坐标系v转到惯性导航坐标系b下，
表示在设备坐标系v下，i-1时刻到i时刻之间的位移量，1时刻到i时刻之间的位移量，表示在惯性导航坐标系b下，矫正轮速计与惯性传感器其位置误差的杆臂矫误差量。
[0040]
在依据imu/轮速计联合预积分构建imu/轮速计联合预积分残差，其公式为其中表示表示在大地坐标系下，imu/轮速计联合预积分观测因子的协方差矩阵，表示包括有位置变化残差、速度变化残差、姿态变化残差、加速度计的零偏残差、陀螺仪的零偏残差和轮速计的位移残差的残差矩阵。
[0041]
残差矩阵为：
[0042][0043]
其中表示东北天坐标系n到k-1时刻的惯导坐标系之间的姿态旋转矩阵；表示的是k时刻的惯导坐标系相对于n系的位置向量在n系下的投影；表示的是k-1时刻的惯导坐标系相对于n系的位置向量在n系下的投影；δtk是k-1到k时刻之间的时间；gn表示n系下的重力向量；是用一阶近似修正的预积分的位置结果，第一列公式计算出的是k-1到k时刻之间的位置残差。
[0044]
相同，表示的是k时刻的惯导坐标系相对于n系的速度向量在n系下的投影；表示的是k-1时刻的惯导坐标系相对于n系的速度向量在n系下的投影；是用一阶近似修正的预积分的速度结果，第二列公式计算出的是k-1到k时刻之间的速度残差。
[0045]
表示k时刻的惯导坐标系相对于n系的姿态；表示k-1时刻的惯导坐标系相对于n系的姿态；是用一阶近似修正的预积分的姿态结果，第三列公式计算出的是k-1到k时刻之间的姿态残差。
[0046]
ba(tk)表示k时刻的加速度计零偏；ba(t
k-1
)表示k-1时刻的加速度计零偏，第四列公式计算出的是k-1到k时刻之间的加速度计零偏残差。bg(tk)表示k时刻的陀螺仪零偏，bg(t
k-1
)表示k-1时刻的陀螺仪零偏，第五列公式计算出的是k-1到k时刻之间的陀螺仪零偏残差。
[0047]
是用一阶近似修正的预积分里程结果，第六列公式计算出的是k-1到k时刻之间轮速计的里程残差，s
odo
(tk)表示k时刻轮速计位移，s
odo
(t
k-1
)表示k-1时刻轮速计位移，第七列公式计算出的是k-1到k时刻之间的轮速计位移残差。
[0048]
通过叠加双差伪距观测残差、双差相位观测残差和imu/轮速计联合预积分残差以生成总的矫正因子，使用矫正因子进一步矫正卫星定位坐标，即使长时间行驶或移动到遮挡物较多的位置时，也能够保证卫星的定位精度。
[0049]
为进一步提高设备的定位精度，可在进行第一次组合导航之前对系统进行初始化；首先将无人车静止放置在开阔空间中，采集100组来自rtk的数据使用最小二乘法得到100个坐标，通过取平均值的方式对位置进行初始化(精准确定设备的初始位置)；无人车的初始姿态是从加速度计的数据中得到的，通过测量重力矢量和无人汽车载体坐标系z轴和oxy平面之间的角度确定的横滚角和俯仰角；当无人汽车刚开始运动时，可以通过rtk和加速度计的数据得到无人车的航向角。得到横滚角和俯仰角的公式为：
[0050][0051]
此外由于imu，gnss天线的安装位置和无人车载体坐标系的原点是不重合的，因此还需要标定传感器的齐次变换矩阵t，将传感器的坐标系转换到载体坐标系上；
[0052][0053]
以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明范围所作的均等变化与改进等，均应仍归属于本专利涵盖范围之内。