CN103884302B

CN103884302B - 空间机械臂与舱体的碰撞检测方法

Info

Publication number: CN103884302B
Application number: CN201410121106.7A
Authority: CN
Inventors: 安凯; 王晓英; 安培亮; 安宏亮
Original assignee: Individual
Current assignee: Suzhou Youdan Internet Technology Co Ltd
Priority date: 2014-03-20
Filing date: 2014-03-20
Publication date: 2016-04-13
Anticipated expiration: 2034-03-20
Also published as: CN103884302A

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θ_is_i=sinθ_i(i＝1，2，…，6)

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

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

1.关节的坐标

无论是机械臂与舱体的碰撞，还是其自身的碰撞，都与机械臂各关节的坐标有密切的关系。在机械臂的基座坐标系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₀₃)，则

(\begin{matrix} x_{03} \\ y_{03} \\ z_{03} \end{matrix}) = (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{2} c_{2} \\ d_{2} \\ d_{1} - a_{2} s_{2} \end{matrix}) = (\begin{matrix} 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{matrix})

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

\begin{matrix} (\begin{matrix} x_{04} \\ y_{04} \\ z_{04} \end{matrix}) = (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{2} c_{2} + a_{3} c_{23} \\ d_{2} \\ d_{1} - a_{2} s_{2} - a_{3} s_{23} \end{matrix}) \\ = (\begin{matrix} 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{matrix}) \end{matrix}

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

\begin{matrix} \begin{matrix}  \end{matrix} (\begin{matrix} x_{05} \\ y_{05} \\ z_{05} \end{matrix}) = (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{2} c_{2} {+ a}_{3} c_{23} \\ 0 \\ d_{1} - a_{2} s_{2} - a_{3} s_{23} \end{matrix}) \\ = (\begin{matrix} 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{matrix}) \end{matrix}

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

\begin{matrix} (\begin{matrix} x_{06} \\ y_{06} \\ z_{06} \end{matrix}) = (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{2} c_{2} + a_{3} c_{23} \\ 0 \\ d_{1} - a_{2} s_{2} - a_{3} s_{23} \end{matrix}) \\ + (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} c_{234} & 0 & s_{234} \\ 0 & 1 & 0 \\ {- s}_{234} & 0 & c_{234} \end{matrix}) (\begin{matrix} c_{5} & - s_{5} & 0 \\ s_{5} & c_{5} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{5} \\ 0 \\ 0 \end{matrix}) \\ = (\begin{matrix} 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{matrix}) + (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{5} c_{5} c_{234} \\ a_{5} s_{5} \\ - a_{5} c_{5} s_{234} \end{matrix}) \\ = (\begin{matrix} 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{matrix}) + (\begin{matrix} 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{matrix}) \\ = (\begin{matrix} 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{matrix}) \end{matrix}

2.与舱体的碰撞检查方法

由机械臂的结构可知，除第1和第2个关节之间的连杆为，其余连杆均有可能与舱体碰撞。将坐标(x_0i，y_0i，z_0i)或对应的点记为A_i(i＝0，1，2，…，6)，点A_i与点A_i+1之间的连杆记为l_i，i+1，则机械臂与舱体的碰撞问题与线段和舱体的对称轴所在的直线

(x，0，-Φ/2)(-∞＜x＜+∞)

的几何关系有关：当线段的延长线和舱体对称轴所在的直线的一个垂足P在线段之外时，若P与舱体对称轴距离大于Φ/2，则连杆l_i，i+1与舱体不会发生碰撞；若P与舱体对称轴距离小于Φ/2，只有当第i个关节和第i+1个关节与舱体对称轴距离均大于Φ/2时，连杆l_i，i+1与舱体才不会发生碰撞。当线段的延长线和舱体对称轴所在的直线的一个垂足P在线段之内时，若P与舱体对称轴距离大于Φ/2，则连杆l_i，i+1与舱体不会发生碰撞；若P与舱体对称轴距离小于Φ/2，连杆l_i，i+1与舱体必然发生碰撞。

考虑到上述碰撞检查是以连杆l_i，i+1及其延长线上的点与舱体对称轴之间的距离是否大于Φ/2为标准的，因此，为运算简单起见，可以将连杆l_i，i+1与舱体对称轴投影到O₀-Y₀Z₀平面内，将碰撞检查问题转化为连杆l_i，i+1的投影及其延长线上的点到点(0，-Φ/2)之间距离是否大于Φ/2的判断问题。为进一步简化运算，O₀-Y₀Z₀坐标平面的原点平移到(0，-Φ/2)点，新的坐标系表示为O-YZ，于是关节坐标A_i=(x_i，y_i，z_i)投影到O₀-Y₀Z₀坐标系中，平移之后在O-YZ坐标系中的坐标为

B_i＝(y_0i，z_0i+Φ/2)=(y_i，z_i)(1)

其中i＝2，3，4，5，6。如图2所示，线段及其延长线上的点可以表示为

B=B_i+λ(B_i+1-B_i)

当0≤λ≤1时意味着B在线段上。若点B是原点O到线段的延长线上的垂足，则有

(B_i+1-B_i)·(B_i+λ(B_i+1-B_i))=0

于是

(B_i+1-B_i)B_i′+λ||B_i+1-B_i||²=0

λ = - \frac{(B_{i + 1} - B_{i}) {B_{i}}^{'}}{{| | B_{i + 1} - B_{i} | |}^{2}} - - - (2)

点B到原点的距离为

D_{i, i + 1} = \sqrt{(B_{i} + λ (B_{i + 1} - B_{i})) {(B_{i} + tλ (B_{i + 1} - B_{i}))}^{'}} = \sqrt{B_{i} {B_{i}}^{'} + 2 λ B_{i} {(B_{i + 1} - B_{i})}^{'} + λ^{2} {| | B_{i + 1} - B_{i} | |}^{2}}

将

λ = - \frac{(B_{i + 1} - B_{i}) {B_{i}}^{'}}{{| | B_{i + 1} - B_{i} | |}^{2}}

代入得

\begin{matrix} D_{i, i + 1} = \sqrt{B_{i} {B_{i}}^{'} - \frac{{(B_{i} {(B_{i + 1} - B_{i})}^{'})}^{2}}{{| | B_{i + 1} - B_{i} | |}^{2}}} = \frac{\sqrt{B_{i} {B_{i}}^{'} B_{i + 1} {B_{i + 1}}^{'} - {(B_{i} {B_{i + 1}}^{'})}^{2}}}{| | B_{i + 1} - B_{i} | |} \\ = \frac{| y_{i} z_{i + 1} - y_{i + 1} z_{i} |}{\sqrt{{(z_{i + 1} - z_{i})}^{2} + {(y_{i + 1} - y_{i})}^{2}}} \end{matrix} - - - (3)

点B_i到原点的距离为

D_{i} = \sqrt{{y_{i}}^{2} + {z_{i}}^{2}} - - - (4)

3.碰撞检查程序

以δ表示连杆上的点到连杆轴线的最大距离，机械臂与舱体的碰撞检查程序为：

1)i＝2，利用等式(1)计算B_i；

2)利用等式(1)计算B_i+1，利用等式(3)计算D_i，i+1，若D_i，i+1＞Φ/2+δ转7)；

3)利用等式(2)计算λ，若0≤λ≤1，转8)；

4)若λ＞1，利用等式(4)计算D_i+1，若D_i+1＞Φ/2+δ转7)；否则转8)；

5)若λ＜0，利用等式(4)计算D_i，若D_i＞Φ/2+δ转7)；否则转8)；

6)若i＞5转9)，否则转2)；

7)输出“无碰撞”；

8)输出“有碰撞”；

9)检查结束。

Claims

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θ_is_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₀₃)，则

(\begin{matrix} x_{03} \\ y_{03} \\ z_{03} \end{matrix}) = (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{2} c_{2} \\ d_{2} \\ d_{1} - a_{2} s_{2} \end{matrix}) = (\begin{matrix} 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{matrix})

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

(\begin{matrix} x_{04} \\ y_{04} \\ z_{04} \end{matrix}) = (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{2} c_{2} + a_{3} c_{23} \\ d_{2} \\ d_{1} - a_{2} s_{2} - a_{3} s_{23} \end{matrix}) = (\begin{matrix} 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{matrix})

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

(\begin{matrix} x_{05} \\ y_{05} \\ z_{05} \end{matrix}) = (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{2} c_{2} + a_{3} c_{23} \\ 0 \\ d_{1} - a_{2} s_{2} - a_{3} s_{23} \end{matrix}) = (\begin{matrix} 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{matrix})

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

(\begin{matrix} x_{06} \\ y_{06} \\ z_{06} \end{matrix}) = (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{2} c_{2} + a_{3} c_{23} \\ 0 \\ d_{1} - a_{2} s_{2} - a_{3} s_{23} \end{matrix}) + (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} c_{234} & 0 & s_{234} \\ 0 & 1 & 0 \\ - s_{234} & 0 & c_{234} \end{matrix}) (\begin{matrix} c_{5} & - s_{5} & 0 \\ s_{5} & c_{5} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{5} \\ 0 \\ 0 \end{matrix})

= (\begin{matrix} 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{matrix}) + (\begin{matrix} c_{1} & - s_{1} & 0 \\ s_{1} & c_{1} & 0 \\ 0 & 0 & 1 \end{matrix}) (\begin{matrix} a_{5} c_{5} c_{234} \\ a_{5} s_{5} \\ - a_{5} c_{5} s_{234} \end{matrix})

= (\begin{matrix} 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{matrix}) + (\begin{matrix} 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{matrix})

= (\begin{matrix} 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{matrix})

以δ表示连杆上的点到连杆轴线的最大距离，其碰撞检查程序为：

1)i＝2，利用等式

B_i＝(y_0i，z_0i+Φ/2)＝(y_i，z_i)

计算B_i；

2)利用等式

B_i＝(y_0i，z_0i+Φ/2)＝(y_i，z_i)

计算B_i+1，利用等式

D_{i, i + 1} = \frac{| y_{i} z_{i + 1} - y_{i + 1} z_{i} |}{\sqrt{{(z_{i + 1} - z_{i})}^{2} + {(y_{i + 1} - y_{i})}^{2}}}

计算D_i，i+1，若D_i，i+1＞Φ/2+δ转7)；

3)利用等式

λ = - \frac{(B_{i + 1} - B_{i}) {B_{i}}^{'}}{| | B_{i + 1} - B_{i} | |^{2}}

计算λ，若0≤λ≤1，转8)；

4)若λ＞1，利用等式

D_{i} = \sqrt{{y_{i}}^{2} + {z_{i}}^{2}}

计算D_i+1，若D_i+1＞Φ/2+δ转7)；否则转8)；

5)若λ＜0，利用等式

D_{i} = \sqrt{{y_{i}}^{2} + {z_{i}}^{2}}

计算D_i，若D_i＞Φ/2+δ转7)；否则转8)；

6)i的值加1，若i＞5转9)，否则转2)；

7)输出“无碰撞”；

8)输出“有碰撞”；

9)检查结束。