基于NDT算法和HybridA*算法的无人船路径规划方法技术

本发明专利技术提供一种基于NDT算法和Hybrid A*算法的无人船路径规划方法，自主定位采用基于NDT的激光雷达点云配准算法，并根据水面环境进行优化改进，提高定位稳定性；路径规划中对传统的Hybrid A*算法进行改进优化，减少算法资源占用与提高路径规划成功率；同时，充分考虑无人船的运动特性并加入路径平滑，使得无人船能在复杂狭窄水域内自主运行。船能在复杂狭窄水域内自主运行。船能在复杂狭窄水域内自主运行。

【技术实现步骤摘要】
基于NDT算法和Hybrid A＊算法的无人船路径规划方法


[0001]本专利技术属于水面无人系统领域，尤其涉及到一种基于NDT算法和Hybrid A*算法的无人船路径规划方法。

技术介绍

[0002]目前传感器性能不断提高，人工智能技术发展迅速，相关的应用逐渐场景从常见的地面，扩展到了空中和水面，其中无人船是典型的无人水面设备，可用于河道清理、水质检测、自主巡逻等场景。实现无人船在水域的自主执行航行任务，关键在于精确的无人船位置信息获取和水面的路径规划技术。
[0003]室外的无人系统一般会采用GPS作为获取定位信息的方案。在卫星信号良好的情况下，GPS的一般定位精度在1米至5米，若采用RTK
‑
GPS，则精度进一步提升，可以到达厘米级精度。但是GPS的定位方案存在一些缺点，最典型的就是GPS信号存在多路径效应，设备周边存在较多障碍物或是遮挡时，定位精度急剧下降，使得后续自主导航无法正常工作。激光雷达是是一种使用激光探测位置、形状和速度等几何特征的设备。目前激光雷达技术不断发展，价格也逐渐下降，采用激光雷达作为无人船的环境感知传感器，凭借激光雷达探测精度高、探测范围大的优点，通过点云配准算法，基于激光雷达的定位方法也能达到厘米级的定位效果。
[0004]当前已经有许多方法来解决的路径规划问题，比如蚁群算法、A*算法和RRT算法等。这些算法不会考虑规划执行的系统特性，使得这些算法生成的路径无法被无人船较好的跟踪执行。Hybrid A*算法是一种在2010年提出,基于A*算法与状态栅格搜索算法的改进算法，并成功应用于阿克曼模型的车辆路径规划中。传统的A*算法可以快速的找出栅格地图中从起点到达终点的最短路径序列，但是获取的路径序列难以被无人船跟踪执行，导致无人船无法在复杂水域运行。状态栅格搜索算法考虑无人系统的运动学模型，按照指定的离散时间段与转向角度，前向模拟可以到达的位姿，生成位姿网格图，因此通过状态栅格搜索算法获得的路径序列可以被较好的跟踪执行。但是由于位姿网格图的中的状态并不存在目的性，可能会与终点存在较大的误差，同时在线生成位姿网格图时间代价较大。Hybrid A*算法结合了这两种算法的特点，在栅格地图搜索的同时，采用无人船的运动模型搜索可行位姿，能够解决规划路径无法较好的跟踪执行的问题，但是算法计算量还是较大，并且在规划成功率较低，无法达到实时规划的效果。

技术实现思路

[0005]本专利技术的一个目的是提供一种基于NDT算法和Hybrid A*算法的无人船路径规划方法，并提供至少后面将说明的优点。
[0006]本专利技术的另一个目的是针对无人船工作环境的特点，提供一种基于NDT算法和Hybrid A*算法的无人船路径规划方法。自主定位采用基于NDT的激光雷达点云配准算法，并根据水面环境进行优化改进，提高定位稳定性。路径规划中对传统的Hybrid A*算法进行
改进优化，减少算法资源占用与提高路径规划成功率。同时，充分考虑无人船的运动特性并加入路径平滑，使得无人船能在复杂狭窄水域内自主运行。
[0007]本专利技术的技术方案如下：
[0008]基于NDT算法和Hybrid A*算法的无人船路径规划方法，其包括以下步骤：
[0009]定义坐标系，并确定激光雷达点云映射关系；获取水域环境激光点云作为参考点云，并将所述参考点云进行栅格化处理，转化为二维栅格地图，并生成障碍物距离地图；
[0010]构建每个栅格单元内参考点云分布的概率密度函数；获取当前扫描点云，并构造目标函数；
[0011]通过高斯牛顿迭代法优化所述目标函数，获得当前扫描点云在参考点云中的最佳变换关系，即获得无人船初始位姿；
[0012]构建船体碰撞模型，并确定终点位置；
[0013]根据无人船初始位姿、终点位置、障碍物距离地图和船体碰撞模型，采用Hybrid A*算法获取规划路径序列；
[0014]对所述规划路径序列进行平滑优化处理，获得最优路径。
[0015]优选的是，所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法中，
[0016]定义无人船坐标系o
b
x
b
y
b
z
b
，记为{B}，船体坐标系的原点o
b
设置在无人船的浮心；坐标系采用右手系，轴x
b
船体轴线指向前方，轴y
b
在水平面内垂直于轴x
b
，指向船体左侧，轴z
b
垂直于水平面，指向上方；
[0017]定义激光雷达坐标系o
l
x
l
y
l
z
l
，记为{L}，激光雷达的原点设置在雷达的中心，坐标系采用右手系，并且各坐标轴与船体坐标系{B}相同；
[0018]激光雷达安装在无人船顶部。
[0019]优选的是，所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法中，
[0020]通过点云降采样与离群点去除方法，对参考点云预处理；
[0021]在确保感知精度的情况下，根据船体运动特征确定栅格大小，将点云分配到对应的栅格中，同时对栅格中数据过少的单元进行剔除。
[0022]优选的是，所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法中，
[0023]参考点云在栅格内的分布遵循正态分布，则计算对应栅格内点云数据的均值与协方差，近似表示环境表面信息，具体公式如下：
[0024][0025][0026][0027]其中表示一个栅格内所有点云数据。
[0028]优选的是，所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法中，
[0029]构造优化目标函数，设定当前扫描点云为X＝x1,x2,
…
,x
n
，采用表示经过
位姿变换后的结果，目标函数设置为最大似然函数；
[0030][0031]为加快计算以及提高运算的稳定性，对目标函数进行调整，
[0032]d1＝
‑
log(c1+c2)
‑
d3[0033][0034][0035][0036]其中c1,c2为常数，可以通过要求p(x)的概率密度在一个栅格跨越的空间内等于1来确定；
[0037]当∑接近奇异值时，进行调整，如果∑中最大特征值λ3比λ1，λ2大100倍时，令λ1＝λ2＝λ3/100，并重新计算∑，公式如下：
[0038]∑＝V^
’
V
[0039][0040]其中V包含∑的特征向量。
[0041]优选的是，所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法中，
[0042]将船体外形近似为矩形，根据无人船的实际尺寸，并采用船体坐标系B，记录船体矩形的四个顶点坐标值；
[0043]预计算无人船在各离散朝向角度下的外形定本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于NDT算法和Hybrid A*算法的无人船路径规划方法，其特征在于，包括以下步骤：定义坐标系，并确定激光雷达点云映射关系；获取水域环境激光点云作为参考点云，并将所述参考点云进行栅格化处理，转化为二维栅格地图，并生成障碍物距离地图；构建每个栅格单元内参考点云分布的概率密度函数；获取当前扫描点云，并构造目标函数；通过高斯牛顿迭代法优化所述目标函数，获得当前扫描点云在参考点云中的最佳变换关系，即获得无人船初始位姿；构建船体碰撞模型，并确定终点位置；根据无人船初始位姿、终点位置、障碍物距离地图和船体碰撞模型，采用Hybrid A*算法获取规划路径序列；对所述规划路径序列进行平滑优化处理，获得最优路径。2.如权利要求1所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法，其特征在于，定义无人船坐标系o
b
x
b
y
b
z
b
，记为{B}，船体坐标系的原点o
b
设置在无人船的浮心；坐标系采用右手系，轴x
b
船体轴线指向前方，轴y
b
在水平面内垂直于轴x
b
，指向船体左侧，轴z
b
垂直于水平面，指向上方；定义激光雷达坐标系o
l
x
l
y
l
z
l
，记为{L}，激光雷达的原点设置在雷达的中心，坐标系采用右手系，并且各坐标轴与船体坐标系{B}相同；激光雷达安装在无人船顶部。3.如权利要求2所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法，其特征在于，通过点云降采样与离群点去除方法，对参考点云预处理；在确保感知精度的情况下，根据船体运动特征确定栅格大小，将点云分配到对应的栅格中，同时对栅格中数据过少的单元进行剔除。4.如权利要求3所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法，其特征在于，参考点云在栅格内的分布遵循正态分布，则计算对应栅格内点云数据的均值与协方差，近似表示环境表面信息，具体公式如下：差，近似表示环境表面信息，具体公式如下：差，近似表示环境表面信息，具体公式如下：其中表示一个栅格内所有点云数据。5.如权利要求4所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法，其特征在于，
构造优化目标函数，设定当前扫描点云为X＝x1,x2,
…
,x
n
，采用表示经过位姿变换后的结果，目标函数设置为最大似然函数；为加快计算以及提高运算的稳定性，对目标函数进行调整，d1＝
‑
log(c1+c2)
‑
d
333
其中c1,c2为常数，可以通过要求p(x)的概率密度在一个栅格跨越的空间内等于1来确定；当∑接近奇异值时，进行调整，如果∑中最大特征值λ3比λ1，λ2大100倍时，令λ1＝λ2＝λ3/100，并重新计算Σ，公式如下：∑＝V^
’
V其中V包含∑的特征向量。6.如权利要求5所述的基于NDT算法和Hybrid A*算法的无人船路径规划方法，其特征在于，将船体外形近似为矩形，根据无人船的实际尺寸，并采用船体坐标系B，记录船体矩形的四个顶点坐标值；预计算无人船在各...

【专利技术属性】
技术研发人员：刘勇，黄涛，陈哲，薛振锋，徐晋宏，
申请(专利权)人：浙江大学湖州研究院，
类型：发明
国别省市：