[发明专利]复杂环境下的高精度定位方法

权利要求书

1.一种复杂环境下的高精度定位方法，其特征在于包括如下步骤：

S1. 获取GNSS传感器、VISON视觉传感器、SINS惯性传感器和WHEEL轮式里程计的原始观测数据，并对传感器数据进行预处理和同步；

S2. 根据获取的传感器数据，进行VISON/SINS/WHEEL联合动态初始化；

S3. 根据步骤S2的初始化结果，将视觉传感器数据、惯性传感器数据、轮式里程计数据与GNSS传感器数据进行对齐；

S4. 构建惯性传感器和轮式里程计的预积分模型；

S5. 基于构建的预积分模型，将GNSS原始观测残差、视觉残差、惯性预积分模型、轮式里程计模型和平面约束进行聚合，构建非线性残差模型；具体包括如下步骤：

采用如下算式作为非线性残差模型：

式中x为待优化的估计量；为边缘化后的先验信息；r_p为边缘化残差；H_p为边缘化更新矩阵；为马氏距离；n为残差约束数量；为传感器测量值的残差函数；

当传感器为SINS惯性传感器时，取值为：

式中为位置增量；为速度增量；为角度增量；为加速度计bias增量；为角速度计bias增量；为局部世界坐标到第k帧IMU的旋转；为第k帧IMU在局部世界坐标下的位置；为第k帧IMU在局部世界坐标系下的速度；为两帧之间的时间间隔；g^w为重力加速度；为位置预积分量；为速度预积分量；为第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坐标系到ECEF坐标系的转换关系；为局部世界坐标系到enu坐标系的转换关系，为第k帧IMU坐标在局部世界坐标系下的位置，为锚点坐标；

需要随机平面约束因子时，取值为：

式中为设定的矩阵，且；为odom到IMU坐标系的旋转量；为IMU在局部世界坐标系的姿态；为局部世界坐标系到虚拟平面的旋转量；e₃为设定的矩阵，且；为IMU在局部世界坐标系的位置；为odom到IMU坐标系的平移量；为roll角度；为pitch角度；为的高斯噪声，为局部世界坐标到虚拟平面的高度值；

S6. 实时求解构建的非线性残差模型，完成复杂环境下的高精度定位。