[发明专利]一种基于毫米波雷达的无人机定位方法

权利要求书

1.一种基于毫米波雷达的无人机定位方法，其特征在于，所述无人机上搭载有毫米波雷达和IMU，包括标定阶段和定位阶段，标定阶段包括：

S1、获取地图，获取毫米波雷达测量的雷达点云数据，获取无人机的地面坐标；

S2、对雷达点云数据进行预处理；提取雷达点云数据中的关键点，基于关键点得到特征线段，特征线段上的关键点记为特征点；

S3、根据无人机的地面坐标，将特征点投影在地图上，得到各个特征点的地面坐标；

定位阶段包括：

S4、获取毫米波雷达测量的当前帧雷达点云数据，对雷达点云数据进行预处理；

S5、获取IMU测量的无人机运动数据，将雷达点云数据和无人机运动数据进行融合，得到修正后的雷达点云数据；

S6、提取雷达点云数据中的关键点，基于关键点得到特征线段，特征线段上的关键点记为特征点；

S7、将当前帧雷达点云数据的特征线段与上一帧雷达点云数据的特征线段进行配准，找到当前帧与上一帧相匹配的特征点，记为匹配特征点，找到当前帧相对于上一帧增加的特征点，记为新增特征点；

S8、基于匹配特征点在地图上的地面坐标，得到无人机的地面坐标、新增特征点的地面坐标；

S9、重复步骤S4。

2.根据权利要求1所述的一种基于毫米波雷达的无人机定位方法，其特征在于，标定阶段还包括毫米波雷达和IMU的坐标标定，具体为：根据毫米波雷达和IMU的安装位置，建立雷达点云数据与IMU测量的无人机运动数据之间的坐标变换矩阵，完成坐标标定。

3.根据权利要求1所述的一种基于毫米波雷达的无人机定位方法，其特征在于，对雷达点云数据进行预处理包括获取无偏转点云数据，获取无偏转点云数据具体为：将三维的雷达点云数据映射到二维平面上。

4.根据权利要求3所述的一种基于毫米波雷达的无人机定位方法，其特征在于，对雷达点云数据进行预处理包括噪声过滤，噪声过滤具体为：通过直接剔除法剔除雷达点云数据中的离散点，使用体素栅格滤波器对雷达点云数据降采样。

5.根据权利要求4所述的一种基于毫米波雷达的无人机定位方法，其特征在于，对雷达点云数据进行预处理包括点云配准，点云配准具体为：通过迭代的算法就近搜索点匹配，将同一帧雷达点云数据中帧头和帧尾的雷达点云数据的坐标转换至同一坐标系。

6.根据权利要求1所述的一种基于毫米波雷达的无人机定位方法，其特征在于，步骤S5具体为：

获取IMU测量的无人机运动数据，包括姿态角、角速度和线速度，基于姿态角、角速度和线速度计算无人机的飞行距离，基于飞行距离对雷达点云数据进行修正，得到修正后的雷达点云数据。

7.根据权利要求1所述的一种基于毫米波雷达的无人机定位方法，其特征在于，提取雷达点云数据中的关键点包括以下步骤：

确定雷达点云数据的第一个点为关键点；遍历剩余的雷达点云数据，如果一个点满足条件1或条件2，则该点为关键点，条件1为：关键点与上一个关键点之间的欧式距离大于预设置的点点距离阈值；条件2为：关键点不落在上两个关键点所连成的直线l上，并且关键点到直线l的距离超过预设置的点线距离阈值。

8.根据权利要求1所述的一种基于毫米波雷达的无人机定位方法，其特征在于，基于关键点得到特征线段具体为使用直线拟合方法得到特征线段，一个特征线段至少由4个关键点拟合得到。

9.根据权利要求1所述的一种基于毫米波雷达的无人机定位方法，其特征在于，步骤S7具体为：

计算当前帧的特征线段和上一帧的特征线段之间的角度差和距离差，如果当前帧的一条特征线段与上一帧的一条特征线段之间的角度差和距离差均小于预设置的配准阈值，则这两条特征线段相互匹配；

对于当前帧的所有特征线段，如果一条特征线段找到相互匹配的特征线段，则该特征线段上的特征点为匹配特征点，如果一条特征线段没有找到相互匹配的特征线段，则该特征线段上的特征点为新增特征点。

10.根据权利要求1所述的一种基于毫米波雷达的无人机定位方法，其特征在于，步骤S8具体为：

根据匹配特征点在上一帧和当前帧的坐标变化，更新更新无人机的地面坐标；根据新增特征点与匹配特征点在当前帧的坐标关系，计算得到新增特征点在地图上的地面坐标。