-
摘要: 双足步行机器人的足迹规划方法难以满足快速步行条件下的计算效率要求, 并存在步幅变化时运动失稳的风险, 2D环境下点机器人栅格规划则难于生成针对双足步行的高效路径.本文提出针对各向异性特征全方位步行机器人的一种路径规划策略, 将状态网格图方法拓展到全方位移动机器人领域, 基于三项基本假设及基元类型划分给出了系统的运动基元枚举及选择方法, 借助实时修正的增量式AD*搜索算法实现仿人机器人在动态环境下的快速路径规划, 通过合理选择启发函数及状态转移代价, 生成了平滑高效的路径, 为后续足迹生成的动力学优化提供了基础.计算机仿真证实了方法对各类环境的适应性, Robocup避障竞速挑战赛的成功表现证明了方法对于机器人样机部署的可行性及其提高步行效率的潜力.Abstract: Footstep planning for bipedal walking robot is difficult to fulfil the requirement of computational efficiency at high walking speed; it also suffers from the risk of falling over with stride variations. On the other hand, 2D grid-based planning strategy for point robots is unable to generate an efficient walking path for bipedal robots. A path planning approach for heterogeneous omnidirectional bipedal walking robot is proposed in this paper. State lattice graph is brought into the omnidirectional moving condition. Based on three assumptions and type classification, a systematic motion primitive enumerating and selecting method is given. The rapid path planning for humanoid robots in dynamic environment can be achieved by making use of the algorithm of anytime repairing incremental search AD*. The smooth and efficient path, generated by reasonable heuristics function and state transfer cost, provides the foundation for dynamics optimized footstep planning. Simulation has demonstrated the adaptability of the biped robot in various environments. The successful performance in obstacle avoidance challenge in Robocup proves the possibility of implementation on physical robots and the capacity for walking efficiency improvement.
-
Key words:
- Humanoid robot /
- path planning /
- state lattice /
- dynamic planning /
- footstep planning
-
路径规划是机器人学的基本问题, 其目的是在构型空间中求解一个特定序列, 使得机器人自起点开始, 避开不可达和存在碰撞风险的构型, 最终到达目标构型[1].针对路径规划问题先后提出了多种求解策略, 主要包括以局部环境信息为基础的人工势场法和以全局环境信息为基础的人工智能图搜索方法等, 前者以建立由目标产生引力和障碍物产生斥力叠加的人工势函数为主要思想[2], 后者则首先在机器人构型空间内建立状态节点和节点间转移关系, 构成搜索图, 而后由广度优先、深度优先、动态规划、随机搜索等多种算法生成代价最优路径[3-4], 由于后者能够避免陷入局部最小的风险, 具有良好的规划结果, 随着计算机软硬件平台性能的升级而获得广泛关注.目前, 基本的路径规划问题, 特别是低维构型空间下的相关问题已得到较好的解决, 相关技术趋于成熟, 在机器人导航等领域得到实际应用.然而若考虑以双足步行实现移动的仿人机器人这一特殊平台, 则无法仅对上述方法进行简单推广和延伸即可解决.这是由于仿人机器人的运动关节多达数十个, 而且其步行运动具有本质不稳定的特征, 因此, 双足步行机器人的规划问题具有高维度构型空间及受到复杂的微分约束的特征, 求解代价极其高昂[4], 因此该问题值得特殊对待.
目前相关研究以足迹规划为主要思想[5], 其通过在规划环境内实施足迹采样，并借助动态规划[6]或随机搜索[7-8]算法, 获得自起点到终点的一个足迹序列, 由机器人依次执行.其能够处理复杂的高度地图、不平路面, 并允许机器人踩踏上障碍物, 完成高效的避障和巡航任务, 已在仿真平台[9-10]及多个机器人样机[11-12]上得到证实.
基于足迹规划算法并配合已知足迹生成关节轨迹的诸多方法为仿人机器人的整体步行运动规划建立了通用的流程, 理论上给出了完整的解决方案, 但其也存在若干局限性, 例如其仅能以一步的步长实施采样规划, 当规划环境面积较大时, 无法快速给出规划结果, 此外当加快仿人机器人步频以实现快速移动时, 没有考虑前后步幅连接的约束, 存在失稳摔倒的风险[13]。
事实上, 足迹规划思想多用于解决充斥障碍物的复杂非结构化环境, 在步行启动之前较长的运动规划时间以及较慢的步速能够满足现实要求, 但倘若需要机器人快速步行, 或者环境较简单的条件下仍旧采用这一策略, 则无法满足预期.
为了解决这一困境, 文献[14]提出低维构型空间下点机器人路径规划和足迹规划相结合的方法, 从而扩大规划环境面积, 文献[15]则提出依据环境复杂程度, 灵活选择二维点机器人规划算法和足迹规划算法, 从而综合提升规划面积和规划灵活度.然而这些局部改良方法将机器人简化为环境中的一点 (即点机器人), 规划算法会产生频繁的以45°为倍数的姿态朝向变化如图 1左图所示, 机器人需要在这些急转弯处停止前进, 并原地旋转, 最终导致规划算法计算效率提升后, 机器人的实际运行效率却急剧下降.此外这些方法也没有考虑快速步行的前后足迹点位置约束问题, 因此无法同时满足步行快速、运动稳定、算法高效的要求.
图 1 基于栅格地图规划及曲线路径规划的对比Fig. 1 Comparison of grid map based planning and curved pathplanning事实上, 将逐个足迹的规划替代为逐段路径的规划能够有效解决算法效率的问题, 但应避免将仿人机器人过渡简化为点机器人, 因为点机器人具有各向同性的全方位移动能力, 与此相反, 仿人机器人在前进、侧移和旋转三个维度的移动能力不同, 是一种典型的各向异性全方位移动机器人[16].如图 1所示, 在两段路径长度相同, 点机器人移动速率相同的条件下, 其完成移动的时间也相同.但仿人机器人则需要在图 1左中的a点和b点原地旋转, 或者以侧移方式完成 ${a} \to {b}$ 点的移动, 其整体移动效率显然不及图 1右图中前进同时旋转的状况.
基于上述考虑, 本文尝试拓展受非完整约束的轮式机器人的Kino-dynamics (运动--动力学) 规划方法, 基于Lattice网格的图搜索, 以类似图 1右中的移动方式实现仿人机器人的路径规划, 使得该路径充分利用各向异性全方位移动特征, 从而易于针对前后步幅连接约束实现步行效率的优化.
1. 建立Lattice网格搜索图
状态网格 (State lattice)[17]方法将构型空间划分为栅格, 但不同于状态栅格 (Stategrid) 方法仅能实现相邻的四联通或八连通的状态转移, 而是拓展到通过可行运动连接的任意两个栅格点之间.如图 2所示, 起始格点、终止格点以及连接两者的曲线一同构成了一个运动基元 (Motionprimitive), 其不断延伸和拓展从而构成状态网格.
路径规划的目标就是从状态网格中搜索到一条自起点至终点的连续无碰运动基元序列[18], 其整体求解过程如图 3所示.
图 3 基于Lattice网格图的路径规划原理图Fig. 3 Illustration of the state lattice graph based pathplanning此前Lattice网格方法主要用于解决受到特殊运动学约束, 无法实现侧向移动或原地旋转的四轮汽车或差动轮式车辆的路径规划问题[17-19], 其运动基元能够实现前进和旋转两个分量合成的曲线运动.如果直接借用这一策略, 将仿人机器人视作轮式移动机器人, 则能够生成类似于图 1右的规划结果, 但这种方法未能充分发挥其全方位移动潜力, 因此本文首先需将Statelattice状态网格图拓展至兼有前进、侧移、旋转的全方位移动机器人领域.而其核心在于构建完整的全方位移动运动基元集合.该方法基于以下三项基本假设:
1) 任何运动基元需满足起始于栅格点, 终止于栅格点的几何约束;
2) 需包含前进、后退、侧移、原地旋转一个栅格单位的运动基元 (如图 4所示);
图 4 前进、侧移、旋转一个栅格单位的基本运动单元Fig. 4 Basic motion primitives including forward walking, sidling and self-spin for one unit3) 同类的多段运动基元能够构造为平滑的路径, 在基元前后连接位置, 位置曲线和姿态角曲线均光滑可导.
其中, 假设1) 确保Lattice网格图的封闭性, 假设2) 则保证解的完备性, 即至少可以通过2D栅格规划给出可行解.根据假设3) 可以得出在一个运动基元内部机器人姿态角与路径切线方向夹角应保持不变的结论.如图 5(a)所示, 当机器人始末姿态角与运动路径的切线方向夹角一致则能够保证路径连续光滑; 而如图 5(b)所示, 当始末姿态角与运动路径的切线方向夹角不一致时要求机器人在两个基元的连接点处原地旋转, 因而不再满足基本假设3).
图 5 始末姿态角关系与光滑路径生成示意图Fig. 5 Illustration of the relationship between the start-endattitude angle and the smooth path generation为描述方便, 将假设2) 所生成的运动基元称作基本单元, 而将同时带有前进和旋转分量的单元称作第一类基本单元, 除此之外的复杂全方位运动基元称作第二类基本单元.其中第一类基本单元如图 6所示.
运动基元起始于状态s0, 结束于状态s1, 假定在运动基元内部机器人做圆周运动, 则根据几何关系可得
$\begin{align}\Delta \theta = 2 \cdot \arctan (\Delta y, \Delta x)\end{align} $
(1) 其中 $(\Delta x, \Delta y, \Delta \theta )$ 分别是平动和转动的位移, 假设沿 $x$ 及 $y$ 方向栅格均匀划分, 栅格边长为 $\delta$ , 同时将圆周角度 $ 2 \pi$ 划分为 $n$ 等份, 依据假设1), 终止状态 $s_1$ 必须位于状态栅格上, 其位移必为 $\delta$ 的整数倍, 对应的角位移为 $2 \pi /n$ 的整数倍, 即
$\left[\begin{matrix} \Delta x \\ \Delta y \\ \Delta \theta \\ \end{matrix} \right]=\left[\begin{matrix} X\delta \\ Y\delta \\ 2\pi \Theta /n \\ \end{matrix} \right], \quad X, Y, \Theta \in \boldsymbol{Z}$
(2) 其中 $X, Y, \Theta$ 分别是该运动基元与单位位移 $\delta$ 和单位角位移 $2 \pi/n$ 的整数倍数.因此每一个运动基元均可表示为 $X, Y, \Theta$ 的一个三元组, 枚举第一类基本单元即求解出所有的三元组的集合.具体方法是针对每一个 $\Theta = 0, 1, 2, \cdots, n -1, $ 求解最终运动基元的参数 $X, Y$ , 即求解如式 (3) 所示最小化问题.
$\begin{align} & \begin{matrix} \min & {{X}^{2}}+{{Y}^{2}} \\ \text{s}\text{.t}\text{.} & \left| \frac{2\pi \Theta }{n}-2\arctan (Y, X) \right|<\varepsilon \\ \end{matrix} \\ & X, Y\in \boldsymbol{Z} \\ \end{align}$
(3) 式中 $\varepsilon$ 是预设的求解角度误差, 可采用遍历算法求解该最优化问题:依据图 6中的几何关系, 沿射线 $\overrightarrow {OL}$ 自 $X = 0, Y = 0$ 起依次将临近栅格点坐标代入式 (3) 验证约束, 从而将首个满足约束条件的栅格点作为求解结果.获得终了状态栅格点 $s_1$ 后, 可求得的运动基元曲线路径如式 (4) 所示:
$\begin{align} \left\{ {\begin{array}{*{20}{c}} {{x_{{P}}} = {\text{circle}\_x}\left( {{s_0}, {s_1}} \right)} \\ {{y_{{P}}} = {\text{circle}\_y}\left( {{s_0}, {s_1}} \right)} \\ {{\theta _{{P}}} = {\text{circle}\_t}\left( {{s_0}, {s_1}} \right)} \end{array} } \right. \end{align}$
(4) 其中, ${\text{circle_}} *({s_0}, {s_1})$ 是状态 $s_0$ 至状态 $s_1$ 之间的圆弧函数, $\left\{{{x_{{p}}}, {y_{{p}}}, {\theta _{{p}}}} \right\}$ 是运动基元 $P$ 所对应的连续路径.
如图 6所示是起点 $s_0$ 姿态角为0, 将圆周角 $2 \pi$ 等间距分为 $n =16$ 的求解结果示例, 圆周角划分粒度 $n$ 以及栅格边长 $\delta$ 的取值取决于环境障碍物的复杂程度, 以及机器人的结构尺寸和运动跟随精度.由于本文重点解决环境较为简单条件下机器人快速步行的问题, 一般而言, 取 $n = 16$ 能够兼顾未来图搜索的算法效率和规划的通过性.
值得注意的是由于对运动基元第一项假设的约束, 起点状态 $s_0$ 的姿态角为 $2 \pi k/n, k = 1, 2, \cdots$ 的状况, 需要根据式 (3) 重新计算并搜索求解, 而不能通过简单的坐标系旋转实现.由于第一类运动基元内的姿态角和曲线切向一致, 因而显然满足运动基元的假设3), 机器人沿第一类运动基元连续运动将实现圆周运动.
基于上述已有的基本单元和第一类运动基元可快速生成第二类运动基元.其方法是使机器人初始状态的姿态角与其平动轨迹产生夹角, 并在基元曲线上的每一点均保持这一夹角不变, 即
$\begin{align}\left\{ {\begin{array}{*{20}{lllllll}} {{x_{{P}}} = {\text{circle}\_x}\left( {{s_0}, {s_1}} \right)} \\ {{y_{{P}}} = {\text{circle}\_y}\left( {{s_0}, {s_1}} \right)} \\ {{\theta _{{P}}} = {\text{circle}\_t}\left( {{s_0}, {s_1}} \right) + \varphi } \\ \end{array} } \right.\end{align}$
(5) 其中, $\varphi$ 是机器人姿态与路径切线方向的夹角.如图 7左图所示, 相对于基本单元中的前进单元, 当初始状态与未来轨迹存在夹角时, 即可产生同时前进和侧移的运动单元, 而当机器人姿态角与第一类基本单元轨迹存在夹角, 如图 7右图时, 即产生了侧移、旋转的运动基元.从而通过这种系统的运动基元生成方法枚举出全部运动基元.
图 7 由基本单元和第一类单元生成第二类单元示意图Fig. 7 Generation of motion primitives of the second classfrom the basic and the first class上述基本单元, 第一类及第二类运动基元离线生成并以列表形式预存, 以备路径规划时在线调用.
2. 状态网格图搜索算法
人工智能图搜索算法主要分为以动态规划 (Dynamicprogramming)[20]理论为基础的确定性搜索方法和基于概率随机的搜索方法.前者包括Dijkstra、A*、D*等算法及其诸多变种.后者则包括PRM算法[21]、RRT算法[22]及其变种.概率随机搜索算法的优势在于能够快速获得高维空间规划问题的可行解, 但劣势在于难以得到动态规划方法的最优解.而上述网格图状态空间维数有限, 同时考虑到提升机器人步行效率的最终目标, 因而适宜采用确定性搜索方法. A*是基于动态规划的典型图搜索算法, 主要依据式 (6) 的代价函数对遍历节点排序, 引导搜索到达目标:
$\begin{align}f(s) = g({s_{{\text{START}}}}, s) + h(s, {s_{{\text{GOAL}}}})\end{align}$
(6) 其中, $g({s_{{\text{START}}}}, s)$ 是自搜索起始状态到当前状态的代价, $h(s, {s_{{\text{GOAL}}}})$ 是自当前状态到目标状态 ${s_{{\text{GOAL}}}}$ 的启发式估计代价.A*虽然在路径规划领域应用广泛, 但难以满足现实环境下双足步行避障巡航对搜索效率所提出的苛刻要求:一方面环境是动态的, 即障碍物可能处于运动之中, 另一方面是机载传感系统对环境的感知能力有限或路径跟随累计误差导致规划地图信息不断更新, 若搜索算法不能及时处理这些更新的信息, 必将导致机器人停止运动等待规划结果, 从而难以提升其步行效率.提升动态规划效率主要从两方面入手, 其一是损失最优性, 快速获得次优解, 通过后续多轮迭代不断修正而得到最优解, 以ARA*~(Anytime repairing A*，实时修正A*算法)[23]为代表, 其将式 (6) 的搜索引导函数修改为式 (7), 通过不断降低 $\varepsilon$ , 直到 $\varepsilon {\text{ = }}1$ 的多轮迭代避免直接求解最优解.
$\begin{align}& f'(s) = g({s_{{\text{START}}}}, s) + \varepsilon h(s, {s_{{\text{GOAL}}}}), \varepsilon > 1 \notag\\&\quad g'({s_{{\text{START}}}}, {s_{{\text{GOAL}}}}) \leq \varepsilon g({s_{{\text{START}}}}, {s_{{\text{GOAL}}}}) \end{align}$
(7) 其二是充分利用先前规划结果, 只针对发生变化的地图执行增量式迭代优化, 以D*算法 (Dynamic A*, 动态A*算法) 为代表, 而Likhachev等人所提出的AD*算法 (Any-time repairing incrementalsearch, 实时修正增量式搜索算法)[18]则兼备这两种改进特征, 算法如下所示.
实时修正增量式搜索算法.
1) $g({s_{{\text{START}}}}) \leftarrow \infty, g({s_{{\text{GOAL}}}}) \leftarrow \infty $
2) ${\text{rhs}}({s_{{\text{START}}}}) \leftarrow\infty, {\text{rhs}}({s_{{\text{GOAL}}}}) \leftarrow 0, \varepsilon\leftarrow {\varepsilon _0}$
3) 置OPEN、CLOSED、INCONS列表为 $\emptyset$
4) 计算 ${\text{key}}({s_{{\text{GOAL}}}})$ , 插入OPEN列表
5) 调用 $\text{ComputeOrImprovePath}()$
6) 输出 $\varepsilon = {\varepsilon _0}$ 的路径规划结果
7) repeat
8) if环境地图的代价发生变化
9) for all代价变化的节点 $u, v$
10) 更新转移代价 $c(u, v)$
11) 调用 $\text{UpdateState}(u)$
12) end for
13) end if
14) if地图变化超出限制范围
15) 增加 $\varepsilon$ 数值或复位重新规划
16) else if $\varepsilon>1$ then
17) 降低 $\varepsilon$ 数值
18) end if
19) 将INCONS表的节点插入OPEN表
20) 对于所有 $s \in {\text{OPEN}}$ 计算 $\text{key}(s)$ 并排序
21) 将CLOSED列表置为 $\emptyset$
22) 调用 $\text{ComputeOrImprovePath}()$
23) 输出规划结果
24) if $\varepsilon=1$
25) 等待直到环境信息变化
26) end if
27) until收到停止规划指令
28)
29) function UpdateStat $s$
30) if $s$ 从未被访问
31) $g(s) = \infty $
32) end if
33) if $s \ne {s_{{\text{GOAL}}}}$
34) ${\text{rhs}}(s) \leftarrow {\min _{s' \in{\text{Succ}}(s)}}(c(s, s') + g(s'))$
35) end if
36) $s \in \text{OPEN}$
37) 从OPEN集合中移除 $s$
38) end if
39) if $g(s) \ne {\text{rhs}}(s)$ then
40) if $s \notin {\text{CLOSED}}$ then
41) 计算 $key(s)$ 并插入OPEN列表
42) else
43) 将 $s$ 插入INCONS列表
44) end if
45) end if
46) end function
47)
48) function COMPUTEORIMPROVEPATH ()
49) repeat
50) 从OPEN列表中移除最小key的节点 $s$
51) if $g(s) > {\text{rhs}}(s)$ then
52) $g(s) \leftarrow {\text{rhs}}(s)$
53) 将 $s$ 插入CLOSED列表
54) 对于 $s$ 的所有前驱节点 $s'=\operatorname{Pred}(s)$ 调用 ${\text{UpdateState}}(s')$
55) else
56) $g(s) \leftarrow \infty $
57) 对于 $s$ 及其前驱节点 $s'=\operatorname{Pred}(s)$ 调用 ${\text{UpdateState}}(s')$
58) end if
59) until ${\min _{s \in OPEN}}({\text{key}}(s)) \geq{\text{key}}({s_{{\text{START}}}})$ 且 ${\text{rhs}}({s_{{\text{START}}}})= g({s_{{\text{START}}}})$
60) end function
61)
62) function KEY (s)
63) if $g(s) > {\text{rhs}}(s)$
64) return $\{\min [g(s), {\text{rhs}}(s)] + \varepsilon h({s_{{\text{START}}}}, s);$
65) $\min [g(s), {\text{rhs}}(s)]\}$
66) else
67) return $\{ \min [g(s), {\text{rhs}}(s)] +h({s_{{\text{START}}}}, s);$
68) $\min [g(s), {\text{rhs}}(s)]\} $
69) end if
70) end function
实时修正增量式搜索算法中, $s$ 是以 $\left\{ {X, Y, \Theta } \right\}$ 栅格坐标表达的节点状态, $s_\text{GOAL}$ , $s_\text{START}$ 分别是终点和起点节点状态, $c(s, s')$ 是从 $s$ 状态到 $s'$ 状态的转移代价, $g(s)$ 是节点 $s$ 到终点 $s_\text{GOAL}$ 的路径总代价, $h(s_\text{START}, s)$ 是起点 $s_\text{START}$ 到节点 $s$ 的估计总代价, ${\text{Pred}}(s), {\text{Succ}}(s) \in S$ 分别代表 $s$ 的前驱节点和后继节点, $\text{rhs}(s)$ 用于记录环境变化所产生的节点代价变化量, 定义为
$\begin{align}{\text{rhs}}(s) = \left\{ {\begin{array}{*{20}{llllllll}} \!\!\!\!\!&0, \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad {s = {s_{{\text{GOAL}}}}} \\ \!\!\!\!\!&{{{\min }_{s' \in {\text{Succ}}(s)}}(c(s, s') + g(s'))}, \quad {{\mbox{其他}}} \\ \end{array} } \right.\end{align}$
(8) $\varepsilon$ 是式 (7) 中的估计代价膨胀系数, 取 ${\varepsilon _0} > 1$ 作为初值, 从而使得算法行5快速获得次优解, $\varepsilon$ 在算法的14)~18) 行发生变化, 当规划地图变化较小时, 逐步降低 $\varepsilon$ 数值, 从而实现次优解的不断修正, 直到 $\varepsilon=1$ 达到最优解, 而当规划地图变化过大时, 则需要重新增大 $\varepsilon$ , 从而更新次优解. $\varepsilon$ 的初值及增减规律依据经验手动调节获得.这一膨胀系数的存在能够显著加快获得首个可行解的进程, 从而保证算法的实时执行.此外算法在8)~13) 行仅对地图发生变化的部分进行代价更新, 并在19)~23) 行执行增量式迭代, 充分利用了先前规划结果, 从而进一步加快了路径规划的效率, 关于算法的理论证明和其他细节分析参见文献[24].
3. 算法实施细节
第2节和第3节分别给出了Lattice网格图的构造方法以及适用于Lattice网格图的搜索算法, 本节则分析优化影响规划效率和效果的若干关键因素, 包括运动基元集合的选择、启发函数 $h({s_{{\text{START}}}}, s)$ 及状态转移代价 $c(s, s')$ 的确定等, 同时分析路径规划与传统足迹规划的异同.
3.1 运动基元集合的选择
确定运动基元集合的本质是给出算法列表 1中的 $\text{Pred}(s)$ 、 $\text{Succ}(s)$ 两个函数.增加运动基元的种类能够有效提升路径搜索成功概率并更易求得优化的路径, 但同时增加了节点状态 $s$ 的分支数目, 从而降低了算法的执行效率.虽然第2节的方法可以枚举出全方位移动所能实现的全部运动基元, 但其数量过于庞大, 在实际应用中还需依据基元存在的价值进行选择.
在三类运动基元中, 基本单元提供了缺省的运动形式而必不可少, 因而甄别选择主要针对第一类单元和其对应的第二类单元.如图 6所示, 当运动基元的终止角度从0°逐步增加到90°时, 平动位移始终位于几个栅格点之内, 从而可能由"旋转--直行--旋转"的基本单元组合运动而替代.事实上, 各运动基元的实际效率取决于栅格物理尺寸, 机器人全方位移动效率等因素, 设完成运动基元的平均时间为 $t_{P}$ , 基本单元组合完成相同运动的时间为 $t_{B}$ , 则有:
$\begin{align}& {t_{{P}}} = \frac{L_{{P}}}{v_{{P}}} \notag\\&{t_{{B}}} =\frac{\Delta \theta}{\omega _{{B}}} + \frac{\sqrt{\Delta {x^2} + \Delta {y^2}}}{v_{{B}}} \end{align}$
(9) 其中, $L_{P}$ 是运动基元的弧长, $v_{P}$ 是仿人机器人完成运动基元的步速, $\left( {\Delta x, \Delta y, \Delta \theta } \right)$ 是图 6所示的基元尺寸, $\omega_{{B}}$ 和 $v_{{B}}$ 分别是机器人原地旋转的角速度和直线前进的速度, 仅当 ${t_{{B}}}/{t_{{P}}}$ 较大时, 该运动基元才具有加入集合的价值.
如图 8所示是一种采用上述基准选择的运动基元集合.其中标号为1, 3, 8, 9, 10, 11的分别是前进、后退、左转、右转、左移、右移的基本单元, 标号为4, 5, 6, 7的是第一类单元, 实现前进同时旋转, 标号为12, 13, 14, 15是第二类单元, 实现侧移同时旋转.
3.2 启发函数的确定
启发函数 $h({s_{{\text{START}}}}, s)$ 表示从起始状态 $s_\text{START}$ 到当前状态 $s$ 的估计代价, 是AD*搜索算法效率的关键影响因素, 采用 $s_\text{START}$ 与 $s$ 的欧氏距离是最为直观的一种启发函数式, 但在狭窄通道或障碍物对称的情况下易产生路径的往复震荡而导致最终规划失败.
为了获得更为精确的启发函数, 可将机器人简化为点机器人, 在Lattice网格图搜索前, 以Dijkstra算法自 $s_\text{START}$ 开始, 执行一次2D栅格搜索, 并记录每一个节点的实际代价, 从而得到：
$\begin{align}h({s_{{\text{START}}}}, s) = {g_{{\text{2D}}}}\left({{s_{{\text{START}}}}(x, y), s(x, y)} \right)\end{align}$
(10) 式中 $s(x, y)$ 是状态节点 $s$ 的位置, 该启发函数本质上已经给出了粗略的规划结果, 仅当 $h({s_{{\text{START}}}}, {s_{{\text{GOAL}}}}) \ne \infty $ , 才执行后续的AD*搜索.这种启发代价解决了欧氏距离方法的困境, 同时2D环境下Dijkstra算法所增加的计算量尚能接受.
3.3 状态转移代价的确定
Lattice网格图搜索基于动态规划理论, 其最终目标是求解式 (11) 所示的最小化问题, 因此任意两个节点间的状态转移代价 $c(s, s')$ 决定了最优解的形态.
$ \begin{align}\begin{array}{*{20}{c}} {\min } & {{g} \left( {s, {s_{{\text{GOAL}}}}} \right)} \\ \end{array} = {{c}}(s, s') + g\left( {s', {s_{{\text{GOAL}}}}} \right)\end{align}$
(11) 为了获得时间最优的路径, 以运动基元的时间消耗表达状态间的转移代价, 但仿人机器人快速高频步行时, 可能无法在一步内完成步幅的过渡[13], 因此还需引入运动基元间的过渡时间代价.如图 9所示路径片段中, 第1段路径内机器人直线步行, 抵达 $a$ 节点后需要转换为左移执行第2段路径, 则在节点 $a$ 处, 机器人需将前进步幅降为零, 并逐步增加侧移步幅; 同理, 在第2段侧移路径和第3段侧移旋转路径之间的 $b$ 点, 机器人需降低侧移步幅, 并逐步加入旋转步幅.显然, 上述两段路径在节点处的过渡代价不同, 只有较为精细的评估这一代价, 才能引导搜索算法生成光滑的、易于快速全方位步行追踪的路径.
图 9 运动基元代价及步幅过渡代价示意图Fig. 9 Illustration of the motion primitive cost and statetransferring cost首先估计单个运动基元的时间代价:根据运动基元生成的第3项假设, 同类型的运动基元满足前后连接的光滑可导特性, 因此可以转化为一个给定全方位前进步幅 $\chi$ 、侧移步幅 $\gamma$ 和旋转步幅 $\varphi$ 的圆周 (或直线) 运动.因此, 对每一个运动基元 $p_m$ , 采用 $u$ 段运动基元连接形成一条圆周路径记为 $P_u$ , 在固定步频条件下, 运动基元的时间代价就转化为完成这一圆周路径的步伐数 $t$ , 则运动基元平均代价可以表示为
$\begin{align}{\bar c_m} = \mathop {\lim }\limits_{u \to \infty } \frac{t} {u}\end{align}$
(12) 针对每一个运动基元进行步行实验, 可构成代价向量 ${{{C}}_{{\text{state}}}} = {\left( {{{\bar c}_1}, {{\bar c}_2}, {{\bar c}_3}, \cdots, {{\bar c}_p}} \right)^{\text{T}}}$ .
第二步, 估计步幅状态过渡代价:采用足够长的 $u_m$ 和 $u_n$ 段运动基元并连接成为路径 $P_{mn}$ , 从而估计运动基元 $p_m$ 和 $p_n$ 的过渡代价, 若总步伐数为 $t_{mn}$ , 则过渡代价可表示为
$\begin{align}{c_{mn}} = \mathop {\lim }\limits_{m \to \infty, n \to \infty }({t_{mn}} - {u_m}{c_m} - {u_n}{c_n})\end{align}$
(13) 针对任意两个运动基元进行快速路径跟随实验, 进而构成状态转移代价矩阵:
$\begin{align}{{{C}}_{{\text{trans}}}} = {\left[{\begin{array}{*{20}{c}} 0 & {{c_{12}}} & {\cdots} & {{c_{1, q}}} \\ {{c_{21}}} & 0 & {\cdots} & {{c_{2, q}}} \\ {\vdots} & {\vdots} & {\ddots} & {\vdots} \\ {{c_{q, 1}}} & {{c_{q, 2}}} & {\cdots} & 0 \\ \end{array} } \right]_{q \times q}}\end{align}$
(14) 为有效利用代价矩阵 ${{{C}}_{{\text{trans}}}}$ 产生平滑的路径, 状态空间需从三维 $(x, y, \theta)$ 拓展到四维 $(x, y, \theta, v)$ ，其中 $v$ 用以描述前驱节点的运动基元ID.尽管这一拓展将增加搜索算法的计算量, 但由于运动基元的数量仅有十几个, 由此产生额外的存储空间的变化和计算时间仍然能够接受.
如图 10所示是忽略式 (14) 的过渡代价以及引入基元过渡代价的路径规划结果对比.
图 10 忽略与考虑运动基元过渡代价的规划结果对比Fig. 10 Comparison of planning results with ignoring and usingmotion primitive transferring cost可见, 考虑运动基元过渡代价能够生成大段的平直路径, 易于路径跟随算法生成较大的步幅.相反, 不考虑到运动基元间的过渡代价, 尽管生成路径长度较短但较为曲折, 导致路径跟随效率难以提高.
3.4 与传统足迹规划思想的对比
上述路径规划算法与传统的足迹规划均实现了仿人机器人的避障巡航的目的, 两者存在相似点也存在较大差异.根据定义, Lattice网格图的组成单元是运动基元, 最终生成的路径是各运动基元在规划空间上的连接拓展, 而传统足迹规划方法最终形成的足迹序列亦可视作可行落地足的空间延续, 对搜索算法而言, 可行落地足位置可归纳为一种特殊的运动基元, 因此所采用的搜索算法本质上可以互换.
然而足迹集合和运动基元存在连续性的差异, 运动基元是连续的, 而可行落地足迹是离散的.如图 11所示, 在生成相同髋部运动轨迹的条件下, 路径规划完成后再确定足迹的放置位置将给予后续算法进一步优化足迹序列的机会, 能够在平直的路径段内将多个原有足迹组合成为一个足迹, 实现了"多步并做一步走"; 而在尖锐转角位置, 提前减速并逐步增加旋转步幅, 实现了"一步拆分多步走", 从而同时提升了步行效率和稳定性.而离散的足迹规划却不具备这种潜力, 机器人需严格遵照落地足位置作为其行动序列, 因而难于可靠的提升机器人步速和步频.
图 11 路径规划与足迹规划的结果对比Fig. 11 Comparison of result with path planning and footstepplanning运动基元和可行落地足迹的另一差异是对地图分辨率的需求.由于足迹集合中的任意一个足迹必须位于机器人的腿部工作空间内, 由此可知, 足迹规划中对规划环境的最大网格粒度不能超出机器人的最大步幅, 事实上为了提高足迹规划的完备性, 这一网格粒度需达到最大步幅的1/5或更小.相比之下运动基元所描述的是拓扑关系, 其构造方法仅与栅格的空间相对位置有关, 而与栅格的具体尺寸无关, 因此不受机器人腿部工作空间的限制, 而是可以根据环境复杂程度动态的设定其栅格粒度.
另一方面, Lattice网格图的运动基元目前还难以处理机器人跨过或踩上障碍物的情形, 而在环境障碍物分布较为复杂的情况下, Lattice网格路径规划的优势则不再明显, 足迹规划的诸多灵活性使之转而占据优势.这一结果符合用于快速高步频步行、规划范围较大, 但复杂度较低条件下的运动规划的预期.
4. 实验结果和讨论
分别采用计算机仿真和样机实验方法证明上述路径规划方法的有效性.仿真计算硬件为Intel Core i3 2 348 MHz的四核心处理器, 操作系统为Ubuntu 10.04并安装在Vmware虚拟机上, 限制其内存为1.2 GB, 其中AD*算法基于ROS (electric版) 平台下的SBPL (Search based planninglibrary) 算法库[25]实现, 限定算法仅在单一CPU核心上运行.如图 12所示是几种典型环境下的路径规划结果, 其中还给出了全方位步行追踪路径的结果.
图 12 特定环境下的路径规划和步行跟随结果Fig. 12 Path planning and path following results in the specified environments在图 12的静态环境下, 算法能够成功的给出规划路径, 能够适应狭窄通道等较为复杂的环境, 所生成的路径平滑, 易于仿人机器人足迹跟随.
采用样机MOS-Strong验证算法在实际机器人上整体部署运行的可能性, 该样机为自主决策小型仿人机器人, 样机外形及其分层控制系统原理如图 13所示.
图 13 MOS-Strong仿人机器人外形及控制系统原理图Fig. 13 Appearance of MOS-Strong humanoid robot and theschematic of its control system其中决策控制器用于机器视觉处理、环境建模、路径规划及足迹生成, 运动控制器实现全方位步态规划算法[13], 本文所述的运动规划算法模块与处理器对应关系如图 14所示.
Robocup足球竞赛涵盖了机器人自主决策和运动所遇到的各种关键问题, 可以作为检验运动规划算法效率和效果的有效手段.本文选择避障竞速技术挑战赛为测试验证环境:机器人的任务是避开代表其他球员的六个随机放置的黑色柱子, 并以最快的速度步行进入球门, 最终以消耗最短时间者为冠军, 该问题凸显机器人运动规划的各项特征, 是一个代表性问题.
机器人采用头部单目视觉系统识别黑色柱子, 以边线和球门识别结果自定位, 除此之外不具备任何获得环境信息的途径.由于黑色柱子远高于机器人, 且相互间存在遮挡, 使机器人难以通过一次观测确定全局环境.同时障碍物会干扰机器人自定位系统, 因此, 机器人在步行期间需不断通过观测数据更新环境信息, 步行运动规划条件处于实时变化状态.
如图 15所示是完成挑战赛任务的连拍图像, 当机器人观测到新的障碍物即更新环境信息, 并调用规划算法模块, 在先前规划的基础上修正路径, 同时以最快的步行速度跟随该路径, 完成避障任务.
图 15 仿人机器人在Robocup避障竞速中的连拍照片Fig. 15 Snapshots of the humanoid robot in the obstacleavoidance challenge of Robocup如图 16所示是机器人步行期间对环境认知的更新结果, 以及算法生成的新路径及足迹序列, 单步周期380 ms, 根据足迹个数推算出的运行时间约为42 s, 实际运行过程中则耗时48 s, 是由于视觉系统识别障碍物偏差出现机器人若干次原地踏步所导致.如图 17是机器人实时在线生成的步幅指令参数, 自起步到加速、转弯均匀过渡, 因而能够在保持步行稳定的同时实现路径的快速跟随.
图 16 机器人在Robocup避障竞速中步行与环境感知重现Fig. 16 Recovery of walking steps and environment perceptionof the robot during the obstacle avoidance challenge in Robocup5. 结论
本文采用全方位Lattice网格图构造方法和实时修正增量式AD*搜索算法实现了仿人机器人在障碍物环境下的路径规划, 通过计算机仿真和样机实验证实了方法的整体有效性.
本文基于三项基本假设给出了系统化的全方位运动基元枚举方法, 实现了Lattice网格图从轮式车辆到全方位移动平台的拓展.通过分析和选择高效的运动基元集合优化了算法的在线执行效率; 通过2D栅格规划作为启发式函数以及状态转移代价的精确估计等方法, 给出了平滑高效的规划结果.所采用的AD*确定性搜索算法能够兼顾在线执行的快速性、最终路径的最优性、环境的动态性以及机器人运动误差.相较传统足迹规划, 本文的方法能够显著增强机器人在快速高步频步行、规划范围较大但复杂度较低条件下的运动规划能力.
虽然所提出的方法成功地给出了几何路径规划结果, 但若考虑对传统足迹规划方法在步行效率方面的提升对比, 则还需深入研究仿人机器人合理安排足迹点, 快速跟随这一几何路径的方法, 使两套算法协同工作, 从而提升机器人的整体巡航效率.同时, 本文所提出的方法是否能够拓展到跨越或踩上障碍物, 或者实现与传统足迹规划方法的综合优化和协同工作, 也值得开展进一步研究.此外, 路径规划只是机器人自主巡航问题中的一个环节, 其需要与环境感知定位以及运动跟随控制两者协同工作, 尽管本文所采用的AD*搜索算法考虑到了环境感知误差以及运动误差存在条件下的重新规划问题, 但三者之间的综合优化和时序管理仍有待进一步研究.
-
图 1 基于栅格地图规划及曲线路径规划的对比
Fig. 1 Comparison of grid map based planning and curved pathplanning
图 3 基于Lattice网格图的路径规划原理图
Fig. 3 Illustration of the state lattice graph based pathplanning
图 4 前进、侧移、旋转一个栅格单位的基本运动单元
Fig. 4 Basic motion primitives including forward walking, sidling and self-spin for one unit
图 5 始末姿态角关系与光滑路径生成示意图
Fig. 5 Illustration of the relationship between the start-endattitude angle and the smooth path generation
图 7 由基本单元和第一类单元生成第二类单元示意图
Fig. 7 Generation of motion primitives of the second classfrom the basic and the first class
图 9 运动基元代价及步幅过渡代价示意图
Fig. 9 Illustration of the motion primitive cost and statetransferring cost
图 10 忽略与考虑运动基元过渡代价的规划结果对比
Fig. 10 Comparison of planning results with ignoring and usingmotion primitive transferring cost
图 11 路径规划与足迹规划的结果对比
Fig. 11 Comparison of result with path planning and footstepplanning
图 12 特定环境下的路径规划和步行跟随结果
Fig. 12 Path planning and path following results in the specified environments
图 13 MOS-Strong仿人机器人外形及控制系统原理图
Fig. 13 Appearance of MOS-Strong humanoid robot and theschematic of its control system
图 15 仿人机器人在Robocup避障竞速中的连拍照片
Fig. 15 Snapshots of the humanoid robot in the obstacleavoidance challenge of Robocup
图 16 机器人在Robocup避障竞速中步行与环境感知重现
Fig. 16 Recovery of walking steps and environment perceptionof the robot during the obstacle avoidance challenge in Robocup
-
[1] Spong M W, Hutchinson S, Vidyasagar M. Robot Modeling and Control. Hoboken, NJ, USA:John Wiley and Sons, 2006. [2] Latombe J C. Robot Motion Planning. Berlin, Germany:Springer, 1991. [3] Choset H, Lynch K M, Hutchinson S, Kantor G A, Burgard W, Kavraki L E, Thrun S. Principles of Robot Motion:Theory, Algorithms, and Implementations. Cambridge, UK:A Bradford Book, 2005. [4] Lavalle S M. Planning Algorithms. Cambridge, USA:Cambridge University Press, 2006. [5] Xia Z Y, Xiong J, Chen K. Global navigation for humanoid robots using sampling-based footstep planners. IEEE/ASME Transactions on Mechatronics, 2011, 16(4):716-723 doi: 10.1109/TMECH.2010.2051679 [6] Hornung A, Maier D, Bennewitz M. Search-based footstep planning. In:Proceedings of the 2013 ICRA Workshop on Progress and Open Problems in Motion Planning and Navigation for Humanoids. Karlsruhe, Germany:IEEE, 2013. [7] Xia Z Y, Chen G D, Xiong J, Zhao Q F, Chen K. A random sampling-based approach to goal-directed footstep planning for humanoid robots. In:Proceedings of the 2009 IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Singapore:IEEE, 2009. 168-173 [8] Xia Z Y, Xiong J, Chen K. Parameter self-adaptation in biped navigation employing nonuniform randomized footstep planner. Robotica, 2010, 28(6):929-936 doi: 10.1017/S0263574709990804 [9] Liu H, Sun Q, Zhang T W. Hierarchical RRT for humanoid robot footstep planning with multiple constraints in complex environments. In:Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vilamoura:IEEE, 2012. 3187-3194 [10] Huang W W, Kim J, Atkeson C G. Energy-based optimal step planning for humanoids. In:Proceedings of the 2013 IEEE International Conference on Robotics and Automation. Karlsruhe:IEEE, 2013. 3124-3129 [11] Perrin N, Stasse O, Baudouin L, Lamiraux F, Yoshida E. Fast humanoid robot collision-free footstep planning using swept volume approximations. IEEE Transactions on Robotics, 2012, 28(2):427-439 doi: 10.1109/TRO.2011.2172152 [12] Maier D, Lutz C, Bennewitz M. Integrated perception, mapping, and footstep planning for humanoid navigation among 3D obstacles. In:Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Tokyo:IEEE, 2013. 2658-2664 [13] 张继文, 刘莉, 李昌硕, 陈恳.仿人机器人参数化全方位步态规划方法.机器人, 2014, 36(2):210-217 http://www.cnki.com.cn/Article/CJFDTOTAL-JQRR201402011.htmZhang Ji-Wen, Liu Li, Li Chang-Shuo, Chen Ken. Parametric Omni-directional gait planning of humanoid Robots. Robot, 2014, 36(2):210-217 http://www.cnki.com.cn/Article/CJFDTOTAL-JQRR201402011.htm [14] Yoshida E, Esteves C, Belousov I, Laumond J P, Sakaguchi T, Yokoi K. Planning 3-D collision-free dynamic robotic motion through iterative reshaping. IEEE Transactions on Robotics, 2008, 24(5):1186-1198 doi: 10.1109/TRO.2008.2002312 [15] Hornung A, Bennewitz M. Adaptive level-of-detail planning for efficient humanoid navigation. In:Proceedings of the 2012 IEEE International Conference on Robotics and Automation. Saint Paul:IEEE, 2012. 997-1002 [16] Sprunk C, Lau B, Pfaffz P, Burgard W. Online generation of kinodynamic trajectories for non-circular omnidirectional robots. In:Proceedings of the 2011 IEEE International Conference on Robotics and Automation. Shanghai, China:IEEE, 2011. 72-77 [17] Pivtoraiko M, Knepper R A, Kelly A. Differentially constrained mobile robot motion planning in state lattices. Journal of Field Robotics, 2009, 26(3):308-333 doi: 10.1002/rob.v26:3 [18] Likhachev M, Ferguson D. Planning long dynamically feasible maneuvers for autonomous vehicles. The International Journal of Robotics Research, 2009, 28(8):933-945 doi: 10.1177/0278364909340445 [19] 张浩杰, 龚建伟, 姜岩, 熊光明, 陈慧岩.基于变维度状态空间的增量启发式路径规划方法研究.自动化学报, 2013, 39(10):1602-1610 doi: 10.3724/SP.J.1004.2013.01602Zhang Hao-Jie, Gong Jian-Wei, Jiang Yan, Xiong Guang-Min, Chen Hui-Yan. Research on incremental heuristic path planner with variable dimensional state space. Acta Automatica Sinica, 2013, 39(10):1602-1610 doi: 10.3724/SP.J.1004.2013.01602 [20] Bertsekas D P. Dynamic Programming and Optimal Control (Third edition). Belmont, Mass:Athena Scientific, 2005. [21] Kavraki L E, Svestka P, Latombe J C, Overmars M H. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 1996, 12(4):566-580 doi: 10.1109/70.508439 [22] Lavalle S M. Randomized kinodynamic planning. The International Journal of Robotics Research, 2001, 20(5):378-400 doi: 10.1177/02783640122067453 [23] Likhachev M, Gordon G J, Thrun S. ARA*:anytime A* with provable bounds on sub-optimality. In:Advances in Neural Information Processing Systems. Massachusetts:MIT Press, 2003. 767-774 [24] Likhachev M, Ferguson D, Gordon G, Stentz A, Thrun S. Anytime search in dynamic graphs. Artificial Intelligence, 2008, 172(14):1613-1643 doi: 10.1016/j.artint.2007.11.009 [25] Likhachev M. Sbpl ROS wiki[Online], available:http://wiki.ros.org/sbpl, June 30, 2015. 期刊类型引用(7)
1. 高双，柳春平，张瞳，陈坤. 可行状态包络下欠驱动水面船全时可跟踪轨迹规划方法. 船舶工程. 2022(S1): 459-466 . 
百度学术2. 刘景森，吉宏远，李煜. 基于改进蝙蝠算法和三次样条插值的机器人路径规划. 自动化学报. 2021(07): 1710-1719 . 本站查看3. 何杰，张晓晖，肖晓晖，丁加涛. 基于三质点模型的仿人机器人节能步行. 中南大学学报(自然科学版). 2020(12): 3396-3406 . 百度学术4. 孙永超，高丙朋，樊小朝，刘庆飞. 基于人工流场的移动机器人路径规划方法研究. 现代电子技术. 2019(18): 182-186 . 百度学术5. 丁加涛，何杰，李林芷，肖晓晖. 基于模型预测控制的仿人机器人实时步态优化. 浙江大学学报(工学版). 2019(10): 1843-1851 . 百度学术6. 王天柱，吴正兴，喻俊志，谭民，张峰. 冰雪运动生物力学及其机器人研究进展. 自动化学报. 2019(09): 1620-1636 . 本站查看7. 崔宝侠，王淼弛，段勇. 基于可搜索24邻域的A~*算法路径规划. 沈阳工业大学学报. 2018(02): 180-184 . 百度学术其他类型引用(12)
-
下载:
下载:
计量
- 文章访问数: 2032
- HTML全文浏览量: 350
- PDF下载量: 1834
- 被引次数: 19
