CN103056872B - 空间机械手遥操作指令安全检测与修正方法 - Google Patents

Abstract

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

Description

空间机械手遥操作指令安全检测与修正方法

技术领域

本发明属于机械手控制领域，具体涉及一种空间机械手遥操作指令速度超限检测及修正方法。

背景技术

在未来的空间活动中,将有大量的空间生产、空间加工、空间装配、空间维护和修理工作进行。这样大量的工作不可能仅仅依靠宇航员完成,必须充分利用空间机器人。在遥操作远端机械手过程中，远端机械手的运动都是由主手发送指令来驱动的。空间机械手遥操作中最主要的问题是空间与地面通讯中的时间延迟以及有限的数据传输带宽。时延对空间机器人最大的影响是使连续遥操作闭环反馈控制系统变得不稳定。同时在存在时延下，即使操作者完成简单工作也需要比无时延情况下长的多的时间，这是由于操作者为避免系统不稳定，必须采取“运动-等待”的阶段工作方式。尤其当回路时延较大时，对操作者影响非常明显。这种情况下对遥操作指令进行速度检测和修正显得尤为必要。另外，搭载在本体上的机械手的运动会对基座航天器产生反作用力和力矩，引起基座航天器位置和姿态发生变化，如果机械手笛卡尔空间或关节空间运动速度较大，这种影响更加明显，所以对遥操作指令速度进行超限检测并加以修正是非常必要的。机械手臂的高速运动往往会激励起弹性结构的振动，而且轻型结构的机械手，弹性变形量大，也会产生低频振动，这不仅影响了机械臂的末端定位精度，还可能影响整个机器人系统的稳定性，同样需要对主手发出的指令进行检测和修正，确保任务可以安全执行。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种空间机械手遥操作指令安全检测与修正方法，通过反复检测限幅得到安全的指令，能够尽量减小时延影响，使机械手运动速度限定在一定范围、振动较小同时保证操作的安全性。

技术方案

一种空间机械手遥操作指令安全检测与修正方法，其特征在于步骤如下：

步骤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时刻关节角；

计算笛卡尔空间位置进动量为： $\mathrm{\ΔP}=\sqrt{{(X_t-X_{t-1})}^2+{(Y_t-Y_{t-1})}^2+{(Z_t-Z_{t-1})}^2};$

计算笛卡尔空间姿态进动量为：笛卡尔空间姿态进动量采用等效轴角表示法，由欧拉角(α,β，γ)分别计算上一时刻的旋转矩阵A_t-1＝[n_t-1,o_t-1,a_t-1]和当前时刻的旋转矩阵A_t＝[n_t,o_t,a_t]，那么末端等效误差

$e=\frac{1}{2}(n_{t-1}\×n_t+o_{t-1}\×o_t+a_{t-1}\×a_t)=K\sin \left(\mathrm{\Δ\Θ}\right)$

笛卡尔空间姿态进动量为等效转角：ΔΘ＝arcsin(|e|)

其中，等效转轴

步骤2：比较ΔP与v_PmaxΔt，如果ΔP＞v_PmaxΔt，更新当前时刻位置，

$X_t=X_{t-1}+v_{P\max }\mathrm{\Δt}*\frac{X_t-X_{t-1}}{\mathrm{\ΔP}}$

$Y_t=Y_{t-1}+v_{P\max }\mathrm{\Δt}*\frac{Y_t-Y_{t-1}}{\mathrm{\ΔP}}$

$Z_t=Z_{t-1}+v_{P\max }\mathrm{\Δt}*\frac{Z_t-Z_{t-1}}{\mathrm{\Δ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，在笛卡尔空间进行限幅，分别更新当前时刻位置和等效转角

$X_t=X_{t-1}+C_{\mathrm{Pv}}\·v_{P\max }\mathrm{\Δt}*\frac{X_t-X_{t-1}}{\mathrm{\ΔP}}$

$Y_t=Y_{t-1}+C_{\mathrm{Pv}}\·v_{P\max }\mathrm{\Δt}*\frac{Y_t-Y_{t-1}}{\mathrm{\ΔP}}$

$Z_t=Z_{t-1}+C_{\mathrm{Pv}}\·v_{P\max }\mathrm{\Δt}*\frac{Z_t-Z_{t-1}}{\mathrm{\ΔP}}$

ΔΘ＝C_Pω·ω_PmaxΔt

否则转至步骤6；依次求出等效旋转矩阵R_K，当前时刻旋转矩阵R_C＝R_K*R_I，解算出欧拉角(α,β，γ)；其中ω_Jmax为关节角速度阈值，C_Pv和C_Pω分别为限幅系数；

步骤5：对当前时刻位姿进行求逆运算，得到更新后的当前时刻关节角θ_t，返回步骤4进行关节空间角速度检测与限幅，否则转至步骤6；

步骤6：生成安全的指令，由发令端发送给远端空间机械手。

有益效果

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

附图说明

图1：空间机械手遥操作指令发送示意图；

其中1为发令端，2为指令，3为远端空间机械手，4为远端环境；

图2：笛卡尔空间限幅示意图；

图3：空间机械手遥操作指令安全检测与限幅流程图。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

在遥操作系统中，机械手的运动都是通过发送指令来驱动的。其中指令包括两种，一种是位姿指令，包含3个位置数据和3个姿态数据，一种是关节角指令，包含6个关节角数据。本发明就是将发令端当前时刻指令与上一时刻的指令分别在笛卡尔空间及关节空间速度进行检测，如果速度超过限定阈值，在笛卡尔空间进行限幅后再次进行检测，直至满足要求生成安全的指令发送给远端机械手。

步骤1：计算笛卡尔空间位置进动量和姿态进动量

首先获取当前时刻指令，如果为位姿指令(X_t,Y_tZ_t,α_t,β_t,γ_t)，逆解出相应的关节角指令(θ_1t,θ_2t,θ_3t,θ_4t,θ_5t,θ_6t)，如果为关节角指令，正解出相应的位姿指令。笛卡尔空间位置进动为：

$\mathrm{\ΔP}=\sqrt{{(X_t-X_{t-1})}^2+{(Y_t-Y_{t-1})}^2+{(Z_t-Z_{t-1})}^2}$

笛卡尔空间姿态进动采用等效轴角表示法，由姿态(α,β，γ)分别计算上一时刻的旋转矩阵A_t-1＝[n_t-1,o_t-1,a_t-1]和当前时刻的旋转矩阵A_t＝[n_t,o_t,a_t]，那么末端等效误差 $e=\frac{1}{2}(n_{t-1}\×n_t+o_{t-1}\×o_t+a_{t-1}\×a_t)=K\sin \left(\mathrm{\Δ\Θ}\right),$ 可得等效转轴等效转角ΔΘ＝arcsin(|e|)。

本实例中，当前时刻关节角指令为(1.3091°,-77.2695°,48.4546°,-5.1018°,-240.4975°,-5.9492°),姿态均采用Z-Y-X欧拉角表示，对应的末端位姿为(457.6429,40.0308,388.2520,4.7311°,-1.0493°,-175.6160°)；上一时刻的关节角指令为(4.7913°,-77.2348°,48.4296°,-0.2360°,-240.0792°,1.1735°),对应的末端位姿为(458.4170,39.7925,387.1278,3.4998°,-1.1110°,-179.7704°)。则笛卡尔空间进动量ΔP＝1.3855mm，等效转轴K＝(0.9502,0.0826,0.3006)，等效转角ΔΘ＝4.3556°。

步骤2：笛卡尔空间线速度检测与限幅

比较ΔP与V_PmaxΔt，如果ΔP＞V_PmaxΔt，更新当前时刻位置同理可得Y_t,Z_t。在本例中，V_Pmax＝38mm/s，Δt＝250ms，由步骤1中ΔP＝1.3855＜v_Pmax*Δt＝9.5，无需对其限幅。

步骤3：笛卡尔空间角速度检测与限幅

比较ΔΘ与ω_PmaxΔt，如果ΔΘ＞ω_PmaxΔt，更新当前时刻姿态，赋ΔΘ＝ω_PmaxΔt，由ΔΘ和K计算出等效旋转矩阵R_K，那么当前时刻旋转矩阵R_C=R_K*_I ，其中R_I为上一时刻旋转矩阵，由R_C可解出姿态(α,β，γ)。

本例中取ω_Pmax＝1.8°/s，那么ΔΘ＝4.3556°＞ω_PmaxΔt＝0.45°，直接令ΔΘ＝0.45°，和等效转轴K带入R_K，计算出此时更新后的姿态角α＝3.6268°,β＝-1.1005°，γ=-179.3412°。

步骤4：计算关节空间进动量

对当前时刻位姿进行求逆运算，得到当前时刻关节角，并与上一时刻关节角逐一想减，得到六个关节的进动量Δθ₁～Δθ₆。

由当前时刻位姿(457.6429,40.0308,388.2520,3.6268°,-1.1005°,-179.3412°)逆解出六个关节角(4.4647°,-77.1439°,48.4626°,-0.7405°,-240.2108°,0.4699°),则进动量Δθ₁＝-0.3266°,Δθ₂＝0.0909°,Δθ₃＝0.0329°,Δθ₄=-0.5045°,Δθ₅＝-0.1316°,Δθ₆＝-0.7036°。

步骤5：关节空间角速度检测与限幅

将Δθ_i与ω_JmaxΔt相比较，如果|Δθ_i|＞ω_JmaxΔt，在笛卡尔空间进行限幅，更新当前时刻位置 $X_t=X_{t-1}+C_{\mathrm{Pv}}\·v_{P\max }\mathrm{\Δt}*\frac{X_t-X_{t-1}}{\mathrm{\ΔP}},$ 同理可得Y_t,Z_t，当前时刻等效转角ΔΘ＝C_Pω·ω_PmaxΔt，依次求出等效旋转矩阵R_K，当前时刻旋转矩阵R_C＝R_K*R_I，解算出姿态(α,β，γ)。

由步骤4可见Δθ₄和Δθ₆均超过角速度阈值，进行笛卡尔空间第一次限幅，C_Pv＝C_Pω＝0.8，此时末端位姿为(457.7978,39.9832,388.0271,3.6014°,-1.1025°,-179.4270°),对应的一组关节角为(4.5301°,-77.1618°,48.4560°,-0.6397°,

-240.1840°,0.6105°),返回步骤4计算发现此时Δθ₆＝-0.5630°仍大于0.45°,进行笛卡尔空间第二次限幅，C_Pv＝C_Pω＝0.6，此时末端位姿为(457.9526,39.9355,387.8023,3.5760°,-1.1046°,-179.5129°)，对应的一组关节角为(4.5954°,-77.1799°,48.4494°,-0.5389,-240.1575,0.7511)，返回步骤4计算，关节角无超限，限幅完毕。

Claims (1)

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时刻关节角；

计算笛卡尔空间位置进动量为： $\mathrm{\ΔP}=\sqrt{{(X_t-X_{t-1})}^2+{(Y_t-Y_{t-1})}^2+{(Z_t-Z_{t-1})}^2};$

计算笛卡尔空间姿态进动量为：笛卡尔空间姿态进动量采用等效轴角表示法，由欧拉角(α,β,γ)分别计算上一时刻的旋转矩阵A_t-1＝[n_t-1,o_t-1,a_t-1]和当前时刻的旋转矩阵A_t＝[n_t,o_t,a_t]，那么末端等效误差

$e=\frac{1}{2}(n_{t-1}\×n_t+o_{t-1}\×o_t+a_{t-1}\×a_t)=K\sin \left(\mathrm{\Δ\Θ}\right)$

笛卡尔空间姿态进动量为等效转角：ΔΘ＝arcsin(|e|)

其中，等效转轴

步骤2：比较ΔP与v_PmaxΔt，如果ΔP＞v_PmaxΔt，更新当前时刻位置，

$X_t=X_{t-1}+v_{P\max }\mathrm{\Δt}*\frac{X_t-X_{t-1}}{\mathrm{\ΔP}}$

$Y_t=Y_{t-1}+v_{P\max }\mathrm{\Δt}*\frac{Y_t-Y_{t-1}}{\mathrm{\ΔP}}$

$Z_t=Z_{t-1}+v_{P\max }\mathrm{\Δt}*\frac{Z_t-Z_{t-1}}{\mathrm{\Δ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，在笛卡尔空间进行限幅，分别更新当前时刻位置和等效转角

$X_t=X_{t-1}+C_{\mathrm{Pv}}\·v_{P\max }\mathrm{\Δt}*\frac{X_t-X_{t-1}}{\mathrm{\ΔP}}$

$Y_t=Y_{t-1}+C_{\mathrm{Pv}}\·v_{P\max }\mathrm{\Δt}*\frac{Y_t-Y_{t-1}}{\mathrm{\ΔP}}$

$Z_t=Z_{t-1}+C_{\mathrm{Pv}}\·v_{P\max }\mathrm{\Δt}*\frac{Z_t-Z_{t-1}}{\mathrm{\ΔP}}$

ΔΘ＝C_Pω·ω_PmaxΔt

否则转至步骤6；依次求出等效旋转矩阵R_K，当前时刻旋转矩阵R_C＝R_K*R_I，解算出欧拉角(α,β,γ)；其中ω_Jmax为关节角速度阈值，C_Pv和C_Pω分别为限幅系数；

步骤5：对当前时刻位姿进行求逆运算，得到更新后的当前时刻关节角θ_t，返回步骤4进行关节空间角速度检测与限幅，否则转至步骤6；

步骤6：生成安全的指令，由发令端发送给远端空间机械手。