随着工业生产对圆盘移动机器人要求的不断提高, 路径规划已经成为圆盘移动机器人在工业生产上的一个重要研究领域, 传统路径规划算法主要有A^*算法^[1]、蚁群算法^[2]、人工势场算法^[3]、遗传算法^[4]等。尽管这些路径规划问题在处理低维空间路径规划问题方面具有一定的优越性, 但是当机器人路径规划的构型空间维度较高时, 算法在精确表达构型空间上需要占用大量的计算资源。基于采样思想的路径规划算法, 如PRM算法^[5-7]、RRT算法^[8-14], 不需要精确表达构型空间, 而是通过在构型空间内获取自由构型形成构型图, 以构型图描述构型空间的连通性, 这类算法在机械臂、人型机器人等高维构型空间上的所体现出的优势更为明显。

Lavalle等^[9]首次提出了快速扩展随机树(rapidly exploring random trees, RRT)算法, 基于随机采样的思想获取自由构型q_free, 用以构建一个树形网络表达自由构型空间。该算法避免了对整个环境空间建模, 在高维构型空间的路径规划问题中优势更为明显, 得到了广泛的关注。但是RRT算法采用的随机采样思想, 也导致了节点的扩展无目标导向性, 容易出现大量冗余节点, 算法的收敛速度过慢。针对RRT算法生成构型无目标导向性、收敛速度慢等缺点, Urmson等^[11]提出了路径代价函数的概念以表征路径的优化程度, 面向目标路径越优则路径代价越小。该算法在扩展中, 不再选择距离随机构型q_free距离最近的节点, 而是选择k个较近的节点进行扩展, 提升了已扩展RRT树内距离随机构型较近节点的搜索性能。代价函数的引入使得RRT树的扩展算法具有较好的目标导向性, 可有效提升算法的收敛速度。但是该算法在狭窄空间或障碍物密集等复杂环境下, 算法收敛性能会有明显下降。

在RRT算法研究初期, RRT及相应变种均采用单一随机树生成的思想, 由初始构型作为随机扩展树的初始节点, 在环境空间内进行扩展。单随机扩展算法构造简单, 但是无论是基础RRT算法还是其改进算法, 均存在收敛速度过慢的缺点。

基于双向搜索的思想, Pohi等^[12]提出了双向随机扩展算法(bidirectional RRT, BI-RRT), 构造两棵分别以起始构型和目标构型为初始点的随机扩展树, 递归进行节点扩展以构建可表达构型空间的树形网络。相较于RRT算法, BI-RRT算法的收敛速度更快。但是该算法采用RRT算法的随机节点扩展思想, 这导致BI-RRT算法也存在构型无目标导向性的缺点。为提升BI-RRT算法的收敛速度, 结合贪心思想, Kuffner等^[10]提出了RRT-connect算法。在BI-RRT算法扩展过程中, 从q_near到q_rand仅步进一个固定步长, 即使在无障碍物空间内也需多次扩展过程才可到达q_rand。在RRT-connect算法的扩展过程中, 从q_near到q_rand会持续步进, 直至遇到障碍物或到达q_rand。贪心思想的应用使得RRT-connect算法具有更高的扩展效率, 在自由构型空间内这一提升更为明显。但该算法扩展过程是以自由采样构型q_rand为目标点进行扩展, 没有改变其导向性差的缺点。Akgun等^[13]基于目标导向采样策略, 提出了概率优化的RRT^*算法, 提升了RRT^*算法的收敛速度及路径质量, 但是该算法采样过程中使用的局部偏置思想在复杂环境中的适应性较差, 存在导致算法收敛速度变缓慢的可能。李晓伟等^[14]将一种目标偏向的思想加入到BI-RRT, 避免了随机树搜索全局空间, 很大程度降低了算法的复杂度, 但是该算法采样过程中选取的目标点是另一棵树的上一个节点, 目标导向性不明确, 存在导致算法收敛速度缓慢的可能。上述所提的RRT算法均存在收敛速度缓慢的缺点, 没有考虑到圆盘移动机器人的碰撞检测问题。

针对BI-RRT算法研究中存在目标导向性差、收敛速度慢的问题, 提出了一种目标导向的BI-RRT算法(goal-oriented BI-RRT, GOBI-RRT)。该算法是在BI-RRT算法的基础上引入了目标导向的思想。通过搜索两棵树的最近节点, 利用目标导向思想产生随机点, 新增节点具有良好的目标导向性, 加快了路径收敛速度。本文在机器人模型选择中, 以圆盘移动机器人为模型, 替代质点模型, 可更好的匹配真实机器人构型。为适应这一改进, 还提出了适用于圆盘机器人的k点碰撞检测算法, 该碰撞算法可有效检测新增节点是否为自由构型。本文算法不同于文献[14]中的算法, 该算法新增节点是通过搜索一棵树的最近节点和另一棵树的上一节点来生成, 而本文中的GOBI-RRT算法新增节点是通过搜索两棵树的最近节点来生成。最后, 将本文方法与文献[12]基本BI-RRT算法和文献[14]中的算法作仿真实验对比, 验证所提出GOBI-RRT算法在路径规划中的优势。通过实验可知, 所提出的方法可以更加快速地寻得路径。本文算法的这一优势, 可促进圆盘底座移动机器人能够在较短时间内获取可行路径, 节约能源消耗。

1 目标导向的双向快速搜索随机树算法 1.1 BI-RRT算法

在BI-RRT中, 定义了两棵随机树T_a和T_b, 树T_a以q_init为树的根节点(起始点)开始扩展, 树T_b以q_goal为目标点开始扩展, q_rand为任意扩展的随机节点, 为每次扩展时任选两棵树中距离q_near最近的节点, 以q_new为新节点。首先在整个搜索空间中采取随机的方式生成随机树的随即扩展节点q_rand, 然后遍历当前已有的随机树, 从树中的节点寻找距离q_rand最近的节点q_near, 在q_near向q_rand延伸一定步长p之后可以得到新节点q_new, 之后需要对新节点q_new进行碰撞检测, 若q_new碰到障碍物便将这个节点舍去；反之, 即将q_new添加到树中, 可知此时q_new的父节点是q_near, 按照上述方式继续扩展, 直到两棵树的q_new小于一定的步长阈值时, 则可确定T_a和T_b连通, 即路径规划成功。图 1表示BI-RRT算法随机树的生长过程。

1.2 GOBI-RRT算法 1.2.1 改进节点生成方式

基本思想:原方案仅采用随机生成采样点, 以树中最近点沿当前方向扩展得到新的节点, 该过程主要的计算任务在碰撞检测阶段。所提出的基于目标导向的方案, 尽管在目标导向阶段增加了计算量, 但节点的选择更具有导向性, 使树的生成更偏向目标点。图 2表示基于目标导向思想生成新节点的过程。

改进了BI-RRT算法只生成一个q_rand确定q_new, 增加目标导向思想是以随机点q_rand和树T_a的最近节点q_near生成扩展方向, 树的最近节点q_near和树T_b的最近节点q_near'生成扩展方向, 再分别以步长p和kp(k为导向系数)生成q_rand'和q_near'', 最后通过平行四边形法则求新的节点q_new。

假设基于目标导向思想下, $ \overrightarrow {{q_{near{\rm{ }}}}{q_{rand{\rm{ }}}}} $和$ \overrightarrow {{q_{near{\rm{ }}}}{q_{near'{\rm{ }}}}} $的方向角为θ, q_near的坐标为(x_near, y_near), q_rand的坐标(x_rand, y_rand), q_near的坐标(x_near', y_near'), 则$\overrightarrow {{q_{near{\rm{ }}}}{q_{rand{\rm{ }}}}} $的直线表达式为:

$ y=\left(y_{ {rand }}-y_{ {near }}\right) /\left(x_{ {rand }}-x_{ {near }}\right) * x+b \text { , } $

(1)

其中，b是$\overrightarrow {{q_{near{\rm{ }}}}{q_{rand{\rm{ }}}}} $的直线段与y轴的交点。

假设q_rand'的坐标为(x_rand', y_rand'), 步长p表示为:

$ p = \sqrt {{{\left( {{x_{rand'{\rm{ }}}} - {x_{near{\rm{ }}}}} \right)}^2} + {{\left( {{y_{rand'{\rm{ }}}} - {y_{near{\rm{ }}}}} \right)}^2}} , $

(2)

依据$\overrightarrow {{q_{near{\rm{ }}}}{q_{rand{\rm{ }}}}} $和$\overrightarrow {{q_{near{\rm{ }}}}{q_{rand'{\rm{ }}}}} $的斜率相同有:

$ \begin{array}{*{20}{l}} {\left( {{y_{rand{\rm{ }}}} - {y_{near{\rm{ }}}}} \right)/\left( {{x_{rand{\rm{ }}}} - {x_{near{\rm{ }}}}} \right) = }\\ {\left( {{y_{rand{\rm{ }}}} - {y_{near{\rm{ }}}}} \right)/\left( {{x_{rand{\rm{ }}}} - {x_{near{\rm{ }}}}} \right), } \end{array} $

(3)

由(2)和(3)得到q_rand'的坐标。

假设q_near''的坐标为(x_near'', y_near''), 步长kp表示为:

$ kp = \sqrt {{{\left( {{x_{near{{\rm{ }}^{\prime \prime }}}} - {x_{near{\rm{ }}}}} \right)}^2} + {{\left( {{y_{near{{\rm{ }}^{\prime \prime }}}} - {y_{near{\rm{ }}}}} \right)}^2}} , $

(4)

依据$ \overrightarrow {{q_{near{\rm{ }}}}{q_{near'{\rm{ }}}}} $和$ \overrightarrow {{q_{near{\rm{ }}}}{q_{near''{\rm{ }}}}} $的斜率相同有:

$ \begin{array}{*{20}{l}} {\left( {{y_{near'}} - {y_{near{\rm{ }}}}} \right)/\left( {{x_{near'}} - {x_{near{\rm{ }}}}} \right) = }\\ {\left( {{y_{near^{\prime \prime }}} - {y_{near{\rm{ }}}}} \right)/\left( {{x_{near{{\rm{ }}^{\prime \prime }}}} - {x_{near{\rm{ }}}}} \right), } \end{array} $

(5)

由(4)和(5)得到q_near''的坐标。

依据q_rand'和q_near''的坐标, q_near的坐标, 假设q_new的坐标为(x_new, y_new), 依据平行四边形法则可得:

$ \left\{ {\begin{array}{*{20}{l}} {{x_{new{\rm{ }}}} = {x_{near{{\rm{ }}^{\prime \prime }}}} + {x_{rand{\rm{ }}^\prime }} - {x_{near{\rm{ }}}}}\\ {{y_{new{\rm{ }}}} = {y_{near{{\rm{ }}^{\prime \prime }}}} + {y_{rand{{\rm{ }}^\prime }}} - {y_{near{\rm{ }}}}} \end{array}, } \right. $

(6)

针对一次节点扩展, 利用目标导向得到新节点q_new的计算过程如下:

Step1:给定树T_a的最近点q_near和随机点q_rand的坐标, 给定树T_b的最近点q_near'的坐标。

Step2:根据给定的步长p和kp求解式(2)、(3)、(4)和(5)得到q_rand'、q_near''的坐标。

Step3:对$\overrightarrow {{q_{near{\rm{ }}}}{q_{rand'{\rm{ }}}}} $和$ \overrightarrow {{q_{near{\rm{ }}}}{q_{near''{\rm{ }}}}} $利用平行四边形法则解方程(6), 求出q_new (x_new, y_new)。

1.2.2 圆盘k点碰撞检测算法

假设圆盘的圆心坐标为(x₀, y₀), 半径为r。将圆盘以圆心将周长k等分, 设圆上任意一点坐标为(x_i, y_i), i=1, 2, 3……k, 设该点与圆心的连线和平行于x轴的直线y=y₀的夹角为α。图 3表示圆盘k点碰撞检测算法。

坐标(x_i, y_i)表示为:

$ \left\{\begin{array}{l} x_{i}=x_{0}+r cos \alpha \\ y_{i}=y_{0}+r sin \alpha \end{array}\right.。$

(7)

1.3 复杂度分析

单节点扩展总的计算复杂度由两部分组成:

第一部分:目标导向的复杂度

由式(3)可以得到

$ \begin{array}{*{20}{l}} {{y_{rand'}} = \left( {{y_{rand{\rm{ }}}} - {y_{near{\rm{ }}}}} \right)\left( {{x_{rand{\rm{ }}}} - {x_{near{\rm{ }}}}} \right) \cdot }\\ {\left( {{x_{rand'}} - {x_{near{\rm{ }}}}} \right), } \end{array} $

(8)

再将y_rand'带入到式(2)中得到

$ \begin{array}{*{20}{l}} {p = {{\left( {{x_{rand'{\rm{ }}}} - {x_{near{\rm{ }}}}} \right)}^2} + \left[ {\left( {{x_{rand'{\rm{ }}}} - {x_{near{\rm{ }}}}} \right)} \right. \cdot }\\ {{{\left. {\left( {{y_{rand{\rm{ }}}} - {y_{near{\rm{ }}}}} \right)\left( {{x_{rand{\rm{ }}}} - {x_{near{\rm{ }}}}} \right) - {y_{near{\rm{ }}}}} \right]}^2}}。\end{array} $

x_rand、x_near、y_rand、y_near都是已知的。而复杂度最高的是求开方部分, 依据1997年RICHARD P的论文^[15], 假设地图表示为n×n矩阵, 该复杂度包括乘法运算和加法运算, 乘法运算复杂度表示为: O(μ(n))≈O(n²), 而加法运算复杂度可以忽略, 目标导向的复杂度为O(n²)。如图 4所示, 蓝色线为重合$ \overrightarrow {{q_{near{\rm{ }}}}{q_{near'{\rm{ }}}}} $的直线和垂直于$ \overrightarrow {{q_{near{\rm{ }}}}{q_{near'{\rm{ }}}}} $的直线, 在目标导向扩展思想, 有效节点在正向生长区占比是100%, 反向生长区占比是0, 而随机扩展思想, 有效节点在正向生长区和反向生长区占比均是50%, 相比随机扩展, 目标导向的扩展思想, 随着生长节点的增加, 减少了50%的有效节点碰撞检测。

第二部分:圆盘k点碰撞检测复杂度

由式(7), 可见求解(x_i, y_i)包含二次乘法和二次加法, 圆盘多点碰撞检测算法的复杂度为2k次乘法和2k次加法。圆盘多点碰撞检测的复杂度为O(n²log₂k)。

图 5为BI-RRT和GOBI-RRT复杂度对比图, 蓝色线为BI-RRT计算复杂度, 红色线为GOBI-RRT计算复杂度, 与随机采样比较, 目标导向可以减少无效随机采样点生成, 随着扩展节点数量的增长, 通过目标导向思想的算法改进的更明显。

2 伪代码实现 2.1 BI-RRT算法

算法1给出了BI-RRT算法的轮廓, 首先初始状态被添加到搜索树, 主循环是line3-24, 在n次迭代后终止。显然, BI-RRT算法通过采样随机点, 扩展完树T_a的新节点q_new后, 以q_new作为T_b的扩展方向。同时树T_b首先会扩展第一步得到q_new', 如果没有碰撞, 继续向着相同的方向扩展第二步, 直到扩展失败或者q_new=q_new'表示与树T_a相连了, 即整个算法结束。每次迭代中必须考虑两棵树的平衡性, 即两棵树的节点数的多少(也可以考虑两棵树总共花费的路径长度), 交换次序选择“小”的那棵树进行扩展, BI-RRT的构造方法如表 1所示。

2.2 GOBI-RRT算法 2.2.1 改进节点生成方式

在节点生成子程序中, 改进了BI-RRT算法通过树上最近点和产生的随机点来确定的新节点, 并提出目标导向的思想生成新节点。首先随机生成q_rand, 通过q_rand与树T_a上的最近点q_near产生一个新节点q_rand', 再通过q_near与树T_b的最近节点q_near'来生成另一个新节点q_near'', 基于目标导向的思想, 对$\overrightarrow {{q_{near{\rm{ }}}}{q_{rand'{\rm{ }}}}} $和$ \overrightarrow {{q_{near{\rm{ }}}}{q_{near''{\rm{ }}}}} $生成新节点q_new, 图 6表示目标导向思想q_new生成。Newpoint的构造方法如表 2所示。

2.2.2 圆盘k点碰撞检测算法

Checkpath:改进了单点碰撞检测函数, 提出了一种圆盘多点碰撞检测算法, 具体操作:假设圆盘机器人的圆心为x_d, 首先将圆盘机器人分割成k等份, 本文中取k=50, 再对分割出来点的坐标合成一个集合{q_i}, i=1, 2, 3…50, 对{q_i}碰撞检测, 来检测圆盘上的点是否在障碍物上, 当这50个点都不在障碍物里和地图外时, 将q_new加入到路径中。Checkpath构造方法如表 3所示。

3 实验与分析

仿真实验是在Windows10, 内存为16GB的电脑安装有Mtalab R2019a仿真平台。仿真实验环境设定在不同的环境(宽阔环境、通道环境、栅格环境、迷宫环境)中的路径规划, 设置实验空间尺寸为500 m×500 m, 起始点坐标设置成(10, 10), 终点坐标设置成(490, 490)。考虑到是现实生活, 本文取圆盘移动机器人直径为1 m。分别对文献[12]BI-RRT算法、文献[14]目标偏向BI-RRT算法和本文GOBI-RRT算法在四种不同地图上进行仿真对比, 每种对比实验进行50次仿真, 取均值进行比较。图 7至图 10, 分别表示宽阔环境、通道环境、栅格环境、迷宫环境下的路径规划图, 图a)表示文献[11], 图b)表示文献[14], 图c)表示本文算法, 蓝色线为树T_a的路径规划, 红色线为树T_b的路径规划, 绿色线为树T_a和树T_b节点的相连。

通过在四种不同的环境下的仿真实验, 可以明显看出, 相比BI-RRT算法和目标偏向BI-RRT算法, GOBI-RRT算法使随机树向着目标点生长, 提高了收敛速度并减少了大量的节点。

表 4至表 7表示分别在宽阔地图、通道地图、栅格地图、迷宫地图四种环境下, 选取路径长度、航迹点数、规划时间三方面进行对比并得到的仿真实验数据。上述实验数据, 均通过Matlab内运行仿真算法提取。

在宽阔地图上, GOBI-RRT算法相比BI-RRT算法平均轨迹点减少了53.49%、平均规划时间减少了14.2%, 相比目标偏向BI-RRT算法平均轨迹点减少了5%、平均规划时间减少了6.5%。

在通道环境上, GOBI-RRT算法相比BI-RRT算法平均轨迹点减少了50%、平均规划时间减少了25.58%, 相比目标偏向BI-RRT算法平均轨迹点减少了3.8%、平均规划时间减少了4.78%。

在栅格环境上, GOBI-RRT算法相比BI-RRT算法平均轨迹点减少了51.16%、平均规划时间减少了30.71%, 相比目标偏向BI-RRT算法平均轨迹点减少了4.5%、平均规划时间减少了6.6%。

在迷宫环境上, GOBI-RRT算法相比BI-RRT算法平均轨迹点减少了54.54%、平均规划时间减少了30.5%, 相比目标偏向BI-RRT算法平均轨迹点减少了3.8%、平均规划时间减少了10.78%。

由表 4至表 7可知, GOBI-RRT算法能够在很短的时间里寻得路径。在同样的环境和参数下, 本文算法的平均轨迹点的个数约为BI-RRT算法的一半, 相比目标偏向BI-RRT算法平均减少了4.28%；在平均规划时间上, 本文算法相比BI-RRT算法平均减少了25.25%, 相比目标偏向BI-RRT算法平均减少了6.64%。GOBI-RRT算法相比目标偏向BI-RRT算法更具有目标导向性, 随着节点数量的增加, GOBI-RRT算法相比BI-RRT算法复杂度得到了优化。因此本文提出的GOBI-RRT算法适用于多种不同环境, 能够以更少的搜索节点、更快的收敛速度得到路径, 有较大的实用价值。

4 结论

针对BI-RRT算法路径规划中存在目标导向性差、收敛速度缓慢的问题提出了GOBI-RRT算法。该算法通过目标导向思想对随机树中随机点的产生进行改进, 同时和圆盘k点碰撞算法相结合, 通过数学模型分析, 相比于BI-RRT算法, 降低了算法复杂度。在4种不同地图环境的仿真实验中, 验证了GOBI-RRT算法在圆盘移动机器人上的优势。通过与BI-RRT算法和目标偏向BI-RRT算法相比, 该算法大幅减少了平均航迹点的个数和平均规划时间, 在路径规划中, 有较大的实用价值。在后续的研究中, 将考虑在平均路径长度与路径平滑上进行持续优化。