基于任务空间的机器人实时运动自碰撞避免控制方法

技术领域

本发明涉及一种机器人控制方法，尤其是涉及一种基于任务空间的机器人实 时运动自碰撞避免控制方法。

背景技术

随着机器人技术的发展、机器人应用领域也开始从传统的工厂制造领域拓展到 家庭、医院、老年关怀中心、军事、旅游、运输、勘探、抢险救灾等现场和服务领 域。在人类社会漫长的发展过程中，人类日常生活的环境已逐渐被改造的适合人类 感官和行为特性，这就要求与人类共存并服务于人类的智能机器人能够模仿人类的 外观与行为处理事务。自上世纪70年代初，第一台双足步行机器人WAP-1诞生以 来，双足步行机器人技术一直是热点研究课题之一，人们一直在不断地尝试制造出 更加与人类行走类似的双足步行机器人。

在机器人的运动规划过程中，规划目标多为机器人的末端轨迹，但由于并不考 虑机器人的三维几何信息，所以可能导致机器人的自我碰撞，即身体的两个部位发 生碰撞。例如，在行走过程中，摆动的手臂可能会与腿部发生碰撞，这些碰撞可能 会导致规划目标不能完成，甚至使机器人失去平衡或控制，对机器人和工作环境产 生损坏。近些年来，很多学者和机构考虑研究了自碰撞的避免，通过建立避障模型 的控制算法，如人工势场法(APF)以及基于采样的方法，如快速拓展随机树(RRTs)、 概率地图(PRMs)等，实现了通过离线学习、训练的自碰撞避免的方法。这类方 法可以对规划的目标轨迹进行学习，使得机器人在进行轨迹规划的同时，避免了自 碰撞。

但是目前对于自碰撞的研究大多集中在采用学习的方法，这类方法对于人型机 器人由于关节多自由度高而产生的高维度配置空间(C-Space)需要花费大量的计 算时间，不适用于实时的运动规划。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于任务空 间的机器人实时运动自碰撞避免控制方法，该方法可以在机器人实时运动规划的过 程中，避免自碰撞的发生，促进了机器人的现场应用。

本发明的目的可以通过以下技术方案来实现：

一种基于任务空间的机器人实时运动自碰撞避免控制方法，该方法包含对机器 人的末端轨迹跟踪，具体包括以下步骤：

1)根据机器人的三维几何信息利用包围体进行建模，将机器人的身体分割为 不同的区域块，对每两个区域块所构成的区域对进行自碰撞检测；

2)若检测到区域对会发生自碰撞，则在任务空间内增加一个用于调整运动轨 迹的自碰撞避免任务；

3)同时完成自碰撞避免任务和末端轨迹跟踪任务。

步骤1)中自碰撞检测的具体过程为：

11)根据机器人的三维几何信息利用包围体进行建模，将机器人的身体分割为 不同的区域块，并将每两个区域块划分为一组区域对；

12)利用最优化方法离线学习产生自碰撞的候选区域对，减少实时检测时需要 检测的区域对数量，

13)采用分离轴方法对候选区域对进行碰撞状态检测。

与现有技术相比，本发明通过建立三维空间模型表示，对可能发生自碰撞的区 域块进行检测与预估，同时通过离线学习引入碰撞候选区域对，并通过建立动态的 任务空间分配来同时实现末端轨迹跟踪以及自碰撞避免，使机器人在实时规划的过 程中，避免了自碰撞的发生，促进了机器人的现场应用。

附图说明

图1为本发明自碰撞避免的主要流程图；

图2为根据机器人三维几何信息所建立的区域块的集合图；

图3为区域对内距离计算的原理图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

一种基于任务空间的机器人实时运动自碰撞避免控制方法，该方法含对机器人 的末端轨迹跟踪，具体的步骤如图1所示：

1)根据机器人的三维几何信息利用包围体进行建模，将机器人的身体分割为 不同的区域块，对每两个区域块所构成的区域对进行自碰撞检测，

其中，自碰撞检测包括了以下几个步骤：

11)根据机器人的三维几何信息利用包围体进行建模，将机器人的身体分割为 N个区域块如图2所示。然后将从N个区域块中任意取两个区域块定 义为区域对这样检测机器人是否发生了自碰撞可以定义为检测每 个区域对之间的距离是否小于阀值，对每个需要检测的区域对运用分离轴 (SAT)算法可以求得该区域对上最近邻的两个点D_i和D_j，两点之间的距离可以 表示为其中是点D_i在坐标系中的位置空间向量，它的求解 可以由求得，可以看出实际上每个区域块的距离是关于关节配置空 间C-Space的函数。

12)实际运动设计中，由于机器人的关节范围，速度约束，刚体长度等机械约 束，机器人末端存在一个可达到的最大区域，同样每个区域对由于这些约束，存在 一个可能达到的最小距离，如果这个最小距离大于一定的阀值，也就是该区域对实 际上在运动规划中，无论与取什么值，都不会发生碰撞，这样在实时进行碰撞检测 时，实际上并不需要对所有的区域对进行检测。

因此，利用最优化方法(SQP)离线学习产生自碰撞的候选区域对，减少实时 检测时需要检测的区域对数量。在本实施例中，对以下几个约束条件进行了考虑： 1，关节空间的极值2，在同一个关节链上的区域块不会产生碰撞。 3，需要具体的计算方程如下：

$\mathrm{st}:\min f\left(q\right)=d^2=(\overline{P_i^D}-\overline{P_j^D})$

condition：

$\left(\begin{array}{c}{q}_x^{\min }\le q_x\le q_x^{\max }\\ d_{\mathrm{mn}}\ge d_b\end{array}\right)$

为了求解最小值，该方程对于关节的偏微分方程需要求得：

$\frac{\partial f}{\partial q}=\frac{\partial \left(d^2\right)}{\partial q}=2*(\overline{P_i^D}-\overline{P_j^D})*(J_i^D-J_j^D)*\mathrm{\Delta q}$

其中，其中和是最近邻点对的位置关于关节角度的雅可比矩阵，其得 到的可能发生碰撞的区域对，则为需要进行自碰撞检测的候选区域对。

13)对于这些候选区域对内的区域块，采用步骤1)中的分离轴(SAT)算法 对候选区域对进行碰撞状态检测；

2)如果存在距离小于阀值的情况，也就是说明该区域对即将发生了碰撞，为 了避免该碰撞的发生，则在任务空间内增加一个用于调整运动轨迹的自碰撞避免任 务，进行自碰撞的避免。该任务的任务描述为使得变大，对于机器人的关节空 间到任务空间的映射为非线性方程t＝h(q)，对该方程进行逆运动计算，该方程可 能有无数解，所以运用微分方程解，即关节空间的速度来代替关节空间本身进行计 算，这样自碰撞任务可以描述为定义该任务为其中h(d)为控制距离变化方向，v(d)为控制距离变化速率，其中，控制距离变化方 向类似于人工势场法(APF)为沿着梯度方法，使得下降的最快方向，如果下降方 向和末端轨迹方向一致，为了避免局部最优，增加一个随机的方向向量以避免，其 方程为：

$h\left(d\right)=\left(\begin{array}{c}\stackrel{·}{d}\\ \stackrel{·}{d}+r\left(\theta \right)\end{array}\right)$

而对于控制距离变化速率，当区域对距离越近时，需要一个越大的速率使得两 区域对避开，采用了一种三次递减曲线：

$v\left(d_k\right)=v_0*a_h=\left(\begin{array}{cc}\frac{v_0}{{(d_m-d_k)}^3}& ,d_k<d_m\\ 0& ,\mathrm{else}\end{array}\right)$

其中v₀为一恒定变量，d_m为阀值，该函数可以满足递减的特性。

3)此时，自碰撞避免任务已经作为一个任务引入了任务空间，当该任务和末 端轨迹跟踪任务需要同时完成时，引入任务零空间N＝I-J^#*J，并同时完成两个 任务。