基于激光惯性里程计定位及导航无人机的方法及其系统与流程

本发明涉及无人机，具体地，涉及一种基于激光惯性里程计定位导航无人机的方法及其系统。

背景技术：

1、由于激光雷达具有精度高、测距范围广等优点，因此目前现有无人机一部分采用激光雷达技术进行导航，但是激光雷达只能在几何纹理特征好的地方，才能够提取雷达特征进行位姿信息和构建地图，如果在空旷的平面场景中，单靠雷达观测量无法约束无人机状态的六个自由度，不能有效的对无人机进行定位、导航、建图。

2、因此，如何在空旷的平面场景中，仍然能够准确建图以及对无人机作出运动决策是亟需解决的技术问题。

技术实现思路

1、本申请提供了一种基于激光惯性里程计定位及导航无人机的方法及其系统，以解决当前无人机定位效果差的技术问题。

2、为了解决上述技术问题，一方面，本申请实施例提供了一种基于激光惯性里程计定位及导航无人机的方法，包括以下步骤：

3、(1)获取imu原始位姿运动信息，包括imu原始加速度、原始角速度信息；

4、(2)状态预测：根据imu当前加速度、角速度信息及前一帧系统速度、姿态和位置状态信息进行处理，获得无人机的状态预测信息，包括速度信息和位置信息，在本申请实施例中还包括姿态信息；

5、(3)点云畸变校正：获取激光雷达探测器原始点云数据，根据imu的原始位姿运动信息及状态预测信息，对原始点云数据进行点云畸变校正，获得同一时刻点云观测数据；

6、(4)点云特征提取：对同一时刻点云观测数据提取特征值；

7、(5)更新特征地图：对特征值与特征地图的数据进行比对匹配，并更新特征地图；

8、(6)构建全局三维地图：对提取特征值数据进行坐标转换，生成里程计世界系地图信息，并融合获得更新的全局三维地图；

9、(7)获得rtk位置观测数据，在本申请实施例中还获得rtk姿态观测数据；

10、(8)状态修正：根据状态预测信息、点云特征匹配后的点云特征值、rtk位置和姿态观测数据进行状态修正，获得修正后的无人机位姿信息、imu的加速度计和陀螺仪零偏及里程计世界系和rtk世界系间的外参。

11、在本申请实施例中，所述特征值为几何特征。

12、优选的，所述步骤(7)包括：

13、(7-1)rtk位姿信息坐标转换：获取rtk装置原始位姿运动信息，包括三维位置与三轴姿态信息并对该原始位姿信息进行坐标系转换，获得rtk世界坐标系的rtk位置运动信息；

14、(7-2)rtk位姿信息坐标对齐：获取rtk世界坐标系与里程世界系的外参，对rtk世界坐标系的rtk位置和姿态运动信息进行坐标对齐，获得rtk位置和姿态观测数据。

15、优选的，在本发明实施例中，还包括根据状态修正结果对imu原始位姿数据进行修正，包括减小imu的加速度零偏和角速度零偏，对imu原始系统速度、姿态和位置状态信息进行修正。

16、在本申请实施例中，还包括根据状态修正结果对坐标对齐使用的里程计世界系和rtk世界系间的外参进行修正。

17、优选的，所述步骤(5)更新特征地图中对特征值与特征地图的数据进行比对匹配，包括保留匹配的特征值数据，去除不匹配的特征值数据和添加新的几何特征到特征地图。

18、优选的，还包括对状态修正后的位姿信息及里程计世界系地图信息进行回环检测及位姿优化，获得优化后的位姿信息及三维地图。

19、另一方面，本申请实施例提供了一种基于激光惯性里程计定位及导航无人机的系统，包括imu传感器：获取无人机原始位姿运动信息，包括原始加速度、原始角速度信息；

20、状态预测单元：对imu传感器当前的加速度、角速度信息及前一帧系统速度、姿态和位置状态信息进行处理，获得无人机的状态预测信息，包括速度信息和位置信息，在本实施例中还包括姿态信息；

21、激光雷达探测器：获取无人机前方外部环境信息，即原始点云数据；

22、点云畸变校正单元：获取激光雷达探测器原始点云数据，根据imu的原始位姿运动信息及状态预测信息，对原始点云数据进行点云畸变校正，获得同一时刻点云观测数据；

23、点云特征提取单元：对同一时刻点云观测数据提取特征值；

24、更新特征地图单元：对特征值与特征地图的数据进行比对匹配，并更新特征地图；

25、构建全局三维地图单元：对提取特征值数据进行坐标转换，生成里程计世界系地图信息，并融合获得更新的全局三维地图；

26、rtk装置：获取无人机的位置运动信息，在本发明实施例中，使用了双天线rtk，可以提供航向角信息，所以rtk装置还可以获取无人机的姿态运动信息；

27、rtk位置观测数据获取单元：根据rtk装置获取的无人机位姿运动信息进行处理，获得rtk位置和姿态观测数据；

28、状态修正单元：根据状态预测信息、点云特征匹配后的点云特征值、rtk位置和姿态观测数据进行状态修正，获得修正后的无人机位姿信息、imu的加速度计和陀螺仪零偏及里程计世界系和rtk世界系间的外参。

29、优选的，所述rtk位置观测数据获取单元还包括：

30、rtk位姿信息坐标转换单元：获取rtk装置原始位姿运动信息，并对该原始位姿信息进行坐标系转换，获得rtk世界坐标系的rtk位置和姿态运动信息；

31、rtk位姿信息坐标对齐单元：根据rtk世界坐标系与里程世界系的外参，对rtk世界坐标系的rtk位置运动信息进行坐标对齐，获得rtk位置和姿态观测数据。

32、优选的，在本申请实施例中，还包括状态修正单元根据修正结果对imu原始位姿数据进行修正，包括减小imu的加速度零偏和角速度零偏，对imu原始系统速度、姿态和位置状态信息进行修正。

33、在本申请实施例中，还包括状态修正单元根据状态修正结果对坐标对齐使用的里程计世界系和rtk世界系间的外参进行修正。

34、优选的，所述更新特征地图单元对特征值与特征地图的数据进行比对匹配，包括保留匹配的特征值数据，去除不匹配的特征值数据和添加新的几何特征到特征地图。

35、优选的，在本申请实施例中，还包括回环检测及位姿优化单元，其对状态修正后的位姿信息及里程计世界系地图信息进行回环检测及位姿优化，获得优化的位姿信息及三维地图。

36、本发明提供一种无人机，通过融合rtk信息，对激光惯性里程计的位姿信息进行修正，使得基于激光惯性里程计定位导航的无人机也可以在空旷的平面场景中进行有效地定位和导航，从而有效的控制无人机的飞行。

技术特征：

1.一种基于激光惯性里程计定位及导航无人机的方法，包括以下步骤：

2.如权利要求1所述的基于激光惯性里程计定位及导航无人机的方法，其特征在于，所述步骤(7)包括：

3.如权利要求2所述的基于激光惯性里程计定位及导航无人机的方法，其特征在于，还包括，根据状态修正结果对imu原始位姿数据进行修正。

4.如权利要求1所述的基于激光惯性里程计定位及导航无人机的方法，其特征在于，所述步骤(5)更新特征地图中对特征值与特征地图的数据进行比对匹配，包括保留匹配的特征值数据，添加新的特征值到特征地图。

5.如权利要求1所述的基于激光惯性里程计定位及导航无人机的方法，其特征在于，还包括对状态修正后的位姿信息及里程计世界系地图信息进行回环检测及位姿优化，获得优化后的位姿信息及三维地图。

6.一种基于激光惯性里程计定位及导航无人机的系统，包括：

7.如权利要求6所述的基于激光惯性里程计定位及导航无人机的系统，其特征在于，所述rtk位置观测数据获取单元还包括：

8.如权利要求7所述的基于激光惯性里程计定位及导航无人机的系统，其特征在于，状态修正单元根据修正结果对imu原始位姿数据进行修正。

9.如权利要求6所述的基于激光惯性里程计定位及导航无人机的系统，其特征在于，所述更新特征地图单元对特征值与特征地图的数据进行比对匹配，包括保留匹配的特征值数据及去除不匹配的特征值数据。

10.如权利要求6所述的基于激光惯性里程计定位及导航无人机的系统，其特征在于，还包括回环检测及位姿优化单元，其对状态修正后的位姿信息及里程计世界系地图信息进行回环检测及位姿优化，获得优化的位姿信息及三维地图。

技术总结
本发明公开了一种基于激光惯性里程计定位及导航无人机的方法及其系统，包括IMU传感器获取IMU原始位姿运动信息，状态预测单元状态预测获得无人机的状态预测信息，点云畸变校正单元对原始点云数据进行点云畸变校正，获得同一时刻点云观测数据，点云特征提取单元对同一时刻点云观测数据提取特征值，更新特征地图，构建全局三维地图，获得RTK位置观测数据，状态修正获得修正后的无人机位姿信息、IMU的加速度计和陀螺仪零偏及里程计世界系和RTK世界系间的外参。本发明使得基于激光惯性里程计定位导航的无人机也可以在空旷的平面场景中进行有效地定位和导航，从而有效的控制无人机的飞行。

技术研发人员：请求不公布姓名,请求不公布姓名,请求不公布姓名,请求不公布姓名
受保护的技术使用者：北京氢源智能科技有限公司
技术研发日：
技术公布日：2024/2/19