动态二叉树表示环境的A<sup>*</sup>算法及其在足球机器人路径规划中的实现

摘要

提出采用二叉树表示二维空间的方法，对全局路径规划和局部路径规划进行综合考虑，设计移动机器人在复杂环境下对动态障碍物进行避障的A^*算法。在足球机器人系统中进行仿真，将二叉树动态地表示球场的机器人与目标为对角线的矩型环境，使搜索范围随搜索进程动态地减小，实现了路径规划的整体优化。

正文

《1 引言》

1 引言

对移动机器人的路径规划研究, 近年来已发展许多新技术, 包括神经网络 (NN) , 进化算法 (GA) 和模糊控制等。对静态障碍物的路径规划已取得较好的解决方法, 而对动态障碍的避障, 仍然是路径规划中的难题, 文献[1,2,3]中提出用带有时标的四叉树表示环境, 对二维空间轴作递归分解, 对时间轴引入匀速度的概念, 试图解决在具有动态障碍物环境下移动机器人的路径规划问题。但当智能机器人的工作环境较大时, 结点分裂的增多使表示环境模型的四叉树过于庞大, 从而增加了存储空间和计算时间, 不同程度地扩大了搜索空间, 不能满足动态环境下智能机器人路径规划的实时性。笔者提出采用二叉树表示二维空间的方法, 利用二叉树遍历方法, 设计移动机器人在复杂环境下对动态障碍物进行避障的A^*算法^[4]。在足球机器人系统中进行仿真, 将二叉树动态地表示球场的机器人与目标为对角线的矩形环境, 使搜索范围随搜索进程动态地减少。采用带有时标的动态二叉树表示方法, 时间信息中增加了加速度, 即考虑了相对的全局路径规划, 又考虑了局部路径规划的智能控制, 从整体上减少了搜索时间, 提高了实时性, 实现了路径规划的整体优化。

《2 动态二叉树表示二维空间方法》

2 动态二叉树表示二维空间方法

二叉树是树结构中最简单的一种结构^[5]。在复杂环境下, 障碍物是动态的。当某时刻动态障碍物处于已知环境中, 将环境简化为一个矩形, 根据障碍物的大小简化为一个矩形或多个矩形的叠加。

设矩形环境E的参数结构为{0, X, 0, Y}。将矩形环境E进行K层二分递归。根结点表示整个矩形环境平面, 根结点的二个子结点表示矩形环境平面上等分的二个子区间, 依此类推, 得到了二维空间的二叉树的表示。其中, K为二叉树的深度, 最小的叶结点为一个矩形障碍物的大小。

定义1 结点n表示某矩形区域内无障碍物, 称结点n为可规划结点A。

定义2 结点n表示某矩形区域内存在障碍物, 称结点n为可转换结点B。

定义3 结点n表示某矩形区域是一个障碍物, 称结点n为不可规划结点C。

定义4 设环境E={0, X, 0, Y}, 其空间面积为S_E, 则任意结点n的空间面积

$S_{n} = S_{E} / 2^{i} ‚$ $S_{n} = S_{E} / 2^{i} ‚$

其中i为结点n在二叉树中的层数, 1≤i≤K, K为二叉树的深度。

二叉树的最多结点为2^K-1, 由定义4可知, K的值取决于最大障碍物的矩形空间S_o, 也就是叶结点的空间, 即:S_o=S_E/ 2^K,

$Κ = L n (S_{E} / S_{o}) 。$ $Κ = L n (S_{E} / S_{o}) 。$

当K=2, 4, 6, … 时, 其结点的分裂是在X轴进行的;

当K=3, 5, 7, … 时, 其结点的分裂是在Y轴进行的。

设矩形障碍物是一个四元组O_s={x_o1, x_o2, y_o1, y_o2}, 表示障碍物的数据结构

$\begin{array}{l} Ο = {(x_{o 1} ‚ x_{o 2}) ‚ (y_{o 1} ‚ y_{o 2}), (v_{o x} ‚ \\ v_{o y}) ‚ (a_{o x}, a_{o y})} 。 \end{array}$ $\begin{array}{l} Ο = {(x_{o 1} ‚ x_{o 2}) ‚ (y_{o 1} ‚ y_{o 2}), (v_{o x} ‚ \\ v_{o y}) ‚ (a_{o x}, a_{o y})} 。 \end{array}$

其中:x_o1表示其左边界的x坐标, x_o2表示其右边界的x坐标, y_o1表示其下边界的y坐标, y_o2表示其上边界的y坐标, v_ox表示其速度的x分量, v_oy表示其速度的y分量, a_ox表示其加速度的x分量, a_oy表示其加速度的y分量。

设结点i是一个四元组n_i= (x_i1, x_i2, y_i1, y_i2) , 其数据结构表示为

${x_{i 1} ‚ x_{i 2} ‚ y_{i 1} ‚ y_{i 2}, l l i n k, r l i n k, f l a g} 。$ ${x_{i 1} ‚ x_{i 2} ‚ y_{i 1} ‚ y_{i 2}, l l i n k, r l i n k, f l a g} 。$

其中:x_i1为结点i在t时刻的左边界x坐标, x_i2为结点i在t时刻的右边界x坐标, y_i1为结点i在t时刻的下边界y坐标, y_i2为结点i在t时刻的上边界y坐标。llink为指向左邻子结点的指针, rlink为指向右邻子结点的指针,

$f l a g = {\begin{cases} 1 \\ 0 \\ - 1 \end{cases} \begin{array}{l} 结点 i 为 A ‚ \\ 结点 i 为 B ‚ \\ 结点 i 为 C 。 \end{array}$ $f l a g = {\begin{cases} 1 \\ 0 \\ - 1 \end{cases} \begin{array}{l} 结点 i 为 A ‚ \\ 结点 i 为 B ‚ \\ 结点 i 为 C 。 \end{array}$

很明显, 当矩形环境中没有障碍物时, 表示环境的二叉树只有一个可规划结点A结点:当矩形环境中充满障碍物时, 表示环境的二叉树只有一个不可规划结点C结点;其他情况, 表示环境的二叉树点数都小于等于2^K-1。

根据以上定义, 把机器人和目标的连线, 作为当前考虑的环境模型的矩形对角线。对此环境建立一棵二叉树。当搜索一个时间周期后, 机器人的位置发生了变化, 搜索空间变小 (除非矩形环境中充满障碍物) , 必须重新建立一棵新的二叉树。依此类推, 随着机器人位置的变化, 矩形环境变小, 而表示矩形环境的动态二叉树也越来越小。这样, 把一个类似于栅格法描述全局路径规划的二叉树表示环境方法, 演变成既考虑全局路径规划, 又考虑了局部路径规划的动态二叉树表示环境模型。

当障碍是静止状态时, 即障碍物的速度v_ox, v_oy, 加速度a_ox, a_oy均为零, 其路径规划只需对此二叉树进行遍历。例如按层次遍历二叉树对flag进行判断, 当flag=-1时, 放弃此结点 (即可绕过障碍物) , 继续遍历其它结点, 从而找到目标。

《3 动态二叉树表示动态环境的A*算法》

3 动态二叉树表示动态环境的A*算法

动态二叉树表示的环境模型, 当障碍物是动态时, 其环境模型也是动态的。当遍历二叉树时, 由于障碍的移入, 原来的A结点可能会变成B结点, 从而可二分递归分解为A, B, C三类结点, 原来是B结点, 可能会由于障碍物的移出, 而变为A结点。所以移动机器人的路径取决于机器人和障碍物对节点n的访问时间, 而访问时间的计算由它们各自的速度、加速度及机器人、障碍物与节点n的直线距离计算得到。

定义5 对结点i, i∈A结点集, 障碍O_i的移入时间, 称对结点i的访问时间, 记为

$V_{t} (i, Ο_{i}) = \underset{i \in A}{\cup} t (i, Ο_{i}),$ $V_{t} (i, Ο_{i}) = \underset{i \in A}{\cup} t (i, Ο_{i}),$

其中t (i, O_i) 是某个障碍物对结点i的访问时间, 而V_t (i, O_i) 是多个障碍物对结点i访问时间中最短的一个访问时间。

定义6 对结点i, i∈A ∪ B集合, 移动机器人R的移入时间称对结点i的访问时间, 记为

$V_{t} (i, R) = \underset{i \in A o r B}{\cup} t (i, R),$ $V_{t} (i, R) = \underset{i \in A o r B}{\cup} t (i, R),$

其中t (i, R_i) 是某个移动机器人对结点i的访问时间, 而V_t (i, R_i) 是多个移动机器人对结点i访问时间中最短的一个访问时间。

访问时间的计算描述如下:

设ΔL_x, ΔL_y分别为障碍物O_i在t时间内运行在X轴和Y轴上的距离坐标分量, 当速度v_o≠0, 加速度为a_o时, 则访问时间t (i, O_i) 可由下式得到:

$\begin{array}{l} Δ L_{x} = V_{o x} t_{o x} + a_{o x} t_{o x}^{2} / 2 ‚ \\ Δ L_{y} = V_{o y} t_{o y} + a_{o y} t_{o y}^{2} / 2 。 \end{array}$ $\begin{array}{l} Δ L_{x} = V_{o x} t_{o x} + a_{o x} t_{o x}^{2} / 2 ‚ \\ Δ L_{y} = V_{o y} t_{o y} + a_{o y} t_{o y}^{2} / 2 。 \end{array}$

其中:ΔL_x=x_n1-x_o1, 表示障碍O_i在结点i的左边;ΔL_x=x_o2-x_n2, 表示障碍O_i在结点i的右边;且

y_n1≤y_o2≤y_n2 or y_n1≤y_o1≤y_n2 and v_ox≠0。

ΔL_y=y_n1-y_o1, 表示障碍O_i在结点i的下边;ΔL_y=y_o2-y_n2, 表示障碍O_i在结点i的上边;且

x_n1≤x_o2≤x_n2 or x_n1≤x_o1≤x_n2 and v_oy≠0。

同理, 设ΔJ_x, ΔJ_y分别为机器人R在t时间内运行在X轴和Y轴上的距离坐标分量, 当速度v_R≠0, 加速度为a_R时, 根据下式求得机器人R访问i结点的时间t:

$\begin{array}{l} Δ J_{x} = v_{R x} t_{R x} + a_{R x} t_{R x}^{2} / 2 ； \\ Δ J_{y} = v_{o y} t_{R y} + a_{R y} t_{R y}^{2} / 2 。 \end{array}$ $\begin{array}{l} Δ J_{x} = v_{R x} t_{R x} + a_{R x} t_{R x}^{2} / 2 ； \\ Δ J_{y} = v_{o y} t_{R y} + a_{R y} t_{R y}^{2} / 2 。 \end{array}$

定义7 设f=g (n) +h (n) 为估计函数, 其中:g (n) 为移动机器人R从初始结点S到n结点的时间估计, h (n) 为移动机器人R从n结点到目标结点G的时间估计。

在动态二叉树表示环境模型的基础上, 对二叉树进行遍历, 筛选掉某时刻具有障碍物的结点C, 利用人工智能启发式搜索算法A^*算法, 经过计算机器人对当前结点n的访问时间, 障碍物对当前结点n的访问时间, 并进行比较, 决定结点n的A, B, C属性。然后进行估价函数的计算, 利用估价函数进行最优路径规划, 设机器人的当前位置为A₁, 目标位置为G, A^*算法如下:

Step 1 设close=∅, open={A₁}, f (s) =h (A₁) +g (A₁) , g (A₁) =0。

Step 2 建立表示以A₁与G为对角线的矩形环境的二叉树, 如果A₁=G, 则成功。

Step 3 遍历二叉树, 将flag=1的结点移入open表中。

Step 4 计算open表中结点的f ( ) 值。

Step 5 在open表中选f ( ) 之最小的一个结点n, open=open-{n}, close=close+{n}。

Step 6 若V_t (n, R) <αV_t (n, O_t) , 则n为路径规划结点, 令A₁= (n的当前位置) , open={A₁}, g (A₁) =0, 转step 2;否则:若S_n≤2S_E/ 2^K, 则放弃n结点, close=close-{n}, 转Step 4;否则, 递归二分结点n, 并将flag=1的后继结点移入open表中, 转Step 4。

算法中α为访问时间的权值, 一般取α≥2。

《4 仿真及结果》

4 仿真及结果

足球机器人系统中的足球机器人的路径规划是一个复杂环境下对动态障碍的避障过程。进行路径规划时, 除了本足球机器人以外, 认为其他足球机器人都是障碍物, 而且是动态障碍物。由于其球场是一个150 mm×130 mm的二维平面, 且足球机器人的速度和加速度已知, 把此算法灵活地应用到足球机器人的路径规划中, 将任何一个机器人A₁与目标G₁的连线, 作为一个动态环境矩形E₁的对角线。对此矩形建立一个动态二叉树。图1为动态环境E₁, 图1a表示具有3个障碍物的动态环境E₁, 图1b为二叉树表示的动态环境E₁。建立动态二叉树算法的流程图如图2所示。

《图1》

图1 动态环境E₁

Fig.1 Dynamic environment E₁

《图2》

图2 建立动态二叉树的流程图

Fig.2 The flow chart of dynamic binary tree

在仿真算法中, 将球场视为C-空间环境, 机器人A₁、目标G₁和障碍均视为一个点, 在一个时间周期内, 任何一个机器人和目标均可构成动态矩形环境, 都用一棵动态二叉树表示, 采用二叉树表示环境的A^*算法, 估价函数为:f (n) =g (n) +h (n) , 其中:g (n) 为机器人R对结点n的访问时间, h (n) 为机器人R从结点n到目标G的时间。

该算法与栅格法相比, 既考虑了全局路径规划, 又考虑了局部路径规划, 是栅格法的一种动态改进。与四叉树表示环境的算法相比, 结点分裂明显减少。在Mirosot平台的仿真中, 因为它是递归算法, 算法的大部分时间都花在建立动态二叉树。动态二叉树的大小决定机器人思考的速度, 同时, 当障碍物与机器人的距离大于4r (r是机器人的平面直径) 时, 碰撞的几率为零, 但当障碍物与机器人的距离小于4r时, 机器人必须加速。当机器人多于3个并且障碍物与机器人的距离小于4r时, 可能产生碰撞。

《5 结语》

5 结语

移动机器人的动态避障是一个智能搜索问题, 其中环境建模技术在智能搜索中显得非常重要。Fujimura提出用八叉树表示环境模型和文献[1,2]中提出用四叉树表示环境模型, 其环境被分割的节点繁多而使计算复杂化。笔者提出的动态二叉树表示环境模型的A^*算法, 从全局路径规划和局部路径规划综合考虑, 利用人工智能的智能搜索技术A^*算法, 在时间信息中考虑了加速度因素, 提出基于动态二叉树表示动态环境的群体障碍物的智能路径规划。在足球机器人系统仿真实现中, 将二叉树动态地表示球场的机器人与目标为对角线的矩形环境, 使搜索范围随着搜索过程动态缩小。从整体上减少了搜索空间, 提高了实时性, 实现了智能的路径规划。

展示更多