0 引言

多移动机器人的路径规划既需考虑机器人的静态避障，又要考虑多机器人之间的协作避碰^[1].避碰避障控制算法主要有:人工势场法、遗传模糊控制法、神经网络控制法、交通规则法和栅格法^[2-4]等.文献[5]对传统人工势场法进行了改进，解决了单个机器人避障控制中的局部极小值问题，但未涉及多机器人之间的协作避碰.文献[6]结合遗传算法，通过模糊控制器的输出，实时调整每个机器人的线速度和角速度，提高了机器人的自主导航能力，但要构造全局模糊规则非常复杂.文献[7]用BP神经网络实现Q学习算法，解决了机器人的避障问题，该算法不需要先验知识，但需要在线或离线训练，占用大量系统资源，严重影响控制系统的实时性和收敛性.文献[8]中当多个机器人相互接近时，机器人沿同一方向(即逆时针方向)运动避开障碍物，该方法忽略了动态误差，可靠性较差.文献[9]提出了基于栅格法的多机器人路径规划方法，将其他机器人视为动态障碍物，根据删格值确定运动路径，缺陷是机器人数量越多算法越复杂.上述文献中的算法在一定程度上解决了机器人的避障避碰问题，但是对稳定性的分析还不够完善.

本文基于机器人的运动学控制模型和自定义的避碰函数，设计了一种用于多移动机器人避障避碰的控制器.在非完整约束下，该控制器同时考虑了机器人的轨迹跟踪和避障避碰.通过李雅普诺夫法分析了整个多移动机器人系统的稳定性，采用分布式控制方案，进行了多机器人的轨迹跟踪与协作避碰仿真实验.

1 控制策略和运动建模

在本文中，多移动机器人系统的整体控制策略是:在跟踪给定参考轨迹的同时，不与静态障碍物或其他机器人发生碰撞；机器人R_i的位姿、参考轨迹以及障碍物位置(或者其他机器人R_j，i≠j)三者决定了机器人R_i下一步的运动速度和角度；采用分布式控制方案，假设每个机器人知道自己的位姿信息，并且能够检测到一定范围内的任何物体.

在直角坐标系中，机器人R_i的位置、方向角、平移速度及旋转速度分别用(x_i，y_i), θ_i, v_i, u_i来表示, 其中i=1, …, N.采用非线性常微分方程(组)运动学模型

$ \left[\begin{array}{l} {{\dot x}_i}\\ {{\dot y}_i}\\ {{\dot \theta }_i} \end{array} \right] = \left[{\begin{array}{*{20}{c}} {\cos \left( {{\theta _i}} \right)}&0\\ {\sin \left( {{\theta _i}} \right)}&0\\ 0&1 \end{array}} \right]\left[\begin{array}{l} {v_i}\\ {u_i} \end{array} \right], $

在非完整约束条件下，机器人R_i满足方程$ {{\dot x}_i}\;\sin \left( {{\theta _i}} \right)-{{\dot y}_i}\;\cos \left( {{\theta _i}} \right) = 0$.

2 多移动机器人避碰规划

为保证机器人在跟踪期望轨迹的过程中，能够实时检测并躲避障碍物及其他机器人，令p_i=[x_i y_i]^T，p_j=[x_j y_j]^T，其中x_i∈R，y_i∈R(i, j∈N, i≠j).

定义1  机器人R_i的避碰函数为

$ F\left( {{\pmb{p}_i}} \right) = {\left[{\min \left\{ {0, \frac{{{{\left\| {{\pmb{p}_i}-{\pmb{p}_j}} \right\|}^2}-{R^2}}}{{{{\left\| {{\pmb{p}_i}-{\pmb{p}_j}} \right\|}^2} - {r^2}}}} \right\}} \right]^2}, $

(1)

其中:R>0，r>0，并且R>r, R的是机器人检测半径，r是避碰半径(机器人与障碍物之间的最小距离)；p_i表示机器人R_i的坐标，p_j表示机器人R_j的(或者静态障碍物)坐标.由式(1) 可知，当机器人R_i与R_j的距离大于R时，F(p_i)值恒为零，当两者距离大于r且小于R时，F(p_i)值不为零.此函数在检测区域内有无限个取值，在检测区域以外值为零.

定义2  机器人R_i的避碰区和检测区为: Ζ={p_i^T:(x_i，y_i)∈R², ‖p_i-p_j‖≤r}，Λ={[p_i^T:(x_i，y_i)∉Ζ, r < ‖p_i-p_j‖≤R}，令l_ij=‖p_i-p_j‖，对避障函数F(p_i)求导，

$ \frac{{\partial F}}{{\partial {\pmb{p}_i}}}\left\{ \begin{array}{l} 0, \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{l_{ij}} \ge R, \\ \frac{{4\left( {{R^2}-{r^2}} \right)\left( {l_{ij}^2-{R^2}} \right)}}{{{{\left( {l_{ij}^2-{r^2}} \right)}^3}}}\left( {{\pmb{p}_i} - {\pmb{p}_j}} \right), r < {L_{ij}} < R, \\ 无定义\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{l_{ij}} = r, \\ 0, \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{l_{ij}} < r. \end{array} \right. $

(2)

定义3  机器人R_i与其他N-1个机器人之间的避碰函数为$ F\left( {{\pmb{p}_i}} \right) = \sum\limits_{j = 1, j \ne i}^N {F\left( {{\pmb{p}_{ij}}} \right)} $.

机器人R_i的参考轨迹为(x_ri, y_ri)，且其导数满足有界. R_i实际位姿与期望位姿的误差定义为e_xi=x_i-x_ri，e_yi=y_i-y_ri，当(ξ_xi, ξ_yi) ≠(0，0) 时.

定义4  机器人R_i的期望角度为θ_ri=arctan ξ_yi/ξ_xi，其中:

$ \left\{ \begin{array}{l} {\xi _{xi}} = {e_{xi}} + \frac{{\partial F}}{{\partial {x_i}}}, \\ {\xi _{yi}} = {e_{yi}} + \frac{{\partial F}}{{\partial {y_i}}}. \end{array} \right. $

(3)

由式(3) 可知，θ_ri由机器人R_i的参考位置、实际位置和障碍物位置共同决定，相对应的角度误差e_θi=θ_i-θ_ri.对受非完整约束的多移动机器人系统，做如下假设:

假设1  机器人R_i的参考轨迹光滑并且其角度误差满足$\left| {{e_{\theta i}}} \right| \ne \frac{{\rm{ \mathsf{ π} }}}{2} $.

假设2  在检测区内机器人R_i的参考轨迹保持常数，即${{\dot x}_{ri}} = {{\dot y}_{ri}} = 0\left( {r \le {l_i} < R} \right) $.

假设1意味着参考轨迹是一条连续且可导的曲线，角度误差满足的条件表明机器人不会发生90°急转.假设2意味着机器人在跟踪参考轨迹的过程中，如果遇到障碍物，会立刻冻结接收到的参考轨迹数据，优先处理避碰问题，一旦避碰结束，又会接收参考轨迹数据.

3 多移动机器人控制器设计

李雅普诺夫理论是分析和研究非线性控制系统稳定性的经典理论^[10-14].其核心是构造李雅普诺夫函数，根据该函数的导数符号，推断出系统的稳定性.本文用构造的李雅普诺夫函数$ V = \sum\limits_{i = 1}^N {\left[{\frac{1}{2}\left( {e_{xi}^2 + e_{yi}^2 + e_{\theta i}^2} \right) + \sum\limits_{j = 1, j \ne i}^N {F\left( {{\pmb{p}_{ij}}} \right)} } \right]} $设计控制器，使整个系统渐近稳定.考虑动态误差，机器人R_i的位姿误差导数为

$ {{\dot e}_{xi}} = {{\dot x}_i}-{{\dot x}_{ri}} = {v_i}\cos \left( {{\theta _{ri}} + {e_{\theta i}}} \right)-{{\dot x}_{ri}}, {{\dot e}_{yi}} = {{\dot y}_i}-{{\dot y}_{ri}} = \\{v_i}\sin \left( {{\theta _{ri}} + {e_{\theta i}}} \right) - {{\dot y}_{ri}}, {{\dot e}_{\theta i}} = \dot {\theta -} {{\dot \theta }_{ri}} = {u_i} - {{\dot \theta }_{ri}}. $

除避碰边界点，对V求导

$ \begin{align} & \dot{V}=\sum\limits_{i=1}^{N}{\left[ {{e}_{xi}}{{{\dot{e}}}_{xi}}+{{e}_{yi}}{{{\dot{e}}}_{yi}}+{{e}_{\theta i}}{{{\dot{e}}}_{\theta i}}+\frac{\partial F}{\partial {{x}_{i}}}{{{\dot{x}}}_{i}}+\frac{\partial F}{\partial {{y}_{i}}}{{{\dot{y}}}_{i}} \right]}= \\ & \sum\limits_{i=1}^{N}{\left[ \left( {{e}_{xi}}+\frac{\partial F}{\partial {{x}_{i}}} \right){{{\dot{x}}}_{i}}+\left( \frac{\partial F}{\partial {{y}_{i}}}+{{e}_{yi}} \right){{{\dot{y}}}_{i}}-{{e}_{xi}}{{{\dot{x}}}_{ri}}-{{e}_{yi}}{{{\dot{y}}}_{ri}}+{{e}_{\theta i}}{{{\dot{e}}}_{\theta i}} \right]=} \\ & \sum\limits_{i=1}^{N}{\left[ {{\xi }_{xi}}{{v}_{i}}\cos \left( {{e}_{\theta i}}+{{\theta }_{ri}} \right)+{{v}_{i}}\sin \left( {{e}_{\theta i}}+{{\theta }_{ri}} \right){{\xi }_{yi}}-{{e}_{xi}}{{{\dot{x}}}_{ri}}-{{e}_{yi}}{{{\dot{y}}}_{ri}}+{{e}_{\theta i}}\left( {{u}_{i}}-{{{\dot{\theta }}}_{ri}} \right) \right]}= \\ & \sum\limits_{i=1}^{N}{\left[ {{v}_{i}}\cos \left( {{e}_{\theta i}} \right)\sqrt{\xi _{xi}^{2}+\xi _{yi}^{2}}-{{e}_{xi}}{{{\dot{x}}}_{ri}}-{{e}_{yi}}{{{\dot{y}}}_{ri}}+{{e}_{\theta i}}\left( {{u}_{i}}-{{{\dot{\theta }}}_{ri}} \right) \right]}. \\ \end{align} $

控制器设计为

$ \left\{ \begin{array}{l} {u_i} =-{K_{\theta i}}{e_{\theta i}} + {{\dot \theta }_{ri}}, \\ {v_i} =-K_i\cos \left( {{e_{\theta i}}} \right)\sqrt {\xi _{xi}^2 + \xi _{yi}^2}, \end{array} \right. $

(4)

其中:K_θi，K_i为增益参数，且都大于零.可通过提高增益参数的值减少跟踪误差.该控制器的设计综合了多机器人系统运动模型和定义的期望方向角θ_ri，且满足上述假设1和2.

当障碍物在机器人的检测范围内(R>l_i>r)时，由假设2可知${{\dot x}_{ri}} = {{\dot y}_{ri}} = 0 $，有

$ \dot{V}=\sum\limits_{i=1}^{N} {\left[{{v_i}\cos \left( {{e_{\theta i}}} \right)\sqrt {\xi _{xi}^2 + \xi _{yi}^2} + {e_{\theta i}}\left( {{u_i}-{{\dot \theta }_{ri}}} \right)} \right]} =\\\sum\limits_{i = 1}^N {\left[{-{K_i}{{\cos }^2}\left( {{e_{\theta i}}} \right)\left( {\xi _{xi}^2 + \xi _{yi}^2} \right)-{K_{\theta i}}e_{\theta i}^2} \right] < 0.} $

当障碍物在机器人的检测区外(l_i>R)时，根据该多移动机器人系统控制策略，每个机器人的主要任务是跟踪给定的参考轨迹，不考虑避碰问题，即式(2) 中避碰函数的偏导值为零.

$ \begin{array}{l} \dot{V}=\sum\limits_{i=1}^{N} {\left[{-{K_i}{{\cos }^2}\left( {{e_{\theta i}}} \right)\left( {e_{xi}^2 + e_{yi}^2} \right)-{e_{xi}}{{\dot x}_{ri}}-{e_{yi}}{{\dot y}_{ri}} - {K_{\theta i}}e_{\theta i}^2} \right]} \le \\ - \left[\begin{array}{l} {\pmb{A}_1}\\ \vdots \\ {\pmb{A}_N} \end{array} \right]\left[{\begin{array}{*{20}{c}} {{\pmb{Q}_1}}&0& \cdots &0\\ \vdots&\vdots&\vdots&\vdots \\ 0& \cdots &0&{{\pmb{Q}_N}} \end{array}} \right]\left[\begin{array}{l} {\pmb{A}_1}\\ \vdots \\ {\pmb{A}_N} \end{array} \right] + ||\left[\begin{array}{l} {\pmb{A}_1}\\ \vdots \\ {\pmb{A}_N} \end{array} \right]||\;||\left[\begin{array}{l} {\pmb{B}_1}\\ \vdots \\ {\pmb{B}_N} \end{array} \right]|||, \end{array} $

其中:ε_i是趋于0的值; $ {\pmb{Q}_i} = \left[{\begin{array}{*{20}{c}} {{K_i}{{\cos }^2}\left( {{e_{\theta i}}} \right)}&0&0\\ 0&{{K_i}{{\cos }^2}\left( {{e_{\theta i}}} \right)}&0\\ 0&0&{{K_{\theta i}}} \end{array}} \right];{\pmb{A}_i} = \left[\begin{array}{l} {e_{xi}}\\ {e_{yi}}\\ {e_{\theta i}} \end{array} \right];{\pmb{B}_i} = \left[\begin{array}{l} {{\dot x}_{ri}}\\ {{\dot y}_{ri}}\\ {\varepsilon _i} \end{array} \right].$

由假设1可知, cos²(e_θi)>0，因此dV/dt < 0, 只需要动态误差满足$\left\| {{{\left[{{\pmb{A}_1}\; \cdots \;{\pmb{A}_N}} \right]}^{\rm{T}}}} \right\| > \frac{{\left\| {{{\left[{{\pmb{B}_1}\; \cdots \;{\pmb{B}_N}} \right]}^{\rm{T}}}} \right\|}}{{{{\min }_i}\left\{ {{\lambda _{{{\min }_i}}}\left( {{\pmb{Q}_i}} \right)} \right\}}} $.

综上所述:无论是躲避障碍物(包括机器人)还是跟踪给定的参考轨迹都有dV/dt < 0, 所以该系统是渐近稳定的.

4 仿真与分析

本文通过两个仿真实验来验证多机器人系统避障与协作避碰算法的有效性.

仿真1  静态避障.

通过稳定性条件和参数灵敏度分析，得到公式(4) 中的一组近似最佳参数为K₁=1，K_θ1=10.在仿真中，机器人跟踪的参考轨迹为(x_r(t), y_r(t)).静态障碍物坐标为(x_o, y_o)=(2.6, 2), 机器人初始位置为(x₀, y₀, θ₀)=(3, 2, π)，避碰与检测半径分别为r=0.1、R=0.2.单个机器人的参考轨迹为x_r=2+0.55 cos(0.1t)，y_r=2+0.55 sin(0.1t).利用所设计的控制器，机器人进行静态避障，其运动轨迹分别如图 1-a(t=20 s时)、图 1-b(t=80 s时)所示.其中实线是所给参考轨迹，虚线是机器人实际轨迹，*是障碍物.从图中可以看出，机器人在运动过程中前后成功进行了两次静态避障.

仿真2  协作避碰.

机器人R₁的初始位置为(x_{1, 0}, y_{1, 0}, θ_{1, 0})=(-6, 11, -π)、参考轨迹为x_r1(t)=6+7 sin(0.1t+π)，y_r1(t)=12+7 sin(0.1t+π).机器人R₂的初始位置为(x_{2, 0}, y_{2, 0}, θ_{2, 0})=(26, 12, -π)、参考轨迹为x_r2(t)=21+7 sin(0.1t)，y_r2(t)=12+7 sin(0.1t).检测和避碰半径分别为R=4、r=2.在连续运动情况下，机器人R₁绕过静态障碍物，通过公式(4) 调节跟踪误差，继续跟踪参考轨迹，如图 2-a(t=20 s时)所示.当机器人R₁、R₂相互进入对方的检测区域时，优先处理它们相互之间的避碰问题，如图 2-b(t=36 s时)所示，当避碰结束后继续跟踪各自的参考轨迹，如图 2-c所示(t=50 s时).图 2-d是整个实验连续的运动轨迹.

5 总结

本文针对受非完整约束的多移动机器人系统进行了避障避碰研究，首先建立了多移动机器人运动模型，然后提出一种多机器人的避碰算法，该算法能够使机器人实时检测并避开障碍物(包括其他机器人).结合避碰函数，根据李雅普诺夫法对该非线性系统进行稳定性分析.从仿真效果来看，应用本文所提出的控制策略及方法，移动机器人具有良好的路径跟踪能力，并且能够实时地避开静态和动态障碍物.由于多机器人实时队形控制需要考虑机器人的避障避碰问题，未来拟将本文方法应用到多移动机器人的编队控制及协调合作中.