一种面向复杂环境的航行器组合定位方法

技术领域

本发明涉及复杂环境组合导航技术领域，具体为一种面向复杂环境的航行器组合定位方法。

背景技术

在城市峡谷等复杂环境中，机器人的定位效果是其完成任务的重要因素。传统的室外导航通常采用惯性测量单元(Inertial Measurement Unit，IMU)/北斗的紧耦合的卡尔曼滤波，但在恶劣环境中，由于北斗定位信号缺失，使得传统的定位方法失效。同时，由于峡谷等场景光环境恶劣，使得视觉信息失去了准确度；在一些开阔的场景如宽阔的城市街道中，纹理、结构化信息缺失也会使激光雷达信息产生较大的偏差；而IMU定位的漂移在实际定位导航中不可忽视。因此，为了提高在城市峡谷等复杂环境导航的鲁棒性和准确性，基于激光雷达的方法和基于视觉的方法通常都与IMU相结合。在激光雷达与惯性的组合导航系统(Lidar-Inertial-System，LIS)中，IMU可以帮助矫正雷达点云畸变，弥补短时间内特征的缺乏。视觉与惯性的组合导航系统(Visio-Inertial-System，VIS)中，IMU可以帮助视觉恢复实际尺度，并在视觉信息丢失时进行短暂定位。

组合导航算法在处理传感器退化任务中具有较好的鲁棒性而被广泛应用于各大复杂环境的导航任务中。针对现有的北斗-雷达-视觉-惯性组合导航算法，可以大致分为两大类：一种是松耦合方法，由于其在优化过程中没有使用IMU测量，其定位精度有着较大的局限性；另一种是紧耦合方法，但该算法一方面在多传感器信息融合的过程中由于数据传输延时以及传感器安装误差的存在，容易出现时空不同步问题，另一方面北斗-雷达-视觉-惯性组合导航算法国内尚无能有效解决此问题的方法。

发明内容

针对现有的城市峡谷等复杂环境中存在着光环境恶劣、扬尘干扰、结构化信息缺失等问题，本发明主要是利用紧耦合北斗-雷达-视觉-惯性组合导航方法对无人车/无人机进行实时的位置姿态估计，并进行建图。本发明首先是选取IMU时刻为配准参考，对北斗定位信号的置信度进行检验，若置信度较高则通过卡尔曼滤波构建北斗/惯性子系统，若置信度不高则进行雷达-视觉-惯性(Lidar-Visual-Inertail，LVI)组合导航，先对LI进行插值配准，然后对VI进行最小化联合残差运动状态估计，最后采用因子图综合构建LVI量测方程解算位姿。

本发明提供的一种面向复杂环境的航行器组合定位方法，包括以下步骤：

对航行器进行IMU姿态解算，获得惯性解算姿态数据；

获取航行器的北斗定位信号，并对定位信号进行置信度检验；

当定位信号置信度低时，获取航行器的去偏移激光雷达点云特征数据；

构建LIS子系统，对惯性解算姿态数据和去偏移激光雷达点云特征数据进行插值配准，完成姿态初始量测；

构建VIS子系统，将LIS子系统姿态初始量测得到的系统状态和偏置作为初始值，通过插值将LIS子系统的时间戳和VIS子系统的时间戳对齐，完成视觉信息的初始化，并以此为基准构建图像序列，根据图像序列，提取图像关键信息；

对图像关键信息和惯性解算姿态数据进行最小化联合残差运动状态估计，完成姿态信息的完善，并对图像关键信息和惯性解算姿态数据进行本地优化；

基于插值配准后的惯性解算姿态数据和去偏移激光雷达点云特征数据与本地优化后的图像关键信息和惯性解算姿态数据，利用因子图联合优化IMU预积分约束、视觉里程计约束、激光雷达里程计约束，综合优化LVI位姿解算结果。

进一步的，本发明提供的一种面向复杂环境的航行器组合定位方法，还包括：

当定位信号置信度较高时，对惯性解算姿态数据和去偏移激光雷达点云特征数据进行紧耦合后，利用卡尔曼滤波进行定位，输出位姿解算结果。

进一步的，本发明提供的一种面向复杂环境的航行器组合定位方法，还包括：

选取IMU时间戳作为后续时刻对准的参考时刻。

进一步的，所述对航行器进行IMU姿态解算时，IMU测量表示方程为：

其中，

进一步的，所述对定位信号进行置信度检验，置信度检验公式为：

式中，T为待优化的北斗/惯性系统输出姿态，A，b分别北斗/惯性系统位姿解算的状态更新矩阵和偏置，当偏置b大于阈值时判定北斗/惯性系统定位失败。

进一步的，所述对惯性解算姿态数据和去偏移激光雷达点云特征数据进行插值配准时，所利用的雷达-惯性系统线性插值模型为：

q

式中，q

进一步的，所述通过插值将LIS子系统的时间戳和VIS子系统的时间戳对齐，包括以下步骤：

获取原始图像，包含双目图像和深度图像，按照时间戳进行标记排序；

对图像其进行去畸变操作，将去畸变后的图像存入图像序列中；

对于相邻两帧图像I

其中，第k帧系统全状态χ

χ

x

式中，x

进一步的，所述对图像关键信息和惯性解算姿态数据进行最小化联合残差运动状态估计，包括以下步骤：

采用迭代最近点法LCP对图像关键信息和惯性解算姿态数据做3D-3D运动估计，借助IMU姿态解算信息进行校正，采用最小化联合残差运动状态估计，其中残差表示为：

式中，ρ为抑制异常值的鲁棒核函数，r

最小化残差代价函数，得到最小化联合残差运动状态估计后的结果，即VIS子系统定位结果。

进一步的，所述利用因子图联合优化IMU预积分约束、视觉里程计约束、激光雷达里程计约束，综合构建LVI量测方程优化位姿信息，其中LVI量测方程为：

其中，x

与现有技术相比，本发明提供的一种面向复杂环境的航行器组合定位方法，其有益效果是：

本发明通过构建紧耦合的北斗/惯性子系统，VIS子系统和LIS子系统，完成闭环导航系统的初始量测及完善，再通过因子图综合构建量测方程解算位姿，可以达到较高的导航精度；

本发明具有较高的导航精度和较好的鲁棒性，当三个子系统中有一个或者两个系统离线时仍然可以较好地工作，以应对复杂环境中纹理信息缺失和结构化信息缺失的复杂环境；本发明可以广泛部署于城市峡谷等复杂环境，也可以用于部分室内定位场景，解决复杂环境定位难、精度低的问题，对于满足与生命安全相关的导航定位服务可靠性意义重大。

附图说明

图1为本发明提供的一种面向复杂环境的航行器组合定位方法的流程图。

具体实施方式

下面结合附图1，对本发明的具体实施方式作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

实施例1：结合图1，本发明提出了一种面向复杂环境的航行器组合定位方法，首先通过构建紧耦合的VIS子系统和LIS子系统，完成闭环导航系统的初始量测及完善，然后通过因子图综合构建量测方程解算位姿，具体包括以下步骤：

步骤1：基于IMU姿态解算模型，进行IMU姿态解算，并选取IMU时间戳作为后续时刻对准的参考时刻；

步骤2：北斗定位信号获取，并进行定位信号置信度检验，若置信度较高则直接执行步骤11，否则继续执行步骤3；

步骤3：获取激光雷达点云特征，获得去偏移的点云特征；

步骤4：构建LIS子系统，对雷达-惯性数据进行插值配准，完成姿态初始量测；

步骤5：将LIS子系统中的系统状态和IMU零偏作为视觉-惯性系统的初始猜测，完成视觉信息的初始化，并构建图像序列；

步骤6：获取视觉特征，提取图形关键帧；

步骤7：使用估计的视觉里程计将激光雷达帧配准到相机帧，并将特征与深度值关联；

步骤8：构建VIS子系统，对视觉-惯性数据进行最小化联合残差运动状态估计，完成姿态信息的完善；

步骤9：对惯性-视觉检测结果在滑动窗口中执行本地优化；

步骤10：利用因子图联合优化IMU预积分约束、视觉里程计约束、激光雷达里程计约束，综合构建LVI量测方程解算位姿，随后执行步骤12；

步骤11：采用卡尔曼滤波算法构建北斗/惯性组合导航系统，获得组合导航定位信息；

步骤12：输出位姿信息。

以上为一种面向复杂环境的航行器组合定位方法的具体实施流程。

由于IMU姿态解算频率明显高于其他两种传感器，因此选取IMU时刻为参考时刻，IMU测量表示方程为：

其中，

对于LIS子系统，由于激光雷达以自身载体坐标系为参考，运动过程中检测物体和载体间有相对运动，每一列点云数据参考坐标系不一致。因此，需要对雷达数据进行插值配准。雷达-惯性系统线性插值模型为：

q

式中，q

由此得到LiDAR-IMU插值配准后的结果，即LI子系统定位结果。

对于VIS子系统，本方法采用基于特征点法的视觉里程计，采用特征提取特征匹配的方式，估计相机的运动轨迹，从而对车载平台的位置姿态进行规划。首先获取原始图像，包含双目图像和深度图像，按照时间戳进行标记排序，由于图像存在畸变，对其进行去畸变操作，将去畸变后的图像存入图像序列中。对于相邻两帧图像I

视觉-惯性系统的第k帧系统全状态χ

χ

x

其中，x

由于VIO位姿估计的结果受到时移和视觉信息缺失的影响，需要借助IMU姿态解算信息对其进行校正，采用最小化联合残差运动状态估计，视觉-惯性系统中残差表示为：

其中，ρ为抑制异常值的鲁棒核函数，r

最小化残差代价函数，可以得到最小化联合残差运动状态估计后的结果，即VIS子系统定位结果。

最后利用因子图联合优化IMU预积分约束、视觉里程计约束、激光雷达里程计约束，综合构建LVI量测方程解算位姿。

综合来看，与现有技术相比，本发明提供的一种面向复杂环境的航行器组合定位方法，具有以下的有益效果：

本发明通过构建紧耦合的北斗/惯性子系统，VIS子系统和LIS子系统，完成闭环导航系统的初始量测及完善，再通过因子图综合构建量测方程解算位姿，可以达到较高的导航精度；

本发明具有较高的导航精度和较好的鲁棒性，当三个子系统中有一个或者两个系统离线时仍然可以较好地工作，以应对复杂环境中纹理信息缺失和结构化信息缺失的复杂环境；本发明可以广泛部署于城市峡谷等复杂环境，也可以用于部分室内定位场景，解决复杂环境定位难、精度低的问题，对于满足与生命安全相关的导航定位服务可靠性意义重大。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。