CN111830986A - 一种用于双足机器人的三维路径规划方法 - Google Patents

Abstract

本发明提供了一种双足机器人的三维路径规划方法，属于机器人路径规划技术领域，综合考虑双足机器人可跨越物体或可上台阶的最大高度、可跨越物体的最大宽度、双足机器人足部长度以及三维环境中物体的位置和尺寸，重新设计节点的代价，并进行代价评估，在存在多种类型物体的复杂环境中提前计算一条代价最小的三维路径；该方法规划得到的路径，能够引导机器人在相应的环境中执行跨越、绕行、上台阶的运动。与现有技术相比，本发明能够充分发挥双足机器人的运动潜能，提高通行效率；同时也使得双足机器人运算负荷减轻，计算速度更高。

Description

一种用于双足机器人的三维路径规划方法

技术领域

本发明属于机器人路径规划技术领域，具体涉及一种用于双足机器人的三维路径规划方法。

背景技术

双足机器人因其具有人类的外形特征，设计之初就是希望他将来能够代替或协助人类完成一些工作。人类在复杂的三维环境中的运动方式是多种多样的，如绕过不能穿越的障碍物，跨过低矮的障碍物，迈上多级的台阶等；而在穿越环境完成任务的过程中是否有必要执行这些动作中的一种或几种，是根据自身条件和经验选择的可通行路径所决定的。

在包含多种类型障碍物的复杂环境中，双足机器人从一个位置运动到另一个位置可能存在多条可行路线。有的路线绕过所有具有高度和宽度信息的物体，到达指定地点；有的路线穿越了具有较低高度和较宽宽度的物体。现有技术主要是利用不同的算法，如A*、RRT、ARA*等，在二维平面规划一条最短路径，或者是利用步态规划获得路径。现有技术存在如下问题：

(1)现有方法没有充分考虑双足机器人腿的优势(即腿所具有的跨越)和上台阶的运动能力，所规划出来的路线单纯停留在二维平面，将所有具有高度的障碍物都绕行，这样规划出的路线并不是最优的，严重限制了双足机器人的运动潜能，降低了运动的效率。

(2)现有方法规划出的路线虽然是一条三维路线，但是这条路线是服务于移动式机械设备，完全不符合双足机器人的运动约束；还有的方法没有考虑环境中多种类型的物体，所规划的结果只是针对较简单类型的环境。

(3)现有方法将路径规划与步态规划相结合，技术复杂、技术成本较高，严重影响双足机器人的快速反应能力。

发明内容

针对现有技术中存在不足，本发明提供了一种用于双足机器人的三维路径规划方法，使得双足机器人充分发挥自身的运动潜能，提高通行效率，顺利通过复杂环境，同时减轻机器人在运动过程中的计算负荷，降低技术成本。

本发明是通过以下技术手段实现上述技术目的的。

一种用于双足机器人的三维路径规划方法，包括如下步骤：

步骤(1)，障碍物标识

在栅格地图上使用(x，y，z，w，mark，inherit)表示节点，其中(x、y、z)表示节点的位置，w表示节点所在平面的长度，w通过判断z值进行赋值：当某个节点n_i的z值与它正前方节点n_i+1，n_i+2…n_i+k的z值相等，则所述节点n_i，n_i+1…n_i+k的w＝(k+1)L_p，根据w的值，判断所述k+1个节点能否放置双足机器人的足部；其中L_p为栅格地图的精度；

mark表示节点的类型，通过z的值进行判断，当z＝0时，mark＝0；当0＜z＜h_max时，该节点是可以被跨越的栅格节点或者台阶栅格节点，如果节点同时满足w≥L_f，则mark＝1，如果节点同时满足0≤w＜L_f：当一系列节点的z值比下一个系列节点的z值小，mark＝2，当一系列节点的z值比下一个系列节点的z值大，且0≤w＜L_a，mark＝3，若w>L_a，mark＝5；当z≥h_max时，mark＝4；其中：mark＝0，表示该节点为水平面地形；mark＝1，表示为可以迈上去的台阶；mark＝2，表示这一系列节点为面窄的台阶面，是不可布置脚的区域；mark＝3，则表示这一系列节点为可跨越区域；mark＝4，表示该节点不可被跨域或被作为台阶踏上去；mark＝5，表示需绕行的普通障碍物；L_f为双足机器人足部长度，h_max为双足机器人可跨越物体或可上台阶的最大高度，L_a为可跨越物体的最大宽度；

inherit用于表示该节点的父节点；

步骤(2)，将标识出来的障碍物信息传输给工控机，计算节点代价

节点的代价f(n)＝g(n)+h(n)+c_h*a(n)+c_w*w(n)，其中g(n)是从起始节点n₀经由其子节点到其某个正在搜索的当前节点的代价；h(n)是从该当前节点到终点n_end的代价；a(n)是评估g(n)所途径的节点和h(n)所途径的节点的高度代价，且

当z≥h_max时，因子c_h→∞，表示不可跨域，当z＜h_max时，c_h＝1，表示可跨域；w(n)是用来衡量节点所在区域的长度代价，且

如果L_f＜w，表示节点所在区域可以放置双足机器人的足部，此时因子c_w＝1，如果L_f≥w，表示节点所在区域不可以放置双足机器人的足部，此时c_w→∞；

步骤(3)，三维路径规划

步骤(3.1)，在待搜索的OPEN列表放入起始节点n₀，以n₀为父节点，判断与其相邻节点A1的mark，如果mark的值为0或1或3中的一个，将相邻节点A1加入到OPEN列表中，将相邻节点A1的父节点inherit设置为n₀，同时将起始节点n₀移入CLOSE列表；

步骤(3.2)，计算OPEN列表中的所有节点代价，选择f(n)值最小的节点，作为当前节点A2，将其移入CLOSE列表；

步骤(3.3)，判断当前节点A2相邻节点A3的mark，如果mark的值为0或1或3中的一个，如果某个相邻节点A3既不在CLOSE列表中，也不在OPEN列表中，则将某个相邻节点A3的父节点inherit设置为当前节点A2，将其加入OPEN列表中；如果当前节点A2的相邻节点A3已经在OPEN列表中，则假设相邻节点A3的父节点为当前节点A2，计算新的代价值f(n)₂，如果新的代价值f(n)₂比原来的代价值f(n)₁小，则将相邻节点A3的父节点inherit设置为当前节点A2，否则不做任何修改；所述原来的代价值f(n)₁在步骤(3.2)中计算得到；

步骤(3.4)，重复步骤(3.2)-(3.3)，直到在OPEN列表中发现终点n_end，停止循环搜索；

步骤(3.5)，从终点n_end开始寻找父节点，直到到达起点n₀为止，即获得一条最优三维路径的所有信息；机器人按照规划的最优三维路径途径的区域，到达终点。

进一步的技术方案，所述g(n)用途径的节点的距离和表示：

其中：x_i，y_i是第i个节点的坐标值。

进一步的技术方案，所述h(n)用两点之间的直线距离表示：

其中：x_k、y_k为当前节点的坐标，x_end、y_end是终点的坐标。

进一步的技术方案，所述w的初始值为栅格地图的精度L_p。

进一步的技术方案，所述栅格地图通过环境感知传感器获取双足机器人的周围环境信息，并在计算机进行栅格化处理得到。

更进一步的技术方案，所述环境感知传感器为视觉传感器或激光雷达。

本发明的有益效果为：

本发明综合考虑双足机器人可跨越物体或可上台阶的最大高度、可跨越物体的最大宽度、双足机器人足部长度以及三维环境中物体的位置和尺寸，重新设计节点的代价，并进行代价评估，在存在多种类型物体的复杂环境中提前计算一条代价最小的路径，该路径允许双足机器人在执行具体的运动时能够选择绕行、跨越、上台阶动作，双足机器人能够最大限度的发挥其运动潜能，提高了机器人穿越三维环境的效率，同时降低运算成本。

附图说明

图1为本发明所述障碍物标识流程图；

图2为本发明三维路径规划结果示意图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

一种用于双足机器人的三维路径规划方法，具体包括如下步骤：

步骤(1)，障碍物标识

通过环境感知传感器(视觉传感器或者激光雷达)获取双足机器人的周围环境信息，在计算机进行栅格化处理，获取栅格地图，在栅格地图上使用(x，y，z，w，mark，inherit)表示每个栅格，称之为一个节点的信息，其中(x、y、z)表示节点的位置。w表示节点所在平面的长度，每个w的初始值均是栅格地图的精度L_p(即栅格的长度)，w是通过判断z值进行赋值：当某个节点n_i的z值与它正前方节点n_i+1，n_i+2…n_i+k的z值相等，则n_i，n_i+1…n_i+k这k+1个节点的w＝(k+1)L_p；然后根据w的取值，判断k+1个节点能否放置双足机器人的足部，如果w＜L_f，则上述k+1个节点均不能放置双足机器人的足部，如果w≥L_f，则上述k+1个节点可以放置双足机器人的足部，其中L_f为双足机器人足部长度。mark表示节点的类型，也是通过z的值进行判断的，当z＝0时，mark＝0，表示该节点为水平面地形；当0＜z＜h_max时，表示该节点为可以被跨越的栅格节点，或者是台阶栅格节点，如果节点同时满足w≥L_f，则mark＝1(表示为可以迈上去的台阶)，如果节点同时满足0≤w＜L_f，再判断与节点n_i的z值相等的一系列节点的z值与下一个系列节点的z值大小，当一系列节点的z值比下一个系列节点的z值小(即z_i，z_i+1…z_i+k＜z_i+k+1，z_i+k+2…z_i+m)，则表示这一系列节点为面窄的台阶面，是不可布置脚的区域，mark＝2，当一系列节点的z值比下一个系列节点的z值大(即z_i，z_i+1…z_i+k≥z_i+k+1，z_i+k+2…z_i+m)，并且0≤w＜L_a，则表示这一系列节点为可跨越区域，则mark＝3，若w>L_a，则mark＝5，表示需绕行的普通障碍物；当z≥h_max时，表示该节点不可被跨域或被作为台阶踏上去，mark＝4；其中h_max为双足机器人可跨越和可上台阶的最大高度，L_a为可跨越物体或可上台阶的最大宽度。inherit用于表示该节点的父节点，初始时为节点本身。障碍物标识的流程图如图1所示。

步骤(2)，将步骤(1)标识出来的障碍物信息传输给工控机，计算节点代价

从起点到终点的最优路径是通过该路径的代价来确定的，代价最小的路径认为是最优的，路径节点的代价的计算方法如下：

f(n)＝g(n)+h(n)+c_h*a(n)+c_w*w(n) (1)

其中：g(n)是从起始节点n₀经由其子节点到其某个正在搜索的当前节点的代价，该代价用所途径的节点的距离和表示：

其中：x_i，y_i是第i个节点的坐标值；

h(n)是从该当前节点到终点n_end的代价，该代价用两点之间的直线距离表示，只将它作为一个估计值，所以不考虑该连线上的不可通行区域，h(n)的表达式为：

其中：x_k、y_k为当前节点的坐标，x_end、y_end是终点的坐标；

a(n)是评估g(n)所途径的节点和h(n)所途径的节点的高度代价，通过下式计算得到：

其中：z为途径的某个节点的真实高度，当z≥h_max时，因子c_h→∞，表示不可跨域，代价无限大；当z＜h_max时，c_h＝1，表示可跨域。

w(n)是用来衡量节点所在区域的长度代价，通过下式计算得到：

如果L_f＜w，表示节点所在区域可以放置双足机器人的足部，此时因子c_w＝1；如果L_f≥w，表示节点所在区域不可以放置双足机器人的足部，此时c_w→∞。

步骤(3)，三维路径规划

步骤(3.1)，首先将起始节点n₀放入待搜索OPEN列表中，以n₀为父节点，判断与其相邻节点A1的mark，如果mark的值为0或1或3中的一个，说明相邻节点A1是可以进行下一步规划的，则将相邻节点A1也加入到OPEN列表中，将相邻节点A1的父节点inherit设置为n₀，同时将起始节点n₀移出OPEN列表，移入CLOSE列表，CLOSE列表是无需再检查的列表；

步骤(3.2)，根据公式(1)计算OPEN列表中的所有节点代价f(n)；

步骤(3.3)，选择OPEN列表中f(n)值最小的节点，作为当前节点A2，将其移出OPEN列表，移入CLOSE列表；

步骤(3.4)，判断当前节点A2相邻节点A3的mark，如果mark的值为0或1或3中的一个，并且某个相邻节点A3既不在CLOSE列表中，也不在OPEN列表中，则将某个相邻节点A3的父节点inherit设置为当前节点A2，将其加入OPEN列表中；如果当前节点A2的相邻节点A3已经在OPEN列表中，则假设相邻节点A3的父节点为当前节点A2，计算新的代价值f(n)₂，如果新的代价值f(n)₂比原来的代价值f(n)₁小(代价f(n)₁已在步骤(3.2)中计算得到)，则将相邻节点A3的父节点inherit设置为当前节点A2，否则不做任何修改；

步骤(3.5)，重复步骤(3.2)-(3.4)，直到在OPEN列表中发现终点n_end，停止循环搜索；

步骤(3.6)，从终点n_end开始寻找父节点，直到到达起点n₀为止，即可获得一条最优三维路径的所有信息；机器人按照规划的最优三维路径途径的区域，到达终点。

如图2所示，11为利用本发明方法最终搜索获得的最优路径，双足机器人2从起点1出发，其中包括绕过高度很高的障碍物3，跨过高度和宽度符合机器人特性的低矮障碍物4，跨上台面宽度足够宽、高度不高的台阶6，没有选择台阶面跨度不足以放置脚的台阶5，最终到达终点7。路线9也能实现起点1到达终点7，绕过高度过高的障碍物3和低矮的障碍物4，跨上台面宽度足够宽、高度不高的台阶8，虽然顺利到达了终点7，但明显路线不是最优的。路线10是二维平面的规划结果，无法到达具有高度信息的终点。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (6)

1.一种用于双足机器人的三维路径规划方法，其特征在于，包括如下步骤：

步骤(1)，障碍物标识

在栅格地图上使用(x，y，z，w，mark，inherit)表示节点，其中(x、y、z)表示节点的位置，w表示节点所在平面的长度，w通过判断z值进行赋值：当某个节点n_i的z值与它正前方节点n_i+1，n_i+2…n_i+k的z值相等，则所述节点n_i，n_i+1…n_i+k的w＝(k+1)L_p，根据w的值，判断所述k+1个节点能否放置双足机器人的足部；其中L_p为栅格地图的精度；

mark表示节点的类型，通过z的值进行判断，当z＝0时，mark＝0；当0＜z＜h_max时，该节点是可以被跨越的栅格节点或者台阶栅格节点，如果节点同时满足w≥L_f，则mark＝1，如果节点同时满足0≤w＜L_f：当一系列节点的z值比下一个系列节点的z值小，mark＝2，当一系列节点的z值比下一个系列节点的z值大，且0≤w＜L_a，mark＝3，若w>L_a，mark＝5；当z≥h_max时，mark＝4；其中：mark＝0，表示该节点为水平面地形；mark＝1，表示为可以迈上去的台阶；mark＝2，表示这一系列节点为面窄的台阶面，是不可布置脚的区域；mark＝3，则表示这一系列节点为可跨越区域；mark＝4，表示该节点不可被跨域或被作为台阶踏上去；mark＝5，表示需绕行的普通障碍物；L_f为双足机器人足部长度，h_max为双足机器人可跨越物体或可上台阶的最大高度，L_a为可跨越物体的最大宽度；

inherit用于表示该节点的父节点；

步骤(2)，将标识出来的障碍物信息传输给工控机，计算节点代价

节点的代价f(n)＝g(n)+h(n)+c_h*a(n)+c_w*w(n)，其中g(n)是从起始节点n₀经由其子节点到其某个正在搜索的当前节点的代价；h(n)是从该当前节点到终点n_end的代价；a(n)是评估g(n)所途径的节点和h(n)所途径的节点的高度代价，且

当z≥h_max时，因子c_h→∞，表示不可跨域，当z＜h_max时，c_h＝1，表示可跨域；w(n)是用来衡量节点所在区域的长度代价，且

如果L_f＜w，表示节点所在区域可以放置双足机器人的足部，此时因子c_w＝1，如果L_f≥w，表示节点所在区域不可以放置双足机器人的足部，此时c_w→∞；

步骤(3)，三维路径规划

步骤(3.1)，在待搜索的OPEN列表放入起始节点n₀，以n₀为父节点，判断与其相邻节点A1的mark，如果mark的值为0或1或3中的一个，将相邻节点A1加入到OPEN列表中，将相邻节点A1的父节点inherit设置为n₀，同时将起始节点n₀移入CLOSE列表；

步骤(3.2)，计算OPEN列表中的所有节点代价，选择f(n)值最小的节点，作为当前节点A2，将其移入CLOSE列表；

步骤(3.3)，判断当前节点A2相邻节点A3的mark，如果mark的值为0或1或3中的一个，如果某个相邻节点A3既不在CLOSE列表中，也不在OPEN列表中，则将某个相邻节点A3的父节点inherit设置为当前节点A2，将其加入OPEN列表中；如果当前节点A2的相邻节点A3已经在OPEN列表中，则假设相邻节点A3的父节点为当前节点A2，计算新的代价值f(n)₂，如果新的代价值f(n)₂比原来的代价值f(n)₁小，则将相邻节点A3的父节点inherit设置为当前节点A2，否则不做任何修改；所述原来的代价值f(n)₁在步骤(3.2)中计算得到；

步骤(3.4)，重复步骤(3.2)-(3.3)，直到在OPEN列表中发现终点n_end，停止循环搜索；

步骤(3.5)，从终点n_end开始寻找父节点，直到到达起点n₀为止，即获得一条最优三维路径的所有信息；机器人按照规划的最优三维路径途径的区域，到达终点。

2.根据权利要求1所述的用于双足机器人的三维路径规划方法，其特征在于，所述g(n)用途径的节点的距离和表示：

其中：x_i，y_i是第i个节点的坐标值。

3.根据权利要求1所述的用于双足机器人的三维路径规划方法，其特征在于，所述h(n)用两点之间的直线距离表示：

其中：x_k、y_k为当前节点的坐标，x_end、y_end是终点的坐标。

4.根据权利要求1所述的用于双足机器人的三维路径规划方法，其特征在于，所述w的初始值为栅格地图的精度L_p。

5.根据权利要求1所述的用于双足机器人的三维路径规划方法，其特征在于，所述栅格地图通过环境感知传感器获取双足机器人的周围环境信息，并在计算机进行栅格化处理得到。

6.根据权利要求5所述的用于双足机器人的三维路径规划方法，其特征在于，所述环境感知传感器为视觉传感器或激光雷达。

Images