[发明专利]一种基于激光里程计的无人机动态障碍物检测方法

权利要求书

1.一种基于激光里程计的无人机动态障碍物检测方法，其特征在于，包括：

步骤1，通过激光点云数据的几何特性，提取符合预设曲率特征的点云作为特征点，建立同一特征点在两帧相邻时刻激光点云数据中的匹配关系，并构建重投影代价函数，使用非线性优化技术使位姿在三维解空间中进行迭代，直至位姿收敛，获得无人机位姿信息；

步骤2，其具体包括：

步骤21，根据步骤1输出的无人机的位姿信息，使用区域生长法将整体点云按照障碍物聚类为多个点云簇，以区分为各自独立的障碍物目标；

步骤22，根据激光里程计计算得到的两帧相对运动以及多帧激光点云数据中障碍物的质心和法线差异程度，计算差异度函数，并建立关联矩阵，将多帧激光点云数据中的障碍物关联起来；步骤22中的“障碍物关联”具体包括：

步骤221，根据激光里程计计算得到的两帧相对运动为以及多帧激光点云数据中障碍物的质心和法线差异程度，采用式(7)计算差异度函数

其中，为对应的旋转矩阵SO(3)，为第k-1帧第v个障碍物的质心在三维空间中位置的齐次坐标表现形式，α、β分别为质心距离差异权重、法线角度差异权重；

步骤222，P_k-1中共检测到的障碍物的总数量为m个，记为其中u＝1，2，…，m；P_k中共检测到的障碍物的总数量为n个，记为其中，v＝1，2，…，n，将关联矩阵表示为下式(8):

式中，表示由式(7)计算的差异度；

步骤223，根据步骤222中建立的关联矩阵(8)，关联当前帧点云数据P_k和上一帧点云数据P_k-1中的动态障碍物；重复上述步骤，直至当前帧点云数据P_k的关联矩阵的每一行对应的障碍物关联完毕；若最小差异度所对应的P_k-1中元素的下标u＞v，则表明发生障碍物丢失；若最小差异度所对应的P_k-1中元素的下标u＜v，则表明有新动态障碍物出现；

步骤23，根据动态障碍物的关联情况，结合激光里程计计算得到的帧间相对运动，判断障碍物运动状态，从而得到动态障碍物的运动方向及速度。

2.如权利要求1所述的基于激光里程计的无人机动态障碍物检测方法，其特征在于，步骤1中，所述提取符合预设曲率特征的点云作为特征点的方法具体包括：

步骤11，对点云数据进行体素滤波；

步骤12，利用下式(1)计算点云数据中各点云的曲率值r，并按照曲率值r的大小排序，曲率值最大的点云定义为特征角点，曲率值最小的点云定义为特征平面点：

式中，X_(k，i)表示第k帧点云数据P_k中的第i个点云在雷达坐标系中的坐标，X_(k，j)表示P_k中的第j个点云在雷达坐标系中的坐标，S表示X_(k，i)的邻域。