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

权利要求书

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：生成安全的指令，由发令端发送给远端空间机械手。