六自由度机器人（机械臂）运动学建模及运动规划系列（五）——避障路径规划

在之前的内容中，我们对机器人的运动轨迹进行了规划，但是，这种规划方法的运动路径是根据简单的轨迹人为组合起来的，具有较大的任意性。在实际的复杂工作环境中，采用人工规划路径的方法，难以保证规划的效率和准确率。
因此，本篇介绍一下机器人避障路径规划的相关知识。

一、路径规划简介

自主避障路径规划：要求利用合适的规划算法，自动生成一条从起始点到目标点的路径，并满足机器人末端和连杆的避障要求。
六轴机器人与移动机器人路径规划的不同之处在于，前者不但要保证机器人末端避开障碍物，还要考虑连杆的避障问题。
进行六轴机器人路径规划的常用方法有两种：

1. 直接对末端轨迹进行规划
   这种方法的步骤是：
   （1）采取合适的算法，对机械臂末端进行规划，形成一条满足避障条件的连接起始点和终止点的路径；
   （2）根据精度要求，选择路径上的一定数量的点，进行逆运动学计算，求出这些末端轨迹点对应的关节角序列，根据之前所述，对于六轴机器人而言，一般一个末端轨迹点对应八组关节角；
   （3）接着从起始点开始，依次对路径上的点进行如下操作：分别判断每个点对应的八组关节角是否满足机器人连杆的避障条件，即机器人在此种关节角姿态下，整个本体不能和障碍物碰撞，若八组关节角都不符合条件，则跳到第一步，重新规划；
   （4）完成所有路径点的判断后，根据一定的准则（如关节角变化之和最小等），从起始点开始，每个路径点选取一组合适的关节角，最终形成从起点到终点的关节角序列，完成规划。
2. 以关节和连杆为对象逐步推导
   这种方法的步骤是：
   从第一个关节开始，首先判断关节1在何种角度范围内，连杆1会与障碍物碰撞，求出碰撞临界角；
   接着在关节1碰撞角之外（即保证连杆1不与障碍物发生碰撞的角度范围），判断关节2的碰撞临界角……依次类推，直到最后一个关节和连杆。
   本篇使用这种方法。

二、改进RRT算法

1. 算法简介

本篇的规划使用改进RRT算法，首先对算法做一个简单的介绍。
快速扩展随机树算法，简称RRT算法。其核心思想是以起始点为树的根节点，接着在空间中利用随机采样的方式生成新节点，如果新的节点与障碍物不发生碰撞，则将该节点加入到随机树上，作为树的子节点[。通过这种方式，随机树在空间中进行扩展，直至找到终止点。最后从终止点反向溯源到起始点，就可以找到规划的路径。这种方法以随机采样的方式探索整个空间，具有以最快的速度遍历整个空间的优点。
RRT算法分为两个步骤：
第一步：通过随机在空间中生成节点的方式，使随机树生长，直到终止点也包含在随机树中。
第二步：从终止点反向溯源，得到连接起始点和目标点的最短路径。

2. 改进点

在标准RRT算法中，确定新节点的前提是在空间中生成一个随机节点，使得随机树的生长没有方向性，算法收敛慢。
针对这个问题，结合人工势场法中引力场和斥力场的概念，将目标引力函数加入到原算法中，对原算法的节点生成方式进行改进，从而使随机树在引力函数的作用下，朝着终止点的方向生长。改进后的RRT算法修正了随机树收敛速度慢的缺点。
生成新节点时，原算法采用：

式中，xr代表产生新节点时的随机生成节点，xn代表距离随机点最近的子节点，s代表步长，xne代表最终生成的新节点。
而改进后的算法采用：

式中xe代表目标节点，kp代表引力分量，值越大，引力分量的调控力度越强，但过大会使得随机树的生长失真，可自行调整，取最佳结果。

3. 仿真结果

RRT算法的仿真结果如下：

矩形框代表障碍物，红色线条为最终生成的结果。
代码如下所示：
RRT算法避障路径规划代码

三、避障路径规划

1. 障碍物包络

在进行映射前，首先需要得到障碍物的具体形状。实际环境中，障碍物通常没有规则的形状，难以精确描述。因此一般采用标准的形状包络障碍物，这里采用球体包络障碍物，同时用圆柱体包络连杆。这样问题就简化为圆柱体与球体的碰撞问题。

2. 递推确定碰撞临界角

对于一般的六自由度机器人，其前三个关节决定机器人末端的位置，后三个关节决定末端的姿态。因此考虑杆件与障碍物碰撞的问题时，只需要考虑前三个关节。本节为简化起见，只研究机器人在竖直平面内的避障问题。将模型向XOZ方向投影，可以得到连杆与障碍物的几何关系，
（1）关节2

连杆1与障碍物发生碰撞时θ_2的临界角度为：

（2）关节3
在关节2的碰撞范围外，关节3的临界碰撞角可根据下图推导：

设障碍点与关节3中心的距离为m，在m取值一定范围内，对于每一个θ_2，θ_3均有一个上下碰撞临界角：

3. 路径规划

完成碰撞角的确定后，就可以在关节空间，使用算法进行规划。
这里使用改进前后的RRT算法分别进行规划，结果如图：

中间的条状即为碰撞角范围，即障碍物，红色线条为最终生成的轨迹。

4. Matlab仿真

规划完成后，可以使用Matlab的机器人工具箱进行验证，形成如图所示的仿真结果

图中，右上角的小球为障碍物，可以看出机械臂末端和连杆均避开了障碍物。

四、小结

本篇主要介绍了自主避障路径规划的主要方法，采用改进RRT算法以及递推法进行了简单的规划示例。
至此，这个系列的所有内容就都写完了。鉴于当时时间有限，很多地方的介绍比较简略，如果大家还有什么想要了解的内容，可以评论，之后有时间我会接着补充的。
最后，非常感谢大家的阅读，希望大家都能常怀热情，为自己所向往的事情奋斗着。