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

摘要

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

主权项

一种空间机械臂与舱体的碰撞检测方法，其特征在于：假定航天器外形是直径为Φ的圆柱，机械臂安装在圆柱的表面，第一个关节转轴的轴线与圆柱对称轴相交，基座坐标系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₀₄)，则$\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)$第5个关节的坐标记为(x₀₅，y₀₅，z₀₅)，则$\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)$末端的坐标记为(x₀₆，y₀₆，z₀₆)，则$\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{c}_1{s}_5\\ d_1-a_2{s}_2-a_3{s}_{23}-a_5{c}_5{s}_{234}\end{array}\right)$以δ表示连杆上的点到连杆轴线的最大距离，其碰撞检查程序为：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)利用等式$\lambda =-\frac{(B_{i+1}-B_i){B_i}^{\prime }}{\left|\right|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)检查结束。