一种基于多个控制点的空间机器人避撞方法

摘要

本发明提供一种基于多个控制点的空间机器人避撞方法，轨迹规划时机械臂关节角速度平滑，避免其发生不连续，能够抑制由单个控制点跳变引起的振动问题。上述方法包括：(1)将障碍物处理成由三角面构成的非凸多面体，将机械臂处理成线段，确定描述线段与三角形之间的碰撞检测算法，得到机械臂和障碍物之间距离最近的一对控制点；(2)建立空间机器人考虑避撞任务的多任务运动学方程；(3)建立空间机器人的避撞约束条件，结合(2)的运动学方程进行避撞求解；(4)根据多个控制点的参数设计调整新控制点参数，结合(2)的运动学方程进行避撞求解；(5)根据多个控制点对一个障碍物的避撞求解，对空间机器人的机械臂进行多障碍物的避撞控制。

权利要求

1.一种基于多个控制点的空间机器人避撞方法，其特征在于，包括，步骤一，将障碍物处理成由三角面构成的非凸多面体，将机械臂处理成线段，确定描述线段与三角形之间的碰撞检测算法，得到机械臂和障碍物之间距离最近的一对控制点；

步骤二，建立空间机器人考虑避撞任务的多任务运动学方程；

步骤三，基于一对控制点建立空间机器人的避撞约束条件，结合步骤二得到的运动学方程进行避撞求解；

步骤四，基于多个控制点建立新控制点的运动信息，根据多个控制点的参数设计调整新控制点参数，结合步骤二得到的运动学方程进行避撞求解；

步骤五，根据多个控制点对一个障碍物的避撞求解，按空间机器人机械臂与障碍物的距离远近加入权重因子，对空间机器人的机械臂进行多障碍物的避撞控制；

步骤一的具体步骤如下，

步骤1.1，定义严格凸目标和Voronoi区域，

定义严格凸目标，令 表示三维空间R3一个闭集， 是 的内部，对于且0＜λ＜1，当且仅当 时，认为 是严格凸的；

定义Voronoi区域，VR(Y)表示特征为Y的Voronoi区域；关于特征为Y的一个Voronoi区域为相比于任何其它特征，最靠近特征Y的点集；

步骤1.2，根据严格凸目标和Voronoi区域的定义，障碍物不是严格凸目标，将其处理成由若干三角形基础面组成的多面体；根据三角形的七个特征，三角形的面、三条边以及三个顶点，通过一个三角形将三维空间分为七个Voronoi区域，当一条线段位于三角形的不同Voronoi区域时，利用不同的函数计算距离最近的一对控制点；当一条线段位于不止一个Voronoi区域中时，利用VR_clip算法搜索距离最近的一对控制点。

2.根据权利要求1所述的一种基于多个控制点的空间机器人避撞方法，其特征在于，步骤二的具体步骤如下，空间机械臂末端执行器的主要任务 和次要避障任务 描述如下：考虑任务优先级满足所有目标的逆运动学解法描述如下，不考虑零空间运动h＝0：其中， 是Je零空间中矩阵Jo的投影。

3.根据权利要求2所述的一种基于多个控制点的空间机器人避撞方法，其特征在于，步骤三的具体步骤如下，障碍物稳定时，分别从机械臂和障碍物上选取点P1、P1'为一对控制点，||P1P1'||距离的导数，即避撞约束，表示如下：其中，n1是 的单位矢量，符号表示矢量u和矢量v的内积；

用一个新雅可比矩阵 替换逆运动学解中的Jo，表示如下：完成主要任务和避障任务的逆运动学新解表示如下：

4.根据权利要求3所述的一种基于多个控制点的空间机器人避撞方法，其特征在于，步骤四中的具体步骤如下，在机械臂上选取一个结合P1、P2两个控制点信息的新控制点P0，用新的控制点P0代替两个控制点P1、P2，描述机械臂运动；新控制点P0在机械臂上连续运动，计算如下：其中，d1＝||P1P1'||、d2＝||P2P2'||分别是P1、P2与障碍物之间的距离；由于控制点P1更为邻近，令相应的逃逸单位矢量n0等于n1，期望的临界点速度 vo是指定的标称速度，αc是避撞增益，其值取决于与障碍物的临界距离：定义一个新的临界距离dsr，称为安全距离；如果机械臂距离障碍物太近，即d1≤duf，则取消由调整参数αe控制的主要任务 根据机械臂动力学特性，能够预设或解算非安全距离duf的大小；

当实现平滑运动时，在dsr处， 的大小为0；则考虑避障任务的逆运动学解表示如下：调整参数定义如下：

5.根据权利要求4所述的一种基于多个控制点的空间机器人避撞方法，其特征在于，步骤五的具体步骤如下，考虑影响区内所有障碍物，将所有障碍物的逆运动学解通过齐次解的加权和表示如下：其中，n0是影响区内障碍物数目，权重因子ωi定义如下：权重因子ωi表示了影响区内不同障碍物的相对重要性；当一个障碍物比其他障碍物更靠近机械臂时，即d1i→0，相应的权重因子增加，对应控制点处速度也增加；从而能够根据权重因子，调整控制点处远离障碍物的速度，对空间机器人的机械臂进行多障碍物的避撞控制。

说明书

一种基于多个控制点的空间机器人避撞方法

技术领域

[0001] 本发明涉及空间机器人执行任务时的避撞技术，具体为一种基于多个控制点的空间机器人避撞方法。

背景技术

[0002] 卫星维护、在轨装配和空间碎片去除的需求日益增长，要求空间机器人在特定恶劣的空间环境中执行任务。具体事例有“机器人技术试验(ROTEX)”、“工程试验卫星VII(ETS-VII)”、“轨道快车(OE)”等。根据目前世界各空间机构计划的空间机器人，在空间任务中机器人数量和性能的增加在未来已成定局。为了有效地执行多项任务，研究人员主要关注在有障碍物的工作空间中空间机器人的运动控制问题。由于空间环境的不确定性，对于模型未知、运动的障碍物必须进行在线避撞规划与控制。因此，需要发展避撞技术以应对空间机器人在轨服务的运动控制问题。

[0003] 现有技术的文献中已经提出了很多空间机器人在执行任务时的避障方法和策略。最为经典的是Faverjon和Tournassoud提出的基于局部的避撞规划方法，通过减小期望速度和避撞不等式约束下的规划速度之间的误差，从而规划机器人的运动。此方法要求机器人和障碍物必须是严格凸的目标，这在实际中很难实现。Helguera和Zeghloul对此方法做了改进，通过修改任务描述避免了局部最小值的问题。Schlemmer等人提出了一种基于距离梯度的在线避撞方法。Sentis和Khatib提出了一种利用势场的局部避撞方法。

[0004] 以上避障方法的研究主要关注将机器人和障碍物处理成严格凸目标后的避撞问题。然而真实物理环境中，只有球、椭圆等可视为严格凸目标。由于目标是非严格凸的，因此目标上控制点运动时会产生不连续的情况。如果这种情况频繁地发生，由于控制点跳变会导致机械臂振动。关于这种现象Kanoun等人采用了一种多速度阻尼约束方法，可以抑制机械臂在含障碍物的工作空间中运动时产生的振动问题。但这种方法约束过程复杂，大大增加了计算量。

发明内容

[0005] 针对现有技术中存在的问题，本发明提供一种基于多个控制点的空间机器人避撞方法，轨迹规划时机械臂关节角速度平滑，避免其发生不连续，能够抑制由单个控制点跳变引起的振动问题。

[0006] 本发明是通过以下技术方案来实现：

[0007] 一种基于多个控制点的空间机器人避撞方法，包括，

[0008] 步骤一，将障碍物处理成由三角面构成的非凸多面体，将机械臂处理成线段，确定描述线段与三角形之间的碰撞检测算法，得到机械臂和障碍物之间距离最近的一对控制点；

[0009] 步骤二，建立空间机器人考虑避撞任务的多任务运动学方程；

[0010] 步骤三，基于一对控制点建立空间机器人的避撞约束条件，结合步骤二得到的运动学方程进行避撞求解；

[0011] 步骤四，基于多个控制点建立新控制点的运动信息，根据多个控制点的参数设计调整新控制点参数，结合步骤二得到的运动学方程进行避撞求解；

[0012] 步骤五，根据多个控制点对一个障碍物的避撞求解，按空间机器人机械臂与障碍物的距离远近加入权重因子，对空间机器人的机械臂进行多障碍物的避撞控制。

[0013] 优选的，步骤一的具体步骤如下，

[0014] 步骤1.1，定义严格凸目标和Voronoi区域，

[0015] 定义严格凸目标，令O表示三维空间R3一个闭集，χ(O)是O的内部，对于且0＜λ＜1，当且仅当λA+(1-λ)B∈χ(O)时，认为O是严格凸的；

[0016] 定义Voronoi区域，VR(Y)表示特征为Y的Voronoi区域；关于特征为Y的一个Voronoi区域为相比于任何其它特征，最靠近特征Y的点集；

[0017] 步骤1.2，根据严格凸目标和Voronoi区域的定义，障碍物不是严格凸目标，将其处理成由若干三角形基础面组成的多面体；根据三角形的七个特征，三角形的面、三条边以及三个顶点，通过一个三角形将三维空间分为七个Voronoi区域，当一条线段位于三角形的不同Voronoi区域时，利用不同的函数计算距离最近的一对控制点；当一条线段位于不止一个Voronoi区域中时，利用VR_clip算法搜索距离最近的一对控制点。

[0018] 进一步的，步骤二的具体步骤如下，

[0019] 空间机械臂末端执行器的主要任务 和次要避障任务 描述如下：

[0020]

[0021] 考虑任务优先级满足所有目标的逆运动学解法描述如下，不考虑零空间运动h＝0：

[0022]

[0023] 其中， 是Je零空间中矩阵Jo的投影。

[0024] 再进一步的，步骤三的具体步骤如下，

[0025] 障碍物稳定时，分别从机械臂和障碍物上选取点P1、P1'为一对控制点，||P1P1'||距离的导数，即避撞约束，表示如下：

[0026]

[0027] 其中，n1是 的单位矢量，符号表示矢量u和矢量v的内积；

[0028] 用一个新雅可比矩阵 替换逆运动学解中的Jo，表示如下：

[0029]

[0030] 完成主要任务和避障任务的逆运动学新解表示如下：

[0031]

[0032] 再进一步的，步骤四中的具体步骤如下，

[0033] 在机械臂上选取一个结合P1、P2两个控制点信息的新控制点P0，用新的控制点P0代替两个控制点P1、P2，描述机械臂运动；新控制点P0在机械臂上连续运动，计算如下：

[0034]

[0035] 其中，d1＝||P1P1'||、d2＝||P2P2'||分别是P1、P2与障碍物之间的距离；由于控制点P1更为邻近，令相应的逃逸单位矢量n0等于n1，

[0036] 期望的临界点速度 vo是指定的标称速度，αc是避撞增益，其值取决于与障碍物的临界距离：

[0037]

[0038] 定义一个新的临界距离dsr，称为安全距离；如果机械臂距离障碍物太近，即d1≤duf，则取消由调整参数αe控制的主要任务 根据机械臂动力学特性，能够预设或解算非安全距离duf的大小；

[0039] 当实现平滑运动时，在dsr处， 的大小为0；则考虑避障任务的逆运动学解表示如下：

[0040]

[0041] 调整参数定义如下：

[0042]

[0043] 再进一步的，步骤五的具体步骤如下，

[0044] 考虑影响区内所有障碍物，将所有障碍物的逆运动学解通过齐次解的加权和表示如下：

[0045]

[0046] 其中，n0是影响区内障碍物数目，权重因子ωi定义如下：

[0047]

[0048] 权重因子ωi表示了影响区内不同障碍物的相对重要性；当一个障碍物比其他障碍物更靠近机械臂时，即d1i→0，相应的权重因子增加，对应控制点处速度也增加；从而能够根据权重因子，调整控制点处远离障碍物的速度，对空间机器人的机械臂进行多障碍物的避撞控制。

[0049] 与现有技术相比，本发明具有以下有益的技术效果：

[0050] 本发明提出了一种基于多个控制点的空间机器人避撞方法，是在空间范围内的一种新的方法，通过考虑真实物理环境中障碍物的非凸特性，结合主要的运动任务和次要的避障任务，不仅不会对机械臂末端执行器执行主要任务产生影响，而且建立了更精准的避撞约束条件，能够自主调节控制点，实现对空间机器人的避撞。

[0051] 进一步的，基于多个控制点时，通过在安全距离处对次要任务的限定，使得轨迹规划时机械臂关节角速度平滑，避免其发生不连续。

附图说明

[0052] 图1为本发明实例中所述的基于多个控制点避撞方法的空间机器人系统示意图。

[0053] 图2为本发明实例中所述线段与三角形空间关系示意图。

[0054] 图3为本发明实例中所述线段与线段、线段与三角形间碰撞检测算法。

[0055] 图4为本发明实例中不加避撞方法的仿真结果。

[0056] 图5为本发明实例中基于一对控制点避撞方法的仿真结果。

[0057] 图6为本发明实例中基于两对控制点避撞方法的仿真结果。

具体实施方式

[0058] 下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

[0059] 本发明基于多个控制点，提出一种适用于动态环境的空间机器人避撞方法。考虑到障碍物的非凸特性，借助Voronoi区域的概念，碰撞检测时建立了更精确的避撞约束。基于多个控制点，可以抑制由单个控制点跳变引起的振动问题，保证轨迹规划时机械臂关节角速度平滑。具体的避撞方法如下。

[0060] 步骤一：将障碍物处理成由三角面构成的非凸多面体，描述线段与三角形之间的碰撞检测算法，如图1、图2和图3所示。

[0061] 首先定义严格凸目标和Voronoi区域：

[0062] 严格凸目标：令O表示R3一个闭集，χ(O)是O的内部。对于 且0＜λ＜1，当且仅当λA+(1-λ)B∈χ(O)时，认为O是严格凸的。

[0063] Voronoi区域(VR)：对于特征Y的VR(Y)。关于特征Y的一个Voronoi区域是，相比于任何其它特征而言，最靠近特征Y的点集。

[0064] 线段由一条边ε和两个端点Vi(i＝1,2)组成，边和端点是线段的三个特征。依据这三个特征，线段周围的三维空间被分为三个Voronoi区域。一条运动的线段可处于任意某个线段的Voronoi区域中，或同时处于不止一个Voronoi区域中。根据VR_clip算法可将此线段分割成若干线段。这是两条线段之间的碰撞检测算法，它是线段与三角形之间碰撞检测算法的基础。

[0065] 由于障碍物不是严格凸目标，可处理成由一些基础面如三角形组成的多面体。根据三角形的七个特征，即三角形的面、边以及顶点，一个三角形可以将三维空间分为七个Voronoi区域。当一条线段位于三角形的不同Voronoi区域中时，利用不同的函数计算距离最近的一对控制点。当它位于不止一个Voronoi区域中时，利用VR_clip算法搜索距离最近的一对控制点。

[0066] 步骤二：建立空间机器人考虑避撞任务的多任务运动学方程。

[0067] 空间机械臂末端执行器的主要任务 和次要避障任务 描述如下：

[0068]

[0069] 这里，考虑任务优先级满足所有目标的逆运动学解法描述如下，不考虑零空间运动h＝0：

[0070]

[0071] 其中， 是Je零空间中矩阵Jo的投影。此方程中的每一项均有意义。第一项 保证了主要任务的实现(为保证稳定跟踪，通常采用 而
非 )。第二项，投影在末端执行器雅克比矩阵的零空间，描述了不受末端执行器运动影响的控制点运动。 项描述了由于末端执行器运动诱导的控制点的运动，矩阵 结合了末端执行器的零空间和控制点，用以将操作空间中分配的控制点速度转化到关节空间中。

[0072] 步骤三：基于一对控制点建立空间机器人的避撞约束条件，结合运动学方程求解。

[0073] 考虑笛卡尔空间，如果想驱动避撞方法并且给控制点分配速度，那么 则是一个三维矢量，即Jo∈R3×n，需要3个额外冗余度去完成三维笛卡尔空间中的避撞。然而，避撞方法仅需要控制控制点和障碍物上相应控制点连线方向上的逃逸速度，这是一个一维约束，一个冗余度足以规避碰撞。障碍物稳定时，分别从机械臂和障碍物上选取点P1、P1'为一对控制点，||P1P1'||距离的导数，即避撞约束，表示为下：

[0074]

[0075] n1是 的单位矢量，符号表示矢量u和矢量v的内积。现用一个新雅可比矩阵 替换逆运动学解中的Jo，那么速度 和 就变成了标量，显著地提高了Moore-Penrose伪逆的计算效率，表示如下：

[0076]

[0077] 由于 是标量，在计算 的伪逆时不需要对任何矩阵求逆。轨迹规划时大大减少了计算量。因此，完成主要任务和避障任务的逆运动学新解可表示如下：

[0078]

[0079] 此解隐含的一个问题是，若雅克比矩阵Je或 不满秩，机械臂就会发生奇异，从而导致关节角速度趋近无穷大。

[0080] 步骤四：如图1所示，基于多个控制点建立新控制点的运动信息，设计调整参数结合运动学方程求解。

[0081] 为实现更可靠的避撞，在机械臂上选取一个结合P1、P2两个控制点信息的新控制点P0。此外，新控制点P0在机械臂上连续运动。计算如下：

[0082]

[0083] 其中，d1＝||P1P1'||、d2＝||P2P2'||分别是P1、P2与障碍物之间的距离。由于控制点P1更为邻近，令相应的逃逸单位矢量n0等于n1，期望的临界点速度 vo是指定的标称速度，αc是避撞增益，其值取决于与障碍物的临界距离：

[0084]

[0085] 定义一个新的临界距离dsr，称安全距离。如果机械臂距离障碍物太近，即d1≤duf，则取消由调整参数αe控制的主要任务 根据机械臂动力学特性，可以预设或解算非安全距离duf的大小。

[0086] 为实现更平滑的运动，在dsr处，令 的大小为0。考虑避障任务的逆运动学解可表示如下：

[0087]

[0088] 调整参数定义如下：

[0089]

[0090] 从式(8)可以看出，控制点P1进入影响区之后，即d1＜dif，避撞方法开始影响机械臂构型，此时仅包含部分齐次解。随着控制点与障碍物之间距离的减小，在dif到dsr之间，执行更多的齐次解以减小控制点和障碍物之间的相对速度。一旦控制点距离出现在dsr范围以内，分配一个包含完整齐次解的跳变速度 此避撞方法可以保证控制点在危险区域外移动，其次，在机械臂运动过程中控制点P1发生跳变时，仍可保证其平滑转变。

[0091] 步骤五：将基于多个控制点的避撞方法拓展应用于多个障碍物的情形。

[0092] 当工作空间中有多个障碍物时，由于缺少冗余度，不能按处理多子任务的方式处理不同的障碍物。此时，若仍不加修改地应用式(8)，机械臂上的控制点会频繁地跳变，进而导致关节角速度振荡。关节角速度不连续对机械臂结构和驱动器造成十分不利。为了提高性能，考虑影响区内所有障碍物，齐次解的加权和表示如下：

[0093]

[0094] 其中，n0是影响区内障碍物数目，权重因子ωi定义如下：

[0095]

[0096] 权重因子ωi表示了影响区内不同障碍物的相对重要性。当一个障碍物比其他障碍物更靠近机械臂时，即d1i→0，那么相应的权重因子增加，控制点速度也随之增加。

[0097] 仿真结果表明，所提的避撞方法不影响机械臂末端执行器执行主要任务，而且具有自主调节控制点、平滑关节角速度，避免其发生不连续的优点，可以扩展到未来应用中。

[0098] 图4显示了未加入避障方法时，多关节运动冗余空间机器人末端穿过类似走廊的运动情形。空间机器人可以精确地实现对末端路径的跟踪，但是，由于未考虑避障问题，空间机器人与右侧的障碍物发生了碰撞。图5显示了仅考虑单个控制点避障方法的仿真情形。可以看出，空间机器人仍能够实现对末端路径的精确跟踪，同时实现了对右侧障碍物的规避，但是由于存在控制点的切换问题，故空间机器人关节角速度存在跳变现象，这对于关节的使用寿命十分不利。图6显示了考虑两个控制点避障方法的仿真情形。与图4和图5类似，其末端可以精确实现对路径的跟踪，并且由于采用多个控制点进行避障，空间机器人各关节的角速度更为平滑，不存在角速度跳变现象，这大大有利于关节电机寿命的延长，且为后续空间机器人轨迹规划提供了较好的条件。