CN109927035A - 一种多臂机器人c-空间线障碍的映射方法 - Google Patents

Abstract

本发明公开了一种多臂机器人C‑空间线障碍的映射方法，涉及机器人避障控制领域。C‑空间是一种常用的障碍物映射空间，将工作空间中的障碍物简化为点、线等模型，通过求解简化机械臂模型对应特征点、线的临界碰撞角，得到C‑空间障碍的上边界和下边界，再通过它们的并集得到障碍物在C‑空间的整个映射边界。本发明提出一种新的特征线段的C‑空间映射方法，将线段与模型的干涉位置分为四种情况进行讨论，得出相应的数学模型和临界碰撞角求解方法，并最终归纳出平面内任意线段的临界碰撞角求解方法。该方法的优势是尽可能考虑了线障碍与机械臂的所有干涉形式，降低机械臂自由空间求解的计算量，避免多臂机器人各关节之间的碰撞。

Description

一种多臂机器人C-空间线障碍的映射方法

技术领域

本发明属于机器人避障控制领域，具体涉及一种多臂机器人C-空间线障碍的映射方法。

背景技术

目前在生活及医疗领域使用最多的是多臂机器人及其系统，多臂机器人的机械臂之间可以协同操作，完成更加复杂的动作，实现更加多样的功能。但是多臂机器人的路径规划控制比常见的串联型工业机器人要复杂，如何准确有效的获得机械臂的最优无障碍路径是目前研究的热点之一。

机器人构型和工作空间描述的不同，路径规划的方法也不同，但都是基于欧式空间和C-空间两种规划空间来进行的。欧式空间也被称之为机器人工作空间，该空间方法常用在串联型工业机器人的路径规划中，因为工业机器人各关节是串联在一起的，容易通过雅克比矩阵实现笛卡尔坐标系和关节坐标系的转换，但计算量比较大。多臂机器人的机械臂比较复杂，除了机械臂自身各关节之间的碰撞可能之外，还存在机械臂与机械臂之间关节的碰撞，如果采用欧式空间法，由于障碍物的存在，机械臂不能达到全部位姿，但是在C-空间中可以将机械臂的障碍位姿表示为特征点、线，将机械臂与特征点、线碰撞形成的上下临界碰撞角计算出来，求得障碍空间，补集则为自由空间。在自由空间中的点所对应的机械臂位姿，都不会与障碍发生碰撞，该方法避免了大量繁琐的计算，具有较高的计算效率。通过相应的搜索算法，就可以在自由空间内寻找连接初始位姿点和目标位姿点的无碰撞路径，跟据该路径进行运动控制。

发明内容

本发明针对上述问题，对C-空间的障碍映射数学模型进行研究，提供了一种多臂机器人C-空间线障碍的映射方法。将工作空间中的障碍简化为特征线段P ₁ P ₂，P ₁和P ₂为线段的两个端点，令其垂直于X轴，且位于第一象限。将机械臂简化为二连杆结构，分为大臂和小臂，其中大臂绕自身轴线旋转的最大距离为L ₁，小臂绕自身轴线旋转的最大距离为L ₂，机械臂绕其大臂自身轴线旋转的最大距离为l _max；设大臂的回转中心为原点O，以原点O为圆心，l _max为半径，做圆O _max，根据圆O _max与线段P ₁ P ₂的位置，可分为四种干涉情况讨论，其中小臂回转中心ro ₁到线段P ₁ P ₂所在直线M的距离为d _rol ，ro ₁与P ₁的距离为d _rop1，ro ₁与P ₂的距离为d _rop2，原点O与P ₁距离为d _op1；分别计算出这四种情况的临界碰撞角，就可以得到基于C-空间的线障碍映射空间。

第一种情况是P ₁ P ₂与O _max没有交点，且P ₁、P ₂都位于圆外，线段P ₁ P ₂的C-空间障碍映射为空集；第二种情况是P ₁ P ₂与O _max有一个交点，线段P ₁ P ₂的C-空间障碍映射，与点P ₁的C-空间障碍映射是同样的；第三种情况是P ₁ P ₂与O _max有一个交点，且P ₁位于圆O _max内；第四种情况是P ₁ P ₂与O _max没有交点，且P ₁、P ₂均位于圆内。

优选的，在第三种情况下，当d _op1≥L ₁时，点P ₁不会对大臂造成运动障碍，只会对小臂造成运动障碍；当d _op1<L ₁时，点P ₁对大臂和小臂都会造成运动障碍；当大臂发生碰撞时，无论小臂取何角度，机器人都会发生碰撞，大臂不发生碰撞时，小臂碰撞的情况与d _op1≥L ₁情况下得出的结论是一样的。

优选的，在第四种情况下，当d _rol ≥L ₂时，小臂不会与线段P ₁ P ₂发生碰撞，C-空间障碍映射为空集；当d _rol <L ₂时，小臂与线段P ₁ P ₂发生碰撞。

优选的，在求解小臂的临界碰撞角计算公式时，分为两种情况：一种是小臂与线段端点发生碰撞，第二种是小臂与线段内部点发生了碰撞，总共对应四条碰撞角公式；d _rol 和d _rop1的值会影响到小臂的下临界碰撞关节角的取值形式；d _rol 和d _rop2的值会影响到小臂的上临界碰撞关节角的取值形式；当线段对大臂构成运动障碍时，大臂的下临界碰撞角取值与d _op1的大小有关。

优选的，对于平面内的任意线段P ₁ P ₂的映射方法，从原点O先作线段P ₁ P ₂的垂线OH，设点H的极坐标形式为H=(r _OH ，θ _OH )，然后进行坐标变换，将原坐标OXY旋转角度θ _OH 得到新坐标，线段就会垂直于X'轴，只需在新坐标系下分析得出临界碰撞关节角，再转换回原坐标即可。

与现有技术相比，本发明的有益效果是：将线障碍与机械臂的干涉位置分成四种情况讨论，充分考虑了线障碍与机械臂碰撞的所有可能性。在分析临界碰撞关节角时，按照一定的规则进行，降低了机械臂自由空间求解的难度和计算量，最终目的在于避免机械臂各关节发生碰撞，有利于进行机械臂避碰路径规划控制。

附图说明

图1是线段与机械臂碰撞模型示意图；

图2是机械臂简化模型示意图；

图3是线段与机械臂模型干涉位置示意图；

图4a、4b、4c是第三种情况下线障碍示意图；

图5a、5b是第四种情况A1条件下线障碍示意图；

图6是任意线段障碍示意图。

具体实施方式

首先需要理解临界碰撞关节角的概念，其指的是连杆L与特征点P碰撞的时候形成的关节角，连杆L顺时针旋转时与特征点P接触形成的关节角，称为上临界碰撞关节角；连杆L逆时针旋转时与特征点P接触形成的关节角，称为下临界碰撞关节角。通过上下临界碰撞关节角即可以描述点的整个C-空间障碍边界。

如图1所示，设连杆L前端关节值已确定，关节J _k的运动范围为[-π,π]，连杆L与线段S产生碰撞，以直线的两个端点P ₁、P ₂作为特征点，把情况转化为连杆与点的碰撞情况，一共存在四个临界碰撞点：P ₁的上临界碰撞关节角θ _1uc 、下临界碰撞关节角θ _1lc ，P ₂的上临界碰撞关节角θ _2uc 、下临界碰撞关节角θ _2lc 。由图1易知θ _1lc 实际上是达不到的，则为无效的下临界碰撞关节角。同理，θ _2uc 也是无效的。那么连杆L与线段S发生碰撞的上下临界碰撞关节角实际上只有两个，分别为θ _1uc 和θ _2lc 。可以看出，对于任意障碍物，尤其是机械臂连杆作为障碍时，可以分解为一条或几条线段来进行分析，但是还是以点的映射为基本原理。

将机械臂简化为二连杆结构，分为大臂和小臂，将其映射到二维空间如图2所示。大臂由长为l ₁，宽为w ₁的矩形和两端的半圆组成；小臂由长为l ₂，宽为w ₂的矩形和一端的半圆组成。大臂绕自身轴线旋转时最大距离为L ₁=l ₁+w ₁/2，小臂绕自身轴线旋转时最大距离为L ₂=(l ₂ ²+w ₂ ²/4)^1/2，二连杆能扫过的最大距离为l _max=l ₁+(l ₁ ²+w ₂ ²/4)^1/2。为方便求解，假设这两个关节可以无约束进行圆周任意位置定位。

接下来研究线段的C-空间映射，先讨论最简单的情况。如图3所示，令线段垂直于X轴，且位于第一象限。设有直线M其方程表示为：M=By+C=0，C≥0，y≥0；线段P ₁ P ₂∈M，其中P ₁=(x,y ₁)，P ₂=(x,y ₂)。以l _max为半径，原点O为圆心，做圆O _max。根据O _max与线段P ₁ P ₂的位置，可分为以下四种情况，如图所示。

(1)情况1：P ₁ P ₂与O _max没有交点，且P ₁、P ₂都位于圆外，此时线段P ₁ P ₂的C-空间障碍映射为空集。

(2)情况2：P ₁ P ₂与O _max有一个交点，此时，线段P ₁ P ₂的C-空间障碍映射，与点P ₁的C-空间障碍映射是同样的，根据点的相关映射规则即可求得其临界碰撞关节角。

(3)情况3：P ₁ P ₂与O _max有一个交点，且P ₁位于圆O _max内。设小臂回转中心ro ₁与P ₁ P ₂所在直线M的距离为d _rol ，ro ₁与P ₁的距离为d _rop1，原点与P ₁距离为d _op1(不侵入内圆)。

1)当d _op1≥L ₁时，点P ₁不会对大臂造成运动障碍，只会对小臂造成运动障碍。

如图4a所示，当d _rol ≤L ₂且d _rop1≥L ₂时，小臂不会碰到P ₁点，小臂在θ _2uc 位置和θ _2lc 位置与线段P ₁ P ₂相碰的点分别是A ₁和A ₂。θ ₁为大臂的旋转角度值，对于任意θ ₁∈[θ _1min,θ _1max]，小臂的临界碰撞关节角为：θ _2uc =f _ol ^uc (θ ₁,x)，θ _2lc =f _ol ^uc (θ ₁,x)。

如图4b所示，当d _rol ≤ L ₂且d _rop1<L ₂时，下临界碰撞关节角θ _2lc ，不再是小臂与线段在A ₂点的碰撞，变为线段端点P ₁与小臂上边界的碰撞。此时，对于任意θ ₁∈[θ ₁ ' _min,θ ₁ ' _max]，其中θ ₁ ' _max=θ _1min，小臂的临界碰撞关节角为θ _2uc =f _ol ^uc (θ ₁,x)，θ _2lc =f _ol ^uc' (θ ₁,x)。

2)当d _op1<L ₁时，点P ₁对大臂和小臂都会造成运动障碍。

如图4c所示，线段P ₁ P ₂与大臂碰撞形成的上临界碰撞关节角为θ _1uc =arccos[(x-w ₂/2)/l ₁]。

下临界碰撞关节角θ _1lc 与d _op1的取值有关。当d _op1∈[l ₁,L ₁)时，如图4c中Ⅰ框内情况所示，线段与大臂相交于圆弧上一点B ₂，此时下临界碰撞关节角θ _1lc =arctan(y ₁/x)-arccos(D/2l ₁ d _op1)，其中D=l ₁ ²+d _op1 ²-w ₁ ²/4。当d _op1∈(0,l ₁)时，如图4c中Ⅱ方框内情况所示，线段与大臂相交于大臂上边沿的一点B ₃，此时有下临界碰撞关节角θ _1lc =arctan(y ₁/x)-arccos(w ₁/2d _op1)。

当θ ₁∈[θ _1lc ,θ _1uc ]时，大臂发生碰撞，无论小臂取何角度，机器人都会发生碰撞，所以线段P ₁ P ₂映射到小臂的C-空间障碍为θ ₂∈[-π,π]；当θ ₁不在上下临界碰撞关节角的取值范围内时，线段P ₁ P ₂便会对小臂造成运动障碍，但是与d _op1≥L ₁情况下得出的结论是一样的。

根据情况3的讨论结果，可知原点O与点P ₁的距离d _op1和原点O与点P ₂的距离d _op2的长短将会关系到线段P ₁ P ₂是否对大臂造成C-空间障碍。

(4)情况4：P ₁ P ₂与O _max没有交点，且P ₁、P ₂均位于圆内。在d _op1≤L ₁或d _op2≤L ₁条件下，线段P ₁ P ₂会对大臂和小臂都造成障碍，这种情况，前面已经讨论过，不再赘述。接下来，只讨论线段P ₁ P ₂对小臂造成障碍的情况。

ro ₁与直线M的距离d _rol 和ro ₁与点P ₁的距离d _rop1都会影响到小臂的下临界碰撞关节角的取值形式；同理，d _rol 和d _rop2也会影响到小臂的上临界碰撞关节角的取值形式。

1)当d _rol ≥L ₂时，小臂不会与线段P ₁ P ₂发生碰撞，C-空间障碍映射为空集。

2)当d _rol <L ₂时，如果有d _rop2≥L ₂，情况如图5a所示，小臂边界不会与点P ₂发生碰撞，上临界碰撞关节角是端点A ₁与线段接触产生的角度且θ _2uc =f _ol ^uc (θ ₁,x)；若d _rop2<L ₂，情况如图5b所示，小臂边界在A ₃点将会与点P ₂发生碰撞，上临界碰撞关节角θ _2uc =f _ol ^uc' (θ ₁,x)；若d _rop1≥L ₂时，下临界碰撞关节角θ _2lc =f _ol ^lc (θ ₁,x)；若d _rop1<L ₂时，下临界碰撞关节角θ _2lc =f _ol ^lc' (θ ₁,x)。

至此，完成了垂直于X轴的线段P ₁ P ₂的C-空间映射，得出了它的C-空间障碍描述。

为不失一般性，如图6所示，对于平面内的任意线段P ₁ P ₂，从原点O先作线段P ₁ P ₂的垂线OH，设点H的极坐标形式为H=(r _OH ，θ _OH )。进行坐标变换，将原坐标OXY旋转角度θ _OH ，得新坐标O'X'Y'，其情况就转换为垂直于X'轴的线段P ₁ 'P ₂ '的情况。只需在新的坐标系下分析得出临界碰撞关节角，再转换回原坐标即可（加上角度θ _OH ）。

其中，在上述方案中部分变量的求解公式如下：

d _rol 可表示为θ ₁,x的函数：d _rol = x-l ₁cosθ ₁；

P ₁极坐标形式为P ₁=(r _p1,θ _p1)，则d _rop1=[r _p1 ²+l ₁ ²-2r _op1 l ₁cos(θ ₁-θ _p1)]^1/2；

P ₂极坐标形式为P ₂=(r _p2,θ _p2)，则d _rop2=[r _p2 ²+l ₁ ²-2r _op2 l ₁cos(θ ₁-θ _p2)]^1/2；

下式为大臂旋转的临界角θ _1min、θ _1max和θ ₁ ' _min的计算公式：

θ _1max=arcos [(x-L ₂)/l ₁] (1)

θ _1min=arcos [(l ₁ ²+x ²+y ₁ ²-L ₂ ²)/2l ₁(x ²+y ₁ ²)] +arctan (y ₁/x) (2)

θ ₁ ' _min=-arcos [(l ₁ ²+x ²+y ₁ ²-L ₂ ²)/2l ₁(x ²+y ₁ ²)] +arctan (y ₁/x) (3)

下式为小臂与线段端点碰撞时产生的上下临界碰撞角公式展开式：

θ _2uc =-[θ ₁-arccos (d _rol /d _rop2)-arcsin (w ₂/2d _rop2)] = f _ol ^uc' (θ ₁, x) (4)

θ _2lc =-[θ ₁+arccos (d _rol /d _rop1) +arcsin (w ₂/2d _rop1)] = f _ol ^lc' (θ ₁, x) (5)

下式为小臂与线段非端点碰撞时产生的上下临界碰撞角公式展开公式：

θ _2uc =-[θ ₁-arccos (d _rol /L ₂)-arctan (w ₂/2l ₂)] = f _ol ^uc (θ ₁, x) (6)

θ _2lc =-[θ ₁+arccos (d _rol /L ₂) +arctan (w ₂/2l ₂)] = f _ol ^lc (θ ₁, x) (7)

上述公式均是根据相应的数学模型求得。

以上所述仅为本发明具体的实施方式，熟悉本领域的技术人员在本发明所述的技术方案范围内所进行的替换都应包含在本发明的保护范围之内。

Claims (5)

1.一种多臂机器人C-空间线障碍的映射方法，其特征是：将工作空间中的障碍简化为特征线段P ₁ P ₂，P ₁和P ₂为线段的两个端点，令其垂直于X轴，且位于第一象限；将机械臂简化为二连杆结构，分为大臂和小臂，其中大臂绕自身轴线旋转的最大距离为L ₁，小臂绕自身轴线旋转的最大距离为L ₂，机械臂绕其大臂自身轴线旋转的最大距离为l _max；设大臂的回转中心为原点O，以原点O为圆心，l _max为半径，做圆O _max，根据圆O _max与线段P ₁ P ₂的位置，可分为四种干涉情况讨论，其中小臂回转中心ro ₁到线段P ₁ P ₂所在直线M的距离为d _rol ，ro ₁与P ₁的距离为d _rop1，ro ₁与P ₂的距离为d _rop2，原点O与P ₁距离为d _op1；分别计算出这四种情况的临界碰撞角，就可以得到基于C-空间的线障碍映射空间；

第一种情况是P ₁ P ₂与O _max没有交点，且P ₁、P ₂都位于圆外，线段P ₁ P ₂的C-空间障碍映射为空集；第二种情况是P ₁ P ₂与O _max有一个交点，线段P ₁ P ₂的C-空间障碍映射，与点P ₁的C-空间障碍映射是同样的；第三种情况是P ₁ P ₂与O _max有一个交点，且P ₁位于圆O _max内；第四种情况是P ₁ P ₂与O _max没有交点，且P ₁、P ₂均位于圆内。

2.如权利要求1所述一种多臂机器人C-空间线障碍的映射方法，其特征是：在第三种情况下，当d _op1≥L ₁时，点P ₁不会对大臂造成运动障碍，只会对小臂造成运动障碍；当d _op1<L ₁时，点P ₁对大臂和小臂都会造成运动障碍；当大臂发生碰撞时，无论小臂取何角度，机器人都会发生碰撞，大臂不发生碰撞时，小臂碰撞的情况与d _op1≥L ₁情况下得出的结论是一样的。

3.如权利要求1所述一种多臂机器人C-空间线障碍的映射方法，其特征是：在第四种情况下，当d _rol ≥L ₂时，小臂不会与线段P ₁ P ₂发生碰撞，C-空间障碍映射为空集；当d _rol <L ₂时，小臂与线段P ₁ P ₂发生碰撞。

4.如权利要求1所述一种多臂机器人C-空间线障碍的映射方法，其特征是：在求解小臂的临界碰撞角计算公式时，分为两种情况：一种是小臂与线段端点发生碰撞，第二种是小臂与线段内部点发生了碰撞，总共对应四条碰撞角公式；d _rol 和d _rop1的值会影响到小臂的下临界碰撞关节角的取值形式；d _rol 和d _rop2的值会影响到小臂的上临界碰撞关节角的取值形式；当线段对大臂构成运动障碍时，大臂的下临界碰撞角取值与d _op1的大小有关。

5.如权利要求1所述一种多臂机器人C-空间线障碍的映射方法，其特征是：对于平面内的任意线段P ₁ P ₂的映射方法，从原点O先作线段P ₁ P ₂的垂线OH，设点H的极坐标形式为H=(r _OH ，θ _OH )，然后进行坐标变换，将原坐标OXY旋转角度θ _OH 得到新坐标，线段就会垂直于X'轴，只需在新坐标系下分析得出临界碰撞关节角，再转换回原坐标即可。