CN103895040B - 空间机械臂连杆间相互碰撞的检测方法 - Google Patents

Abstract

发明了一种机械臂两连杆之间的碰撞检查方法。针对一种六关节机械臂，导出了各关节和末端的坐标，对可能发生相互碰撞的机械臂的每对连杆，将其上面的各点表示成两端关节坐标的函数，并由此给出了每对连杆相互碰撞的检查方法。本发明的有益效果在于：不仅能检查机械臂两连杆两端关节的相互碰撞，而且能够检查机械臂两连杆上任何位置之间的相互碰撞。

Description

空间机械臂连杆间相互碰撞的检测方法

技术领域

本发明涉及一种空间机械臂与舱体的碰撞检测方法。

背景技术

作为航天器的一种重要工具，空间机械臂可用于卫星的回收和维修、大型设备的组装、空间站的在轨建设、以及作为航天员出舱的辅助平台，提高安全性及舱外工作能力等。对于失效或出现故障的航天器，利用空间机械臂将其捕获、维修、回收或再利用，不仅可以节约大量的人力财力，还可以清除宇宙空间日益增多的太空垃圾。

由于被抓取的空间物体的位置不同，机械臂末端运动路径也各不相同，即使在最简单的空间环境中，机械臂运动过程中也可能发生连杆之间的相互碰撞。这种碰撞不仅可能导致航天器和机械臂的严重损坏，产生的扰动力还会造成系统动量的改变，特别是角动量的改变，直接影响到整个航天器的稳定运行。因此在机械臂的运动过程，根据各关节的角度估计各关节的空间位置，依此判断是否存在连杆之间的相互碰撞的可能，并实时规划出无碰撞的路径，对提高机械臂空间应用的安全性具有十分重要的意义。

为了避免机械臂连杆之间的相互碰撞间题，不仅需要考虑关节的碰撞，还要考虑机械臂的每对连杆上任何位置的碰撞，因此，仅考虑空间中两连杆端点的空间关系是不够的。本发明将对发生在机械臂每对连杆上任何位置的相互碰撞问题，给出相互碰撞的检查方法。

发明内容

发明了一种机械臂两连杆之间的碰撞检查方法。针对一种六关节机械臂，导出了各关节和末端的坐标，对可能发生相互碰撞的机械臂的每对连杆，将其上面的各点表示成两端关节坐标的函数，并由此给出了每对连杆相互碰撞的检查方法。

本发明的有益效果在于：不仅能检查机械臂两连杆两端关节的相互碰撞，而且能够检查机械臂两连杆上任何位置之间的相互碰撞。

附图说明

图1是通用6关节机械臂示意图。

图2是两连杆之间的距离的示意图。

具体实施方式

机械臂的示意图如图1所示。假定航天器外形是直径为Φ的圆柱，机械臂安装在圆柱的表面，第一个关节转轴的轴线与圆柱对称轴相交，基座坐标系O₀-X₀Y₀Z₀的X₀轴与圆柱对称轴平行。第i个关节的角位移为θ_i，第i个坐标系的坐标原点为O_i，i＝1，2，…，6，定义与前一坐标系对应坐标轴平行的位置为0角位移位置，由X_i向Y_i、由Y_i向Z_i，或由Z_i向X_i的旋转为角位移的正方向，相邻坐标原点的连线 和的长度分别为d₁、d₂、a₂，a₃、d₂和a₅，且

c_i＝cosθ_i s_i＝sinθ_i(i＝1，2，…，6)

c₂₃＝cos(θ₂+θ₃)，s₂₃＝sin(θ₂+θ₃)

c₂₃₄＝cos(θ₂+θ₃+θ₄)，s₂₃₄＝sin(θ₂+θ₃+θ₄)

无论是机械臂与舱体的碰撞，还是其自身的碰撞，都与机械臂各关节的坐标有密切的关系。在机械臂的基座坐标系O₀-X₀Y₀Z₀中

基座坐标系原点为(x₀₀，y₀₀，z₀₀)＝(0，0，0)

第1坐标系原点为(x₀₁，y₀₁，z₀₁)＝(0，0，d₁)；

第2个关节的坐标为

(x₀₂，y₀₂，z₀₂)＝(-d₂s₁，d₂c₁，d₁)

第3个关节的坐标记为(x₀₃，y₀₃，z₀₃)，则

$\left(\begin{array}{c}{x}_{03}\\ y_{03}\\ z_{03}\end{array}\right)=\left(\begin{array}{ccc}{c}_1& -s_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{a}_2{c}_2\\ d_2\\ d_1-a_2{s}_2\end{array}\right)=\left(\begin{array}{c}{a}_2{c}_1{c}_2-d_2{s}_1\\ a_2{s}_1{c}_2+d_2{c}_1\\ d_1-a_2{s}_2\end{array}\right)$

第4个关节的坐标记为(x₀₄，y₀₄，z₀₄)，则

$\begin{array}{c}\left(\begin{array}{c}{x}_{04}\\ y_{04}\\ z_{04}\end{array}\right)=\left(\begin{array}{ccc}{c}_1& -s_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{a}_2{c}_2+a_3{c}_{23}\\ d_2\\ d_1-a_2{s}_2-a_3{s}_{23}\end{array}\right)\\ =\left(\begin{array}{c}{c}_1(a_2{c}_2+a_3{c}_{23})-d_2{s}_1\\ s_1(a_2{c}_2+a_3{c}_{23})+d_2{c}_1\\ d_1-a_2{s}_2-a_3{s}_{23}\end{array}\right)\end{array}$

第5个关节的坐标记为(x₀₅，y₀₅，z₀₅)，则

$\begin{array}{c}\left(\begin{array}{c}{x}_{05}\\ y_{05}\\ z_{05}\end{array}\right)=\left(\begin{array}{ccc}{c}_1& -s_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{a}_2{c}_2+a_3{c}_{23}\\ 0\\ d_1-a_2{s}_2-a_3{s}_{23}\end{array}\right)\\ =\left(\begin{array}{c}{c}_1(a_2{c}_2+a_3{c}_{23})\\ s_1(a_2{c}_2+a_3{c}_{23})\\ d_1-a_2{s}_2-a_3{s}_{23}\end{array}\right)\end{array}$

末端的坐标记为(x₀₆，y₀₆，z₀₆)，则

$\begin{array}{c}\left(\begin{array}{c}{x}_{06}\\ y_{06}\\ z_{06}\end{array}\right)=\left(\begin{array}{ccc}{c}_1& -s_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{a}_2{c}_2+a_3{c}_{23}\\ 0\\ d_1-a_2{s}_2-a_3{s}_{23}\end{array}\right)\\ +\left(\begin{array}{ccc}{c}_1& -s_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{c}_{234}& 0& s_{234}\\ 0& 1& 0\\ -s_{234}& 0& c_{234}\end{array}\right)\left(\begin{array}{ccc}{c}_5& -s_5& 0\\ s_5& c_5& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{a}_5\\ 0\\ 0\end{array}\right)\\ \left(\begin{array}{c}{c}_1(a_2{c}_2+a_3{c}_{23})\\ s_1(a_2{c}_2+a_3{c}_{23})\\ d_1-a_2{s}_2-a_3{s}_{23}\end{array}\right)+\left(\begin{array}{ccc}{c}_1& -s_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{a}_5{c}_5{c}_{234}\\ a_5{s}_5\\ -a_5{c}_5{s}_{234}\end{array}\right)\\ \left(\begin{array}{c}{c}_1(a_2{c}_2+a_3{c}_{23})\\ s_1(a_2{c}_2+a_3{c}_{23})\\ d_1-a_2{s}_2-a_3{s}_{23}\end{array}\right)+\left(\begin{array}{c}{a}_5{c}_1{c}_5{c}_{234}-a_5{s}_1{s}_5\\ a_5{s}_1{c}_5{c}_{234}+a_5{c}_1{s}_5\\ -a_5{c}_5{s}_{234}\end{array}\right)\\ \left(\begin{array}{c}{a}_2{c}_1{c}_2+a_3{c}_1{c}_{23}+a_5{c}_1{c}_5{c}_{234}-a_5{s}_1{s}_5\\ a_2{s}_1{c}_2+a_3{s}_1{c}_{23}+a_5{s}_1{c}_5{c}_{234}+a_5{a}_1{s}_5\\ d_1-a_2{s}_2-a_3{s}_{23}-a_5{c}_5{s}_{234}\end{array}\right)\end{array}$

1.两连杆之间碰撞的检查方法

记A_i＝(x_0i，y_0i，z_0i)(i＝0，1，2，…，6)。由机械臂的结构可以看出，相临两个连杆的碰撞可以通过对旋转角度的控制得以避免，其余需要考虑的可能碰撞的连杆对包括l₄₅与l₀₁， l₅₆与l₀₁、l₁₂、l₂₃和l₃₄等.

考虑l_i，i+1与l_j，j+1-对连杆的碰撞问题，其中i＜j-1。连杆l_i，i+1和l_j，j+1所在直线上的点可分别表示为

A_i(u)＝A_i+u(A_i+1-A_i)       (0≤u≤1)        (1) 

和

A_j(v)＝A_j+v(A_j+1-A_j)       (0≤v≤1)        (2) 

这两点之间的距离的平方可表示为

$\begin{array}{c}{L_{\mathrm{ij}}}^2(u,v)={\left|\right|A_j+v(A_{j+1}-A_j)-A_i-u(A_{i+1}-A_i)\left|\right|}^2\\ ={\left|\right|A_j-A_i\left|\right|}^2+2v(A_j-A_i){(A_{j+1}-A_j)}^{\′}-2u(A_j-A_i){(A_{i+1}-A_i)}^{\′}+v^2{\left|\right|A_{j+1}-A_j\left|\right|}^2\\ -2\mathrm{uv}(A_{j+1}-A_j){(A_{i+1}-A_i)}^{\′}+u^2{\left|\right|A_{i+1}-A_i\left|\right|}^2\end{array}---\left(3\right)$

当0≤u≤1，且0≤v≤1时，L_ij(u，v)表示连杆上任意两点之间的距离，利用其最小值可判断两连杆是否发生碰撞。为求出其最小值，将上式两端分别对u和v求偏导数并令其为零得

$\left\{\begin{array}{c}u{\left|\right|A_{i+1}-A_i\left|\right|}^2-v(A_{j+1}-A_j){(A_{i+1}-A_i)}^{\′}\\ =(A_j-A_i){(A_{i+1}-A_i)}^{\′}\\ u(A_{j+1}-A_j){(A_{i+1}-A_i)}^{\′}-{\left|\right|A_{j+1}-A_j\left|\right|}^2\\ =(A_j-A_i){(A_{j+1}-A_j)}^{\′}\end{array}\right.$

解方程组得 

$\begin{array}{cc}u=\frac{F_{\mathrm{ij}}}{E_{\mathrm{ij}}}& v=\frac{G_{\mathrm{ij}}}{E_{\mathrm{ij}}}\end{array}---\left(4\right)$

其中

$E_{\mathrm{ij}}={\left|\right|A_{i+1}-A_i\left|\right|}^2{\left|\right|A_{j+1}-A_j\left|\right|}^2-{\left[{(A_{i+1}-A_i)}^{\′}(A_{j+1}-A_j)\right]}^2---\left(5\right)$

F_ij＝(A_i+1-A_i)′(A_j+1-A_j)(A_j+1-A_j)′(A_i-A_j)-||A_j+1-A_j||²(A_i+1-A_i)′(A_i-A_j)   (6)

G_ij＝(A_j+1-A_j)′(A_i-A_j)||A_i+1-A_i||²-(A_i+1-A_i)′(A_i-A_j)(A_j+1-A_j)′(A_i+1-A_i)    (7) 

当0≤u≤1，且0≤v≤1时，说明连杆l_i，i+1和l_j，j+1所在的两条直线的公垂线的两个垂 足均在连杆上。如图2所示，将u和v的值代入等式(1)和(2)中可求出这两个垂足A_i(u)和A_j(v)，它们之间的距离为

L_ij(u，v)＝||A_i(u)-A_j(v)||        (8) 

若公垂线的长度L_ij(u，v)＞2δ，两个连杆不会碰撞，否则两连杆发生碰撞。

当时，函数L_ij ²(u，v)在区域[0，1]×[0，1]中无极值点，因此其最小值只能在区域[0，1]×[0，1]的边界上达到。

由等式(3)得

L_ij ²(0，v)＝||A_j-A_i||²+2v(A_j-A_i)(A_j+1-A_j)′+v²||A_j+1-A_j||²

L_ij ²(1，v)＝||A_j-A_i||²+2v(A_j-A_i)(A_j+1-A_j)′-2(A_j-A_i)(A_i+1-A_i)′+v²||A_j+1-A_j||²

-2v(A_j+1-A_j)(A_i+1-A_i)′+||A_i+1-A_i||²

上式两端对v求偏导数并令其为零得

v||A_j+1-A_j||²+(A_j-A_i)(A_j+1-A_j)′＝0

v||A_j+1-A_j||²+(A_j-A_i)(A_j+1-A_j)′

-(A_j+1-A_j)(A_i+1-A_i)′＝0

由此得

$v_0=\frac{(A_{j+1}-A_j){(A_i-A_j)}^{\′}}{{\left|\right|A_{j+1}-A_j\left|\right|}^2}---\left(9\right)$

$v_1=\frac{(A_{j+1}-A_j){(A_i-A_j)}^{\′}}{{\left|\right|A_{j+1}-A_j\left|\right|}^2}---\left(10\right)$

若0≤v₀≤1，则A_i到连杆l_j，i+1的最小距离为点A_i到点A_j(v₀)＝A_j+v₀(A_j+1-A_j)的距离L_ij(0，v₀)；若v₀＜0，则A_i到连杆L_j，j+1的最小距离为点A_i到点A_j的距离L_ij(0，0)；若v₀＞1，则A_i到连杆l_j，j+1的最小距离为点A_i到点A_j+1的距离L_ij(0，1)。

若0≤v₁≤1，则A_i+1到连杆L_j，j+1的最小距离为点A_i+1到点A_j(v₁)＝A_j+v₁(A_j+1-A_j)的距离L_ij(1，v₁)；若v₁＜0，则A_i+1到连杆L_j，j+1的最小距离为点A_i+1到点A_j的距离L_ij(1，0)；若v₁＞1，则A_i+1到连杆l_j，j+1的最小距离为点A_i+1到点A_j+1的距离L_ij(1，1)。

同理，由等式(3)得

${L_{\mathrm{ij}}}^2(u,0)={\left|\right|A_j-A_i\left|\right|}^2-2u(A_j-A_i){(A_{i+1}-A_i)}^{\′}+u^2{\left|\right|A_{i+1}-A_i\left|\right|}^2$

${L_{\mathrm{ij}}}^2(u,1)={\left|\right|A_j-A_i\left|\right|}^2+2(A_j-A_i){(A_{j+1}-A_j)}^{\′}$

-2u(A_j-A_i)(A_i+1-A_i)′+||A_j+1-A_j||²-2u(A_j+1-A_j)(A_i+1-A_i)′+u²||A_i+1-A_i||²

上式两端对u求偏导数并令其为零得

$u_0=\frac{(A_{j+1}-A_j){(A_i-A_j)}^{\′}}{{\left|\right|A_{j+1}-A_j\left|\right|}^2}---\left(11\right)$

$u_1=\frac{(A_{j+1}-A_j){(A_i-A_j)}^{\′}}{{\left|\right|A_{j+1}-A_j\left|\right|}^2}---\left(12\right)$

若0≤u₀≤1，则A_j到连杆l_i，i+1的最小距离为点A_j到点A_i(u₀)＝A_i+u₀(A_i+1-A_i)的距离l_ij(u₀，0)；若u₀＜0，则A_j到连杆l_i，i+1的最小距离为点A_j到点A_i的距离L_ij(0，0)；若u₀＞1，则A_j到连杆l_i，i+1的最小距离为点A_j到点A_i+1的距离L_ij(1，0)。

同理，若0≤u₁≤1，则A_j+1到连杆l_i，i+1的最小距离为点A_j+1到点A_i(v₁)＝A_i+u₁(A_i+1-A_i)的距离L_ij(u₁，1)；若u₁＜0，则A_j+1到连杆l_i，i+1的最小距离为点A_j+1到点A_i的距离L_ij(0，1)；若u₁＞1，则A_j+1到连杆l_i，i+1的最小距离为点A_j+1到点A_i+1的距离L_ij(1，1)。

根据这种规律，定义向量函数

$F_j\left(v_0\right)=\left\{\begin{array}{cc}{A}_j& v_0<0\\ A_j\left(v_0\right)& 0\&le;v_0\&le;1\\ A_{j+1}& v_0>1\end{array}\right.---\left(13\right)$

则L_ij(u，v)在区域[0，1]×[0，1]的边界上的最小值为

L_min＝min{||A_i-F_j(v₀)||，||A_i+1-F_j(v₁)||，||A_j-F_i(u₀)||，||A_j+1-F_i(u₁)||}    (14) 

2.两连杆之间碰撞的检查程序

根据机械臂两连杆之间碰撞的检查方法，机械臂一对连杆l_i，i+1和l_j，j+1(i＝0，j＝4；i＝0，1，2，3，j＝5)之间的碰撞检查程序可归纳为：

1)利用等式(1)，(2)，(4)-(7)计算u，v，当0≤u≤1，且0≤v≤1时计算

L_ij(u，v)＝||A_i(u)-A_j(v)||，

若L_ij(u，v)＞2δ时转4)；

2)利用等式(9)-(14)计算L_min，若L_min＞2δ转4)；

3)转5)；

4)输出“无碰撞”，检查结束。

5)输出“有碰撞”，检查结束。

Claims (1)

1.一种空间机械臂连杆间相互碰撞的检测方法，其特征在于：假定航天器外形是直径为Ф的圆柱，机械臂安装在圆柱的表面，第一个关节转轴的轴线与圆柱对称轴相交，基座坐标系O₀-X₀Y₀Z₀的X₀轴与圆柱对称轴平行；第i个关节的角位移为θ_i，第i个坐标系的坐标原点为O_i，i＝1，2，…，6，定义与前一坐标系对应坐标轴平行的位置为0角位移位置，由X_i向Y_i、由Y_i向Z_i，或由Z_i向X_i的旋转为角位移的正方向，相邻坐标原点的连线和的长度分别为d₁、d₂、a₂，a₃、d₂和a₅，且

c_i＝cosθ_i s_i＝sinθ_i (i＝1，2，…，6)

c₂₃＝cos(θ₂+θ₃)，s₂₃＝sin(θ₂+θ₃)

c₂₃₄＝cos(θ₂+θ₃+θ₄)，s₂₃₄＝sin(θ₂+θ₃+θ₄)

则在机械臂的基座坐标系O₀-X₀Y₀Z₀中，基座坐标系原点为(x₀₀，y₀₀，z₀₀)＝(0，0，0)，第1坐标系原点为(x₀₁，y₀₁，z₀₁)＝(0，0，d₁)，第2个关节的坐标为(x₀₂，y₀₂，z₀₂)＝(-d₂s₁，d₂c₁，d₁)，第3个关节的坐标记为(x₀₃，y₀₃，z₀₃)，则

$\left(\begin{array}{c}{x}_{03}\\ y_{03}\\ z_{03}\end{array}\right)=\left(\begin{array}{c}{a}_2{c}_1{c}_2-d_2{s}_1\\ a_2{s}_1{c}_2+d_2{c}_1\\ d_1-a_2{s}_2\end{array}\right)$

第4个关节的坐标记为(x₀₄，y₀₄，z₀₄)，则

$\left(\begin{array}{c}{x}_{04}\\ y_{04}\\ z_{04}\end{array}\right)=\left(\begin{array}{c}{c}_1(a_2{c}_2+a_3{a}_{23})-d_2{s}_1\\ s_1(a_2{c}_2+a_3{c}_{23}){+d}_2{c}_1\\ d_1-a_2{s}_2-a_3{s}_{23}\end{array}\right)$

第5个关节的坐标记为(x₀₅，y₀₅，z₀₅)，则

$\left(\begin{array}{c}{x}_{05}\\ y_{05}\\ z_{05}\end{array}\right)=\left(\begin{array}{c}{c}_1(a_2{c}_2+a_3{a}_{23})\\ s_1(a_2{c}_2+a_3{c}_{23})\\ d_1-a_2{s}_2-a_3{s}_{23}\end{array}\right)$

末端的坐标记为(x₀₆，y₀₆，z₀₆)，则

$\left(\begin{array}{c}{x}_{06}\\ y_{06}\\ z_{06}\end{array}\right)=\left(\begin{array}{c}{a}_2{c}_1{c}_2+a_3{c}_1{c}_{23}+a_5{c}_1{c}_5{c}_{234}-a_5{s}_1{s}_5\\ a_2{s}_1{c}_2+a_3{s}_1{c}_{23}+a_5{s}_1{c}_5{c}_{234}+a_5{c}_1{s}_5\\ d_1-a_2{s}_2-a_3{s}_{23}-a_5{c}_5{s}_{234}\end{array}\right)$

以δ表示连杆上的点到连杆轴线的最大距离，机械臂一对连杆l_i，i+1和l_j，j+1(i＝0，j＝4；i＝0，1，2，3，j＝5)之间的碰撞检查程序为：

1)记A_i＝(x_0i，y_0i，z_0i)(i＝0，1，2，…，6)，计算

A_i(u)＝A_i+u(A_i+1-A_i)

A_j(v)＝A_j+v(A_j+1-A_j)

其中

$u=\frac{{(A_{i+1}-A_i)}^{\&prime;}(A_{j+1}-A_j){(A_{j+1}-A_j)}^{\&prime;}(A_i-A_j)-{\left|\right|A_{j+1}-A_j\left|\right|}^2{(A_{i+1}-A_i)}^{\&prime;}(A_i-A_j)}{{\left|\right|A_{i+1}-A_i\left|\right|}^2{\left|\right|A_{j+1}-A_j\left|\right|}^2-{\left[{(A_{i+1}-A_i)}^{\&prime;}(A_{j+1}-A_j)\right]}^2}$

$v=\frac{{(A_{j+1}-A_j)}^{\&prime;}(A_i-A_j)\left|\right|A_{i+1}-A_i\left|\right|-{(A_{i+1}-A_i)}^{\&prime;}(A_i-A_j){(A_{j+1}-A_j)}^{\&prime;}(A_{i+1}-A_i)}{{\left|\right|A_{i+}-A_i\left|\right|}^2{\left|\right|A_{j+1}-A_j\left|\right|}^2-{\left[{(A_{i+1}-A_i)}^{\&prime;}(A_{j+1}-A_j)\right]}^2}$

2)当0≤u≤1，且0≤v≤1时计算L_ij(u，v)＝||A_i(u)-A_j(v)||，L_ij(u，v)＞2δ时转5)；

3)计算

$v_0=\frac{(A_{j+1}-A_j){(A_i-A_j)}^{\&prime;}}{{\left|\right|A_{j+1}-A_j\left|\right|}^2}$

$v_1=\frac{(A_{j+1}-A_j){(A_{i+1}-A_j)}^{\&prime;}}{{\left|\right|A_{j+1}-A_j\left|\right|}^2}$

$u_0=\frac{(A_j-A_i){(A_{i+1}-A_i)}^{\&prime;}}{{\left|\right|A_{i+1}-A_i\left|\right|}^2}$

$u_1=\frac{(A_{j+1}-A_i){(A_{i+1}-A_i)}^{\&prime;}}{{\left|\right|A_{i+1}-A_i\left|\right|}^2}$

L_min＝min{||A_i-F_j(v₀)||，||A_i+1-F_j(v₁)||，||A_j-F_i(u₀)||，||A_j+1-F_i(u₁)||}

其中

$F_j\left(v_0\right)=\left\{\begin{array}{cc}{A}_j& v_0<0\\ A_j\left(v_0\right)& 0\&le;v_0\&le;1\\ A_{j+1}& v_0>1\end{array}\right.$

若L_min＞2δ转5)；

4)转6)；

5)输出“无碰撞”，检查结束；

6)输出“有碰撞”，检查结束。