CN105843222B - 一种六轮/腿机器人复合运动路径规划方法 - Google Patents

Abstract

本发明公开一种六轮/腿机器人复合运动路径规划方法，首先建立机器人工作环境的DEM图，并进行地貌特征提取；采用地貌特征与评价指标之间的映射，遍历DEM图中每个栅格，得到机器人的通过性地图及腿式、轮式运动代价图；然后在代价图上，以机器人出发点为随机树的根节点，采用单纯腿式运动，以标准随机树算法进行扩展，逐渐增加叶节点直至随机树的叶节点中包含了终点，得到从出发点到终点之间的一条单纯腿式运动目标路径，并估计该路径的代价，作为初始代价迭代求解，至所用的时间超过用户设定的最大时限。本发明为轮/腿机器人提供了一种混合运动规划的解决方法，运动规划结果可以显著提高机器人的通过性及平均运行速度。

Description

一种六轮/腿机器人复合运动路径规划方法

技术领域

本发明属于机器人技术领域，涉及一种具有多功能轮/腿机器人复合运动路径规划方法

背景技术

腿式机器人可通过泥泞路面、崎岖山地等复杂环境，具有较强的环境适应能力，但是具有速度慢，能耗高，控制困难等缺点。轮式机器人可以快速，高效地通过平坦路面，但是其越障能力较差。轮/腿复合运动模式机器人旨在通过结合轮式、腿式运动各自的优点，在不同环境中应用不同的运动模式，实现发挥轮式的高速机动同时又具有良好的越障能力。轮/腿复合机器人为移动机器人的发展提供了新的方向，在探测，救灾，军事侦查，星球探测等方面具有广阔的应用前景。

针对轮/腿复合移动机器人，目前开展了许多轮/腿复合运动机构的设计研究，如申请号为201410025261.9的发明专利一种多功能腿轮复合机器人及其多运动模式智能切换方法,提供了一种新型的轮/腿复合运动机构，并提供了多种运动模式之间的切换方法，为轮/腿复合运动的实现提供了基础。但是对轮/腿复合运动技术的核心，即决策在环境中何处采用轮式运动，何处采用腿式运动以及如何通过轮式、腿式运动组合实现快速、低耗地运动到目标地点，缺乏有效的解决方法，这严重地制约了轮/腿复合运动机器人的发展和应用。因此，设计一种高效的轮/腿复合运动路径规划方法是当前急需要解决的问题。

发明内容

本发明为实现轮/腿机器人在复杂环境中快速、稳定的运动，提供了一种面向不确定环境，最大限度发挥轮/腿混合运动优势的路径规划方法。基于对环境的感知建模，对轮、腿各自运动特点的总结分析，能够迅速在复杂环境中找出满意的最佳混合运动路径。

1、本发明一种六轮/腿机器人复合运动路径规划方法，其特征在于：通过下述步骤完成：

步骤1：探测并确定机器人工作环境信息，建立其工作环境的DEM图。

步骤2：对步骤1中建立的DEM图进行特征提取，包括：坡度、台阶类障碍高度与粗糙度三种地貌特征；采用三种地貌特征与评价指标之间的映射，遍历的DEM图中每个栅格，得到机器人的通过性地图及腿式、轮式运动代价图，图中标记有机器人轮式及腿式运动都可以通过的栅格，轮式及腿式运动都不能通过的栅格，以及只有腿式运动可以通过的栅格。

步骤3：规划获得机器人最佳混合路径。

A、在步骤2中得到的腿式运动代价图上，以机器人出发点q_start为随机树的根节点，机器人采用单纯腿式运动，以标准随机树算法进行扩展，逐渐增加叶节点直至随机树的叶节点中包含了终点q_goal，得到从q_start到q_goal之间的一条单纯腿式运动目标路径T{1}。

B、估计目标路径T{1}的代价T_c。

C、以T_c为初始代价,进行迭代求解，至所用的时间超过用户设定的最大时限；每次迭代中寻找到的混合运动路径代价需小于(1-ε)T_c，其中ε为迭代因子。

本发明的优点为:

1、本发明六轮/腿机器人复合运动路径规划方法，为轮/腿机器人提供了一种混合运动规划的解决方法，运动规划结果可以显著提高机器人的通过性及平均运行速度。

2、本发明六轮/腿机器人复合运动路径规划方法，应用范围广，该运动规划方法采用参数化建模思想，适用于各种具有轮腿两种运动形式的机器人。

3、本发明六轮/腿机器人复合运动路径规划方法，运行速度快，该运动规划方法，采用改进型rrt算法，显著的提高了运行速度。

附图说明

图1本发明六轮/腿机器人复合运动路径规划方法流程图；

图2为本发明六轮/腿机器人复合运动路径规划方法中机器人工作环境的DEM图；

图3为改进型RRT寻找混合路径方法中已经生成的部分随机树示意图；

图4为改进型RRT寻找混合路径方法中随机树的子路径拓展示意图；

图5为改进型RRT寻找混合路径方法中随机树的子路径优化示意图。

具体实施方式

下面结合附图对本发明作进一步详细说明。

本发明六轮/腿机器人复合运动路径规划方法，如图1所示，通过下述步骤实现：

步骤1：探测并确定机器人工作环境信息，建立其工作环境的DEM(数字高程)图；

根据机器人的直径及运动状态，利用感知系统探测机器人周围一定区域的地貌特征，包括机器人周围障碍物的位置形状与地表纹理特征，并建立机器人工作环境的DEM(数字高程)图。

步骤2：生成相应的栅格化轮/腿运动代价图；

a、对步骤1中建立的DEM图进行特征提取，包括：坡度、台阶类障碍高度与粗糙度三种地貌特征。

如图1所示，令机器人所处的世界坐标系为∑_G，机器人本体中心在世界坐标系中的坐标为Z_i,j，机器人本体中心在X-Y平面内的投影处于栅格cell(i,j)内,其中i,j为栅格坐标。机器人本体在X-Y平面内投影的最小包络矩形为R(i,j),在R(i,j)内每个栅格提取上述三种地形特征：

①坡度是影响机器人运动的主要特征，当机器人采用腿式运动形式爬坡或者采用轮式通过坑洼地形时，机器人倾覆的风险会随着坡度的增大而急剧增加。目前有许多方法提取坡度特征，本发明中采用R(i,j)内的平均倾角作为坡度特征，表示为：

其中，Θ_x表示沿x轴方向的平均倾角，Θ_y表示沿y方向的平均倾角。

②粗糙度表征了机器人运动环境表面的崎岖及粗糙程度，是影响轮式运动的主要因素。当粗糙度过大时，机器人采用腿式行走也变得困难。本发明中粗糙度采用R(i,j)区域内高程的标准差进行估计，表示为：

其中，表示R(i,j)区域内的高程平均值，n为R(i,j)区域内栅格的个数。

③台阶类障碍高度；

台阶类障碍高度定义为投影区域内的最大绝对高程差，主要用来表征台阶、峡谷与峭壁等地形特征。对于这些地形特征，坡度指标变化不大，而绝对高度的变化超出了机器人的移动能力。本发明中台阶类障碍高度表示为：

E_i,j＝z_max(R_i,j)-z_min(R_i,j) (3)

其中，z_max和z_min为R(i,j)内最高和最低高程值。

b、生成轮/腿运动形式的代价图；

根据步骤a提取的地形特征，结合机器人轮式运动与腿式运动的特点，分别计算机器人在R(i,j)内每个栅格的轮/腿运动形式的评价指标，包括轮运动形式的评价指标与腿运动形式的评价指标该指标既描述了机器人的通过性，也描述了机器人采用不同运动形式通过区域R(i,j)时所消耗的代价，具体的包括能耗、时间等指标；评价指标C_ij具有如下形式[Trc,Effc]，其中，Trc为通过性指标，Trc＝0表示可通过区域R(i,j)；Trc＝0表示不可通过区域R(i,j)；Effc为代价评价指标，描述通过区域R(i,j)的代价，Effc＝(0，1)，为0到1之间的连续值，数值越高，代价越高。

通过BP人工神经网络，建立每个栅格内提取的各个地形特征Θ_i,j，B_i,j，E_i,j和评价指标 之间的映射。建立三层BP人工神经网络，并通过ADAMS仿真环境及实验中获得的不同地形中机器人能耗，时间数据对神经网络进行训练，模拟地形特征与评价指标之间的映射。采用上述模拟地形特征与评价指标之间的映射，遍历步骤1中建立的DEM图中每个栅格，得到机器人的通过性地图及腿式、轮式运动代价图，图中标记有机器人轮式及腿式运动都可以通过的栅格，轮式及腿式运动都不能通过的栅格(障碍物区域)，以及只有腿式运动可以通过的栅格。

步骤3：规划获得机器人最佳混合路径。

本发明中采用对快速随机树法进行改进，来规划获得机器人的最佳混合路径，具体方式为：

A、在步骤2中得到的腿式运动代价图上，以机器人出发点q_start为随机树的根节点，机器人采用单纯腿式运动，以标准随机树算法进行扩展，逐渐增加叶节点直至随机树的叶节点中包含了终点q_goal，得到从q_start到q_goal之间的一条单纯腿式运动目标路径T{1}。

B、估计目标路径T{1}的代价；

在腿式运动代价图中，路径上每个栅格的评价指标为则目标路径T{1}的代价T_c为：

C、利用改进型RRT寻找轮腿混合运动路径；

以T_c为初始代价,进行迭代求解，至所用的时间超过用户设定的最大时限。每次迭代中寻找到的混合运动路径代价需小于(1-ε)T_c，其中ε为迭代因子；ε决定每次迭代过程路径的优化程度，如果ε很大，则要求每次运动代价减小很多。

每一次迭代步骤如下：

Ⅰ、假设机器人周围的通过性地图，如图3、图4与图5所示，其中，图3为通过性地图中已生成的部分随机树，起始点为q_start，终点为q_goal，由操作者设定；圆点代表随机树的节点，其中实心点表示采用腿式运动通过的点，空心点表示采用轮式运动通过的点。节点之间的连线表示机器人的运动路径，若一段路径的两端节点分别为实心和空心点，或两端节点均为实心点，则机器人采用腿式运动模式通过该段路径；若一段路径的两端节点均为空心点，则机器人采用轮式运动模式通过该段路径。图中A区表示轮式运动和腿式运动都不能通过的障碍物区域，B区表示只有腿式运动可以通过，C区表示轮式及腿式运动都可以通过的区域。

以起始点q_start点为随机树的起始点，初始化q_new＝q_start，q_new代表通过最新一次拓展获得的可以加入随机树的节点。

Ⅱ、若q_new与q_goal之间的距离小于设定值阈值dis_threshold,则进行步骤Ⅵ，否则进行步骤Ⅲ。

Ⅲ、在已建立的通过性地图上选取一个目标点q_target，选取方法如下：

在区间[0,1]内生成一个随机数p，且p服从均匀分布，若p<p_g,p_g为设定值，则q_target＝q_goal,否则q_target从通过性地图上的机器人轮式及腿式运动都可以通过的区域内随机选取，且要求选取的q_target满足:

h(q_start,q_target)+h(q_target,q_goal)<Tc

其中，h为定义的代价启发函数：

其中，q₁,q₂分别为起始和目标点变量，line(q₁,q₂)为连接q₁,q₂的线段上所有栅格点的集合。

h(q₁,q₂)提供了连接q₁,q₂路径的最小能耗估计，若：

h(q_start,q_target)+h(q_target,q_goal)>Tc

则添加q_target后，整个路径的最小能耗估计超过能耗限制Tc，因此q_target不能作为目标点。通过上述目标点选取方法，大大的减小了目标点的选取范围，同时保证了目标点选取方法的探索性，加快了随机树的建立过程。

Ⅳ、如图4所示，在已生成的随机树上选取距离q_target依次最近的3个节点q_{near_1},q_{near_2},q_{near_3},并通过以下方法，从选取的3个节点扩展3条子路径到q_target，每条子路径为从q_{near_k}向q_target生长的直线段，k为自路径编号，k＝1,2,3；并估计这三条子路径的可行性：

1)若子路径经过机器人轮式及腿式运动不可以通过的栅格，则子路径扩展失败，进行其它子路径的扩展；

2)子路径从q_{near_k}向q_target生长时，遇到轮式及腿式运动都可以通过的栅格时，采用轮式运动方式通过。

3)子路径从q_{near_k}向q_target生长时，遇到只有腿式运动可以通过的栅格时，机器人转化为腿式运动模式，采用腿式通过。

4)形成的子路径中，若存在一个轮式及腿式运动都可以通过的栅格时，相邻的两个栅格均为机器人轮式及腿式运动不可以通过的栅格时，则将这个轮式及腿式运动都可以通过的栅格的通过方式改为机器人轮式及腿式运动不可以通过的栅格，如图5所示。

上述生成的子路径可行性的判断方法为：

Cost(q_start,q_near)+Cost(q_near,q_target)+h(q_target,q_goal)<Tc

其中，函数为连接a,b的子路径的代价总和，即a,b的子路径上每个栅格的轮/腿运动形式的评价指标其中node为自路径上栅格编号，mode代表w,l，postion为连接a,b的子路径上的节点的累积。

Ⅴ、如3条子路径均满足上述代价要求，则三条子路径可行，若3条子路径中，有一条及以上的路径可行，则随机选取一条子路径加入到现有随机树中，并令q_new＝q_target，返回步骤Ⅱ。若3条子路径均扩展不成功，则在每个子路径上重新获取目标点，

λ＝λ-0.2，初始时，令λ＝0.8，获取每个子路径新目标点后，重复步骤Ⅳ进行扩展；若λ＝0时，则整个子扩展过程失败，返回步骤Ⅲ，否则返回步骤Ⅴ继续进行子扩展。

Ⅵ、返回形成的RRT,获得从起始点q_start到目标点q_goal的路径，并估计该路径的代价,令其等于Tc，至此完成一次路径规划迭代子过程。

Claims (4)

1.本发明一种六轮/腿机器人复合运动路径规划方法，其特征在于：通过下述步骤完成：

步骤1：探测并确定机器人工作环境信息，建立其工作环境的DEM图；

步骤2：对步骤1中建立的DEM图进行特征提取，包括：坡度、台阶类障碍高度与粗糙度三种地貌特征；采用三种地貌特征与评价指标之间的映射，遍历DEM图中每个栅格，得到机器人的通过性地图及腿式、轮式运动代价图，图中标记有机器人轮式及腿式运动都可以通过的栅格，轮式及腿式运动都不能通过的栅格，以及只有腿式运动可以通过的栅格；

步骤3：规划获得机器人最佳混合路径；

A、在步骤2中得到的腿式运动代价图上，以机器人出发点q_start为随机树的根节点，机器人采用单纯腿式运动，以标准随机树算法进行扩展，逐渐增加叶节点直至随机树的叶节点中包含了终点q_goal，得到从q_start到q_goal之间的一条单纯腿式运动目标路径T{1}；

B、估计目标路径T{1}的代价T_c；

C、以T_c为初始代价，进行迭代求解，至所用的时间超过用户设定的最大时限；每次迭代中寻找到的混合运动路径代价需小于(1-ε)T_c，其中ε为迭代因子；

每一次迭代步骤如下：

Ⅰ、在机器人周围的通过性地图中已生成的部分随机树，设定起始点q_start，终点为q_goal点，由操作者设定；以点代表随机树的节点，其中，实心点表示采用腿式运动通过的点，空心点表示采用轮式运动通过的点；节点之间的连线表示机器人的运动路径，若一段路径的两端节点分别为实心和空心点，或两端节点均为实心点，则机器人采用腿式运动模式通过该段路径；若一段路径的两端节点均为空心点，则机器人采用轮式运动模式通过该段路径；以起始点q_start点为随机树的起始点，初始化q_new＝q_start，q_new代表通过最新一次拓展获得的可以加入随机树的节点；

Ⅱ、若q_new与q_goal之间的距离小于设定值阈值dis_threshold，则进行步骤Ⅵ，否则进行步骤Ⅲ；

Ⅲ、在已建立的通过性地图上选取一个目标点q_target，选取方法如下：

在区间[0，1]内生成一个随机数p，且p服从均匀分布，若p<p_g，p_g为设定值，则q_target＝q_goal，否则q_target从通过性地图上的机器人轮式及腿式运动都可以通过的区域内随机选取，且要求选取的q_target满足:

h(q_start，q_target)+h(q_target，q_goal)<Tc

其中，h为定义的代价启发函数：

其中，q₁，q₂分别为起始和目标点变量，line(q₁，q₂)为连接q₁，q₂的线段上所有栅格点的集合；

Ⅳ、在已生成的随机树上选取距离q_target依次最近的3个节点q_{near_1}，q_{near_2}，q_{near_3}，并通过以下方法，从选取的3个节点扩展3条子路径到q_target，每条子路径为从q_{near_k}向q_target生长的直线段，k为自路径编号，k＝1，2，3；并估计这三条子路径的可行性：

1)若子路径经过机器人轮式及腿式运动不可以通过的栅格，则子路径扩展失败，进行其它子路径的扩展；

2)子路径从q_{near_k}向q_target生长时，遇到轮式及腿式运动都可以通过的栅格时，采用轮式运动方式通过；

3)子路径从q_{near_k}向q_target生长时，遇到只有腿式运动可以通过的栅格时，机器人转化为腿式运动模式，采用腿式通过；

4)形成的子路径中，若存在一个轮式及腿式运动都可以通过的栅格时，相邻的两个栅格均为机器人轮式及腿式运动不可以通过的栅格时，则将这个轮式及腿式运动都可以通过的栅格的通过方式改为机器人轮式及腿式运动不可以通过的栅格；

生成的子路径可行性的判断方法为：

Cost(q_start，q_near)+Cost(q_near，q_target)+h(q_target，q_goal)<Tc

其中，函数为连接a，b的子路径的代价总和，即a，b的子路径上每个栅格的轮/腿运动形式的评价指标其中node为自路径上栅格编号，mode代表w，l，postion为连接a，b的子路径上的节点的累积；

Ⅴ、三条子路径可行，若3条子路径中，有一条及以上的路径可行，则随机选取一条子路径加入到现有随机树中，并令q_new＝q_target，返回步骤Ⅱ；若3条子路径均扩展不成功，则在每个子路径上重新获取目标点，

λ＝λ-0.2，初始时，令λ＝0.8，获取每个子路径新目标点后，重复步骤Ⅳ进行扩展；若λ＝0时，则整个子扩展过程失败，返回步骤Ⅲ，否则返回步骤Ⅴ继续进行子扩展；

Ⅵ、返回形成的RRT，获得从起始点q_start到目标点q_goal的路径，并估计该路径的代价，令其等于Tc，至此完成一次路径规划迭代子过程。

2.如权利要求1所述一种六轮/腿机器人复合运动路径规划方法，其特征在于：步骤2中，坡度、台阶类障碍高度与粗糙度三种地貌特征提取方式为：

①坡度：令i，j为栅格坐标，机器人本体在X-Y平面内投影的最小包络矩形为R(i，j)，将R(i，j)内的平均倾角作为坡度特征，坡度特征表示为：

其中，Θ_x表示沿x轴方向的平均倾角，Θ_y表示沿y方向的平均倾角；

②粗糙度：采用R(i，j)区域内高程的标准差进行估计，粗糙度特征表示为：

其中，表示R(i，j)区域内的高程平均值，n为R(i，j)区域内栅格的个数；

③台阶类障碍高度特征表示为：

E_i，j＝z_max(R_i，j)-z_min(R_i，j)

其中，z_max和z_min为R(i，j)内最高和最低高程值；

3.如权利要求1所述一种六轮/腿机器人复合运动路径规划方法，其特征在于：步骤2的b中生成轮/腿运动形式的代价图的具体方式为:根据地形特征，结合机器人轮式运动与腿式运动的特点，分别计算机器人在R(i，j)内每个栅格的轮/腿运动形式的评价指标，包括轮运动形式的评价指标与腿运动形式的评价指标通过BP人工神经网络，建立每个栅格内提取的各个地形特征Θ_i，j，B_i，j，E_i，j和评价指标之间的映射；建立三层BP人工神经网络，并通过ADAMS仿真环境及实验中获得的不同地形中机器人能耗，时间数据对神经网络进行训练，模拟地形特征与评价指标之间的映射；采用上述模拟地形特征与评价指标之间的映射，遍历步骤1中建立的DEM图中每个栅格，得到机器人的通过性地图及腿式、轮式运动代价图，图中标记有机器人轮式及腿式运动都可以通过的栅格，轮式及腿式运动都不能通过的栅格，以及只有腿式运动可以通过的栅格；其中轮式及腿式运动都不能通过的栅格为障碍物区域。

4.如权利要求3所述一种六轮/腿机器人复合运动路径规划方法，其特征在于：评价指标C_ij的形式为[Trc，Effc]，其中，Trc为通过性指标，Trc＝0表示可通过区域R(i，j)；Trc＝1表示不可通过区域R(i，j)；Effc为代价评价指标，描述通过区域R(i，j)的代价，Effc＝(0，1)，为0到1之间的连续值。