一种基于激光里程计的无人机动态障碍物检测方法

1.本发明涉及一种面向无人机领域的动态障碍物检测方法，特别是关于一种基于激光里程计的无人机动态障碍物检测方法。


背景技术：

2.在无人机领域中，由于飞行安全的需要，无人机的自主避障技术一直以来都是研究热点之一。无人机执行任务时，复杂的飞行环境中除地形和静态障碍物以外，还存在动态障碍物。若将动态障碍物当作静态障碍物处理，则会得出不正确的碰撞倒计时，这对于自主避障功能来说是不可接受的。自主避障功能往往希望能够通过预测动态障碍物的运动状态，来达到灵活修改飞行路径的目的，故动态障碍物检测是自主避障技术中不可或缺的一环。
3.现如今常用的动态障碍物检测方式是通过障碍物状态估计实现的，即通过观测多帧数据的障碍物并使其关联后，通过判断其运动状态，区分动、静态障碍物。在不依靠其它传感器的前提下，由于无法获取当前无人机的运动状态，仅依靠多帧障碍物状态，估计判断出的障碍物运动状态是相对于当前无人机运动状态，例如：当多个无人机分布式匀速飞行时，无人机间的相对速度为零，会被判定为静态障碍物。若依靠额外传感器，如惯性导航单元等，会增加额外的标定工作和硬件拓扑复杂度。通常在无人机的实际应用中，希望尽可能降低硬件拓扑复杂度，使系统更加简单、可靠。目前基于单一传感器的无人机动态障碍物检测方法仍不成熟，相关理论有待进一步完善。


技术实现要素：

4.本发明的目的在于提供一种基于激光里程计的无人机动态障碍物检测方法来克服或至少减轻现有技术的上述缺陷中的至少一个。
5.为实现上述目的，本发明提供一种基于激光里程计的无人机动态障碍物检测方法，该方法包括：
6.步骤1，通过激光点云数据的几何特性，提取符合预设曲率特征的点云作为特征点，建立同一特征点在两帧相邻时刻激光点云数据中的匹配关系，并构建重投影代价函数，使用非线性优化技术使其在三维解空间中进行迭代，直至位姿收敛，获得无人机位姿信息；
7.步骤2，其具体包括：
8.步骤21，根据步骤1输出的无人机的位姿信息，使用区域生长法将整体点云按照障碍物聚类为多个点云簇，以区分为各自独立的障碍物目标；
9.步骤22，根据激光里程计计算得到的两帧相对运动以及多帧激光点云数据中障碍物的质心和法线差异程度，计算差异度函数，并建立关联矩阵，将多帧激光点云数据中的障碍物关联起来；
10.步骤23，根据动态障碍物的关联情况，结合激光里程计计算得到的帧间相对运动，判断障碍物运动状态，从而得到动态障碍物的运动方向及速度。
11.进一步地，步骤1中，所述提取符合预设曲率特征的点云作为特征点的方法具体包括：
12.步骤11，对点云数据进行体素滤波；
13.步骤12，利用下式(1)计算点云数据中各点云的曲率值r，并按照曲率值r的大小排序，曲率值最大的点云定义为特征角点，曲率值最小的点云定义为特征平面点：
[0014][0015]
式中，x
(k，i)
表示第k帧点云数据p
k
中的第i个点云在雷达坐标系中的坐标，x
(k，j)
表示p
k
中的第j个点云在雷达坐标系中的坐标，s表示x
(k，i)
的邻域。
[0016]
进一步地，所述特征点为特征角点的情形下，步骤1中，所述同一特征点在两帧相邻时刻点云数据中的匹配关系的方法包括特征角点和特征线之间的点云线距离和/或特征平面点和特征面之间的点云面距离
[0017]
所述特征点为特征角点的情形下，在第k
‑
1帧点云数据p
k
‑1中寻找p
k
中的第a个特征角点x
(k，a)
对应的特征线段，该线段上具有点云b和点云c，其中，点云b为p
k
‑1中与特征角点x
(k，a)
距离最近的点云，点云c为p
k
‑1中在特征角点x
(k，a)
相邻扫描线上与特征角点x
(k，a)
距离最近的点云，表示为下式(2)：
[0018][0019]
式中，x
(k
‑
1，b)
表示p
k
‑1中的第b个特征角点在雷达坐标系中的坐标，x
(k
‑
1，c)
表示p
k
‑1中的第c个特征角点在雷达坐标系中的坐标；
[0020]
所述特征点为特征平面点的情形下，在p
k
‑1中寻找第d个特征平面点x
(k，d)
所对应的特征面，该特征面具有点云e、点云f和点云g，点云e为p
k
‑1中与特征平面点x
(k，d)
距离最近的点云，点云f为p
k
‑1中在点云e同一相邻扫描线上与点云d距离最近的点云，点云g为在相邻扫描线上与特征平面点x
(k，d)
距离最近的点云，点云面距离表示为下式(3)：
[0021][0022]
式中，x
(k
‑
1，e）
表示p
k
‑1中的第e个特征平面点在雷达坐标系中的坐标，x
(k
‑
1，f)
表示p
k
‑1中的第f个特征平面点在雷达坐标系中的坐标，x
(k
‑
1，g)
表示p
k
‑1中的第g个特征平面点在雷达坐标系中的坐标。
[0023]
进一步地，步骤1中的“构建重投影代价函数，使用非线性优化技术使其在三维解空间中进行迭代，直至位姿收敛，获得无人机位姿信息”的方法具体方法包括：
[0024]
根据和建立重投影代价函数，将位姿解算问题转化成式(4)表示的非线性最小二乘求解问题，求解最优变换矩阵估计值
[0025]
[0026]
其中，表示p
k
中第i个特征点在雷达坐标系中的坐标，n表示特征点的数里；为在p
k
‑1中的齐次坐标，即中的齐次坐标，即表示在p
k
中的齐次坐标。
[0027]
重复步骤1中的“建立同一特征点在两帧相邻时刻激光点云数据中的匹配关系”以及“根据重投影代价函数求解直至位姿收敛。
[0028]
进一步地，步骤21具体包括：
[0029]
步骤211，将步骤12获得的曲率值最小的点云作为起始种子点p0；
[0030]
步骤212，对每个种子点p
i
，搜索该点的所有近邻点，并且，按照如下步骤执行：
[0031]
步骤2121，计算每个近邻点与当前种子点p
i
法线角度的差值，若差值小于设定角度阈值，则进入步骤2122；
[0032]
步骤2122，计算该近邻点曲率，若曲率值小于设定曲率阈值，则将该点添加进当前种子点集；
[0033]
返回步骤211，直到当前种子点集所有近邻点都不满足生长准则，随后从原始采集的点云集中删除所述种子点，并将该种子点集作为一个障碍物点云簇保存下来。
[0034]
进一步地，步骤22中的“数据关联”具体包括：
[0035]
步骤221，根据激光里程计计算得到的两帧相对运动为以及多帧激光点云数据中障碍物的质心和法线差异程度，采用式(7)计算差异度函数
[0036][0037]
其中，为对应的旋转矩阵so(3)，第k
‑
1帧第v个障碍物的质心为在三维空间中位置的齐次坐标表现形式，α、β分别为质心距离差异权重、法线角度差异权重；
[0038]
步骤222，根据p
k
‑1中共检测到的障碍物的总数量为m个，记为中共检测到的障碍物的总数量为m个，记为其中u＝1，2，
…
，m；p
k
中共检测到的障碍物的总数量为n个，记为其中，v＝1，2，
…
，n，将关联矩阵表示为下式(8)：
[0039][0040]
式中，表示由式(10)计算的差异度；
[0041]
步骤223，根据步骤222中建立的关联矩阵(8)，关联当前帧p
k
和上一帧p
k
‑1中的动态障碍物；重复上述步骤，直至当前帧p
k
的关联矩阵的每一行对应的障碍物关联完毕；若最小差异度所对应的p
k
‑1中元素的下标u＞v，则表明发生障碍物丢失；若最小差异度所对应的
p
k
‑1中元素的下标u＜v，则表明有新动态障碍物出现。
[0042]
进一步地，步骤23具体包括：
[0043]
根据步骤21关联的动态障碍物，结合步骤1计算得到的两帧点云数据相对运动为判断障碍物运动状态，利用式(9)计算障碍物的运动距离：
[0044][0045]
根据激光雷达传感器的帧率f，则动态障碍物的速度为v＝δd
×
f，方向为f，方向为表示t
k
的齐次坐标，表示t
k
‑1的齐次坐标。
[0046]
本发明由于采取以上技术方案，其具有以下优点：
[0047]
1.不需要结合多传感器，在使用单一传感器的前提下进行动态障碍物检测目的，使系统更加高效。
[0048]
2.激光里程计在短时内的位姿估计较为准确，且不会出现零漂现象，也不需额外的标定工作，所以使用激光里程计代替惯导可以使系统更简单和稳定，惯导会出现零漂现象，且需要和激光雷达进行外参标定，会使得系统变复杂，不确定因素增多。
[0049]
3.由于地面可能为静态障碍物，故不需要进行地面分割，相比于先地面分割后进行聚类的一般障碍物检测算法，使用基于法线方向的区域生长法对点云进行分割聚类可将分割聚类算法复杂度减小的前提下使其更加适用于无人机飞行场景。此外在使用区域生长法时利用了激光里程计过程中计算的曲率信息，避免了冗余计算；同时在数据关联阶段加入了法线相似性判断，充分利用了分割聚类阶段产生的障碍物信息。
[0050]
4.由于障碍物跟踪是在估计了自身位姿的基础上进行的，相比于不考虑无人机自身位姿的跟踪算法，本发明的障碍物跟踪效果更好。
附图说明
[0051]
图1为本发明的无人机动态障碍物检测算法架构示意图。
具体实施方式
[0052]
下面结合附图和实施例对本发明进行详细的描述。
[0053]
本实施例利用多帧点云数据的空间特征点信息进行配准操作，实时解算无人机位姿，从而实现激光里程计功能，再通过障碍物检测及跟踪技术，结合由激光里程计得到的自身运动状态，达到检测动态障碍物并综合自主避障目的。
[0054]
如图1所示，本发明实施例提供的基于激光里程计的无人机动态障碍物检测系统包括激光里程计模块和动态障碍物候选检测模块，其中：激光里程计模块通过如下步骤1，计算同一特征点在两帧相邻时刻激光点云数据中的匹配关系，以解算得到无人机的位姿信息。动态障碍物候选检测模块通过如下步骤2，检测出激光点云数据中的所有障碍物，判断其中的动态障碍物的运动状态，即计算动态障碍物的运动速度。考虑到算法实时性要求，算法架构分为双线程，激光里程计模块和动态障碍物候选检测模块各占用一个线程，提供并行计算，将信息整合至状态估计阶段，输出最终的所有障碍物及其运动信息。下面通过步骤的形式，来描述各模块的实现方式。
[0055]
步骤1，通过激光点云数据的几何特性，提取符合预设曲率特征的点云作为特征
点，建立同一特征点在两帧相邻时刻激光点云数据中的匹配关系，并构建重投影代价函数，使用非线性优化技术使位姿在三维解空间中进行迭代，直至位姿收敛，获得无人机位姿信息。
[0056]
在一个实施例中，步骤1中的“提取符合预设曲率特征的点云作为特征点”的方法具体包括：
[0057]
步骤11，对激光点云数据进行体素滤波，以在保证形状特征不变的前提下大幅度降低点云的数据量，为降低算法复杂度提供有利条件。
[0058]
步骤12，计算激光点云数据中各点云的曲率值，比如利用下式(1)，并按照曲率值大小排序，曲率值最大的点云定义为特征角点，曲率值最小的点云定义为特征平面点。选择特征角点和特征平面点作为特征点，为后续点云配准做准备。
[0059][0060]
式中，x
(k，i)
表示第k帧激光点云数据p
k
中的第i个点云在雷达坐标系中的坐标，描述为：x
(k，i)
＝[x y z]
t
∈p
k
；x
(k，j)
表示p
k
中的第j个点云在雷达坐标系中的坐标；s表示x
(k，i)
的邻域，其可以通过kd树等现有方法快速获得。
[0061]
进一步地，步骤1中，通过分区域提取所述特征点，这样可以保证特征点分布均匀，即将点云云数据划分为不同区域，在每个区域内提取一定阈值数量的特征点。此外，在选取特征点过程中，应当避免将符合以下三种情况的点云被选作为特征点：1)局部平面与激光束大致平行的点云，此类点云可靠性较低，其中的局部平面可以理解为周围(一定邻域内)的点云与该点云共面而形成的平面；2)一定半径范围内(根据经验而定)已存在特征点的点云；3)在该点云位于遮挡区域的边界上。
[0062]
步骤1中的“建立同一特征点在两帧相邻时刻激光点云数据中的匹配关系”具体包括特征角点和特征线之间的点云线距离或特征平面点和特征面之间的点云面距离通过和得到同一特征点在两帧相邻时刻激光点云数据中的匹配关系，以便计算特征点的相对运动关系。
[0063]
设定：当前第k帧激光点云数据表示为p
k
，其上一帧(k
‑
1)激光点云数据表示为p
k
‑1，对于两类特征点处理分别如下：
[0064]
对于p
k
中的每一个特征角点，下面以第a个特征角点x
(k，a)
为例，对步骤1中的建立同一特征点在两帧相邻时刻激光点云数据中的匹配关系的方法进行说明。
[0065]
在p
k
‑1中寻找特征角点x
(k，a)
所对应的特征线段，该线段可以由两点云(b，c)表示，点云b为p
k
‑1中与特征角点x
(k，a)
距离最近的点云，点云c是p
k
‑1中在特征角点x
(k，a)
相邻扫描线上与特征角点x
(k，a)
距离最近的点云，相邻扫描线即为雷达扫描线中在径向上与该扫描线最为接近的一条扫描线，其中，表示为下式(2)：
[0066][0067]
式中，x
(k，a)
表示p
k
中的第a个特征角点在雷达坐标系中的坐标，x
(k
‑
1，b)
表示p
k
‑1中的第b个特征角点在雷达坐标系中的坐标，x
(k
‑
1，c)
表示p
k
‑1中的第c个特征角点在雷达坐标系中的坐标。
[0068]
对于p
k
中的每一个特征平面点，下面以p
k
中的第d个特征平面点x
(k，d)
为例，对步骤1中的建立同一特征点在两帧相邻时刻激光点云数据中的匹配关系的方法进行说明。
[0069]
在p
k
‑1中寻找该特征平面点所对应的特征面，特征面可由三点云(e，f，g)表示，点云e为p
k
‑1中与特征平面点x
(k，d)
距离最近的点云，点云f为p
k
‑1中在点云e同一相邻扫描线上与点云d距离最近的点云，点云g为在相邻扫描线上与特征平面点x
(k，d)
距离最近的点云，其中，表示为下式(3)：
[0070][0071]
式中，x
(k
‑
1，e)
表示p
k
‑1中的第e个特征平面点在雷达坐标系中的坐标，x
(k
‑
1，f)
表示p
k
‑1中的第f个特征平面点在雷达坐标系中的坐标，x
(k
‑
1，g)
表示p
k
‑1中的第g个特征平面点在雷达坐标系中的坐标。
[0072]
步骤1中的“构建重投影代价函数，使用非线性优化技术使其在三维解空间中进行迭代，直至位姿收敛，获得无人机位姿信息”的方法具体方法包括：
[0073]
根据和建立重投影代价函数，将位姿解算问题转化成式(4)表示的非线性最小二乘求解问题，求解最优变换矩阵估计值
[0074][0075]
其中，表示p
k
中第i个特征点在雷达坐标系中的坐标，n表示特征点的数量；为在p
k
‑1中的齐次坐标，即中的齐次坐标，即表示在p
k
中的齐次坐标。
[0076]
重复步骤1中的“建立同一特征点在两帧相邻时刻激光点云数据中的匹配关系”以及“根据重投影代价函数求解直至位姿收敛。
[0077]
在一个实施例中，可由levenberg
‑
marquardt方法求解，利用梯度下降使其不断迭代至位姿收敛，该方法具体包括：
[0078]
首先，通过下式(5)计算其雅克比矩阵
[0079][0080]
式中，f为式(4)表示的重投影代价函数。
[0081]
然后，按照下式(6)更新位姿，不断迭代直至重投影代价函数收敛：
[0082][0083]
其中，λ为levenberg
‑
marquardt的迭代参数，λ较小时，其更接近于高斯牛顿法，λ较大时其更接近于梯度下降法。
[0084]
步骤2具体包括：
[0085]
步骤21，根据步骤1输出的无人机的位姿信息，使用区域生长法将整体点云按照障
碍物聚类为多个点云簇，以区分为各自独立的障碍物目标。
[0086]
步骤22，根据激光里程计计算得到的两帧相对运动以及多帧激光点云数据中障碍物的质心和法线差异程度，计算差异度函数，并建立关联矩阵，将多帧激光点云数据中的同一障碍物关联起来。
[0087]
步骤23，根据动态障碍物的关联情况，结合激光里程计计算得到的帧间相对运动判断障碍物运动状态，从而得到动态障碍物的运动方向及速度，即状态估计。
[0088]
在一个实施例中，由于无人机飞行环境下地面可能为障碍物，故不需要常规的地面分割操作，考虑到不同障碍物间会产生较大的法线角度变化，故选择使用基于曲率区域生长法分割聚类无人机飞行环境下的激光点云数据，步骤21具体包括：
[0089]
步骤211，种子点选取：将步骤12获得的曲率值最小的点云作为起始种子点p0。这是由于曲率最小点位于平坦区域中，从最平坦区域开始生长可以减少分割的总数。
[0090]
步骤212，制定生长准则：对每个种子点p
i
，搜索该点的所有近邻点，并且，按照如下步骤执行：
[0091]
步骤2121，计算每个近邻点与当前种子点p
i
法线角度的差值，若差值小于设定角度阈值，则进入步骤2122。其中，一般平面的法线角度变化是平缓变化的，可以取20
°
为一个阈值，就是说如果超过20
°
，可以认为有较大的法线变化。
[0092]
步骤2122，计算该近邻点曲率，若曲率值小于设定曲率阈值(比如1.0)，则将该点添加进当前种子点集
[0093]
返回步骤211，直到当前种子点集所有近邻点都不满足生长准则。随后从原始采集的点云集中删除这些种子点p
i
，并将该种子点集作为一个障碍物点云簇保存下来。
[0094]
步骤213，设定终止条件：重复上述步骤211至步骤212，直到点云集中所有点云处理完毕。
[0095]
当然，除了采用区域生长法对点云聚类进行分割外，也可以使用现有的其它方法进行分割操作。
[0096]
步骤22中的“数据关联”具体包括：
[0097]
步骤221，根据激光里程计计算得到的两帧相对运动为以及多帧激光点云数据中障碍物的质心和法线差异程度，采用式(7)计算差异度函数
[0098][0099]
其中，为对应的旋转矩阵so(3)，第k
‑
1帧第v个障碍物的质心为在三维空间中位置的齐次坐标表现形式，α、β分别为质心距离差异权重、法线角度差异权重，二者的数值考虑到式(7)的加号前一项和后一项的量纲不同，需要将这两项的均值进行归一化处理，比如α，其对应的前一项的大概均值为2m，那么，α＝质心距离重要程度/2。同理，而两个分子：质心距离重要程度+法线角度重要程度＝1。
[0100]
步骤222，关建立联矩阵：根据p
k
‑1中共检测到的障碍物的总数量为m个，记为
其中u＝1，2，
…
，m；p
k
中共检测到的障碍物的总数量为n个，记为其中，v＝1，2，
…
，n，将关联矩阵表示为下式(8)：
[0101][0102]
式中，表示由式(10)计算的差异度，其中的每一行的第二个下标相同，这意味每一行对应着当前帧的同一个障碍物，那么，第一行的下标均为1，则表示第一障碍物；第二行的下标均为2，则表示第二障碍物；
……
；第v行的下标均为v，则表示第v障碍物。
[0103]
步骤223，根据步骤222中建立的关联矩阵(8)，关联当前帧p
k
和上一帧p
k
‑1中的动态障碍物。具体地，关联某一障碍物时，将当前帧p
k
的关联矩阵的选定行所对应的障碍物作为选定障碍物，再计算选定行中的各元素分别与上一帧p
k
‑1中关联矩阵中所有的元素的差异度，并以计算得到的最小差异度所对应的p
k
‑1中元素所对应的障碍物作为选定障碍物的关联障碍物，此时判定选定障碍物与关联障碍物是同一障碍物。
[0104]
重复上述步骤，直至当前帧p
k
的关联矩阵的每一行对应的障碍物关联完毕。
[0105]
由于以上操作是在潜在动态点云基础上实现的，故关联成功的障碍物一定为动态障碍物。
[0106]
进一步地，若最小差异度所对应的p
k
‑1中元素的下标u＞v，则表明发生障碍物丢失；若最小差异度所对应的p
k
‑1中元素的下标u＜v，则表明有新动态障碍物出现。
[0107]
比如：p
k
中有10个障碍物，上一帧p
k
‑1有10个障碍物，并且这10个障碍物在这两帧点云数据中能够一一对应，那么，该关联矩阵为10行10列的矩阵，将第一行的10个元素对应的是当前帧p
k
的第一个障碍物作为选定障碍物。而步骤223是将第一行的10个元素与p
k
‑1的关联矩阵中的所有元素，利用式(10)进行差异度计算，其中，计算得到的最小差异度所对应的p
k
‑1中元素所对应的障碍物作为选定障碍物的关联障碍物，二者被认定为同一个障碍物。
[0108]
步骤23具体包括：
[0109]
根据步骤21关联的动态障碍物，结合步骤1计算得到的两帧点云数据相对运动为判断障碍物运动状态，从而得到动态障碍物的运动方向及速度。
[0110]
例如：步骤23可以通过如下步骤实现，也可以通过本领域公知的其它方法获得：
[0111]
若某障碍物在连续两帧点云数据中被关联，其在p
k
‑1中的坐标为中的坐标为p
k
中的坐标为步骤1计算得到的两帧点云数据相对运动为则利用式(9)计算障碍物的运动距离：
[0112][0113]
根据激光雷达传感器的帧率f，则动态障碍物的速度为v＝δd
×
f，方向为v＝t
k
‑
t
k
‑1，表示t
k
的齐次坐标，表示t
k
‑1的齐次坐标。
[0114]
经上述步骤可以得到点云数据中动态障碍物信息。
[0115]
若要得到静态障碍物信息，可建立栅格地图，根据无人机高度将相应范围点云存入栅格地图，若栅格被占据，即表示为无人机不可通过，从而达到在动态环境下高效安全避障目的。
[0116]
本发明通过障碍物检测及跟踪技术，结合两帧点云数据相对运动，达到检测动态障碍物、提高系统安全性目的。
[0117]
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。