一种六轮/腿机器人复合运动路径规划方法
【专利摘要】本发明公开一种六轮/腿机器人复合运动路径规划方法，首先建立机器人工作环境的DEM图，并进行地貌特征提取；采用地貌特征与评价指标之间的映射，遍历DEM图中每个栅格，得到机器人的通过性地图及腿式、轮式运动代价图；然后在代价图上，以机器人出发点为随机树的根节点，采用单纯腿式运动，以标准随机树算法进行扩展，逐渐增加叶节点直至随机树的叶节点中包含了终点，得到从出发点到终点之间的一条单纯腿式运动目标路径，并估计该路径的代价，作为初始代价迭代求解，至所用的时间超过用户设定的最大时限。本发明为轮/腿机器人提供了一种混合运动规划的解决方法，运动规划结果可以显著提高机器人的通过性及平均运行速度。
【专利说明】
一种六轮/腿机器人复合运动路径规划方法
技术领域
[0001] 本发明属于机器人技术领域，涉及一种具有多功能轮/腿机器人复合运动路径规 划方法
【背景技术】
[0002] 腿式机器人可通过泥泞路面、崎岖山地等复杂环境，具有较强的环境适应能力，但 是具有速度慢，能耗高，控制困难等缺点。轮式机器人可以快速，高效地通过平坦路面，但是 其越障能力较差。轮/腿复合运动模式机器人旨在通过结合轮式、腿式运动各自的优点，在 不同环境中应用不同的运动模式，实现发挥轮式的高速机动同时又具有良好的越障能力。 轮/腿复合机器人为移动机器人的发展提供了新的方向，在探测，救灾，军事侦查，星球探测 等方面具有广阔的应用前景。
[0003] 针对轮/腿复合移动机器人，目前开展了许多轮/腿复合运动机构的设计研究，如 申请号为201410025261.9的发明专利一种多功能腿轮复合机器人及其多运动模式智能切 换方法，提供了一种新型的轮/腿复合运动机构，并提供了多种运动模式之间的切换方法， 为轮/腿复合运动的实现提供了基础。但是对轮/腿复合运动技术的核心，即决策在环境中 何处采用轮式运动，何处采用腿式运动以及如何通过轮式、腿式运动组合实现快速、低耗地 运动到目标地点，缺乏有效的解决方法，这严重地制约了轮/腿复合运动机器人的发展和应 用。因此，设计一种高效的轮/腿复合运动路径规划方法是当前急需要解决的问题。
【发明内容】
[0004] 本发明为实现轮/腿机器人在复杂环境中快速、稳定的运动，提供了一种面向不确 定环境，最大限度发挥轮/腿混合运动优势的路径规划方法。基于对环境的感知建模，对轮、 腿各自运动特点的总结分析，能够迅速在复杂环境中找出满意的最佳混合运动路径。
[0005] 1、本发明一种六轮/腿机器人复合运动路径规划方法，其特征在于:通过下述步骤 完成：
[0006] 步骤1:探测并确定机器人工作环境信息，建立其工作环境的DEM图。
[0007] 步骤2:对步骤1中建立的DEM图进行特征提取，包括:坡度、台阶类障碍高度与粗糙 度三种地貌特征;采用三种地貌特征与评价指标之间的映射，遍历的DEM图中每个栅格，得 到机器人的通过性地图及腿式、轮式运动代价图，图中标记有机器人轮式及腿式运动都可 以通过的栅格，轮式及腿式运动都不能通过的栅格，以及只有腿式运动可以通过的栅格。
[0008] 步骤3:规划获得机器人最佳混合路径。
[0009] A、在步骤2中得到的腿式运动代价图上，以机器人出发点qstart为随机树的根节点， 机器人采用单纯腿式运动，以标准随机树算法进行扩展，逐渐增加叶节点直至随机树的叶 节点中包含了终点q g〇al，得到从qstart到qg〇al之间的一条单纯腿式运动目标路径T{1}。
[0010] B、估计目标路径T {1}的代价Tc。
[0011] C、以T。为初始代价，进行迭代求解，至所用的时间超过用户设定的最大时限;每次 迭代中寻找到的混合运动路径代价需小于(1-ε)Τ。，其中ε为迭代因子。
[0012]本发明的优点为：
[0013] 1、本发明六轮/腿机器人复合运动路径规划方法，为轮/腿机器人提供了一种混合 运动规划的解决方法，运动规划结果可以显著提高机器人的通过性及平均运行速度。
[0014] 2、本发明六轮/腿机器人复合运动路径规划方法，应用范围广，该运动规划方法采 用参数化建模思想，适用于各种具有轮腿两种运动形式的机器人。
[0015] 3、本发明六轮/腿机器人复合运动路径规划方法，运行速度快，该运动规划方法， 采用改进型rrt算法，显著的提高了运行速度。
【附图说明】
[0016] 图1本发明六轮/腿机器人复合运动路径规划方法流程图；
[0017] 图2为本发明六轮/腿机器人复合运动路径规划方法中机器人工作环境的DEM图；
[0018] 图3为改进型RRT寻找混合路径方法中已经生成的部分随机树示意图；
[0019]图4为改进型RRT寻找混合路径方法中随机树的子路径拓展示意图；
[0020]图5为改进型RRT寻找混合路径方法中随机树的子路径优化示意图。
【具体实施方式】
[0021]下面结合附图对本发明作进一步详细说明。
[0022]本发明六轮/腿机器人复合运动路径规划方法，如图1所示，通过下述步骤实现： [0023]步骤1:探测并确定机器人工作环境信息，建立其工作环境的DEM(数字高程)图；
[0024] 根据机器人的直径及运动状态，利用感知系统探测机器人周围一定区域的地貌特 征，包括机器人周围障碍物的位置形状与地表纹理特征，并建立机器人工作环境的DEM(数 字高程)图。
[0025] 步骤2:生成相应的栅格化轮/腿运动代价图；
[0026] a、对步骤1中建立的DEM图进行特征提取，包括:坡度、台阶类障碍高度与粗糙度三 种地貌特征。
[0027] 如图1所示，令机器人所处的世界坐标系为Σ(；，机器人本体中心在世界坐标系中 的坐标为Z1^机器人本体中心在X-Y平面内的投影处于栅格cell(i，j)内，其中i，j为栅格 坐标。机器人本体在X-Y平面内投影的最小包络矩形为R(i，j)，在R(i，j)内每个栅格提取上 述三种地形特征：
[0028] ①坡度是影响机器人运动的主要特征，当机器人采用腿式运动形式爬坡或者采用 轮式通过坑洼地形时，机器人倾覆的风险会随着坡度的增大而急剧增加。目前有许多方法 提取坡度特征，本发明中采用R( i，j)内的平均倾角作为坡度特征，表示为：
[0029]
(1)
[0030]其中，θχ表示沿X轴方向的平均倾角，?y表示沿y方向的平均倾角。
[0031]②粗糙度表征了机器人运动环境表面的崎岖及粗糙程度，是影响轮式运动的主要 因素。当粗糙度过大时，机器人采用腿式行走也变得困难。本发明中粗糙度采用R(i，j)区域 内尚程的标准差进彳丁估计，表不为：
[0032]
C2)
[0033] 其中，表示R(i，j)区域内的高程平均值，η为R(i，j)区域内栅格的个数。 [0034]③台阶类障碍高度；
[0035] 台阶类障碍高度定义为投影区域内的最大绝对高程差，主要用来表征台阶、峡谷 与峭壁等地形特征。对于这些地形特征，坡度指标变化不大，而绝对高度的变化超出了机器 人的移动能力。本发明中台阶类障碍高度表示为：
[0036] Eij = Zmax(Rij)-Zmin(Rij) (3)
[0037]其中，Zmax和Zmin为R(i，j)内最高和最低高程值。
[0038] b、生成轮/腿运动形式的代价图；
[0039] 根据步骤a提取的地形特征，结合机器人轮式运动与腿式运动的特点，分别计算机 器人在R( i，j)内每个栅格的轮/腿运动形式的评价指标，包括轮运动形式的评价指标与 腿运动形式的评价指标(? 4亥指标既描述了机器人的通过性，也描述了机器人采用不同运 动形式通过区域R(i，j)时所消耗的代价，具体的包括能耗、时间等指标;评价指标Clj具有如 下形式[Trc ,Effc]，其中，Trc为通过性指标，Trc = O表示可通过区域R(i，j) ;Trc = 0表示不 可通过区域R( i，j);Ef f c为代价评价指标，描述通过区域R( i，j)的代价，Ef f c = (0，I)，为0 到1之间的连续值，数值越高，代价越高。
[0040] 通过BP人工神经网络，建立每个栅格内提取的各个地形特征?^，8^4^和评价 指标之间的映射。建立三层BP人工神经网络，并通过ADAMS仿真环境及实验中获得的 不同地形中机器人能耗，时间数据对神经网络进行训练，模拟地形特征与评价指标之间的 映射。采用上述模拟地形特征与评价指标之间的映射，遍历步骤1中建立的DEM图中每个栅 格，得到机器人的通过性地图及腿式、轮式运动代价图，图中标记有机器人轮式及腿式运动 都可以通过的栅格，轮式及腿式运动都不能通过的栅格(障碍物区域），以及只有腿式运动 可以通过的栅格。
[0041 ]步骤3:规划获得机器人最佳混合路径。
[0042] 本发明中采用对快速随机树法进行改进，来规划获得机器人的最佳混合路径，具 体方式为：
[0043] A、在步骤2中得到的腿式运动代价图上，以机器人出发点qstart为随机树的根节点， 机器人采用单纯腿式运动，以标准随机树算法进行扩展，逐渐增加叶节点直至随机树的叶 节点中包含了终点q g〇al，得到从qstart到qg〇al之间的一条单纯腿式运动目标路径T{1}。
[0044] B、估计目标路径T {1}的代价；
[0045] 中，路径上每个栅格的评价指标为(^·:，则目标路径T{ 1}的代价 Tc为：
[0046]
[0047] U利用c：又进型KKT寻找轮腿混合运动路径；
[0048]以T。为初始代价，进行迭代求解，至所用的时间超过用户设定的最大时限。每次迭 代中寻找到的混合运动路径代价需小于（1-ε)Τ。，其中ε为迭代因子;ε决定每次迭代过程路 径的优化程度，如果ε很大，则要求每次运动代价减小很多。
[0049] 每一次迭代步骤如下：
[0050] I、假设机器人周围的通过性地图，如图3、图4与图5所示，其中，图3为通过性地图 中已生成的部分随机树，起始点为q start，终点为qg〇ai，由操作者设定；圆点代表随机树的节 点，其中实心点表示采用腿式运动通过的点，空心点表示采用轮式运动通过的点。节点之间 的连线表示机器人的运动路径，若一段路径的两端节点分别为实心和空心点，或两端节点 均为实心点，则机器人采用腿式运动模式通过该段路径;若一段路径的两端节点均为空心 点，则机器人采用轮式运动模式通过该段路径。图中A区表示轮式运动和腿式运动都不能通 过的障碍物区域，B区表示只有腿式运动可以通过，C区表示轮式及腿式运动都可以通过的 区域。
[0051 ]以起始点qstart点为随机树的起始点，初始化qnew=qstart，qnew代表通过最新一次拓 展获得的可以加入随机树的节点。
[0052] Π 、若qnew与qgoal之间的距离小于设定值阈值dis_threshold，则进行步骤VI，否则 进行步骤m。
[0053] m、在已建立的通过性地图上选取一个目标点qtarget，选取方法如下：
[0054] 在区间[0，1 ]内生成一个随机数P，且P服从均勾分布，若p〈pg，Pg为设定值，则qtarget = qg〇ai，否则qtarget从通过性地图上的机器人轮式及腿式运动都可以通过的区域内随机选 取，且要求选取的q target满足：
[0055] h(q start, Qtarget ) ( qtarget, Qgoal ) ^Tc
[0056] 其中，h为定义的代价启发函数：
[0057]
[0058]其中，qi，q2分别为起始和目标点变量，I ine(qi，q2)为连接qi，q2的线段上所有栅格 点的集合。
[0059] h(qi，q2)提供了连接qi，q2路径的最小能耗估计，若：
[0060] h ( qstart, Qtarget ) ( qtarget, Qgoal ) ^Tc
[0061 ] 则添加qtarg(3t后，整个路径的最小能耗估计超过能耗限制Tc，因此q targ(3t不能作为 目标点。通过上述目标点选取方法，大大的减小了目标点的选取范围，同时保证了目标点选 取方法的探索性，加快了随机树的建立过程。
[0062] IV、如图4所示，在已生成的随机树上选取距离qtarget依次最近的SffAq nearJ, q_r_2，qn__3，并通过以下方法，从选取的3个节点扩展3条子路径到 qtarge3t，每条子路径为从 q_r_k向qtarget生长的直线段，k为自路径编号，k= 1，2，3;并估计这三条子路径的可行性： [0063] 1)若子路径经过机器人轮式及腿式运动不可以通过的栅格，则子路径扩展失败， 进行其它子路径的扩展；
[0064] 2)子路径从qnear_k向qtarget生长时，遇到轮式及腿式运动都可以通过的栅格时，采 用轮式运动方式通过。
[0065] 3)子路径从qnear_k向qtarget生长时，遇到只有腿式运动可以通过的栅格时，机器人 转化为腿式运动模式，采用腿式通过。
[0066] 4)形成的子路径中，若存在一个轮式及腿式运动都可以通过的栅格时，相邻的两 个栅格均为机器人轮式及腿式运动不可以通过的栅格时，则将这个轮式及腿式运动都可以 通过的栅格的通过方式改为机器人轮式及腿式运动不可以通过的栅格，如图5所示。
[0067] 上述生成的子路径可行性的判断方法为：
[0068] CoS t ( Qstart，Qnear ) +CoS t ( Qnear，Qtarget) +h ( Qtarget，Qgoal )〈Tc
[0069] 其中，函数c〇st(a,b) = Σ,?〇办为连接a，b的子路径的代价总 和，即a，b的子路径上每个栅格的轮/腿运动形式的评价指标，其中node为自路 径上栅格编号，mode代表w，I，postion为连接a，b的子路径上的节点的累积。
[0070] V、如3条子路径均满足上述代价要求，则三条子路径可行，若3条子路径中，有一 条及以上的路径可行，则随机选取一条子路径加入到现有随机树中，并令q new=qtarget，返回 步骤Π。若3条子路径均扩展不成功，则在每个子路径上重新获取目标点，
[0071 ] (hargetjc - - )〇qneurk + X(hii.ryet 入仁(〇，.l).
[0072] λ = λ-〇.2,初始时，令λ = 〇.8,获取每个子路径新目标点后，重复步骤IV进行扩展； 若λ=ο时，则整个子扩展过程失败，返回步骤m，否则返回步骤V继续进行子扩展。
[0073] VI、返回形成的RRT，获得从起始点qstart到目标Aqgcial的路径，并估计该路径的代 价，令其等于Tc，至此完成一次路径规划迭代子过程。
【主权项】
1. 本发明一种六轮/腿机器人复合运动路径规划方法，其特征在于：通过下述步骤完 成： 步骤1:探测并确定机器人工作环境信息，建立其工作环境的DEM图； 步骤2:对步骤1中建立的呢Μ图进行特征提取，包括:坡度、台阶类障碍高度与粗糖度Ξ 种地貌特征;采用Ξ种地貌特征与评价指标之间的映射，遍历DEM图中每个栅格，得到机器 人的通过性地图及腿式、轮式运动代价图，图中标记有机器人轮式及腿式运动都可W通过 的栅格，轮式及腿式运动都不能通过的栅格，W及只有腿式运动可W通过的栅格； 步骤3:规划获得机器人最佳混合路径； A、 在步骤2中得到的腿式运动代价图上，W机器人出发点qstart为随机树的根节点，机器 人采用单纯腿式运动，W标准随机树算法进行扩展，逐渐增加叶节点直至随机树的叶节点 中包含了终点qgoal，得到从qstart到qgoal之间的一条单纯腿式运动目标路径T { 1 }; B、 估计目标路径Τ{1}的代价Tc; C、 WT。为初始代价，进行迭代求解，至所用的时间超过用户设定的最大时限;每次迭代 中寻找到的混合运动路径代价需小于(l-OTc，其中ε为迭代因子。2. 如权利要求1所述一种六轮/腿机器人复合运动路径规划方法，其特征在于：步骤2 中，坡度、台阶类障碍高度与粗糖度Ξ种地貌特征提取方式为： ① 坡度:令i，j为栅格坐标，机器人本体在Χ-Υ平面内投影的最小包络矩形为R( i，j)，将 R( i，j)内的平均倾角作为坡度特征，表示为：其中，Θχ表示沿X轴方向的平均倾角，Θγ表示沿y方向的平均倾角； ② 粗糖度:采用R( i，j)区域内高程的标准差进行估计，表示为：其中，刮表示R(i，j)区域内的高程平均值，η为R(i，j)区域内栅格的个数； ③ 台阶类障碍高度:表示为： Ei, j - Zmax (Ri,j)~Zmin(Ri,j) 其中，Zmax和Zmin为R( i，j )内最高和最低高程值。 3 .如权利要求1所述一种六轮/腿机器人复合运动路径规划方法，其特征在于:步骤2的 b中生成轮/腿运动形式的代价图的具体方式为:根据地形特征，结合机器人轮式运动与腿 式运动的特点，分别计算机器人在R(i，j)内每个栅格的轮/腿运动形式的评价指标，包括轮 运动形式的评价指标与腿运动形式的评价指标4;通过BP人工神经网络，建立每个栅格 内提取的各个地形特征01,^，81,^，61,神日评价指标;巧；.（^之间的映射;建立^层8?人工神 经网络，并通过ADMS仿真环境及实验中获得的不同地形中机器人能耗，时间数据对神经网 络进行训练，模拟地形特征与评价指标之间的映射;采用上述模拟地形特征与评价指标之 间的映射，遍历步骤1中建立的DEM图中每个栅格，得到机器人的通过性地图及腿式、轮式运 动代价图，图中标记有机器人轮式及腿式运动都可W通过的栅格，轮式及腿式运动都不能 通过的栅格(障碍物区域），w及只有腿式运动可w通过的栅格。4. 如权利要求3所述一种六轮/腿机器人复合运动路径规划方法，其特征在于:评价指 标Cij的形式为[TrCiEffc]，其中，Trc为通过性指标，T;rc = 0表示可通过区域R(i，j) ;T;rc = l 表示不可通过区域R(i，j); Ef f c为代价评价指标，描述通过区域R(i，j)的代价，Ef f c = (Ο， 1)，为Ο到1之间的连续值。5. 如权利要求1所述一种六轮/腿机器人复合运动路径规划方法，其特征在于:所述步 骤A中的每一次迭代步骤如下： I、在机器人周围的通过性地图中已生成的部分随机树，设定起始点qstart，终点为qgoal 点，由操作者设定；W点代表随机树的节点，其中，实屯、点表示采用腿式运动通过的点，空屯、 点表示采用轮式运动通过的点；节点之间的连线表示机器人的运动路径，若一段路径的两 端节点分别为实屯、和空屯、点，或两端节点均为实屯、点，则机器人采用腿式运动模式通过该 段路径，；若一段路径的两端节点均为空屯、点，则机器人采用轮式运动模式通过该段路径； W起始点qstart点为随机树的起始点，初始化qnew= qstart，qnew代表通过最新一次拓展获得的 可W加入随机树的节点； Π 、若qnew与qgoal之间的距离小于设定值阔值dis_threshold，则进行步骤VI，否则进行 步骤虹； 虹、在已建立的通过性地图上选取一个目标点qtarget，选取方法如下： 在区间[〇，1]内生成一个随机数P，且P服从均匀分布，若P<Pg，Pg为设定值，则qtarget = qgnal，否则qtarget从通过性地图上的机器人轮式及腿式运动都可W通过的区域内随机选取， 且要求选取的qtarget满足： h ( qstart) qi;arge〇 +h ( qi;arget) qgoal〈Tc 其中，h为定义的代价启发函数：其中，qi，Q2分别为起始和目标点变量，1 ine (qi，Q2)为连接qi，Q2的线段上所有栅格点的 集合； IV、在已生成的随机树上选取距离qtarget依次最近的3个节点qnear_l，qnear_2，qnear_3，并通 过W下方法，从选取的3个节点扩展3条子路径到qtarget，每条子路径为从qnear_k向qtarget生长 的直线段，k为自路径编号，k= 1，2，3 ;并估计运Ξ条子路径的可行性： 1 )若子路径经过机器人轮式及腿式运动不可W通过的栅格，则子路径扩展失败，进行 其它子路径的扩展； 2) 子路径从qnear_k向qtarget生长时，遇到轮式及腿式运动都可W通过的栅格时，采用轮 式运动方式通过； 3) 子路径从qnear_k向qtarget生长时，遇到只有腿式运动可W通过的栅格时，机器人转化 为腿式运动模式，采用腿式通过； 4) 形成的子路径中，若存在一个轮式及腿式运动都可W通过的栅格时，相邻的两个栅 格均为机器人轮式及腿式运动不可W通过的栅格时，则将运个轮式及腿式运动都可W通过 的栅格的通过方式改为机器人轮式及腿式运动不可W通过的栅格； 生成的子路径可行性的判断方法为： Cost ( Qstart, Qiiear )+C〇St ( qnear , qtarget) +h (qtarget, Qgoal )〈Tc 其中，函数cost紅b)二二Pf加知b)旬茲f品迸Li，.为连接a，b的子路径的代价总和，即 a,b的子路径上每个栅格的轮/腿运动形式的评价指标C品蓝品益ij，。，其中node为自路径上 栅格编号，mode代表W, 1，pos t i on为连接a, b的子路径上的节点的累积； ν、Ξ条子路径可行，若3条子路径中，有一条及W上的路径可行，则随机选取一条子路 径加入到现有随机树中，并令qnew= qtarget，返回步骤Π ;若3条子路径均扩展不成功，则在每 个子路径上重新获取目标点， Qtarget k _ ( 1 -入)Qneark+入Ql;arget入 E ( 〇 , 1 ) 入=λ-0.2，初始时，令λ = 0.8，获取每个子路径新目标点后，重复步骤IV进行扩展;若入 =0时，则整个子扩展过程失败，返回步骤虹，否则返回步骤V继续进行子扩展； VI、返回形成的RRT，获得从起始点qstart到目标点qgoal的路径，并估计该路径的代价，令 其等于Tc，至此完成一次路径规划迭代子过程。
【文档编号】G05D1/02GK105843222SQ201610158085
【公开日】2016年8月10日
【申请日】2016年3月18日
【发明人】丁希仑, 杨帆, 彭赛金, 郑羿, 尹业成
【申请人】北京航空航天大学