Sensor-based skeleton modeling and MVEEs approach of the redundant manipulator for reaction motion

Abstract

With the wide application of redundant manipulators, sharing a working space with humans and dealing with uncertainty seems an inevitable problem, especially in the dynamic and unstructured domain. How to deal with obstacle avoidance is of particular importance that robots and humans/environments are safe interactions to fulfill the complex cooperating tasks. This paper aimed at solving the problem of multiple points avoidance for the reaction motion based on the skeleton algorithm in unstructured and dynamic environments. A method named “sensor-based skeleton modeling and MVEEs approach of the redundant manipulator for the reaction motion” is proposed. The extraction of skeleton information from image is obtained to calculate the distances of the multiple control points and establish the repulsion in this method. Afterward, the force Jacobian related to the priority weighting factors is calculated and then a reaction force with damping term is established, which is corresponding nominal torque commands. For the redundant manipulator, the joint angles are obtained through torque iteration instead of inverse kinematics to reduce calculation cost. Finally, the method was tested by a 7-DOF manipulator in the ROS framework. The obtained results indicate that the method in this method can realize dynamic obstacle avoidance and time cost reduction.

Introduction

Collaborative robots are finding many applications in fields such as manufacturing, health care, agricultural production, and social tasks. Compared to tightly structured environments, cooperation-type robots are made to interact closely with humans in collaborative contexts in an unknown and dynamic environment while robots have to detect and avoid obstacles. In this case, it is necessary for performing real-time reactive strategies to deal with all possible collisions instead of precisely following a predefined path. Some related researches include fixed control points, ¹ multiple-point control, ² modification of reaction paths. ³

For high-dimensional configuration space (C-space) formed by the high degree of freedom (high-DOF) robots, the problem of planning a feasible path in a dynamic environment is one of the most complex and time-consuming tasks in the field of robotics and it can cause easily a dimension explosion. Probabilistic and randomized approaches, such that Rapidly-Exploring Random Tree (RRT), ⁴ Probabilistic Roadmap Method (PRM), ⁵ have been used to resolve this problem of the dimensionality. ⁶ When the changes in the environment are inevitable, the potential fields are commonly used to estimate the reactive motion intent of the robots modeled as particles. ⁷ In particular, it has obvious advantages in dynamic obstacle avoidance ⁸ and dynamic target tracking, ⁹ however, these focused primarily on the mobile robots. When it comes to a redundant manipulator which is consists of a multiple-link structure with a certain volume and cannot be represented by a particle, the problem becomes challenging because there exist some intrinsic constraints such as local minima issues and fluctuations transferred to narrow passages. ¹⁰ Many attempts have been performed to develop various ways to handle these limitations.^11–15 Whereas these main studies are the target for collision avoidance at the end-effector of the manipulator from the perspective of kinematics. For the redundant manipulator composed of multiple-link, the collision of arbitrary points may occur in the process of planning, but it is seldom discussed.

For collision avoidance, it needs to regard the motion of the robot at the arbitrary point as a possible reactive movement, and the problem has been extensively described in the robotics literature. The complete framework and its implementation have been elaborated in Brock and Khatib, ³ although the major effort remains on the computation of control points position and corresponding Jacobian matrix. Alejo et al. ¹⁶ put forward a real-time reactive collision avoidance method for multiple aircraft which has been applied in industrial environments. From the control viewpoint, a more general reactive motion approach called the “replanning and deformation methods” has been described. ¹⁷ While approaching an obstacle, the planner first attempts to apply deformation and the robot can continue to replan if only the path remains feasible. In Cui et al., ¹⁸ combining a reactive path-planned approach with the Dubins curves and the artificial potential field (APF) method solves the path planning of the robot in a dynamic environment.

For reactive collision avoidance in human-robot interaction (HRI), it is necessary to track the key parts of the objects while a control system can take corrective actions before collisions. One major way for the introduction in unstructured environments depends on sensors to compute the distances between possibly colliding parts. In this context, the reliability and stability of the sensor directly correlate with the safe interaction with robot and human/environment. The Point Cloud Library (PCL)^19–21 is the most common approach for obstacles or environments modeling with the visual sensors. However, the main shortcoming is that this method cannot fully describe the obstacle information in the environment. In Wu et al., ²² a high-dimensional grid model was used to model the known static obstacles, and the potential field method was adopted to process the unmodeled dynamic obstacles for the manipulator trajectory planning. Flacco Fabrizio ²³ proposed a method based on depth space offline mapping to divide depth space into depth grids. These two methods require that the main camera must cover the entire robot workspace, which has low computing efficiency and requires parallel computing to accelerate. As a common characteristic, the idea of directly calculating the distance of the environment image was introduced in Kuhn and Henrich. ²⁴ To improve performance in the computational time cost, the authors^25,26 proposed a new method to assess the distances between a general point in Cartesian space and an object perceived by the depth sensor, but it can only be applied to a single sensor. In Li et al. ²⁷ presents an environment modeling method for the manipulator using the offline mapping between the depth image and OctoMap in a dynamic non-structured environment. However, when the distance evaluation is calculated, only the real-time obstacle avoidance research of the end-effector is regarded.

In this paper, a method of sensor-based skeleton modeling and multiple virtual end-effectors (MVEEs) approach of the redundant manipulator for the reaction motion is proposed. In this framework, the geometry and architecture of the manipulator and objects are obtained by the extraction of the skeleton based on the three-dimensional (3D) sensor. Then the possible multiple collision points and distances can be calculated based on the skeleton model. Finally, the priority weighting factors are defined according to the virtual repulsive force and the reaction force is generated, resulting in corresponding nominal torque commands. Compared to the previous research, the primary contributions are listed as follows: (1) A method to get the skeleton online based on 3D vision sensor is proposed. (2) The bounding box was constructed and the distance calculation of the manipulator’s multiple control points and obstacles based on the real-time detection of skeleton model, which dramatically reduces the computational complexity. (3) For the redundant manipulator composed of multiple links, the MVEEs approach based on the skeleton model is realized to avoid the dynamic obstacle and the joint angles are obtained through torque iteration instead of inverse kinematics to reduce calculation cost.

Vision-based skeleton and bounding box modeling

A policy that automatically selects the control points in the whole process of motion, based on sensor information, could make it efficient for the application of human-robot interaction. It is important to note, however, that these control points should be able to be calculated quickly and result in simple distance computation. In this section, the skeleton algorithm and the bounding box modeling for dynamic objects for fast multiple-point control, are described.

Depth space model of the object

Figure 1 shows four coordinate systems that exist in the process of obtaining image information by the 3D sensor. The origin of the camera frame, which is described by the world frame, is defined as the optical center. The image pixel frame is on the top-left side of the image, (u, v) are the columns and rows of the pixel, respectively. The focal length f is the distance between the imaging plane’s center and the camera optical center.

Figure 1.

Coordinate relation of the 3D vision sensor.

The vision sensor is modeled as a typical pinhole camera which is constituted of two main parts: the intrinsic parameters matrix K denoted the projection of a Cartesian point on the image plane, and the extrinsic parameters matrix ε represented the coordinate transformation between the world and the sensor frame. Considering each point has the pixels given by the RGB camera and the distance in the depth space which is expressed as P_d = (u, v, d_p)^T, the Cartesian point P_c = (x_c, y_c, z_c)^T in the sensor frame is given by:

$$\begin{array}{l}{x}_c=\frac{u-c_x}{f{s}_x}{z}_c\\ y_c=\frac{v-c_y}{f{s}_y}{z}_c\\ z_c=d_p\end{array}$$

(1)

Where ${\mathrm{s}}_{\mathrm{x}}$ and ${\mathrm{s}}_{\mathrm{y}}$ are the pixel dimensions, ${\mathrm{c}}_{\mathrm{x}}$ and ${\mathrm{c}}_{\mathrm{y}}$ correspond to the image center coordinates in pixels.

Thus, we can calculate point P = (x_W, y_W, z_W)^T in the world frame as follows:

$$z_c\left[\begin{array}{c}u\hfill \\ \multicolumn{1}{c}{v}\\ \multicolumn{1}{c}{1}\end{array}\right]=K\epsilon \left[\begin{array}{c}{x}_W\hfill \\ \multicolumn{1}{c}{y_W}\\ \multicolumn{1}{c}{z_W}\\ \multicolumn{1}{c}{1}\end{array}\right]=\left[\begin{array}{cccc}f{s}_x\hfill & 0\hfill & c_x\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& f{s}_y\hfill & c_y\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 0\hfill \end{array}\right]\left[\begin{array}{cc}{R}^{\prime }\hfill & T^{\prime }\hfill \\ \multicolumn{1}{c}{0}& 1\hfill \end{array}\right]\left[\begin{array}{c}{x}_W\hfill \\ \multicolumn{1}{c}{y_W}\\ \multicolumn{1}{c}{z_W}\\ \multicolumn{1}{c}{1}\end{array}\right]$$

(2)

Where ${\mathrm{R}}^{\prime }$ and ${\mathrm{T}}^{\prime }$ represent the rotation matrix and the translation vector.

Building the skeleton

In this section, the skeleton extraction is based on the idea of thinning, that is, remove some points from the original image, but keep the original shape, until the skeleton of the image is obtained. All the definitions are followed by the Zhang-Suen method. ²⁸ Supposed a 3 × 3 window is applied and each element of the pixel (i, j) is related to its eight neighboring elements as shown in Figure 2.

Figure 2.

Representation in a 3 × 3 window.

As well known that a pixel value at the nth iteration rests on the new pixel and its eight adjacent pixels at the (n − 1)th iteration in image processing. The variables A(p₁) and B(p₁) are the 0–1 patterns numbers and nonzero neighbors numbers of p₁, respectively. To preserve the skeleton pixels in the 8-connectivity sense, the two sub-iterations are described as follows.

In the first sub-step, remove the pixel p₁ from the digital pattern if conditions (a)–(d) are all satisfied: (a) $2\le \mathrm{B}({\mathrm{p}}_1)\le 6$ ; (b) $\mathrm{A}({\mathrm{p}}_1)=$ 1; (c) ${\mathrm{p}}_2*{\mathrm{p}}_4*{\mathrm{p}}_6=0$ ; (d) ${\mathrm{p}}_4*{\mathrm{p}}_6*{\mathrm{p}}_8=0$ .

In the second sub-step, the last two of the four conditions are: (c′) ${\mathrm{p}}_2*{\mathrm{p}}_4*{\mathrm{p}}_8=0$ ; (d′) ${\mathrm{p}}_2*{\mathrm{p}}_6*{\mathrm{p}}_8=0$ . Meanwhile, to keep the remaining pixels away from each other, the pixel p₁ is deleted if ${\mathrm{p}}_2+{\mathrm{p}}_3+{\mathrm{p}}_8+{\mathrm{p}}_9\ge 1$ . Thus, it can be simplified the whole volumetric analysis of the manipulator/obstacle by taking into account a skeleton of the structure (Figure 3).

Figure 3.

Skeleton extraction.

Constructing bounding box used the geometric primitive

The characteristic of the objects in the workspace can be described as simple geometric primitives using the shapes of the general objects, such as cuboid, cylindrical, conical, etc. As shown in Figure 4, {O_bX_bY_bZ_b} denotes the world coordinate system, and the arbitrary point P relative to {O_bX_bY_bZ_b} are represented by (x, y, z). When the satellite component assembly is the main application scenario, the main types of geometric primitives using super-quadric surface functions depiction can be defined in 3D space ²⁹ and the expression to depict a single geometry primitive is represented as follows.

• (a) For a cylindrical surface, the super-quadric surface function is represented by:

  $$S(x,y,z)={\left(\frac{x}{r}\right)}^2+{\left(\frac{y}{r}\right)}^2+{\left(\frac{z}{h}\right)}^8-1$$

  (3)

• (b) For a cuboid surface

  $$S(x,y,z)={\left(\frac{x}{a}\right)}^8+{\left(\frac{y}{b}\right)}^8+{\left(\frac{z}{c}\right)}^8-1$$

  (4)

Figure 4.

Geometric primitives description of the objects.

Based on the established skeleton, the parameters such as a, b, c can be obtained by:

$$\begin{array}{c}{p}_{\min }=\left[\begin{array}{ccc}{x}_{\min },\hfill & y_{\min },\hfill & z_{\min }\hfill \end{array}\right]\hfill \\ \multicolumn{1}{c}{p_{\max }=\left[\begin{array}{ccc}{x}_{\max },\hfill & y_{\max },\hfill & z_{\max }\hfill \end{array}\right]}\end{array}$$

(5)

Then the center of the bounding box can be calculated by:

$$c=(p_{\min }+p_{\max })/2$$

(6)

We define dimension vector $s$ as the vector from point $p_{\min }$ to point $p_{\max }$ , which contains the information on the length, width, and height of the bounding box:

$$s=p_{\max }-p_{\min }$$

(7)

Combined with the above analysis, the eight vertex coordinates of the bounding box can be easily obtained on the basis of the established skeleton (see Figure 5(a)). In general, the Cartesian position of each joint, computed by the D-H table described in Table 2, can constitute the endpoints of the skeleton segments.

Figure 5.

Bounding box model: (a) bounding box based on the skeleton, (b) a segment of the bounding box, and (c) the acquisition of the radius r.

Aim at safety collision avoidance, a cylindrical bounding box with the skeleton as a central axis, to which add two hemispherical bounding boxes, was constructed for the manipulator. Figure 5(b) shows a segment of the bounding box, which line AB represents the central axis of the skeleton, and r is the bounding box radius. It can be obtained that points A and B are the intersections or endpoints of the skeleton, and the radius r can be directly approximated in the SolidWorks model or calculated by the projection method (see Figure 5(c)). In view of the method for the bounding box construction based on the skeleton, it greatly provides convenience for the calculation of distance in the next section.

The corresponding distance calculation of the multiple control points

Distance calculation for a control point

Based on the description in the previous section, the control endpoints of the skeleton segments can be obtained. For any point P (x, y, z), we can judge whether it is on the obstacle surface according to the calculation of equations (3) and (4).

$$\{\begin{array}{cc}S(x,y,z)<0,\hfill & P\mathrm{is}\mathrm{inside}\mathrm{the}\mathrm{surface}\hfill \\ \multicolumn{1}{c}{S(x,y,z)=0,}& P\mathrm{is}\mathrm{on}\mathrm{the}\mathrm{surface}\hfill \\ \multicolumn{1}{c}{S(x,y,z)>0,}& P\mathrm{is}\mathrm{outside}\mathrm{the}\mathrm{surface}\hfill \end{array}$$

(8)

As shown in Figure 6, this paper takes the case of a cuboid object as the research description. The value $S(x,y,z)$ from equation (8) is proportional to the proximity of point P to the obstacle, that is the larger $|S(x,y,z)|$ , the farther away P is from the surface. Thus, as commonly-used distance metric is the Euclidean distance, $S(x,y,z)$ called the pseudo distance can be used to assess the proximity.

$$\widetilde{d_p}(P)=S(x,y,z)$$

(9)

Figure 6.

The location description between any point P and the bounding box.

Distance calculation for skeleton segments

As well known that a segment is the most likely to collide at the minimum distance, which should also be considered as a moving control point on the basis of endpoints of segments. If all the points on the segment which is denoted as link S₁S₂ as an example are regarded, The calculation of the real minimum value is generally very complex. In this paper, we can first judge whether the control endpoints S₁, S₂ are external according to equation (8). The distances of the control points S₁, S₂, as shown in Figure 7(b), can be approximately calculated according to equation (9), which is represented by $\widetilde{d_p}({\mathrm{P}}_{{\mathrm{S}}_1})$ , $\widetilde{d_p}({\mathrm{P}}_{{\mathrm{S}}_2})$ . Then when the segment S₁S₂ is located on the same plane, obviously the distance between S₁ or S₂ is the minimum distance point. However, another case is that the segment S₁S₂ spans the two planes, as shown in Figure 7(a). The minimum distance of the segment is not at the endpoint, but a point in the middle of the segment. Thus, we can get the plane in which each endpoint is located based on the angle between the bounding box normal vector $\overrightarrow{\mathrm{n}}$ (red solid line) and the normal vector $\overrightarrow{{\mathrm{n}}_0}$ (red dashed line) composed of the endpoint vector $\overrightarrow{v_1{s}_2}$ and the plane segment $\overrightarrow{v_1{v}_5}$ . In this way, we judge that the control endpoints S_1,S₂ are related to the intersecting line segment of the two adjacent planes such as (v₁, v₃, v₅, v₇) and (v₀, v₁, v₂, v₃), with the minimum distance between the segment v₁v₃ and segment S₁S₂ (Figure 7(c)).

Figure 7.

Schematic diagram of human-robot interaction: (a) the segment S₁S₂ spans the two planes, (b) the distances of the control points S₁, S_2, and (c) the minimum distance between the two segments.

Let $\overrightarrow{P_a}$ and $\overrightarrow{P_b}$ represent the generic point position vectors in the two segments, whose extreme points are v₁, v₃, and S₁, S₂, respectively.

$$\overrightarrow{P_a}=\overrightarrow{v_1}+t_1\overrightarrow{u_1}$$

(10)

$$\overrightarrow{P_b}=\overrightarrow{s_2}+t_2\overrightarrow{u_2}$$

(11)

Where $\overrightarrow{u_1}$ and $\overrightarrow{u_2}$ are calculated by:

$$\overrightarrow{u_1}=\frac{\overrightarrow{v_3}-\overrightarrow{v_1}}{\Vert \overrightarrow{v_3}-\overrightarrow{v_1}\Vert }$$

(12)

$$\overrightarrow{u_2}=\frac{\overrightarrow{s_2}-\overrightarrow{s_1}}{\Vert \overrightarrow{s_2}-\overrightarrow{s_1}\Vert }$$

(13)

and {t₁, t₂} is scalar values, with $t_1/\Vert \overrightarrow{v_3}-\overrightarrow{v_1}\Vert \in \{0,1\}$ , $t_2/\Vert \overrightarrow{s_2}-\overrightarrow{s_1}\Vert \in \{0,1\}$ .

When the distance is minimal, the points on the two segments are denoted the collision points p_ac and p_bc, respectively. Thus, the common normal line can be calculated as:

$$\overrightarrow{P_{\mathrm{ac}}{P}_{\mathrm{bc}}}=\Vert \overrightarrow{s_1}-\overrightarrow{v_1}+t_2\overrightarrow{u_2}-t_1\overrightarrow{u_1}\Vert$$

(14)

$$\overrightarrow{P_{\mathrm{ac}}{P}_{\mathrm{bc}}}\times \overrightarrow{u_1}=0$$

(15)

$$\overrightarrow{P_{\mathrm{ac}}{P}_{\mathrm{bc}}}\times \overrightarrow{u_2}=0$$

(16)

When $t_1$ and $t_2$ are both determined, the points p_ac and p_bc generating minimum distances within the two segments can be obtained . Thus, this analytical approach is adopted to compute the distance of the points p_ac and p_bc.

$$d_M=\Vert \overrightarrow{P_{\mathrm{ac}}}-\overrightarrow{P_{\mathrm{bc}}}\Vert$$

(17)

Thus, the minimum distance of a segment is obtained as:

$$\widetilde{d_{\mathrm{link}}}({\mathrm{S}}_1{\mathrm{S}}_2)=\min (\widetilde{d_p}({\mathrm{P}}_{{\mathrm{S}}_1}),\widetilde{d_p}({\mathrm{P}}_{{\mathrm{S}}_2}),d_M)$$

(18)

The distances of all segments can be calculated based on the equation (18), thus the minimum distance between the robot and the obstacle can be descried by :

$$\widetilde{d_{\mathrm{arm}}}=\min (\widetilde{d_{\mathrm{link}1}},\widetilde{d_{\mathrm{link}2}}...\widetilde{d_{\mathrm{linkn}}})$$

(19)

When there is no collision at the minimum distance, the manipulator can safely achieve collision avoidance. In the next section, details on MVEEs approach are given.

The MVEEs approach of the redundant manipulator for reactive motion

Figure 8 shows the system flow chart of collision avoidance. After constructing the bounding box based on the skeleton algorithm to calculate the distance of the multiple control points in real-time, the multiple points of each segment are equivalent to MVEEs. Then the priority weighting factors are defined according to the virtual repulsive force value and the force Jacobian matrix is obtained. Next, the velocity mapping of the joint angle to the minimum distance control point is realized by Jacobian and the reaction force, combined with repulsive force and reverse resistance (damping term) induced by velocity under the regulation of the parameter $D_a$ , is established. Based on the above analysis, the torque generated by a reaction force to collide avoidance is derived, then the actual trajectory can be calculated by between kinematics and dynamic of the manipulator.

Figure 8.

System flow chart of collision avoidance.

Generating reaction force in MVEEs

Given the framework of Figure 8, the reaction force to avoid collision consists of two parts: the repulsive force and the reverse resistance. To compute the repulsive field intensity, we calculate the distance between the multiple points of the robot skeleton and the object from the 3D sensor. The principle of constructing a repulsion field is: the robot and the obstacle are mutually repulsive and do not touch each other; meanwhile the repulsive force should gradually decrease as soon as the robot is away from the object. The safety index (defined as $d_{\mathrm{safe}}$ ) is derived from the obstacle and the manipulator bounding box within which the repulsive field is activated. The repulsive force function can be expressed as:

$$U_{\mathrm{rep},i}(q)=\{\begin{array}{cc}0\hfill & d_i(q)>d_{\mathrm{safe}}\hfill \\ \multicolumn{1}{c}{\frac{1}{2}{\eta }_i{(\frac{1}{d_i(q)}-\frac{1}{d_{\mathrm{safe}}})}^2\frac{d_g^n}{1+d_g^n}}& d_i(q)\le d_{\mathrm{safe}}\hfill \end{array}$$

(20)

Where ${\eta }_i$ is a positive weight coefficient, $d_i\left(q\right)=\Vert p_i\left(q\right)-p_i\left(q_{\mathrm{obstacle}}\right)\Vert$ , which represents the nearest distance between the ith point and the obstacle. The distance index $d_g=\frac{L_N(q)}{d_i(q)}$ , namely the ratio of the distance from the control point to the goal and that to the obstacle. When the robot is far from the obstacle, that is, $d_g^n>>1$ , the expression $\frac{d_g^n}{1+d_g^n}$ is equivalent to approximately 1 and the repulsive field is equivalent to amplification. When the robot is ready for the objective, that is $d_g^n<<1$ , the expression $\frac{d_g^n}{1+d_g^n}\approx d_g<1$ . That is to say, the repulsive field is weaker and $d_g$ plays a critical role in the weakening of the repulsive field.

Figure 9 shows the repulsive force consisted of radial force $f_{\mathrm{rep}1}$ and tangential force $f_{\mathrm{rep}2}$ , as be given the role of $d_g$ . The repulsive force of any control point is given by:

$$f_{\mathrm{rep},i}(q)=\{\begin{array}{cc}0\hfill & d_i(q)>d_{\mathrm{safe}}\hfill \\ \multicolumn{1}{c}{f_{\mathrm{rep}1}{n}_{\mathrm{or}}+f_{\mathrm{rep}2}{n}_{\mathrm{rg}}}& d_i(q)\le d_{\mathrm{safe}}\hfill \end{array}$$

(21)

Figure 9.

Repulsive force.

Where the unit normal vector $n_{\mathrm{or}}$ indicates the direction from the obstacle to the robot control point and $n_{\mathrm{rg}}$ denotes the direction of the manipulator to the objective. The $f_{\mathrm{rep}1}$ component is represented by the negative repulsive field gradient as follows:

$$f_{\mathrm{rep}1}={\eta }_i(\frac{1}{d_i\left(q\right)}-\frac{1}{d_{\mathrm{safe}}})\frac{d_g^n}{d_i^2\left(q\right)(1+d_g^n)}$$

(22)

In fact, the tangential repulsive force from obstacle $f_{\mathrm{rep}2}$ is equivalent to attraction in the direction of the goal, represented by taking the gradient of $d_g$ as follows:

$$f_{\mathrm{rep}2}=\frac{1}{2}{\eta }_i{(\frac{1}{d_i\left(q\right)}-\frac{1}{d_{\mathrm{safe}}})}^2\frac{d_g^{n-1}}{{(1+d_g^n)}^2}$$

(23)

The control point $p_{\mathrm{ac}}$ , which defined the minimum distance of the manipulator, is obtained according to equation (19). When we ensure that the minimum distance place does not collide, then the manipulator can safely achieve collision avoidance. Thus, the reaction force of adding the damping term is expressed as follows:

$$f_{\mathrm{ac}}=f_{\mathrm{rep},i}(q)-D_a{\stackrel{·}{p}}_{\mathrm{ac}}$$

(24)

Where $f_{\mathrm{rep}}(q)$ is the repulsion of the control point $p_{\mathrm{ac}}$ , ${\stackrel{·}{p}}_{\mathrm{ac}}$ is the velocity of the control point $p_{\mathrm{ac}}$ , and $D_a$ is a suitable tuning parameter based on fuzzy rules in which the input variables are respectively the velocity ${\stackrel{·}{p}}_{\mathrm{ac}}$ and the repulsive force $f_{\mathrm{rep}}(q)$ . ${\stackrel{·}{p}}_{\mathrm{ac}}$ has a differential mapping relationship with the responding the joint velocities $\stackrel{·}{q}$ through the Jacobian matrix $J_{\mathrm{ac}}$ :

$${\stackrel{·}{p}}_{\mathrm{ac}}=J_{\mathrm{ac}}\stackrel{·}{q}=J_s\left[\begin{array}{c}{\stackrel{·}{p}}_{v1}\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{p}}_{v3}}\end{array}\right]=J_s\left[\begin{array}{c}{J}_{v1}\hfill \\ \multicolumn{1}{c}{J_{v3}}\end{array}\right]\stackrel{·}{q}$$

(25)

Where $J_{\mathrm{ac}}$ depends on the two endpoints of the responding segment in which $p_{\mathrm{ac}}$ is located, that is v1, v3. $J_{v1}$ , $J_{v3}$ are the Jacobians of the two endpoints of the segment, respectively, and

$$J_s=\left[\begin{array}{cc}\frac{\partial p_{\mathrm{ac}}}{\partial p_{v1}}\hfill & \frac{\partial p_{\mathrm{ac}}}{\partial p_{v2}}\hfill \end{array}\right]$$

(26)

It can be seen that the next is on the solution of $\left[\begin{array}{c}{J}_{v1}\hfill \\ \multicolumn{1}{c}{J_{v3}}\end{array}\right]\stackrel{·}{q}$ . In fact, v1, v3 is the part of the manipulator’s multiple control points, and we can get the $J_{v1}$ , $J_{v3}$ , $\stackrel{·}{q}$ from the force Jacobian based on priority weighting factors in the next section.

Definition of force Jacobian matrix based on priority weighting factors in MVEEs

As shown in Figure 10, the presence of the virtual repulsive force makes the robot offset, and the greater the force, the greater the offset of the corresponding control point. Thus, we can get the displacement change by the proportion of the force generated on each control point, that is, the priority value of each point is defined as follows:

$${\mathcal{W}}_j^i=\{\begin{array}{cc}\frac{\left|F_j^i\right|}{{\sum }_{i=1}^n{\sum }_{j=1}^l\left|F_j^i\right|}\hfill & F_j^i\ne 0\hfill \\ \multicolumn{1}{c}{\mathrm{const}}& F_j^i=0\hfill \end{array}$$

(27)

Figure 10.

Generation of collision avoidance motion: (a) generated repulsive force at control point and (b) displacement of the control point.

Where n represents the control point numbers and l is the displacement directions caused by corresponding points. $F_j^i$ is the jth elements of control point force $F^i$ . From equation (26), it can be seen that the weighting factors of each control point are changed when the force $F_j^i$ are different. Thus, the priority function W is defined as:

$$W=\mathrm{diag}[{\mathcal{W}}_1^1,...,{\mathcal{W}}_l^1,...,{\mathcal{W}}_1^n,...,{\mathcal{W}}_l^n]$$

(28)

Where ${\mathcal{W}}_j^i$ represents weighting factors element of ith control point and jth displacement direction. According to the next section, it is easy to reach the desired displacement when the weight factor value of the corresponding point is high.

Using the decided $W$ , the force Jacobian matrix $J_F$ , resulting from the virtual repulsive forces at the control points, can be expressed as follows:

$$J_F=\mathrm{WJ}$$

(29)

Where $J=[J^1,J^2,J^3,...,J^n{]}^T$ is the concatenated Jacobian matrix. $J^i\in R^{l\times m}$ is the Jacobian matrix of the $C{P}_i$ and m is the degree of freedom of the manipulator. When the column of $J_F$ has a full rank, there exists a left-inverse matrix and thus $J_F^{+}$ , used to represent the pseudo-inverse of the Jacobian matrix $J_F$ , can be achieved by:

$$J_F^{+}={\left(J^T\mathrm{WJ}\right)}^{-1}{J}^{T}W$$

(30)

Since the manipulator is redundant, the corresponding joint-angular vectors associated with the VEE position vector based on the differential kinematics mapping can be described as follows:

$$\stackrel{·}{q}=J_F^{+}\stackrel{·}{p}$$

(31)

Based on the above analysis, the velocity mapping relationship between the control points of each section can be obtained. Thus, the velocity-related terms can be mapped to compute the force value of equation (24), which provides a basis for the torque calculation in the next section.

Computing avoidance torques

As introduced, it is necessary to convert the desired trajectories to joint velocities or torques and the related torque is tracked in system dynamics. The avoidance torques generated by the reaction forces via the transpose of the Jacobian matrix defined in equation (25) are given:

$${\tau }_c=J_{\mathrm{ac}}^T{f}_{\mathrm{ac}}$$

(32)

Hence, we compute joint angles through incremental iteration of the torque. Here we use a batch update rule:

$$\mathrm{q}(\mathrm{t}+1)=\mathrm{q}\left(\mathrm{t}\right)+\mathrm{\beta }{\tau }_c$$

(33)

Where $\mathrm{\beta }$ is an iterative step in joint space. The pseudo-code for the proposed approach in this paper is shown in the following Table 1.

Table 1.

Iterative updating of the joint angle.

Step 1:	Initialize $q$ , $\stackrel{·}{q}$ , p
Step 2:	Calculate $f_{\mathrm{rep},i}$ , ${\mathcal{W}}_j^i$ for ∀i
Step 3:	Obtain $J_F$ , $J_{\mathrm{ac}}$ , $\stackrel{·}{p},\stackrel{·}{\mathrm{q}}$ , through the Step 2
Step 4:	Get ${\stackrel{·}{p}}_{\mathrm{ac}}$ , $f_{\mathrm{ac}}$ , ${\tau }_c$ through the Step 3
Step 5:	Update joint angles $q$  ←  $q+\mathrm{\beta }{\tau }_c$

When the end-effector is close enough to the goal if ${\mathrm{d}}_{\mathrm{i}}\left(\mathrm{q}\right)=\Vert {\mathrm{p}}_{\mathrm{i}}\left(\mathrm{q}\right)-{\mathrm{p}}_{\mathrm{i}}\left({\mathrm{q}}_{\mathrm{goal}}\right)\Vert <{\mathrm{d}}_{\mathrm{threshold}}$ or the maximum number of iterations reached.

Simulation and results analysis

To verify the effectiveness of the proposed method in this paper, the 7-DOF manipulator is applied in the scenario of satellite component assembly via Gazebo and ROS software. As shown in Figure 11(a), human-robot collaboration in the shared narrow space to complete the work of satellite component assembly meanwhile the collision is needed to avoid. Figure 11(b) shows the simulation environment implemented through the ROS framework for this task. The detailed D-H parameters of the manipulator are presented in Table 2.

Figure 11.

Simulation validation scenario: (a) Satellite component assembly scene and (b) simulation structure used ROS and Gazebo.

Table 2.

D-H parameters of the manipulator.

Frame	Link	αi− 1 (°)	ai− 1 (m)	di (m)	θi (°)
0–1	1	0	0	0	θ 1
1–2	2	90	0	0	θ 2
2–3	3	−90	0	0.4495	θ 3
3–4	4	90	30	0	θ 4
4–5	5	−90	−30	0.4495	θ 5
5–6	6	90	0	0	θ 6
6–7	7	−90	0	0	θ 7

The simulation procedure is described as follows: When a human (obstacle) is shared with the robot in the workspace, the Kinect sensor is used for the 3D image acquisition and the skeleton is extracted according to section “Vision-based skeleton and bounding box modeling” in this paper. The bounding box can be obtained combined with the skeleton and the geometric primitive. When the manipulator is moving, we can calculate the distance information between each segment of the skeleton and the obstacle bounding box in real-time, then the movements are displayed in the Gazebo virtual environment. We set the coordinates of the target point to be (0, −0.65, 0.45) m. The initial position of the robot is in the vertical state and the starting position of the end-effector is (0, 0, 0.899) m. The safety index $d_{\mathrm{safe}}$  = 0.1 m, ${\mathrm{d}}_{\mathrm{threshold}}$  = 0.05 m, repulsion coefficient ${\eta }_i$  = 50, iterative step $\mathrm{\beta }=0.1$ , the damping term parameter $D_a$ is designed based on fuzzy rules and the input variables respectively are v ∈ (−0.015, 0.015), F∈ (−5, 5) in Figure 12(f). The simulation results are presented in Figure 12.

Figure 12.

Simulation results of 7-DOF redundant robot: (a) Minimum distance of segments, (b) force of control points in the Y direction, (c) force of control points in the Z direction, (d) priority weighting factor W of control points in the Y direction,(e) priority weighting factor W of control points in the Z direction, (f) reaction force in the Y and Z direction, (g) distance to goal, and (h) comparison test of some obstacle avoidance methods.

Figure 12(a) shows the distance between the robot and the human from the starting point to the goal. The lower graph represents the minimum distance between each segment of the robot and the obstacle (human). Since the robot pedestal is fixed (see Figure 11(b)), only the elbow segment (segment 2) and wrist segment (segment 3) may collide during movement. Under this circumstance, the minimum distance of the manipulator can be obtained in the upper graph of Figure 12(a).

The repulsive forces by human versus time correspond to the control points CP1, CP2, CP3 in the Y and Z direction are drawn in Figure 12(b) and (c). It can be seen that the maximum repulsive force of the control point CP2 occurs at approximately 28 s and the distance is also the minimum (Figure 12(a)). Thus, a higher force was required for the control point being in danger of collisions to realize the avoidance.

As shown in Figure 12(d) and (e), the priority weighting factors ${\mathcal{W}}_{\mathrm{cp}1}$ , ${\mathcal{W}}_{\mathrm{cp}2}$ , ${\mathcal{W}}_{\mathrm{cp}3}$ of each control point in the Y and Z direction are generated, respectively. The weighting factors are changed with the proportion of each repulsive force according to equation (21).

As shown in Figure 12(f), there are two graphs. The inset graph represents the corresponding parameter $D_a$ based on the fuzzy inference system (FIS) is variable according to the velocity of the minimum distance control point and the responding repulsive force, which is described as estimation of interactive intention in our previous study. ³⁰ The fuzzy field parameters are (−0.015, 0.015), (−5, 5), (10, 20), respectively. Thus the reaction forces in the Y and Z direction can be obtained by equation (24).

Figure 12(g) shows the distance from the end-effector of the robot to the target point. It could be shown that the manipulator kept getting close to the goal in the first 10 s. The dynamic obstacle appeared after that time and considering the influence of reaction force, the distance from the target fluctuated. After 30 s, the reaction force is 0, indicating that the manipulator has been far away from the obstacle, so it follows the original path. It can be seen that the distance approaches zero and the manipulator reaches the goal as the gradual movement to the target point.

Discussion

As it can be seen in the simulation results section, the MVEEs approach can achieve obstacle avoidance for a manipulator with higher degrees of freedom. In view of the time cost and the superiority considerations, the graph-based basic method A*, the classical method artificial potential field (APF) for multi-DOF manipulator, and the optimization-based approach ant colony APF (AC_APF) are selected to be compared with the method MVEEs in this paper. Figure 12(h) shows the comparison tests of these methods in C-space. The results of the contrast are shown in Table 3. Two points, green and yellow, indicated the initial and goal points, while the objects, surrounded by blue, represent obstacles. The coordinates of the starting points and target points are respectively (580, 580), (160, 40).

Table 3.

Results for the method comparison.

Factor	APF	AC_APF	A*	MVEEs
Attraction gain	0.2	0.2	/	/
Repulsion gain	100	100	/	50
Last position	(160.2, 188.7)	(160.15, 40.8)	(159.2, 40.2)	(159.28, 40.3)
Distance to goal (cm)	148.7	0.81	0.82	0.78
Time (s)	7.4804	121.6748	245.8535	52.4186
Reached goal	No	Yes	Yes	Yes
Collision	No	No	No	No

Some important remarks about the feasible methods are given: the probabilistic-based method A* is applicable for manipulators to search a reachable path in a static environment. The time (245.8535 s) for the simulation process is greatly increased while the distance is almost 0.82 cm which is closer to the target point. However, it has many inflection points which lead to more vulnerability to the robot’s stopping and starting frequently, meanwhile it increases the probability of collision with the edge of obstacles that means a poor real-time performance. Moreover, it uses a pre-calculation process to identify collisions with obstacles. It means that this disadvantage grows exponentially with the complexity and size of the environment and results in the computing time and storage capacity increasing significantly.

The collision-avoidance path for manipulator using APF shows the effective simulation results in a dynamic environment and avoids obstacles in real-time. It only takes 7.4804 s in the APF method and the distance between the end-effector is approximately 148.7 cm. However, the multi-DOF manipulator is trapped in local minima, especially in a narrow passage, due to the attraction to the desired goal configuration and the repulsion to the configuration space obstacles almost equal. Aim to this problem, some optimization-based approaches are applicable to APF. The AC_APF algorithm to improve the planning of manipulator end-effector is adopted for obstacle avoidance planning. The computing time (121.6748 s) decreased by half when compared to method A* and the distance is almost 0.81 cm.

In our work, the collision-avoidance method MVEEs for the manipulator has been carried out using deterministic methodologies, and therefore the defects of probabilistic methods can be avoided. It can be seen that two methods, which take respectively 121.6748 and 245.8535 s, are at the expense of computing time compared to our method which takes 52.4186 s.

To authors’ knowledge, there is no document in the literature supporting the method MVEEs to plan multiple control points collision-avoidance trajectory for the manipulator. Meanwhile, there are fewer parameters to be adjusted only the damping term parameter $D_a$ which is based on the interactive intention to guarantee deterministic collision avoidance. This may be a significant advantage as it opens the possibility of finding a collision-avoidance path, similar to the use of the resistance behavior of the damping term for the higher-DOF manipulator. Therefore, we ensure that our present work is worth research for the manipulator in the field of reaction motion.

The limitation of the proposed method and future research should be focused on the following aspects:

• The method of obstacle bounding box modeling based on the geometric primitive is a convex polygon in this paper, more work on concave polygon and complex obstacles in this respect should be considered.

• When the multi-control point obstacle avoidance, facing multi-dynamic obstacles sharing a narrow space with the robot, is inevitable, the compliant contact control of some parts is also a worthy exploring problem.

• Extending the proposed method to modeling and simulation of nonlinear systems with greater degrees of freedom robot.

Conclusion

In summary, a method of MVEEs control based on a skeleton model with the 3D-sensor is proposed to solve the problem of multiple control points avoidance for the reaction motion. The proposed approach is used to get the multiple possible collision points and distances between the manipulator and obstacles in the unstructured dynamic environment. Then, it greatly reduces the time and calculation cost through the Jacobian matrix related to the priority weighting factors. Meanwhile, a new form of a reaction force, which is corresponding nominal torque commands, to realize the joint angles iteration instead of inverse kinematics. The simulation results mean that this method can be successfully applied for the manipulator’s obstacle avoidance from beginning to end. Consequently, the MVEEs proposed in this paper could be widely applied in the field of human-robot interactions and provide a way to implement the non-contact compliant obstacle avoidance. However, it is noted that the multi-control point obstacle avoidance, facing multi-dynamic obstacles sharing a narrow space with the robot, sometimes is inevitable. In the future, based on the MVEEs, the contact compliant control of some parts deserve consideration. Also, further practical experiments will be needed to verify the simulation effect in the next step.