A Control Method of Mobile Manipulator Based on Null-Space Task Planning and Hybrid Control

Abstract

The mobile manipulator is a floating base structure with wide space operability. An integrated mechanical device for mobile operation is formed through the organic combination of the mobile platform and multi-axis manipulator. This paper presents a general kinematic modeling method for mobile manipulators and gives the relevant derivation of the dynamic model. Secondly, the null-space composition of the mobile manipulator is analyzed, the task space is divided, and a variety of task-switching criteria are designed. Finally, a hybrid control model combining dynamic feedback and synovial control based on dynamic parameter identification is designed, and stability proof is given. The theoretical method is also verified by the experimental platform. The proposed method can effectively improve the control accuracy of the mobile manipulator, and the hybrid control method can effectively control the output torque to reach the ideal state.

1. Introduction

A mobile manipulator is a kind of robot that can naturally interact with the working environment, people, and other robots, independently adapt to the complex dynamic environment and work together. Due to the functions of grasping, moving, detection and recognition, the mobile manipulator can perfectly realize the application scenario that one robot replaces multiple workers or multiple robots. A mobile manipulator is a very complex multi-input and multi-output structure. It has the nonlinear dynamic characteristics of being time-varying and with strong coupling. Coupled with the characteristics of sliding friction uncertainty and accidental external force interference in its dynamic model, it is a typical nonlinear uncertain system. The mobile manipulator is a complex redundant system with a floating base. The kinematics of the platform increases the workspace of the manipulator and the complexity of the system. Its kinematics and dynamics analysis methods are very different from the traditional manipulator. A mobile manipulator is a strongly coupled nonlinear system. Generally, the mobile platform has heavy mass and slow dynamic response, while the manipulator has a light mass and fast dynamic response. Due to the combination of a mobile platform and manipulator, the compound mobile robot has great spatial redundancy. For the same task, it can be realized not only by moving the mobile platform or manipulator alone but also by moving the platform and manipulator at the same time. Therefore, there are two different research schools. The first one is to separate the motion planning of the manipulator and the motion planning of the mobile platform; it includes the hybrid force/position control [1,2,3], impedance control [4], and the combination of deep learning [5] and optimal control [6,7,8,9].

At present, there are task planning methods based on the null-space for whole-body control, and the main characteristics are as follows: different subtasks can be decomposed according to different meanings, and priority can be divided according to the importance; when necessary, low-priority tasks should not affect the execution of high-priority tasks. Related work: Dietrich A et al. proposed a torque control solution based on null-space prediction, extended the weighting matrix from the classical operation space method, and demonstrated that there was an infinite number of weighting matrices that could make the mechanism obtain dynamic consistency [10]; Li et al. proposed a whole control framework inspired by the leg-type bio-robot and a mobile manipulation control method based on the non-holonomic constraint of speed control based on task priority [11]. In the above research, the mobile robot focuses on the robustness of the control system and the innovation of the mechanical structure. It obtains higher dexterity through the design method of bionic or configuration topology synthesis. The control perspective pays more attention to its own stability and walking gait control, but there is little research on systemic coordination control, especially in task coordination. In order to solve the non-cooperative problem of the mobile manipulator, Heng Zhang et al. proposed a motion planning method with task priority coordination, which effectively solved the cooperative control of the mobile manipulator in grasping [12]; a coordinated motion planner for autonomous mobile manipulator based on capability graph is also proposed. Under the condition of a given end-effector path, the platform path of the mobile manipulator is planned by online querying CM [13]. In addition, some methods based on model predictive control are used to realize the overall dynamic control of the mobile manipulator. For example, J.-P. Sleiman et al. have proposed a whole-body planning framework based on an MPC control approach that unifies dynamic motion and maneuvering tasks by formulating a single multi-contact optimal control problem [14]; it uses visual information to avoid obstacles and uses tactile perception to control the interaction force [15].

In addition to considering its motion state, the control process of the mobile manipulator needs multiple considerations. The motion control of the manipulator itself belongs to high accuracy control and the control method of this part can be designed according to the existing methods, and good results can be achieved. The omnidirectional mobile platform relies on the friction between the wheel and the ground to obtain the driving force, which needs to be affected by the environment. At the same time, the sliding caused by insufficient friction will affect the accuracy of the platform motion. Therefore, the motion control of this part is rough, and the accuracy control is not high. Taking into account the constraints of the mobile manipulator task, the overall terminal position constraints need to balance the differences between the two parts; thus, it is necessary to find a control method that can consider the platform and the manipulator as a whole. Sliding mode control (SMC) is a special type of variable structure control (VSC); thus, it is also called a sliding mode variable structure control. It is a control method that is widely used and developed in recent years. Sliding mode control is essentially a nonlinear control, that is, the control structure changes with time. The sliding mode variable structure control is a commonly used analysis method in nonlinear systems. Its significant advantage is that it has strong robustness to uncertain parameters and external disturbances. Therefore, it has been widely used in aerospace, robot control, and chemical control. The stable sliding mode control algorithm is naturally invariant to the system parameter perturbation and external disturbance, which makes the system have ideal robustness, but the discontinuous control near the sliding mode surface makes the stable system state have inherent chattering. Widely used in the field of engineering control, Bijan Bandyopadhyay [16] proposes a robust stabilization of the linear time-invariant system achieved by rejecting the effect of matched and bounded input disturbances using the proposed state-dependent intermittent sliding mode control (SDI-SMC). Yong Chen et al. [17] proposed a novel hybrid second-order sliding-mode control (HSSMC) strategy for the permanent magnet synchronous motor speed control. Hongqi Li] et al [18. developed a robust sliding-mode nonlinear predictive controller for brain-controlled robots with enhanced performance, safety, and robustness. Therefore, the mobile manipulator based on the sliding mode controller has good robust control, but the friction of the mobile manipulator itself and the inertia superposition of the mobile platform will lead to a weak output torque, and the output of the robust control will be weakened to a certain extent. Therefore, it is necessary to eliminate this part of the influence by means of torque compensation. Dynamics feedforward is an effective output torque compensation method, which is often used in industrial robots and CNC machining centers. This method can overcome the influence of the inaccuracy of the analytical inverse dynamic model on the control performance, and effectively improve the control accuracy of the robot. Wu, J [19,20] proposes an iterative learning method to accurately design the industrial feedforward controller and compensate for the external uncertain dynamic load of the robot. Mei, ZW et al. [21] proposed a novel feedforward control method of an elastic-joint robot based on a hybrid inverse dynamic model. Emil Madsen [22] presents a novel extension of the Generalized Maxwell-Slip friction model to describe said phenomena in a combined framework. Dynamic feedforward can well compensate for the internal interference caused by system reasons. The torque output after compensation can better complete some control tasks such as trajectory tracking and achieve ideal results.

The contributions of this paper are:

1. The motion state and modeling method of the mobile manipulator is analyzed, and the kinematic expression and trajectory calculation method of the mobile manipulator are obtained. The task planning method of the null space is constructed, the task space of the mobile manipulator is divided, and several task-switching criteria are designed.

2. A hybrid control method using the dynamic feedforward and sliding film controller is designed, its stability is proved, and the output control law is derived.

The structure of this paper is divided into the following parts: In Section 2, we model the typical mobile manipulator system and obtain the basic kinematics and dynamics equation. In Section 3, the null-space layering of the mobile manipulator is constructed to obtain the mathematical expression of the task space, and several state judgment criteria are introduced. Section 4, introduces the method of the dynamic parameter identification of mobile manipulators, and proposes a hybrid control method using dynamic feedforward and sliding film control. In Section 5, the experimental verification of the proposed method is presented. Section 6 summarizes the methods and conclusions proposed in this paper.

2. Kinematic and Dynamics Model for the Mobile Manipulator

2.1. Preliminary Work

The mecanum wheel is subjected to the gravity of the platform. Without considering the deformation, the contact between the stick and the ground is a point contact. It is assumed that the contact point is the geometric center of the center stack of the mecanum wheel. The friction force of the ground acting on rollers decomposes into a rolling friction force in a stick coordinate system and static friction. Promoting the roll rotation is an invalid movement. We drive the roller to move relative to the ground, and the roller is fixed on the wheel axle of the wheel, thus driving the wheel to move along the roller axis. Therefore, changing the angle between the roller axis and the wheel axis can change the actual (force) motion direction of the wheel.

Before the model analysis, the following conditions should be assumed:

(a)

In the process of the Omni-directional drive platform movement, the wheel fully contacts with the ground and rotates on empty;

(b)

The density distribution of the omnidirectional drive platform is uniform, the resultant force point of the platform gravity coincides with the geometric center of the vertical projection, and the load distribution on the four wheels is equal.

(c)

Performance of wheel drive motor consistent with power output curve.

The motion causes of the wheel are analyzed from the perspective of itself. According to the motion decomposition, the relationship between the speed of the platform center and the driving speed of the mecanum wheel can be deduced, as shown in Figure 1.

$$\left[\begin{array}{c}{\dot{\theta }}_A\\ {\dot{\theta }}_B\\ {\dot{\theta }}_C\\ {\dot{\theta }}_D\end{array}\right]=\frac{1}{R}\left[\begin{array}{ccc}1& -1& -\left(W+L\right)\\ 1& 1& -\left(W+L\right)\\ 1& -1& W+L\\ 1& 1& W+L\end{array}\right]\left[\begin{array}{c}{V}_X\\ V_Y\\ {\omega }_c\end{array}\right]\hspace{1em}\hspace{1em}\hspace{1em}\left[\begin{array}{c}{V}_X\\ V_Y\\ {\omega }_c\end{array}\right]=\frac{R}{4}\left[\begin{array}{cccc}1& 1& 1& 1\\ -1& 1& -1& 1\\ \frac{-1}{W+L}& \frac{-1}{W+L}& \frac{1}{W+L}& \frac{1}{W+L}\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_A\\ {\dot{\theta }}_B\\ {\dot{\theta }}_C\\ {\dot{\theta }}_D\end{array}\right]$$

(1)

Constructing the world coordinate system and the omnidirectional mobile platform, the geometric center of the omnidirectional mobile platform is the origin of the coordinate system, the front of the platform is in the $X$ direction, and the vertical to the $X$ axis is in the $Y$ direction. The speed of the whole platform in the $X$ axis direction is $v_x$, the speed in the $Y$ axis direction is $v_{\mathrm{y}}$, and the overall rotation angular speed is ${\omega }_c$. Set the pulse period triggering wheel rotation as $\Delta t$, and project the displacement of the platform in $\Delta t$ into the global coordinate system. The projection length in the $X$ and $Y$ directions is the displacement of the platform in the world coordinate system.

$$\begin{array}{l}\Delta X_x=v_x\Delta t\cos {\theta }_t\\ \Delta Y_y=v_x\Delta t\sin {\theta }_t\\ {\theta }_t={\theta }_{t-\Delta t}+{\omega }_c\Delta t\end{array}\hspace{1em}\hspace{1em}\hspace{1em}\begin{array}{l}\Delta X_y=-v_y\Delta t\sin {\theta }_t\\ \Delta Y_y=v_y\Delta t\cos {\theta }_t\\ {\theta }_t={\theta }_{t-\Delta t}+{\omega }_c\Delta t\end{array}\hspace{1em}\hspace{1em}\hspace{1em}\begin{array}{l}{X}_t=X_{t-\Delta t}+v_x\Delta t\cos {\theta }_t-v_y\Delta t\sin {\theta }_t\\ Y_t=Y_{t-\Delta t}+v_x\Delta t\sin {\theta }_t+v_y\Delta t\cos {\theta }_t\\ {\theta }_t={\theta }_{t-\Delta t}+{\omega }_c\Delta t\end{array}$$

(2)

Referring to Equations (1) and (2), we can obtain the position of the mobile manipulator based on dead-reckoning [23]. The calculated position of the track is obtained by accumulating the change of $\Delta t$ time from the position at the previous time. Therefore, the calculated result has a cumulative error, so external calibration is required to achieve accurate positioning.

$${}_{}{}^{W}T{}_M{=\mathrm{D}}_X\left(dx\right)·{\mathrm{D}}_Y\left(dy\right)·R_Z\left(\Delta \phi \right)=\left[\begin{array}{cccc}\cos \left({\displaystyle \underset{0}{\overset{\Delta t}{\int }}{\omega }_{c}dt}\right)& \sin \left({\displaystyle \underset{0}{\overset{\Delta t}{\int }}{\omega }_{c}dt}\right)& 0& {\displaystyle \underset{0}{\overset{\Delta t}{\int }}{v}_{x}dt}\\ -\sin \left({\displaystyle \underset{0}{\overset{\Delta t}{\int }}{\omega }_{c}dt}\right)& \cos \left({\displaystyle \underset{0}{\overset{\Delta t}{\int }}{\omega }_{c}dt}\right)& 0& {\displaystyle \underset{0}{\overset{\Delta t}{\int }}{v}_{y}dt}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(3)

2.2. Kinematic Model

The mobile manipulator of n degrees of freedom can quickly realize the movement in space. The kinematics model of this part is obtained by the MDH method. The specific construction form is shown in Figure 2. This part of the manipulator base is fixedly connected with the mobile platform, and the absolute value encoder is used to measure the angle at the joint.

$${}^{i-1}T_i=A_i=\mathrm{Rot}\left(x,{\alpha }_{i-1}\right)\times \mathrm{Trans}\left(a_{i-1},0,0\right)\times \mathrm{Rot}\left(z,{\theta }_i\right)\times \mathrm{Trans}\left(0,0,d_i\right)$$

(4)

$$A_i=\left[\begin{array}{cccc}\cos {\theta }_i& -\sin {\theta }_i& 0& a_{i-1}\\ \sin {\theta }_i\cos {\alpha }_{i-1}& \cos {\theta }_i\cos {\alpha }_{i-1}& -\sin {\alpha }_{i-1}& -d_i\sin {\alpha }_{i-1}\\ \sin {\theta }_i\sin {\alpha }_{i-1}& \cos {\theta }_i\sin {\alpha }_{i-1}& \cos {\alpha }_{i-1}& d_i\cos {\alpha }_{i-1}\\ 0& 0& 0& 1\end{array}\right]$$

(5)

As shown in Figure 2, assuming the world coordinate system $O_W^{}$, the platform coordinate system $O_M^{}$, the base coordinate system $O_R^{}$ of the manipulator, and the end coordinate system $O_T^{}$ of the manipulator, the conversion relationship between multiple coordinate systems is unified by means of a homogeneous matrix transformation.

The homogeneous transformation matrix between the adjacent joints of the manipulator can be obtained by the DH method. The homogeneous transformation matrix of the end effector coordinate $O_T^{}$ relative to the base coordinate $O_R^{}$ of the manipulator is shown in Equation (6):

$${}^{R}T_T={}^{R}T_1{}^{1}T_2{}^{2}T_3{}^{3}T_4{}^{4}T_5\dots {}^{n}T_T$$

(6)

Considering the movement of the base in space, the homogeneous transformation matrix of the end effector coordinate $O_T^{}$ relative to the world coordinate system $O_W^{}$ can be obtained as Equation (7):

$${}^{W}T_T={}^{W}T_M{}^{M}T_R{}^{R}T_T$$

(7)

In Equation (7), ${}^{W}T_T$ is the mapping of the joint space $\left(d_x,d_y,{\theta }_M,d_1,{\theta }_2,{\theta }_3,{\theta }_4\right)$ to the end effector operation space $\left(X_T,Y_T,Z_T,R_{XT},R_{YT},R_{ZT}\right)$, which is the overall forward kinematics model of the robot.

2.3. Dynamics Model

Based on the Newton-Euler method, the dynamic model of the manipulator is established, and the interaction between the mobile platform and the manipulator is solved. Then, the dynamic model of the mobile platform is established based on the Lagrange second-type equation. Finally, the mobile platform–manipulator interaction is used as a bridge to establish the overall dynamic model.

Based on the Newton-Euler method, the dynamic equation of the manipulator is established, and Equation (8) is obtained:

$$\tau =H\left(q\right)\ddot{q}+C\left(q,\dot{q}\right)\dot{q}+G\left(q\right)$$

(8)

According to the Lagrange method, the dynamic equation of the mobile platform is obtained as Equation (9):

$$\left[\begin{array}{c}{T}_{M1}\\ T_{M2}\\ T_{M3}\\ T_{M4}\end{array}\right]=\left[\begin{array}{cccc}A& B& C& D\\ B& A& D& C\\ C& D& A& B\\ D& C& B& A\end{array}\right]\left[\begin{array}{c}{\dot{\omega }}_1\\ {\dot{\omega }}_2\\ {\dot{\omega }}_3\\ {\dot{\omega }}_4\end{array}\right]+\left[\begin{array}{cccc}{f}_1& 0& 0& 0\\ 0& f_2& 0& 0\\ 0& 0& f_3& 0\\ 0& 0& 0& f_4\end{array}\right]\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\\ {\omega }_4\end{array}\right]+\mu R\cdot \left[\begin{array}{c}{F}_{N1}\\ F_{N2}\\ F_{N3}\\ F_{N4}\end{array}\right]+\frac{R{F}_{rd}}{4}I$$

(9)

In Equation (9): $A=\frac{J_z{R}^2}{16\left(W+L\right)}+\frac{m{R}^2}{8}+J_{\omega }$, $B=-\frac{J_z{R}^2}{16\left(W+L\right)}$, $C=\frac{J_z{R}^2}{16\left(W+L\right)}$,

$D=\frac{m{R}^2}{8}-\frac{J_z{R}^2}{16\left(W+L\right)}$.

The dynamic model of the mobile platform can be simplified as:

$$T_M=J\ddot{\theta }+f\dot{\theta }+N$$

(10)

N represents the resistance moment matrix. Therefore, combined with Equations (8)–(10), the dynamic equation of the mobile manipulator can be summarized as Equation (11):

$$\left[\begin{array}{c}{T}_M\\ \tau \end{array}\right]=\left[\begin{array}{cc}J& 0\\ 0& H\left(q\right)\end{array}\right]\left[\begin{array}{c}\ddot{\theta }\\ \ddot{q}\end{array}\right]+\left[\begin{array}{c}f\\ C\left(q,\dot{q}\right)\end{array}\right]\left[\begin{array}{c}\dot{\theta }\\ \dot{q}\end{array}\right]+\left[\begin{array}{c}N\\ G\left(q\right)\end{array}\right]$$

(11)

3. Null-Space Task Planning Method of Mobile Manipulator

The null-space $\mathbf{N}\left(\mathbf{A}\right)$ of matrix $\mathbf{A}$ is the set of all solutions satisfying $\mathbf{A}X=0$. For matrix $m\times n$, the column space is a subspace of $\mathbf{A}$ space, and the null-space ${ℝ}^m$ is a subspace of ${ℝ}^N$ space. For any element $v$ and $w$ belonging to the $\mathbf{A}X=0$ solution, the properties of Equations (10) and (11) are satisfied:

$$A\left(v+w\right)=Av+Aw=0+0=0A\left(cv\right)=cAv=0$$

(12)

Therefore, when the state space of the system can be expressed as null-space, the control input is adjusted in the corresponding null-space, which will not cause changes in the whole and other parts. Therefore, it has good distributed control performance and can realize the coordinated control of multiple structures.

3.1. System Description

The mobile manipulator needs to control the movement of the mobile platform and the manipulator at the same time. The motion model of the general mobile platform can be expressed as:

$${\dot{q}}_c=J_c{\dot{\theta }}_c$$

(13)

where $q_c\in {ℝ}^{n_c}$ represents the generalized coordinate $O_M^{}$ vector of the moving base, ${\dot{\theta }}_c\in {ℝ}^c$ represents the velocity vector of the four wheels, $J_c\in {ℝ}^{n_c\times c}$ represents the constraint matrix of the base (complete or incomplete), and $n_c$ and c represent the dimensions of the generalized coordinate vector and the wheel velocity vector, respectively. If the slip effect is considered, the output speed of the mobile platform is different from the actual speed, and the slip effect can be defined, as Equation (14):

$$s_i=\frac{R{\theta }_i-V_{si}}{\max \left\{R{\theta }_i,V_{si}\right\}}$$

(14)

$V_{si}$ represents the actual linear velocity of i th wheel, slip rate $s_i\in \left(-1,1\right)$, so when there is slip, $R{\theta }_i<V_{si}$. The angular velocity of the drive wheel with slip attribute can be expressed as Equation (15):

$${\dot{\theta }}_s=\left(E+S\right){\dot{\theta }}_c$$

(15)

Thus, Equation (13) can be rewritten as:

$${\dot{q}}_{cs}=J_c{\dot{\theta }}_s$$

(16)

The manipulator has a mature expression that can be summarized as:

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

(17)

Therefore, the kinematic description of the mobile manipulator based on the velocity Jacobian matrix can be expressed as Equation (17):

$$\dot{x}=\left[\begin{array}{cc}J& J_{we}(q)\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_{cs}\\ {\dot{q}}_e\end{array}\right]=J_{MM}\left(q\right)\left[\begin{array}{c}{v}_c\\ {\dot{q}}_e\end{array}\right]\dots , J=J\left(\varphi \right)J_c$$

(18)

$J\left(\varphi \right)$ is a generalized transformation matrix that converts platform speed into space speed. The number of actuators of the mobile manipulator is 10, and the spatial degree of freedom is 6. The dimension of the operation space is less than the number of joints of the mobile manipulator; thus, it belongs to the redundant structure, and the right pseudo-inverse is used to calculate the joint angular velocity. Let the state position R of the mobile manipulator be Equation (19):

$$R={\left[\begin{array}{cc}R1& R2\end{array}\right]}^T={\left[\begin{array}{cccccc}{X}_{WE}& Y_{WE}& Z_{WE}& X& Y& \phi \end{array}\right]}^T$$

(19)

Let $J^{\#}{}_{MM}$ denote the right pseudo-inverse of the Jacobian matrix, and the right pseudo-inverse can be obtained by $J^{\#}{}_{MM}=J^{\mathrm{T}}{}_{MM}{\left(J_{MM}\cdot J^{\mathrm{T}}{}_{MM}\right)}^{-1}$. At the same time, considering the singular point problem of the pseudo-inverse solution, if $J^{\#}{}_{MM}$ satisfies Equation (20) under full rank, the minimum norm solution can be obtained, and the solution is orthogonal to the null-space $N\left(J\right)$.

$$\{\begin{array}{c}{J}_{MM}\cdot J^{\#}{}_{MM}\cdot J_{MM}=J_{MM}\\ J^{\#}{}_{MM}\cdot J_{MM}\cdot J^{\#}{}_{MM}=J^{\#}{}_{MM}\\ {\left(J^{\#}{}_{MM}{J}_{MM}\right)}^{\mathrm{T}}=J^{\#}{}_{MM}{J}_{MM}\\ {\left(J_{MM}{J}^{\#}{}_{MM}\right)}^{\mathrm{T}}=J_{MM}{J}^{\#}{}_{MM}\end{array}$$

(20)

Thus, we can obtain:

$$\dot{q}=J^{\#}{}_{MM}{}_E{}^W\dot{R}$$

(21)

If $J_{MM}$ has the inverse, then $J^{\#}{}_{MM}$ is the inverse of $J_{MM}$; if $J_{MM}$ is irreversible, then $J^{\#}{}_{MM}$ satisfies the least square solution of Equation (20).

Suppose the expected position state $R^d$ is Equation (21):

$$R^d={\left[\begin{array}{cc}R{1}^d& R{2}^d\end{array}\right]}^T={\left[\begin{array}{cccccc}{X}^d{}_{WE}& Y^d{}_{WE}& Z^d{}_{WE}& X^d& Y^d& {\phi }^d\end{array}\right]}^T$$

(22)

3.2. Task Space Division

Mobile manipulator in space with redundant characteristics belongs to the redundant robot, in the redundant robot, which is usually $J^{\#}J\ne I$ in the Equation (21) generalized expression:

$$\dot{q}=J^{\#}{}_{MM}\dot{R}+\left(I-J^{\#}{}_{MM}{J}_{MM}\right)v$$

(23)

where $v$ is any vector that satisfies the number of rows. For control tasks ${}_E{}^W\dot{R}$, $I-J^{\#}{}_{MM}{J}_{MM}$ is a null-vector matrix that filters out the robot degrees of freedom that have nothing to do with control ${}_E{}^W\dot{R}$ according to the nature of the null-space. That $v$ will have no effect on control ${}_E{}^W\dot{R}$. This controls lower priority control tasks with $v$ without affecting higher priority tasks.

• Control Task 1: TCP Position Priority Control at Manipulator End

The end position $R1=\left[\begin{array}{ccc}{X}_{WE}& Y_{WE}& Z_{WE}\end{array}\right]$ of the manipulator corresponds to the Jacobian matrix $J_{WE}$ of the control part;

$${\dot{q}}_1=J_{we}{}^{\#}\dot{R}1+\left(I-J^{\#}{}_{we}{J}_{we}\right)J^{\#}\dot{R}2$$

(24)

• Control Task 2: Mobile Platform Spatial Position Priority Control

Mobile platform position $R2=\left[\begin{array}{ccc}X& Y& \phi \end{array}\right]$ corresponds to the control part of the Jacobian matrix $J$.

$${\dot{q}}_2=J^{\#}\dot{R}2+\left(I-J^{\#}J\right)J^{\#}{}_{we}\dot{R}1$$

(25)

Thus, the control model can be expressed as Equation (26), where $\psi$ is a variable diagonal matrix.

$$\dot{q}=\psi {\dot{q}}_1+\left(I-\psi \right){\dot{q}}_2$$

(26)

3.3. Controller State Switching Criterion

The switching matrix $S$ of Equation (26) is the key factor to determining the control switching of each part; thus, it is necessary to set the switching criterion to determine the state of $S$. In this paper, three criteria of target distance, motion ability, and motion time are designed by considering the distance between the current position and the set target and the movement ability of the manipulator at the set position.

• Criterion 1: Contact target distance criterion

The distance between the TCP at the end of the mobile manipulator and the target M to be contacted is set to $D_{\mathrm{TCP}-\mathrm{M}}$, and the distance between the mobile platform and M is set to $D_{\mathrm{C}-\mathrm{M}}$. $P_R$ is defined as the position of the end of the manipulator. $P_C$ represents the position of the platform and $P_M$ represents the position of the contact target. The distance of each part of the mobile manipulator to the contact target can be summarized as:

$$D_{*M}={\delta }_{*M}=\Vert P_i-P_M\Vert =\sqrt{{\left(x_{p_i}-x_{p_M}\right)}^2+{\left(y_{p_i}-y_{p_M}\right)}^2+{\left(z_{p_i}-z_{p_M}\right)}^2}$$

(27)

where * represents $TCP-M$ and $C-M$, and ${\delta }_{*M}$ represents the task control variable.

• Criterion 2: Singular motion ability criterion

For an m-segment ($m<n$) open-chain robot whose task space is represented by coordinates $x\in {ℝ}^n$, the end-effector angular velocity $\dot{\theta }$ satisfies the operability ellipsoid of $\Vert \dot{\theta }\Vert =1$. The ratio $\mu$ of the long axis to the short axis of the operable ellipsoid is usually defined as the mobility of the robot under the set attitude, $\mu \ge 1$.

$$\mu ={\lambda }_{\max }/{\lambda }_{\min }$$

(28)

$\lambda$ represents the eigenvalue of $J{J}^T$. When $\mu$ is closer to 1, the mobility of the current position is stronger.

4. Hybrid Control of Mobile Manipulator Based on Synovial and Dynamics Feedforward Control

4.1. Dynamic Parameter Identification

The overall identification is an identification experiment on the actual robot that is given an optimized trajectory of each joint of the robot; during the movement of the robot, the torque and joint angle parameters of each joint are measured, and the measured data is brought into the identification model. The value of the dynamic parameters can be calculated by the constructed identification algorithm.

For the rigid connecting rod, the basic dynamic parameters of a single connecting rod are inertial parameters, which are mass parameter $m_i$, first-order mass moment parameters $\left[s_{xi},s_{yi},s_{zi}\right]$, and inertial tensor parameters $\left[I_{xxi},I_{yyi},I_{zzi},I_{xyi},I_{xzi},I_{yzi}\right]$. It can be expressed as follows:

$$P_i=\left[I_{xxi},I_{yyi},I_{zzi},I_{xyi},I_{xzi},I_{yzi},s_{xi},s_{yi},s_{zi},m_i\right]$$

(29)

For specific robotic manipulators, not all inertial parameters can be identified. Not all inertial parameters have an effect on the dynamic model, while other inertial parameters only affect the linear combination. This article only considers the linearly independent parameter sets.

$$P_i=\left[I_{xxi},I_{zzi},I_{xyi},I_{xzi},I_{yzi},m_{xi},m_{yi}\right]$$

(30)

The dynamic model can be linearized by the parallel axis theorem. The linear equation containing only the inertial parameters is:

$${\tau }_B=W_B\left(q,\dot{q},\ddot{q}\right)P_B$$

(31)

${\tau }_B$ is the $6\times 1$ joint torque vector. $W_B\left(q,\dot{q},\ddot{q}\right)$ is the regression matrix of the dynamic model containing the information on the inertial force, Coriolis force, centrifugal force, and gravity, and $P_B$ is the set of the basic dynamic parameters without friction. The inertial parameter set is calculated by a three-dimensional model. After linearization, because the values of some columns in $W_B\left(q,\dot{q},\ddot{q}\right)$ are always 0 or there is a linear relationship, the regression matrix cannot be full rank, which affects the solution of the least square method. Therefore, the inertial parameters are linearly reorganized. The dynamic linear identification model can be expressed as:

$${\tau }_B=W_{\min }\left(q,\dot{q},\ddot{q}\right)P_{\min }$$

(32)

In order to ensure that the manipulator can accurately track the excitation trajectory, it is necessary to limit the boundary conditions of the excitation trajectory. The limiting conditions are expressed as:

$$q\left(0\right)=q\left(t_{end}\right)==q_{init},\dot{q}\left(0\right)=\dot{q}\left(t_{end}\right)==0,\ddot{q}\left(0\right)=\ddot{q}\left(t_{end}\right)==0$$

(33)

$t_{end}$ is the termination time of the experiment. $q_{init}$ is the initial vector of each joint. The excitation trajectory can be expressed as:

$$q_i\left(t\right)={\displaystyle \sum _{l=1}^N\left[\frac{a_{il}}{{\omega }_{f}l}\sin ({\omega }_{f}lt)-\frac{b_{il}}{{\omega }_{f}l}\cos ({\omega }_{f}lt)\right]}+q_{i0}$$

(34)

The highest frequency of the excitation trajectory should be lower than the lowest resonance frequency of the robot structure. Therefore, the highest order of the Fourier series N = 5 is selected in many studies. In this paper, the excitation frequency is set to 0.1 Hz, and the fundamental frequency ${\omega }_f=0.2\pi$ rad/s. When $W_B\left(q,\dot{q},\ddot{q}\right)$ and ${\tau }_B$ are perturbed, Equation (32) is rewritten as:

$${\tau }_B+{\tau }_{\delta }=\left(W_{\min }+W_{\delta }\right)\left(P_{\min }+P_{\delta }\right)$$

(35)

According to Reference [24], we can calculate $P_{\delta }$:

$$P_{\delta }=\frac{\Vert W_{\min }^{-1}\Vert }{1-\Vert W_{\min }^{-1}\Vert \Vert Y_{\delta }\Vert }\left(\Vert {\tau }_{\delta }\Vert +\Vert W_{\delta }\Vert \Vert P_{\min }\Vert \right)\hspace{1em}\Vert {\tau }_{\min }\Vert \le \Vert W_{\min }^{}\Vert \Vert P_{\min }\Vert$$

(36)

Defining the condition number $cond\left(W_K\right)=\Vert W_K^{-1}\Vert \Vert W_{\delta }\Vert$ of the regression matrix yields:

$$\frac{\Vert P_{\delta }\Vert }{\Vert P_{\min }\Vert }\le \frac{cond\left(W_K\right)}{1-cond\left(W_K\right)\frac{W_{\delta K}}{W_K}}\left(\frac{\Vert {\tau }_{\delta K}\Vert }{\Vert {\tau }_K\Vert }+\frac{W_{\delta K}}{W_K}\right)$$

(37)

Among them, K represents the number of samples. We can conclude that the larger the $cond\left(W_K\right)$ is, the greater the relative error is. Therefore, a smaller $cond\left(W_K\right)$ is set to ensure the best results.

It can be directly obtained through the linear relationship between the current and output torque of each joint motor, and the relationship is:

$${\tau }_{\mathrm{id}en}=K_{id}I+B_{id}$$

(38)

$I$ is the current of the joint motor, and $K_{id}$ is the torque sensitivity, $B_{id}$ represents an offset term. The model quality is evaluated by the sum of mean square error of each joint (normalized by its average torque).

$$\mathrm{NMSE}={\displaystyle \sum _{j=1}^N{\displaystyle \sum _{k=1}^M\frac{{\left({\tau }_{j,i,k}-{\widehat{\tau }}_{j,i,k}\right)}^2}{\left|{\tau }_{j,i,k}\right|}}}$$

(39)

Therefore, we use the current instead of the torque detection sensor to sense the torque, and verify the identified dynamic model through the current trend, so that we can obtain the identification effect more quickly and intuitively.

$$\mathrm{NMSE}={\displaystyle \sum _{j=1}^N{\displaystyle \sum _{k=1}^M\frac{K_{id}{\left(I-\widehat{I}\right)}^2}{\left|I\right|}}}$$

(40)

The process according to Figure 3 is a total of 500 sets of data that were collected after running the excitation trajectory; after data preprocessing, the data were used for identification. For 500 sets, each set of data includes the position, velocity, torque, and processed acceleration of six joints. By optimizing the excitation curve and least square identification, we obtain the identified dynamic model. Through parameters’ identification, we obtain 36 sets of inertial parameters of the manipulator, as shown in

Table 1. Results of system dynamics parameter identification.

Identified Joint	${\mathit{I}}_{\mathit{x}\mathit{x}\mathit{i}}$	${\mathit{I}}_{\mathit{x}\mathit{y}\mathit{i}}$	${\mathit{I}}_{\mathit{x}\mathit{z}\mathit{i}}$	${\mathit{I}}_{\mathit{y}\mathit{z}\mathit{i}}$	${\mathit{I}}_{\mathit{z}\mathit{z}\mathit{i}}$	${\mathit{M}}_{\mathit{x}\mathit{i}}$	${\mathit{M}}_{\mathit{y}\mathit{i}}$
	$\mathrm{kg}\cdot {\mathbf{m}}^2$					$\mathbf{kg}$
Identify joints 1					2.3263	0.3692
Identify joints 2	−1.7362	−0.0539	0.4014	0.0879	−3.9862	−0.0166	−0.5987
Identify joints 3	−0.2369	0.1869	−0.1569	0.0183	−1.6937	0.0763	−0.0371
Identify joints 4	−0.1085	0.0241	0.0183	0.0002	0.0003	−0.0145	−0.1596
Identify joints 5	0.0159	−0.0289	0.0136	0.0009	0.0299	−0.0156	0.0002
Identify joints 6	0.0236	0.0103	0.0209	−0.0132	0.0128	0.0175	−0.0003

. By verifying the current measurement, as shown in Figure 4.

4.2. Control Laws Design

Mobile manipulator is a typical second-order nonlinear system; for second-order nonlinear systems, we use synovial control to design the control law.

$$\{\begin{array}{c}{\dot{x}}_1=x_2\\ {\dot{x}}_2=-H_{\tau }{}^{-1}\left(x_1\right)\left(C_{\tau }\left(x_1,x_2\right)\dot{q}+G_{\tau }\left(x_1\right)\right)+H_{\tau }{}^{-1}\tau \end{array}$$

(41)

In Equation (41), $x_1=Q$, this is a typical expression of second-order systems; considering the control characteristics of the system, we designed a global fast synovium. The fast convergence of the terminal sliding mode control has two meanings: one is that the system state in the approach phase or the sliding mode phase can maintain a high-speed convergence when it is far away from the equilibrium point or close to the equilibrium point, and the other is that both the approach phase and the sliding mode phase can maintain high-speed convergence. In order to further accelerate the finite-time convergence of the terminal sliding mode, improve the dynamic characteristics of the terminal sliding mode and avoid the singularity problem, this section first adds the double power term to the switching control law of the nonsingular terminal sliding mode.

Assuming that the speed tracking command is $x_d$, the speed tracking error can be written as:

$$\begin{array}{l}{e}_1=x_2-x_d\\ {\dot{e}}_2=e_1\\ e_2=x_1-x_d\cdot t\end{array}$$

(42)

The synovial surface can be selected:

$$s=x_2+\alpha sig{(x_1)}^{\gamma }+\beta \sin {(x_1)}^{\rho }$$

(43)

In Equation (43), $\alpha ,\beta >0$,$\gamma >1$, $0<\rho <1$. While $\gamma =m_1/n_1$ and $\rho =p_1/q_1<1$, the equivalent control law of the sliding mode control refers to the control term which makes the derivative of the sliding mode surface variable null when the uncertain disturbance of the system is ignored. The equivalent control law is expressed as:

$$\tau =-\frac{1}{g\left(t,x\right)}\left[f\left(t,x\right)+x_2\left(\alpha \gamma {\left|x_1\right|}^{\gamma -1}+\beta \rho {\left|x_1\right|}^{\rho -1}\right)\right]$$

(44)

Global sliding mode refers to adding a term containing the initial state information of the system to the original sliding mode surface. The initial value of the global terminal sliding surface (32) is null, that is, $s\left(0\right)=0$, which becomes the global sliding mode. The stability analysis method proposed by Lyapunov, especially the second method of the Lyapunov stability, is the most important method for the stability analysis of the modern control theory.

Lemma 1.

In the closed domain$\left(t,x\right)\in D:\left[t_0,+\infty \right)\times \left[-{\epsilon }_0,+\right]$, if the set-valued function$F\left(t,x\right)$satisfies the basic conditions in chapter 2 and chapter 7 of Filippov [23]; in a closed domain D, there exist first-order continuous derivable functions$v\left(t,x\right)$and continuous functions$v_0\left(x\right),v_1\left(x\right),w\left(x\right)$of$\left(t,x\right)$satisfying:

$$v\left(t,0\right)=0,v_1\left(0\right)=0,0<v_0\left(x\right)\le v\left(t,x\right)\le v_1\left(x\right),{\dot{v}}^{*}\le -w\left(x\right)<0\left(0<\left|x\right|<{\epsilon }_0\right)$$

Then, the solution$x\left(t\right)=0$of the differential inclusion$\dot{x}\in F\left(t,x\right)$is asymptotically stable. If$D\to ℝ$, the above conditions still hold and have:$v_0\left(x\right)\to +\infty ,\left|x\right|\to +\infty$, then$x\left(t\right)=0$is asymptotically stable in a large scale.

Lemma 2.

For nonlinear systems,

$$\dot{y}=-\alpha y^{\frac{m}{n}}-\beta y^{\frac{p}{q}},y\left(t_0\right)=y_0$$

Among them, α > 0, β > 0, m, n, p, q are positive odd numbers and satisfy m > n, p < q. Then, the equilibrium point of the system is finite-time stable over a wide range, i.e., the system can be stabilized from any initial state$y_0\in ℝ$to the equilibrium point$y=0$in finite time T, and T is:

$$T<\frac{1}{\alpha }\frac{n}{m-n}+\frac{1}{\beta }\frac{q}{q-p}$$

According to Theorem 1-1-15 in Filippov [23], the continuously differentiable positive definite function of the sliding surface variable s and time t in Equation (43) can be selected as:

$$V\left(t,s\right)=\frac{1}{2}\left(1+e^{-t}\right)s^2,\left(t,s\right)\in G$$

(45)

Obviously, $V\left(t,s\right)\in C^1,V\left(t,0\right)=0$. Define two continuous positive definite functions about $s$.

$$W_1\left(s\right)=\frac{1}{2}{s}^2,W_s\left(s\right)=s^2,s\in ℝ$$

(46)

In Equation (46), $W_1\left(S\right)\to +\infty ,\left|s\right|\to +\infty$ and $W_1\left(S\right)\le V\left(t,s\right)\le W_2\left(S\right),\left(t,s\right)\in G$. Define the upper derivative of V as:

$${\dot{V}}^{*}=\sup \left(\frac{\partial V}{\partial t}+\frac{\partial V}{\partial s}\dot{s}\right)=\underset{\dot{s}\in F\left(t,s\right)}{\sup }\left(-\frac{1}{2}{e}^{-t}{s}^2+\left(1+e^t\right)s\dot{s}\right)=-\frac{1}{2}{e}^{-t}{s}^2+\left(1+e^t\right)\underset{\dot{s}\in F\left(t,s\right)}{\sup }\left(s\dot{s}\right)$$

(47)

When $s=0$, ${\dot{V}}^{*}\left(t,0\right)=0$, and while $s\ne 0$, we can obtain:

$${\dot{V}}^{*}=-\frac{1}{2}{e}^{-t}{s}^2-\left(1+e^t\right)\left|s\right|\left(K_0-{\left|d\right|}_{\max }\right)<-\frac{1+e^{-t}}{2}\left(K_0-{\left|d\right|}_{\max }\right)\left|s\right|<\frac{{\left|d\right|}_{\max }-K_0}{2}\left|s\right|$$

(48)

We can set $W_3\left(s\right)=\frac{{\left|d\right|}_{\max }-K_0}{2}\left|s\right|$, so $K_0\ge D>{\left|d\right|}_{\max }$, and $W_3\left(s\right)$ is a continuous positive definite function of $s$. Therefore, we can conclude:

$${\dot{V}}^{*}\le 0\left(s\in ℝ\right),{\dot{V}}^{*}<-W_3\left(s\right)<0\left(s\in ℝ,\mathrm{ex}\left\{0\right\}\right)$$

(49)

According to Lemma 1, 2, the sliding surface s = 0 is asymptotically stable in a large range, because s = 0 is a gradual large-scale. The final overall control structure block diagram is shown in Figure 5. According to the previously constructed kinematics and dynamics model, the modified dynamic equation after identification is obtained after the dynamic parameter identification. The feedforward torque is obtained by the dynamic model of the mobile manipulator after identification. The error of the position calculated by the null-space model is input into the synovial controller to obtain the output torque ${\tau }_c$. The deviation between the position calculated by kinematics and the desired position is input into the state judgment model to obtain the S matrix.

5. Experimental and Simulation Results

As shown in Figure 6, the mobile manipulator controller is the Jetson AGX ORIN. The platform adopts four 100 W DC motors and is controlled by a mecanum wheel direct drive. It is equipped with a HITZ-ArmS1630 manipulator with a load of 3 KG and a repetitive positioning accuracy of ±0.01 mm. The MEMS galvanometer 3D sensor is used in the vision system, which can directly output a 500 W pixel RGB color image, corresponding grayscale image, depth image, and point cloud image. The infrared image resolution is 1280 × 1024 pixels, the color image is 1296 × 972 pixels, the ranging working distance is 700–2000 mm, and the image frame rate is 60 FPS. The controller for visual guidance and robotic arm control is equipped with 64 GB of memory, Intel Xeon E5 2680V3 2.5 GHZ CPU, and a Nvidia GeForce RTX2080TI graphics card.

The parameters of the controller are set as follows: α = 20, β = 20, γ = 1.8, $\rho$ = 7/9, $K_0$= 35, $k_p$ = 39, and $k_D$ = 0.1. We track a set of target curves to obtain the output speed and output torque of all joints, and obtain the torque output of the feedforward controller and the feedback torque output of the synovial controller, respectively. The sliding noise is randomly added to affect the movement of the mobile platform. By randomly adding the sliding noise $s\left\{0.1,0.1,0.1,0.1\right\}$ to the motion of the mobile platform, the sliding film controller can handle similar sudden disturbances well, because of the sliding characteristics of the sliding film control. The mechanical arm dynamic parameters used in this article are shown in

Table 2. Kinematics and dynamics parameters of mobile manipulator.

Kinematics	Theta [rad]	a [m]	d [m]	Alpha [rad]	Dynamics	Mass [kg]	Center of Mass [m]
Joint 1	0	0	0.1625	$\pi /2$	Joint 1	3.761	[0, −0.02561, 0.00193]
Joint 2	0	−0.425	0	0	Joint 2	8.058	[0.2125, 0, 0.11336]
Joint 3	0	−0.3922	0	0	Joint 3	2.846	[0.15, 0.0, 0.0265]
Joint 4	0	0	0.133	$\pi /2$	Joint 4	1.37	[0, −0.0018, 0.01634]
Joint 5	0	0	0.0997	$-\pi /2$	Joint 5	1.3	[0, 0.0018,0.01634]
Joint 6	0	0	0.0996	0	Joint 6	0.365	[0, 0, −0.001159]
Mobile platform 1	*	*	*	*	Mobile platform	36.7	[0, −0.0312, 0.0463]

¹ Mobile platform length: 1.25 m, width: 0.625 m, height: 0.35 m, and wheel diameter: 0.165 m.

. In this paper, the above control system is taken as the object to find a set of optimal parameters for the sliding surface coefficient, so that the energy consumption and tracking error of the system is minimized. Using the PSO algorithm, initialize a group of random particles, and then iterate to gradually approach the optimal solution, and finally, converge to the optimal solution.

We set up a variety of experiments to verify the performance. The first is the performance test to resist the slip interference. We simulate the actual slip effect by increasing the slip effect on the normal tracking trajectory and adding noise to the speed of the hub. Slip ratio is set to [0.8,0.8,0.8,0.8], and the adding slip time is t1: 50 s–100 s; t2: 200 s–250 s.

We set up an arc trajectory to let the end of the mobile manipulator draw a circle in space. At the same time, we randomly add the sliding noise. As shown in Figure 7, the ideal motion trajectory of the platform is red, the blue line segment is added to the sliding noise trajectory, and the gray is the actual tracking trajectory. It can be observed that after passing through the synovial controller, the path can be well-tracked and the accuracy is high. In Figure 8, this part is the movement of the platform part of the mobile manipulator. We draw the output torque corresponding to the four wheels at each moment. The motion accuracy of the mobile platform directly determines the absolute positioning accuracy of the mobile manipulator. From the curve of the output torque, we can also see the smoothness of the control system. The red curve is the desired torque output curve. It can be observed from the diagram that although the actual curve fluctuates greatly at the initial time, the curve trend is more consistent after smoothing. Similarly, Figure 9, Figure 10 and Figure 11 are the torque output and speed output curves of the manipulator. The blue represents the actual output, and red is the expected output.

In the experiment, we will also analyze the torque curve of the dynamic feedforward output. As shown in Figure 12, the increased feedback torque can help the manipulator reach the desired position more accurately and stably, and compensate the joints with larger torque. The output torque can effectively participate in the corresponding torque output. As shown in Figure 13, this part is the output torque of the mobile manipulator after synovial control. This part of the torque is the control torque that mainly controls the manipulator to meet the desired position. Therefore, after the dynamic feedforward compensation, the synovial controller can control the end position well.

At the same time, we also compared the original SMC control method with the existing control methods. The tracking situation is shown in Figure 14, red lines represent the desired actual trajectory by the controller, the green line is the tracking result of the controller proposed in this paper, and the black line is the tracking effect of the general SMC controller without the torque feedforward. From the data in the figure, it can be observed that our method has a good tracking effect when dealing with large curvature. This is mainly because the platform participates in the overall movement of the mobile manipulator when turning, which brings a large acceleration item. Therefore, when the curvature is large, the controller can provide more sufficient power to overcome the sudden inertial influence. The mobile manipulator is a complex multi-input multi-output structure with time-varying and strong coupling nonlinear dynamic characteristics. Coupled with the sliding friction uncertainty and accidental external force interference in its dynamic model, it is a typical nonlinear uncertain system. After the mobile manipulator is subjected to external forces or load changes, the kinematic model will change, and it is easy to produce vibration misalignment problems in these variable load environments. The jitter generated in the experimental results shows the anti-interference performance of the synovial controller to a certain extent, especially in the face of such uncertain systems, and the jitter of the synovial membrane is currently there, and there are some ways to suppress or offset it. In Figure 15, we set up three groups of control experiments and compare them with the Hybrid control, SMC, and PID control methods, respectively. The red lines in the figure represent the tracking effect of the proposed method, the green lines are the tracking effect of the SMC method, and the blue lines are the effect of the PID control method. It can be observed from the error that the proposed method has a higher accuracy in the tracking accuracy and faster error convergence.

6. Conclusions

In this paper, the kinematics and dynamics model of the mobile manipulator is constructed. According to the characteristics of the redundant structure, the null-space task is layered, and the null-space control model considering the influence of the sliding factor is obtained. A dynamic feedforward control based on the dynamic parameter identification is designed to compensate for the torque output and improve the stability of the main output torque. The double power terminal sliding surface is designed, the control law of torque output is derived, and the stability is proved by the Lyapunov function. The speed output curve and torque output curve of the mobile manipulator are given in the experiment. The validity and accuracy of the controller’s output are proved by the experiments.