Research on Hybrid Force Control of Redundant Manipulator with Reverse Task Priority

Abstract

This paper presents the reverse priority impedance control of manipulators with reference to redundant robots of a given task. The reverse priority kinematic control of redundant manipulators is first expressed in detail. The motion in the joint space is derived following the opposite order compared with the classical task priority–based solution. Then the Cartesian impedance control is combined with the reverse priority impedance control to solve the reverse hierarchical impedance controlled, so that the Cartesian impedance behavior can be divided into the primary priority impedance control and the secondary priority impedance control. Furthermore, the secondary impedance control task will not disturb the primary impedance control task. The motion in the joint space is affected following the opposite order and working in the corresponding projection operators. The primary impedance control tasks are implemented at the end, so as to avoid the possible deformations caused by the singularities occurring in the secondary impedance control tasks. Hence, the proposed reverse priority impedance control of manipulator can achieve the desired impedance control tasks with proper hierarchy. In this paper, the simulation experiments of the manipulator will verify the proposed reverse priority control algorithm.

1. Introduction

In spite of the past several decades of intense studies, inverse kinematics remains as one of the most fundamental issues of redundant manipulator research. Researchers have developed a great deal of methods, such as the position level inverse kinematics, velocity level kinematics and acceleration level kinematics [1,2,3]. Besides this, singularity robust control of redundant manipulators is also studied, such kinematics uncertainty and dynamics uncertainty. All of these studies aim to provide better control effects of manipulator [4].

The redundant manipulator has more flexibility and has more interest from scholars. The inverse kinematics of redundant manipulators aims to solve the relationship between the variables in the operational space and the variables in the joint space. With the complete utilization of the main DOFs (degrees of freedom), the residual DOFs are utilized to singularity, obstacle avoidance, multi-task optimization and so on [5,6,7]. Taking the joint limits avoidance index for example, the seminal work focused on the potential functions in order to complete the obstacle avoidance or limits avoidance efficiently [8]. Generally speaking, the multi-task optimization was always implemented in the null-space of the main tasks, so the frame of kinematics were based on the Jacobian of the manipulator.

Unlike the above solutions, another seminal work introduced the task priority–based solution. A primary task was implemented firstly, and a secondary task was implemented in the null space of the main task. Thus, the task accuracy of the primary task would be guaranteed, then the task accuracy of secondary task would be guaranteed as soon as possible [9,10]. Oussama Kanoun (2011) generalized the task priority framework of inequality tasks for kinematic control of redundant manipulators [11], and they also deal with the issue of inequality constraints by transformed the inequality constraints on equality constraints, and the potential fields are also introduced [12,13]. These approaches establish a strict hierarchy between tasks, i.e., tasks of lower priority do not affect the performance of the highest priority task, since they are projected in the null space of the kinematic Jacobian [14]. Chiaverini (1997) proposed the singularity-robust task-priority redundancy resolution, and it is used for real-time kinematic control of robot manipulators [15]. Gianluca (2009) also gave the stability analysis for task priority inverse kinematic solver [16]. A novel practical technique is proposed to integrate both the activation and deactivation of tasks and the inequality controlled objectives, and thus the discontinuity of enabling and disabling tasks [17,18]. Recently, the reverse priority approach was proposed to eliminate the algorithmic singularity and improve the task accuracy [19,20].

The above research results are focused on the position control of manipulators in the freedom space. As for the manipulator–environment interaction control, the impedance control was proposed [21,22]. Generally speaking, the impedance control can be divided as: typical impedance control, position–force hybrid control and hybrid impedance control [23,24,25]. Moreover, the impedance control can also be divided into the Cartesian impedance control and the joint impedance control, and in the Cartesian impedance control, the end-effector force/torque sensor was utilized, and in the joint impedance control algorithms, the joint torque sensor was utilized. The usage of the impedance relationship between the force/torque and the position response was to transform the dates from the sensors into the control input.

The force control of manipulators has been researched for many years. Hogan first proposed the concept of impedance control. It was also applied to the industry area. Heinrichs gave the position-based impedance control of an industrial hydraulic manipulator, and the nonlinear proportional-integral controller was developed to achieve the accurate positioning requirements [26]. Focchi (2016) proposed the inner torque and velocity feedback loops–based robot impedance control [27]. Lastly, the velocity feedback aimed to improve the bandwidth of the torque loop without the need of a complex controller.

The impedance control is a general approach to achieve tasks in which the robot behaves as a mass–spring–damper system whose inertia–damping–stiffness can be specified arbitrarily. The impedance control aimed to give a best solution to regulate the interaction force which may be changed due to the uncertainty of the location of the contact point and the environment’s structural properties, and the impedance controllers of manipulators have been widely utilized [28,29,30,31,32].

In this article, the concept of reverse priority forced control is proposed. The research topics on the proposed reverse priority force control aims to achieve the follow goals:

(1)

The implementation of hierarchical force control in the Cartesian space.

(2)

The algorithmic singularities can be reduced.

(3)

The manipulator–environment interaction can be guaranteed.

In this paper, we are willing to give a reverse priority impedance control law from the view of robotics. The paper is organized as follows: In Section 2, the inverse kinematics solutions are expressed. The reverse task priority inverse kinematics are described in Section 3. The reverse task priority impedance control of redundant manipulator is described in Section 4. Section 5 shows the simulation results. The conclusions is presented in Section 6.

2. Preliminaries

2.1. Jacobian-Based Solution

The inverse kinematics are based on the following Jacobian-based relationship [33,34]:

$$\dot{x}=Jq,$$

(1)

where $\dot{x}\in R^m,\dot{q}\in R^n$, and $J\in R^{m\times n}$.

Considering about the least-square-based solution, the optimal issue can be listed as:

$$\underset{\dot{q}}{min}\parallel \dot{x}-J\dot{q}\parallel ,$$

(2)

The solution can be found by searching the best, that is:

$$\left(J^{T}J\right)\dot{q}=J^T\dot{x},$$

(3)

Thus, the pseudo-inverse solution of the above equation can be shown as:

$$\dot{q}={\left(J^{T}J\right)}^{-1}{J}^T\dot{x},$$

(4)

The above equation stands for the end-effector position and posture control. It is necessary to add the residual arbitrariness in the above equation to give a general expression including the null space. Hence, the total manifold can be shown as:

$$\dot{q}=J^{+}\dot{x}+\left(I-J^{+}J\right)\dot{\zeta },$$

(5)

where $\dot{\zeta }$ is the arbitrary null space vector. The additional multi-task optimization can be implemented in the null vector by the above equation.

However, the above equation ignored the ill condition of Jacobian matrix. Furthermore, the regularization equation can be modified by adding additional regularization cost such as:

$$\underset{q}{min}\parallel \dot{x}-J\dot{q}{\parallel }^2+{\parallel \dot{q}\parallel }_{\dot{\lambda }}^2,$$

(6)

where $\lambda \ge 0$ is the weighted matrix, and $\parallel \dot{q}{\parallel }_{\lambda }^2$ is the weighted norm, and $\parallel \dot{q}{\parallel }_{\lambda }^2={\dot{q}}^{\mathrm{T}}\lambda \dot{q}$.

The solution of the above equation can be shown as:

$$\dot{q}={\left(J^{\mathrm{T}}J+\lambda \right)}^{-1}{J}^{\mathrm{T}}\dot{x}+\left(I-{\left(J^{\mathrm{T}}J+\lambda \right)}^{-1}\left(J^{\mathrm{T}}J+\lambda \right)\right)\dot{\zeta },$$

(7)

As for the redundant manipulator, the null space vector stands for the gradient direction of position-dependent scalar index such as the joint limits avoidance:

$$H\left(\theta \right)=\sum \left(\frac{{\theta }_i-{\theta }_{i_{-}mid}}{{\theta }_{i_{-}\mathrm{mid}}-{\theta }_{i_{-}max}}\right).$$

(8)

2.2. The Task-Priority-Based Solution

In the above Jacobian-based solution of redundant manipulator, the optimization task are implemented in the null-space of the main task. The task hierarchy inverse kinematics is well-established on the following task hierarchy forward kinematics as:

$${\dot{x}}_i=J_i\dot{q},i=1,2,$$

(9)

where and represent task1 and task2, respectively.

The inverse kinematics of redundant manipulator that learned from expression (5) can be expressed as:

$$\dot{q}=J_i^{+}{\dot{x}}_i+\left(I-J_i^{+}{J}_i\right)\dot{\zeta },$$

(10)

Implementing task1 as the primary task, and task2 as the secondary task, the secondary impedance control task will not disturb the primary impedance control task [35,36]. That is to say, task2 is implemented in the null-space of task1. The final inverse kinematics expression of the redundant manipulator is as follows:

$$\dot{q}=J_1^{+}{\dot{x}}_{1c}+N_1{\overline{J}}_2^{+}\left({\dot{x}}_{2c}-J_2{J}_1^{+}{\dot{x}}_{1c}\right)+N_2\dot{\zeta },$$

(11)

where $N_1=\left(I-J_1^{+}{J}_1\right)$ and $N_2=N_1\left(I-{\overline{J}}_2^{+}{J}_2\right)·{\overline{J}}_2=J_2{N}_1$ are the projected matrix, which gives the available range of the secondary task to the primary task. ${\dot{x}}_{1c}$ and ${\dot{x}}_{2c}$ are the desired command velocities. Generally speaking, ${\dot{x}}_{1c}$ is the primary task, and ${\dot{x}}_{2c}$ is the secondary task.

If the two related generic tasks are dependent, then the corresponding Jacobian matrix will be singular. If the task Jacobian is singular, the corresponding task will not be satisfied. In this case, the Jacobian-related matrix will be singularity, and this is defined as algorithmic singularity. Namely, the two generic tasks are dependent if

$$\rho \left(J_1\right)+\rho \left(J_2\right)>\rho \left(\left[\begin{array}{c}{J}_1\\ J_2\end{array}\right]\right),$$

(12)

where $\rho$([ ]) is the rank of a matrix.

It is obvious that the algorithmic singularity is introduced by the task conflict between the secondary and the primary task. Moreover, the task-priority-based inverse kinematics of redundant manipulator aims to give a better control effect of the primary task; for example, let the position control direction as the primary task, and then the position control direction task accuracy will be guaranteed. In a prior research’s results, Chiaverini proposed the singularity robust solution to give an algorithmic singularity-free algorithm, and the two priority tasks are given as follows:

$$\dot{q}=J_1^{+}{\dot{x}}_1+\left(I-J_1^{+}{J}_1\right)J_2^{+}{\dot{x}}_2.$$

(13)

The algorithmic singularity is eliminated by the special mathematical derivation.

2.3. Singularity-Robust Solution

In the classical Jacobian pseudo-inverse–based solution will occur the kinematics singularity, and this is caused by the matrix under-rank. It is necessary to give the damped least squares (DLS) solution for the issue of kinematic singularity.

As for the DLS solution, the general cost function can be modified as:

$$\underset{q}{min}\parallel \dot{x}-J\dot{q}{\parallel }^2+{\eta }^2{\parallel \dot{q}\parallel }^2,$$

(14)

Hence, the singularity robust pseudo-inverse solution of the above equation can be shown as:

$$\dot{q}={\left(J^{T}J+{\eta }^{2}I\right)}^{-1}{J}^T\dot{x},$$

(15)

Let $\lambda ={\eta }^{2}I$ , and the above DLS solution is equivalent to the additional regularization solution. The scalar value balances the task accuracy and the singularity.

As for the calculation of the Jacobian matrix pseudo-inverse, we can give the singular value decomposition (SVD) form of the Jacobian:

$$J=U\Sigma V^{\mathrm{T}},$$

(16)

where $U\in R^{m\times m},V\in R^{n\times n}$ , $\Sigma \in R^{m\times n}$ and U is a unitary matrix composed by column vectors $u_i·V$ is a unitary matrix composed by column vectors $v_i·\sum$ is a block matrix with a leading $m\times m$ diagonal matrix containing the singular values ${\sigma }_i\ge 0\mathrm{of}J$ in decreasing order following by $n-m$ zero columns.

$$J^{+}=V{\Sigma }^{-1}{U}^{\mathrm{T}}=\sum _{i=1}^r\frac{1}{{\sigma }_i}{v}_i{u}_i^{\mathrm{T}},$$

(17)

where $r\le m$ is the rank of J .

As for the kinematic singularity, referring to the SVD used for computing the needed pseudo-inverse, the large generated joint velocities are due to the smallest singular value rapidly approaching 0. The approach is followed as:

$$\frac{1}{{\sigma }_i}=\frac{{\sigma }_i}{{\sigma }_i^2+{\lambda }_0^2},$$

(18)

The factor ${\lambda }_0$ will affect the singularity. The higher the ${\lambda }_0$ is, the more damped are the joint velocity near singularity. Besides this, there are different methods of defining a variable damping factor. Then we have:

$${\lambda }_0=\left\{\begin{array}{c}0{\sigma }_m\ge \delta \\ \left(1-{\left(\frac{{\sigma }_m}{\delta }\right)}^2\right){\lambda }_{max}^2\mathrm{otherwise}.\end{array}\right.$$

(19)

From the above equation, we can see that the parameter $\delta >0$ monitors the smallest singular value.

3. Reverse Priority Control of Manipulator

The classical Jacobian pseudo-inverse–based solution can not give a task hierarchical control. That is to say, all tasks are in the same priority, and then DLS-based singularity-robust solution will sacrifice the accuracy of all tasks. Moreover, the classical task-priority-based solution will cause algorithmic singularity. So it is necessary to introduce a novel inverse kinematic solver for handling multi-robotic-task with hierarchy that is unaffected by algorithmic singularity, and the task accuracy will also be guaranteed in case of kinematic singularity. Thus, in the following part, we give the reverse priority approach to solve the kinematic issue of redundant manipulator.

3.1. Reverse Priority Control of Redundant Manipulator with Multiple Tasks

To elaborate the reverse priority kinematic control of redundant manipulator, it is necessary to introduce the concept of reverse priority projection matrix ${\breve{P}}_{k+1}$. This matrix includes the $l-k-1$ tasks of the lowest priority that are not dependent from the k-th tasks. Hence, we have:

$${\breve{P}}_{k+1}=I-{\breve{J}}_{k+1}{J}_{k+1},$$

(20)

$${\breve{J}}_{k+1}={\left(J_{k+1\mid k}^{\mathrm{T}}{J}_{k+2\mid k}^{\mathrm{T}}\cdots J_{1\mid k}^{\mathrm{T}}\right)}^{\mathrm{T}},$$

(21)

where $J_{i\mid j}$ is the Jacobian associated to all components of the i-th tasks that are linearly independent from the j-th task.

The reverse priority recursive formula is as follows:

$${\dot{q}}_k={\dot{q}}_{k+1}+{\left(J_k{\breve{P}}_{k+1}\right)}^{+}\left({\dot{x}}_k-J_k{\dot{q}}_{k+1}\right),$$

(22)

In the above derivation, $k=l,l-1,\cdots ,1$. The initialization value is ${\dot{q}}_{l+1}=0$.

To give a general calculation form of the linearly independent Jacobians $J_{i|j}$ , we define the reverse augmented Jacobian matrix as:

$$J_k^{RP}={\left(J_k^T{J}_{k+1}^T\cdots J_1^T\right)}^T=\left(\begin{array}{c}{J}_k\\ J_{k+1}^{RP},\end{array}\right)$$

(23)

Then we have:

$$\prod _{k+1}{J}_{k=1}^{RP}=\left(\begin{array}{c}{\breve{J}}_{k+1}\hfill \\ {\overline{J}}_{k+1},\hfill \end{array}\right)$$

(24)

where ${\overline{J}}_{k+1}$ denotes the row of $J_{k=1}^{RP}$ .

Furthermore the pseudo-inverse of can be partitioned as:

$$J_k^{R{P}_{+}}=\left(T_k,H_k\right),$$

(25)

$$H_k=J_k^{RP+}-T_k{J}_k{J}_{k+1}^{RP+},$$

(26)

where $T_k$ denotes the augmentation of $J_{k+1}^{RP+}$ .

The final reverse priority projector can be written as:

$${\breve{P}}_{k+1}=I-\left({\breve{J}}_{k+1}^{+}0\right)\left(\begin{array}{c}{\breve{J}}_{k+1}\hfill \\ {\overline{J}}_{k+1}\hfill \end{array}\right)=I-\left(I-T_k{T}_k^{+}\right)H_k{J}_{k+1}^{RP},$$

(27)

Then we have the expression of the pseudo-inverse calculation:

$$\begin{array}{c}\hfill {\left(J_k{\breve{P}}_{k+1}\right)}^{+}\&={\left(J_k\left(P_k^{RP}+T_k{T}_k^{+}\right)\right)}^{+}\\ \hfill ={\left(J_k{P}_k^{RP}+J_k{T}_k{T}_k^{+}\right)}^{+}\\ \hfill ={\left(J_k{T}_k{T}_k^{+}\right)}^{+}\\ \hfill =T_k{\left(J_k{T}_k\right)}^{+}\end{array}$$

(28)

In summary, the reverse priority calculation can be followed as:

$${\dot{q}}_k^{RP}={\dot{q}}_{k+1}^{RP}+T_k{\left(J_k{T}_k\right)}^{+}\left(\dot{x}-J_k{\dot{q}}_{k+1}^{RP}\right).$$

(29)

3.2. Reverse Priority Control of Redundant Manipulator with Two Tasks

As for the 6-DOF manipulator or 7-DOF redundant manipulator, there are no abundant DOFs to achieve many hierarchical tasks. It is necessary to give the two-task priority control. That is to say, the classical kinematic control of the manipulator is just the primary task and the secondary task.

From the basic mathematical derivation, we can see:

$$\dot{q}=J_2^{+}{\dot{x}}_2+{\left(J_1{\breve{P}}_2\right)}^{+}\left({\dot{x}}_1-J_1{J}_2^{+}{\dot{x}}_2\right),$$

(30)

The above formula is significantly different from the previous expression (11), but the algorithm framework is similar. In the above equation, ${\dot{x}}_2$ is the secondary task, and ${\dot{x}}_1$ is the primary task. It is also obvious that the secondary task is calculated firstly. The primary task ${\dot{x}}_2$ is implemented in the specified null-space of the primary task. The kernel point of the reverse priority is the calculation of the projection matrix ${\breve{P}}_2$. The expression of ${\breve{P}}_2$ can be as follows:

$${\breve{P}}_2=I-{\breve{J}}_2{J}_2,$$

(31)

Drawing on previous similar derivations from (23)–(29), we can also obtain the concise expression:

$$\dot{q}=J_2^{+}{\dot{x}}_2+T_1{\left(J_1{T}_1\right)}^{+}\left({\dot{x}}_1-J_1{J}_2^{+}{\dot{x}}_2\right).$$

(32)

4. Reverse Priority Impedance Control

The Cartesian impedance control can be divided into three cases. The first one is the Cartesian force tracking control, and this case can be applied to the human–manipulator interaction behavior, like the handshake, leg follower and so on. The secondary one is the Cartesian impedance control, and this case can be applied to the target operation control. The manipulator can track a desired path, and prevent itself from being hit by external objects. The last case is the Cartesian hybrid impedance control, and this case can be applied to a special operation or capturing situations.

4.1. The Reverse Priority Force Control of Manipulator

The dynamics of manipulator in the force control space can be written as follows:

$$M\left(X\right)\ddot{X}+H(X,\dot{X})=F-F_e,$$

(33)

where X is the position in the Cartesian space, $M\left(X\right)$ is the inertia matrix, $H(X,\dot{X})$ is the nonlinear force, F is the input control force, and is the contact force.

Furthermore, the input joint torque can be obtained by the Jacobian matrix–based transformation:

$$\tau =J^T\left(q\right)F,$$

(34)

The desired motion equation of the manipulator in the force control space can be defined as follows:

$$M_d\ddot{X}+B_d\dot{X}-F_d=-F_e,$$

(35)

where $M_d$ and $B_d$ are the inertia and damping matrix. $F_d$ is the commanded force and $F_e$ is the contact force. The scheme map of force control is shown in Figure 1:

The relationship between the environment and the manipulator response can be written as:

$$M_e\ddot{X}+B_e\dot{X}+K_{e}X=F_e,$$

(36)

The combination of the above two equations can be followed as:

$$\left(M_d+M_e\right)\ddot{X}+\left(B_d+B_e\right)\dot{X}+K_{e}X=F_d.$$

(37)

From the above equation, we can see that if the $M_e$ ,$B_e$ and $K_e$ are known, the adjustment of $M_d$ and $B_d$ will influence the system response.

The force control makes it so that the manipulator can interact with the environments or human beings. Moreover, in some situations, it is not necessary to implement the force control in all directions, or it is not necessary to guarantee the force control in all directions, that is to say, sometimes, we only want to guarantee the force tracking control accuracy in some directions.

For example, if the manipulator interacted with a planer, it is only necessary to keep the accurate force tracking control in the vertical direction, and the other direction does not need accurate force tracking control. In other cases, the position–direction force control is more important than the pose direction force control. Thus, it is necessary to provide a hierarchy force control of the manipulator. That is to say, the novel framework of the hierarchy force control is necessary.

From the above equations we can obtain the desired hierarchy force control relationship as follows:

$$\begin{array}{c}{M}_{d1}{\ddot{X}}_1+B_{d1}{\dot{X}}_1-F_{d1}=-F_{e1},\end{array}$$

(38)

$$\begin{array}{c}{M}_{d2}{\ddot{X}}_2+B_{d2}{\dot{X}}_2-F_{d2}=-F_{e2}.\end{array}$$

(39)

The integral formula of these two equations can be written as:

$$\begin{array}{c}{\dot{X}}_{1c}=\int M_{d1}^{-1}\left(-F_{e1}+F_{d1}-B_{d1}{\dot{X}}_1\right)dt,\end{array}$$

(40)

$$\begin{array}{c}{\dot{X}}_{2c}=\int M_{d2}^{-1}\left(-F_{e2}+F_{d2}-B_{d2}{\dot{X}}_2\right)dt.\end{array}$$

(41)

If the manipulator end-effector can track the desired Cartesian velocity as ${\dot{X}}_{1c}$ and ${\dot{X}}_{2c}$, then the accurate force control of manipulator can be implemented. The relationship between the Cartesian velocity and the joint velocity should learn from the reverse priority control. Hence, the reverse priority control of manipulator can be obtained as:

$$\begin{array}{c}\dot{q}=J_2^{+}\int M_{d2}^{-1}\left(-F_{e2}+F_{d2}-B_{d2}{\dot{X}}_2\right)dt\hfill \\ +{\left(J_1{\breve{P}}_2\right)}^{+}\left(\begin{array}{c}\int M_{d1}^{-1}\left(-F_{e1}+F_{d1}-B_{d1}{\dot{X}}_1\right)dt\hfill \\ -J_1{J}_2^{+}\int M_{d2}^{-1}\left(-F_{e2}+F_{d2}-B_{d2}{\dot{X}}_2\right)dt.\hfill \end{array}\right)\hfill \end{array}$$

(42)

The desired joint velocity as in the above equation will guarantee the force control of the manipulator. It is worth mentioning that the above force control law is just velocity level control law, and it is dependent on the inner-velocity-loop control. If the inner position control has a good effect, then the accurate force control will be implemented. As the inner-velocity-loop control can achieve a low frequency position tracking, the outer-force loop can also achieve a low frequency force tracking.

4.2. The Reverse Priority Impedance Control of Manipulator

When the manipulator implements the force control, the manipulator acts as the initiator to some degree, that is to say, the manipulator has already been prepared for responding to the external environment. If the manipulator is working as the impedance control model, the manipulator passively responds to external forces. The following Figure 2 shows the dynamics of this behavior.

The corresponding impedance relationship between the external force and the joint acceleration can be shown as

$$\begin{array}{c}{M}_{d1}\left({\ddot{X}}_1-{\ddot{X}}_1^d\right)+B_{d1}\left({\dot{X}}_1-{\dot{X}}_1^d\right)+K_{d1}\left(X_1-X_1^d\right)=-F_{e1},\end{array}$$

(43)

$$\begin{array}{c}{M}_{d2}\left({\ddot{X}}_2-{\ddot{X}}_2^d\right)+B_{d2}\left({\dot{X}}_2-{\dot{X}}_2^d\right)+K_{d2}\left(X_2-X_2^d\right)=-F_{e2},\end{array}$$

(44)

The reference velocity can be shown as:

$${\dot{X}}_{1ref}=\int \left(M_{d1}^{-1}\left(\begin{array}{c}-B_{d1}\left({\dot{X}}_1-{\dot{X}}_1^d\right)-\\ K_{d1}\left(X_1-X_1^d\right)-F_{e1}\end{array}\right)+{\ddot{X}}_1^d\right)dt,$$

(45)

$${\dot{X}}_{2ref}=\int \left(M_{d2}^{-1}\left(\begin{array}{c}-B_{d2}\left({\dot{X}}_2-{\dot{X}}_2^d\right)-\\ K_{d2}\left(X_2-X_2^d\right)-F_{e2}\end{array}\right)+{\ddot{X}}_2^d\right)dt,$$

(46)

The reverse priority impedance control of manipulator can be obtained as:

$$\begin{array}{c}\dot{q}=J_2^{+}\int \left(M_{d2}^{-1}\left(\begin{array}{c}-B_{d2}\left({\dot{X}}_2-{\dot{X}}_2^d\right)-K_{d2}\left(X_2-X_2^d\right)-F_{e2}\hfill \end{array}\right)+{\ddot{X}}_2^d\right)dt\hfill \\ +{\left(J_1{\breve{P}}_2\right)}^{+}\left(\begin{array}{c}\int \left(M_{d1}^{-1}\left(\begin{array}{c}-B_{d1}\left({\dot{X}}_1-{\dot{X}}_1^d\right)-K_{d1}\left(X_1-X_1^d\right)-F_{e1}\hfill \end{array}\right)+{\ddot{X}}_1^d\right)dt\hfill \\ -\int \left(M_{d2}^{-1}\left(\begin{array}{c}-B_{d2}\left({\dot{X}}_2-{\dot{X}}_2^d\right)-K_{d2}\left(X_2-X_2^d\right)-F_{e2}\hfill \end{array}\right)+{\ddot{X}}_2^d\right)dt\hfill \end{array}\right).\hfill \end{array}$$

(47)

The above desired joint velocity can guarantee the desired impedance behavior of the manipulator in this case. Moreover, the velocity-based impedance control law will be easily adopted.

4.3. The Reverse Priority Hybrid Impedance Control of Manipulator

The hybrid impedance application is the combination of the above two, that is to say, the Cartesian task can be divided into two cases: the first is the position control subspace, and the impedance control is implemented in this subspace; the second is the force control subspace, and the force control is implemented in this subspace. Hence, a selection matrix is selected. The relationship between the external force and the position response is as follows:

$$\begin{array}{c}{M}_{1d}\mathsf{\Delta }{\ddot{X}}_1+B_{1d}\mathsf{\Delta }{\dot{X}}_1+K_{1d}{S}_1\mathsf{\Delta }{X}_1=\left(I-S_1\right)F_{1d}-F_{1e},\hfill \end{array}$$

(48)

$$\begin{array}{c}{M}_{2d}\mathsf{\Delta }{\ddot{X}}_2+B_{2d}\mathsf{\Delta }{\dot{X}}_2+K_{2d}{S}_2\mathsf{\Delta }{X}_2=\left(I-S_2\right)F_{2d}-F_{2e}.\hfill \end{array}$$

(49)

So the simplified form of the desired velocity can be shown as:

$$\begin{array}{c}{\dot{X}}_{1ref}=\int \left(\mathsf{\Delta }{\ddot{X}}_1+{\ddot{X}}_1^d\right)dt,\hfill \end{array}$$

(50)

$$\begin{array}{c}{\dot{X}}_{2ref}=\int \left(\mathsf{\Delta }{\ddot{X}}_2+{\ddot{X}}_2^d\right)dt.\hfill \end{array}$$

(51)

Then we have the reverse priority-based solution:

$$\begin{array}{cc}\hfill \dot{q}& =J_2^{+}{\dot{x}}_{2ref}+{\left(J_1{\breve{P}}_2\right)}^{+}\left({\dot{x}}_{1ref}-J_1{J}_2^{+}{\dot{x}}_{2ref}\right)\hfill \\ \hfill & =J_2^{+}{\dot{x}}_{2ref}+T_1{\left(J_1{T}_1\right)}^{+}{\left({\dot{x}}_{1ref}-J_1{J}_2^{+}{\dot{x}}_{2ref}\right)}^{\prime }.\hfill \end{array}$$

(52)

The scheme map is shown in Figure 3:

Regarding n-hierarchy tasks, the corresponding impedance control tasks also belong to the n-hierarchy framework, so the general framework of the velocity-level reverse priority impedance control of the manipulator can be written as follows:

$$\begin{array}{c}{\dot{q}}_k=J_{k+1}^{+}{\dot{x}}_{k+1_{-}ref}+{\left(J_k{\breve{P}}_{k+1}\right)}^{+}\left({\dot{x}}_{k+1_{-}ref}-J_k{J}_{k+1}^{+}{\dot{x}}_{k+1_{-}ref}\right)\hfill \\ =J_{k+1}^{+}{\dot{x}}_{k+1}{}_{-1}{}^{k+}+T_k{\left(J_k{T}_k\right)}^{+}\left({\dot{x}}_{k+1}\mathrm{ref}-J_k{J}_{k+1}^{+}{\dot{x}}_{k+1_{-}ref}\right).\hfill \end{array}$$

(53)

The main work of the mathematical derivation of the reverse priority impedance control law in the above expressions aims to expand the reverse priority calculation of the position control space into that of the force control space. Moreover, the reverse priority impedance control framework derived in this paper will also be applied to the underwater manipulator, space manipulator and mobile robot. In addition, the velocity-level impedance control law will also be suitable for the real controller of the manipulator. The reverse priority impedance control gives an algorithmic singularity free impedance control, and the primary impedance control task will be guaranteed. The strict reverse priority will also give a better control effect of the manipulator.

5. Simulation

To verify the proposed control laws, it is necessary to build the simulation system to give a task verification. The corresponding manipulator system is 7-DOF sawyer manipulator. The total mass of this manipulator is 11 kg, and its payload is 4 kg.

Table 1. The DH table of evaluation of sawyer.

	d	a	alpha	qlim
Link1	0.317	−0.081	−pi/2	[−pi,pi]
Link2	−0.1925	0	pi/2	[−pi,pi]
Link3	0.4	0	−pi/2	[−pi,pi]
Link4	−0.1685	0	pi/2	[−pi,pi]
Link5	0.4	0	−pi/2	[−pi,pi]
Link6	−0.1363	0	pi/2	[−pi,pi]
Link7	0.13375	0	pi/2	[−pi,pi]

gives the DH parameters, and the manipulator sketch map is shown in Figure 4.

5.1. Example 1

In this example, the comparison between the reverse priority forced control and the Jacobian-based force control is made. The effect of the force tracking down the proposed reverse priority force control is also given. Assume the desired force was chosen as follows:

$$F_d=30+5sin(t/\mathrm{pi})\left(N\right).$$

(54)

In the proposed reverse priority force control of manipulator, assume the position control direction X is primary force control task, and the Y and Z directions and all the pose control directions are the secondary force control tasks. The desired impedance control parameters were set as $M_d=diag\left(151515808080\right)$, and the damping are $B_d=diag\left(150150150280280280\right)$. For the convenience of explanation, we take X and Y directions as an example. Figure 5 shows the RP force control. And the classical force control is showed in Figure 6.

From the above Figure 5 and Figure 6, we can see that the manipulator can track the desired alternating force. It is worth mentioning that the alternating force supposed in this paper is a low frequency signal. This takes into account the bandwidth of the PID controller. Besides this, the Fx in the Figure 5 is obviously better than that in the Figure 6. This is because the Fx is the primary forced control task in the reverse priority force control algorithm.

5.2. Example 2

In this example, we will give the experiments about approaching and contacting a stiff wall. The control period and control parameters are as in the first experiment. Moreover, the aluminum alloy wall was used as the environment object. The stiffness is about ${10}^5\mathrm{N}/\mathrm{m}$, and the wall is assumed in the X direction. The whole process is an approaching-contact-stability process. Approaching object is an important behavior of automatic tasks with manipulators. The corresponding control results are shown in Figure 7 and Figure 8 (case 1 is the classical impedance control, case 2 is the reverse priority impedance control, and the X direction is the primary force control direction).

It can be seen from the above curve that the robot arm can achieve close approaching operation well via the proposed reverse priority impedance control law. The curves in case 2 overwhelm the curves in case 3. This demonstrates that the reverse priority impedance control indeed overwhelmed the classical impedance control algorithm. The final contact force in X direction is 30 N; the manipulator contacted the wall with a stable force.

6. Conclusions

This article has introduced the reverse priority force control algorithm to handle multi-force-control-task with strict priority. The novel method considers the primary priority force control task after the calculation of the secondary force control task. Moreover, the reverse control framework guaranteed that there are no algorithmic singularities. The task hierarchy in the force controller is also implemented. The primary force control task will first be guaranteed, and the secondary force control task can be implemented as soon as possible. Furthermore, the task hierarchy will also improve the task accuracy in the Cartesian force controller. The strict mathematical derivation of the reverse priority force control is obtained. The proposed reverse priority force control of the manipulator gives the algorithmic singularity free force control, and the completeness and validity of the theory are validated in the simulation.