一种基于复杂操作任务的非完整遥操作约束控制方法
【专利摘要】本发明公开了一种基于复杂操作任务的非完整遥操作约束控制方法，通过计算约束矩阵CVF，复杂操作任务分解，最终设计约束控制器；本发明针对复杂的空间操控任务，与一般的机械臂控制方法相比较，具有任务层面和控制方法的两大有益效果。首先，机械臂在空间中要完成的操控任务包括目标接近、路径跟踪及躲避障碍等，本发明根据操作任务的不同选取合适的运动旋量集合来构造约束矩阵，较以往方法更具有适应复杂任务的能力；其次，非完整约束控制方法可以使操作更加灵活，操作者可以控制机械臂仅在平移或旋转方向运动，无需提供额外的约束，从而减轻操作者压力。
【专利说明】-种基于复杂操作任务的非完整遥操作约束控制方法 【技术领域】
[0001] 本发明属于空间遥操作技术领域，设及一种基于复杂操作任务的非完整遥操作约 束控制方法。 【【背景技术】】
[0002] 在诸多机器人领域中已经证明了虚拟夹具在任务执行时间及总体精准度方面都 有提高，然而夹具也是非灵敏型的辅助，如在空间遥操作过程中，需要通过虚拟夹具对机械 臂末端的位置及姿态同时进行控制，在保证位置控制最优的情况下往往会失去对末端姿态 的准确控制。为了在空间遥操作中实现良好的操作效果，需要给机械臂末端添加合适的辅 助，使其灵活的根据任务环境及操作任务进行位置和姿态协调。
[0003] 在非结构化环境中，机器人的运动受到几何及动力学约束，平移和旋转自由度受 到限制。在空间遥操作任务过程时，机械臂末端一般只在平动方向约束情况下便能完成操 作任务，然而针对一些复杂的空间操控任务，仅仅平动方向的约束限制难W保证操作的高 效性和安全性，故要求在机械臂末端旋转方向也添加辅助作用，使操作者能够动态的根据 实际操作环境改变平动和旋转方向的虚拟夹具辅助效果，从而更好的完成遥操作任务。 【
【发明内容】
】
[0004] 本发明的目的在于克服上述现有技术的缺点，提供一种基于复杂操作任务的非完 整遥操作约束控制方法。
[0005] 为达到上述目的，本发明采用W下技术方案予W实现：
[0006] -种基于复杂操作任务的非完整遥操作约束控制方法，包括W下步骤：
[0007] 1)计算约束矩阵Cvf;
[000引定义约束矩阵Cvf表示对应的约束能力，且Cvf是6X6半正定对称矩阵，并将空间作 用力映射成Cvf的矩阵元素;操作者控制机械臂末端向着最优方向和非最优方向的运动构成 了空间几何约束，且都由S e ( 3 )中的单位旋量表示，给出旋量集合S :
弦m个rank-1的半正定矩阵之和：
[0010] 其中，正比例系数Cl代表了每个约束矩阵Cl的约束能力大小，强约束使操作者控制 机械臂末端沿着最优方向运动，弱约束使沿着非最优方向发生偏离；
[0011]操作者控制力巧=(心，/6)的大小决定了机械臂末端在旋转和平移方向发生的位 移变化时b= Snb) _有
[001。 （2)
[0013]其中，巧表示作用在机械臂末端的操作者控制力，姑b表示旋转和平移方向的微小 位移;虚拟夹具辅助下的完整约束等同于约束矩阵C为满秩矩阵，而非完整约束则对应约束 矩阵C为非满秩矩阵；
[0014] 2)复杂操作任务分解；
[0015] 遥操作中的复杂操作任务能够分解成多个任务的组合;针对几个不同的任务，选 取最优的运动旋量集合；
[0016] 3)约束控制器设计；
[0017] 设机械臂的空间运动速度为yb=( cAyb)，其中wb、vb分另懐肃肢性空间中机械臂 的旋转角速度和切向速度;控制器设计如下：
[001引
巧）
[0019] 其中，约束矩阵CvF(g，gd)表示对机械臂的约束能力，操作者控制力定义为Fb=(mb, fb)，比例系数Kc = diagkrl3X3，Cpl3X3)控制操作者约束作用力的大小。
[0020] 2.根据权利要求1所述的基于复杂操作任务的非完整遥操作约束控制方法，其特 征在于，所述步骤2)中，选取最优的运动旋量集合的具体方法如下：
[0021] 2-1)目标接近
[0022] 设机械臂在位形空间中的参考轨迹为gd，其当前位形为g，则距离参考轨迹的误差 表不为
[0023] Oj
[0024] 取误差的对数表达，有
[0025] (4)
[00%] 对应的约巧矩阵Cvf = CS由运动旋量集合构成，C的大小决定了约束作用力的强弱；
[0027] 2-2)轨迹跟踪
[002引设机械臂运动的参考轨迹为gr(A，t)，机械臂运动的轴线为沪+ls:leR}，r为运 动轴线上任意一点，A为平动方向的进动参数，M S M =1，则相对应的切向速度为
[0029]
(5)
[0030] 记切向速度为单位旋量集合Si, S2为误差的对数形式：
[0031] S^=IogC?,'.?) 货)
[0032] 则约束矩阵为
[0033] CvF = ciSi+C2S2 (7)
[0034] 其中，正比例系数Cl的大小决定了机械臂受虚拟夹具约束的运动速度大小，且Cl越 大对应的运动速度会减小，C2的大小决定了机械臂跟踪参考轨迹的能力；
[0035] 2-3)平面运动
[0036] 假设机械臂末端的运动速度向量为Vi和V2,则span {VI, V2}是指Wvi, V2为基生成的 平面，操作者控制机械臂在该二维平面内运动，定义运动旋量C = (? ,V)，当沿着Vl方向运 动时，取W = S = 0，则Vl方向的运动旋量集合为Sl =(0，Vl )，同理V2方向的运动旋量集合为S2 = (0，V2)，则机械臂在约束作用下沿轴向s运动的旋量集合为S3 = (s，rXs)，r为s上任意一 点' ；
[0037] 从而，约束矩阵定义为
[003引 CvF = ClSl+C2S2+C3S3 (8)
[0039] 其中，Cl的大小决定机械臂沿Vl方向运动的能力，C2的大小决定机械臂沿V2方向运 动的能力，C3的大小决定机械臂在约束轴向S运动的能力，其中包括绕轴向转动的约束作 用。
[0040] 与现有技术相比，本发明具有W下有益效果：
[0041] 本发明针对复杂的空间操控任务，与一般的机械臂控制方法相比较，具有任务层 面和控制方法的两大有益效果。首先，机械臂在空间中要完成的操控任务包括目标接近、路 径跟踪及躲避障碍等，本发明根据操作任务的不同选取合适的运动旋量集合来构造约束矩 阵，较W往方法更具有适应复杂任务的能力;其次，非完整约束控制方法可W使操作更加灵 活，操作者可W控制机械臂仅在平移或旋转方向运动，无需提供额外的约束，从而减轻操作 者压力。 【【附图说明】】
[0042] 图1约束功率原理图。
[0043] 图2平面约束运动示意图。
[0044] 图3球面及其切平面示意图。
[0045] 图4球面截面操作示意图。 【【具体实施方式】】
[0046] 下面结合附图对本发明做进一步详细描述：
[0047] 参见图1-4,基于复杂操作任务的非完整遥操作约束控制方法，其特征在于，包括 W下步骤：
[004引步骤一:计算约束矩阵CvF。
[0049] 操作者控制机械臂在空间环境中完成复杂任务，要求对机械臂末端控制力进行有 效约束作用，运样的一种约束作用构成虚拟夹具，定义约束矩阵Cvf表示对应的约束能力，且 Cvf是6X6半正定对称矩阵，并将空间作用力映射成Cvf的矩阵元素。操作者控制机械臂末端 向着最优方向和非最优方向的运动构成了空间几何约束，且都可W由se(3)中的单位旋量 表示，给出旋量集合S:
'^=I,2,…，6，则媒可W分解成m个rank-1的半正定
矩阵之
[0050] (1)
[0051 ] 其中，正比例系数Ci代表r每个约束矩阵Ci的约束能力大小，强约束使操作者控制 机械臂末端沿着最优方向运动，弱约束使沿着非最优方向发生偏离。运样的运动优先级区 分降低了操作者的操作压力，即操作者只需控制机械臂在虚拟夹具约束作用范围内运动。 [0化2]操作者控制力巧=如6.，/)的大小决定了机械臂末端在旋转和平移方向发生的位 移变化姑b=(Sd)b，Sqb)，有
[0053]
(2)
[0054] 其中，巧表示作用在机械臂末端的操作者控制力，姑b表示旋转和平移方向的微小 位移。虚拟夹具辅助下的完整约束等同于约束矩阵C为满秩矩阵，而非完整约束则对应约束 矩阵C为非满秩矩阵。
[0055] 步骤二:复杂操作任务分解。
[0056] 基于上述理论知识，遥操作中的复杂操作任务可W分解成多个简单任务(如目标 接近、路径跟踪、躲避障碍等）的组合。针对几个不同的简单任务，选取最优的运动旋量集 合：
[0057] (1)目标接近
[0058] 设机械臂在位形空间中的参考轨迹为gd，其当前位形为g，则距离参考轨迹的误差 表示为
[0059] (3)
[0060] ^取误差的对数表达，有
[006。 （4)
[0062] 对应的约束矩阵Cvf = CS由运动旋量集合构成，C的大小决定了约束作用力的强弱， 操作者只需按照虚拟夹具提供的约束作用力随动完成操作任务即可，不需要提供额外的控 制力。
[0063] (2)轨迹跟踪
[0064] 设机械臂运动的参考轨迹为g;r(>,t)，机械臂运动的轴线为.《=贫，r为运 动轴线h件意一点，A为平动方向的进动参数，I IsM =1，则相对应的切向速度为
[0065； 巧
[0066； I旋量集合Si，S2同4.3.1节中一样，定义为误差的对数形式
[0067； (6)
[0068] 则约束矩阵为
[0069] CvF = ciSi+C2S2 (7)
[0070] 其中，正比例系数Cl的大小决定了机械臂受虚拟夹具约束的运动速度大小，且Cl越 大对应的运动速度会减小，C2的大小决定了机械臂跟踪参考轨迹的能力。
[0071] (3)平面运动
[0072] 假设机械臂末端的运动速度向量为Vi和V2,则span {VI, V2}是指Wvi, V2为基生成的 平面，操作者控制机械臂在该二维平面内运动，如图2所示，定义运动旋量C = (?，v)，当沿 着Vl方向运动时，取O = S = 0，则vi方向的运动旋量集合为Sl = (0，Vl )，同理V2方向的运动旋 量集合为S2=(0，V2)，则机械臂在约束作用下沿轴向S运动的旋量集合为S3=(s，rXs)，r为 S上任意一点。
[0073] 从而，约束矩阵可定义为
[0074] CvF = ClSl+C2S2+C3S3 (8)
[0075 ]其中，C1的大小决定机械臂沿Vi方向运动的能力，C2的大小决定机械臂沿V2方向运 动的能力，C3的大小决定机械臂在约束轴向S运动的能力，其中包括绕轴向转动的约束作 用。
[0076] 步骤S:约束控制器设计。
[0077] 操作者控制机械臂完成给定任务时，需要机械臂在旋转和平移方向都保持在稳定 的速度阔值内，基于此，设机械臂的空间运动速度为Vb=(cAvb)，其中cAvb分别表示惯性 空间中机械臂的旋转角速度和切向速度。控制器设计如下：
[007引 ㈱
[0079] 共T，巧氷W件LVKg,gd;表示对机械臂的约束能力，操作者控制力定义为Fb= (mb, fb)，比例系数Kc = diag(Crl3X3，Cpl3X3)控制操作者约束作用力的大小，由于强约束使机械臂 沿着最优方向运动速度较快，为防止速度过快导致碰撞等意外情况的发生，选取合适的。、 Cp使操作者约束力施加的大小得W控制，防止机械臂因失去控制而导致任务的失败。
[0080] 本发明的原理：
[0081] 经典分析力学中，约束可分为完整约束和非完整约束。空间遥操作中机械臂在操 作者控制下能否实现给定的运动，与约束控制力的完整性和简化操作任务等有关。在非完 整约束情况下，即对机械臂进行部分约束限制，具体表现为参考轨迹的操作自由度小于机 械臂位形空间的维数，如在二维曲面上控制机械臂末端从A点运动至B点，给定任务序列Tr ([mx，my,mz] ,Pr)，其中（mx，my,mz)指的是任务序列上的任一点坐标，Pr表示约束功率，如图1 所示，对于被约束对象是否能实现给定的运动，其判别方法通常是考察约束力旋量郑'与给 定速度旋量WkS的乘积即瞬时约束功率(简称约束功率)是否为零，若约束功率为零则该运动 为约束允许的运动。
[0082] 为了验证所设计控制器在变化操作环境中的对机械臂的有效约束作用，设计动态 轨迹跟踪实验如下：
[0083] (1)实验中，操作者控制末端操作工具沿参考轨迹运动，并设计控制末端所在的操 作环境是动态变化的。在动态轨迹跟踪实验中，首先设置实验环境(动态变化球面）的动态 变化频率为O (0~45rad/s)，设参考轨迹为球面上r = 50mm的缔线圆（球体半径为ro = IOOmm)，如图3所示，
[0084] (2)机械臂末端在由起始点向着目标点运动时，运动旋量集合由两部分组成，记切 向速度为单位旋量集合Si，S2定义为误差的对数形式- k，g.化i,g)，则约束矩阵为
[00 化]CvF = ClSl+C2S2
[0086] 其中，令Cl = 0.6,C2 = 0.8,正比例系数Cl的大小决定了机械臂受虚拟夹具约束下 的运动速度大小，C2的大小决定了机械臂跟踪参考轨迹的能力。
[0087] (3)在二维曲面中，使约束功率Pr = O的操作状态是控制机械臂末端沿某一轴向运 动（如Z轴），而围绕Z轴的所有转动对Z方向的运动没有约束影响，则机械臂末端可沿着Z轴 做连续运动。
[008引设定变化的球体半径为r = ；r0+Ar sinot，其中Ar = 20mm。比例系数Kc = diag (Crl3X3,Cpl3X3)控制操作者约束作用力的大小，试验过程中，选取Cr = O. Irad/s,Cp=IOOmm/ S，操作者控制末端操作工具从起始位置go向着参考轨迹gd运动，如图4所示为球面直径方向 截平面操作示意图：
[0089] 实验结果表明，在变化环境中，操作者根据不同操作任务选取相应的运动旋量集 合构造约束矩阵，从而可W很好的约束末端操作工具沿着指定的方向运动。
[0090] W上内容仅为说明本发明的技术思想，不能W此限定本发明的保护范围，凡是按 照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书 的保护范围之内。
【主权项】
1. 一种基于复杂操作任务的非完整遥操作约束控制方法，其特征在于，包括W下步骤： 1) 计算约束矩阵Cvf; 定义约束矩阵Cvf表示对应的约束能力，且Cvf是6 X 6半正定对称矩阵，并将空间作用力 映射成Cvf的矩阵元素;操作者控制机械臂末端向着最优方向和非最优方向的运动构成了空 间几何约束，且都由S e ( 3 )中的单位旋量表示，给出旋量集合S : S = WhA = W，《,T_r，!' = 1，2，。.，6，则Cf';分解成m个rank-1的半正定矩阵之和：(1) 其中，正比例《数Ci代表/每个约巧矩阵Ci的约束能力大小，强约束使操作者控制机械 臂末端沿着最优方^々束使沿着非最优方向发生偏离； 操作者控制；9大小决定了机械臂末端在旋转和平移方向发生的位移变 化姑6= (Sd) b，Sqb),右(2) 其中，持表示作用在机械臂末端的操作者控制力，SXb表示旋转和平移方向的微小位 移;虚拟夹具辅助下的完整约束等同于约束矩阵C为满秩矩阵，而非完整约束则对应约束矩 阵C为非满秩矩阵； 2) 复杂操作任务分解； 遥操作中的复杂操作任务能够分解成多个任务的组合;针对几个不同的任务，选取最 优的运动旋量集合； 3) 约束控制器设计； 设机械臂的空间运动速度为yb=( C〇b，vb)，其中c〇b、vb分别表示惯性空间中机械臂的旋 转角速度和切向速度，梓制盤期i+々n下，(9) 其中，约束矩阵CvF(g，gd)表示对机械臂的约束能力，操作者控制力定义为Fb=(mb，fb)， 比例系数Kc = diag(Crl3X3，Cpl3X3)控制操作者约束作用力的大小。2. 根据权利要求1所述的基于复杂操作任务的非完整遥操作约束控制方法，其特征在 于，所述步骤2)中，选取最优的运动旋量集合的具体方法如下： 2-1)目标接近设机械臂在位形空间中的参考轨迹为gd，其当前位形为g，则距离参考轨迹的误差表示 为 (3) 对应的运动旋 ；，有 (4) 对应的约束矩阵Cvf = cS由运动旋量集合构成，C的大小决定了约束作用力的强弱； 2-2)轨迹跟踪 设机械臂运动的参考轨迹为grO，t)，机械臂运动的轴线为/ = -!f +J.S': i m，r为运动轴 线上任意一点，人为平动方向的进动参数，I |s I I =1，则相对应的切向速度为巧 记切向速度为单^ I误差的对数形式： 樹 则约束矩阵为 CvF=ClSl+C2S2 (7) 其中，正比例系数Cl的大小决定了机械臂受虚拟夹具约束的运动速度大小，且Cl越大对 应的运动速度会减小，C2的大小决定了机械臂跟踪参考轨迹的能力； 2-3)平面运动 假设机械臂末端的运动速度向量为Vi和V2,则span {Vi ,V2}是指Wvi, V2为基生成的平 面，操作者控制机械臂在该二维平面内运动，定义运动旋量C = (? ,V)，当沿着Vl方向运动 时，取W = S = O，则Vl方向的运动旋量集合为Sl = (O，Vl )，同理V2方向的运动旋量集合为S2 = (0，V2)，则机械臂在约束作用下沿轴向s运动的旋量集合为S3=(s，rXs)，r为s上任意一点； 从而，约束矩阵定义为 Cvf= C1S1+C2S2+C3S3 (8) 其中，Cl的大小决定机械臂沿Vl方向运动的能力，C2的大小决定机械臂沿V2方向运动的 能力，C3的大小决定机械臂在约束轴向S运动的能力，其中包括绕轴向转动的约束作用。
【文档编号】B25J9/16GK105904458SQ201610323695
【公开日】2016年8月31日
【申请日】2016年5月16日
【发明人】黄攀峰, 潘吉祥, 刘正雄, 孟中杰
【申请人】西北工业大学