[发明专利]空间机械手遥操作指令安全检测与修正方法

摘要

本发明涉及一种空间机械手遥操作指令安全检测与修正方法，对发令端指令分别在笛卡尔空间和关节空间进行速度检测，如果速度超出限定阈值，在笛卡尔空间进行限幅后再次超限检测及限幅，直到满足要求为止。通过限幅可以减小机械手运动引起的振动，保证机械手定位精度。并且保持规划路径笛卡尔空间的一致性。本发明提到的空间机械手为自由飞行机器人，位置和姿态相对于基座安装坐标系。

主权项

1.一种空间机械手遥操作指令安全检测与修正方法，其特征在于步骤如下：步骤1：首先获取当前时刻指令，如果当前时刻指令为位姿指令(X_t，Y_t，Z_t,α_t，β_t，γ_t)，逆解出相应的关节角指令(θ_1t,θ_2t,θ_3t,θ_4t,θ_5t,θ_6t)，如果当前时刻指令为关节角指令，正解出相应的位姿指令；其中：(X_t，Y_t，Z_t)为t时刻笛卡尔空间位置，(α_t，β_t，γ_t)为t时刻欧拉角，θ_t为t时刻关节角；计算笛卡尔空间位置进动量为：ΔP=(Xt-Xt-1)2+(Yt-Yt-1)2+(Zt-Zt-1)2;]]>计算笛卡尔空间姿态进动量为：笛卡尔空间姿态进动量采用等效轴角表示法，由欧拉角(α,β，γ)分别计算上一时刻的旋转矩阵A_t-1＝[n_t-1,o_t-1,a_t-1]和当前时刻的旋转矩阵A_t＝[n_t,o_t,a_t]，那么末端等效误差12(nt-1×nt+ot-1×ot+at-1×at)=Ksin(ΔΘ)]]>笛卡尔空间姿态进动量为等效转角：ΔΘ＝arcsin(|e|)其中，等效转轴步骤2：比较ΔP与V_PmaxΔt，如果ΔP＞V_PmaxΔt，更新当前时刻位置，Xt=Xt-1+vPmaxΔt*Xt-Xt-1ΔP]]>Yt=Yt-1+vPmaxΔt*Yt-Yt-1ΔP]]>Zt=Zt-1+vPmaxΔt*Zt-Zt-1ΔP]]>否则转至步骤3；其中V_Pmax为末端速度阈值，Δt为指令发送间隔；步骤3：比较ΔΘ与ω_PmaxΔt，如果ΔΘ＞ω_PmaxΔt，更新当前时刻姿态：ΔΘ＝ω_PmaxΔt否则转至步骤4；由ΔΘ和K计算出等效旋转矩阵R_K，那么当前时刻旋转矩阵R_C＝R_K*R_I，其中R_I为上一时刻旋转矩阵，由R_C解出欧拉角(α,β，γ)；其中ω_Pmax为末端角速度阈值；步骤4：对当前时刻位姿进行求逆运算，得到当前时刻关节角θ_i，计算关节空间进动量：Δθ_i＝θ_i(t)-θ_i(t-1)；将Δθ_i与ω_JmaxΔt相比较，如果|Δθ_i|＞ω_JmaxΔt，在笛卡尔空间进行限幅，分别更新当前时刻位置和等效转角Xt=Xt-1+CPv·vPmaxΔt*Xt-Xt-1ΔP]]>Yt=Yt-1+CPv·vPmaxΔt*Yt-Yt-1ΔP]]>Zt=Zt-1+CPv·vPmaxΔt*Zt-Zt-1ΔP]]>ΔΘ＝C_Pω·ω_PmaxΔt否则转至步骤6；依次求出等效旋转矩阵R_K，当前时刻旋转矩阵R_C＝R_K*R_I,解算出欧拉角(α,β，γ)；其中ω_Jmax为关节角速度阈值，C_Pv和C_Pω分别为限幅系数；步骤5：对当前时刻位姿进行求逆运算，得到更新后的当前时刻关节角θ_t，返回步骤4进行关节空间角速度检测与限幅，否则转至步骤6；步骤6：生成安全的指令，由发令端发送给远端空间机械手。