CN117685994B - 一种空地协同的无人车路径规划方法 - Google Patents

Abstract

本发明属于多无人系统协同控制领域，公开了一种空地协同的无人车路径规划方法，步骤如下：首先，基于无人车和无人机搭载的激光雷达，分别得到各自的点云地图；其次，将无人机的点云地图转换到无人车坐标系下，得到融合点云，生成三维栅格地图；再次，由三维栅格地图生成地面二维栅格地图；最后，基于导航地图进行可执行路径搜索，基于复杂地形的改进A*算法生成初始路径，由B样条曲线将初始路径拟合为光滑轨迹，然后基于软约束的优化方法，通过动力学可行性、轨迹光滑性、碰撞安全性三项惩罚对光滑轨迹进行优化，得到规划轨迹，进而可实现无人车高效快速安全的目标区域搜索任务。

Description

一种空地协同的无人车路径规划方法

技术领域

本发明属于多无人系统协同控制领域，尤其涉及一种空地协同的无人车路径规划方法。

背景技术

近年来，随着感知定位、决策规划、智能控制等相关技术的逐渐成熟，以各种无人机（Unmanned Aerial Vehicle，UAV）和无人车（Unmanned Ground Vehicle，UGV）为代表的移动机器人及无人智能系统正处于飞速发展当中，并在各个领域中展现出极高的应用价值和广阔的应用前景。单独的无人机或无人车系统都难以在陆空立体空间环境中满足复杂任务的需求。为了结合无人机和无人车的优点和特性，不少研究围绕无人机和无人车异构系统平台而展开，将这两者考虑为不同的受控子系统，通过智能体系统的整体优化和协同控制，实现异构联合探测感知、智能决策与任务分配以及路径搜索与轨迹优化。以此拓展异构智能体系统作业任务的时间、空间可达能力，进而提升整体系统的机动性、鲁棒性、高效化和全覆盖能力。

然而实际应用中存在多种复杂空间约束，这对无人机-无人车陆空异构系统的自主协同任务提出挑战。例如对于密林环境，无人机的空中视野可能被遮挡，需要由无人车进行地面环境感知；而对于城市道路环境，无人车的地面道路复杂，可能存在高楼等多种障碍物，需要借助无人机的高视野进行局部路径规划。因此对于复杂障碍物约束下的未知空间，无人机和无人车可以根据各自的特性与优点进行决策和任务分工，针对不同环境地形，通过协同感知获取环境信息，构建三维地图，同时针对障碍物遮挡等特殊约束环境，通过路径搜索和轨迹优化实现无人车的智能运动规划，使其能够穿越复杂地形。

发明内容

针对现有技术中的不足，本发明提出了一种空地协同的无人车路径规划方法，用于实现无人车高效快速安全的目标区域搜索任务。从而解决由于无人车对地形感知能力较弱，导致在复杂地形环境下经常容易陷入危险的问题。

本发明的技术方案具体如下：

一种空地协同的无人车路径规划方法，包括以下步骤：

步骤一、基于无人车和无人机搭载的激光雷达，分别得到各自的点云地图；

步骤二、将无人机的点云地图转换到无人车坐标系下，得到融合点云，并生成三维栅格地图；

步骤三、由三维栅格地图生成地面二维栅格地图；

步骤四、基于导航地图进行可执行路径搜索，基于复杂地形的改进A*算法生成初始路径，由B样条曲线将初始路径拟合为光滑轨迹，然后基于软约束的优化方法，通过动力学可行性、轨迹光滑性、碰撞安全性三项惩罚对光滑轨迹进行优化，得到无人车的规划轨迹。

优选的，所述步骤三具体包括：从融合点云中提取地面点云，结合无人车运动能力，建立包含高程信息的二维栅格地图。

优选的，所述步骤三还包括：将地面点云高度差超过无人车可通行范围的栅格标记占用栅格，获得可通行的二维占据栅格地图。

优选的，所述步骤四中的初始路径采用欧式距离计算无人车的当前位置点与目标位置点的直线距离，代价函数为：

其中，为二维栅格地图中无人车的初始位置点所在栅格到无人车的当前位置点所在栅格的估计距离，/>为无人车的当前位置点所在栅格到无人车的目标位置点所在栅格的估计距离。

优选的，结构化地形中，和/>采用曼哈顿距离或欧式距离。

优选的，非结构化地形中，和/>的计算方式分别如下：

其中，为无人车的当前位置点到初始位置点的横纵坐标差，/>为无人车的当前位置点所在栅格与初始位置点所在栅格之间的高程距离，/>为从无人车的初始位置点到当前位置点的总穿越代价，/>为无人车的当前位置点到目标位置点的横纵坐标差，/>为无人车的当前位置点所在栅格到目标位置点所在栅格之间的高程距离，/>为从无人车的当前位置点到目标位置点的总穿越代价。

相比于现有技术，本发明的有益效果在于：

本发明提出的一种空地协同的无人车路径规划方法，基于统一框架下的规划路径，在复杂环境下可实现连续轨迹规划与运动控制，利用无人机的高空视角优势为无人车提供了细致的点云地图，从而使无人车可以根据点云地图建立带高程信息的二维栅格地图，实现了无人车在复杂地形下的安全快速运动规划。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，通过参考附图可以更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据这些附图获得其他的附图。

图1是本发明提出的一种空地协同的无人车路径规划方法的原理图。

图2是本发明融合点云生成的三维栅格地图。

图3是本发明得到的二维占据栅格地图。

图4是改进A*算法的流程图。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

如图1所示，本发明提出的一种空地协同的无人车路径规划方法，包括以下步骤：

首先，基于无人机和无人车搭载的激光雷达分别得到各自的点云地图。然后，将无人机的点云地图转换到无人车坐标系下，将两个点云地图进行融合，生成三维栅格地图，如图2所示。

其次，对融合点云进行地面点云提取，得到地面点云后根据地面点云高度差生成可通行的二维占据栅格地图，将高度差超过无人车可通行范围的栅格标记为占用栅格，如附图3所示。

再次，基于复杂地形的改进A*算法生成初始路径，欧式距离计算欧几里得空间中无人车的当前位置点与目标位置点的直线距离，代价函数为：

其中，为二维栅格地图中无人车的初始位置点所在栅格到无人车的当前位置点所在栅格的代价成本，/>为无人车的当前位置点所在栅格到无人车的目标位置点所在栅格的估计距离。

对于结构化地形，和/>采用曼哈顿距离或欧式距离。

对于非结构化地形，无人车导航要考虑节点间的可穿越成本，因此采用改进代价函数：

其中，为无人车的当前位置点到初始位置点的横纵坐标差，/>为无人车的当前位置点所在栅格与初始位置点所在栅格之间的高程距离，/>为从无人车的初始位置点到当前位置点的总穿越代价，/>为无人车的当前位置点到目标位置点的横纵坐标差，/>为无人车的当前位置点所在栅格到目标位置点所在栅格之间的高程距离，/>为从无人车的当前位置点到目标位置点的总穿越代价。

所述基于复杂地形的改进A*算法的具体步骤如图4所示，将无人车初始位置点加入开集，从开集中弹出f值最小的节点加入闭集并设置为当前点，对当前点周边栅格进行遍历，对每个遍历的节点有三种情况，如果点在闭集中，则重新弹出一个新的开集点；如果第一次遍历到点，则计算其f值放入开集中；如果点已经在开集中，判读其是否有更小的f值，如果是，则将其设置为当前节点，原当前节点为该点父节点；循环以上操作，得到初始路径。

最后基于初始路径进行分段B样条曲线插值，得到实际执行的光滑轨迹。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、 “上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅仅表示第一特征水平高度小于第二特征。

在本发明中，术语“第一”、“第二”、“第三”、“第四”仅用于描述目的，不能理解为指示或暗示相对重要性。术语“多个”指两个或两个以上，除非另有明确的限定。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种空地协同的无人车路径规划方法，其特征在于，包括以下步骤：

步骤一、基于无人车和无人机搭载的激光雷达，分别得到各自的点云地图；

步骤二、将无人机的点云地图转换到无人车坐标系下，得到融合点云，并生成三维栅格地图；

步骤三、由三维栅格地图生成地面二维栅格地图；

步骤四、基于导航地图进行可执行路径搜索，基于复杂地形的改进A*算法生成初始路径，由B样条曲线将初始路径拟合为光滑轨迹，然后采用基于软约束的优化方法，通过动力学可行性、轨迹光滑性、碰撞安全性三项惩罚对光滑轨迹进行优化，得到无人车的规划轨迹；

所述步骤三具体包括：从融合点云中提取地面点云，结合无人车运动能力，建立包含高程信息的二维栅格地图；

所述步骤三还包括：将地面点云高度差超过无人车可通行范围的栅格标记占用栅格，获得可通行的二维占据栅格地图；

所述步骤四中的初始路径中计算无人车的当前位置点与目标位置点的直线距离，代价函数为：

其中，为二维栅格地图中无人车的初始位置点所在栅格到无人车的当前位置点所在栅格的估计距离，/>为无人车的当前位置点所在栅格到无人车的目标位置点所在栅格的估计距离；

非结构化地形中，和/>的计算方式分别如下：

其中，为无人车的当前位置点到初始位置点的横纵坐标差，/>为无人车的当前位置点所在栅格与初始位置点所在栅格之间的高程距离，/>为从无人车的初始位置点到当前位置点的总穿越代价，/>为无人车的当前位置点到目标位置点的横纵坐标差，/>为无人车的当前位置点所在栅格到目标位置点所在栅格之间的高程距离，/>为从无人车的当前位置点到目标位置点的总穿越代价。

2.根据权利要求1所述的空地协同的无人车路径规划方法，其特征在于，结构化地形中，和/>采用曼哈顿距离或欧式距离。