一种冗余机械臂零空间避障规划方法与流程

1.本发明属于机械臂避障设计领域，涉及一种冗余机械臂零空间避障规划方法。


背景技术：

2.冗余机械臂一般是指具有7个关节的串联型机器人，三维空间具有6个自由度，因此冗余机械臂逆运动学求解具有无穷多解，在机械臂末端位姿不变的情况下，无穷多解构成的空间称为冗余机械臂的零空间。当机械臂执行任务时，多会关注机械臂末端的位姿精度，为了与周围环境不发生干涉，冗余机械臂零空间的避障方法显得尤为重要。
3.目前冗余机械臂零空间避障方法多采用梯度投影法及其改进方法，主要思想是在机械臂连杆上布置多个点，计算每一点与周围环境的欧式距离，然后对所有点与周围环境的欧式距离求导，寻找距离增大的方向。其建模及计算量巨大，计算周期达到秒级以上，这在执行实时任务过程中是不可接受的。


技术实现要素：

4.本发明解决的技术问题是：克服现有技术的不足，提出一种冗余机械臂零空间避障规划方法，将三维空间简化为二维空间，包括障碍物的简化以及避障空间的简化，计算量大大降低。
5.本发明解决技术的方案是：
6.一种冗余机械臂零空间避障规划方法，包括：
7.制作7个关节和基座组成的机械臂，7个关节分别为第一关节、第二关节、
……
、第七关节；第一关节与基座连接；第二关节、
……
、第七关节与第一关节依次串联；设定第一关节、第二关节、
……
、第七关节的旋转角度为θ1、θ2、
……
、θ7；其中，第一关节、第二关节、第三关节为机械臂的肩部；第四关节为机械臂的肘部；第五关节、第六关节、第七关节为机械臂的腕部；
8.设定第一关节、第二关节、第三关节的轴线相交于s点；第五关节、第六关节、第七关节的轴线相交于w点；第四关节位于原点e
′
；se
′
w所在平面为臂型面；当机械臂末端位姿给定后，臂型面绕sw旋转；
9.设定第一关节轴线方向的矢量j
1z
与sw组成参考平面sew，设定臂型面se
′
w与参考平面sew的夹角为臂角
10.建立臂型面坐标系ax
′y′z′
；建立机械臂基坐标系bxyz，建立第一障碍物c和第二障碍物d；
11.将第一障碍物c和第二障碍物d向臂型面坐标系的ax
′y′
平面投影；臂型面se
′
w与x
′
轴正方形的夹角记为臂形角此时，机械臂基坐标系bxyz下的障碍物随着机械臂的运动在臂型面坐标系ax
′y′z′
中运动；此时寻找臂形角的目标则是臂型面se
′
w的在ax
′y′
平面投影线ae
′
与所有障碍物之间的最小距离最大；
12.建立机械臂dh建模图；第一关节、第二关节、
……
、第七关节在dh建模中自动生成
各关节的对应坐标系；获得第七关节相对于机械臂基坐标系的位姿矩阵0t7；
13.设定0x7为第七关节的位置的表达式、0r7为第七关节的姿态的表达式；计算在机械臂基坐标系bxyz下s点指向w点的向量0x
sw
的表达式；
14.令w＝0x
sw
，定义v为机械臂基坐标系bxyz中z轴的方向向量，即v＝[0 0 1]
′
；计算矢量j
1z
与sw组成的参考平面与w的垂直向量k；对向量k进行单位化处理得到臂型面坐标系ax
′y′z′
的x
′
轴的单位向量kn；则期望臂形角即为ae
′
与臂型面坐标系ax
′y′z′
的x
′
轴的夹角；
[0015]
将障碍物向臂型面坐标系ax
′y′z′
的x
′y′
面投影后，求解最优臂形角转化为求解ae
′
所在直线y＝mx的斜率m，使障碍物c、d的圆心与直线y＝mx的距离减去半径均最远；
[0016]
设第一障碍物c的坐标为(xc,yc)，半径为rc；第二障碍物d的坐标为(xd,yd)，半径为rd；ae
′
所在直线y＝mx，存在两种情况：
[0017]
s1、当(xc,yc)、(xd,yd)的连线穿过臂型面坐标系ax
′y′z′
的原点a，则ae
′
所在直线y＝mx的斜率而ae
′
方向存在2种，即远离坐标原点或指向坐标原点；计算s1情况下的期望臂形角，并选取最优臂形角；
[0018]
s2、当(xc,yc)、(xd,yd)的连线不穿过臂型面坐标系ax
′y′z′
的原点a，求解方程得到两个斜率m1和m2；即向量ae
′
的方向存在4种情况，斜率为m1时的两个方向和斜率为m2的两个方向；计算s2情况下的期望臂形角，并选取最优臂形角；
[0019]
完成避障规划。
[0020]
在上述的一种冗余机械臂零空间避障规划方法，臂型面坐标系ax
′y′z′
的建立方法为：
[0021]
臂型面坐标系x
′y′z′
的坐标原点a位于e点向sw连线的垂点；z
′
轴与sw连线同向；x
′
轴平行于参考平面，且指向第四关节；y
′
由右手定则确定；
[0022]
机械臂基坐标系bxyz的建立方法为：
[0023]
机械臂基坐标系bxyz的坐标原点位于基座上；z轴竖直向上；x轴、y轴位于水平面，且x轴、y轴、z轴互相垂直。
[0024]
在上述的一种冗余机械臂零空间避障规划方法，对第一障碍物c和第二障碍物d进行建模时，形状选用球形。
[0025]
在上述的一种冗余机械臂零空间避障规划方法，第七关节相对于机械臂基坐标系的位姿矩阵0t7为：
[0026][0027]
式中，[n
x n
y nz]为第七关节坐标系的x轴在机械臂基坐标系bxyz中的单位矢量；
[0028]
[o
x o
y oz]为第七关节坐标系的y轴在机械臂基坐标系bxyz中的单位矢量；
[0029]
[a
x a
y az]为第七关节坐标系的z轴在机械臂基坐标系bxyz中的单位矢量；
[0030]
[p
x p
y pz]为第七关节坐标系的原点在机械臂基坐标系bxyz中的单位矢量。
[0031]
在上述的一种冗余机械臂零空间避障规划方法，向量0x
sw
的表达式为：
[0032]0x
sw
＝0x
7-0
l
bs-0r77
l
wt
＝0r3(3l
se
3l 3r
44
l
ew
)
[0033]
式中，0l
bs
为机械臂基坐标系原点b指向s点在机械臂基坐标系bxyz坐标系下的表达式；
[0034]0x7为第七关节的位置的表达式；
[0035]0r7为第七关节的姿态的表达式；
[0036]7l
wt
为w点指向工具坐标系原点在dh建模中第七关节的对应坐标系下的表达式；
[0037]0r3为第三关节的姿态表达式；
[0038]3l
se
为s点指向e点在dh建模中第三关节的对应坐标系下的表达式；
[0039]3l为偏置杆长在dh建模中第三关节的对应坐标系下的表达式；
[0040]3r4为dh建模中第四关节的对应坐标系的表达式相对于第三关节的对应坐标系的姿态矩阵；
[0041]4l
ew
为e点指向w点在dh建模中第四关节的对应坐标系下的表达式。
[0042]
在上述的一种冗余机械臂零空间避障规划方法，垂直向量k的计算方法为：
[0043]
k＝cross(cross(w,v),w)；
[0044]
式中，cross为向量的叉乘运算；
[0045]
单位向量kn为：
[0046]kn
＝k/||k||。
[0047]
在上述的一种冗余机械臂零空间避障规划方法，s1中，期望臂形角的计算方法为：
[0048]
计算期望臂形角
[0049][0050]
式中，norm为对向量单位化；
[0051]
cross为向量的叉乘运算；
[0052]ms
′
为向量ms的转置；
[0053]kn
为单位向量；
[0054]
选取y＝mx直线上的两个向量m
s1
和m
s2
；其中，m
s1
＝(x,mx)，m
s2
＝(-x,-mx)；
[0055]
将m
s1
和m
s2
分别代入期望臂形角计算公式中的ms，得到期望臂形角和
[0056]
在上述的一种冗余机械臂零空间避障规划方法，s1中，最优臂形角的选取方法为：
[0057]
设定上一时刻臂形角为分别计算和较小值对应的期望臂形角即为最优臂形角。
[0058]
在上述的一种冗余机械臂零空间避障规划方法，s2中，期望臂形角的计算方法为：
[0059]
选取斜率m1对应的两个向量m
1s1
＝(x1,m1x1)、m
1s2
＝(-x1,-m1x1)和斜率m2对应的两个向量m
2s1
＝(x2,m2x2)、m
2s2
＝(-x2,-m2x2)；则期望臂形角为：
[0060][0061]
将m
1s1
、m
1s2
、m
2s1
和m
2s2
分别代入期望臂形角计算公式中的ms，得到期望臂形角
和
[0062]
在上述的一种冗余机械臂零空间避障规划方法，s2中，最优臂形角的选取方法为：
[0063]
设定上一时刻臂形角为分别计算和最小值对应的期望臂形角即为最优臂形角。
[0064]
本发明与现有技术相比的有益效果是：
[0065]
(1)本发明创新性的利用机械臂零空间的旋转轴和臂形角的参考平面建立臂型面坐标系ax
′y′z′
；将障碍物进行三维建模，利用机械臂基坐标系bxyz与障碍物的空间关系，将障碍物向臂型面坐标系ax
′y′z′
的x
′y′
平面投影；利用机械臂基坐标系bxyz与臂型面坐标系ax
′y′z′
的空间关系，将障碍物在机械臂基坐标系bxyz内的投影转化到臂型面坐标系ax
′y′z′
的x
′y′
平面内；实现了将三维模型简化为二维空间后，利用臂形角与障碍物的二维几何关系进行避障计算；本发明将三维空间简化为二维空间，包括障碍物的简化以及避障空间的简化，计算量大大降低；
[0066]
(2)本发明计算实时性大幅提高，经dsp6701(120m主频)测算对臂形角的避障计算在10ms内完成，考虑逆运动学计算的时间，实时避障周期可达20hz，满足实时避障工作的要求。
附图说明
[0067]
图1为本发明7自由度冗余机械臂示意图；
[0068]
图2为本发明机械臂dh模型示意图；
[0069]
图3为本发明臂形角示意图；
[0070]
图4为本发明坐标系关系示意图；
[0071]
图5为本发明障碍物投影及避障示意图。
具体实施方式
[0072]
下面结合实施例对本发明作进一步阐述。
[0073]
本发明提供一种冗余机械臂零空间避障规划方法，利用机械臂零空间的旋转轴和臂形角的参考平面建立臂型面坐标系ax
′y′z′
；将障碍物进行三维建模，利用机械臂基坐标系bxyz与障碍物的空间关系，将障碍物向臂型面坐标系ax
′y′z′
的x
′y′
平面投影；利用机械臂基坐标系bxyz与臂型面坐标系ax
′y′z′
的空间关系，将障碍物在机械臂基坐标系bxyz内的投影转化到臂型面坐标系ax
′y′z′
的x
′y′
平面内；上述三维模型简化为二维空间后，利用臂形角与障碍物的二维几何关系进行避障计算。
[0074]
冗余机械臂零空间避障规划方法，具体包括如下步骤：
[0075]
步骤一、制作7个关节和基座组成的机械臂，7个关节分别为第一关节、第二关节、
……
、第七关节；第一关节与基座连接；第二关节、
……
、第七关节与第一关节依次串联；设定第一关节、第二关节、
……
、第七关节的旋转角度为θ1、θ2、
……
、θ7；其中，第一关节、第二关节、第三关节为机械臂的肩部；第四关节为机械臂的肘部；第五关节、第六关节、第七关节为机械臂的腕部，如图1所示。
[0076]
设定第一关节、第二关节、第三关节的轴线相交于s点；第五关节、第六关节、第七
关节的轴线相交于w点；第四关节位于原点e
′
；se
′
w所在平面为臂型面；当机械臂末端位姿给定后，臂型面绕sw旋转。
[0077]
设定第一关节轴线方向的矢量j
1z
与sw组成参考平面sew，设定臂型面se
′
w与参考平面sew的夹角为臂角如图3所示。
[0078]
步骤二、建立臂型面坐标系ax
′y′z′
；建立机械臂基坐标系bxyz；其中，臂型面坐标系ax
′y′z′
的建立方法为：
[0079]
臂型面坐标系x
′y′z′
的坐标原点a位于e点向sw连线的垂点；z
′
轴与sw连线同向；x
′
轴平行于参考平面，且指向第四关节；y
′
由右手定则确定，如图3所示。
[0080]
机械臂基坐标系bxyz的建立方法为：
[0081]
机械臂基坐标系bxyz的坐标原点位于基座上；z轴竖直向上；x轴、y轴位于水平面，且x轴、y轴、z轴互相垂直。如图4所示。
[0082]
建立第一障碍物c和第二障碍物d；对第一障碍物c和第二障碍物d进行建模时，形状选用球形，且位置任意。
[0083]
本发明中，臂型面坐标系ax
′y′z′
的z
′
轴与冗余机械臂零空间旋转轴同向，x’轴方向平行于臂形角参考平面，指向肘关节，y’轴方向利用右手定则确定。基坐标系b如图所示，障碍物c和d如图所示，在障碍物进行三维建模时，可以根据障碍物的形状选用球形建模、圆柱体建模、长方体建模、圆锥体建模等方法。为描述方便，本专利交底书采用球形建模进行零空间避障算法描述，采用其他几何体建模时，避障方法相同，不再赘述。
[0084]
步骤三、利用臂型面坐标系x
′y′z′
与机械臂基坐标系bxyz的空间关系，将第一障碍物c和第二障碍物d向臂型面坐标系的ax
′y′
平面投影；臂型面se
′
w与x
′
轴正方形的夹角记为臂形角此时，机械臂基坐标系bxyz下的障碍物随着机械臂的运动在臂型面坐标系ax
′y′z′
中运动；此时寻找臂形角的目标则是臂型面se
′
w的在ax
′y′
平面投影线ae
′
与所有障碍物之间的最小距离最大。
[0085]
步骤四、建立机械臂dh建模图，如图2所示，第一关节、第二关节、
……
、第七关节在dh建模中自动生成各关节的对应坐标系。获得第七关节相对于机械臂基坐标系的位姿矩阵0t7；第七关节相对于机械臂基坐标系的位姿矩阵0t7为：
[0086][0087]
式中，[n
x n
y nz]为第七关节坐标系的x轴在机械臂基坐标系bxyz中的单位矢量；
[0088]
[o
x o
y oz]为第七关节坐标系的y轴在机械臂基坐标系bxyz中的单位矢量；
[0089]
[a
x a
y az]为第七关节坐标系的z轴在机械臂基坐标系bxyz中的单位矢量；
[0090]
[p
x p
y pz]为第七关节坐标系的原点在机械臂基坐标系bxyz中的单位矢量。
[0091]
步骤五、设定0x7为第七关节的位置的表达式、0r7为第七关节的姿态的表达式；计算在机械臂基坐标系bxyz下s点指向w点的向量0x
sw
的表达式；向量0x
sw
的表达式为：
[0092]0x
sw
＝0x
7-0
l
bs-0r77
l
wt
＝0r3(3l
se
3l 3r
44
l
ew
)
[0093]
式中，0l
bs
为机械臂基坐标系原点b指向s点在机械臂基坐标系bxyz坐标系下的表
达式；
[0094]0x7为第七关节的位置的表达式；
[0095]0r7为第七关节的姿态的表达式；
[0096]7l
wt
为w点指向工具坐标系原点在dh建模中第七关节的对应坐标系下的表达式；
[0097]0r3为第三关节的姿态表达式；
[0098]3l
se
为s点指向e点在dh建模中第三关节的对应坐标系下的表达式；
[0099]3l为偏置杆长在dh建模中第三关节的对应坐标系下的表达式；
[0100]3r4为dh建模中第四关节的对应坐标系的表达式相对于第三关节的对应坐标系的姿态矩阵；
[0101]4l
ew
为e点指向w点在dh建模中第四关节的对应坐标系下的表达式。
[0102]
步骤六、令w＝0x
sw
，定义v为机械臂基坐标系bxyz中z轴的方向向量，即v＝[0 0 1]
′
；计算矢量j
1z
与sw组成的参考平面与w的垂直向量k；对向量k进行单位化处理得到臂型面坐标系ax
′y′z′
的x
′
轴的单位向量kn；则期望臂形角即为ae
′
与臂型面坐标系ax
′y′z′
的x
′
轴的夹角；垂直向量k的计算方法为：
[0103]
k＝cross(cross(w,v),w)；
[0104]
式中，cross为向量的叉乘运算；
[0105]
单位向量kn为：
[0106]kn
＝k/||k||。
[0107]
步骤七、将障碍物向臂型面坐标系ax
′y′z′
的x
′y′
面投影后，求解最优臂形角转化为求解ae
′
所在直线y＝mx的斜率m，使障碍物c、d的圆心与直线y＝mx的距离减去半径均最远。
[0108]
步骤八、当将障碍物简化为球形模型并将其向臂型面坐标系ax
′y′z′
的x
′y′
平面投影后，求解最优臂形角的问题简化为求解ae
′
所在直线y＝mx的斜率m，使得障碍物c、d的圆心与直线y＝mx的距离减去半径均最远即可。设第一障碍物c的坐标为(xc,yc)，半径为rc；第二障碍物d的坐标为(xd,yd)，半径为rd；ae
′
所在直线y＝mx，存在两种情况：
[0109]
s1、当(xc,yc)、(xd,yd)的连线穿过臂型面坐标系ax
′y′z′
的原点a，则ae
′
所在直线y＝mx的斜率而ae
′
方向存在2种，即远离坐标原点或指向坐标原点；计算s1情况下的期望臂形角，并选取最优臂形角；期望臂形角的计算方法为：
[0110]
计算期望臂形角
[0111][0112]
式中，norm为对向量单位化；
[0113]
cross为向量的叉乘运算；
[0114]ms
′
为向量ms的转置；
[0115]kn
为单位向量；
[0116]
选取y＝mx直线上的两个向量m
s1
和m
s2
；其中，m
s1
＝(x,mx)，m
s2
＝(-x,-mx)；
[0117]
将m
s1
和m
s2
分别代入期望臂形角计算公式中的ms，得到期望臂形角和
[0118]
最优臂形角的选取方法为：
[0119]
设定上一时刻臂形角为分别计算和较小值对应的期望臂形角即为最优臂形角。
[0120]
s2、当(xc,yc)、(xd,yd)的连线不穿过臂型面坐标系ax
′y′z′
的原点a，求解方程得到两个斜率m1和m2；即向量ae
′
的方向存在4种情况，斜率为m1时的两个方向和斜率为m2的两个方向；计算s2情况下的期望臂形角，并选取最优臂形角；期望臂形角的计算方法为：
[0121]
选取斜率m1对应的两个向量m
1s1
＝(x1,m1x1)、m
1s2
＝(-x1,-m1x1)和斜率m2对应的两个向量m
2s1
＝(x2,m2x2)、m
2s2
＝(-x2,-m2x2)；则期望臂形角为：
[0122][0123]
将m
1s1
、m
1s2
、m
2s1
和m
2s2
分别代入期望臂形角计算公式中的ms，得到期望臂形角和
[0124]
最优臂形角的选取方法为：
[0125]
设定上一时刻臂形角为分别计算和最小值对应的期望臂形角即为最优臂形角，如图5所示。
[0126]
完成避障规划。
[0127]
本发明利用机械臂零空间的旋转轴和臂形角的参考平面建立臂型面坐标系ax
′y′z′
；将障碍物进行三维建模，利用机械臂基坐标系bxyz与障碍物的空间关系，将障碍物向臂型面坐标系ax
′y′z′
的x
′y′
平面投影；利用机械臂基坐标系bxyz与臂型面坐标系ax
′y′z′
的空间关系，将障碍物在机械臂基坐标系bxyz内的投影转化到臂型面坐标系ax
′y′z′
的x
′y′
平面内；实现了将三维模型简化为二维空间后，利用臂形角与障碍物的二维几何关系进行避障计算；本发明将三维空间简化为二维空间，包括障碍物的简化以及避障空间的简化，计算量大大降低；计算实时性大幅提高，经dsp6701(120m主频)测算对臂形角的避障计算在10ms内完成，考虑逆运动学计算的时间，实时避障周期可达20hz，满足实时避障工作的要求。
[0128]
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。