复杂环境下的高精度定位方法与流程

1.本发明属于导航技术领域，具体涉及一种复杂环境下的高精度定位方法。


背景技术：

2.随着经济技术的发展和人们生活水平的提高，定位和导航服务已经广泛应用于人们的生产和生活当中，给人们的生产和生活带来了无尽的便利。因此，保证定位和导航的精确性，就成为了研究人员的研究重点。
3.目前，在实际应用中，实际环境存在许多弱gnss（global navigation satellite system，全球导航卫星系统）信号或无gnss信号的区域，比如隧道、高架、车库等；在这些区域，如果单纯使用纯gnss定位，则定位系统无法实时提供高精度的定位结果。因此，目前一般采用融合多种传感器的综合定位方式，来保证实时定位的鲁棒性和高可靠性。
4.但是，目前的多源融合定位方法，一般都存在部分缺陷：目前采用的vio（visual-inertial odometry，视觉-惯性轨迹法）与gnss融合的方法，在无gnss信号区域，定位系统退化为纯vio系统，由于地面载体近似平面，单目vio系统会出现额外的不可观自由度，导致定位漂移；此外，目前的vio与gnss的融合多以松耦合或半紧组合为主，在复杂环境下无法提供高鲁棒的定位结果。


技术实现要素：

5.本发明的目的在于提供一种可靠性高、精确性好且鲁棒性好的复杂环境下的高精度定位方法。
6.本发明提供的这种复杂环境下的高精度定位方法，包括如下步骤：s1. 获取gnss传感器、vison视觉传感器、sins惯性传感器和wheel轮式里程计的原始观测数据，并对传感器数据进行预处理和同步；s2. 根据获取的传感器数据，进行vison/sins/wheel联合动态初始化；s3. 根据步骤s2的初始化结果，将视觉传感器数据、惯性传感器数据、轮式里程计数据与gnss传感器数据进行对齐；s4. 构建惯性传感器和轮式里程计的预积分模型；s5. 基于构建的预积分模型，将gnss原始观测残差、视觉残差、惯性预积分模型、轮式里程计模型和平面约束进行聚合，构建非线性残差模型；s6. 实时求解构建的非线性残差模型，完成复杂环境下的高精度定位。
7.所述的步骤s1，具体包括如下步骤：gnss传感器的原始观测数据包括卫星星历数据、载波相位原始观测数据、gnss基站差分数据、实时模糊度解算数据和多普勒原始观测数据；vison视觉传感器的原始观测数据为摄像头实时传输的灰度图像数据；sins惯性传感器的原始观测数据为imu输出数据；wheel轮式里程计的原始观测数据为车辆实时的左右轮转速数据；对gnss原始观测数据进行预处理，剔除异常数据；然后基于pps授时系统，对四种传感器数据进行时间同步。
8.所述的步骤s2，具体包括如下步骤：a. 在滑窗中，利用纯视觉sfm（structure from motion，运动恢复结构）方法，计算得到相对尺度的相机位姿和特征点坐标；b. 根据步骤a得到的计算结果，标定陀螺仪的零漂值；c. 根据视觉、imu和轮式里程计的初始化速度、重力加速度和尺度因子，构建初始化向量；d. 利用当地的实际重力加速度，对步骤c构建的初始化向量进调整，并恢复到真实尺度并对齐，最终得到精确的局部世界坐标系下的初始化结果。
9.所述的步骤c，具体包括如下步骤：根据视觉、imu和轮式里程计的初始化速度、重力加速度和尺度因子，构建待初始化量为，其中为第k帧的imu坐标系下的速度，为第c0帧的重力加速度，s为尺度因子；然后，在滑窗中连续两帧满足如下方程1：然后，在滑窗中连续两帧满足如下方程1：然后，在滑窗中连续两帧满足如下方程1：式中为imu的位置预积分量；为c0相对第k帧imu的旋转；为第k帧imu相对c0帧的位置；为两帧之间的时间间隔；为第k帧imu相对c0帧的旋转；为imu的速度预积分量；为轮式里程计的预积分量；为第k帧odom相对c0帧的旋转；为第k帧odom相对c0帧的位置；为第k帧odom的速度；将方程1写成待优化量的线性表达式，然后转换成求解线性最小二乘，得到最优的初始化量，完成速度、重力加速度和尺度因子的初始化。
10.所述的步骤s3，具体包括如下步骤：a. 基于载波相位双差模型，计算得到粗略的锚点位置；b. 根据vison/sins/wheel联合动态初始化的结果、多普勒观测值和卫星的速度，计算得到enu坐标系与局部世界坐标系之间的偏航角；c. 根据vison/sins/wheel联合动态初始化的结果，对齐局部世界坐标系与全局ecef坐标系，完成对锚点位置的精调。
11.所述的步骤a，具体包括如下步骤：求解以下模型，计算得到全局ecef坐标系中的粗略锚点位置：
式中为接收机与基站对第i颗卫星的载波相位观测之差与第j颗卫星的载波相位观测之差的差值；为载波波长；为基站对卫星i的观测矢量；b
ur
为接收机当前位置与基站位置的相对偏差，为待求解的量；为第i颗卫星与第j颗卫星观测的整周模糊度的差值；n为滑窗的大小；k为滑窗序号；m为第k个滑窗中载波相位原始观测量数目；为马氏范数。
12.所述的步骤b，具体包括如下步骤：求解以下模型，计算得到偏航角：求解以下模型，计算得到偏航角：式中为接收机频漂；为载波波长；fd为多普勒原始观测量；为接收机对卫星i的观测矢量；为第i颗卫星的速度；为局部世界坐标系到enu坐标系的转换关系；为局部世界坐标系到enu坐标系的转换关系；vw为局部世界坐标系下的载体速度；为第i颗卫星的频漂；n为滑窗的大小；k为滑窗序号；m为第k个滑窗中载波相位原始观测量数目；为马氏范数；yaw为偏航角。
13.所述的步骤c，具体包括如下步骤：求解以下模型，计算得到精确的锚点位置，完成局部世界坐标系与全局ecef坐标系的对齐：式中为锚点的粗略坐标；为基站对卫星i的观测矢量；为载波波长；为局部世界坐标系到ecef坐标系的转换关系；pw为局部世界坐标系下的载体位置；为基站的坐标；为第i颗卫星与第j颗卫星观测的整周模糊度的差值；为接收机与基站对第i颗卫星的载波相位观测之差与第j颗卫星的载波相位观测之差的差值；n为滑窗的
大小；k为滑窗序号；m为第k个滑窗中载波相位原始观测量数目；为马氏范数。
14.所述的步骤s4，具体包括如下步骤：根据imu预积分模型，计算得到两帧之间imu预积分量模型如下：两帧之间imu预积分量模型如下：两帧之间imu预积分量模型如下：两帧之间imu预积分量模型如下：式中为位置预积分量；bk为k时刻的imu；为t时刻相对于bk的旋转量；为加速度计的输出值；b
at
为加速度计的零漂；na为加速度计的高斯白噪声；为角速度计的输出值；为角速度计的零漂；为角速度计的高斯白噪声；为t时刻相对于bk的旋转量；tk为k时刻；为速度预积分量；为旋转预积分量；表示叉积。
15.根据线速度和角速度建模，得到轮式里程计的预积分模型如下：根据线速度和角速度建模，得到轮式里程计的预积分模型如下：式中为位置预积分量；ok为k时刻的odom；为t时刻相对于ok的旋转量；为线速度；nv为线速度的高斯白噪声；为旋转预积分量；为偏航角速度；为偏航角速度的高斯白噪声；为t时刻相对于ok的旋转。
16.所述的步骤s5，具体包括如下步骤：采用如下算式作为非线性残差模型：
式中x为待优化的估计量；为边缘化后的先验信息；r
p
为边缘化残差；h
p
为边缘化更新矩阵；为马氏距离；n为残差约束数量；为传感器测量值的残差函数；当传感器为sins惯性传感器时，取值为：式中为位置增量；为速度增量；为角度增量；为加速度计bias增量；为角速度计bias增量；为局部世界坐标到第k帧imu的旋转；为第k帧imu在局部世界坐标下的位置；为第k帧imu在局部世界坐标系下的速度；为两帧之间的时间间隔；gw为重力加速度；为位置预积分量；为速度预积分量；为第k帧imu坐标到局部世界坐标的旋转量；为旋转预积分量；为四元素的虚部；为第k帧的加速度零漂；为第k帧陀螺仪的零漂；为四元素乘法；当传感器为wheel轮式里程计时，取值为：
式中为位置增量；为角度增量；为局部世界坐标到第k帧odom的旋转；为第k帧轮速里程计在局部世界坐标下的位置；为第k帧轮速里程计在局部世界坐标系下的；为位置预积分量；为第k帧轮速里程计坐标到局部世界坐标的旋转量；为旋转预积分量；当传感器为vision视觉传感器时，取值为：式中为第j个相机坐标系路标点的归一化相机坐标；为第j个相机坐标系下路标点三维坐标；x
cj
、y
cj
和z
cj
的计算公式为，式中为相机与imu坐标系的旋转参数，为局部世界坐标到第j帧imu的旋转，为第i帧 imu到局部世界坐标系的旋转量，为相机到imu的旋转，为逆深度值，为相机与imu的平移参数，为第i帧imu在局部世界坐标系的坐标，为局部世界坐标系与第j帧imu平移，为imu与相机坐标系的平移参数；当传感器为gnss传感器时，取值为：
式中为载波波长；为第i颗卫星的观测矢量；为接收机的ecef坐标；为第i颗卫星与第j颗卫星观测的整周模糊度的差值；为接收机与基站对第i颗卫星的载波相位观测之差与第j颗卫星的载波相位观测之差的差值；为基准站的ecef坐标，且，为局部世界坐标系到enu坐标系的转换关系；为局部世界坐标系到enu坐标系的转换关系，为第k帧imu坐标在局部世界坐标系下的位置，为锚点坐标；需要随机平面约束因子时，取值为：式中为设定的矩阵，且；为odom到imu坐标系的旋转量；为imu在局部世界坐标系的姿态；为局部世界坐标系到虚拟平面的旋转量；e3为设定的矩阵，且；为imu在局部世界坐标系的位置；为odom到imu坐标系的平移量；为roll角度；为pitch角度；为的高斯噪声，为局部世界坐标到虚拟平面的高度值。
17.本发明提供的这种复杂环境下的高精度定位方法，通过创新的定位方法流程设计以及创新的多传感器融合计算过程，不仅实现了复杂环境下的高精度定位过程，而且本发明方法的可靠性高、精确性好且鲁棒性好。
附图说明
18.图1为本发明的方法流程示意图。
19.图2为本发明实施例中的现有高精度定位轨迹与真值轨迹的对比示意图。
20.图3为本发明实施例中的本发明定位轨迹与真值轨迹的对比示意图。
21.图4为本发明实施例中的现有高精度定位方法和本发明方法在x方向上的误差对比示意图。
22.图5为本发明实施例中的现有高精度定位方法和本发明方法在y方向上的误差对
比示意图。
23.图6为本发明实施例中的现有高精度定位方法和本发明方法在z方向上的误差对比示意图。
具体实施方式
24.如图1所示为本发明的方法流程示意图：本发明提供的这种复杂环境下的高精度定位方法，包括如下步骤：s1. 获取gnss传感器、vison视觉传感器、sins惯性传感器和wheel轮式里程计的原始观测数据，并对传感器数据进行预处理和同步；具体包括如下步骤：gnss传感器的原始观测数据包括卫星星历数据、载波相位原始观测数据、gnss基站差分数据、实时模糊度解算数据和多普勒原始观测数据；vison视觉传感器的原始观测数据为摄像头实时传输的灰度图像数据；sins惯性传感器的原始观测数据为imu输出数据；wheel轮式里程计的原始观测数据为车辆实时的左右轮转速数据；对gnss原始观测数据进行预处理，剔除异常数据；然后基于pps授时系统，对四种传感器数据进行时间同步；s2. 根据获取的传感器数据，进行vison/sins/wheel联合动态初始化；具体包括如下步骤：a. 在滑窗中，利用纯视觉sfm（structure from motion，运动恢复结构）方法，计算得到相对尺度的相机位姿和特征点坐标；b. 根据步骤a得到的计算结果，标定陀螺仪的零漂值；c. 根据视觉、imu和轮式里程计的初始化速度、重力加速度和尺度因子，构建初始化向量；具体包括如下步骤：根据视觉、imu和轮式里程计的初始化速度、重力加速度和尺度因子，构建待初始化量为，其中为第k帧的imu坐标系下的速度，为第c0帧的重力加速度，s为尺度因子；然后，在滑窗中连续两帧满足如下方程1：然后，在滑窗中连续两帧满足如下方程1：然后，在滑窗中连续两帧满足如下方程1：式中为imu的位置预积分量；为c0相对第k帧imu的旋转；为第k帧imu相对c0帧的位置；为两帧之间的时间间隔；为第k帧imu相对c0帧的旋转；为imu的速度预积分量；为轮式里程计的预积分量；为第k帧odom相对c0帧的旋转；
为第k帧odom相对c0帧的位置；为第k帧odom的速度；将方程1写成待优化量的线性表达式，然后转换成求解线性最小二乘，得到最优的初始化量，完成速度、重力加速度和尺度因子的初始化；d. 利用当地的实际重力加速度，对步骤c构建的初始化向量进调整，并恢复到真实尺度并对齐，最终得到精确的局部世界坐标系下的初始化结果；s3. 根据步骤s2的初始化结果，将视觉传感器数据、惯性传感器数据、轮式里程计数据与gnss传感器数据进行对齐；具体包括如下步骤：a. 基于载波相位双差模型，计算得到粗略的锚点位置；具体包括如下步骤：求解以下模型，计算得到全局ecef坐标系中的粗略锚点位置：式中为接收机与基站对第i颗卫星的载波相位观测之差与第j颗卫星的载波相位观测之差的差值；为载波波长；为基站对卫星i的观测矢量；b
ur
为接收机当前位置与基站位置的相对偏差，为待求解的量；为第i颗卫星与第j颗卫星观测的整周模糊度的差值；n为滑窗的大小；k为滑窗序号；m为第k个滑窗中载波相位原始观测量数目；为马氏范数；b. 根据vison/sins/wheel联合动态初始化的结果、多普勒观测值和卫星的速度，计算得到enu坐标系与局部世界坐标系之间的偏航角；具体包括如下步骤：求解以下模型，计算得到偏航角：求解以下模型，计算得到偏航角：式中为接收机频漂；为载波波长；fd为多普勒原始观测量；为接收机对卫星i的观测矢量；为第i颗卫星的速度；为局部世界坐标系到enu坐标系的转换关系；为局部世界坐标系到enu坐标系的转换关系；vw为局部世界坐标系下的载体速度；为第i颗卫星的频漂；n为滑窗的大小；k为滑窗序号；m为第k个滑窗中载波相位原始观测量数目；为马氏范数；yaw为偏航角；c. 根据vison/sins/wheel联合动态初始化的结果，对齐局部世界坐标系与全局
ecef坐标系，完成对锚点位置的精调；具体包括如下步骤：求解以下模型，计算得到精确的锚点位置，完成局部世界坐标系与全局ecef坐标系的对齐：式中为锚点的粗略坐标；为基站对卫星i的观测矢量；为载波波长；为局部世界坐标系到ecef坐标系的转换关系；pw为局部世界坐标系下的载体位置；为基站的坐标；为第i颗卫星与第j颗卫星观测的整周模糊度的差值；为接收机与基站对第i颗卫星的载波相位观测之差与第j颗卫星的载波相位观测之差的差值；n为滑窗的大小；k为滑窗序号；m为第k个滑窗中载波相位原始观测量数目；为马氏范数；s4. 构建惯性传感器和轮式里程计的预积分模型；具体包括如下步骤：根据imu预积分模型，计算得到两帧之间imu预积分量模型如下：根据imu预积分模型，计算得到两帧之间imu预积分量模型如下：根据imu预积分模型，计算得到两帧之间imu预积分量模型如下：根据imu预积分模型，计算得到两帧之间imu预积分量模型如下：式中为位置预积分量；bk为k时刻的imu；为t时刻相对于bk的旋转量；为加速度计的输出值；b
at
为加速度计的零漂；na为加速度计的高斯白噪声；为角速度计的输出值；为角速度计的零漂；为角速度计的高斯白噪声；为t时刻相对于bk的旋转量；tk为k时刻；为速度预积分量；为旋转预积分量；表示叉积；根据线速度和角速度建模，得到轮式里程计的预积分模型如下：
式中为位置预积分量；ok为k时刻的odom；为t时刻相对于ok的旋转量；为线速度；nv为线速度的高斯白噪声；为旋转预积分量；为偏航角速度；为偏航角速度的高斯白噪声；为t时刻相对于ok的旋转；s5. 基于构建的预积分模型，将gnss原始观测残差、视觉残差、惯性预积分模型、轮式里程计模型和平面约束进行聚合，构建非线性残差模型；具体包括如下步骤：采用如下算式作为非线性残差模型：式中x为待优化的估计量；为边缘化后的先验信息；r
p
为边缘化残差；h
p
为边缘化更新矩阵；为马氏距离；n为残差约束数量；为传感器测量值的残差函数；当传感器为sins惯性传感器时，取值为：式中为位置增量；为速度增量；为角度增量；为加速度计
bias增量；为角速度计bias增量；为局部世界坐标到第k帧imu的旋转；为第k帧imu在局部世界坐标下的位置；为第k帧imu在局部世界坐标系下的速度；为两帧之间的时间间隔；gw为重力加速度；为位置预积分量；为速度预积分量；为第k帧imu坐标到局部世界坐标的旋转量；为旋转预积分量；为四元素的虚部；为第k帧的加速度零漂；为第k帧陀螺仪的零漂；为四元素乘法；当传感器为wheel轮式里程计时，取值为：式中为位置增量；为角度增量；为局部世界坐标到第k帧odom的旋转；为第k帧轮速里程计在局部世界坐标下的位置；为第k帧轮速里程计在局部世界坐标系下的；为位置预积分量；为第k帧轮速里程计坐标到局部世界坐标的旋转量；为旋转预积分量；当传感器为vision视觉传感器时，取值为：式中为第j个相机坐标系路标点的归一化相机坐标；为第j个相机坐标系下路标点三维坐标；x
cj
、y
cj
和z
cj
的计算公式为，式中为相机与imu坐标
系的旋转参数，为局部世界坐标到第j帧imu的旋转，为第i帧 imu到局部世界坐标系的旋转量，为相机到imu的旋转，为逆深度值，为相机与imu的平移参数，为第i帧imu在局部世界坐标系的坐标，为局部世界坐标系与第j帧imu平移，为imu与相机坐标系的平移参数；当传感器为gnss传感器时，取值为：式中为载波波长；为第i颗卫星的观测矢量；为接收机的ecef坐标；为第i颗卫星与第j颗卫星观测的整周模糊度的差值；为接收机与基站对第i颗卫星的载波相位观测之差与第j颗卫星的载波相位观测之差的差值；为基准站的ecef坐标，且，为局部世界坐标系到enu坐标系的转换关系；为局部世界坐标系到enu坐标系的转换关系，为第k帧imu坐标在局部世界坐标系下的位置，为锚点坐标；需要随机平面约束因子时，取值为：式中为设定的矩阵，且；为odom到imu坐标系的旋转量；为imu在局部世界坐标系的姿态；为局部世界坐标系到虚拟平面的旋转量；e3为设定的矩阵，且；为imu在局部世界坐标系的位置；为odom到imu坐标系的平移量；为roll角度；为pitch角度；为的高斯噪声，为局部世界坐标到虚拟平面的高度值；
具体实施时，n取值是否就是5，n=1~5时，分别对应于imu残差因子、轮式里程计残差因子、视觉重投影残差因子、载波相位双差测量因子和随机平面约束因子；s6. 实时求解构建的非线性残差模型，完成复杂环境下的高精度定位。
25.以下结合一个实施例，对本发明的复杂环境高境地定位方法进行进一步说明：本实例基于小觅d1000-ir-120相机和自研卫星信号接收机采集图像、imu、gnss原始观测量，基于网络tcp方式接收gnss差分数据，基于odb方式接收车载轮速里程计数据，图像数据接收频率为30hz，只用左目数据，imu数据接收频率为200hz，gnss原始观测量和差分数据接收频率为1hz，轮速里程计数据接收频率为30hz，所有数据已经过相关工具同步处理并发布成ros格式。将硬件安装好标定好后，围绕楼栋密集区域的复杂环境和车库录制30分钟左右数据，并基于本发明方法与现有高精度定位方法进行对比。图2为现有高精度定位轨迹与真值轨迹对比情况，图中，1号轨迹为真值，2号轨迹为高精度定位结果；图3为本发明的定位轨迹与真值轨迹对比情况，图中，1号轨迹为真值，3号轨迹为本发明的定位结果。从图2中可以明显看到，采用现有的高精度定位方法定位时，轨迹1和轨迹2依旧存在部分区段有明显的分离痕迹，表明定位结果并不令人满意；而在图3中，采用本发明方法进行定位时，轨迹1和轨迹3几乎一直处于重叠状态，因此表明本发明方法的精度确实较高。
26.分别将现有高精度定位方法和本发明方法的坐标值，与真值相减得到x/y/z方向上的误差值，图4为高精度定位方法和本发明方法在x方向上的误差对比情况，图5为高精度定位方法和本发明方法在y方向上的误差对比情况，图6为高精度定位方法和本发明方法在z方向上的误差对比情况，从图可以看出，基于本发明的定位方法的误差值在x、y、z方向上都远远小于现有高精度定位方法，说明基于本发明的定位方法能大幅提升在复杂环境下的定位精度。