一种基于位置速度控制的遥操作机器人碰撞预警方法
【专利摘要】本发明提供一种基于位置速度控制的遥操作机器人碰撞预警方法，其步骤包括：首先将主手控制器视为积分器，通过位置积分计算出对机器人位置的期望值，实现机器人任务速度与手控器位置的有效对应，然后根据手控器位置信号主动预测出未来时刻机器人的位置，当机器人与环境临近碰撞状态时生成预警力，最后将反馈力与操作者手动控制力进行融合，引导操作者控制机器人避开障碍物，完成作业过程。本发明可将人类智能决策与机器智能相结合，有效提高机器人的作业精度与效率，减轻操作者的作业负担，降低对操作者技术熟练度的依赖，避免系统延时导致对机器人控制的盲目性。
【专利说明】
一种基于位置速度控制的遥操作机器人碰撞预警方法
技术领域
[0001] 本发明属于机器人人机交互及机器人遥操作领域，特别涉及一种基于位置速度控 制的遥操作机器人碰撞预警方法。
【背景技术】
[0002] 在危及人身安全的极限环境或危险环境下作业，例地震灾后或核事故现场清理救 援、海洋探索、宇宙空间开发和利用等，机器人作为代替人类进行作业的工具，发挥越来越 重要的作用，因而遥操作技术得到充分发展与运用。但是，由于受机构、控制、人工智能、传 感和材料技术等发展水平的制约，自主作业机器人的开发在短期内无法实现。机器人控制 中通常所采用的操作方法是直接控制法，而机器人控制领域的操作性及安全性目前还存在 诸多问题：
[0003] 首先，遥操作系统存在着不可避免的延时问题，这极易导致操控不当而带来作业 对象的损毁。现有的机器人控制方法中，机器人的位置控制方式属于被动方式，即机器人只 是顺应跟随手控器的位置进行作业，操作者并不能主动预测出机器人未来某一时刻的姿 态。由于数据传输量以及传输距离等问题，遥操作系统存在着不可避免的延时问题，这将导 致作业过程中极易操控不当，造成无法弥补的损失。其次，在远程操作系统中，手控器的种 类繁多，通用性差 [W]。不仅不同领域的手控器结构各不相同，即使同一领域的手控器结构 也存在差异。再次，手控器有效活动空间同机器人的活动空间存在差异，这将导致高精度作 业不易实现，操作者工作负担增加 [8]。当手控器的活动空间范围远远小于机器人的活动范 围时，操作者对于手控器的控制精度难以掌握，机器人便难以进行高精度作业。最后，操作 者的技能水平和熟练程度极大地影响着机器人作业效率的高低。手控器同机器人在结构、 尺寸、有效活动空间范围等方面通常存在很大差异，操作者在操控机器人过程中，特别是精 细作业任务中，往往需要反复调整机器人的姿态，才可以完成作业任务+ 11]，这无疑会导致 机器人作业效率不高。
[0004] [1]Adelstein B.D.,Rosen M.U.J..Design and implementation of a force reflecting manipulandum for manual control research.Advances in Robotics，ASME DSC，1992,5(3):卜12.
[0005] [2]Howe R.D.,Force-reflecting teleoperated hand system for the study of tactile sensing in precision manipulation.Proceedings of the IEEE International Conference on Robotics and Automation，1992,1321-1326.
[0006] [3]Massie T，Salisbury K.The phantom haptic interface:a device for probing virtual objects.ASME Journal of Dynamic Systems，Measurement，and Control，1994，116:295-299.
[0007] [4]Berkelman PJ.,Hollis RL?，Salcudean SE..Interacting with virtual environments using a magnetic levitation haptic interface.Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems，1995，117- 122.
[0008] [5]Ellis RE,Ismaei1 0M，Lipsett MG.Design and evaluation of high-performance haptic interface.Robotica,1996,14:321-327.
[0009] [6]Chiaverini S. .Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators.IEEE Transaction on Robotic and Automation,1997,13(3):398-410.
[0010] [7]Jin B.D.,Ki-Young,Kwon D.-S..A 6D0F force-reflecting master hand controller using the fivebar parallel mechanism.Journal of Control,Automation and System Engineers,1999,5(3):312-317.
[0011] [8]宁祎.手控器的性能和评价方法.机器人，2000,22(4):282-288
[0012] [9]邓乐，赵丁选，倪涛等.基于Stewart平台的6自由度力反馈手控器.农业机械学 报，2005,36(7):118-121
[0013] [10]倪涛，赵丁选，张红彦.电液手控器型遥操作机器人力反馈控制策略.农业机 械学报，2010,41(12): 190-194
[0014] [11]Huang Lingtao,Kawamura Takuya，Hirinao Yamada.Application of a position-force control method in a master-slave teleoperation construction robot system.Applied Mechanics and Materials,2012,229-231：2243-2247.
【发明内容】
[0015] 本发明针对遥操作机器人控制领域中的操作性及安全性所存在的问题，提供一种 基于位置速度控制的遥操作机器人碰撞预警方法，具体步骤如下：
[0016] S1、基于手控器位置速度控制方法，以手控器的位置信号控制机器人运动；
[0017] S2、通过手控器的位置信号对遥操作机器人碰撞预警进行建模，构建机器人接近 目标对象的预警反力；
[0018] S3、将S1中操作者的操作力矩与S2中的接触反馈力矩和预警反馈力矩进行融合， 对手控器和机器人的运动进行控制，实现机器智能与人类智能共同作用下控制机器人接近 目标对象，完成作业过程。
[0019] 所述步骤S1包括以下步骤：
[0020] SI. 1、搭建基于摇杆式手控器控制机器人运动的系统控制平台；
[0021] S1.2、计算机采集人手力及手控器位置信号，传送数据为系统控制手控器的运动 做准备；
[0022] S1.3、通过将手控器视作为积分器，根据公式
[0023] < ~ +qs qm ^ 0 l q：：l=q： f：=〇
[0024] 计算手控器期望的机器人关节角控制信号qsd。式中，kms为手控器与机器人关节角 比例因子，t sam为系统的采样周期，f；：为n时刻手控器关节角，^/+1为t+1+l时刻手控器期望 的机器人关节角，k ms为手控器按钮修改比例因子。
[0025] S1.4、为避免积分引起控制量的过饱和及消除积分误差，输入信号为零时，设定 为上一采样时刻机器人关节角fsH。
[0026] S1.5、按照公式
[0027] xm=JTFm
[0028]计算在操纵力Fm下手控器的操作力矩
[0029] S1.6、将手控器的运动简化为质量-阻尼-弹簧系统，手控器动力学模型表示为公 式
[0030] Mmqm + Bmqm + Kmqm = kxTm ,
[0031] 式中，Fm为手控器操纵力，J为手控器的雅可比矩阵，Mm、BdPK m为手控器的惯性系 数矩阵，阻尼系数矩阵和弹簧系数矩阵，h为操作者手动控制的权值系数。
[0032] 所述步骤S2包括以下步骤：
[0033] S2.1、根据公式
[0034] q[t + ^
[0035] 预测t秒后机器人关节角位置<。式中，咖、办为手控器及机器人当前时刻关节角。
[0036] S2.2、通过立体视觉技术获取作业场景图像数据并实时重构作业对象环境。
[0037] S2.3、结合环境相机测得的背景环境(或障碍物）的高度信息，及预测得到的机器 人末端执行器位置，求解机器人末端沿速度方向与背景环境的距离d。
[0038] S2.4、依据公式
[0039] FtJk'^-d)v d<(!''
[0 d>d0
[0040]计算机器人所受的碰撞预警力Ft，式中，kt为比例因子，do为机器人末端与障碍物 间的安全距离，^为机器人末端运动速度的单位向量。
[0041] S2.5、依据公式
[0043]计算机器人与环境间的接触反馈力矩^和预警反馈力矩式中，Fe为机器人末端 与环境间的作用力，Mt、Me为力矩向量，R为机器人末端执行器坐标系相对手控器坐标系的旋 转矩阵。
[0044] 所述步骤S3包括以下步骤：
[0045] S3.1、按照公式
[0046] +BJmd +Kmqm =kf.m -kjz.+z^
[0047] 设计手控器的位置预测控制器。式中，1?为预警反馈力矩的权值系数。h为手控器 在操纵杆的操作力矩、Tr为机器人与环境的接触力矩，^为预警反馈力矩，q md为手控器的运 动趋势。
[0048] S3.2、按照公式
[0049] um - kmd(qmd -qm) + kmp(q,lld -qm)
[0050] 设计手控器的控制量um，使手控器跟随反馈力矩输出的运动趋势。式中，k_、k md为 手控器的比例增益和微分增益。
[0051] S3.3、依据公式
[0052] <： qd - q) + {ijd - a J + Ktl {q(i -q,) = F,
[0053]设计机器人阻抗模型，式中，Md、Bd和Kd分别为期望阻抗模型的惯性矩阵、阻尼矩阵 和刚度矩阵，qd和qs分别为机器人参考关节角和实际关节角。
[0054] S3.4、机器人与环境间的作用力矩满足公式
[0055] MAq + B!：IM} + = Fr,
[0056] S3.5、按公式
[0058] 计算作用力Fe经过阻抗模型后，机器人关节控制的轨迹修正量犯（At从.，
[0059] S3.6、依据公式
[0060] Q〇 = Qd-AQ
[0061 ]计算经过修正的机器人关节控制指令，式中，& = 。
[0062] S3.7、根据公式
[0063] w.' =々,.+ -么）十夂,,(义一<：/、）
[0064] 设计经阻抗修正的机器人控制量Us，实现机器人对手控器的跟随。
[0065] S3.8、机器人与环境(或障碍物)无接触情况时，依据公式
[0066] us =qm +kjqin -qj + ksp(cL, -qj
[0067] 设计机器人的控制量us，实现机器人对手控器的跟随。式中，ksp、ksd为机器人控制 器的比例增益和微分增益。
[0068]本发明相对于现有技术具有如下的优点及技术效果：
[0069] 1.提出基于位置积分的位置速度控制策略，采用位置增量式的控制，改变传统手 控器对机器人位置的点对点操作方式；
[0070] 2.降低作业效率对操作者技术熟练度的依赖；
[0071 ] 3.能够对机器人未来时刻的姿态进行预测，在机器人与周边环境发生碰撞前，生 成防碰撞预警力，避免由延时引起的误操作，改善操作系统的安全性；
[0072] 4.为手控器的开发提供一种新的思路，为通用型手控器的标准化设计奠定基础。
【附图说明】
[0073] 图1为位置速度控制技术路线；
[0074]图2为碰撞预警力建模技术路线。
【具体实施方式】
[0075]下面结合实施例及附图对本发明作进一步详细的描述，图1为位置速度控制技术 路线，图2为碰撞预警力建模技术路线。
[0076]本发明根据基于位置速度控制的遥操作机器人碰撞预警方法，包括如下步骤S1、 S2、S3:
[0077] SI、基于手控器位置积分的方法，以手控器的位置信号控制机器人运动，所述步骤 S1 包括步骤 S1.1、S1.2、S1.3、S1.4、S1.5、S1.6:
[0078] SI. 1、搭建基于摇杆式手控器控制机器人运动的系统控制平台；
[0079] S1.2、计算机采集人手力及手控器位置信号，传送数据为系统控制手控器的运动 做准备；
[0080] S1.3、通过将手控器视作为积分器，根据公式 r t+l
[0081 ] < = q;; :矣〇 Cl) 、 €jx = € =°
[0082] 计算手控器期望的机器人关节角控制信号qsd。式中，kms为手控器与机器人关节角 比例因子，t sam为系统的采样周期，<为1!时刻手控器关节角，if1为t+1+l时刻手控器期望 的机器人关节角，k ms为手控器按钮修改比例因子。
[0083] S1.4、为避免积分引起控制量的过饱和及消除积分误差，输入信号^为零时，设定 为上一采样时刻机器人关节角?：。
[0084] S1.5、按照公式
[0085] xm=JTFm (2)
[0086]计算在操纵力Fm下手控器的操作力矩
[0087] S1.6、将手控器的运动简化为质量-阻尼-弹簧系统，手控器动力学模型表示为公 式
[0088] Mmqm +Bmqm +Kmqm = klTm , (3)
[0089] 式中，Fm为手控器操纵力，J为手控器的雅可比矩阵，Mm、BdPK m为手控器的惯性系 数矩阵，阻尼系数矩阵和弹簧系数矩阵，h为操作者手动控制的权值系数。
[0090] S2、根据手控器的位置信号对遥操作机器人碰撞预警进行建模，构建机器人接近 目标对象的预警反力。所述步骤S2包括步骤S2.1、S2.2、S2.3、S2.4、S2.5:
[0091] S2.1、根据公式
[0092] (f * ^ 0) (4)
[0093] 预测t秒后机器人关节角位置<。式中，咖、办为手控器及机器人当前时刻关节角。
[0094] S2.2、通过立体视觉技术获取作业场景图像数据并实时重构作业对象环境。
[0095] S2.3、结合环境相机测得的背景环境(或障碍物）的高度信息，及预测得到的机器 人末端执行器位置，求解机器人末端沿速度方向与背景环境的距离d。
[0096] S2.4、依据公式 123 Ft = \k,{dn-d)v d<d0 (5)
[0 d > clL) 2
[0098]计算机器人所受的碰撞预警力Ft，式中，kt为比例因子，do为机器人末端与障碍物 间的安全距离，^为机器人末端运动速度的单位向量。 3 S2.5、依据公式 _ n
[0100] =-JT' C6) U」 RFe L舰」
[0101] 计算机器人与环境间的接触反馈力矩Tr和预警反馈力矩Tt。式中，Fe为机器人末端 与环境间的作用力，M t、Me为力矩向量，R为机器人末端执行器坐标系相对手控器坐标系的旋 转矩阵。
[0102] S3、将S1中操作者的操作力矩与S2中的接触反馈力矩和预警反馈力矩进行融合， 对手控器和机器人的运动进行控制，实现机器智能与人类智能共同作用下控制机器人接近 目标对象，完成作业过程。所述步骤S3包括步骤S3.1、S3.2、S3.3、S3.4、S3.5、S3.6、S3.7、 S3.8：
[0103] S3.1、按照公式
[0104] Mmqmd +Bmqmd +Kmqmd -kz{z t+Tr) (7)
[0105] 设计手控器的位置预测控制器。式中，1?为预警反馈力矩的权值系数。h为手控器 在操纵杆的操作力矩、Tr为机器人与环境的接触力矩，^为预警反馈力矩，q md为手控器的运 动趋势。
[0106] S3.2、按照公式
[0107] Un = - q,n) + kinr{qiiit! -qj ( 8 )
[0108] 设计手控器的控制量Um，使手控器跟随反馈力矩输出的运动趋势。式中，k_、k md为 手控器的比例增益和微分增益。
[0109] S3.3、依据公式
[0110] Mlt (qu，~ cj) 4 Bd (qri -q,) + Kd (a-qj = F (9 )
[0111] 设计机器人阻抗模型，式中，Md、Bd和Kd分别为期望阻抗模型的惯性矩阵、阻尼矩阵 和刚度矩阵，qd和qs分别为机器人参考关节角和实际关节角。
[0112] S3.4、机器人与环境间的作用力依照下列公式计算。
[0113] + Br.Ai] + KtiA(] = F (10)
[0114] S3.5、由阻抗控制理论，按公式
(11)
[0116] 计算作用力Fe经过阻抗模型后，机器人关节控制的轨迹修正量A&( ~c)。
[0117] S3.6、依据公式
[0118] Q〇 = Qd-AQ (12)
[0119] 计算经过修正的机器人关节控制指令見），式中，込=(也Uw)。
[0120] S3.7、根据公式
[0121] % > ( 13)
[0122] 设计经阻抗修正的机器人控制量Us，实现机器人对手控器的跟随。
[0123] S3.8、机器人与环境(或障碍物)无接触情况时，依据公式
[0124] Us = q!H -I- k^.{qm + /v\,,(<7v/ -fs) (.14》
[0125] 设计机器人的控制量us，实现机器人对手控器的跟随。式中，ksp、k sd为机器人控制 器的比例增益和微分增益。
【主权项】
1. 一种基于位置速度控制的遥操作机器人碰撞预警方法，其特征在于，包括W下步骤： 51、 基于手控器位置速度控制方法，W手控器的位置信号控制机器人运动； 52、 通过手控器的位置信号对遥操作机器人碰撞预警进行建模，构建机器人接近目标 对象的预警反力； 53、 将Sl中操作者的操作力矩与S2中的接触反馈力矩和预警反馈力矩进行融合，对手 控器和机器人的运动进行控制，实现机器智能与人类智能共同作用下控制机器人接近目标 对象，完成作业过程。2. 根据权利要求1所述的基于位置速度控制的遥操作机器人碰撞预警方法，其特征在 于，所述步骤Sl包括W下步骤： Sl. 1、搭建基于摇杆式手控器控制机器人运动的系统控制平台； SI. 2、计算机采集人手力及手控器位置信号，传送数据为系统控制手控器的运动做准 备； Sl. 3、通过将手控器视作为积分器，根据公式计算手控器期望的机器人关节角控制信号qsd，式中，kms为手控器与机器人关节角比例 因子，tsam为系统的采样周期，g:为n时刻手控器关节角，矣为t+1 + l时刻手控器期望的机 器人关节角，kms为手控器按钮修改比例因子； SI.4、为避免积分引起控制量的过饱和及消除积分误差，输入信号姑为零时，设定妃f 为上一采样时刻机器人关节角躬； SI. 5、按照公式 Tm =/Fm 计算在操纵力Fm下手控器的操作力矩Tm. SI.6、将手控器的运动简化为席畳-阳辰-強當系统.丰梓器动力学模型表示为公式式中，Fm为手控器操纵力，J为手控器的雅可比矩阵，Mm、Bm和Km为手控器的惯性系数矩 阵，阻尼系数矩阵和弹黃系数矩阵，kl为操作者手动控制的权值系数。3. 根据权利要求1所述的基于位置速度控制的遥操作机器人碰撞预警方法，其特征在 于，所述步骤S2包括W下步骤： 52.1、 根据公式预测t秒后机器人关节角位置妃，式中，qm、qs为手控器及机器人当前时刻关节角； 52.2、 通过立体视觉技术获取作业场景图像数据并实时重构作业对象环境； 52.3、 结合环境相机测得的背景环境或障碍物的高度信息，及预测得到的机器人末端 执行器位置，求解机器人末端沿速度方向与背景环境的距离d; 52.4、 依据公式计算机器人所受的碰撞预誉刀11，AT，kt刃CL W囚于，日O为机器人末端与障碍物间的 安全距离，：为机器人末端运动速度的单位向量； S2.5、依据公式计算机器人与环境间的接触反馈力矩Tr和预警反馈力矩Tt,式中，Fe为机器人末端与环 境间的作用力，Mt、Me为力矩向量，R为机器人末端执行器坐标系相对手控器坐标系的旋转矩 阵。4.根据权利要求1所述的基于位置速度控制的遥操作机器人碰撞预警方法，其特征在 于，所述步骤S3包括W下步骤： 53.1、 按照公式设计手控器的位置预测控制器，式中，k2为预警反馈力矩的权值系数，Tm为手控器在操 纵杆的操作力矩、Tr为机器人与环境的接触力矩，Tt为预警反馈力矩，qmd为手控器的运动趋 势； 53.2、 按照公式设计手巧器的巧制量Um,使手巧器跟随反馈力矩输出的;is动趋势，式中，kmp、kmd为手巧 器的比例增益和微分增益； 53.3、 依据公式设计机器人阻抗模型，式中，Md、ai和Kd分别为期望阻抗模型的惯性矩阵、阻尼矩阵和刚 度矩阵，qd和qs分别为机器人参考关节角和实际关节角； 53.4、 机器人与环境间的作用力矩满足公式 53.5、 按公式计算作用力Fe经过阻抗模型后，机器人关节控制的轨迹修正量A0( A务，A如，Aa); 53.6、 依据公式 Qc = Qd-A Q 计算经过修正的机器人关节控制指令53.7、 根据公式设计经阻抗修正的机器人控制量Us，实现机器人对手控器的跟随； S3.8、机器人与环境(或障碍物)无接触情况时，依据公式设计机器人的控制量Us,实现机器人对手控器的跟随；式中，ksp、ksd为机器人控制器的 比例增益和微分增益。
【文档编号】G05D1/02GK105911995SQ201610429644
【公开日】2016年8月31日
【申请日】2016年6月16日
【发明人】黄玲涛, 倪涛, 黄海东, 张红彦
【申请人】吉林大学