基于OpenHRP的多机器人协作运动规划与仿真

万鸾飞, 李中望, 陈慧蓉

(芜湖职业技术学院 电气工程学院，安徽 芜湖 241006)

多机器人系统的“协作”分为程序协作和运动协作两大类别.程序协作仅要求某段轨迹开始的时刻相同；运动协作要求运动某段轨迹的时间相同，即同时开始和同时结束运动.

目前多机器人系统基本都采用了主从式的控制方式，将参与协作的机器人分为主机器人和从机器人两类.主机器人的运动独立，从机器人的运动由主控制器控制，在运动时间、末端执行器位姿与主机器人保持对应关系.比如在多机器人焊接系统中，通常选取搬运机器人为主机器人，焊接机器人为从机器人[1-2]，焊接机器人的运动要配合搬运机器人的运动以保证满足焊接的要求.

1 多机器人协作运动学约束关系

1.1 并发协作的运动学约束关系[3]

并发协作过程中只要求各机器人的运动开始时刻相同，机器人末端执行器没有约束关系.因此，并发协作过程中机器人末端执行器约束关系不存在.

图1 双机器人协作搬运系统示意图

1.2 耦合运动的运动学约束关系

耦合协作运动过程要求从机器人与主机器人具有相同形式的运动，主从机器人的末端执行器在整个协作过程中保持固定不变.多机器人耦合协作运动的示例如图1所示，该类协作运动主要应用于负荷分担的任务，如协作搬运等.

考虑到耦合协作过程中，主从机器人的末端执行器的位姿保持不变，可用一个常量变换矩阵来表示.

假设齐次矩阵mbPm(t)∈R4×4是主机器人末端在主机器人基坐标系下t时刻的齐次变换矩阵，sbPs(t)∈R4×4是从机器人末端在从机器人基坐标系下t时刻的齐次变换矩阵.假定从机器人末端在主机器人基坐标系下的齐次变换矩阵为mbPs(t)∈R4×4，则该矩阵可表示为式(1).

mbPs(t)=mbHsb·sbPs(t)

(1)

其中,mbHsb∈R4×4是从机器人基坐标系到主机器人基坐标系的齐次变换矩阵.此变换矩阵在当两个机器人被安装之后位置固定，因此mbHsb在整个过程中是一个恒定的齐次变换矩阵.设定t0作为mbPm(t)和sbPs(t)共同的起始时间，并设定mHs∈R4×4为在主机器人基坐标系下由从机器人末端到主机器人末端在t0时刻的变换矩阵，即得

mbPm(0)=mHs·mbPs(0)

(2)

将式(1)代入式(2)在t=t0得到

mbPm(0)=mHs·mbHsb·sbPs(0)

(3)

由式(3)可得

(4)

根据耦合运动的定义得知mHs在整个耦合运动中是一个常量矩阵，由此可得

(5)

由式(5)可得

(6)

式(6)表示在耦合协作运动过程中主从机器人末端位姿之间的约束关系，根据式(6)可知当两个机器人的起始位姿都确定后，耦合运动过程中从机器人的末端位姿sbPs(t)完全取决于主机器人的末端位姿mbPm(t)，式(6)为耦合运动轨迹规划建立了理论基础.

2 多机器人协作规划算法

2.1 主从机器人单独轨迹规划算法

2.1.1 主机器人轨迹规划

主机器人轨迹规划包括直线和圆弧规划，直线规划通过LineInterpolatePoints函数实现，圆弧规划通过ArcInterpolatePoints函数实现，主机器人的直线和圆弧的测试程序如图2所示.

程序中o表示搬运机器人的初始位姿，圆弧运动的起点、中点和终点分别是a、b、c.机器人运动轨迹为一段直线、一段圆弧，最后从圆弧的终点回到初始位置.整个运动过程中，机器人的姿态不发生变化.

2.1.2 从机器人轨迹规划

从机器人轨迹规划包括直线和圆弧规划，实现函数与上述一致，测试程序如图3所示.

图2 主机器人直线-圆弧测试程序

图3 从机器人直线-圆弧测试程序

2.2 耦合运动算法

2.2.1 直线耦合运动

直线耦合运动是指具有协作关系的机器人在某一时刻同时开始直线形式的运动，运动过程中保持各机器人末端位姿关系不变[4-5]，依靠直线耦合运动函数CoupleMotionL[6]实现，测试程序如图4所示.

图4 直线耦合运动测试程序

以双机器人进行直线耦合为例，已知主机器人基座标系与从机器人的相对位姿，主机器人的起点和终点，程序根据直线耦合运动原理从机器人的起点可计算出从机器人相对于自身基座标系的运动轨迹.

2.2.2 圆弧耦合运动

圆弧耦合运动是指具有协作关系的机器人在某一时刻同时开始圆弧形式的运动[7-8]，通过圆弧耦合运动函数CoupleMotionC实现，测试程序如图5所示.

图5 圆弧耦合运动测试程序

规划圆弧耦合运动时，给出主机器人与从机器人的相对位姿，主机器人圆弧的起点、中点、终点和速度，只需给出从机器人圆弧运动的起点，从机器人具体的运动轨迹和速度均可通过耦合关系计算得出.

3 多机器人协作仿真功能验证

3.1 直线耦合运动三维仿真

在规划直线耦合运动的仿真环境中，主机器人末端安装抓手工具—从机器人手持焊枪.机器人初始位置行走至直线耦合运动起点，协作机器人耦合运动一段直线，直线耦合运动结束后，机器人回到各自的初始位置.仿真图如图6所示.

图6 直线耦合运动三维仿真图

3.2 圆弧耦合运动三维仿真

在规划圆弧耦合运动的仿真环境中，主机器人末端安装抓手工具—从机器人手持焊枪.机器人均从初始位置行走至圆弧耦合运动起点，协作机器人耦合运动一段圆弧，圆弧耦合运动结束后，机器人回到各自的初始位置.具体的仿真图如图7所示.

图7 圆弧耦合运动三维仿真图

4 总结

本文是以主从机器人单独轨迹规划算法和耦合运动算法为依据，进行主机器人和从机器人直线轨迹规划和圆弧轨迹规划.以双机器人协作耦合展开实验，通过仿真实验，得出多机器人协作直线耦合和圆弧耦合运动三维仿真实例，确保了双机器人协作加工路径规划的科学性.

[1] 甘亚辉.柔性化焊接系统中的多机器人协作控制研究[D].南京：东南大学，2014.

[2] 孟石,戴先中,甘亚辉.多机器人协作系统轨迹约束关系分析及示教方法[J].机器人,2012,34(5)：546-552，565.

[3] 梅乐.基于多智能体的多机器人跟踪控制研究[D].合肥：合肥工业大学,2016.

[4] 阮志虎.两轮差速驱动移动机器人的运动模型与控制研究[D].重庆：重庆大学,2015.

[5] 张颖.多移动机器人编队导航的双边遥操作方法研究[D].南京：东南大学,2016.

[6] 陈宇航,赵铁石,宋晓鑫,等.变结构并联多维振动台自由度特性与运动学分析[J].机器人,2016,38(2)：135-143.

[7] 高焕兵.带电抢修作业机器人运动分析与控制方法研究[D].济南：山东大学,2015.

[8] 马勇.多移动机器人路径规划研究[D].武汉：华中科技大学,2012.