Dynamic grasping of manipulator based on realtime smooth trajectory generation

Abstract

For safe and effective grasping in a dynamic environment, planning algorithms need real-time to deal with changing the target’s movement and obstacles. This paper proposes a new sequential Sense-Plan-Act (SeqSPA) dynamic grasping framework to generate a robot’s real-time and smooth grasping trajectory. Specifically, we cluster all stable grasps of the target, transform the clustering centers into pregrasps, and predict the future motion of the moving objects by using the observed values. The trajectory optimization algorithm constructing the approximative joint space gradient field can generate a smooth trajectory for a 6-DOF industrial robot arm within 2 ms. Our method generates trajectories for multiple pregrasps and selects the time-optimal trajectory for execution. Simulation comparison and actual experiments verify that our framework can immediately respond to environmental changes and efficiently find a grasping trajectory of the near-optimal time. The trajectory optimization algorithm in the framework can also be used alone to generate a real-time grasping trajectory when the prediction module cannot accurately predict the target motion.

Introduction

Many automated tasks require robots to operate objects. Usually, robots operate targets in a static environment with fixed trajectories. However, the robot must generate trajectory autonomously when the environment changes dynamically. For example, in dynamic assembly, grasping and human-machine collaboration, targets and obstacles interacting with the manipulator may move with unknown motion.

Allowing robots to manipulate objects in a dynamic environment can improve the system’s overall efficiency but present more significant challenges. First, motion prediction of dynamic objects becomes extremely important, requiring higher real-time and robustness for visual tracking and prediction. Secondly, the existing grasping planning algorithms filter and sort the grasp poses and only use one grasp to plan a trajectory. In a dynamic environment, especially in the case of moving obstacles, the optimal grasp pose cannot be determined only by a grasping planning algorithm. Due to the change in the external environment, the non-optimal grasp pose may be more conducive to dynamic grasping when considering the whole capturing trajectory. Finally, continuous changes require online and fast motion replanning. With the change in the motion state of the target, we must update the trajectory of the manipulator and grasping pose in real time. Sampling-based algorithms generate random, unstable and fluctuating paths in each replanning, which does not meet these requirements. Optimization-based algorithms consume more time in cluttered scenes and are unsuitable for replanning in a dynamic environment.

Aiming at the problem of existing algorithms, we propose a new dynamic grasping framework in this paper. First, Bézier trajectories are used to predict target and obstacle trajectories. Secondly, ideally, the framework plans trajectories for all grasp poses and selects the optimal trajectory for execution. However, the calculation is heavy, and the trajectories of the similar grasp poses are also close. Therefore, it is necessary to cluster the grasp poses, take each clustering center as the target for trajectory planning, and select the optimal trajectory for execution. Finally, this paper simplified our earlier motion planning algorithm. ¹ The trajectory planning module calculates the intersection time, use B-spline curves to represent joint trajectories, and generates smooth, collision-free and dynamically feasible trajectories after nonlinear optimization. The trajectory planner has good real-time performance and can be used in the whole grasping framework or alone. The grasping process for a moving object is shown in Figure 1.

Figure 1.

A mechanical arm is grasping a moving object. We online predict the object motion, plan trajectories for multiple pregrasps, and execute the shortest time of the trajectory. The gripper reaches the pregrasp position and then grasps and lifts the object.

The main contributions of this paper are summarized as follows:

1. This paper makes the motion planner ¹ suitable for multi-pregrasps real-time planning. After predicting the intersection time, we construct a joint space gradient field and use nonlinear optimization to calculate the joint trajectory quickly.

2. This paper proposes a new dynamic grasping framework, which includes a polynomial trajectory prediction module, a grasping planning module, and a trajectory planning module. The framework clusters many grasps into a small number of pregrasps, generates the trajectories of all pregrasps, and selects the optimal time trajectory for execution.

3. We systematically evaluate the performance of the dynamic grasping framework through simulation and actual grasping experiments, which are multi-target and multi-task, and demonstrate the feasibility and robustness of the framework.

Related work

This section summarizes relevant work in four parts: Grasping in Dynamic Environments, Robotic Grasping, Object Motion Prediction and Motion Planning.

Grasping in dynamic environments

The two mainly alternative system architectures for grasping in dynamic environments are Locally Reactive Control and Sense-Plan-Act.^2,3

1. Locally Reactive Control: Such system architectures do not use global motion planning modules but local strategies to generate real-time reactive motions. ² They respond immediately to change and are robust to uncertainties in sensors and object motion. Wholly based on feedback control systems^4–6 are used in grasping in dynamic environments for a long time. Recently, many studies are based on deep reinforcement learning.^7–9 The robotic control policy neural network that uses raw input from a camera is established in Levine et al. ⁷ A mobile operation control framework ⁸ based on multi-task reinforcement learning is proposed to achieve universal dynamic target tracking and grasping. In Pham et al., ⁹ the neural network predicts the action that meets the constraint closest to the unsafe action so that the arm can grasp the moving object while avoiding the obstacle. However, local reactive control may fall into a local minimum in a complex workspace.

2. Sense-Plan-Act: In the high degree of the freedom robot system, constructing sensing, planning, and action components through strong modularity is still the primary mode. In this system, the perception module builds a complete environment model, the planning module generates a collision-free and optimal trajectory, and finally, it is executed by the precise controller. Systems built around Sense-Plan-Act (SPA) are well suited to structure and well-defined environments. ² The planner based on graph search ¹⁰ is used to generate the trajectory. After the manipulator reaches the pregrasp, the controller is used to grasp in Cartesian space. A search-based kinodynamic motion planning algorithm is presented to generate time-parameterized trajectories to grasp moving objects smoothly in Menon et al. ¹¹ The RRT-Connect method in time configuration space is used in Yang et al. ¹² to generate a path to pick up the moving target. Still, it cannot cope with the uncertainty and changing environment. Due to the SPA’s lack of environmental feedback during movement execution, researchers have proposed extensions such as Sense-Plan-Act (SeqSPA). Constant-Time Motion Planning (CTMP) ¹³ uses real-time heuristic search planning to grasp moving objects in the finite horizon. The Reachability-Aware Grasping and Motion-Aware Grasping are used in Akinola et al. ¹⁴ to filter reachable grasps and incorporated the path from the previous timestep to seed the search process to achieve a quicker and smoother trajectory. Compared with SPA, SeqSPA systems still have better-grasping results when there is uncertainty in sensor signal and motion state.

However, the methods in Islam et al. ¹³ and Akinola et al. ¹⁴ cannot quickly cope with the dynamic obstacles. The core reason is that a motion planner requires a longer computing time than a local motion controller, which makes it impossible to react quickly in a dynamic environment. This paper develops a new SeqSPA dynamic grasping framework. The global planner in this paper is fast and can be used as a local motion controller. Moreover, the prediction module increases the reliable length of each trajectory.

Robotic grasping

Methods based on deep learning can directly extract the grasp pose of the target from an RGB-D view^15,16 or Point Clouds. ¹⁷ These methods collect expensive grasp data sets in the real world to train a neural network. The physics-based simulation method is used in Eppner et al. ¹⁸ to evaluate the grasping quality of the parallel jaw and establish a dense data set. The method in Akinola et al. ¹⁴ generates robust grasping data sets through physical simulation, and uses pre-calculated reachability space ¹⁹ and motion intention neural network to evaluate feasible grasp poses. In this paper, a stable grasping dataset is generated by physical simulation, the same as Eppner et al, ¹⁸ and the clustering algorithm reduces the similar grasp poses.

Object motion prediction

The use time of the optimized trajectory can be improved by estimating the future motion of the target through the prediction module. The target pose detection system uses Kalman and Buey filtering methods^20–22 to filter the noisy pose results into more stable values. However, it is unreliable to use rough motion models to estimate the motion of future targets. The Covariant Optimization is utilized in Jeon et al. ²³ to consider the target motion prediction of collision, and the collision term needs much time to calculate. The polynomial regression to predict object motion and generates a corresponding trajectory through the constraint-free QP optimizer is adopted in Chen et al. ²⁴ Fast-Tracker^25,26 improves this method by using Bezier curves to represent the target prediction trajectory, and adding dynamic constraints and time-varying confidence in the optimization. We use this method to predict the movement of all objects.

Motion planning

The process of motion planning usually consists of path search and trajectory optimization. Sampling-based methods^12,27,28 and search-based methods^{10,11,13,14,29} are the main ways of front-end path search. The front-end path is jerky and leads to wavy motion. The back-end trajectory optimization algorithm calculates a smooth trajectory with time parameters based on the front-end path. The back-end methods of the manipulator^30,31 can optimize a satisfied constrained trajectory, but the optimization process does not consider the obstacles. CHOMP algorithm ³² introduces the Euclidean Signed Distance Field (ESDF) information into motion planning to optimize trajectory using workspace gradient information. STOMP algorithm ³³ uses sampling technology to generate multiple alternative trajectories and obtains the optimal trajectory through multiple iterations and evaluation of the cost of candidate trajectories. Machine learning techniques^34,35 are used to optimize trajectories. The clearance estimation is used for motion planning in Chase Kew et al., ³⁴ but the algorithm cannot quickly and adaptively handle dynamic obstacles. The method in Fujii and Pham ³⁵ can quickly smooth the trajectory while processing dynamic obstacles but must obtain an initial collision-free path in advance. EGO algorithm ³⁶ establishes a gradient field for a UAV without ESDF and uses the properties of the B-spline curve for rapid trajectory optimization.

Problem definition and algorithm framework

The task simulates a scenario in which multiple objects are placed on the conveyor belt in a warehouse. ¹⁴ The robot arm must avoid colliding with non-target objects on the conveyor belt or static obstacles around it and pick up the target object. In grasping, there will be an external human being or robot as a moving obstacle or the conveyor belt speed mutation. We assume that we know the models of the target objects and obstacles, but their motions are unknown. The robot must catch and lift the target without colliding with surrounding obstacles.

We propose the manipulator dynamic grasping system, as shown in Figure 2. The SeqSPA Dynamic Grasping Frame is presented in Alg.1. Our method first extracts the pre-computed grasp data $G_{\mathrm{DB}}$ of the target object O. By the K-Means algorithm, the close grasps in $G_{\mathrm{DB}}$ are combined, and the centers of each class are taken to form $G_{\mathrm{K}}$ . The grasp poses $G_{\mathrm{K}}$ retreat a distance b along the grasp direction to generate pregrasp poses $G{\prime }_{\mathrm{K}}$ . We assume the obstacle models are known and predict all obstacles’ future trajectories. If the obstacle is static, the predicted trajectory is a point. In the dynamic grasping loop, through visual sensors, the MotionPrediction Module records previous and evaluates current poses of the target and obstacles, and predicts the future target motion trajectory $g_{\mathrm{traj}}$ and obstacle trajectories $o_{\mathrm{traj}}$ . According to $g_{\mathrm{traj}}$ and $G{\prime }_{\mathrm{K}}$ , the pose of the manipulator $G_J$ when it arrives at the pregrasp is predicted through the IntersectionPoints module. We also obtain the initial trajectories $\mathrm{Ini}{t}_{\mathrm{traj}}$ and the intersection time T. The calculation process of the IntersectionPoints module is related to the position and speed of the end-effector and the target.

Figure 2.

Dynamic grasping system architecture diagram. The predicted trajectories of the target and obstacles generated by the motion prediction module and the pregrasp poses generated by the grasp planning are input into the trajectory planning module to generate the optimal trajectory. When using the motion prediction model, if there is a significant error between the actual position of the target and the predicted position, or the obstacle’s latest predicted trajectory will collide with the manipulator, the framework carries out the planner again. When not using the prediction module, the planner uses the current target and obstacles positions to generate the trajectory at a specific frequency.

Convert all initial trajectories to the B-spline control points $Q_{\mathrm{all}}$ . All initial paths are optimized through the gradient-based TrajectoryOptimization module to obtain multiple collision-free and smooth trajectories $Q{\prime }_{\mathrm{all}}$ . The optimal trajectory $\mathrm{obtimiz}{e}_{\mathrm{traj}}$ is selected from multiple alternative trajectories for execution, and the prediction module predicts the trajectory of the objects in real-time. When the difference between the observed position of the target and the predicted position is greater than one threshold value, or the obstacle may collide with the optimal trajectory at some point in the future, the trajectory optimization is carried out again. After the manipulator reaches the pregrasp $g_j$ , we get the pose of the target object after $t=1\mathrm{s}$ , use the newly target predicted pose $p_f$ to generate grasp $g_c$ . The arm is then moved to $g_c$ . After moving the end-effector to $g_c$ , the gripper moves with the target for $t\prime =1\mathrm{s}$ , closes the gripper, and then observes whether the target is lifted.

Grasp planning

We used an approach similar to Akinola et al. ¹⁴ and Eppner et al., ¹⁸ where a grasping database is precomputed while all target objects remain static, to collect data in the physical simulation. First, many dense samples are taken to generate the grasp poses on the object’s surface. Then the object is lifted 20 times in the physical simulation environment, with random noise added each time. The success rate is the selection measure of the reliability of each grasp pose. The first 600 grasps are used as the grasping dataset.

We use the widely used 4-dimensional grasp representation in the physical simulation to represent the object grasp configuration. The four dimensions are: grip width w, opening size l, grasping position g and orientation u. We use a model the same width w as the actual gripper in the simulation. We sample on the target object’s surface, and the intersection point with the other side of the object is found through the normal line corresponding to the sampling point. The two points’ connection length is l, and the center point coordinate value of the segment is g. The gripper takes the line between two points as the rotation axis and samples in [−pi/3, pi/3] around the axis as u.

Our evaluation metric is based on distances between grasps and does not consider w and l. A grip pose $T(g,u)\in \mathrm{SE}(3)$ consists of the grasp position $g\in {\mathrm{R}}^3$ and the orientation represented as unit quaternion $u\in S^3$ . We use a weighted metric, ¹⁸ the distance between T and $T\prime$ is defined as:

$$\rho (T,T^{\prime })=\omega {\Vert g-g^{\prime }\Vert }_2+\arccos (\left|〈u,u^{\prime }〉\right|)$$

(1)

where $\omega \in \mathrm{R}$ is a relative metric ratio that relates to rotation and translation. We define that a pure translation of 1 mm equals a pure rotation of 1°.

We use the K-Means to aggregate the robust grasps into k classes, where k = 10. In K-Means, the initial cluster center is randomly generated, and the distance between each sample and each cluster center is calculated by $\rho$ . The samples are assigned to the nearest cluster center to form the sample groups. After obtaining the new groups, the average value of all g and u in one group is calculated respectively to generate the new center. The sample closest to the new center is taken as the cluster center. Recalculated the distance between the samples and the cluster centers, the cluster centers are generated again. The algorithm is calculated several times until convergence, and the final clustering centers as the candidate grasps are obtained. All the candidate grasps are converted to the pregrasp set by each grasp pose moving d distance along the opposite direction of the unit quaternion u.

The transform relationship between the grasp and the pregrasp is shown in Figure 3. The center of the gripper at the grasp pose with closed gripper is g, and the center of the gripper after opening is $g\prime$ , as shown in Figure 3(a) and (b), respectively. $\mathrm{\Delta }b$ is the distance from g to the object boundary along the opposite direction of u, which is a parameter to generate robust grasp. The distance from g to $g\prime$ is $\mathrm{\Delta }b\prime$ , which can be calculated from the geometry of the gripper. The center of the gripper in the pregrasp is $g″$ , and the distance from $g″$ to the object along the direction of u is a fixed value $b_0=0.06\mathrm{m}$ . $b=b_0+\mathrm{\Delta }b-\mathrm{\Delta }b\prime$ is the distance from $g″$ to $g\prime$ , that is, the movement distance of the gripper center relative to the target within t. The grasp is closed using $t\prime$ , and the center point of the gripper is from $g\prime$ to g.

Figure 3.

The transformation relation between grasp and pregrasp. (a) is the position after the gripper is closed, which is one grasp in the grasps database including grasping orientation u, gripper center g, and the distance $\mathrm{\Delta }b$ from g to the edge of the object along the opposite direction of u; (b) is the grasp pose before the claw is closed; (c) is the pregrasp pose of the gripper: (a) grasp with closed gripper, (b) grasp with open gripper, and (c) pregrasp pose.

Since there are moving obstacles around the target, we conduct collision detection on the gripper, part of the arm and the obstacle, in which the gripper is in the pregrasp, as shown in Figure 4. We use a metric to rank the collision-free pregrasps. Our metric can be defined as:

$$f(T_e,T_w)=\rho (T_e,T_w)+\mathrm{W}$$

(2)

Figure 4.

The mechanical arm components for collision detection in the grasp planning.

where $T_e$ is the gripper pose of the mechanical arm at initial time, $T_w$ is the pregrasp which is converted from the goal coordinate to the world coordinate. W = 0, if the inverse kinematics (IK) can be solved; W = 10,000, if IK cannot be solved.

Using f to arrange the candidate data from small to large, the first data is the best pregrasp at this moment.

Motion prediction

The planner needs time to calculate trajectories, quickly becoming obsolete as the object moves. Therefore, predicting the future pose of the target can improve the task’s overall success rate. We adopt the Bézier curve $B(t)$ to indicate the object predicted trajectory. ²⁶

The method in Pan et al. ²⁶ maintains a queue $\mathrm{Q}=[q_1,q_2,\cdots ,q_L]$ , where $q_i=\{p_{t_i},t_i\}$ . $p_{t_i}$ is the instantaneous pose of the moving object, and $t_i$ is the corresponding timestamp. Queue length is L and $t_L$ is the current time. The prediction module runs in real-time. We add the new target observation data to the queue and generate the new target prediction trajectory by fitting the queue data. Our predicted target movement period is $(t_L,t_p]$ . $t_p$ is the end time of the Bézier curve.

The cost function minimizes the residual distance between the observation data and the prediction trajectory ²⁶ :

$$J_{\mathrm{pre}}=\sum _{i=1}^L{w}_{t_i}\Vert B(t_i)-{\mathrm{p}}_{t_i}{\Vert }_2^2+w_{p}L{\int }_{t_1}^{t_p}{\Vert B^{(2)}(t)\Vert }_2^2\mathrm{dt}$$

(3)

Where, $w_{t_i}$ is the weight term to distinguish the confidence of the observed values at different times. $w_p$ represents the weight of the regularizer to avoid over-fitting.

The Object-oriented software for Quadratic Programming ³⁷ is used to solve the optimization problem of the Bézier curve that meets the velocity and acceleration constraints in Han et al. ²⁵ and Pan et al. ²⁶

Our complete motion predicted trajectory is shown as follows:

$$f\left(t\right)=\{\begin{array}{c}\multicolumn{1}{c}{B\left(t\right),t_L\le t\le t_p}\\ \multicolumn{1}{c}{B\left(t_p\right)+B\prime \left(t_p\right)•(t-t_p),t>t_p}\end{array}$$

(4)

We set $t_p-t_L=1.5\mathrm{s}$ . When the time exceeds the period of the Bézier curve, assuming that the object is moving uniformly, the object’s position is calculated using the velocity corresponding to $t_p$ .

Motion generation

When grasping a dynamic object, the movement of the end-effector can be divided into four stages: pregrasping, grasping, tracking, and lifting. The manipulator reaches the pregrasp pose, approaches the grasp pose, closes the gripper while tracking the target, and lifts it. Our previous algorithm framework ¹ is the SPA system. The trajectory generation algorithm to reach the pregrasp position contains sampling-based path search and nonlinear trajectory optimization. To further improve the computational efficiency of trajectory generation in our framework, this paper removed the time-consuming front-end sampling algorithm and trajectory feasibility adjustment module. When a trajectory falls to a minimum or the velocity and acceleration of the trajectory are not feasible, we remove it directly. This paper carries out real-time trajectory planning with multiple grasping poses. Removing several infeasible trajectories does not affect the grasping task.

We first estimate the intersection position using the rapid generation of motion primitive algorithm in Mueller et al. ³⁸ according to the position and speed of the end-effector and the target. Then the planner generates multiple trajectories using the gradient-based method and selects the optimal trajectory for execution. We use the Cartesian controller to move the mechanical arm during the grasping, tracking and lifting stages.

Estimate intersection point

Assuming no obstacles, we need to calculate an optimal intersection trajectory to grasp the moving target. The total initial trajectory cost function $J_{\Sigma }$ of H + 1 dimensional joint space is defined as ¹ :

$$J_{\Sigma }=\sum _{k=0}^{\mathrm{H}}{J}_k+\rho T,\mathrm{where}{J}_k=\frac{1}{T}\underset{0}{\overset{T}{\int }}{j}_k{\left(t\right)}^2\mathrm{dt}.$$

(5)

Where, $J_k$ is the cost function of the $k^{\mathrm{th}}$ joint, $\rho$ is the time factor, T is the trajectory time, and $j_k\left(t\right)$ is the third derivative of the $k^{\mathrm{th}}$ joint curve.

By using Pontryagin’s minimum principle, fixed trajectory end angle and angular velocity, the optimal state trajectory can be straightforwardly solved ³⁸ :

$$\begin{array}{c}{s}^{*}\left(t\right)=\left[\begin{array}{c}{\theta }^{*}\left(t\right)\hfill \\ \multicolumn{1}{c}{{\omega }^{*}\left(t\right)}\\ \multicolumn{1}{c}{0}\end{array}\right]\hfill \\ =\left[\begin{array}{c}\frac{\alpha }{120}{t}^5+\frac{\beta }{24}{t}^4+\frac{\gamma }{6}{t}^3+\frac{a_0}{2}{t}^2+{\omega }_{0}t+{\theta }_0\hfill \\ \multicolumn{1}{c}{\frac{\alpha }{24}{t}^4+\frac{\beta }{6}{t}^3+\frac{\gamma }{2}{t}^2+a_{0}t+{\omega }_0}\\ \multicolumn{1}{c}{\alpha t^2+2\beta t+2\gamma }\end{array}\right].\hfill \end{array}$$

(6)

Assuming the initial angle, angular velocity and angular acceleration of the $k^{\mathrm{th}}$ joint trajectory is $s\left(0\right)=({\theta }_0,{\omega }_0,a_0)$ , and motion parameters at intersection time is $s\left(T\right)=({\theta }_f,{\omega }_f)$ . Then, the unknowns $\alpha$ , $\beta$ , and $\gamma$ can be solved.

$$\left[\begin{array}{c}\alpha \hfill \\ \multicolumn{1}{c}{\beta }\\ \multicolumn{1}{c}{\gamma }\end{array}\right]=\frac{1}{T^5}\left[\begin{array}{cc}\begin{array}{c}320\hfill \\ \multicolumn{1}{c}{-200T}\\ \multicolumn{1}{c}{40T^2}\end{array}\hfill & \begin{array}{c}-120T\hfill \\ \multicolumn{1}{c}{72T^2}\\ \multicolumn{1}{c}{-12T^3}\end{array}\hfill \end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }\theta \hfill \\ \multicolumn{1}{c}{\mathrm{\Delta }\omega }\end{array}\right]$$

(7)

Where

$$\left[\begin{array}{c}\mathrm{\Delta }\theta \hfill \\ \multicolumn{1}{c}{\mathrm{\Delta }\omega }\end{array}\right]=\left[\begin{array}{c}{\theta }_f-{\theta }_0-{\omega }_{0}T-\frac{1}{2}{a}_0{T}^2\hfill \\ \multicolumn{1}{c}{{\omega }_f-{\omega }_0-a_{0}T}\end{array}\right].$$

(8)

Each joint cost value ³⁸ can be calculated as follows:

$$J_k={\gamma }^2+\beta \gamma T+\frac{1}{3}{\beta }^2{T}^2+\frac{1}{3}\alpha \gamma T^2+\frac{1}{4}\alpha \beta T^3+\frac{1}{20}{\alpha }^2{T}^4.$$

(9)

The only variable in the expression of $\alpha$ , $\beta$ , $\gamma$ is T. We will substitute $\alpha$ , $\beta$ , $\gamma$ into $J_{\Sigma }^{*}\left(T\right)$ . To find the optimal time T, we set $\partial J_{\Sigma }^{\ast }\left(T\right)/\partial T=0$ to solve the flight time T, and then solve $\alpha$ , $\beta$ , $\gamma$ . We substitute $\alpha$ , $\beta$ , $\gamma$ into $s^{*}\left(t\right)$ to obtain the optimal angle trajectory ${\theta }^{*}\left(t\right)$ and angular velocity trajectory ${\omega }^{*}\left(t\right)$ .

We sample the path points along the target’s predicted trajectory starting from the starting point. The target center sampling point $[p,v]$ at time $T_{\mathrm{move}}$ , which is the time from the starting point to the sampling point, and the pregrasp pose $[x_c,y_c,z_c,{\theta }_{\alpha },{\theta }_{\beta },{\theta }_{\gamma }]$ that change with the object’s center coordinate are transformed into robot arm joint space as $\{{\theta }_{f_0},\cdots ,{\theta }_{f_H},{\omega }_{f_0},\cdots ,{\omega }_{f_H}\}$ . We calculate the total cost function $J_{\Sigma }$ to obtain the trajectory running time $T_{\mathrm{angle}}$ . The time constraint $T_{\mathrm{angle}}<T_{\mathrm{move}}$ and angular velocity constraint $\underset{t\subset [0T_{\mathrm{angle}}]}{max}\left|{\omega }^{*}\left(t\right)\right|<{\omega }_{max}$ of each joint trajectory must be satisfied, where ${\omega }_{max}$ is the maximum angular velocity. $p^{*}$ is the first intersection point to satisfy the constraints and the related ${\theta }^{*}\left(t\right)$ is the initial path of the trajectory optimization in joint space. The intersection time is $T^{*}=T_{\mathrm{move}}$ .

Gradient-based trajectory optimization

Due to ignoring obstacles’ information in the manipulator workspace when calculating the initial joint trajectory, the initial trajectory is close to or through obstacles. Therefore, we transform ESDF information into the joint space and use nonlinear optimization to generate smooth, safe and kinematically feasible trajectories in a short time.

This paper uses the method in Wei et al. ¹ to establish the approximate repulsive gradient field generated by obstacles in high-dimensional space. Thus, we establish the distance and gradient expressions between the path point and a single obstacle in joint space for the collision penalty.

Our planner uses robotic control points to optimize the trajectory and the oriented bounding boxes (OBB) for collision detection. The distribution is shown in Figure 5. As shown in Figure 6, $r_i$ is radius of the control point $c_i$ . The distance between control point $c_i$ and obstacle $O_j$ is $D_{\mathrm{ij}}$ . $s_f$ is the obstacle safety threshold constant. The repulsive function is:

$$F_{\mathrm{ij}}=\frac{2b}{e^{k(a+{d\prime }_{\mathrm{ij}})}+1}$$

(10)

Figure 5.

The distribution of control points (green points) and oriented bounding boxes (transparent cubes).

Figure 6.

The repulsive force generated by the obstacle on the control point.

where $F_{\mathrm{ij}}$ is repulsive force, $d{\prime }_{\mathrm{ij}}=D_{\mathrm{ij}}-r_i$ , and the parameters $a,b,k$ determine the smooth function shape. Assuming the maximum repulsion constant is $F_{max}$ , $d{\prime }_{\mathrm{ij}}=s_f$ and $F_{\mathrm{ij}}=0.01$ , so we can figure out $a,b,k$ , where $b=F_{max}/F_{max}22$ .

We calculate the repulsion force of each control point, convert the repulsion force to the origin of the corresponding connecting rod coordinate, and combine the force and torque values. Repulsive force and torque at the origin of the coordinate affect the joint coordinates from this link to the base. The force and torque at the origin of each link coordinate system are recursively deduced to the base coordinate system by using the recursive formula of force and torque in the series mechanism of robotics. In this manipulator configuration, the torque values $\{{\tau }_0,{\tau }_1,\cdots ,{\tau }_{\mathrm{H}}\}$ of the rotation axis of each joint are the repulsion torques generated by the repulsion field of the obstacle.

Repulsive torques will move the manipulator away from the obstacle. $\{{\tau }_0,{\tau }_1,\cdots ,{\tau }_{\mathrm{H}}\}$ is converted to unit vector $\left[{{\tau }^{\prime }}_0,{{\tau }^{\prime }}_1,\cdots ,{{\tau }^{\prime }}_{\text{H}}\right]$ , and the unit vector is approximated as the direction of joint angle change. The configuration $Q=\{{\theta }_0,{\theta }_1,\cdots ,{\theta }_{\mathrm{H}}\}$ moves one step in the unit vector to obtain the new robotic configuration $Q^{\prime }=\{{{\theta }^{\prime }}_0,{{\theta }^{\prime }}_1,\cdots ,{{\theta }^{\prime }}_{\text{H}}\}$ , and the formula ¹ is: $\theta {\prime }_i={\theta }_i+\tau {\prime }_i·\mathrm{step},i\in \{0,1,\cdots ,\mathrm{H}\}$ .

Approximate repulsive gradient field was represented by $\{p,v\}$ pairs and the establishing process is shown in Figure 7. Each waypoint of trajectory is calculated to obtain a $\{p_{\mathrm{ij}},v_{\mathrm{ij}}\}$ pair. The initial configuration is $Q_i$ . $p_{\mathrm{ij}}$ represents the critical point where the repulsion torque generated by obstacle $O_j$ to configuration $Q_i$ is zero. $v_{\mathrm{ij}}$ represents the corresponding gradient direction, which is the unit vector from $Q_i$ to $p_{\mathrm{ij}}$ . The distance between $Q_i$ and $O_j$ is $d_{\mathrm{ij}}=(Q_i-p_{\mathrm{ij}})·v_{\mathrm{ij}}$ .

Figure 7.

The establishment process of $\{p,v\}$ pairs. ¹

We use nonlinear optimization method to plan uniform B-spline curve control points.^39,40 The derivative of a B-spline curve is also a B-spline curve, whose convex hull characteristics can ensure dynamic feasibility and safety. The cost function is as follow:

$$\underset{Q}{min}J={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_d{J}_d+{\lambda }_e{J}_e$$

(11)

where $J_s$ , $J_c$ , $J_d$ are smoothness penalty, collision penalty and feasibility penalty, which are the same cost function of. ¹ $J_e$ is endpoint penalty. ${\lambda }_s,{\lambda }_c,{\lambda }_d,{\lambda }_e$ are the coefficient of each penalty term.

In order to successfully grasp the moving object, the gripper needs to reach the pregrasp accurately and has the same speed as the target. When no end-point penalty is imposed, optimizing the penalty function of the first several terms will change the control points for determining the terminal position and velocity. So, we add the end trajectory point constraint to reduce the deviation of the manipulator reaching the pregrasp pose after optimization. The function is defined as follows:

$$J_e=k_{\theta }({\theta }^{*}(T^{*})-{\theta }_e{)}^2+k_{\omega }({\omega }^{*}(T^{*})-{\omega }_e{)}^2$$

(12)

where ${\theta }^{*}(T^{*})$ and $w^{*}(T^{*})$ are the values of initial angle, angular velocity trajectory at intersection time $T^{*}$ , respectively. $k_{\theta }$ and $k_{\omega }$ are the weighting parameters. ${\theta }_e$ and ${\omega }_e$ are the endpoint angle, angular velocity in the optimization process, which are expressed as some control points and defined as follows:

$$\begin{array}{c}{\theta }_e=m_{0,0}{Q}_{\mathrm{N}-{\mathrm{p}}_b-1}+m_{0,1}{Q}_{\mathrm{N}-{\mathrm{p}}_b-2}+...+m_{0,k-2}{Q}_{\mathrm{N}},\hfill \\ {\omega }_e=m_{1,0}{Q}_{\mathrm{N}-{\mathrm{p}}_b-1}+m_{1,1}{Q}_{\mathrm{N}-{\mathrm{p}}_b-2}+...+m_{1,k-2}{Q}_{\mathrm{N}}.\hfill \end{array}$$

where

$$k=p_b+1,$$

(13)

$$m_{i,j}=\frac{1}{(k-1)!}{C}_{k-1}^{k-1-i}\sum _{s=j}^{k-1}{(-1)}^{s-j}{C}_k^{s-j}{(k-s-1)}^{k-1-i},$$

$$C_n^i=\frac{n!}{i!(n-i)!}.$$

(14)

Implementation details and results

We experimented with the simulation environment and the actual robot to evaluate our method. In the simulation, we evaluate the performance of our algorithm in different tasks for different targets. The simulation environment includes the target’s random linear or nonlinear movement, whether there are static obstacles, obstacle with the same speed as the target, and external moving obstacles. Then, the algorithm’s feasibility is verified by typical working conditions in the real world with different targets.

L-BFGS ⁴¹ is a quasi-Newton Algorithm to solve the nonlinear optimization problem. The dynamic grasping framework runs on a standard desktop computer with a single-core CPU of 3.20 GHz, Intel Core i7-8700, through Ubuntu 18.04 and ROS melodic.

Experimental setup

According to the actual task and previous experience, ¹⁴ we design seven working conditions to evaluate our method on the UR5-Robotiq robots, and the specific settings are as follows:

1. Linear Motion: The target moves uniformly in a straight line on the conveyor belt with no obstacles around, as shown in Figure 8(a). We random set $0.3m\le r\le 0.7m$ , $v=0.1\mathrm{m}/\mathrm{s}$ and $0.6m\le l\le 1.0m$ .

2. Linear with Static Obstacles: In the Linear Motion scene, we divide the near region surrounding the robot into three regions, as shown in Figure 8(b). We place one obstacle in each area and randomly locate the obstacle’s position in each region.

3. Linear with Moving Obstacle on The Belt: In the Linear Motion scene, we place an obstacle around the target that moves at the same speed as the target, as shown in Figure 8(c).

4. Linear with External Moving Obstacle: As the arm grasps the target in the Linear Motion scene, the external moving obstacle sweeps over the conveyor belt, as shown in Figure 8(d). The moving obstacle speed is about 0.05–0.15 m/s and changes dramatically some times. The height of the obstacle track to the conveyor belt surface is 0.2–0.8 m.

5. Linear with All Obstacles: In the Linear Motion scene, we add one static obstacle, one obstacle at the same speed as the target, and an external moving obstacle, as shown in Figure 8(e). The distribution and velocity of the three obstacles are the same as tasks (2)−(4).

6. Linear with Varying Speed: In the Linear Motion scene, the target object’s speed will change dramatically 3–5 times, and the linear velocity range is 0.05–0.1 m/s.

7. Circular Motion: The object moves uniformly around the manipulator’s base, as shown in Figure 8(f). The target trajectory radius is $0.3m\le r\le 0.7m$ , the object velocity is 0.05–0.1 m/s, and it moves in a single direction.

Figure 8.

Schematic diagram of random experimental motion parameters in seven working conditions. Schematic diagrams of working conditions 1–5 are (a)–(e), random parameters of working conditions 6 and 1 are the same, and (f) corresponds to working conditions 7. The distance between the target trajectory and the base coordinate is r. l is the projected length of the target starting point to the base along the conveyor belt direction. v is the target motion velocity. When the target moves in the x-axis’s positive direction, v is positive.

We selected five grasping targets and generated pregrasps, as shown in Figure 9. Each target’s initial stable grasp poses are about 600, and the clustering algorithm sets 10 classes. The grasps, after clustering, are transformed into pregrasps. All objects are used for the simulation and the actual robot experiment. Five kinds of targets are captured in each simulation experiment scene of each method. We run 40 times for each object in one scene and count the average success rate and capture time of 200 tests.

Figure 9.

The graspable objects and corresponding pregrasps. The five graspable objects from the YCB Object Database ⁴² are shown in (a). Many stable grasp poses for five targets are shown in (b). In (c), the grasp poses are clustered, and 10 grasps are generated for each target. The grasps transform into the pregrasps in (d): (a) five graspable objects, (b) stable grasps, (c) grasps after clustering, and (d) pregrasps.

We compare the performance of the below methods in the experiment:

1. OUR: This is our complete framework. We removed the trajectories with the speed and acceleration exceeding the limit in the obtained trajectories and selected the trajectory with the shortest time for execution.

2. One grasp: Unlike OUR, this method only uses one grasp with the lowest score for each plan and runs the calculated trajectory. When the first planning fails, we rerank the pregrasps and carry out the trajectory optimization again.

3. No prediction: This method removes the prediction module to study its importance. It does not predict the trajectory of the target and obstacles but directly plans the trajectory of all pregrasps at the moment and selects the trajectory with the shortest time for execution. The planner takes an average of 2 ms to generate trajectories from the starting position. When the end-effector is close to the target, the trajectory length will decrease, so each planning time of a single trajectory is less than 2 ms in the grasping process. Therefore, the maximum time for each plan should not exceed 20 ms. The planner is used as a controller, and the control rate is about 50 Hz.

4. No prediction + One grasp: This method removes the prediction module and uses one grasp with the lowest score. Only one trajectory is generated per planning. The planner is used as a controller, and we set the control rate to be 100 Hz.

Experimental results

The simulation results are shown in Table 1. The bold entries are the best results of the success rate and dynamic gripping time among the four methods Under working conditions 1 and 5, planning trajectories generated by different algorithms are shown in Figures 10 and 11, respectively.

Table 1.

The average success rate (%) and dynamic gripping time(s) of simulation experiments under each condition.

Method	Linear	Linear with static obstacles	Linear with moving obstacle on the belt	Linear with external obstacle	Linear with all obstacles	Linear with Varying Speed	Circular Motion
Our	100, 5.32	100, 5.28	100, 5.33	100, 6.76	95, 7.32	100, 8.11	–
One grasp	100, 5.95	97, 5.96	100, 5.87	96, 8.26	91, 7.40	98, 8.73	–
No prediction	92, 7.01	86, 7.69	90, 6.81	80, 8.12	77, 9.49	96, 7.18	86, 6.87
No prediction + One grasp	96, 5.97	86, 5.54	96, 6.24	86, 9.01	84, 9.31	100, 7.76	88, 8.09

Figure 10.

Trajectories generated by four algorithms in working condition 1. The yellow arrows near the object are the selected pregrasps. In task 1, the target is moving uniformly on the conveyor belt. Algorithms 1,2 contain the prediction module and plan the trajectory at one time. The green trajectory at the geometric center of the target object is the prediction trajectory, and the blue trajectory is the historical trajectory, as shown in (a)–(b). Algorithms 3, 4 do not include the prediction module and carry out real-time trajectory planning, as shown in (c)–(d). Algorithms 1 and 3 plan trajectories for all grasp poses and generate multiple feasible trajectories.

Figure 11.

Trajectories generated by four algorithms in working condition 5. The target and the obstacle in front are on the conveyor belt in uniform motion, one obstacle is above the target and moves in a straight line with variable speed along the conveyor belt, and one static obstacle is on the right side of the manipulator. Algorithms 1, 2 contain the prediction module to predict the trajectory of all moving objects and generate a long-term trajectory. However, when the velocity of the outside moving obstacle changes to lead original trajectory colliding with the obstacle in the future, the planner carries out again to generate a new trajectory, as shown in (a)–(b). Algorithms 3 and 4 carry out real-time trajectory planning, as shown in (c)–(d). Before the planning, the grasping planner sorted the grasps and eliminated the grasps where collisions occurred. Therefore, in (a) and (c), the target only has eight pregrasp poses. In (d), real-time trajectory planning selected the nearest pregrasp to the gripping jaw, so the pregrasp changes in real-time.

In working conditions 1–5, the conveyor belt velocity is unknown but fixed, and the trajectory prediction module can better predict the target trajectory. Among them, in conditions 4 and 5, the movement state of the obstacle changes in real-time. Algorithm 1 generates multiple trajectories and finds the one with the shortest execution time. The success rate of algorithm 1 is slightly higher than algorithm 2, much higher than algorithms 3 and 4, and the grasping time of algorithm 1 is closer to the global minimum. Compared with Algorithm 3, Algorithm 4 uses only one pregrasp, so the trajectory update frequency of Algorithm 4 is faster, and the success rate is higher. When the environment is entirely predictable, each execution of the local shortest path is not the entire shortest path. Therefore, algorithms 3, 4, which updated the trajectory in real-time, has a longer grasping time than algorithms 1, 2. In condition 5, with the most obstacles, algorithms 3 and 4 are only planned for the current environment, so they easily fall into local extremums and have the lowest grasping success rate.

In case 6, the speed of the conveyor belt will mutate several times, and algorithm 1 can only calculate the shortest grasping time at the speed of the conveyor belt at this moment but cannot calculate the global shortest time. The grasping time of algorithms 3, 4 is shorter than 1, 2. Thus, the real-time trajectory update algorithm has a shorter grasp time in an unpredictable environment. The success rate of algorithms 1, 2 and 3, 4 is similar, demonstrating our overall algorithm framework’s real-time performance and robustness.

The target moves in a circle, and its speed constantly changes in condition 7. In algorithms 1 and 2, the manipulator’s predicted intersection point is not on the target trajectory circle. The Bezier curve used only has a predicted length of 1.5 s. When the time is more significant than 1.5 s, the position can be predicted according to the speed at the end of the Bezier curve and the predicted trajectory is a straight line, as shown in equation (4). When the initial intersection time is greater than 1.5 s, the intersection point will be on the straight line instead of the Bezier curve and the prediction error is large. So the grasping success rate is low, which is not compared in Table 1. Algorithms 3 and 4 generate real-time trajectories and can respond to changes immediately, and they also have a high grasping success rate in condition 7. In working conditions 6 and 7, the environment cannot be completely predicted. Algorithm 3 updates the trajectory in real time and executes the shortest one among multiple trajectories each time, so the overall time is the shortest.

In summary, when the target trajectory can be predicted, our algorithm framework can respond immediately to environmental changes and has the highest success rate. When the target trajectory cannot be accurately predicted, algorithms 3 and 4 generate the real-time trajectory, while algorithm 4 has a faster calculation speed and higher success rate. However, algorithms 3 and 4 may fall into local extreme values in a complex environment with many obstacles, such as task 5.

Real robot grasp

As shown in Figure 12, the experimental platform consists of an AUBO-i5 manipulator, a parallel gripper, a conveyor belt and a Kinect2 depth camera. The camera in front of the conveyor belt is used to collect visual information. Based on the simulation experiment results, we selected algorithms 1 (Our) and 4 (No prediction + One grasp) for the real-world experiment, and the target objects are the same in the simulation experiment. We add a position based visual servoing (PBVS) controller for comparison of algorithms. Based on the target information collected by the camera, the difference between the end pose of the current manipulator and the target pose is obtained. The servo controller is used to calculate the motion speed of the manipulator. Then the motion speed of each joint of the manipulator is obtained to drive the manipulator to approach the target.

Figure 12.

The experimental platform.

We use three tasks in the actual experiment. In the Linear Motion and Linear with Moving Obstacle on The Belt tasks, the conveyor belt speed does not exceed 0.1 m/s. In the Linear with Varying Speed task, the linear velocity range is 0.02 to 0.1 m/s, changing the belt speed three times. We test each object 10 times in a task, resulting in 50 times for each algorithm.

The experimental results are shown in Table 2. The bold entries are the best results of the success rate and dynamic gripping time among the three methods. The conveyor belt velocity is unknown in the first two tasks but does not occur mutation. In the Linear Motion task, algorithm 1 uses a prediction module to generate a long trajectory with a higher success rate. As the trajectory with the shortest time is executed every time, the grasping time is the fastest. The PBVS method responds quickly and has the highest success rate. In the Linear with Moving Obstacle on The Belt task, algorithm 1 has the highest success rate and takes the least time. Due to the not complicated environment, algorithm 4 will not fall into the extreme value. However, the PBVS method moves directly to the target in the workspace and cannot avoid obstacles, so the success rate is the lowest. In the Linear with Varying Speed task, The PBVS algorithm achieves the highest success rate and takes the least time. Algorithms 1 and 4 have similar success rates.

Table 2.

The average success rate (%) and dynamic gripping time(s) of real-world experiments under each condition.

Method	Linear	Linear with moving obstacle on the belt	Linear with varying speed
Our	88, 5.67	80, 6.13	76, 8.91
No prediction + One grasp	82, 6.83	74, 7.24	78, 7.86
PBVS	92, 6.24	56, 6.82	83, 7.27

Overall, our algorithmic framework (Algorithms 1) generates complete trajectories and works best in the presence of obstacles. When the target moves regularly, algorithm 1 calculates the intersection position in advance, making the grasping time the shortest. PBVS is a local reactive control method that works best when there are no obstacles, and the target velocity varies greatly. Algorithm 4 generates the complete trajectory in real time, and the performance is between algorithm 1 and PBVS. In the Linear with moving obstacle on the belt task, the dynamic grasping process of the target moving at a speed of 0.09 m/s on the conveyor belt is shown in Figure 13.

Figure 13.

The actual robot captures the moving object while avoiding an obstacle. We place the target and obstacle on the unknown speed conveyor belt. The prediction module records the target’s location information for a period to predict the target trajectory, as shown in (a)–(b). The robot arm executes the corresponding trajectory and arrives at the pregrasp pose in (c)–(e). After reaching the grasp pose, the robot closes the gripper and lifts the target object in (f)–(h).

Conclusions

This paper proposes a new dynamic grasping framework for effective grasping in a dynamic environment. Many grasp poses of the target are clustered to determine multiple pregrasp poses, and we use the Bézier curve motion prediction module to generate predicted trajectories of all moving objects. The trajectory optimization algorithm constructing an approximate gradient field in joint space can generate a smooth trajectory. Simulation comparison and actual experiments verify the dynamic grasping framework’s high performance and feasibility. The algorithm framework is robust to environmental changes and has a high success rate in complex environments. When the target’s motion state can be accurately predicted, we can achieve fast and robust moving target grasping using the prediction module and trajectory planning for all grasp poses. When the environment is not easy to predict, we only quickly plan for the current optimal grasp pose and do not use the prediction module, which can also get good results.

In the future, we will use deep learning to establish the mapping relationship between the deep image features and the manipulator’s local trajectory and use deep reinforcement learning to train the dynamic grasping framework. The new method will combine the prediction and planner modules to achieve more efficient dynamic grasping.