Finite-time control strategy for swarm planar underactuated robots via motion planning and intelligent algorithm

Abstract

The swarm planar underactuated robots have the ability to organize each robot to complete task in a finite time and the characteristics of ignoring gravity, energy saving, light weight, and so on. In this paper, we propose control strategy for such robot via motion planning and intelligent algorithm. First, we establish a unified dynamic model and analysis its underactuated characteristics. In order to enable all links to reach the target position smoothly in a finite time, a suitable trajectory is planned and parameters are optimized by the differential evolution algorithm (DEA). Then, the controllers are designed for each planar underactuated robot. Finally, the simulation results show that the proposed strategy is effective.

Introduction

In recent years, the research on swarm robots inspired by biological colonies behavior has become a hot topic in the control field. Each robot is responsible for different tasks, but as a member of the colony, it obeys the self-organizing cooperative control and completes complex tasks together in finite time. At present, the swarm robots have been widely used in exploration, ¹ transportation, ² medical rehabilitation, ³ and so on. When the joint of robot encounters failure, the original robot become underactuated robot.

The underactuated robot^4,5 is an important research direction in the field of nonlinear control, whose the degree of freedoms are more than the number of inputs.^6,7 The nonlinear coupling relationship makes its control full of challenges. The surface vehicle, ⁸ tower crane, ⁹ and planar underactuated robot^10–12 etc. are all high-frequency research objects in underactuated system.

The planar underactuated robot does not consider gravity, and any point in the plane is equilibrium. But the linear approximate model of the arbitrary equilibrium point is uncontrollable, which makes the robot more difficult to control. ¹³ According to the number of links and the position of passive joint in robot, the underactuated characteristics are different. ¹⁴ The planar PA robot has angle constraints among links, and it belongs to holonomic system. Wang et al. ¹⁵ fully consider the coupling relationship and use Lyapunov function to design the controller to realize the rapid angle control of each link. The planar AP robot has angular acceleration constraints, and it belongs to second-order nonholonomic system. Based on Fourier transform, Wu et al. ¹⁶ design a time-dependent controller for the planar AP robot corresponding to the frequency of harmonic term to realize the control target of moving from the initial to target position.

In order to meet the diversified functions and complex operation, the research on the underactuated control strategy of multi-link robot is necessary.^17–19 The planar PAA robot belongs to first-order nonholonomic system with angular velocity constraints. Zhang et al. ²⁰ adapt a two-stages control method to solve the problem that the asymptotic stability of such robot at the target position. In stage 1, the controller is designed to stabilize one active link at the target angle and the others at the transition angles; In stage 2, the other links will be moved to the target states simultaneously due to the angle constraint. The passive middle or last joint of the planar underactuated robot belong to the second-order nonholonomic system. Huang et al. ²¹ propose a bidirectional trajectory planning and tracking control method based on time scale for planar APA robot, calculate the time scale factor by using intelligent algorithm, and realize the position control through a single trajectory tracking controller.

For the planar AAP robot, the position control of its first active link is realized firstly, and then the fuzzy control method is used to realize the stability of other links. ²² Huang et al. ²³ propose an unified method for the planar three-link robot. Consider the underactuated states and constraints, he designs trajectory for active link that can be optimized by intelligent algorithm and uses the designed tracking controller to realize the control objectives. Huang et al. ²⁴ reduce the planar multi-link robot with a last passive joint to the planar AP robot, and make the state variables converge rapidly based on nilpotent approximation and iterative theory. De Luca and Oriolo ²⁵ transform the planar ${\mathrm{A}}^n\mathrm{P}$ (Active-⋯-Active-Passive) robot into a second-order canonical chained form, calculate the impact coordinates and obtain expected trajectory of the passive link, and realize the stability control of the active link by designing a feedback controller.

The last joint of the planar swarm robot is very fragile because it is used frequently. Once damaged, it will cause huge property losses. Meanwhile, there is no effective control strategy for the swarm robot with a last passive joint in a finite time. Therefore, based on motion planning and intelligent algorithm, we study the control strategy for such system. First, an unified dynamic model is established for the planar ${\mathrm{A}}^n\mathrm{P}$ robot, and its underactuated constraints are analyzed. Then, a trajectory with adjustable parameters is designed for each active link. Meanwhile, the DEA is used to optimize the parameters so that all links are moved and maintained to the target states in a finite time. Finally, the sliding mode variable structure controllers are designed to achieve the objective. Simulation results prove the effectiveness of the proposed strategy.

Preparations

Model

The unified structure of the planar ${\mathrm{A}}^n\mathrm{P}$ robot is shown in Figure 1. For the $r$ -th $(r=1,\cdots ,n+1)$ link, $m_r$ , $q_r$ , $L_r$ and $J_r$ are the mass, angle, length, and the moment of inertia of the robot, respectively. $l_r$ is the distance from $r$ -th joint to its center of the mass, and ${\tau }_r$ denote the inputs applied to active joints.

Figure 1.

The unified structure of the swarm planar ${\mathrm{A}}^n\mathrm{P}$ robot.

The dynamic equations is

$$M(\mathit{q})\stackrel{··}{\mathit{q}}+H(\mathit{q},\stackrel{·}{\mathit{q}})=\mathit{\tau }$$

(1)

where the vector of $q$ , $\stackrel{·}{q}$ , $\stackrel{··}{q}$ , $\mathit{\tau }$ and the matrix of $\mathit{M}(q)$ , $\mathit{H}(q,\stackrel{·}{q})$ are detailed expression in Yoshikawa et al. ²⁶

Underactuated constraint analysis

The underactuated portion in (1) is

$$M_{(n+1)1}^k{{\stackrel{··}{q}}^k}_1+\cdots +M_{(n+1)(n+1)}^k{{\stackrel{··}{q}}^k}_{(n+1)}+H_{n+1}^k=0$$

(2)

where $k=1,2,\cdots ,6$ .

According to (2), the underactuated constraints among the underactuated and active link are obtained.

$$\{\begin{array}{c}{{\stackrel{·}{q}}^k}_{(n+1)}=\hfill \\ \multicolumn{1}{c}{-{\int }_0^T\left(\frac{{\sum }_{i=1}^n{M}_{(n+1)i}^k{{\stackrel{··}{q}}^k}_i+H_{(n+1)}^k}{M_{(n+1)(n+1)}^k}\right)\mathrm{d}t-{{\stackrel{·}{q}}^k}_{(n+1)0}}\\ \multicolumn{1}{c}{q_{(n+1)}^k={\int }_0^T{{\stackrel{·}{q}}^k}_{n+1}\mathrm{d}t-q_{(n+1)0}^k}\end{array}$$

(3)

where $q_{(n+1)0}^k$ and ${\stackrel{·}{q}}_{(n+1)0}^k$ are the initial states of the underactuated link, respectively.

Trajectory planning

In order to quickly realized the stability control objective of each robot in a finite time, ²⁷ we fully consider the initial and target angles of the system and the termination time $t_f$ of the following designed trajectory $T^k(t)$ for the active link.

$$\begin{array}{c}\multicolumn{1}{c}{T^k\left(t\right)=q_{n0}^k+(q_{\mathrm{nd}}^k-q_{n0}^k)(\frac{t}{t_f}-\frac{1}{2\pi }\sin \left(\frac{2\pi t}{t_f}\right))}\\ +A_{n1}^k\mathrm{sech}\left({\alpha }_1^k\right)+A_{n2}^k\mathrm{sech}\left({\alpha }_2^k\right)\hfill \end{array}$$

(4)

where

$$\{\begin{array}{c}{\alpha }_1^k=\frac{9\left(2t-t_m^k\right)}{t_m^k}\\ {\alpha }_2^k=\frac{9\left(2t-t_f-t_m^k\right)}{t_f-t_m^k}\end{array}$$

(5)

The derivatives of $T^k(t)$ is

$$\begin{array}{c}\multicolumn{1}{c}{{\stackrel{·}{T}}^k\left(t\right)=\frac{q_{\mathrm{nd}}^k-q_{n0}^k}{t_f}(1-\cos \left(\frac{2\pi t}{t_f}\right))}\\ -\frac{18A_{n1}^k\mathrm{sech}\left({\alpha }_1^k\right)\tanh \left({\alpha }_1^k\right)}{t_m^k}\hfill \\ -\frac{18A_{n2}^k\mathrm{sech}\left({\alpha }_2^k\right)\tanh \left({\alpha }_2^k\right)}{t_f-t_m^k}\hfill \end{array}$$

(6)

$$\begin{array}{c}\multicolumn{1}{c}{{\stackrel{··}{T}}^k\left(t\right)=\frac{q_{\mathrm{nd}}^k-q_{n0}^k}{t_f^2}\left(2\pi \sin \left(\frac{2\pi t}{t_f}\right)\right)}\\ +\frac{324A_{n1}^k\mathrm{sech}\left({\alpha }_1^k\right){\left(\tanh \left({\alpha }_1^k\right)\right)}^2}{{\left(t_m^k\right)}^2}\hfill \\ -\frac{324A_{n1}^k{\left(\mathrm{sech}\left({\alpha }_1^k\right)\right)}^3}{{\left(t_m^k\right)}^2}\hfill \\ +\frac{324A_{n2}^k\mathrm{sech}\left({\alpha }_2^k\right){\left(\tanh \left({\alpha }_2^k\right)\right)}^2}{{(t_f-t_m^k)}^2}\hfill \\ -\frac{324A_{n2}^k{\left(\mathrm{sech}\left({\alpha }_2^k\right)\right)}^3}{{(t_f-t_m^k)}^2}\hfill \end{array}$$

(7)

when $t=t_f$ , all active links reach the target states, but the underactuated link will rotate freely due to the nonholonomic constraints. In order to indirectly stabilize the passive part to the objective by controlling the active links, the parameters $A_{n1}^k$ , $A_{n2}^k$ , and $t_m^k$ need to be optimized by the DEA. ²⁸

The evaluation function is

$$h=|q_{(n+1)}^k(t_f^k)-q_{(n+1)d}^k|+|{\stackrel{·}{q}}_{(n+1)}^k(t_f^k)|$$

(8)

The optimization procedure is as follows:

• ① The initialization population is randomly generated, and the parameters of the trajectory are assumed to be ${\tilde{A}}_{n1}^k$ , ${\tilde{A}}_{n2}^k$ , and ${\tilde{t}}_m^k$ .

• ② According to (4) to (7) and ${\tilde{A}}_{n1}^k$ , ${\tilde{A}}_{n2}^k$ , ${\tilde{t}}_m^k$ calculate $q_{n+1}(t_f)$ and ${\stackrel{·}{q}}_{n+1}(t_f)$ , and substitute them into (8).

• ③ When $h\le {\epsilon }_1$ ; ${\epsilon }_1$ is a small positive constant, $A_{n1}^k={\tilde{A}}_{n1}^k$ , $A_{n2}^k={\tilde{A}}_{n2}^k$ , $t_m^k={\tilde{t}}_m^k$ . Otherwise, the program is to ④.

• ④ After mutation, crossover and selection, update ${\tilde{A}}_{n1}^k$ , ${\tilde{A}}_{n2}^k$ , ${\tilde{t}}_m^k$ , go to ②.

The underactuated link can reach the objectives at the same time when the active links return to the target at $t=t_f$ .

Controller design

The sliding mode variable structure controller is designed for active links tracking.

Let $x=[x_1,\cdots ,x_{(n+1)},x_{(n+2)},\cdots ,x_{(2n+2)}{]}^T$ = $[q_1,\cdots ,q_{(n+1)},{\stackrel{·}{q}}_1,\cdots ,{\stackrel{·}{q}}_{(n+1)}{]}^T$ , the state space equation of the planar ${\mathrm{A}}^n\mathrm{P}$ robot is

$$\stackrel{·}{x}=f\left(x\right)+g\left(x\right)\tau$$

(9)

where

$$f(x)=[f_1{f}_2\cdots f_{2n+2}]=-M^{-1}\left(q\right)H(q,\stackrel{·}{q})$$

(10)

and

$$g\left(x\right)=\left[\begin{array}{ccc}{g}_{11}\left(x\right)\hfill & \cdots \hfill & g_{1(n+1)}\left(x\right)\hfill \\ \multicolumn{1}{c}{g_{21}\left(x\right)}& \cdots \hfill & g_{2(n+1)}\left(x\right)\hfill \\ \multicolumn{1}{c}{⋮}& \ddots \hfill & ⋮\hfill \\ \multicolumn{1}{c}{g_{(n+1)1}\left(x\right)}& \cdots \hfill & g_{(n+1)(n+1)}\left(x\right)\hfill \end{array}\right]=M^{-1}\left(q\right)$$

(11)

The sliding mode surface is

$$S_i^k={\mu }_k{e}_i^k+{\stackrel{·}{e}}_i^k(i=1\cdots n)$$

(12)

and

$$e_i^k=q_i^k-T^k(t),{\stackrel{·}{e}}_i^k={\stackrel{·}{q}}_i^k-{\stackrel{·}{T}}^k(t)$$

(13)

The derivative of $S_i^k$ is

$${{\stackrel{·}{S}}^k}_i={\mu }_k{{\stackrel{·}{e}}^k}_i+{{\stackrel{··}{e}}^k}_i={\mu }_k{{\stackrel{·}{e}}^k}_i+f_i+\sum _{j=1}^n{g}_{\mathrm{ij}}{\tau }_j^k-{\stackrel{··}{T}}^k(t)$$

(14)

Let

$${\stackrel{·}{S}}_i^k=-{\phi }_k{S}_i^k-{\epsilon }_k\mathrm{sgn}(S_i^k)$$

(15)

where $[{\mu }_k,{\phi }_k,{\epsilon }_k]>[0,0,0]$ , and $\mathrm{sgn}(S_i^k)$ denote the sign of $S_i^k$ .

The controllers are designed as

$$\begin{array}{c}{\tau }_i^k=(-{\phi }_k{S}_i^k-{\epsilon }_k\mathrm{sgn}(S_i^k)-{\mu }_k{{\stackrel{·}{e}}^k}_i-f_i\hfill \\ \multicolumn{1}{c}{-\sum _{j=1,i\ne j}^n{g}_{\mathrm{ij}}{\tau }_j^k+{\stackrel{··}{T}}^k\left(t\right))g_{\mathrm{ij},i=j}^{-1}}\end{array}$$

(16)

The Lyapunov function is constructed as:

$$V=\frac{1}{2}(S_i^k{)}^2$$

(17)

then

$$\begin{array}{c}\stackrel{·}{V}=S_i^k{{\stackrel{·}{S}}^k}_i=-{\phi }_k{(S_i^k)}^2-{\epsilon }_k\left|S_i^k\right|\le 0\hfill \end{array}$$

(18)

When $\stackrel{·}{V}\equiv 0$ , $S_i^k\equiv 0$ . Thus, according to the LaSalle’s theorem, ²⁹ $e_i^k\to 0$ and ${{\stackrel{·}{e}}^k}_i\to 0$ . the controller (16) realized the control objective of the planar ${\mathrm{A}}^n\mathrm{P}$ robot.

Simulations

The feasibility and superiority of the proposed strategy in the stability control of the swarm planar ${\mathrm{A}}^n\mathrm{P}$ $(n=2,3)$ robot in finite time are verified by Matlab simulation. The parameters of the DEA and controller (16) are $p_c=0.7$ , $p_m=0.3$ , $\mathrm{Maxgen}=200$ , $e_i^k=0.005$ , ${\mu }_j=1.2$ , ${\phi }_j=1$ , and ${\epsilon }_j=0.3$ .

The structure parameters of a single planar AP robot ¹⁶ in the swarm planar underactuated robot are shown in Table 1.

Table 1.

Structure parameters of the planar AP robot.

link $r$	$m_r$ (kg)	$L_r$ (m)	$l_r$ (m)	$J_r$ (kg · m 2)
1	1.0	1.0	0.5	0.3333
2	1.0	1.0	0.5	0.3333

We choose the termination time is $t_f=15$ s, the target angle is same as Wu et al. ¹⁶ and the different initial states of each link are shown in Table 2.

Table 2.

The initial states of the swarm planar AP robot.

No. $k$	$q_{10}^k$ (rad)	$q_{20}^k$ (rad)	${\stackrel{·}{q}}_{10}^k$ (rad/s)	${\stackrel{·}{q}}_{20}^k$ (rad/s)
1	0	0	0	0
2	0.1	0.2	0	0
3	−0.1	−0.3	0	0
4	0.2	−0.1	0	0
5	−0.4	0.1	0	0
6	0.3	0.1	0	0

The parameters of the designed trajectory are optimized by the DEA, and the results are shown in Table 3.

Table 3.

Trajectory parameter optimization results.

No. $k$	$A_{11}^k$	$A_{12}^k$	$t_m^k$
1	−0.2723	0.0578	10.0000
2	−1.9322	−0.2704	9.0033
3	1.4172	−0.1777	8.7240
4	−0.3537	1.3247	4.7127
5	−0.2063	−2.2304	3.1014
6	0.2803	−0.7620	6.8077

The simulation results (Figure 2) show that the swarm planar AP robot realize the stable control objective. The angles and angular velocities converge to their target values smoothly. Compared with Wu et al. ¹⁶ the control torque of our proposed strategy is smaller.

Figure 2.

Simulation results of swarm planar AP robot: (a) angles of the first link, (b) angles of the second link, (c) angular velocities of the first link, (d) angular velocities of the second link, and (e) control torques .

The structure parameters of a single planar AAP robot ²² are shown in Table 4.

Table 4.

Structure parameters of the planar AAP robot.

link $r$	$m_r$ (kg)	$L_r$ (m)	$l_r$ (m)	$J_r$ (kg · m 2)
1	0.3	0.3	0.15	0.0023
2	0.3	0.3	0.15	0.0023
3	0.3	0.3	0.15	0.0023

We choose the termination time is $t_f=20$ s, the target angle is same as Liu et al. ²² and the different initial states of each link are shown in Table 5.

Table 5.

The initial states of the swarm planar AAP robot.

NO. $k$	$q_{10}^k$	$q_{20}^k$	$q_{30}^k$	${\stackrel{·}{q}}_{10}^k$	${\stackrel{·}{q}}_{20}^k$	${\stackrel{·}{q}}_{30}^k$
1	0	0	0	0	0	0
2	0.2	0.4	0.1	0	0	0
3	0.1	0.3	−0.4	0	0	0
4	0.3	−0.2	0.1	0	0	0
5	−0.4	0.2	0.3	0	0	0
6	−0.3	−0.2	0.1	0	0	0

The optimization results of trajectory parameters are shown in Table 6.

Table 6.

Trajectory parameter optimization results.

NO. $k$	$A_{11}^k$	$A_{12}^k$	$A_{21}^k$	$A_{22}^k$	$t_m^k$
1	−0.1460	0.1540	−0.1028	0.6697	8.4052
2	−0.9901	0.0296	1.7867	1.4031	10.4286
3	−0.3888	−0.0563	1.1684	0.6518	13.5295
4	−0.7338	−0.0704	1.3556	0.6821	12.9062
5	−0.0548	−0.0051	−0.4186	0.7995	9.4230
6	−0.0431	0.8455	−0.2331	−0.3947	5.2324

The simulation results (see Figures 3–5) show that the swarm planar AAP robot can also achieve stable control in finite time, all links gradually converges to the target states along a continuous and smooth trajectory, and the control torques are maintained within $\pm 0.30$ N· m, which is relatively small. Compared with Liu et al. ²² the proposed strategy does not need piecewise control.

Figure 3.

Angles of each link: (a) angles of the first link, (b) angles of the second link, and (c) angles of the third link.

Figure 4.

Angular velocities of each link: (a) angular velocities of the first link, (b) angular velocities of the second link, and (c) angular velocities of the third link.

Figure 5.

Control torques of each active link: (a) control torques of the first active link, (b)control torques of the second active link.

Conclusions

This paper presents an advanced trajectory planning and tracking control strategy for the swarm planar ${\mathrm{A}}^n\mathrm{P}$ robot, and analyzes two typical robots through simulation. Now the main conclusions are as follows.

1. A unified dynamic model for swarm planar ${\mathrm{A}}^n\mathrm{P}$ robot is established, and the underactuated constraints are analyzed based on the above model.

2. The suitable trajectory with parameters for active links are planned and the parameters are optimized by the DEA.

3. The controllers are designed for active links to track the optimized trajectory.

The control strategy can be extended to the same type of typical systems with multiple links. Therefore, we will further investigate this kind of control strategy to the other swarm underactuated robot with different structures in the future research.