Two-Stage Control Strategy Based on Motion Planning for Planar Prismatic–Rotational Underactuated Robot

Abstract

Intelligent robots are often used to explore various areas instead of humans. However, when the driving joint is damaged, the actuated robot degenerates to an underactuated robot, and the traditional control method is not suitable for the underactuated robot. In this work, a two-stage control approach for a planar prismatic–rotational (PR) underactuated robot is introduced. Firstly, we establish the dynamic model and describe the underactuated constraint between an underactuated rotational joint and active prismatic joint. Secondly, the trajectory with multiple parameters is planned to ensure that the two joints reach the target position. Based on underactuated constraints and the evaluation function, the differential evolution algorithm (DEA) is used to optimize these parameters. After that, in stage 1, we design the controller to move the active prismatic joint to the desired position. Meanwhile, the underactuated rotational joint is rotating freely. In stage 2, we design the controller for the active prismatic joint to track the planned trajectory. By means of this strategy, both joints reach their target locations simultaneously. The final simulation result demonstrates that this strategy is effective.

1. Introduction

In recent years, with the rapid development of artificial intelligence [1], machine learning [2], sensor technology [3], machine vision and automation technology [4], robots are widely used in many walks of life, including industrial manufacturing [5], medical care [6], home services [7], agriculture [8] and logistics storage [9]. Usually, most of these robots are fully driven, but when one of the controllers is damaged, the actuated robots will become underactuated robots [10]. At this time, the actuated control strategy is no longer applicable. If the control strategy is not adjusted in time, the robot that is out of control in the working state will have great security risks.

Underactuated robots have been an important research object in intelligent robot and nonlinear control theory for nearly century [11]. Such robots have the typical characteristics of the underactuated system that the number of actuators is less than the number of degrees of freedom to be controlled [12,13,14]. Aircraft [15], bionic instruments [16], pendulums [17] and inverted pendulums [18] are mature underactuated systems that have been widely used in daily life and have created enormous value for human beings. The underactuated robot has fewer independent actuators, and this not only makes the structure more compact, but also reduces the total mass and energy consumption [19,20,21]. The underactuated robot still has many advantages that are worthy of indepth exploration and development by scholars in various fields.

With the application of intelligent robots in the unexplored fields of deep sea and space, the efficiency and safety of exploration have been greatly improved [22]. In order to practice the concept of green development and low-carbon environmental protection, underactuated robots have seen increasing applications. Wang et al. [23] proposed a stable control strategy for a vertical underactuated manipulator. As an important branch of underactuated robots [24,25], the horizontal planar underactuated robot disregards effects of gravity, and all of the reachable places are balancing positions [26]. Nevertheless, the complex nonlinear characteristics make it difficult to design a continuous controller for a planar manipulator [27]. The above factors undoubtedly greatly increase the difficulty of control [28,29]. Michele [30] used the Laplace transform method to control the motion of planar underactuated robots, and the influence of friction is considered. The differential flatness characteristics of underactuated planar open-chain robots are studied in [31]. Ref. [32] proposed a motion planning method for planar underactuated robots. For the planar underactuated robot with an underactuated last joint, a trajectory planning control method is proposed in [33].

Different structures of the planar underactuated robot determine different dynamic coupling characteristics [34,35,36]. The PR underactuated robot is a special robot composed of an active prismatic link and underactuated rotational link, which has strong randomness and instability [37]. Wu et al. [38] derived a new control equation of the underactuated link for the PR underactuated robot to achieve position control. Huang et al. [39] designed an iterative control approach such that the PR underactuated robot would quickly reach the appointed position exponentially. Ichida et al. [40] used the Hespanha–Morse switching technique to realize stable control of the PPR underactuated robot. Luca et al. [41] exploited the recursive property of the dynamic system to design the dynamic feedback linearization law, and used the polynomial interpolation method to plan the smooth motion trajectory for the planar PPRⁿ underactuated robot. Moreover, the development of the model degradation method provides a new control idea for simplifying the complex planar PⁿR underactuated robot into the simplest planar PR underactuated robot [42,43].

Most of the above position control strategies do not make full use of the underactuated constraint, which makes the design complicated. Therefore, we propose a two-stage control method that combines tracking control with motion planning. Firstly, the coupling constraint relationships between the two links are analyzed based on the dynamic model. Secondly, to ensure the robot can reach the goal point smoothly, a trajectory with several customizable parameters is developed. The parameters are calculated using the DEA based on the underactuated coupling characteristics of the robot. Next, the prismatic link is driven to the desired position by designing a PD controller, while the rotational link is rotating freely due to the lack of a controller. Then, the active joint is driven by a second controller to track the trajectory. The underactuated link is indirectly regulated to the target position because of the coupling relationship. The final simulation result demonstrates that this strategy is effective.

2. Preparations

This section mainly includes the establishment of the dynamic model, the derivation of the underactuated coupling relationship and the description of the control strategy.

2.1. Dynamic Model

For this paper, the function of the dynamic model is to establish the relationship between input torque and output states of the joints.

According to Figure 1, the planar PR underactuated robot consists of an active prismatic joint and a rotational joint. The control torque of robot joints is $\tau ={[{\tau }_1,0]}^T$, where ${\tau }_1$ is applied on the active prismatic link, and the control torque of the underactuated rotational link is 0 N·m. The meaning of the parameters of the planar PR robot is shown in

Table 1. The meaning of parameters.

Parameters	Meaning
$L_p$	length of the pth link
$l_p$	distance from pth joint to its center of mass
$q_1$	distance the first joint moves
$q_2$	rotation angle of the second joint
$m_p$	mass of the pth link
$J_p$	inertia of the pth link

.

The dynamic model is as follows:

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

(1)

where $\mathit{q}$ = ${[q_1,q_2]}^T$ is a two-dimensional vector representing positions of two links. Similarly, the angular acceleration and angular velocity are denoted, respectively, by $\dot{\mathit{q}}$ and $\ddot{\mathit{q}}$, respectively. $\mathit{M}\left(\mathit{q}\right)\in {\mathbb{R}}^{2\times 2}$ is the inertia matrix, and the centrifugal and coriolis forces are included in $\mathit{H}(\mathit{q},\dot{\mathit{q}})\in {\mathbb{R}}^{2\times 1}$, their specific expression in [39].

2.2. Underactuated Constraints

For the un-actuated joint, because of its lack of a controller, we cannot directly control it to complete the expected task. Therefore, according to the coupling characteristics between an un-actuated joint and actuated joint, we indirectly control the un-actuated joint by controlling the actuated joint. The establishment of underactuated constraints describes the coupling characteristics of the un-actuated joint and actuated joint.

The underactuated part in (1) is

$$(m_2{l}_{2}sin{q}_2){\ddot{q}}_1-(m_2{l}_2^2+J_2){\ddot{q}}_2=0$$

(2)

According to (2), the underactuated coupling constraints between active and underactuated links are described as follow.

$$\left\{\begin{array}{c}{\dot{q}}_2=\underset{0}{\overset{T}{\int }}\left({\displaystyle \frac{m_2{l}_{2}sin{q}_2{\ddot{q}}_1}{m_2{l}_2^2+J_2}}\right)\mathrm{d}t+{\dot{q}}_{20}\hfill \\ q_2=\underset{0}{\overset{T}{\int }}{\dot{q}}_2\mathrm{d}t+q_{20}\hfill \end{array}\right.$$

(3)

where $q_{20}$ is the initial angle and ${\dot{q}}_{20}$ is the angular velocity of the underactuated rotational link.

2.3. Control Strategy

The underactuated characteristics of the system make its control more complicated, so we design a two-stage control strategy to achieve the control goal. The following is the concrete implementation process of the two-stage control method.

Stage 1: the active prismatic link is driven to the desired position by designing a PD controller, while the rotational link is rotating freely due to the lack of a controller.

Stage 2: for the active prismatic link, an oscillation trajectory with several parameters is developed, and the DEA is used to optimize the parameters. Motion planning is carried out according to the final state of the un-actuated joint in the first stage and its target state. The second controller is designed to drive the prismatic link to track the trajectory. Finally, the underactuated rotational link arrives at the appointed position under the underactuated constraint relation.

3. Motion Planning

The core idea of the motion planning method is to dynamically plan the motion state of the driving link by using the underactuated constraint relationship, so as to control the underactuated link. In this section, we fully consider the initial state, control target and underactuated characteristic of the underactuated robot to design the oscillatory trajectory, $T_r\left(t\right)$, for the active prismatic link.

$$\begin{array}{c}{\displaystyle T_r\left(t\right)=q_{1d}+B_1\mathrm{sech}\left({\theta }_2\right)+B_2\mathrm{sech}\left({\theta }_1\right)}\hfill \end{array}$$

(4)

where $q_{1d}$ represents the target angle of the first link, and $B_1$ and $B_2$ are the amplitude values.

$$\left\{\begin{array}{c}{\displaystyle {\theta }_1=\frac{9\left(2t-t_f-t_m\right)}{t_f-t_m}}\hfill \\ {\displaystyle {\theta }_2=\frac{9\left(2t-t_1-t_m\right)}{t_m-t_1}}\hfill \end{array}\right.$$

(5)

where $t_m$ is the connection time of two sech(·) functions, respectively, and $t_f$ is the terminal time when the robot realizes the control objective.

The first and second derivatives of $T_r\left(t\right)$ are

$${\dot{T}}_r\left(t\right)=-{\displaystyle \frac{18B_1\mathrm{sech}\left({\theta }_2\right)tanh\left({\displaystyle {\theta }_2}\right)}{t_m-t_1}}-{\displaystyle \frac{18B_2\mathrm{sech}\left({\theta }_1\right)tanh\left({\theta }_1\right)}{t_f-t_m}}$$

(6)

$${\displaystyle \begin{array}{cc}{\ddot{T}}_r\left(t\right)& =\frac{324B_1\mathrm{sech}\left({\theta }_2\right){\left({\displaystyle tanh\left({\theta }_2\right)}\right)}^2}{{\left(t_m-t_1\right)}^2}+\frac{324B_2\mathrm{sech}\left({\theta }_1\right){\left({\displaystyle tanh\left({\theta }_1\right)}\right)}^2}{{\left(t_f-t_m\right)}^2}\hfill \\ & -\frac{324B_1{\left({\displaystyle \mathrm{sech}\left({\theta }_2\right)}\right)}^3}{{\left(t_m-t_1\right)}^2}-\frac{324B_2{\left({\displaystyle \mathrm{sech}\left({\theta }_1\right)}\right)}^3}{{\left(t_f-t_m\right)}^2}\hfill \end{array}}$$

(7)

When $t=t_1$, the active prismatic link reaches $q_{1d}$, and the free rotation state of the underactuated rotational link is caused by the uncontrollable drift characteristic due to the nonholonomic constraints.

The trajectory of the driving joint is roughly composed of target angle $q_{1d}$ and two hyperbolic secant functions. This trajectory reciprocates around $q_{1d}$, and the initial and final speeds are both zero, which meet the control goal of the driving joint. In the optimization process, the amplitude parameters $B_1$ and $B_2$ of the two hyperbolic secant functions of the trajectory are opposite. Due to the underactuated constraint, the underactuated link obtains opposite speeds in two time periods, which is beneficial to stabilize it to the target angle. In order to make the underactuated link converge to the target state accurately, it is necessary to calculate the undetermined parameters of the trajectory.

$B_1,B_2,t_m$ and $t_f$ are undetermined trajectory parameters, which determine different dynamic trajectories. Meanwhile, we find that the state of the underactuated rotational link is determined by different trajectories. Therefore, we select appropriate trajectory parameters of $T_r\left(t\right)$ with the DEA. Trajectories with different parameters will eventually lead to different un-actuated joint states, which are compared with the target states as evaluation indexes.

The evaluation function is

$$k=\left|q_2\left(t_f\right)-q_{2d}\right|+\left|{\dot{q}}_2\left(t_f\right)\right|$$

(8)

The optimization process is described as follows:

①

Randomly generating an initialized population, while the initial parameters are expressed as ${\tilde{B}}_1$, ${\tilde{B}}_2$, ${\tilde{t}}_m$ and ${\tilde{t}}_f$.

②

Using ${\tilde{B}}_1$, ${\tilde{B}}_2$, ${\tilde{t}}_m$, ${\tilde{t}}_f$ and Equation (7) to calculate the current ${\ddot{T}}_r\left(t\right)$. Then, obtain $q_2\left(t_f\right)$ and ${\dot{q}}_2\left(t_f\right)$ according to Equation (3). Finally, obtain k according to Equation (8).

③

If $k\le {\epsilon }_1$, $B_1={\tilde{B}}_1$, $B_2={\tilde{B}}_2$, $t_m={\tilde{t}}_m$ and $t_f={\tilde{t}}_f$, where ${\epsilon }_1$ is small enough. Otherwise, the program is to ④.

④

The parameters are mutated, crossed and selected, then update ${\tilde{B}}_1$, ${\tilde{B}}_2$, ${\tilde{t}}_m$ and ${\tilde{t}}_f$. Back to②.

At last, we obtain a feasible optimized trajectory with appropriate parameters $B_1$, $B_2$, $t_m$ and $t_f$. The active prismatic link tracks the optimized trajectory, and the underactuated rotational link indirectly moves under the coupling constraints between the two links.

4. Controller Design

4.1. The First Controller Design

The state space equation is:

$$\dot{x}=f\left(x\right)+g\left(x\right)\tau \iff \left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\\ {\dot{x}}_3\\ {\dot{x}}_4\end{array}\right]=\left[\begin{array}{c}{x}_3\\ x_4\\ f_1\\ f_2\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& 0\\ g_{11}& g_{12}\\ g_{21}& g_{22}\end{array}\right]\left[\begin{array}{c}{\tau }_1\\ 0\end{array}\right]$$

(9)

where

$$\left\{\begin{array}{c}\begin{array}{c}x={[x_1,x_2,x_3,x_4]}^T={[q_1,q_2,{\dot{q}}_1,{\dot{q}}_2]}^T\hfill \\ \\ {[f_1,f_2]}^T=-M^{-1}\left(\mathit{q}\right)H(\mathit{q},\mathit{q})\hfill \end{array}\hfill \\ \\ \left[\begin{array}{cc}{g}_{11}& g_{12}\\ g_{21}& g_{22}\end{array}\right]=M^{-1}\left(\mathit{q}\right)\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right]\hfill \end{array}\right.$$

(10)

Based on the control objective, we construct the Lyapunov function as follows

$$V_1\left(x\right)=0.5P_1{(x_1-q_{1d})}^2+0.5x_3^2$$

(11)

The derivative is

$${\dot{V}}_1\left(x\right)=x_3[P_1(x_1-q_{1d})+f_1+g_{11}{\tau }_1]$$

(12)

To ensure ${\dot{V}}_1\left(x\right)\le 0$, the first controller is constructed as

$${\tau }_1=\left[P_1(-x_1+q_{1d})\right]-f_1-D_1{x}_3]g_{11}^{-1}$$

(13)

where $P_1,D_1\in {\mathbb{Q}}^{+}$. Since $\mathit{M}\left(\mathit{q}\right)$ is a positive-definite symmetric matrix and $g_{11}>0$, the controller, ${\tau }_1$, is non-singular.

The invariant set is defined as:

$${\Omega }_1=\{\mathit{x}\in {\mathbb{R}}^4\mid V_1\left(x\right)\le {\lambda }_1\}$$

(14)

where ${\lambda }_1$ is a small positive constant.

Defining sets as:

$${\Psi }_1=\{x\left(t\right)\in {\Omega }_1\mid {\dot{V}}_1\left(x\right)=0\}$$

(15)

Let $\omega$ be the maximum invariant set in ${\Psi }_1$, then $x\left(0\right)\in {\Psi }_1$, $x\left(t\right)\in \omega$, $t\to \infty$.

When ${\dot{V}}_1\left(x\right)\equiv 0$, then $x_3\equiv 0$ and

$$f_1=-g_{11}{\tau }_1$$

(16)

Substituting $f_1$ into ${\tau }_1$, we obtain

$$\omega =\{\mathit{x}\in {\mathbb{R}}^4\mid x_1=q_{1d},x_3=0\}$$

(17)

According to LaSalle’s principle [44], the first controller realizes the control objective of the active prismatic link if the following conditions are met.

$$\mid x_1-q_{1d}\mid \le e_1,\mid x_3\mid \le e_2$$

(18)

where $e_1$ and $e_2$ are sufficiently small positive constants.

4.2. The Second Controller Design

In stage 2, we design the second controller to track the designed trajectory. First, we construct the Lyapunov function as

$$V_2\left(x\right)=0.5P_2{(x_1-T_r)}^2+0.5{(x_3-{\dot{T}}_r)}^2$$

(19)

and the derivative is

$${\dot{V}}_2\left(x\right)=(x_3-{\dot{T}}_r)(P_2(x_1-T_r)+f_1+g_{11}{\tau }_1^{\left\{2\right\}}-{\ddot{T}}_r)$$

(20)

To ensure that ${\dot{V}}_2\left(x\right)\le 0$, we construct the second controller as

$${\tau }_1^{\left\{2\right\}}=(P_2(-x_1+T_r)-D_2(x_3+{\dot{T}}_r))g_{11}^{-1}-(f_1-{\ddot{T}}_r)g_{11}^{-1}$$

(21)

The above equation can meet the following conditions:

$${\dot{V}}_2\left(x\right)=-D_2(x_3-{\dot{T}}_r)\le 0$$

(22)

when $x_3\equiv {\dot{T}}_r$, ${\dot{V}}_2\left(x\right)=0$.

According to LaSalle’s principle, the prismatic link converges to the state again, while the underactuated rotational link reaches its target states when $t=t_f$.

5. Simulation

In this section, MATLAB/Simulink is used for simulation experiments to build the control system, and three groups of simulations are designed to test the control method. The main part of the simulation experiment system includes the dynamic model, two-stage controller and trajectory planning. The simulation flow of the two-stage control strategy is as follows:

①

Use the first stage controller to drive the first joint to the target position, and record the angle and angular velocity of the second joint at this time as $q_{2i}$ and ${\dot{q}}_{2i}$.

②

Taking $q_{2i}$ and ${\dot{q}}_{2i}$ as the initial states of the second passive joint and combining with the target state, trajectory planning is carried out. The second stage controller is used to drive the first joint to move along the planned trajectory.

The basic parameters are shown in

Table 2. The structure parameters.

Link p	${\mathit{m}}_{\mathit{p}}$ (kg)	${\mathit{L}}_{\mathit{p}}$ (m)	${\mathit{l}}_{\mathit{p}}$ (m)	${\mathit{J}}_{\mathit{p}}$ (kg·m2)
1	1.0000	1.0000	0.5000	0.0000
2	1.0000	1.0000	0.5000	0.0833

. The parameters of two controllers (13) and (21) are chosen as $P_1=P_2=1,D_1=D_2=1.8$. The parameters of the DEA are $p_m=0.3,p_c=0.7,{\epsilon }_1=0.0004$.

5.1. Case 1

In this case, we set the initial and final states of the system to carry out simulation experiments to prove the effectiveness of the two-stage control strategy. The initial value and target value are chosen to be

$$\left\{\begin{array}{c}{x}_0={[0.2,-0.1,0,0]}^T\hfill \\ \\ x_d={[-0.3,0.3,0,0]}^T\hfill \end{array}\right.$$

(23)

From the simulation results (Figure 2a,b), the first stage is $t\in [0,9.5]$ s. At this stage, under the action of the first controller, the active prismatic link reaches the target state for the first time at 9.5 s, and the underactuated rotational link moves uncontrollably to the state $[q_2,{\dot{q}}_2]=[0.1\mathrm{rad},0.02\mathrm{rad}/\mathrm{s}]$. In the second stage, using the DEA to plan the trajectory, the active prismatic link tracks the designed trajectory to converge to the target state again at 23 s, and the underactuated rotational link also converges to the target state smoothly. In Figure 2c, the control torque of the active prismatic joint in stage 1 does not exceed $1.5$ N·m, and in stage 2 does not exceed 2 N·m.

From the simulation results, the two-stage control strategy is feasible. Next, we design several groups of comparative simulations, selecting the same simulation parameters as those in reference [39] and verifying the superiority through the simulation results.

5.2. Case 2

In Case 2, we compare our method with the strategy that combines the nilpotent approximation with the iterative control method in [39] to verify the superiority. For this reason, we select the same system model, initial conditions and control objectives as those in [39]. The states are chosen as

$$\left\{\begin{array}{c}{x}_0={[1,-\pi /4,0,0]}^T\hfill \\ \\ x_d={[0,\pi /4,0,0]}^T\hfill \end{array}\right.$$

(24)

Figure 3 shows the simulation result of Case 2, where Figure 3a,b represent the position and speed of two joints, respectively, and Figure 3c represents the driving torque of the first joint. By analyzing Figure 3, the prismatic link reaches the appointed state for the first time at 11.8 s. Then, the active prismatic link tracks the designed trajectory to return to the appointed state again at 24.5 s, and the underactuated rotational link also reaches the target state smoothly.

In reference [39], the underactuated rotational link is not required to meet any conditions before the start of stage 2, and the control flow is more complicated. Compared with the results in [39], our control method has more advantages, as follows:

(1)

Our method does not require the underactuated rotational link to meet any conditions before the start of stage 2, which greatly simplifies the control difficulty.

(2)

Our method can control the system to the target position in 15 s, but the method in [39] needs 38 s. This means that our method can achieve the control goal more quickly.

(3)

Our control process is smoother and more feasible. It can be seen from Figure 3c that the control force of the first joint will not change frequently when using our control method. But in the simulation result of [39], this torque changes sharply frequently, which is very unfavorable to the controller.

(4)

It is worth mentioning that we fully utilize the underactuated coupling constraint relationship, instead of complex coordinate transformation calculations.

5.3. Case 3

In order to eliminate contingency and further prove the universality of our method advantages, we design another group of simulations in Case 3.

In this case, we choose a different target point, considering the periodicity. The states of the system are selected to be the same as in [39], which is convenient for comparison of control effects. The states are chosen to be

$$\left\{\begin{array}{c}{x}_0={[1,-\pi /4,0,0]}^T\hfill \\ \\ x_d={[0.1,(7\pi /6)+2k\pi ,0,0]}^T\hfill \end{array}\right.$$

(25)

where k is an integer.

From the simulation results (Figure 4), the active prismatic link reaches the target state for the first time at 11.5 s. At this time, the underactuated link swings to 4.1395 rad freely. In the next stage, the prismatic link follows the designed trajectory to move back to the target position again at 22.89 s, and the underactuated rotational link also converges to the target state smoothly. Finally, the underactuated rotational link converges to 19$\pi$/6, where k = 1 in Equation (25). Compared with the control method in [39], the underactuated rotational link is not required to meet any conditions before the start of the stage 2, and the control process is faster. It is noteworthy that we make full use of the inherent coupling constraint, thereby eliminating the need for intricate coordinate transformation calculations. This proves that our method is universal.

Therefore, from the whole simulation section, we can draw the conclusion that our proposed two-stage control method can solve the control problem of planar PR robots. And by comparing with [39], it is proved that our method is innovative and superior.

6. Conclusions

We present a control method with two stages based on the physical configuration and underactuated constraints for the underactuated PR robot. The detailed work is as follows:

(1)

The dynamic model of the planar PR underactuated robot is established and the underactuated coupling relationships are derived.

(2)

In stage 1, the active prismatic link is intended to be driven by the PD controller in order to attain the desired position. In stage 2, the active prismatic link tracks the designed oscillation trajectory to the target position under the action of the designed tracking controller, and the underactuated rotational link indirectly converges to the target state during this process.

(3)

The simulation results demonstrate this control strategy’s viability and superiority for planar PR underactuated robots.