【论文笔记】基于在线预测和规划的机器人动态跟踪抓取方法

摘要

内容：基于ROS框架，以6自由度ABB机器人为研究对象，设计了一种基于在线预测和规划的机器人动态跟踪抓取方法。
问题的提出与解决：

1. 通过预先检测目标物的运动状态在线预测其运行轨迹；
2. 对预估轨迹进行运动规划得到机械臂的跟踪轨迹；
3. 实现机器人对移动目标物体进行动态跟踪抓取；

针对问题	解决
ROS系统与机器人通信反馈机制造成机器人跟踪过程断续、跟踪轨迹不连续	一种多轨迹融合方法，用于运动 规划时融合多段轨迹，实现了机器人持续地跟踪移动物体
机器人对不同移动速度的目标物体跟踪不及时、跟踪落后	一种运动规划模型，以物体预估轨迹的时间长度规划机械跟踪轨迹，解决了机器人动态跟踪滞后

传送带获取状态，并实现动态跟踪

关键词

1. 动态跟踪;
2. 轨迹规划;
3. 多轨迹融合;
4. 移动物体抓取;
5. ROS

0 引言

ROS优点：（写论文引言可以借鉴，嘿嘿）

1. 以点对点的设计；
2. 多语言支持；
3. 开源；
4. 集成众多仿真工具包

之前学者工作如下：

学者	工作
Cowley 等	基于ROS系统采用3D视觉引导，使得PR2机器人成功抓取传送带上的物体	A. Cowley, B. Cohen, W. Marshall, C. J. Taylor and M. Likhachev. Perception and motion planning for pick-and-place of dynamic objects[J]. 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2013: 816-823．
Menon 等	提出了基于搜索的运动规划算法，生成与目标物体的轨迹相匹配的PR2机器人运行轨迹	MENON A, COHEN B, LIKHACHEV M. Motion planning for smooth pickup of moving objects [J]. International Conference on Robotics and Automation, 2014:453-460.
Boschetti 等	提出一种路径规划技术，圆形输送机	G. Boschetti. A picking strategy for circular conveyor tracking [J]. International Conference on Mechatronic and Embedded Systems and Applications ( MESA) [J]. 2014, 10:1-6.
Shin 等	几何路径约束的机械臂最小时间控制算法，实现在最短时间内对传送带上物体的抓取和拦截	I. S. Shin, S. Nam, R. G. Roberts and S. B. Moon. Minimum Time Algorithm for Intercepting an Object on the Conveyor Belt by Robot [J]. 2007 International Symposium on Computational Intelligence in Robotics and Automation [J]. 2007: 362-367.
幸杰	采用 PI 控制算法以实时获得目标物与机器人坐标	幸杰. 工业机器人运动工件跟踪抓取技术研究与实现 [D]. 重庆: 重庆大学, 2018.
Lan 等	提出了轨迹竞争多目标粒子群优化算法，搜索机器人最优轨迹的帕累托最优解集，完成多目标轨迹规划的方法	Lan, J. ; Xie, Y. ; Liu, G. ; Cao, M. A Multi-Objective Trajectory Planning Method for Collaborative Robot [J]. Electronics 2020, 9: 859.
张弛	一种运动物体跟踪算法，能够实现无障碍环境下移动物体的平滑抓取	张驰，尚伟伟，丛爽，刘宜. 机器人平滑抓取移动物体的运动规划方法 [J]. 机械工程学报，2018，54( 19) : 10－17．

通信反馈机制问题 $\to$ 无法在ROS系统和工业机械臂间搭建实时的跟踪系统

1 在线预测与运动规划算法设计

建立机械臂末端点与传送带的坐标系
经过坐标转换矩阵统一在机械臂坐标系

1. for 机械臂末端点：
   末端抓手中心点 $\to$ 机械臂基坐标系 $\to$ 正运动学模型推导
   抓手和机械臂末端同轴相连 $\to$ 中心直线距离： $d_g$
2. for 传送带：
   目标物体在传送带坐标系 $\to$ ${}_o^{c}T={[}_o^{c}x{,}_o^{c}y{]}^T$
   传送带坐标系相对于机械臂基坐标系的变换矩阵 $\to$
   $${}_c^{B}T={[}_c^{B}x{,}_c^{B}y{,}_c^{B}z{,}_c^B\gamma {,}_c^B\varphi {,}_c^B\psi {]}^T$$
   传送带坐标系相对于机械臂基坐标系的位姿 $\to$
   $${}_o^{B}T{=}_c^{B}T{\times }_o^{c}T$$
   通过硬件设备获得并经坐标系转换后移动物体在基坐标系下的实时坐标

(1) 在线预测

1. 移动物体在 $t_0$ 时刻触发光电信号
2. 系统获得速度 $v$
3. 以 $t_0$ 时刻目标物所在 $G_0$ 点为基准点，预估目标物 $t_1\sim t_N$ 时刻在传送带坐标系中的位置
4. 得到在机器人坐标系下的位姿 ${}_o^B{V}_1{\cdots }_o^B{V}_N$
   

(2) 运动规划

快速扩展树算法

S. R. Martin, S. E. Wright and J. W. Sheppard. Offline and Online Evolutionary Bi-Directional RRT Algorithms for Efficient Re-Planning in Dynamic Environments [J]. International Conference on Automation Science and Engineering, 2007: 1131-1136.

五次样条插补算法进行轨迹插补

Yuhang Li, Tian Huang, Derek G. Chetwynd. An approach for smooth trajectory planning of highspeed pick-and-place parallel robots using quintic B-splines [J]. Mechanism and Machine Theory, 2018, 126: 479-490.

ROS系统与机器人之间采用 simple_message 协议，并存在通信反馈机制；
机器人执行上位机指令结束后
$\to$将机械臂关节信息反馈回上位机系统，以实现控制系统中机器人
的姿态同步
$⟶$ 系统与目标姿态无法实时跟踪
$⟶$ 跟踪过程断续、跟踪轨迹不连续

1. 生成过渡性轨迹：根据目标物体的位姿${}_o^B{V}_0$，生成机械臂末端抓手从预备位置运动到 $G_0$ 位置的过渡性轨迹；
2. 再生轨迹：根据目标物体的位姿${}_o^B{V}_2$，生成机械臂的第二段规划路径，即末端抓手从 $G_1$ 位置到 $G_2$ 位置的跟踪轨迹；两段跟踪轨迹是连续的，意味着末端抓手从 $G_0$ 位置运行途经 $G_1$ 位置到 $G_2$ 位置；
3. 依次规划机械臂剩下的轨迹，最终得到一条完整的连贯的机械臂跟踪轨迹；

(3) 动态跟踪及抓取

1. 系统实时获得末端抓手 TCP 点与移动物体在基坐标系下的坐标值。
2. 当机械臂和物体坐标满足$V_{trans}<\delta ,V_{rot}=0$ 时，系统控制抓手完成抓取过程。
3. 末端抓手和物体之间的最大距离 $\to$ $\delta$
   末端抓手和目标物体之间的平移矢量 $\to$ $$V_{trans}={[}_g^{B}x{－}_o^{B}x{,}_g^{B}y{－}_o^B{y}_g^{B}z{－}_o^{B}z{]}^T$$
   末端抓手和目标物体之间的旋转矢量 $\to$
   $$V_{rot}={[}_g^B\varphi {－}_o^B{\varphi }_g^B\gamma {－}_o^B{\gamma }_g^B\psi {－}_o^B\psi {]}^T$$

2 动态跟踪算法设计

目标物预估轨迹的时间长度来规划机械臂跟踪轨迹的时间长度

预估轨迹 $t_r(x)$ $\to$ 机械臂末端抓手到达传送带上某一点所需要的时间
预估轨迹 $t_o(x)$ $\to$ 目标物体到达$x$(传送带上某一点)所需要的时间
$$t_o(x)=c_1\times x+c_2,c1,c2\in \mathbf{R}$$

两曲线能相交 $\to$ 能抓取 反之不能

机械臂末端抓手的运行轨迹分为三段

1. A 段为加速运行轨迹；
2. B 段为匀速运行轨迹；
3. C 段为减速运行轨迹。

算法设计须保证机械臂末端抓手在运行至 B 段轨迹时与目标物体实现动态跟踪相遇并抓取

机械臂在执行完第一段轨迹后才能抵达 $G_0$ 位置，当末端抓手抵达 $G_0$ 位置时，目标物体已从 $G_0$ 位置( 坐标系$x$轴的起始点) 运行至之后位置。
$\to$ 相对于 $t_o(x)$ 而言，跟踪轨迹 $t_r(x)$ 落后于 $t_o(x)$ 一段时间 $\Delta t$
$\to$ 在引入时间差值$\Delta t$后，当两条轨迹的交点仍在B段之中，末端抓手才可实现稳定地动态跟踪抓取。

3 实验验证

3. 1 动态跟踪抓取实验

(1) 系统框架

(2) 动态跟踪抓取实验

传送带速度经编码器测得 $v=6.04cm/s$
目标物体和末端抓手 TCP 点在机器人基坐标系下的坐标如表所示

3. 2 融合轨迹连续性

融合轨迹的连续性体现在机器人末端抓手在跟踪目标物体过程中是否存在轨迹停顿，轨迹速度是否连续。
由ROS系统下发到控制柜的关节轨迹信息是组成跟踪轨迹的多个路径点的机械臂六轴控制参数值，每个路径点六轴的参数包含控制各轴转动的时间值、转动角度、转动速度以及转动加速度等信息。

3. 3 动态跟踪鲁棒性

实验证明在一定传送带速度范围内，机械臂对不同移动速度的目标物体可实现稳定地动态跟踪抓取，实验结果取得了良好的效果，验证了以预估轨迹的时间度量来规划机械臂跟踪轨迹模型，即动态跟踪的鲁棒性。

4 结语