一种基于复杂操作任务的非完整遥操作约束控制方法

【技术领域】

本发明属于空间遥操作技术领域，涉及一种基于复杂操作任务的非完整遥操作约束控制方法。

【背景技术】

在诸多机器人领域中已经证明了虚拟夹具在任务执行时间及总体精准度方面都有提高，然而夹具也是非灵敏型的辅助，如在空间遥操作过程中，需要通过虚拟夹具对机械臂末端的位置及姿态同时进行控制，在保证位置控制最优的情况下往往会失去对末端姿态的准确控制。为了在空间遥操作中实现良好的操作效果，需要给机械臂末端添加合适的辅助，使其灵活的根据任务环境及操作任务进行位置和姿态协调。

在非结构化环境中，机器人的运动受到几何及动力学约束，平移和旋转自由度受到限制。在空间遥操作任务过程时，机械臂末端一般只在平动方向约束情况下便能完成操作任务，然而针对一些复杂的空间操控任务，仅仅平动方向的约束限制难以保证操作的高效性和安全性，故要求在机械臂末端旋转方向也添加辅助作用，使操作者能够动态的根据实际操作环境改变平动和旋转方向的虚拟夹具辅助效果，从而更好的完成遥操作任务。

【发明内容】

本发明的目的在于克服上述现有技术的缺点，提供一种基于复杂操作任务的非完整遥操作约束控制方法。

为达到上述目的，本发明采用以下技术方案予以实现：

一种基于复杂操作任务的非完整遥操作约束控制方法，包括以下步骤：

1)计算约束矩阵C_VF；

定义约束矩阵C_VF表示对应的约束能力，且C_VF是6×6半正定对称矩阵，并将空间作用力映射成C_VF的矩阵元素；操作者控制机械臂末端向着最优方向和非最优方向的运动构成了空间几何约束，且都由se(3)中的单位旋量表示，给出旋量集合S：则分解成m个rank-1的半正定矩阵之和：

$\begin{array}{c}{C}_{VF}^b=c_1{S}_1{S}_1^T+c_2{S}_2{S}_2^T+...+c_m{S}_m{S}_m^T\\ =C_1+C_2+...+C_m,c_i>0\end{array}---\left(1\right)$

其中，正比例系数c_i代表了每个约束矩阵C_i的约束能力大小，强约束使操作者控制机械臂末端沿着最优方向运动，弱约束使沿着非最优方向发生偏离；

操作者控制力的大小决定了机械臂末端在旋转和平移方向发生的位移变化δX^b＝(δφ^b,δq^b)，有

$\left(\begin{array}{c}\delta {\phi }^b\\ {\mathrm{\delta q}}^b\end{array}\right)=C_{VF}^b\left(\begin{array}{c}{m}^b\\ f^b\end{array}\right)---\left(2\right)$

其中，表示作用在机械臂末端的操作者控制力，δX^b表示旋转和平移方向的微小位移；虚拟夹具辅助下的完整约束等同于约束矩阵C为满秩矩阵，而非完整约束则对应约束矩阵C为非满秩矩阵；

2)复杂操作任务分解；

遥操作中的复杂操作任务能够分解成多个任务的组合；针对几个不同的任务，选取最优的运动旋量集合；

3)约束控制器设计；

设机械臂的空间运动速度为V^b＝(ω^b,v^b)，其中ω^b、v^b分别表示惯性空间中机械臂的旋转角速度和切向速度；控制器设计如下：

$\left(\begin{array}{c}{\omega }^b\\ v^b\end{array}\right)=K_c{C}_{VF}(g,g_d)\left(\begin{array}{c}{m}^b\\ f^b\end{array}\right)---\left(9\right)$

其中，约束矩阵C_VF(g,g_d)表示对机械臂的约束能力，操作者控制力定义为F^b＝(m^b,f^b)，比例系数K_c＝diag(c_rI_3×3,c_pI_3×3)控制操作者约束作用力的大小。

2.根据权利要求1所述的基于复杂操作任务的非完整遥操作约束控制方法，其特征在于，所述步骤2)中，选取最优的运动旋量集合的具体方法如下：

2-1)目标接近

设机械臂在位形空间中的参考轨迹为g_d，其当前位形为g，则距离参考轨迹的误差表示为

$e_1=g_d^{-1}g---\left(3\right)$

对应的运动旋量集合取误差的对数表达，有

$S=log\left(g_d^{-1}g\right)=(\hat{\psi },q)---\left(4\right)$

对应的约束矩阵C_VF＝cS由运动旋量集合构成，c的大小决定了约束作用力的强弱；

2-2)轨迹跟踪

设机械臂运动的参考轨迹为g_r(λ,t)，机械臂运动的轴线为r为运动轴线上任意一点，λ为平动方向的进动参数，||s||＝1，则相对应的切向速度为

$V_t(\lambda ,t)=\frac{\partial g_r}{\partial \lambda }{g}_r^{-1}---\left(5\right)$

记切向速度为单位旋量集合S₁，S₂为误差的对数形式:

$S_2=log\left(g_r^{-1}g\right)---\left(6\right)$

则约束矩阵为

C_VF＝c₁S₁+c₂S₂>

其中，正比例系数c₁的大小决定了机械臂受虚拟夹具约束的运动速度大小，且c₁越大对应的运动速度会减小，c₂的大小决定了机械臂跟踪参考轨迹的能力；

2-3)平面运动

假设机械臂末端的运动速度向量为v₁和v₂，则span{v₁,v₂}是指以v₁，v₂为基生成的平面，操作者控制机械臂在该二维平面内运动，定义运动旋量ξ＝(ω,v)，当沿着v₁方向运动时，取ω＝s＝0，则v₁方向的运动旋量集合为S₁＝(0,v₁)，同理v₂方向的运动旋量集合为S₂＝(0,v₂)，则机械臂在约束作用下沿轴向s运动的旋量集合为S₃＝(s,r×s)，r为s上任意一点；

从而，约束矩阵定义为

C_VF＝c₁S₁+c₂S₂+c₃S₃>

其中，c₁的大小决定机械臂沿v₁方向运动的能力，c₂的大小决定机械臂沿v₂方向运动的能力，c₃的大小决定机械臂在约束轴向s运动的能力，其中包括绕轴向转动的约束作用。

与现有技术相比，本发明具有以下有益效果：

本发明针对复杂的空间操控任务，与一般的机械臂控制方法相比较，具有任务层面和控制方法的两大有益效果。首先，机械臂在空间中要完成的操控任务包括目标接近、路径跟踪及躲避障碍等，本发明根据操作任务的不同选取合适的运动旋量集合来构造约束矩阵，较以往方法更具有适应复杂任务的能力；其次，非完整约束控制方法可以使操作更加灵活，操作者可以控制机械臂仅在平移或旋转方向运动，无需提供额外的约束，从而减轻操作者压力。

【附图说明】

图1约束功率原理图。

图2平面约束运动示意图。

图3球面及其切平面示意图。

图4球面截面操作示意图。

【具体实施方式】

下面结合附图对本发明做进一步详细描述：

参见图1-4，基于复杂操作任务的非完整遥操作约束控制方法，其特征在于，包括以下步骤：

步骤一：计算约束矩阵C_VF。

操作者控制机械臂在空间环境中完成复杂任务，要求对机械臂末端控制力进行有效约束作用，这样的一种约束作用构成虚拟夹具，定义约束矩阵C_VF表示对应的约束能力，且C_VF是6×6半正定对称矩阵，并将空间作用力映射成C_VF的矩阵元素。操作者控制机械臂末端向着最优方向和非最优方向的运动构成了空间几何约束，且都可以由se(3)中的单位旋量表示，给出旋量集合S：则可以分解成m个rank-1的半正定矩阵之和：

$\begin{array}{c}{C}_{VF}^b=c_1{S}_1{S}_1^T+c_2{S}_2{S}_2^T+...+c_m{S}_m{S}_m^T\\ =C_1+C_2+...+C_m,c_i>0\end{array}---\left(1\right)$

其中，正比例系数c_i代表了每个约束矩阵C_i的约束能力大小，强约束使操作者控制机械臂末端沿着最优方向运动，弱约束使沿着非最优方向发生偏离。这样的运动优先级区分降低了操作者的操作压力，即操作者只需控制机械臂在虚拟夹具约束作用范围内运动。

操作者控制力的大小决定了机械臂末端在旋转和平移方向发生的位移变化δX^b＝(δφ^b,δq^b)，有

$\left(\begin{array}{c}\delta {\phi }^b\\ {\mathrm{\delta q}}^b\end{array}\right)=C_{VF}^b\left(\begin{array}{c}{m}^b\\ f^b\end{array}\right)---\left(2\right)$

其中，表示作用在机械臂末端的操作者控制力，δX^b表示旋转和平移方向的微小位移。虚拟夹具辅助下的完整约束等同于约束矩阵C为满秩矩阵，而非完整约束则对应约束矩阵C为非满秩矩阵。

步骤二：复杂操作任务分解。

基于上述理论知识，遥操作中的复杂操作任务可以分解成多个简单任务(如目标接近、路径跟踪、躲避障碍等)的组合。针对几个不同的简单任务，选取最优的运动旋量集合：

(1)目标接近

设机械臂在位形空间中的参考轨迹为g_d，其当前位形为g，则距离参考轨迹的误差表示为

$e_1=g_d^{-1}g---\left(3\right)$

对应的运动旋量集合取误差的对数表达，有

$S=log\left(g_d^{-1}g\right)=(\hat{\psi },q)---\left(4\right)$

对应的约束矩阵C_VF＝cS由运动旋量集合构成，c的大小决定了约束作用力的强弱，操作者只需按照虚拟夹具提供的约束作用力随动完成操作任务即可，不需要提供额外的控制力。

(2)轨迹跟踪

设机械臂运动的参考轨迹为gr(λ,t)，机械臂运动的轴线为r为运动轴线上任意一点，λ为平动方向的进动参数，||s||＝1，则相对应的切向速度为

$V_t(\lambda ,t)=\frac{\partial g_r}{\partial \lambda }{g}_r^{-1}---\left(5\right)$

记切向速度为单位旋量集合S₁，S₂同4.3.1节中一样，定义为误差的对数形式

$S_2=log\left(g_r^{-1}g\right)---\left(6\right)$

则约束矩阵为

C_VF＝c₁S₁+c₂S₂>

其中，正比例系数c₁的大小决定了机械臂受虚拟夹具约束的运动速度大小，且c₁越大对应的运动速度会减小，c₂的大小决定了机械臂跟踪参考轨迹的能力。

(3)平面运动

假设机械臂末端的运动速度向量为v₁和v₂，则span{v₁,v₂}是指以v₁，v₂为基生成的平面，操作者控制机械臂在该二维平面内运动，如图2所示，定义运动旋量ξ＝(ω,v)，当沿着v₁方向运动时，取ω＝s＝0，则v₁方向的运动旋量集合为S₁＝(0,v₁)，同理v₂方向的运动旋量集合为S₂＝(0,v₂)，则机械臂在约束作用下沿轴向s运动的旋量集合为S₃＝(s,r×s)，r为s上任意一点。

从而，约束矩阵可定义为

C_VF＝c₁S₁+c₂S₂+c₃S₃(8)

其中，c₁的大小决定机械臂沿v₁方向运动的能力，c₂的大小决定机械臂沿v₂方向运动的能力，c₃的大小决定机械臂在约束轴向s运动的能力，其中包括绕轴向转动的约束作用。

步骤三：约束控制器设计。

操作者控制机械臂完成给定任务时，需要机械臂在旋转和平移方向都保持在稳定的速度阈值内，基于此，设机械臂的空间运动速度为V^b＝(ω^b,v^b)，其中ω^b、v^b分别表示惯性空间中机械臂的旋转角速度和切向速度。控制器设计如下：

$\left(\begin{array}{c}{\omega }^b\\ v^b\end{array}\right)=K_c{C}_{VF}(g,g_d)\left(\begin{array}{c}{m}^b\\ f^b\end{array}\right)---\left(9\right)$

其中，约束矩阵C_VF(g,g_d)表示对机械臂的约束能力，操作者控制力定义为F^b＝(m^b,f^b)，比例系数K_c＝diag(c_rI_3×3,c_pI_3×3)控制操作者约束作用力的大小，由于强约 束使机械臂沿着最优方向运动速度较快，为防止速度过快导致碰撞等意外情况的发生，选取合适的c_r、c_p使操作者约束力施加的大小得以控制，防止机械臂因失去控制而导致任务的失败。

本发明的原理：

经典分析力学中，约束可分为完整约束和非完整约束。空间遥操作中机械臂在操作者控制下能否实现给定的运动，与约束控制力的完整性和简化操作任务等有关。在非完整约束情况下，即对机械臂进行部分约束限制，具体表现为参考轨迹的操作自由度小于机械臂位形空间的维数，如在二维曲面上控制机械臂末端从A点运动至B点，给定任务序列T_r([m_x,m_y,m_z],p_r)，其中(m_x,m_y,m_z)指的是任务序列上的任一点坐标，p_r表示约束功率，如图1所示，对于被约束对象是否能实现给定的运动，其判别方法通常是考察约束力旋量与给定速度旋量的乘积即瞬时约束功率(简称约束功率)是否为零，若约束功率为零则该运动为约束允许的运动。

为了验证所设计控制器在变化操作环境中的对机械臂的有效约束作用，设计动态轨迹跟踪实验如下：

(1)实验中，操作者控制末端操作工具沿参考轨迹运动，并设计控制末端所在的操作环境是动态变化的。在动态轨迹跟踪实验中，首先设置实验环境(动态变化球面)的动态变化频率为ω(0～45rad/s)，设参考轨迹为球面上r＝50mm的纬线圆(球体半径为r₀＝100mm)，如图3所示，

(2)机械臂末端在由起始点向着目标点运动时，运动旋量集合由两部分组成，记切向速度为单位旋量集合S₁，S₂定义为误差的对数形式则约束矩阵为

C_VF＝c₁S₁+c₂S₂

其中，令c₁＝0.6，c₂＝0.8，正比例系数c₁的大小决定了机械臂受虚拟夹具约束下的运动速度大小，c₂的大小决定了机械臂跟踪参考轨迹的能力。

(3)在二维曲面中，使约束功率p_r＝0的操作状态是控制机械臂末端沿某一轴向运动(如z轴)，而围绕z轴的所有转动对z方向的运动没有约束影响，则机械臂末端可沿着z轴做连续运动。

设定变化的球体半径为r＝r₀+Δr>c＝diag(c_rI_3×3,c_pI_3×3)控制操作者约束作用力的大小，试验过程中，选取c_r＝0.1rad/s,c_p＝100mm/s，操作者控制末端操作工具从起始位置g₀向着参考轨迹g_d运动，如图4所示为球面直径方向截平面操作示意图：

实验结果表明，在变化环境中，操作者根据不同操作任务选取相应的运动旋量集合构造约束矩阵，从而可以很好的约束末端操作工具沿着指定的方向运动。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。