An Obstacle Avoidance and Trajectory Tracking Algorithm for Redundant Manipulator End

An obstacle avoidance algorithm was suggested in this study to handle the redundant manipulator’s end trajectory planning problem, with the accuracy of the end trajectory guaranteed to be within the projected error. To accomplish this goal, the calculation method of vector pseudo distance is used instead of the traditional Euclidean distance to express the minimum proximity between the robot arm and the obstacle. This method can express the constraint conditions between the obstacle and the robot arm more clearly and improve the obstacle avoidance efficiency. The virtual repulsion force is generated to make the end of the manipulator far away from the obstacle through the relationship between the velocity vector and the angle vector of the end motion, and the end recovers to the desired trajectory quickly and stably after avoiding the obstacle through the adjustment of the tangent velocity function. The error adjustment coefficient is intended to communicate back the actual trajectory of the end obstacle avoidance to the tracking function in real time, and to adaptively change the velocity and acceleration of the end-executor to reduce the end trajectory tracking error. The simulation results show that when an envelope body obstacle appears unexpectedly on the end executor’s expected mission locus, if the expected distance between the end-effector r and the obstacle is greater than or equal to 0.02m, the end of the actuator can avoid the obstacle and return to the expected track quickly and smoothly.

SECTION I. Introduction

With each passing day, the robot industry evolves, with redundant robotic arms gaining more degrees of freedom and flexibility to improve the performance of diverse jobs. Obstacle avoidance is a vital capability not only for mobile robots, but also for robots working in unstructured environments. So far, it has emerged as a popular study topic in the realm of robot technology. Obstacle avoidance of redundant robotic arms, on the other hand, cannot assure safety and adaptability in complicated scenarios [1]–​[3]. Scholars both at home and abroad have conducted extensive research to address these issues.

The manipulator’s obstacle avoidance algorithm is separated into two parts: offline planning and online adjustment [4]. The offline planning algorithm plans a collision-free path from the beginning to the completion of the work to solve the obstacle avoidance problem. The online adjustment algorithm typically combines control with an optimization performance index function, employs sensors to obtain obstacle information in the path, and the GPM master-slave task conversion obstacle avoidance method is the longest method to realize real-time feedback obstacle avoidance [5], [6]. The joint position will jitter, the terminal speed will fluctuate, and the trajectory tracking error will increase when the manipulator conducts the master-slave job transformation. HAN et al. achieved obstacle avoidance by determining the minimal distance between the robot connecting rod and the obstacle envelope in real time using Kinect V2 [7]. CHEN et al. analyzed the obstacle and redundant robot connecting rod using the ellipsoid nearest to its volume, and presented a virtual force for obstacle avoidance based on the shortest distance between them [8]. WANG et al. simplified robot obstacle avoidance into obstacle avoidance between line segment and ball [9], [10] using the artificial potential field approach. To build the coordinated control strategy of the dual-arm robot, Liu et al. introduced the sliding mode controller based on the hyperbolic tangent function as the switch function. The proposed control technique can output the needed internal forces consistently and provide a high-precision track tracking effect [11]. A weighted minimal norm method is proposed in reference [12] to avoid collisions between redundant manipulators and moving impediments. A new approach for kinematic analysis and singularity analysis for a 7-DOF redundant manipulator with three consecutive parallel axes was proposed [13]. Based on the speed space optimization technique in a dynamic environment, an effective and real-time obstacle avoidance system with smooth and reliable pathways is obtained [14]. Scholars in the literature [15], [16] used repulsive force vectors to achieve active full-arm collision avoidance of a 7-DOF redundant manipulator. A trajectory tracking control approach based on FFSR with a general non-singular pre-defined time terminal sliding mode is proposed in [17], [18]. When there is continuous interference, the tracking error can converge to any small zero neighborhood within the time limit. Because the suggested trajectory planning approach includes pose feedback, it can be used with a trajectory tracking controller in the control process to boost the control effect even more. Based on polynomial functions, this work provides a collision avoidance strategy for redundant robots with fixed obstacles [19], [20]. The proposed technique allows for smooth trajectories based on the derivative continuity criteria in trajectory curve transitions. When the robot moves away from the imminent collision, an adaptive extended Jacobian matrix is proposed to tackle the inverse kinematics problem. In the literature [21], [22], the author advocated using pseudo distance instead of traditional Euclidean distance to monitor the distance between obstacles and robotic arms more qualitatively and reduce collision risk.

This work proposes an obstacle avoidance and path tracking method for the end of a redundant manipulator. As the distance between the manipulator and the monitoring arm, the new pseudo-distance is used instead of the usual Euclidian distance. The distance model between the obstacles is built by solving the problem where the obstacles arrive unexpectedly on the desired trajectory with the hyper quadratic function. Simultaneously, the error adjustment coefficient is used to feedback the final obstacle avoidance of the actual trajectory real-time tracking function, as well as to adapt the speed and acceleration of the end-effector, in order to solve the problem of low trajectory tracking precision during the robot obstacle avoidance process.

SECTION II. The Creation of an Obstacle Avoidance Model and the Computation of Preudo Distance

A. The Establishment of Obstacle Models

The minimum Euclidean distance between the obstruction and the manipulator is a regularly used collision-free distance requirement. Because it is difficult to solve and analyze the minimal distance using a standard mathematical expression, the Euclidean distance expression becomes more sophisticated when different shapes of the obstacle are considered. However, in the real test, the obstacle avoidance control algorithm does not need to be precisely computed; it merely needs to calculate that the manipulator does not collide with the envelope in the working space where the obstacle is located. In this paper, pseudo-distance is introduced as a discriminant index of collision-free distance to replace Euclidean distance, analytic functions are used to represent multi-shape obstacles, a coordinate system is established with the geometric center of obstacles as the origin, and the unified analytic equation of hyperquadric surface is used to describe obstacles as: $$\begin{align*} \text {S}\left ({{s_{a},q_{c}} }\right)\!=\!\left ({{\frac {x-x_{0}}{h_{1}}} }\right)^{2m}+\left ({{\frac {y-y_{0}}{h_{2}}} }\right)^{2n}+\left ({{\frac {z-z_{0}}{h_{3}}} }\right)^{2p}\!-\!1 \\ {}\tag{1}\end{align*}$$

In Formula (1), ${S}\left ({{s_{a},q_{c}} }\right)$ represents the pseudo-distance function expression, $s_{a} =(h_{1},h_{2},h_{3},m,n,p)$ , Where $(h_{1},h_{2},h_{3})$ and $(m,n,p)$ are the shape parameters and volume parameters of the obstacle hypersurface fitting respectively, $q_{c} (x,y,z)$ is the coordinate position of any point in space, $(x_{0},y_{0},z_{0})$ is the coordinate of the center point of the hyperquadric in the obstacle coordinate system $\left \{{ {{\boldsymbol{o}}_{obs} -{\boldsymbol{x}}_{obs} {\boldsymbol{y}}_{obs} {\boldsymbol{z}}_{obs}} }\right \}$ .

Regular geometry is used to suit the shapes of some irregular barriers in the environment in order to ease the estimation of their shapes. In this study, ideal geometric spheres are utilized to describe irregularly shaped barriers, and the pseudo-distance of a hyperquadric surface from space points to obstacles is expressed as: $$\begin{align*} S_{p} (x,y,z)=\left ({{\frac {x-x_{0}}{R_{s}}} }\right)^{2}+\left ({{\frac {y-y_{0}}{R_{s}}} }\right)^{2}+\left ({{\frac {z-z_{0}}{R_{s}}} }\right)^{2}-1 \\ {}\tag{2}\end{align*}$$

In Equations (2), $S_{p} (\cdot)$ is the obstacle’s pseudo distance in coordinate space, $R_{s}$ is the safety sphere radius, $R_{s} =r_{obs} +r_{i},r_{obs}$ is the specified radius of the obstacle, and $r_{i}$ is the maximum radius of the fitting rod cylinder. It is beneficial to detect the collision distance during movement and improve the accuracy of obstacle avoidance results by modifying the previous condition in which the manipulator’s rod is viewed as a straight line with no thickness.

At any point $q_{c} (x,y,z)$ in the task space, the corresponding pseudo distance is calculated based on equation (2), The three relations with the hyperquadric fitting the shape of the obstacle are shown in Figure 1. If $S_{p} (x,y,z)=0$ , that is, the connecting rod of the manipulator just contacts the surface of the obstacle and is in the critical state between collision and non-collision, and the manipulator is in an unsafe state; If $S_{p} (x,y,z)< 0$ , there is a collision between the manipulator and the obstacle, the manipulator is in an unsafe state; If $S_{p} (x,y,z)>0$ , there is no collision between the manipulator and the obstacle, the manipulator is in a safe state.

FIGURE 1.

Pseudo distance diagram of space point to hyperquadric surface.

B. Minimum Pseudo Distance Point Calculation Method

Since complex formula calculation can be avoided through vector method, equation (2) above is expressed as vector $$\begin{equation*} {\textbf {X}}^{\text {T}}{\textbf {AX}}+{\textbf {B}}^{\text {T}}{\textbf {X}}+{\textbf {C}}={\textbf {0}}\tag{3}\end{equation*}$$ where ${\textbf {X}}=[\text {x},\text {y},\text {z}]^{\text {T}}$ $$\begin{align*} {\textbf {A}}=&\textrm {diag}\left [{ {{\begin{array}{ccc} {\frac {1}{a^{2}}} & {\frac {1}{b^{2}}} & {\frac {1}{c^{2}}} \\ \end{array}}} }\right]\!, \\ {\textbf {B}}=&\left [{ {{\begin{array}{ccc} {-\frac {x_{0}}{a^{2}}} & {-\frac {y_{0}}{b^{2}}} & {-\frac {z_{0}}{c^{2}}} \\ \end{array}}} }\right]\!, \\ {\textbf {C}}=&\frac {x_{0}^{2}}{a^{2}}+\frac {y_{0}^{2}}{b^{2}}+\frac {z_{0}^{2} }{c^{2}}-1,\end{align*}$$

In the $\left \{{{{\boldsymbol{o}}_{obs} -{\boldsymbol{x}}_{obs} {\boldsymbol{y}}_{obs} {\boldsymbol{z}}_{obs}} }\right \}$ coordinate system, any point on the rod is expressed in vector form as: $$\begin{equation*} {\textbf {M}}={\textbf {N}}+\lambda {\textbf {U}}\tag{4}\end{equation*}$$

• ${\textbf {M}}$ is the vector from ${\boldsymbol{o}}_{obs}$ to $q_{c}$ ;

• ${\textbf {N}}$ is the vector from ${\boldsymbol{o}}_{obs}$ to $A_{i}$ ;

• ${\textbf {U}}$ is the vector from ${\boldsymbol{o}}_{obs}$ to $B_{i}$ ; $\lambda$ is a constant $\left ({{0\le \lambda \le 1} }\right)$ ;

That is, the pseudo distance between D and the barrier envelope is expressed as: $$\begin{align*} S_{q_{c}}=&{\textbf {X}}_{q_{c}}^{\textrm {T}} {\textbf {A}}X_{q_{c} }^{\textrm {T}} +{\textbf {B}}^{\textrm {T}}X_{q_{c}} +{\textbf {C}} \\=&({\textbf {N}}+\lambda {\textbf {U}}){\textbf {A}}({\textbf {N}}+\lambda { \textbf {U}})+{\textbf {B}}^{\textrm {T}}({\textbf {N}}+\lambda {\textbf {U}})+{\textbf {C}} \\=&\lambda ^{2}+\lambda {\textbf {U}}^{\textrm {T}}\boldsymbol {\Delta }_{1} +{\textbf {W}}_{{\textbf {1}}}\end{align*}$$

Formula (1) $\boldsymbol {\Delta }_{{\textbf {1}}} =2{\textbf {AN}}+{\textbf {B}}$ ; ${\textbf {W}}_{{\textbf {1}}} ={\textbf {NAN}}+{\textbf {BN}}+{\textbf {C}}$ .

The above formula is the functional expression of $\lambda$ , denoted as $S_{q_{c}} (\lambda)$ , and the minimum pseudo-distance can be obtained by finding its extreme value, as $$\begin{align*} \frac {\textrm {d}\left ({{S_{q_{c}} (\lambda)} }\right)}{\textrm {d}\lambda }=&2\lambda {\textbf {U}}^{\textrm {T}}{\textbf {QU}}+{\textbf {U}}^{\textrm {T}}\boldsymbol {\Delta }={\textbf {0}}\tag{5}\\ \lambda _{q_{c\min }}=&\lambda =-\frac {1}{2}\frac {{\textbf {U}}^{\textrm {T}}\boldsymbol {\Delta }_{{\textbf {1}}}}{{\textbf {U}}^{\textrm {T}}{\textbf {QU}}}\tag{6}\end{align*}$$

As illustrated in Figure 2, endpoints $A_{i}$ , $B_{i}$ , and $q_{c}$ are chosen as the manipulator’s connecting rod’s minimal pseudo-distance points, which is expressed as $$\begin{align*} S_{\min,i}=&S_{link,i} (A_{i} B_{i}) \\=&\min \left \{{{{\begin{array}{ccc} {S_{link,i} (A_{i})} & {S_{link,i} (q_{c})} & {S_{link,i} (B_{i})} \\ \end{array}}} }\right \}\tag{7}\end{align*}$$

FIGURE 2.

Schematic diagram of minimum pseudo distance point calculation.

By type (6) $$\begin{equation*} {\boldsymbol{q}}_{c} ={\boldsymbol{N}}+\lambda _{q_{c\min }} {\boldsymbol{U}}\tag{8}\end{equation*}$$

When $\lambda _{q_{c\min }} \le 0$ , take $\lambda _{q_{c\min }} =0$ , then $q_{c}$ is located at point $A_{i},S_{\min,i} =S_{\min,i} (A_{i})$ ;

When $\lambda _{q_{c\min }} \ge 1$ , take $\lambda _{q_{c\min }} =1$ , then $q_{c}$ is located at point $B_{i},S_{\min,i} =S_{\min,i} (A_{i})$ ;

When $0< \lambda _{q_{c\min }} < 1$ , then point $q_{c}$ is the closest point, $S_{\min,i} =S_{\min,i} (q_{c})$ .

The minimal pseudo distance between all of the manipulator’s connections and the obstruction for the multi-link manipulator is $$\begin{align*} S_{q_{c}} =\min \left \{{{{\begin{array}{cccc} {S_{\min,1}} & {S_{\min,2}} & \cdots & {S_{\min,n}} \\ \end{array}}} }\right \}\tag{9}\end{align*}$$

The goal of the minimal pseudo-distance value of the manipulator’s obstacle avoidance is expressed as a function of the joint vector Q, namely $$\begin{equation*} S_{q_{c}} ({\boldsymbol{q}})={\textbf {X}}_{\textrm {m}}^{\textrm {T}} ({\boldsymbol{q}}){\textbf {AX}}_{\textrm {m}}^{\textrm {T}} ({\boldsymbol{q}})+{\textbf {B}}^{T}{\textbf {X}}_{\textrm {m}} ({\boldsymbol{q}})+{\textbf {C}}\tag{10}\end{equation*}$$

To ensure the safety of the obstacle avoidance process, the minimum pseudo distance is always greater than the obstacle envelope’s preset threshold: $$\begin{equation*} -S_{q_{c}} ({\boldsymbol{q}})+d_{pm}^{2} \le 0\tag{11}\end{equation*}$$

SECTION III. Terminal Trajectory Planning for Redundant Manipulators

A. Traditional Obstacle Avoidance

Obstacles appear in the operating environment of the manipulator, and collisions may occur between obstacles and the connecting rod of the manipulator because the distance is too close, as shown in Figure 3.

FIGURE 3.

Obstacle appears on desired trajectory.

FIGURE 4.

A diagram of $\theta$ in a virtual force.

The obstacle avoidance motion of the manipulator based on redundancy has two basic requirements, namely, meeting the requirements of terminal trajectory and avoiding obstacles as: $$\begin{align*} \dot {x}=&J\dot {q}\tag{12}\\ \dot {x_{0}}=&J_{0} \dot {q}\tag{13}\end{align*}$$

In formula (12), $\dot {x}$ is the terminal velocity vector, Jacobian matrix $J$ is the linear mapping of the joint angular velocity vector $\dot {q} \in R^{n}$ in $\dot {x} \in R^{m}$ ($\dot {q} \in R^{n}$ , $\dot {x} \in R^{m}$ , $J\in R^{n\times m}$ , n, m is the dimension of joint space and operation space). In Formula (13), $\dot {x_{0}}$ is the speed of avoiding the obstacle, and $J_{0}$ is the Jacobian matrix corresponding to the projection point of the minimum Euclidean distance of the obstacle on the connecting rod.

In robotics, for the null-space obstacle avoidance problem of redundant robots, the inverse kinematics solution is: $$\begin{equation*} \dot {q} =J^{+}\dot {x} +\left ({{J_{0} N} }\right)^{+}\left ({{\dot {x_{0}} -J_{0} J^{+}\dot {x}}}\right)\tag{14}\end{equation*}$$ where, $N=I-J^{+}J,J^{+}$ is the pseudo inverse of the matrix, $J^{+}=J^{T}\left ({{JJ^{T}} }\right)^{-1}$ .

Physical significance of Equation (14): The first ensures the joint motion required for the desired end-effector velocity; The second item realizes the projection point to avoid the obstacle movement. Therefore, based on the Equation (15) can not only realize manipulator end estimated tracking and planning of redundant manipulator null space obstacle avoidance route, the traditional algorithm of obstacle avoidance in most environment configuration is valid, but it also has some limitations, such as when obstacles appear on the expected at the end of the track or too close to the route, it will lead to manipulator swinging around the obstacle, If the obstacle cannot be avoided in a safe way, the end track tracking motion will conflict with the null-space obstacle avoidance motion, that is, the algorithm fails.

B. Improved Obstacle Avoidance Algorithm for Manipulator End

For the above situation, this paper intends to use virtual gravitational repulsion force to plan the end trajectory of the manipulator, that is, a novel virtual repulsion force is generated between the obstacle envelope body and the end executor, and the end executor trajectory is modified to make it appear on the desired trajectory when the obstacle is completed.

In the traditional artificial potential field method, the repulsive potential field is generated around the obstacle and only acts in the local task space. The repulsive force function is defined as $$\begin{align*} U_{re} (q_{c})=\begin{cases} {\frac {1}{2}\eta \left ({{\frac {1}{\rho ({\textbf {x}})}-\frac {1}{\rho _{0} }} }\right)^{2}} & {\rho ({\textbf {x}})\le \rho _{0}} \\ 0 & {\rho ({\textbf {x}})>\rho _{0}} \\ \end{cases}\tag{15}\end{align*}$$ where $q_{c} \left ({{x,y,z} }\right)$ is the position of the robot, $\eta$ represents the proportionality coefficient, $\rho _{0}$ represents the radius of the scope of action of the repulsive force field, and $S_{p} (x,y,z)$ is the Euclidean distance between the robot and the obstacle.

By applying negative gradient in the potential field, the repulsive force acting on the robot can be obtained as follows: $$\begin{align*} F_{re} ({\textbf {X}})=&-\text {grad}\left [{ {U_{re} (X)} }\right] \\=&\begin{cases} {\eta \left ({{\frac {1}{\rho ({\textbf {x}})}-\frac {1}{\rho _{0}}} }\right)\frac {1}{\rho ({\textbf {x}})^{2}}\frac {\partial \rho ({\textbf {x}})}{\partial ({ \textbf {x}})}} & {\rho ({\textbf {x}})\le \rho _{0}} \\ 0 & {\rho ({\textbf {x}})>\rho _{0}} \end{cases} \\ {}\tag{16}\end{align*}$$

However, the traditional potential field method only plans the distance between the end executor and the obstacle, it ignores the influence of the speed and movement direction of the end executor. Therefore, it is not enough to only consider the distance in obstacle avoidance movement, which may lead to the possibility of collision. To solve these problems, the repulsive potential field function above is improved as follows: $$\begin{align*} U_{dyn} \left ({{q_{c}} }\right)=\begin{cases} {(-\cos \theta)^{\beta }\frac {\|({\textbf {v}})\|}{S_{p} ({\textbf {x}})}} & {\frac {\pi }{2}< \theta \le \pi } \\ 0 & {0\le \theta \le \frac {\pi }{2}} \\ \end{cases}\tag{17}\end{align*}$$

The dynamic repulsive force produced by the negative gradient of the repulsive potential field function is: $$\begin{align*}&\hspace {-1.4pc}F_{dyn} ({\textbf {x}},{\textbf {v}}) \\=&-\nabla _{x} U_{\textrm {dyn}} ({\textbf {x}}, {\textbf {v}}) \\=&\begin{cases} (-\cos \theta)^{\beta -1}\frac {{\textbf {v}}}{\left \|{ {S_{p} (x)} }\right \|} & \\ \quad \left ({{\beta \nabla _{x} \cos \theta \!-\!\frac {\cos \theta }{\left \|{ {S_{p} (x)} }\right \|}\nabla _{x} \left \|{ {S_{p} (x)} }\right \|} }\right) & {\frac {\pi }{2}\!< \!\theta \!\le \! \pi } \\ 0 & {0\!\le \! \theta \!\le \! \frac {\pi }{2}} \\ \end{cases}\!\! \\ \tag{18}\\ \theta=&\arccos \left ({{\frac {{\textbf {v}}\cdot {\textbf {x}}}{\|{\textbf {v}}\|\| {\textbf {x}}\|}} }\right)\tag{19}\end{align*}$$ where $S_{p} ({\textbf {x}})$ is the pseudo distance between the end of the manipulator and the obstacle, $\beta$ is a constant that can adjust the influence of $\theta$ . As shown in Figure 3, the angle between the current velocity vector$\nu$ and the position of the end executor $q_{c}$ (relative to the position of the obstacle) can be calculated by using a Formula (18). When $\theta$ between $\left(\frac {\pi }{2},\pi\right]$ , it means that the robot is approaching the obstacle and the dynamic repulsion field starts to work; When the value $\theta$ is within $\left(0,\frac {\pi }{2}\right]$ , the robot is away from the obstacle and the dynamic repulsive field should not work.

The repulsive force generated by dynamic state field can improve the obstacle avoidance behavior of the robot considering three factors: velocity, position and direction. The repulsive force is inversely proportional to the distance $s_{p}$ to the obstacle and proportional to the velocity $\boldsymbol {\nu }$ , and is also related to the angle between the velocity vector and the direction vector to the obstacle when the velocity $\boldsymbol {\nu }$ is null or $s_{p}$ greater than the threshold, and zero if the angle $\theta$ is less than $\frac {\pi }{2}$ (The end executor is away from the obstacle).

Based on the virtual repulsive force generated by formula (9) above, the terminal velocity $\dot {x_{0}}$ of the manipulator is modified, and the resulting velocity adjustment quantity is $\Delta \dot {x_{adj}}$ , and the modified velocity trajectory is: $$\begin{equation*} \dot {x_{c}} = \dot {x_{0}} +\Delta \dot {x_{adj}}\tag{20}\end{equation*}$$

That is, the inverse kinematics solution of Equation (14) is modified as $$\begin{equation*} \dot {q} =J^{+}\dot {x} +\left ({{J_{0} N} }\right)^{+}\left ({{\dot {x_{c}} -J_{0} J^{+}\dot {x_{d}}} }\right)\tag{21}\end{equation*}$$

The second $\dot {x}$ is the avoidance speed related to obstacle avoidance, which is related to the nearest point on the connecting rod of the manipulator to the obstacle. $\Delta \dot {x_{adj}}$ is amount of speed adjustment for the virtual repulsive force generated when obstacles appear in the end of the actuator on the desired trajectory, the closest point for manipulator end-effector position, change the course of speed at the end, the end executor constantly to be near the obstacles, the decrease of the minimum pseudo range, At the same time, $\theta$ between $\left(\frac {\pi }{2},\pi\right]$ , the virtual repulsion force increases continuously, resulting in a large velocity correction, which acts on the end executor to avoid obstacles on the desired trajectory.

C. Terminal Trajectory Correction Algorithm

In the last section of this paper, the traditional obstacle avoidance algorithm is improved, formula (21) above is the inverse solution of the redundant degree of the optimized manipulator to realize obstacle avoidance at the end of the manipulator. In order to improve the tracking accuracy of the trajectory of the end, the control method of correcting the error between the expected task trajectory of the end and the actual motion trajectory is adopted below. In speed control, replace terminal speed $\dot {x_{ce}}$ with modified trajectory speed $\dot {x}$ , defined: $$\begin{align*} \dot {x_{ce}}=&\dot {x_{d}} +K_{e} e\tag{22}\\ \dot {x_{d}}=&\begin{cases} V_{\max } \tanh (4d_{pm} -8\left \|{ {S_{p}} }\right \|)\frac {S_{p}}{\left \|{ {S_{p}} }\right \|} & S_{p} >d_{pm} \\ 0 & S_{p} \le d_{pm} \\ \end{cases} \\ \tag{23}\\ e=&x_{d} -x\tag{24}\end{align*}$$ where, $\dot {x_{ce}}$ is the correction speed, $K_{e}$ is the positive definite gain matrix of$m\times m$ , $x_{d}$ is the desired trajectory, and the corresponding expected velocity is $\dot {x_{d}}$ ; $x$ is the actual trajectory, and the corresponding velocity is $\dot {x}$ , $e$ is the trajectory tracking error, which is the difference between the expected task trajectory $\dot {x_{d}}$ and the actual motion trajectory $\dot {x}$ , and $V_{\max }$ represents the maximum velocity scalar of the manipulator in the task space.

The inverse solution of the redundant degree of the manipulator after optimization is: $$\begin{equation*} \dot {q} =J^{+}\left ({{\dot {x_{d}} +K_{e} e} }\right)+\left ({{J_{0} N} }\right)^{+}\left ({\dot {{x_{c}} -J_{0} J^{+}\dot {x_{d}}} }\right)\tag{25}\end{equation*}$$

Proper selection of positive definite matrix can reduce the terminal trajectory error, an adaptive matrix positive definite coefficient matrix is designed $K_{e}$ , $$\begin{align*} K_{e} =\left [{ {{\begin{array}{ccc} {k_{1} \left \|{ {e_{x}} }\right \|} & & \\ & {k_{2} \left \|{ {e_{y}} }\right \|} & \\ & & {k_{3} \left \|{ {e_{z}} }\right \|} \\ \end{array}}} }\right]\tag{26}\end{align*}$$

$k_{1}$ , $k_{2}$ and $k_{3}$ are the gain coefficients that reduce errors on $x$ , $y$ and $z$ axes respectively. The $e_{x},e_{y},e_{z}$ are adjusted adaptively to compensate the errors generated by track tracking during obstacle avoidance and ensure that the terminal still has high precision track tracking.

To sum up, based on the proposed obstacle avoidance and trajectory tracking algorithm at the end of redundant mechanical arm (equation (25)), the mechanical arm can not only solve the obstacle appearing on the expected trajectory of the task quickly and effectively, but also recover to the expected trajectory smoothly after obstacle avoidance.

SECTION IV. Simulation Results and Analysis

In order to verify the effectiveness and feasibility of the obstacle avoidance algorithm proposed in this paper, this method is applied to the LBRiiwa14R280, a 7-DOF mechanical arm model, as shown in Figure 5. The robot is composed of shoulder-elbow-wrist (S-R-S) structure and consists of seven rotating joints. The algorithm proposed in this paper only performs null-space obstacle avoidance, end obstacle avoidance and end track tracking of iiWA14 manipulator, so the attitude of the end can be ignored and only its position can be considered. The simulation experiments were carried out in matlab2020A software to verify the superiority and effectiveness of the proposed algorithm.

FIGURE 5.

Kuka LBR iiwa-14 7-Dof redundant manipulator.

The simulation conditions are pre-set as follows: The plane is taken as the simulation research object. In order to observe whether the mechanical arm can proceed stably in the whole movement process and whether the end can accurately achieve the trajectory tracking, the end is expected to move with an arc curve.

End expected trajectory: starting point position and end position are (0.3,0.3,0.6), Center of the circle:$O_{rid}$ (0.2,0.3, 0.6), The expected trajectory is $$\begin{align*} x_{d} =\begin{cases} {x=O_{rid,x} +R_{rid} \cos \theta } \\ {y=O_{rid,y} +R_{rid} \sin \theta } \\ {z=0.6} \end{cases} \quad R_{rid} =0.1m\tag{27}\end{align*}$$

Initial joint angle of manipulator: $$\begin{align*} \dot {q}=&\Biggl ({-0.1639,0.7263,1.2648,2.0117,-0.8201,} \\&{-\,2.0944,-1.4991 }\Biggr)\end{align*}$$

Equivalent cylinder radius of manipulator link: $r_{i} =0.03m$

Preset obstacle environment: The experimental simulation is carried out in the presence of multiple obstacles to ensure the effectiveness of obstacle avoidance algorithm. The radius of three spherical envelope bodies with the same size is preset for obstacles, $r_{obs} =0.03m$ , and the spherical coordinates are respectively in the reference frame of the manipulator base:

• Obstacle Ball 1 (in null space): $O_{obs1} =(0.3,0,0.4)$

• Obstacle Ball 2 (in null space): $O_{obs2} =(0,0.1,0.6)$

• Obstacle Ball 3 (in null space): $O_{obs3} =(0.1,0.3,0.6)$

Pseudo distance threshold: $d_{pm} =3$ , The corresponding Euclidean distance is $0.06m$ .

The simulation results of the classic obstacle avoidance algorithm are shown in Figure 6–​9. The total running time of the simulation movement is T=3s. Figure 6 depicts the entire obstacle avoidance movement process when the mechanical arm performs the typical obstacle avoidance algorithm. Figure 7 shows that when T1=0–0.5s and T2=2.7s–3s, the mechanical arm reaches the warning area of the obstacle envelope, performs null-space obstacle avoidance, and successfully avoids obstacles 1 and 2. The end of the manipulator, however, enters the risky area of the obstacle envelope and collides with obstacle 3 at T3=0.9–2.3s and T5=1s–1.7s, indicating that the typical obstacle avoidance algorithm fails in this environment and the obstacle avoidance goal cannot be met. Figure 8 illustrates that the tracking error distance of the mechanical arm’s end track in the standard obstacle avoidance algorithm is small only at the speed of the beginning and ending points. As the obstacle avoidance movement progresses, the error becomes higher and larger, and the collision error reaches the maximum at T3=0.9–2.3s instant and the tracking accuracy is average. At the same time, the joint angle diagram in Figure 9 shows that obstacles appear on the expected trajectory of the manipulator’s end, which does not meet the traditional algorithm’s expectation requirements, resulting in small amplitude chattering and discontinuity in the space movement of the manipulator joints, and the operation is not smooth enough.

FIGURE 6.

Motion path planning of manipulator based on traditional algorithm.

FIGURE 7.

Minimum pseudo distance under traditional obstacle avoidance algorithm.

FIGURE 8.

The tracking error of terminal trajectory under traditional obstacle avoidance algorithm.

FIGURE 9.

The tracking error of terminal trajectory under traditional obstacle avoidance algorithm.

The obstacle avoidance algorithm proposed in this paper will be adopted below. The coefficient in the main diagonal of the error adjustment coefficient matrix $K_{e}$ is $$\begin{equation*} {\boldsymbol{K}}_{e} =diag[400\left \|{ {e_{x}} }\right \|,400\left \|{ {e_{y}} }\right \|,1]\tag{28}\end{equation*}$$

In this paper, only the $xoy$ axis planes are studied, as $k_{3} \left \|{ {e_{z}} }\right \|=1$ . The error adjustment coefficient matrix $K_{e}$ is in dynamic change in the whole obstacle avoidance movement process. According to the error generated by the end position, the joint velocity is adjusted adaptively, and the tracking accuracy is adjusted to achieve the purpose of reducing the error.

Figure 10–​14 depicts the obstacle avoidance effect and end track tracking error of a mechanical arm using the obstacle avoidance algorithm proposed in this paper. Figure 10 depicts the changes in the spatial configuration of each rod during the mechanical arm’s obstacle avoidance. The end follows the expected trajectory, successfully avoids all predetermined obstacles, and completes the obstacle avoidance movement at the joint and the end of the mechanical arm. Figure 11 shows that in the first stage, the minimum pseudo distance $S_{p}$ between the mechanical arm and the obstacles is greater than the pseudo distance $d_{pm} =3$ threshold corresponding Euclidean distance (0.06m) at simulation time T=0–0.9s, the obstacles have no effect on mechanical arm terminal trajectory tracking precision, and the motion continues along the desired trajectory. Figure 12 shows that the actual error generated by the expected trajectory of the end is less than 0.02m, and the tracking accuracy is higher than the traditional obstacle avoidance algorithm.

FIGURE 10.

Based on the algorithm presented in this paper, the process of obstacle avoidance and trajectory tracking at the end of the manipulator is presented.

FIGURE 11.

Minimum pseudo distance variation of manipulator.

FIGURE 12.

The end track tracking error based on the algorithm in this paper.

FIGURE 13.

Virtual repulsion during obstacle avoidance movement.

FIGURE 14.

Joint Angle change based on obstacle avoidance algorithm in this paper.

When T=0.9s in the second stage, the manipulator’s end enters the dangerous area of obstacle 3, and the minimum pseudo distance $S_{p}$ decreases. Figure 13 shows that the virtual force increases significantly in the X- axis and Y-axis directions, and high-precision tracking of the end track is sacrificed quickly for obstacle avoidance measures, promoting the end to stay away from the obstacle, which is consistent with the actual situation. Simultaneously, it is demonstrated that the proposed algorithm has greater sensitivity and effectiveness, as well as greater practicability, than the traditional obstacle avoidance algorithm.

When the simulation time T7=2.3–3s gradually increases in the third stage, the end-effector leaves the dangerous area of obstacles and the obstacle avoidance motion begins to weaken. The end track’s tracking error gradually decreases until it converges to the desired track and returns to the starting point. As a result, the end obstacle avoidance method proposed in this paper realizes the robot end’s rapid obstacle avoidance task.

Simultaneously, the diagram of joint Angle changes of the mechanical arm in Figure 14 shows that there is no shaking phenomenon throughout the movement process, and the joint position is stable, preventing the mutation of each joint velocity induced by obstacle avoidance movement. Furthermore, the overall fluctuation range of the robot’s joint velocity is smaller than that of the classic obstacle avoidance algorithm, implying that the proposed method, to some extent, avoids the problems of joint singularity and velocity saturation. During the trajectory tracking process, the adaptive error adjustment matrix $K_{e}$ continuously changes itself based on the terminal’s real-time tracking situation, and the adjustment results are fed back to the manipulator online in real time. It also indicates that the error adjustment matrix $K_{e}$ was properly chosen and that the system’s adaptive feedback result is good, both of which play an essential part in the continuous and stable changing of joint angle.

The tracking quality of the common obstacle avoidance manipulator’s end track is currently low, and the position inaccuracy grows with the sampling time step. When the sample step dt=0.1s, the end error is frequently more than 0.05m. When compared to the traditional trajectory tracking technique, the greatest error at the end of the proposed algorithm under the identical conditions is $0.02m$ in the y direction, as shown in Figure 12. The precision of the z axis tracking is also improved, as is the mechanical arm terminal trajectory tracking effect. The movement is smooth and continuous, and simulation is utilized to test the algorithm’s performance.

After completing four groups of experiments, this paper asserts that the proposed manipulator end obstacle avoidance algorithm can avoid obstacles in the task quickly and efficiently on the desired trajectory, that in the recovery part our algorithm can smoothly return to the desired trajectory, and that the trajectory tracking accuracy at the end is within 0.02 m. It also demonstrated that using pseudo range as the minimum distance monitoring index is superior to that of Euclidean distance. The actual trajectory of the end obstacle avoidance is fed back to the tracking function in real time in the error adjustment coefficient $K_{e}$ , and the speed and acceleration of the end executor are adjusted adaptively to ensure the obstacle avoidance of the manipulator’s connecting rod in the case of high-precision trajectory tracking. Figure 15 depicts the manipulator’s path planning process based on the obstacle avoidance algorithm proposed in this paper.

FIGURE 15.

Flow chart of end obstacle avoidance algorithm.

SECTION V. Conclusion

In this paper, the redundant manipulator obstacle avoidance method was investigated, in light of the traditional mechanical arm obstacle avoidance algorithm’s inability to overcome obstacles appearing at the end of the desired trajectory, can adjust local movement problem of joint space, is proposed based on a type of obstacles and the virtual force at the end of the mechanical arm obstacle avoidance method, combined with the movement of the joint space to adjust, Complete the end obstructed path.

1. In this paper, a vector calculation method of pseudo distance is used to indicate mechanical arm and obstacles between the minimum approach degree, in the form of a simple and primitive unified modeling of various obstacles, which replaced the traditional Euclidean distance, more qualitatively expressed the obstacles between manipulator and the constraint conditions, and finally the experimental simulation is proven feasible.

2. The disadvantage of the traditional algorithm only considering distance in solving the end obstacle avoidance problem is changed. The dynamic repulsive field can generate smooth virtual force, add an obstacle avoidance velocity increment to the end-effector, and solve the problem of the obstacle appearing on the desired trajectory of the end effector via the relationship between the velocity vector and the Angle vector of the end motion.

3. An adaptive error adjustment matrix $K_{e}$ was designed to feed the end-actual effector’s trajectory to the tracking function in real time, and the end-velocity effector’s and acceleration were adaptively adjusted to reduce the tracking error of the end-effector trajectory.

The effectiveness and superiority of the proposed algorithm are confirmed by comparing simulation experiments, which can realize high-precision tracking of the terminal trajectory while completing the multi-obstacle avoidance, and the tracking effect of the terminal trajectory is good. The static obstacle avoidance problem of a redundant manipulator is primarily addressed in this paper. Later research focuses on the dynamic obstacle avoidance problem of a manipulator in an unstructured environment.