具有复杂地形可通过性判断的移动机器人路径规划方法与流程

1.本发明涉及路径规划技术领域，具体涉及一种具有复杂地形通过性判断的移动机器人路径规划方法。


背景技术：

2.环境感知、路径规划、运动控制是自主移动机器人的三大主要技术，其中路径规划作为承上启下的关键步骤起着重要作用。现主流的路径规划算法，如基于搜索的a*及其变体、基于采样的rrt及其变体等算法，其主要目的在于如何有效避障，但是在野外、矿区、灾后等非结构化环境下作业机器人的路径规划，如果单纯考虑避障，会导致无法生成最优路径或者降低机器人的工作效率。因此，在复杂环境下的路径规划中加入障碍物的可通过性判断无疑是有必要的。
3.公布号为cn 103869820 a的发明专利结合巡视器本身的几何结构参数以及其本身的越障性能指标，从高度、坡度和粗糙度等地形几何特征方面进行地形的可过性判断。但是，单纯的基于地形几何特征地形通过性的判断依赖于前端传感器对于环境复现的准确性，而且这种方法忽略了诸如地面摩擦、地形软硬等重要特征。
4.公布号为cn 111413962 a的发明专利从概率的角度出发，建立了路径节点间可通过性概率评估模型，再依据局部环境的通路可通过性进行节点间的路径规划。但是，节点间可通过性概率是通过无人机拍摄分析或者人工经验值分析得出，一方面复杂地形或者场景下对无人机的工作性能提出了更高的要求，另一方面依赖人工经验也容易造成较大的误差。


技术实现要素：

5.为了解决上述现有技术的不足，本发明提出了一种具有复杂地形通过性判断的移动机器人路径规划方法，能够利用混合高斯来获得地形属性和机器人移动代价之间的关系，移动机器人可以通过收集相关地形可通过性的参考样本进行地形可通过性的评估。
6.具体地，本发明提供一种具有复杂地形通过性判断的移动机器人路径规划方法，其包括以下步骤：
7.s1、为保证约束条件，定义三个约束参数c1、c2、c3，并得到最终约束参数，具体包括以下子步骤：
8.s11、定义c1，c1表示相邻两结点之间的欧氏距离，c1越大则代表节点之间的转移越困难，
[0009][0010]
其中，(x
m
,y
m
,z
m
)和(x
n
,y
n
,z
n
)分别表示相邻节点的坐标值，
[0011]
s12、定义c2，c2用球面方差来定义，将地图栅格转换成三角网格，计算每个三角网格的法向量并求出法向量的模长，c2越大则代表路面的粗糙度越大，机器人
的移动代价就越大；
[0012][0013][0014]
其中，r表示法向量的模长，n表示三角网格的个数；x
i
，y
i
，z
i
分别为每个三角网格法向量的三个分量；
[0015]
s13、定义c3，c3对基于上述约束规划的路径是否合理进行判断，
[0016][0017]
s14、计算最终约束参数，最终约束参数c
final
的表达式如下：
[0018][0019]
s2、对地形可通过性进行评估，其具体包括以下步骤：
[0020]
s21、采用机器人在以往实验中的移动数据获得参考地图，预测机器人在未知复杂地形下的可通过性：基于回归算法的可通过性评估，采用核密度估计的概率框架，机器人根据参考样本的可通过性特性，根据这些数据获得复杂地形的可通过性评估：
[0021]
定义基于复杂地形下的概率密度函数：p(p
t
|c1,c2,c3)，即在c1、c2、c3三个参数所定义的条件下，得到两个节点间的可转移概率p
t
；
[0022]
s22、两个节点间的可转移概率p
t
根据联合概率密度和边缘概率密度计算得到，其中：
[0023]
联合概率密度的计算公式如下：
[0024][0025]
其中，c＝[c1,c2,c3]，p
t
表示两个节点之间的转移概率，c
i
、p
ti
分别表示参考样本的c和t，k
h1
和k
h2
表示高斯核，n表示样本数量；
[0026]
边缘概率密度的计算公式如下：
[0027][0028]
得到条件概率密度的计算公式如下：
[0029][0030]
最终节点间的转移概率p
t
依据均值漂移算法得出：
[0031][0032]
s23、将上述计算出来的概率值离散化，根据最终结果值对地形可通过性进行评
估；
[0033]
s3、根据步骤s2得到的地形可通过性评估结果，基于a*算法，将路径点分为起点、多个中间点和终点，定义路径的代价函数为：
[0034]
f(q)＝g(q) h(q) (1)
[0035]
其中，h(q)表示预测的中间点到终点的代价，g(q)表示起点到中间点的代价。
[0036]
优选地，步骤s23中，当最终结果值为0
‑
0.5时，定义为不可穿越障碍，当最终结果值为0.5
‑
1时，定义为可穿越障碍。
[0037]
优选地，步骤s12中n的取值为8。
[0038]
优选地，h(q)采用欧式距离来度量，即：
[0039][0040]
其中，(x
m
,y
m
,z
m
)和(x
s
,y
s
,z
s
)分别表示中间点和起点位置；
[0041]
g(q)由s1中所述的三个约束参数进行定义，即：
[0042][0043]
p
t
是步骤s2得到的地形可通过性评估结果。
[0044]
与现有技术相比，本发明具有以下有益效果：
[0045]
(1)本发明将可通过性判断加入至路径规划中，基于a*算法进行路径搜索，并且在搜索过程中考虑到了复杂地形的特征和机器人的运动特性，得到复杂地形的可通过判断。
[0046]
(2)本发明为克服a*算法的路径搜索过于保守的缺点，在搜索过程中加入了基于核密度估计的可通过性评估，该方法很好地解决了复杂地形下无法找到最优路径或者只找到次优路径的问题，有效提高了机器人的作业效率。
附图说明
[0047]
图1为本发明的流程示意图。
具体实施方式
[0048]
以下将参考附图详细说明本发明的示例性实施例、特征和方面。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
[0049]
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
[0050]
本发明提出了一种加入复杂地形可通过性判断的移动性机器人路径规划方法，该方法首先基于a*的搜索策略进行复杂地形下的路径规划，然后加入了基于核密度估计的可通过性评估，该方法主要包括以下两个方面的内容：
[0051]
(1)基于a*的路径规划：在路径搜索过程中，加入了3个参数考虑了地形的一些几
何特性以及机器人的运动特征。
[0052]
(2)基于核密度估计的复杂地形可通过性判断：根据机器人以往的移动数据，预测机器人在复杂地形下的可通过性。采用了核密度估计的概率框架来评估可通过性，并将最终的预测结果反馈至路径规划中。
[0053]
具体实施例
[0054]
本发明提供一种具有复杂地形通过性判断的移动机器人路径规划方法，其包括以下步骤：
[0055]
s1、为保证上述约束，定义三个参数c1、c2、c3，具体包括以下子步骤：
[0056]
s11、，定义c1，c1表示相邻两结点之间的欧氏距离，c1越大则代表节点之间的转移越困难，
[0057][0058]
其中，(x
m
,y
m
,z
m
)和(x
n
,y
n
,z
n
)分别表示相邻节点的坐标值，
[0059]
s12，定义c2，c2用球面方差来定义，将地图栅格转换成三角网格，计算每个三角网格的法向量并求出其模长，c2越大则代表路面的粗糙度越大，机器人的移动代价就越大；
[0060][0061][0062]
其中，r表示法向量的模长，n表示三角网格的个数；
[0063]
s13，定义c3，c3对基于上述约束规划的路径是否合理进行判断，
[0064][0065]
s14、计算最终约束参数，最终约束参数c
final
的表达式如下：
[0066][0067]
s2、对地形可通过性进行评估，其具体包括以下步骤：
[0068]
由于上述基于a*的启发式搜索策略过于保守，即使有些障碍物可穿越，该算法也可能将障碍物评估为不可穿越。为克服上述缺点，在本发明中，采用机器人在以往实验中的移动数据获得参考地图，并以此来预测机器人在未知复杂地形下的可通过性。在本发明中加入了基于回归算法的可通过性评估，采用了核密度估计的概率框架。机器人可以根据参考样本的可通过性特性，并根据这些数据获得复杂地形更一般的可通过性评估。定义基于此种情形下的概率密度函数：p(p
t
|c1,c2,c3)，即在c1、c2、c3三个参数所定义的条件下，找到两个节点间的可转移概率p
t
。为了计算上述条件概率，需要知道相应的联合概率密度和边缘概率密度。
[0069]
s22、两个节点间的可转移概率p
t
根据联合概率密度和边缘概率密度计算得到，其
中：
[0070]
联合概率密度的计算公式如下：
[0071][0072]
其中，c＝[c1,c2,c3]，p
t
表示两个节点之间的转移概率，c
i
、p
ti
分别表示参考样本的c和t，k
h1
和k
h2
表示高斯核，n表示样本数量；
[0073]
边缘概率密度的计算公式如下：
[0074][0075]
得到条件概率密度的计算公式如下：
[0076][0077]
最终节点间的转移概率p
t
依据均值漂移算法得出：
[0078][0079]
s23、将上述计算出来的概率值离散化，根据最终结果值对地形可通过性进行评估。
[0080]
本发明将可通过性判断加入至路径规划中，基于a*算法进行路径搜索，并且在搜索过程中考虑到了复杂地形的特征和机器人的运动特性。为克服a*算法的路径搜索过于保守的缺点，在搜索过程中加入了基于核密度估计的可通过性评估，所述方法很好地解决了复杂地形下无法找到最优路径或者只找到次优路径的问题，有效提高了机器人的作业效率。
[0081]
s3、基于a*算法，将路径点分为起点、多个中间点和终点，定义路径的代价函数为：
[0082]
f(q)＝g(q) h(q) (1)
[0083]
其中，h(q)表示预测的中间点到终点的代价，g(q)表示起点到中间点的代价，其计算考虑到了地形的特征以及机器人的实时运动学性质。h(q)在本发明采用欧式距离来度量，即：
[0084][0085]
其中，(x
m
,y
m
,z
m
)和(x
s
,y
s
,z
s
)分别表示中间点和起点位置。
[0086]
g(q)由s12步所述的三个参数定义，即：
[0087][0088]
c
final
的具体定义详见步骤s12
‑
s13所述。p
t
是本发明所述的可通过性的评估值，
[0089]
最后应说明的是：以上所述的各实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或全部技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。