CN114815899A - 基于3d激光雷达传感器的无人机三维空间路径规划方法 - Google Patents

Abstract

本发明公开了一种基于3D激光雷达传感器的无人机三维空间路径规划方法，包括：1根据使用者的需求，输入需要到达的目标点，然后根据激光雷达的点云数据，构建三维栅格地图；2根据得到的地图信息使用混合A*算法规划出一条路径；3对路径使用MinimumSnap算法进行二次优化，得到性能更优的路径；4随着无人机的飞行，点云数据不断变化，更新三维栅格地图并重新计算路径，追踪新的路径直到找到目标点。本发明将传统的混合A*算法应用到无人机的路径规划中，并对规划路径进行了二次优化，提升了路径的平滑度和安全性，并通过不断地更新地图实现了在未知环境下的自主导航。

Description

基于3D激光雷达传感器的无人机三维空间路径规划方法

技术领域

本发明涉及机器人路径规划技术，无人机控制等技术领域，具体的说是一种基于3D激光雷达传感器的无人机三维空间路径规划方法。

背景技术

随着无人机技术的快速发展，无人机应用越来越广泛。由于无人机在各领域广泛的应用、使用场景的多变和任务的复杂，无人机往往需要在在充满障碍物动态场景中执行任务。现有技术如大数据、云计算无法解决路径查找和定位的难题,这就需要无人机具有自主导航和安全飞行的能力，能够利用由传感器、摄像头或全球定位系统GPS采集的信息自行做出决策。路径规划是自主导航的重要技术。尽管现在已有很多关于路径规划上的研究，但应用在无人机上仍然存在下面两个问题。第一，传统算法虽然能够得到一条可通行的路径，但由于其只考虑了环境几何信息，往往忽略了无人机本身的运动学模型。因此，其得到的轨迹质量较低，并不适合直接作为无人机的控制指令；第二，无人机的自主导航常常发生在动态未知的环境下，这就要求无人机的的路径规划需要满足可以在短时间内不断地重新生成安全平滑的轨迹的要求。

发明内容

本发明为克服上述现有技术的不足之处，提供一种基于3D激光雷达传感器的无人机三维空间路径规划方法，以期能针对于动态未知的环境下场景，实现无人机的路径规划，从而高效的解决现有技术中轨迹平滑度较低的问题。

本发明为达到上述发明目的，采用如下技术方案：

本发明一种基于3D雷达激光传感器的无人机三维空间路径规划方法的特点是按如下步骤进行：

步骤1，以无人机上集成的三维激光雷达为原点，利用所述三维激光雷达获得采集区域内的点云数据；

步骤2，利用点云膨胀处理算法对所采集的点云数据进行预处理，从而得到新的点云数据；

步骤3，将新的点云数据映射到三维栅格地图中；其中，所述三维栅格地图中每个栅格的状态用布尔值表示，若布尔值为true，则表示相应栅格为占据状态，若布尔值为false，则表示相应栅格为空闲状态，并将所述三维栅格地图中未映射的栅格记为未知状态；

步骤4，基于所述三维栅格地图，使用混合A*算法计算出无人机从起点栅格到终点栅格的最优路径；

步骤4.1，利用式(1)构建无人机的运动模型：

p(t)＝[p_x(t),p_y(t),p_z(t)]^T (1)

式(1)中，p_x(t)，p_y(t)，p_z(t)分别表示所述三维栅格地图中无人机在t时刻所在栅格的三维坐标值；p(t)表示t时刻无人机的运动轨迹；令维度参数μ∈{x,y,z}，并利用式(2)计算t时刻的维度参数μ的运动轨迹p_μ(t)；

式(2)中，a_k表示多项式中第k项系数；K表示轨迹定义的阶数；

步骤4.2利用式(3)和式(4)分别计算无人机在t时刻的状态向量x(t)和控制输入u(t)：

式(3)和式(4)中，T表示转置，

表示运动轨迹p(t)的导数，n表示偏导的阶数；

表示实数；p⁽ⁿ⁾(t)表示运动轨迹p(t)的第n阶导数；

步骤4.3，利用式(5)得到无人机在T₁时段的运动轨迹的代价traj(T₁)：

步骤4.4，应用庞特里亚金最小原理对式(5)进行求解，得到无人机在所述三维栅格地图中从起点栅格的状态到当前栅格的状态的任意一条路径的估计代价traj^*(T₁)；

步骤4.5，利用式(6)计算无人机在所述三维栅格地图中从起点栅格的状态到当前栅格的状态的任意一条路径的实际代价g_c：

式(6)，J表示从起点栅格到当前栅格所经过的栅格个数，u_j表示无人机在第j个栅格的控制输入；τ_j表示无人机经过第j个栅格的时间；

步骤4.6，利用式(7)计算无人机从起点栅格的状态到目标栅格的状态的任意一条路径的总代价f_c：

f_c＝g_c+traj^*(T₁) (7)

步骤4.7，按照4.4-步骤4.6的过程计算从起点栅格的状态到目标栅格的状态的所有路径的总代价，并从中选取总代价最小的一条路径作为最优路径；

步骤5，对所述最优路径进行平滑处理，并生成新的路径；

步骤5.1，根据最优路径所经过的每个栅格的三维坐标点，将最优路径分成h段轨迹，利用式(8)所示的多项式曲线表示每条路段轨迹：

式(8)中，p_i为最优路径中第i段路段轨迹的参数向量；t_i表示无人机经过第i段路段轨迹终点的时刻；

步骤5.2，利用式(9)构建h条路段轨迹组成的参数向量p：

步骤5.3，利用式(10)构建最优路径的优化目标函数minf(p)：

式(10)中，Q表示二次型矩阵；

步骤5.4，利用二次型矩阵求解法对所述优化目标函数minf(p)进行求解，得到h条路段轨迹组成组成的参数向量p，从而得到一条新的优化轨迹；

步骤6：设定地图更新时间，用于定时更新地图并不断追踪所述新的优化轨迹；

步骤6.1，遍历新的优化轨迹中每个栅格的三维坐标点，找到与无人机之间距离最近的栅格，并利用式(11)和式(12)得到无人机追踪新的优化轨迹中最近距离的栅格的速度v和加速度a；

v＝v_e (11)

a＝a_e+k_p(p_e-p_cur)+k_d(v_e-v_cur) (12)

式(11)和(12)中，p_e、v_e、a_e表示新的优化轨迹中与无人机之间距离最近的栅格的三维坐标点的位置、速度和加速度；p_cur、v_cur、a_cur表示无人机在当前栅格的三维坐标点的位置、速度和加速度；k_p、k_d为常系数；

步骤6.2，向无人机发送与其距离最近的栅格的三维坐标点的位置p_e、速度v_e和加速度a_e，使得无人机追踪到新的优化后轨迹中的最近距离的栅格，再返回步骤6.1执行，直到达到地图更新时间后，返回步骤1执行，直到达到目标栅格为止。

与已有技术相比，本发明的有益效果是：

1.本发明通过在无人上的3D激光雷达传感器点云信息的采集与三维栅格地图的建立，并在混合A*生成的轨迹的基础上进行了二次优化；在实际飞行中时可以快速生成安全平滑的轨迹；相比于传统的无人机路径规划算法，提升了路径平滑度，并通过不断地更新地图和追踪路径实现了在动态未知环境下的路径规划。

2.通过无人机上集成3D激光雷达实现了三维点云信息的采集，从而能够快速而精确的采集无人机周围的环境信息。

3.本发明步骤2中，先对点云数据进行形态上的膨胀处理。膨胀操作主要用于去噪,并且保持原有形状，其效果就像对点云图像边缘进行向外扩展。从而避免传感器的测量误差而引起与障碍物碰撞。

4.本发明步骤3中，将新的点云数据映射到三维栅格地图中。使用布尔值表示三维栅格的地图的状态，未被映射的栅格表示未探知的区域。从而区分已经探知的区域和未被地图探知的区域，并实现了三维栅格地图在内存中的紧凑排列，减少了访问地图的时间，提高了搜索地图的效率。

5.本发明步骤4中，重新设计了混合A*算法中的实际代价和估计代价并得到最优路径，实现了考虑无人机本身运动学约束的路径规划，让无人机的运动空间从离散空间变成连续空间，从而提高了无人机路径规划的性能。

6.本发明步骤5中，对混合A*生成的路径进行了平滑处理，从而提高了路径平滑度。

7.本发明步骤6中对三维栅格地图定时更新，并不断追踪优化后路径轨迹直到找到目标点为止，从而实现在动态未知环境中的路径规划，从而提高了无人机自主导航的安全性。

附图说明

图1为本发明方法流程示意图；

图2为本发明三维栅格地图构键方法示意图；

图3为本发明路径规划方法的示意图；

图4为本发明路径优化的示意图。

具体实施方式

本实施例中，如图1所示，一种基于3D雷达激光传感器的无人机三维空间路径规划方法，包括以下步骤：

步骤1，以无人机上集成的三维激光雷达为原点，利用三维激光雷达获得采集区域内的点云数据；

步骤2，利用点云膨胀处理算法对所采集的点云数据进行预处理，从而得到新的点云数据；

步骤2.1，为了避免传感器的测量误差而发生的障碍物碰撞的情况出现，需要先对点云数据进行形态上的膨胀处理如式1。膨胀操作主要用于去噪,并且保持原有形状，其效果就像对点云图像边缘进行向外扩展。

步骤2.2,等式(1)中集合A表示图像和集合B表示卷积核，A⊕B代表B对A的膨胀就是将点云图像与核进行卷积。

步骤3，将新的点云数据映射到三维栅格地图中；其中，三维栅格地图中每个栅格的状态用布尔值表示，若布尔值为true，则表示相应栅格为占据状态，若布尔值为false，则表示相应栅格为空闲状态，并将三维栅格地图中未映射的栅格记为未知状态；

步骤3.1,如图2所示，得到膨胀得点云图像后，将点云数据映射到三维栅格地图。

式2中x_i,y_i,z_i表示点云位置；x_o,y_o,z_o表示地图的起点；k表示地图的分辨率；x,y,z表示点云映射后对应栅格节点的坐标。最终通过对空间的栅格化，点云的膨胀操作和地图的映射，从而构建了可以用于搜索算法的地图。

步骤4，如图3所示，基于三维栅格地图，使用混合A*算法计算出无人机从起点栅格到终点栅格的最优路径；

步骤4.1,用三个独立的一维时间参数化多项式函数来表示轨迹。利用式(1)构建无人机的运动模型：

p(t)＝[p_x(t),p_y(t),p_z(t)]^T (3)

式(3)中，p_x(t)，p_y(t)，p_z(t)分别表示三维栅格地图中无人机在t时刻所在栅格的三维坐标值；p(t)表示t时刻无人机的运动轨迹；令维度参数μ∈{x,y,z}，并利用式(4)计算t时刻的维度参数μ的运动轨迹p_μ(t)；

式(4)中，a_k表示多项式中第k项系数；K表示轨迹定义的阶数；

步骤4.2从四旋翼系统的角度来看，它对应于一个线性时不变(LTI)系统，利用式(4)和式(5)分别计算无人机在t时刻的状态向量x(t)和控制输入u(t)：

式(5)和式(6)中，T表示转置，

表示运动轨迹p(t)的导数，n表示偏导的阶数；

表示实数；p⁽ⁿ⁾(t)表示运动轨迹p(t)的第n阶导数；

步骤4.3定义状态空间模型：

步骤4.4该状态方程的完整解表示为：

步骤4.5，利用式(10)得到无人机在T₁时段的运动轨迹的代价traj(T₁)：

步骤4.6，应用庞特里亚金最小原理对式(10)进行求解，得到无人机在三维栅格地图中从起点栅格的状态到当前栅格的状态的任意一条路径的估计代价traj^*(T₁)；

步骤4.7，利用式(11)计算无人机在三维栅格地图中从起点栅格的状态到当前栅格的状态的任意一条路径的实际代价g_c：

式(11)，J表示从起点栅格到当前栅格所经过的栅格个数，u_j表示无人机在第j个栅格的控制输入；τ_j表示无人机经过第j个栅格的时间；

步骤4.8，利用式(12)计算无人机从起点栅格的状态到目标栅格的状态的任意一条路径的总代价f_c：

f_c＝g_c+traj^*(T₁) (12)

步骤4.9，按照4.4-步骤4.6的过程计算从起点栅格的状态到目标栅格的状态的所有路径的总代价，并从中选取总代价最小的一条路径作为最优路径；

步骤5，对最优路径进行平滑处理，并生成新的路径点；

步骤5.1，对步骤4生成的轨迹进行维度分解。由于三维轨迹计算非常的复杂，为了简化计算需要先对三维轨迹进行维度分解，分解为x，y，z三个维度的轨迹。

步骤5.2，如图4所示，根据最优路径所经过的每个栅格的三维坐标点，将最优路径分成h段轨迹，利用式(8)所示的多项式曲线表示每条路段轨迹：

式(13)中，p_i为最优路径中第i段路段轨迹的参数向量；t_i表示无人机经过第i段路段轨迹终点的时刻；

步骤5.3，利用式(14)计算多段轨迹组成的参数向量p：

步骤5.4，轨迹需满足一系列的约束条件，两端连续轨迹无碰撞，并且轨迹平滑。这样就可以将问题描述成一个约束优化问题。

轨迹优化问题就建模成了一个数学上的二次规划问题

步骤5.5.构建约束目标设定某一个点的位置、速度、加速度或者更高为一个特定的值，可以构成一个等式约束如式(18)所示。由于要过中间点，对中间点的位置也构建等式约束。相邻段之间的位置、速度、加速度连续可以构成一个等式约束。

位置约束:

式(18)中，t_i表示无人机经过第i段轨迹终点的时刻，h将最优路径分段的段数，n表示n表示轨迹的阶数，p_i，v_i，a_i表示无人机经过第i段轨迹终点时的位置、速度、和加速度约束。

步骤5.4，利用二次型矩阵求解法对优化目标函数minf(p)进行求解，得到h条路段轨迹组成组成的参数向量p，从而得到一条新的优化轨迹；

步骤5.5，最后将求出x,y,z三个维度的的p值合成优化后的轨迹。

步骤6：设定地图更新时间，用于定时更新地图并不断追踪新的优化轨迹；

步骤6.1，遍历新的优化轨迹中每个栅格的三维坐标点，找到与无人机之间距离最近的栅格，并利用式(11)和式(12)得到无人机追踪新的优化轨迹中最近距离的栅格的速度v和加速度a；

v＝v_e (19)

a＝a_e+k_p(p_e-p_cur)+k_d(v_e-v_cur) (20)

式(19)和(20)中，p_e、v_e、a_e表示新的优化轨迹中与无人机之间距离最近的栅格的三维坐标点的位置、速度和加速度；p_cur、v_cur、a_cur表示无人机在当前栅格的三维坐标点的位置、速度和加速度；k_p、k_d为常系数；

步骤6.2，向无人机发送与其距离最近的栅格的三维坐标点的位置p_e、速度v_e和加速度a_e，使得无人机追踪到新的优化后轨迹中的最近距离的栅格，再返回步骤6.1执行，直到达到地图更新时间后，返回步骤1执行，直到达到目标栅格为止。

Claims (1)

1.一种基于3D雷达激光传感器的无人机三维空间路径规划方法，其特征是按如下步骤进行：

步骤1，以无人机上集成的三维激光雷达为原点，利用所述三维激光雷达获得采集区域内的点云数据；

步骤2，利用点云膨胀处理算法对所采集的点云数据进行预处理，从而得到新的点云数据；

步骤3，将新的点云数据映射到三维栅格地图中；其中，所述三维栅格地图中每个栅格的状态用布尔值表示，若布尔值为true，则表示相应栅格为占据状态，若布尔值为false，则表示相应栅格为空闲状态，并将所述三维栅格地图中未映射的栅格记为未知状态；

步骤4，基于所述三维栅格地图，使用混合A*算法计算出无人机从起点栅格到终点栅格的最优路径；

步骤4.1，利用式(1)构建无人机的运动模型：

p(t)＝[p_x(t),p_y(t),p_z(t)]^T (1)

式(1)中，p_x(t)，p_y(t)，p_z(t)分别表示所述三维栅格地图中无人机在t时刻所在栅格的三维坐标值；p(t)表示t时刻无人机的运动轨迹；令维度参数μ∈{x,y,z}，并利用式(2)计算t时刻的维度参数μ的运动轨迹p_μ(t)；

式(2)中，a_k表示多项式中第k项系数；K表示轨迹定义的阶数；

步骤4.2利用式(3)和式(4)分别计算无人机在t时刻的状态向量x(t)和控制输入u(t)：

式(3)和式(4)中，T表示转置，

表示运动轨迹p(t)的导数，n表示偏导的阶数；

表示实数；p⁽ⁿ⁾(t)表示运动轨迹p(t)的第n阶导数；

步骤4.3，利用式(5)得到无人机在T₁时段的运动轨迹的代价traj(T₁)：

步骤4.4，应用庞特里亚金最小原理对式(5)进行求解，得到无人机在所述三维栅格地图中从起点栅格的状态到当前栅格的状态的任意一条路径的估计代价traj^*(T₁)；

步骤4.5，利用式(6)计算无人机在所述三维栅格地图中从起点栅格的状态到当前栅格的状态的任意一条路径的实际代价g_c：

式(6)，J表示从起点栅格到当前栅格所经过的栅格个数，u_j表示无人机在第j个栅格的控制输入；τ_j表示无人机经过第j个栅格的时间；

步骤4.6，利用式(7)计算无人机从起点栅格的状态到目标栅格的状态的任意一条路径的总代价f_c：

f_c＝g_c+traj^*(T₁) (7)

步骤4.7，按照4.4-步骤4.6的过程计算从起点栅格的状态到目标栅格的状态的所有路径的总代价，并从中选取总代价最小的一条路径作为最优路径；

步骤5，对所述最优路径进行平滑处理，并生成新的路径；

步骤5.1，根据最优路径所经过的每个栅格的三维坐标点，将最优路径分成h段轨迹，利用式(8)所示的多项式曲线表示每条路段轨迹：

式(8)中，p_i为最优路径中第i段路段轨迹的参数向量；t_i表示无人机经过第i段路段轨迹终点的时刻；

步骤5.2，利用式(9)构建h条路段轨迹组成的参数向量p：

步骤5.3，利用式(10)构建最优路径的优化目标函数minf(p)：

式(10)中，Q表示二次型矩阵；

步骤5.4，利用二次型矩阵求解法对所述优化目标函数minf(p)进行求解，得到h条路段轨迹组成组成的参数向量p，从而得到一条新的优化轨迹；

步骤6：设定地图更新时间，用于定时更新地图并不断追踪所述新的优化轨迹；

步骤6.1，遍历新的优化轨迹中每个栅格的三维坐标点，找到与无人机之间距离最近的栅格，并利用式(11)和式(12)得到无人机追踪新的优化轨迹中最近距离的栅格的速度v和加速度a；

v＝v_e (11)

a＝a_e+k_p(p_e-p_cur)+k_d(v_e-v_cur) (12)

式(11)和(12)中，p_e、v_e、a_e表示新的优化轨迹中与无人机之间距离最近的栅格的三维坐标点的位置、速度和加速度；p_cur、v_cur、a_cur表示无人机在当前栅格的三维坐标点的位置、速度和加速度；k_p、k_d为常系数；

步骤6.2，向无人机发送与其距离最近的栅格的三维坐标点的位置p_e、速度v_e和加速度a_e，使得无人机追踪到新的优化后轨迹中的最近距离的栅格，再返回步骤6.1执行，直到达到地图更新时间后，返回步骤1执行，直到达到目标栅格为止。

Images