1 引言（Introduction）

随着机器人技术的智能化与机械臂在各行业中更广泛的应用，机械臂需要能够适应非结构化的工作环境。在该类环境中，常有意料外的障碍物阻挡在机械臂的原工作路径上，因此，机械臂的避障路径规划是一个有意义的研究方向。

机械臂实现避障路径规划的前提是，机械臂需要准确、可靠地感知到障碍物，即障碍物的目标检测问题。目前，主流的目标检测方法都是基于视觉的，并且随着深度学习的发展，已有许多学者针对结合视觉与深度学习的目标检测算法进行了大量研究^[1-3]，并且不少学者将其应用于机械臂上，如完成抓取任务^[4-6]、在复杂环境中实现多物体的识别、分拣^[7]以及可视化^[8]。

然而，基于视觉的方法往往容易出现遮挡问题，相对应地，Tsuji等^[9]设计了一种可扩展的接近觉传感器模块。受文[9] 启发，本文提出了基于接近觉的障碍物感知方法，该方法将接近觉传感器阵列布置在机械臂连杆上以感知障碍物。当机械臂感知到障碍物后，需要使用合理的路径规划方法规划避障路径，其可分为全局方法与局部方法。其中，前者适用于环境信息已知的情况，包括A* 算法^[10]、C空间法^[11]、栅格法^[12]和快速扩展随机树法^[13-14]等；后者适用于环境信息未知的情况，包括人工势场法^[15]、遗传算法^[16]和神经网络算法^[17]等。基于李娜^[18]采用的适用于机械臂的基于势函数与关节空间的人工势场法，并结合基于接近觉的障碍物感知方法，本文进一步提出了基于接近觉的机械臂避障路径规划方法，并在UR10小臂连杆上进行了验证。与较为常见的基于视觉的方法相比，该方法可以避免视觉遮挡问题，并且接近觉传感器响应迅速、得到的数据量非常小、易于处理。

本文的主要贡献与创新为：1) 提出了假想圆锥与安全平面的概念，实现了有效的基于接近觉的机械臂障碍物感知；2) 提出了连杆运动方向的概念，优先读取该方向上的接近觉传感器，降低传感器读取周期、提升算法实时性与传感器阵列的扩展潜力；3) 考虑到所提出避障方法的特点，提出了基于人工势场法的改进的绕行法，使得机械臂在避障过程中得以逃离局部最优陷阱。

2 基于接近觉的机械臂障碍物感知（Obstacle perception by the manipulator based on proximity） 2.1 接近觉传感器阵列分布

本文在UR10机器人小臂上安装接近觉传感器阵列以验证所提出的避障方法，如图 1所示。阵列以6个间隔为75 mm的圆周的方式均匀对称分布，每个圆周上以45$ ^{\circ} $间隔等分安装8个接近觉传感器，总计有48个接近觉传感器。

2.2 障碍物表面建模

假设障碍物表面封闭且连续，则接近觉传感器阵列会以一系列投影点的距离信息的形式，在其检测方向上感知到障碍物表面，并且可以从中找出距离连杆表面最近的点。然而，检测到的最近投影点未必就是障碍物离连杆实际最近的点，在传感器间隙处障碍物的局部表面还可能存在凸起的特征。因此考虑将投影距离最近的相邻的3个传感器之间的障碍物局部表面构造为一个假想的顶角一定的圆锥。并且将该圆锥的顶点作为障碍物表面离连杆表面距离最近的危险点，假想圆锥顶角的设定会对机械臂连杆与障碍物表面间的距离产生影响，当设定该假想圆锥顶角为90$ ^{\circ} $时，机械臂连杆在避障时与感知到的障碍物表面保持合理的间距以实现有效、灵活的避障，同时该避障具有较好的鲁棒性。

如图 2所示，点$ A $、$ B $、$ C $为接近觉传感器测得的最近相邻投影点，绿色边界表示封闭、连续的障碍物的局部实际表面，$ T $为假想顶角一定的圆锥顶点。由于圆锥顶角一定，易得圆锥顶点的空间坐标。

设$ A $、$ B $、$ C $点的空间坐标分别为$ (x_{1}, y_{1}, z_{1}) $、$ (x_{2}, y_{2}, z_{2}) $、$ (x_{3}, y_{3}, z_{3}) $，圆锥顶角为$ \theta $，易求得$ A $、$ B $、$ C $三点的外接圆圆心$ O $的坐标为$ (x_{0}, y_{0}, z_{0}) $、半径为$ r $，显然可求得圆锥顶点$ T $的坐标为

$ \begin{align} \begin{bmatrix} {x_{T}} \\ {y_{ T}} \\ {z_{ T}} \end{bmatrix}=\mathit{\boldsymbol{v}}\cdot t+\begin{bmatrix} {x_{0}} \\ {y_{0}} \\ {z_{0}} \end{bmatrix} \end{align} $

(1)

其中，

$ \begin{align} \mathit{\boldsymbol{v}} & =\begin{bmatrix} {x_{2} -x_{1}} \\ {y_{2} -y_{1}} \\ {z_{2} -z_{1}} \end{bmatrix} \begin{bmatrix} {x_{3} -x_{1}} \\ {y_{3} -y_{1}} \\ {z_{3} -z_{1}} \end{bmatrix} \end{align} $

(2)

$ \begin{align} t & =\pm \frac{d^{2}}{ | \mathit{\boldsymbol{v}} | } \end{align} $

(3)

$ \begin{align} d & =\frac{r}{ \tan \dfrac{\theta} {2}} \end{align} $

(4)

需要注意的是，$ t $取值的正负符号由圆锥顶点在靠近机械臂连杆时的位置决定。

2.3 连杆运动方向

由于传感器数量较多，在一个控制周期内轮询全部传感器难以满足算法的实时性，限制了进一步增加传感器数量的升级潜力。因此，本节定义了连杆的运动方向，在一个控制周期内优先读取该方向上的传感器，并在空闲周期轮询剩余传感器。

机械臂的连杆可采用包围盒的方法进行简化，图 3所示为UR10小臂的包围盒简化图，对于简化后的连杆来说，其绕着自身轴线旋转的并不是与障碍物发生碰撞的主要自由度，因此将连杆的运动简化为包围盒轴线段的运动。于是，定义连杆的运动方向为轴线段2个端点的空间平移方向，此时在轴线段上的所有点在空间中的运动也全部确定。

如图 4所示，红色线段表示连杆包围盒轴线段，代表连杆包围盒在当前空间中的姿态，蓝色线段代表在一个时间步$ \Delta t $后连杆轴线段的期望位姿，在连杆上的红色和绿色小球表示所部署的接近觉传感器阵列，红色小球为需要读取数据的传感器。当前时刻，连杆在红色小球之间的方位上可能发生因连杆主动运动而引起的碰撞，图中某传感器圆周中心点$ O $在$ \Delta t $时间步内的位移为$ OO'' $，$ OO' $为$ OO'' $在该圆周所在平面上的投影。此时，所需优先读取的传感器为与$ OO' $相邻的最近的2个传感器S1、S2，在该连杆上其他所有同一圆周上的传感器也按照以上原则依次优先选取传感器读取数据。

2.4 机械臂障碍物感知

接近觉传感器阵列无法同视觉传感器、激光雷达传感器等一样相对完整、精确地获取障碍物的空间信息。因此，考虑构造安全平面以对连杆的运动进行约束，使连杆包围盒不能与其发生碰撞。显然，只要包围盒两端点同时与安全平面保持在安全距离外即可满足约束，易于实现。

部署了接近觉传感器阵列的机械臂连杆在空间中运动，并采用2.3节的方法读取该连杆运动方向上的传感器，在此过程中，机械臂对障碍物的感知共分为图 5所示的3种情形。图 5(a)中连杆的运动安全不受限；图 5(b)中有且仅有少于3个传感器的投影距离信息，此时采取如下步骤构造假想圆锥：1) 选取距离最近的投影点$ O $构造基准坐标系$ O $-$ XYZ $，其$ Z $轴与该传感器投影方向平行。2) 令运动方向上其余无数据的传感器投影在基准平面$ XOY $上以获得假想的投影点与距离数据。3) 使用假想的投影点补足3个投影点$ O $、$ O_{1} $、$ O_{2} $，如图 5(b)中选取的$ O_{2} $点即为假想的投影点，使用式(1) 得到圆锥顶点$ O_{3} $的坐标以构造出假想圆锥；图 5(c)中至少有3个传感器的投影距离信息，此时采用类似图 5(b)中的步骤选取3个投影点$ O $、$ O_{1} $、$ O_{2} $构造假想圆锥。需要注意的是，所选取的假想投影点需要与检测到的实际距离最近的投影点相邻，且选出的3个投影点$ O $、$ O_{1} $、$ O_{2} $不能在连杆包围盒的同一母线上，且优先选取投影距离较近的投影点。特别地，在图 5(c)中，当部署在连杆包围盒同一母线上的传感器均检测到距离信息时，只能选用距离最近的相邻的2个投影点，此时退化为图 5(b)中的情形。通过上述方法构造出假想圆锥后，过该圆锥顶点$ O_{3} $作平行于基准平面$ XOY $的安全平面$ P $，如图 6中绿色部分所示。

所提出的基于接近觉的机械臂障碍物感知方法将障碍物简化为一个安全平面，实现了基于接近觉的机械臂障碍物感知。连杆需要到达目标位姿，而连杆与目标位姿间存在将其分割开的安全平面。连杆在运动过程中安全平面会改变，当连杆在其运动方向上的安全平面消失时，连杆可以在避开障碍物的同时到达目标位姿。

3 基于接近觉的机械臂避障路径规划（Obstacle avoidance path planning of manipulator based on proximity） 3.1 基于接近觉的人工势场法

在基于接近觉的机械臂障碍物感知方法能够准确感知障碍物后，需要合理的路径规划方法以最终实现避障。因此，需要一种高效且适用于环境未知的局部路径规划方法，该方法能够适应安全平面并随着机械臂的运动而发生变化。综合上述考虑，本文采用人工势场法进行机械臂的避障路径规划。

人工势场法在移动机器人路径规划中得到了广泛应用，而不能简化为质点的机械臂的路径规划问题则更为复杂，人工势场法需要加以改进才能应用。本文基于前文提出的障碍物感知方法，采用文[18] 中基于势场函数与关节空间的人工势场法构造机械臂的引力势能

$ \begin{align} U_{\rm{att}} ({{\mathit{\boldsymbol{\varTheta}}}_{t}})=\frac{1}{2}\zeta \rho^{2}({{\mathit{\boldsymbol{\varTheta}}}_{t}, {\mathit{\boldsymbol{\varTheta}}}_{\rm{goal}}})+k|{\mathit{\boldsymbol{\varTheta}}}_{t} -{\mathit{\boldsymbol{\varTheta}}}_{t-1} | \end{align} $

(5)

其中，$ \zeta $为引力权重系数，$ {\mathit{\boldsymbol{\varTheta}}}_{t}, {\varTheta}_{\rm{goal}} $分别为当前时间的步关节角向量与目标关节角向量，$ \rho ( {{\mathit{\boldsymbol{\varTheta}}}_{t}, {\mathit{\boldsymbol{\varTheta}}}_{\rm{goal}}}) $表示$ {\mathit{\boldsymbol{\varTheta}}}_{t} $与$ {\mathit{\boldsymbol{\varTheta}}}_{\rm{goal}} $的距离，$ {\mathit{\boldsymbol{\varTheta}}}_{t-1} $表示前一时间步关节角向量，$ k|{\mathit{\boldsymbol{\varTheta}}}_{t} -{\mathit{\boldsymbol{\varTheta}}}_{t-1} | $用于惩罚过快的关节角度变化，$ k $为关节角距离系数。斥力场方向由安全平面垂直指向避障连杆，其构造的斥力势能为

$ \begin{align} &U_{\rm{rep}} ({{\mathit{\boldsymbol{\varTheta}}}_{t}})=\\ &\begin{cases} \frac{1}{2}\eta \left({\frac{1}{\min({d_{\rm a} ({{\mathit{\boldsymbol{\varTheta}}}_{t} | \mathit{\boldsymbol{P}}}), d_{\rm b} ({{\mathit{\boldsymbol{\varTheta}}}_{t} | \mathit{\boldsymbol{P}}})})-r}-\frac{1}{d_{0}}}\right)^{2}, \\ \quad \quad 0<\min({d_{\rm a} ({{\mathit{\boldsymbol{\varTheta}}}_{t} | \mathit{\boldsymbol{P}}}), d_{\rm b} ({{\mathit{\boldsymbol{\varTheta}}}_{t} | \mathit{\boldsymbol{P}}})})-r\leqslant d_{0} \\ 0, \quad\rm{其他} \end{cases} \end{align} $

(6)

其中，$ \eta $表示斥力权重系数，$ d_{0} $表示斥力场影响距离，$ \mathit{\boldsymbol{P}} $表示当前时刻安全平面在空间中的位姿，$ d_{\rm a} ({\mathit{\boldsymbol{\varTheta}}}_{t} |\mathit{\boldsymbol{P}}) $、$ d_{\rm b} ({\mathit{\boldsymbol{\varTheta}}}_{t} |\mathit{\boldsymbol{P}}) $分别为连杆圆柱型包围盒轴线段两端点到安全平面的距离，$ r $表示包围盒半径。由式(5) 与式(6) 可得机械臂总势能：

$ \begin{align} U({{\mathit{\boldsymbol{\varTheta}}}_{t}})=U_{\rm{att}} ({{\mathit{\boldsymbol{\varTheta}}}_{t}})+U_{\rm{rep}} ({{\mathit{\boldsymbol{\varTheta}}}_{t}}) \end{align} $

(7)

在某时刻下，机器人关节$ i $角度为$ \theta_{i} $，设各关节角的搜索步长为$ \lambda $，则每个关节角有$ \{\theta -\lambda, \theta, \theta +\lambda \} $ 3种选择。设当前时间步下最优关节角组合为$ {\mathit{\boldsymbol{\varTheta}}}_{t}^{*} $，$ A_{{\mathit{\boldsymbol{\varTheta}}}} $为当前所有可能的关节角组合构成的集合，则其满足：

$ \begin{align} U({{\mathit{\boldsymbol{\varTheta}}}_{t}^{*}})=\min\limits_{{\mathit{\boldsymbol{\varTheta}}}_{t} \in A_{{\mathit{\boldsymbol{\varTheta}}}}} \{{U({{\mathit{\boldsymbol{\varTheta}}}_{t}})}\} \end{align} $

(8)

事实上，对于一般的6自由度机械臂，第1、2、3关节对其避障起主要作用，为了提高算法的实时性，在实验中仅搜索1、2、3关节角组合即可。

3.2 基于绕行法的改进算法

当构造了3.1节中的引力场与斥力场后，连杆、安全平面以及目标位姿的空间关系如图 7所示。由图 7(d)可知，人工势场法陷入了局部最优陷阱，此时有：

$ \begin{align} U({{\mathit{\boldsymbol{\varTheta}}}_{t}^{*}})=U({{\mathit{\boldsymbol{\varTheta}}}_{t-1}}) \end{align} $

(9)

即此时任何关节角组合都不能使连杆总势能下降。

人工势场法局部最优问题已有许多种改进方法^[19-21]。但当平面障碍物在空间中可无限延伸且会随着连杆运动而变化时，常规改进方法难以确保算法不重新陷入局部最优陷阱。

本文假设障碍物表面是连续且封闭的，受文[22] 中的移动机器人沿墙导航法的启发，提出了基于绕行的机械臂人工势场改进方法，当连杆沿着障碍物表面向一个方向绕行时，最终连杆必定可以避开阻挡在路径上的障碍物并到达其目标位姿。对于人类来说，当有障碍物挡在小臂前时，可以灵活地转动其肘关节躲避障碍物，因此可以定义机械臂连杆的绕行为：当连杆$ i $受到阻挡并陷入局部最优陷阱时，保持同一转向继续转动前一关节，直至安全平面的法向量发生变化时停止绕行，此时该法向量方向相对于该连杆发生改变。此状态下机械臂的引力势能为

$ \begin{align} U_{\rm{att}} ({{\mathit{\boldsymbol{\varTheta}}}_{t}})=\;&\frac{1}{2}\zeta_{1} \sum\limits_ {i\ne m} ({\theta_{it} -\theta_{{\rm{goal}}\_i}})^{2} \pm \zeta_{2} \theta_{m_{t}} + \\ & k|{\mathit{\boldsymbol{\varTheta}}}_{t} -{\mathit{\boldsymbol{\varTheta}}}_{t-1} | \end{align} $

(10)

其中，$ \theta_{it} $表示第$ i $个关节在$ t $时刻的角度，$ m_{t} $为在$ t $时刻绕行关节序号，正数$ \zeta_{1} $、$ \zeta_{2} $分别为引力权重系数与绕行系数，$ \theta_{{\rm{goal}}\_i} $表示第$ i $个关节角的目标角度。$ \pm \zeta_{2} \theta_{m} $项起绕行主导作用，需要根据实际情况选择符号，在进入绕行后该项符号不再改变，直到安全平面消失。其余项使得作为辅助的其他关节依旧被原有的目标关节位置所吸引。

如图 8所示，在上述绕行时的引力场与斥力场的共同作用下，连杆在避开障碍物的同时能离开局部最优陷阱，最终到达目标位姿。所提出的避障算法流程图如图 9所示。

4 实验（Experiment） 4.1 实验设置

采用图 10所示的TOF（飞行时间）传感器组成如2.1节中所述的阵列，并部署于UR10机器人的小臂连杆上用于接近觉感知与测距，通过串口轮询完成数据读取。为便于传感器阵列的安装、固定与拆卸，设计了如图 11所示的外壳，该外壳采用3D打印制造。将TOF传感器安装在壳体上并接通电源与串行总线。由于实验对传感器的安装精度要求不高，因此安装后无需标定。实验采用Python语言编写程序，PC上位机端通过串口读取TOF传感器阵列的距离数据，并采用全双工的TCP/IP协议与UR10机器人控制器进行网络通信，实现UR10机械臂的控制和其关节位置信息的读取。实验控制系统如图 12所示。TOF传感器单个读取周期最快可达1 ms，采用2.3节中所述的轮询方法后，一个控制周期需优先读取12个传感器，即需要12 ms，UR10机器人上位机的收发数据通信周期为8 ms，但考虑到后续工作中传感器阵列的扩展潜力以及需要留有足够的空闲周期轮询所有传感器，因此将控制周期设为80 ms，足够满足实时性即可。

实验中UR10机器人平放于水平面，初始状态到目标状态需要其基座关节顺时针摆过60$ ^{\circ} $，并且在小臂连杆的路径上放置障碍物阻挡，如图 13所示。由于小臂连杆的初始位姿平行于地面，因此当使用人工势场法规划避障路径且不采用绕行策略时，如果小臂连杆足够接近障碍物，算法一定会陷入局部最优陷阱且无法逃离。算法搜索步长采用0.05 rad且仅有第1、2、3关节参与搜索，当所有关节与其对应目标位置的偏差小于该步长时结束实验。连杆包围盒的直径设为120 mm，安全平面的斥力场影响范围为50 mm，障碍物圆锥顶角设为90$ ^{\circ} $。人工势场法的引力权重系数为0.4，斥力权重系数为0.5，关节角距离系数为0.1；绕行时引力权重系数为0.08，绕行系数为0.32，斥力权重系数为0.5，关节角距离系数为0.1。首先，采用如图 14所示的较为常见的3种形状的障碍物来验证所提算法的有效性；然后，进行由不规则形状组合而成的障碍物避障实验和同样不规则形状且尺寸较大的人体躯干的避障实验，从而验证算法的鲁棒性；最后，进行手部干涉避障实验以验证算法的实时性。

4.2 避障实验

实验过程分别如图 15~图 20所示，图 21~图 26展示了算法中参与关节角组合搜索的第1、2、3关节的角度位置变化与人工势场法的势能变化，图 21~图 26(b)中红色曲线表示原势能曲线、绿色曲线表示绕行时的势能曲线。

在3种常见形状的障碍物避障实验中，UR10机器人在第3、4阶段通过绕行成功地逃离了局部最优陷阱并且最终在第5~8阶段避开了挡在原路径上的障碍物抵达目标位姿，验证了所提算法的有效性。在进行不规则形状组合障碍物避障实验与人体躯干避障实验时，UR10机器人也成功实现了避障，前者局部最优陷阱发生在第3、4阶段，后者发生在第3、4与第5、6阶段，算法均通过绕行来逃离局部最优陷阱，说明对于多障碍物、障碍物形状不规则以及尺寸较大的情况，该算法同样适用，验证了所提算法的鲁棒性。在手部干涉避障实验中，如图 26(a)所示，UR10机器人的第1、3关节在第2、3、6阶段感应到突然出现的手部肢体时能够及时作出反应，当手部逐渐靠近连杆时，连杆相应地后退以避免碰撞并试图避开，最终在第9~12阶段UR10机器人通过绕行成功地逃离了局部最优陷阱，避开了挡在路径上的手部肢体并抵达目标位姿，这说明应对突然出现的障碍物时，本文算法也可以及时调整避障路径，验证了该算法的实时性。值得注意的是，当算法进入局部最优陷阱并采用绕行方式时，人工势场法的原势能曲线上升，在绕行时势能曲线下降。

然而，实验中传感器会通过串口互相干扰造成随机性的数据异常，这会造成构造出的安全平面发生突变从而导致了图 21 ~图 26中各关节在运行时出现轻微抖动的现象，同时也引起人工势场法势能曲线的波动。

5 结论（Conclusion）

本文提出了一种基于接近觉的机械臂避障路径规划方法，将接近觉传感器组成阵列布置在机械臂连杆表面，该方法采用了基于势函数与关节空间的人工势场法，并提出了基于绕行的改进方法以解决传统人工势场法的局部最优问题。最终，在UR10机器人上验证了所提方法的有效性、鲁棒性与实时性。