[paper notes] robot dynamic tracking and grasping method based on online prediction and planning

Abstract

Content ： be based on ROS frame , With 6 freedom ABB robot As the research object , A design based on Online forecasting and planning Robot dynamic tracking and grasping method .
Raising and solving problems ：

1. adopt Detect the motion state of the target in advance On-line Predict its trajectory ;
2. Move the estimated trajectory planning Get the robot arm Track track ;
3. Realize the robot to move the target object Dynamic tracking and grabbing ;

Aiming at problems	solve
ROS The feedback mechanism of communication between the system and the robot causes the robot The tracking process is intermittent 、 The tracking track is discontinuous	A multi trajectory fusion method , be used for motion planning when Fuse multi segment tracks , Realized the robot Keep track of moving objects
The robot tracks the target objects with different moving speeds Not in time 、 Tracking lag	A sport planning Model , Using objects to predict the trajectory Length of time Plan the mechanical tracking track , Solved the robot Dynamic tracking lag

Belt acquisition status , And realize dynamic tracking

key word

1. Dynamic tracking ;
2. Trajectory planning ;
3. Multi trajectory fusion ;
4. Moving objects grab ;
5. ROS

0 introduction

ROS advantage ：（ Writing the introduction of the thesis can be used for reference , Hey ）

1. Point to point design ;
2. Multilingual support ;
3. Open source ;
4. Integrate many simulation toolkits

The previous work of scholars is as follows ：

scholars	Work
Cowley etc.	be based on ROS System USES 3D Visual guidance , bring PR2 The robot successfully grabbed the object on the conveyor belt	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 etc.	Put forward Search based motion planning algorithm , Generate PR2 Robot trajectory	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 etc.	Put forward A path planning technology , Circular conveyor	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 etc.	Manipulator with geometric path constraints Minimum time control algorithm , Realize the capture and interception of objects on the conveyor belt in the shortest time	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.
Xing Jie	use PI Control algorithm In real time Target object And robot coordinate	Xing Jie . Research and implementation of tracking and grasping technology for moving workpiece of industrial robot [D]. Chongqing : Chongqing University , 2018.
Lan etc.	Proposed trajectory competition Multi objective particle swarm optimization algorithm , Search the Pareto optimal solution set of the optimal trajectory of the robot , Method of completing multi-objective trajectory planning	Lan, J. ; Xie, Y. ; Liu, G. ; Cao, M. A Multi-Objective Trajectory Planning Method for Collaborative Robot [J]. Electronics 2020, 9: 859.
Zhang Chi	A kind of Moving object tracking algorithm , It can realize the smooth grasp of moving objects in an obstacle free environment	Zhang Chi , Shang Weiwei , Cong Shuang , Liu Yi . A motion planning method for robots to grasp moving objects smoothly [J]. Journal of Mechanical Engineering ,2018,54( 19) : 10－17．

Communication feedback mechanism problem $\to$ Cannot be in ROS A real-time tracking system is built between the system and the industrial manipulator

1 Online prediction and motion planning algorithm design

establish End point of mechanical arm And Conveyor belt The coordinate system of
after Coordinate transformation matrix Unified in Manipulator coordinate system

1. for End point of mechanical arm ：
   Center point of end grab $\to$ Manipulator base coordinate system $\to$ Forward kinematics model derivation
   The gripper is coaxially connected with the end of the manipulator $\to$ Center straight-line distance ： $d_g$
2. for Conveyor belt ：
   The target object is in the conveyor coordinate system $\to$${}_o^{c}T={[}_o^{c}x{,}_o^{c}y{]}^T$
   The coordinate system of the conveyor belt is relative to the base coordinate system of the manipulator Transformation matrix $\to$
   $${}_c^{B}T={[}_c^{B}x{,}_c^{B}y{,}_c^{B}z{,}_c^B\gamma {,}_c^B\varphi {,}_c^B\psi {]}^T$$
   The coordinate system of the conveyor belt is relative to the base coordinate system of the manipulator Postures $\to$
   $${}_o^{B}T{=}_c^{B}T{\times }_o^{c}T$$
   adopt Hardware device Get and transform the coordinate system to move the object in the base coordinate system Real time coordinates

(1) Online forecasting

1. Moving objects in $t_0$ Trigger photoelectric signal at any time
2. The system gains speed $v$
3. With $t_0$ Where is the target at all times $G_0$ Points for Datum point , Estimate the target $t_1\sim t_N$ All the time Conveyor belt coordinate system Position in
4. Get the pose in the robot coordinate system ${}_o^B{V}_1{\cdots }_o^B{V}_N$
   

(2) Sports planning

Fast extended tree algorithm

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.

Quintic spline interpolation algorithm Trajectory interpolation

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 Between the system and the robot use simple_message agreement , Coexist In the communication feedback mechanism ;
After the robot executes the instructions of the upper computer
$\to$ Feedback the joint information of the manipulator back to the upper computer system , To realize the robot in the control system
Attitude synchronization
$*$ The system and target attitude cannot be tracked in real time
$*$ The tracking process is intermittent 、 The tracking track is discontinuous

1. Generate transitional trajectories ： According to the pose of the target object ${}_o^B{V}_0$, Generate Manipulator end grab from Preparation position Moving to $G_0$ Transitional trajectory of position ;
2. Regeneration trajectory ： According to the pose of the target object ${}_o^B{V}_2$, Generate a robot arm The second planning path , That is, the end gripper starts from $G_1$ Position to $G_2$ Tracking track of position ; The two tracking tracks are Successive , It means that the end grab is from $G_0$ Position operation path $G_1$ Position to $G_2$ Location ;
3. Plan the remaining trajectory of the manipulator in turn , Finally, a complete and coherent tracking trajectory of the manipulator is obtained ;

(3) Dynamic tracking and grabbing

1. The system obtains the end gripper in real time TCP Coordinate values of points and moving objects in the base coordinate system .
2. When the coordinates of the manipulator and the object meet $V_{trans}<\delta ,V_{rot}=0$ when , The system controls the gripper to complete the grasping process .
3. The maximum distance between the end grab and the object $\to$$\delta$
   Translation vector between the end grab and the target object $\to$$$V_{trans}={[}_g^{B}x{－}_o^{B}x{,}_g^{B}y{－}_o^B{y}_g^{B}z{－}_o^{B}z{]}^T$$
   Rotation vector between the end gripper and the target object $\to$
   $$V_{rot}={[}_g^B\varphi {－}_o^B{\varphi }_g^B\gamma {－}_o^B{\gamma }_g^B\psi {－}_o^B\psi {]}^T$$

2 Dynamic tracking algorithm design

Target object Estimate the time length of the trajectory to plan Robot tracking The length of time of the track

Estimated trajectory $t_r(x)$$\to$ The time required for the gripper at the end of the manipulator to reach a certain point on the conveyor belt
Estimated trajectory $t_o(x)$$\to$ The target object arrives $x$( Somewhere on the conveyor belt ) The time required
$$t_o(x)=c_1\times x+c_2,c1,c2\in \mathbf{R}$$

Two curves can intersect $\to$ Can grab On the contrary, we can't

The running track of the manipulator end grab is divided into three sections

1. A The segment is the acceleration track ;
2. B The section is a constant speed track ;
3. C The section is the deceleration track .

Algorithm design must ensure The gripper at the end of the manipulator is running to B Segment trajectory And The target object realizes dynamic tracking, meets and grabs

The robot arm is finished The first track Before arriving $G_0$ Location , When the end grab arrives $G_0$ When in position , The target object has changed from $G_0$ Location ( Coordinate system $x$ The starting point of the axis ) Run to the rear position .
$\to$ be relative to $t_o(x)$ for , Track track $t_r(x)$ Behind the $t_o(x)$ A span $\Delta t$
$\to$ Introducing time difference $\Delta t$ after , When the intersection of two tracks is still B Duanzhong , The end gripper can realize stable dynamic tracking and grasping .

3 Experimental verification

3. 1 Dynamic tracking and grabbing experiment

(1) System framework

(2) Dynamic tracking and grabbing experiment

The speed of the conveyor belt is measured by the encoder $v=6.04cm/s$
Target object and end grab TCP The coordinates of the point in the robot base coordinate system are shown in the table

3. 2 Fusion trajectory continuity

The continuity of the fusion trajectory is reflected in the process of the robot end gripper tracking the target object Whether there is track pause , Whether the track speed is continuous .
from ROS The joint trajectory information sent by the system to the control cabinet is composed of Track multiple path points The six axis control parameter value of the manipulator , The parameters of six axes of each path point contain control Time value of each axis rotation 、 Rotation angle 、 Rotational speed as well as Rotational acceleration Etc .

3. 3 Dynamic tracking robustness

Experiments show that within a certain speed range of conveyor belt , The robot arm can track and grasp the target objects with different moving speeds stably and dynamically , The experimental results have achieved good results , It is verified that the time measurement of the predicted trajectory is used to plan the trajectory model of the manipulator , That is, the robustness of dynamic tracking .

4 Conclusion