Trajectory tracking control of two degrees-of-freedom helicopter based on Udwadia–Kalaba theory

Abstract

In this article, by using Lagrange energy method, we establish the dynamical model of a two degrees-of-freedom helicopter, which is subject to holonomic constraints. A control method based on Udwadia–Kalaba theory is proposed to achieve the trajectory tracking control of the 2-degrees-of-freedom helicopter. Different from traditional methods, this method could solve the constraint force of the mechanical system without adding additional parameters such as Lagrange multipliers. When initial conditions are compatible, we can use the nominal control which is based on Udwadia–Kalaba equation to control 2-degrees-of-freedom helicopter in real time. But when initial conditions have incompatibility, the simulation result could produce divergence phenomenon. To solve the trajectory tracking control problem of 2-degrees-of-freedom helicopter under incompatible initial conditions, a modified controller is proposed. We also make simulation contrast by different control methods to validate the effectiveness and superiority of the modified controller. Simulation results show that the modified controller can drive the 2-degrees-of-freedom helicopter to perfectly track the desired trajectory with less control cost and high control accuracy.

Introduction

At present, there are many methods that focus on the trajectory-tracking control of 2-degrees-of-freedom unmanned helicopters. The control methods are mainly divided into two types: the methods based on model (such as linear–quadratic–regulator (LQR), linear–quadratic–Gaussian (LQG), and sliding mode control)^1,2 and the methods based on experience (such as proportional–integral–derivative (PID), neural network control and fuzzy control).^3,4 However, PID control is the most extensive one in the actual application. Owing to the risk of helicopter flight experiments and the huge cost of sensors, many research institutions use auxiliary flight device to simulate real flight and verify the control algorithm. First, the control algorithm is designed and validated on the auxiliary flight device. After that the designed algorithm is applied to a real small helicopter for actual flight test. According to the above view, this article establishes the dynamical model of the 2DOF helicopter with constraints by using Lagrange approach and designs a new kind of 2DOF helicopter flight control method to realize a small unmanned helicopter’s yaw and pitch movement.

Since Lagrange first established analytical mechanics 200 years ago, the dynamic modeling method of a constrained mechanical system is one of the core contents in the field of dynamical analysis. In most of the research results, the model is based on D’Alembert principle and virtual displacement principle. For constrained mechanical systems, Lagrange multipliers can be used to calculate the constraint force effectively. But in practical application, the calculation of Lagrange multipliers is not an easy task, especially for multi-degree-of-freedom complex systems. Udwadia and Kalaba^5–7 at university of Southern California have carried on the long-term research in this domain and obtained many useful results, which are summarized to Udwadia–Kalaba equation. By using this method, we can simply establish motion equations of mechanical systems which are subject to holonomic constraints. By using this equation, we can obtain the analytic solution of the constraint force without solving the Lagrange multipliers. Udwadia–Kalaba theory has become an important breakthrough in the field of analytic dynamics. ⁸

Udwadia first made a preliminary research of the servo constrained control for the trajectory tracking of mechanical systems. Compared with traditional control methods, this method designed by Udwadia can be achieved without increasing the computational complexity, which also can accomplish precise trajectory control of mechanical systems. In combination with Udwadia–Kalaba theory, Chen systematically proposed the concept of servo constraint control of mechanical systems.^9,10 In 2008, Chen solved the servo constraint problem on the basis of Maggi’s equation.^11–12 It is believed that by reasonable design, servo control can be achieved to meet the needs of trajectory requirements. Based on Udwadia–Kalaba theory and Chen’s viewpoint, Schutte focused on the control problem of nonlinear mechanical systems with holonomic constraints and nonholonomic constraints. Then, Schutte ¹³ proposed a state feedback controller, which is verified by theoretical analysis and numerical simulation in 2010.

There are many research results on trajectory tracking control. For example, we can use PID control, adaptive control, robust control, fuzzy control, neural network control, or the integrated use of these control methods. The control method proposed in this article is essentially different from the conventional methods. In this article, the nominal control method based on Udwadia–Kalaba equation can accomplish the trajectory tracking control task of 2-DOF helicopter. The control torque will be analytically produced by the tail rotor, and the main rotor motor of 2-DOF helicopter can be solved by Udwadia–Kalaba equation. When we consider that initial conditions are compatible, the trajectory tracking control of 2-DOF helicopter can be achieved by traditional Udwadia–Kalaba theory. When initial conditions have incompatibility, we propose a modified controller to solve the trajectory tracking control problem of 2-DOF helicopter. We also make simulation contrast by different control methods to validate control effect further. Simulation results show that the control method possess a great control effect and high control accuracy. In general, this control method can enrich the classical control theory. It also has a very good application prospect in many fields such as automatic production line, logistics transportation, and intelligent robot, but this is the first time that the control method based on Udwadia–Kalaba theory is applied in the helicopter model.

The main contributions of this article are threefold. First, an explicit, closed-form expression of the nominal control required to precisely meet the trajectory tracking requirements of the 2-DOF helicopter is obtained by Udwadia–Kalaba equation. Second, a modified controller is presented on the basis of Udwadia–Kalaba equation to deal with the possible initial condition deviation from the constraints so as to drive the 2-DOF helicopter mechanical system to accurately track the pre-specified constraints. Third, we validate the stability of control system by the theoretical calculation with the Lyapunov function and verify the control effects which include control cost and control accuracy of the modified controller by comparing with other control methods.

The advantages of the control method can be summarized as follows: this control design is applicable to a wide range of tasks (which we summarize as constraint following), including trajectory following and set-point regulation. This control method can also follow nonholonomic constraints.

Udwadia–Kalaba theory for constrained mechanical system

In this section, we talk about the fundamental theory which is called Udwadia–Kalaba theory. The mathematical foundation of Udwadia–Kalaba theory is the Moore–Penrose generalized inverse matrix. We shall define an $m\times n$ matrix H and an $n\times m$ matrix $H^{+}$ . If matrix $H^{+}$ satisfies the following four conditions

$$\begin{array}{c}H{H}^{+}H=H,H^{+}H{H}^{+}=H^{+}\hfill \\ H{H}^{+}=(H{H}^{+}{)}^T,H^{+}H=(H^{+}H{)}^T\hfill \end{array}$$

(1)

we say that matrix $H^{+}$ is matrix H’s Moore–Penrose or M-P. At present, the research on the generalized inverse matrix is very mature, and it is described in detail in the relevant references.^14,15

First, we consider an unconstrained mechanical system, whose configuration is described by the n generalized coordinates $q:=(q_1,q_2,q_3,q_4,\dots ,q_n{)}^T$ . By using Newtonian or Lagrangian method, its equation of motion can be described as ¹⁶

$$M(q,t)\stackrel{··}{q}=Q(q,\stackrel{·}{q},t)$$

(2)

with initial conditions

$$q\left(t_0\right)=q_0,\stackrel{·}{q}\left(t_0\right)={\stackrel{·}{q}}_0$$

(3)

where the n × n matrix $M(q,t)\in R^{n\times n}$ is symmetric and positive definite, $\stackrel{·}{q}=(\mathrm{dq}/\mathrm{dt})\in R^n$ denotes the velocity, $\stackrel{··}{q}=(d^{2}q/d{t}^2)\in R^n$ denotes the acceleration, $Q(q,\stackrel{·}{q},t)\in R^{n\times 1}$ is a generalized active force matrix which acts on the unconstrained mechanical system, and t is independent variable time. ⁹ The generalized acceleration at any time t of the unconstrained mechanical system, which is denoted by a, is thus given by ¹³

$$a(q,\stackrel{·}{q},t):=\stackrel{··}{q}=M^{-1}(q,t)Q(q,\stackrel{·}{q},t)$$

(4)

Then, we should consider the constraints which are presented in the system. We can assume that this mechanical system obeys the following constraint equations which are in the form of

$${\epsilon }_i(q,\stackrel{·}{q},t)=0,i=1,2,\dots ,h$$

(5)

where ${\epsilon }_i(q,\stackrel{·}{q},t)$ is a scalar. Under the assumption that equation (5) is sufficiently smooth, we can obtain a set of constraint equations by differentiating constraints (5) once with respect to time, to obtain a matrix equation in the form ¹⁷

$$A(q,\stackrel{·}{q},t)\stackrel{··}{q}=b(q,\stackrel{·}{q},t)$$

(6)

where $A(q,\stackrel{·}{q},t)\in R^{m\times n}$ is called the constraint matrix and $b(q,\stackrel{·}{q},t)\in R^{m\times 1}$ denotes an m vector (m × 1 column vector). In fact, equation (6) could represent a variety of common constraints: holonomic constraints, nonholonomic constraints, scleronomic constraints, and rheonomic constraints. ¹⁶

In the end, we can put constraint (6) as an additional constraint force which is imposed on the original unconstrained system to ensure the desired trajectory. Thus, the equation of motion of the system is rewritten in the form of

$$M(q,t)\stackrel{··}{q}=Q(q,\stackrel{·}{q},t)+Q_c(q,\stackrel{·}{q},t)$$

(7)

where $Q_c(q,\stackrel{·}{q},t)\in R^{n\times 1}$ denotes an $n\times 1$ constraint force matrix which acts on mechanical system. Our target is to determine the general explicit form of $Q_c(q,\stackrel{·}{q},t)$ at any time t. ¹⁸

The solution of the constraint force in the Udwadia–Kalaba theory

Assuming that there exist m groups of constraint (6) and a corresponding n-dimensional virtual displacement vector v, we have

$$\mathrm{Av}=0$$

(8)

Let $v=M^{-1/2}\mu$ and $B=A{M}^{-1/2}$ , and we can obtain

$$B\mu =0$$

(9)

where $\mu$ is an n-dimensional nonzero vector and B is the constrained matrix.

Let

$$\stackrel{··}{q}=M^{-1/2}\stackrel{··}{r}$$

(10)

where $\stackrel{··}{r}$ is the n-vector (n × 1 column vector). By equation (6), we can obtain

$$B\stackrel{··}{r}=b$$

(11)

$$\stackrel{··}{r}=B^{+}b+(I-B^{+}B)y$$

(12)

where superscript “+” represents Moor–Penrose generalized inverse, y is a random n-dimensional vector, and I is a unit matrix.

Since the sum of work done by the constraint force in the virtual displacement is 0, we have

$$v^T{Q}_c=v^T[M\stackrel{··}{q}-Q]=0$$

(13)

According to the previous definition, we can obtain

$${\mu }^T{M}^{-\frac{1}{2}}{Q}_c={\mu }^T{M}^{-\frac{1}{2}}(M^{\frac{1}{2}}\stackrel{··}{r}-Q)={\mu }^T(\stackrel{··}{r}-M^{-\frac{1}{2}}Q)=0$$

(14)

$${\mu }^T[B^{+}b+(I-B^{+}B)y-M^{-\frac{1}{2}}Q]=0$$

(15)

Since $B\mu =0,{\mu }^T{B}^T=0$ , according to the property of Moor–Penrose inverse, we can get

$${\mu }^T{B}^{+}=0$$

(16)

Equation (15) can be simplified as

$${\mu }^T(y-M^{-\frac{1}{2}}Q)=0$$

(17)

As $\mathrm{\mu }$ is a n-dimensional nonzero vector, then we can obtain

$$y-M^{-\frac{1}{2}}Q=0,y=M^{-\frac{1}{2}}Q$$

(18)

By equation (12) we can obtain

$$\stackrel{··}{r}=B^{+}b+(I-B^{+}B)M^{-\frac{1}{2}}Q$$

(19)

Combining with equation (10), we have

$$\begin{array}{cc}\multicolumn{1}{c}{\stackrel{··}{q}}& =M^{-\frac{1}{2}}{B}^{+}b+M^{-\frac{1}{2}}(I-B^{+}B)M^{-\frac{1}{2}}Q\hfill \\ \multicolumn{1}{c}{}& =M^{-1}Q+M^{\frac{1}{2}}{B}^{+}b-M^{-\frac{1}{2}}{B}^{+}B{M}^{-\frac{1}{2}}Q\hfill \\ \multicolumn{1}{c}{}& =M^{-1}Q+M^{-\frac{1}{2}}{B}^{+}(b-B{M}^{-\frac{1}{2}}Q)\hfill \end{array}$$

(20)

Multiplying the matrix M on both sides of equation (20) and substituting $B=A{M}^{-1/2}$ , we can obtain

$$M\stackrel{··}{q}=Q+M^{\frac{1}{2}}{\left(A{M}^{-\frac{1}{2}}\right)}^{+}(b-A{M}^{-1}Q)$$

(21)

Equation (21) is called the Udwadia–Kalaba equation. Thus, constraint force of constrained system can be written in the form^19–23

$$Q_c=M^{\frac{1}{2}}{\left(A{M}^{-\frac{1}{2}}\right)}^{+}(b-A{M}^{-1}Q)$$

(22)

Remark 1

It is clear that the Udwadia–Kalaba equation (21) just uses some physical quantities. And there are no additional parameters such as Lagrange multipliers or similar variables in the constraint force equation (22). Thus, in practical applications, we can avoid the complex variables to solve the practical problems. Therefore, the Udwadia and Kalaba theory is more applicable to the actual in the aspect of dynamics analysis. ²⁴

The dynamical model of 2-DOF helicopter

The 2-DOF unmanned helicopter is a complex nonlinear, strongly coupled multivariable system. ²⁵ Nowadays, there are two common modeling methods for small unmanned helicopter. One of them is to obtain input and output data through experiments. For instance, the actual flight test data are directly established through the neural network to establish the dynamics model. But the controller based on this model needs a lot of experience or expert data. The other one is to obtain linear or nonlinear dynamic differential equations by theoretical calculation. ²⁵ Some scholars consider the specificity of small helicopter and modify the existing model of large helicopters. They may be based on the structural features of small helicopter directly, and corresponding differential equations are established. Then, the unknown parameters in the equation are obtained by wind tunnel test. ²⁵ The equations derived by these methods are very complex and have many parameters, which limit the practical application. In this article, the dynamical model of a 2-DOF small unmanned helicopter is established by Lagrange method, and the specific object diagram ²⁶ is shown in Figure 1.

Figure 1.

2-DOF helicopter material object.

The 2-DOF helicopter which includes two main rotor blades, two stabilizing wing blades, and a tail rotor uses a single-cylinder two-stroke engine to drive. ²⁵ Compared with the general helicopter, the main rotor blade of the 2-DOF helicopter does not have freedom of flapping motion. There is a stabilizer fin under the main rotor. The main rotor adjusts the lift by changing the overall angle. The course is maintained or changed by the lateral force generated by the change of the rotor angle. ¹⁷ Through the above-mentioned physical model, the simplified model of the 2-DOF helicopter is shown in Figure 2 with rigid body and particle handling. ²⁷

Figure 2.

Simplified model of 2-DOF helicopter.

In the simplified dynamical model of 2-DOF helicopter, we only consider the force generated by the tail rotor and ignore its mass since the mass of the tail rotor is very small. The cross bar $l_2$ is regarded as homogeneous slender rod. The main rotor and tail rotor are driven by direct current (DC) motor to produce the pitch angles $\theta$ and the yaw angle $\phi$ of 2-DOF helicopter. The pitch angles $\theta$ reflect promotion and demotion of the helicopter. ²⁵ The yaw angle $\phi$ reflects left and right courses of the helicopter. ²⁸ Then, the main rotor and motor are regarded as a whole, and the tail rotor and motor are also regarded as a whole. Since it is all considered as a multi-rigid body structure, we ignore the flapping motion of the main rotor, namely, the force problem of different blades. It is assumed that the $c_2$ coincides with o and we can further simplify the process of mathematical modeling. The variables and parameters of dynamical model are described in Table 1.

Table 1.

Variables and parameters of the 2-DOF helicopter.

Variables and parameters	Symbol definition
o	The center of the coordinate system
${\mathit{c}}_1$	The center of gravity of vertical support bar
${\mathit{c}}_2$	The center of gravity of the cross bar
${\mathit{l}}_1$	The vertical support rod
${\mathit{l}}_2$	The cross bar
${\mathit{l}}_3$	The distance between the center of gravity of the cross bar $c_2$ and the geometric center of the main rotor
${\mathit{m}}_1$	The mass of a vertical support rod
${\mathit{m}}_2$	The mass of cross bar
${\mathit{m}}_3$	The mass of main rotor with motor
$\mathit{\theta }$	The pitch angles
$\mathbf{\varpi }$	The yaw angles
R	The radius of a cylindrical bar
g	The gravitational acceleration
${\mathit{J}}_1,{\mathit{J}}_2$	The moment of inertia
${\mathit{E}}_K$	The kinetic energy of system
${\mathit{E}}_p$	The potential energy of the system

2-DOF: 2-degrees-of-freedom.

The kinetic energy of the system includes three parts: the vertical support rod $l_1$ , the cross bar $l_2$ , and the main rotor with motor.

• 1. The kinetic energy of the vertical support rod $l_1$ :

The moment inertia of the Z axis is

$$J_1=\frac{1}{2}{m}_1{R}^2$$

(23)

Therefore, the kinetic energy of the vertical support rod 1 is

$$E_{K_1}=\frac{1}{2}{J}_1{\stackrel{·}{\phi }}^2=\frac{1}{2}\times \frac{1}{2}{m}_1{R}^2{\stackrel{·}{\phi }}^2$$

(24)

• 2. At this point, the movement of the cross bar 2 is regarded as the rotation of the X axis, the Y axis and the Z axis, and its moment inertia is

$$J_2=\frac{1}{3}{m}_2{l}_2^2$$

(25)

The kinetic energy of the cross bar $l_2$ is

$$E_{K_2}=\frac{1}{2}{J}_2({\stackrel{·}{\theta }}^2+{\stackrel{·}{\phi }}^2)=\frac{1}{6}{m}_2{l}_2^2({\stackrel{·}{\theta }}^2+{\stackrel{·}{\phi }}^2)$$

(26)

• 3. Here, we assume that $l_3$ is half the length of $l_2$ . According to the coordinate relationship, the position coordinates are described as

$$\left[\begin{array}{ccc}{x}_3\hfill & y_3\hfill & z_3\hfill \end{array}\right]=\left[\begin{array}{ccc}{l}_3\cos \theta \cos \phi \hfill & l_3\cos \theta \sin \phi \hfill & l_3\sin \theta \hfill \end{array}\right]$$

Its velocity coordinates can be calculated as

$$v_3=[\begin{array}{ccc}-l_3\cos \theta \cos \phi -l_3\cos \theta \sin \phi \stackrel{·}{\phi }\hfill & l_3\sin \theta \stackrel{·}{\theta }\sin \phi +l_3\cos \theta \cos \phi \stackrel{·}{\phi }\hfill & l_3\cos \theta \stackrel{·}{\theta }\hfill \end{array}{]}^T$$

Therefore, the kinetic energy of the main rotor with motor is

$$E_{K_3}=\frac{1}{2}{m}_3{v}_3^T{v}_3$$

(27)

• 4. Eventually, the kinetic energy of the overall system can be written as

$$E_K=E_{K_1}+E_{K_2}+E_{K_3}$$

(28)

The gravitational potential energy of the system can also be written as

$$E_p=-m_{1}g\frac{1}{2}{l}_1+m_{3}g{l}_4\sin \theta$$

(29)

Then, we can obtain

$$L=E_K-E_p$$

(30)

Equation (30) can be put into the Lagrange equation (31)

$$\frac{d}{\mathrm{dt}}\frac{\partial L}{\partial {\stackrel{·}{q}}_j}-\frac{\partial L}{\partial q_j}={\tau }_k(j=1,2,\dots ,n)$$

(31)

where ${\tau }_k$ denotes the constraint torque vector.

According to the Lagrange method, the general equation of motion of a helicopter with 2DOF can be written as

$$\tau =D\left(q\right)\stackrel{··}{q}+H(q,\stackrel{·}{q})+G\left(q\right)$$

(32)

where

$$\tau =\left[\begin{array}{c}{\tau }_1\hfill \\ \multicolumn{1}{c}{{\tau }_2}\end{array}\right],q=\left[\begin{array}{c}\theta \hfill \\ \multicolumn{1}{c}{\phi }\end{array}\right],\stackrel{·}{q}=\left[\begin{array}{c}\stackrel{·}{\theta }\hfill \\ \multicolumn{1}{c}{\stackrel{·}{\phi }}\end{array}\right],\stackrel{··}{q}=\left[\begin{array}{c}\stackrel{··}{\theta }\hfill \\ \multicolumn{1}{c}{\stackrel{··}{\phi }}\end{array}\right]$$

$$\begin{array}{c}D\left(q\right)=\hfill \\ \left[\begin{array}{cc}\frac{1}{3}{m}_2{l}_2^2+\frac{1}{4}k{(a_1)}^2{(a_2)}^2+\frac{1}{4}k{(b_1)}^2\hfill & -\frac{1}{64}k[\cos (2\theta +2\phi )-\cos (2\theta -2\phi )]\hfill \\ \multicolumn{1}{c}{-\frac{1}{64}k[\cos (2\theta +2\phi )-\cos (2\theta -2\phi )]}& \frac{1}{2}{m}_1{R}^2+\frac{1}{3}{m}_2{l}_2^2+\frac{1}{4}k{(a_2)}^2{(b_1)}^2+\frac{1}{8}k{[\cos (\theta +\phi )-\cos (\theta -\phi )]}^2\hfill \end{array}\right]\hfill \end{array}$$

$$\begin{array}{c}H(q,\stackrel{·}{q})=\hfill \\ \left[\begin{array}{c}-\frac{1}{4}k(a_1{b}_2\stackrel{·}{\phi }+a_2{b}_1\stackrel{·}{\theta })\stackrel{·}{\theta }+\frac{1}{32}k(c_1{d}_{2}2\stackrel{·}{\phi }+c_2{d}_{1}2\stackrel{·}{\theta })\stackrel{·}{\phi }-\frac{1}{4}k{c}_1{\stackrel{·}{\theta }}^2+\frac{1}{8}k{c}_1[{(\stackrel{·}{\phi })}^2+c_2\stackrel{·}{\phi }-{(a_2)}^2{(\stackrel{·}{\theta })}^2+\left(\stackrel{·}{\theta }{)}^2\right]-\frac{1}{16}k{c}_2\stackrel{·}{\theta }\stackrel{·}{\phi }{d}_1\hfill \\ \multicolumn{1}{c}{\frac{1}{4}k\stackrel{·}{\phi }(b_2{b}_1\stackrel{·}{\phi }-a_2{a}_1\stackrel{·}{\theta })-\frac{1}{8}k{c}_1{c}_2+\frac{1}{4}k{(b_1)}^2{d}_2\stackrel{·}{\phi }-\frac{1}{2}k(a_1{b}_2\stackrel{·}{\theta }+a_1{b}_2\stackrel{·}{\phi })+\frac{1}{16}k(c_1{d}_2\stackrel{·}{\phi }+d_1{c}_2\stackrel{·}{\theta })\stackrel{·}{\theta }-\frac{1}{8}k{(b_1)}^2{c}_2{(\stackrel{·}{\phi })}^2}\\ \multicolumn{1}{c}{-\frac{1}{8}k{(b_1)}^2{e}_1\stackrel{·}{\phi }-\frac{1}{8}k{(a_1)}^2{c}_2{(\stackrel{·}{\theta })}^2+\frac{1}{8}k{(b_1)}^2{c}_2{(\stackrel{·}{\phi })}^2-\frac{1}{32}k{c}_1{e}_1\stackrel{·}{\theta }\stackrel{·}{\phi }}\end{array}\right]\hfill \end{array}$$

$$G\left(q\right)=\left[\begin{array}{c}\frac{1}{2}{m}_{3}g{l}_2{b}_1+\frac{1}{8}k{c}_1{(b_2)}^2\hfill \\ \multicolumn{1}{c}{\frac{1}{8}k{(b_1)}^2{c}_2}\end{array}\right]$$

Remark 2

$D(q)$ is a positive definite inertia matrix, $H(q,\stackrel{·}{q})$ is the centrifugal force and Coriolis force vector, and $G(q)$ is the gravity vector.

In matrix vectors, let $a_1:=\sin \theta$ , $a_2:=\sin \phi$ , $b_1:=\cos \theta$ , $b_2:=\cos \phi$ , $c_1:=\sin 2\theta$ , $c_2:=\sin 2\phi$ , $d_1:=\cos 2\theta ,d_2:=\cos 2\phi$ , $e_1:=\sin 4\phi$ , and $k:=m_3{l}_2^2$ .

The 2-DOF helicopter dynamics model in generalized coordinates can be written as an unconstrained equation:

where

$$\tau =0D\left(q\right)=-H(q,\stackrel{·}{q})-G\left(q\right)$$

(33)

The system dynamics equations are denoted by Udwadia–Kalaba theory ²⁶

$$M(q,t)\stackrel{··}{q}=Q(q,\stackrel{·}{q},t)+Q_c(q,\stackrel{·}{q},t)$$

(34)

By combining equations (33) and (34), we can get

$$M(q,t)=D\left(q\right);Q(q,\stackrel{·}{q},t)=-H(q,\stackrel{·}{q})-G\left(q\right)$$

From equation (22), we know

$$Q_c=M^{\frac{1}{2}}{\left(A{M}^{-\frac{1}{2}}\right)}^{+}(b-A{M}^{-1}Q)$$

(35)

Under the effect of the constraint force $Q_c$ , the dynamic system (34) will satisfy the given constraint equation (6).^29,30

Trajectory tracking control of 2-DOF helicopter

Here, the pitch angle $\theta$ and the yaw angle $\phi$ are specified to define the trajectory of the 2-DOF helicopter. The change in pitch angle $\theta$ can be achieved by changing the lift of the main rotor. The change in the yaw angle $\phi$ can be achieved by changing the pulling force of the tail rotor. ²⁹ We also need to find out how much output torque the main rotor and tail rotor motor need to produce to achieve this trajectory. The concrete steps can be described as follows:

• 1. It is assumed that the system needs to meet the trajectory tracking constraints ²⁴ due to some performance requirements, which are given as

  $$\{\begin{array}{c}\theta +\phi =\frac{\pi }{4}\hfill \\ \multicolumn{1}{c}{\phi =1.5\sin \left(\frac{\pi }{6}t\right)}\end{array}$$

  (36)

• 2. Differentiating the constraint equation (36) with respect to t twice, ²⁴ the second-order constraints can be obtained

  $$\{\begin{array}{c}\stackrel{··}{\theta }+\stackrel{··}{\phi }=0\hfill \\ \multicolumn{1}{c}{\stackrel{··}{\phi }=-\frac{{\pi }^2}{24}\sin \left(\frac{\pi }{6}t\right)}\end{array}$$

  (37)

Hence, the system’s constraints can be written in the form of equation ³¹

$$A(q,\stackrel{·}{q},t)\stackrel{··}{q}=b(q,\stackrel{·}{q},t)$$

(38)

where

$$A=\left[\begin{array}{cc}1\hfill & 1\hfill \\ \multicolumn{1}{c}{0}& 1\hfill \end{array}\right],\stackrel{··}{q}=\left[\begin{array}{c}\stackrel{··}{\theta }\hfill \\ \multicolumn{1}{c}{\stackrel{··}{\phi }}\end{array}\right],b=\left[\begin{array}{c}0\hfill \\ \multicolumn{1}{c}{-\frac{{\pi }^2}{24}\sin \left(\frac{\pi }{6}t\right)}\end{array}\right]$$

Remark 3

We handle some performance requirements of the helicopter as constraints. As a simple illustration, we just introduce holonomic constraint (36) as the desired system performance to demonstrate the effectiveness of the control method based on Udwadia–Kalaba theory. Of course, our method can also be used to deal with nonholonomic constraints. We can differentiate the nonholonomic constraints with respect to t once and obtain the second-order form of constraint equation which is needed in the Udwadia–Kalaba theory.

• 3. According to Udwadia–Kalaba theory, in order to meet the trajectory requirements of 2-DOF helicopter, the corresponding motor torque should be exerted on the main rotor and tail rotor. Then, comparing equations (32), (34), and (35), we can get

$$\tau =M^{\frac{1}{2}}{\left(A{M}^{-\frac{1}{2}}\right)}^{+}(b-A{M}^{-1}Q)$$

(39)

where $\tau =\left[\begin{array}{c}{\tau }_1\hfill \\ \multicolumn{1}{c}{{\tau }_2}\end{array}\right]$ and $\tau$ is a constraint control torque vector.

Remark 4

We can validate the effectiveness of the designed control system by detailed theoretical analysis. ⁷

Initial conditions, compatibility, and incompatibility

The phenomenon of incorrect initial conditions

In general, if we apply the Udwadia–Kalaba theory directly, the constraints must be satisfied at each instant of time during the operation including the initial time $(t=0)$ . ¹⁸ However, it is usually quite difficult to satisfy these constraints at the initial time in practice due to a variety of reasons. ³¹ We can use the following example to explain the phenomenon of incorrect initial conditions.

Differentiating the constraints equation (36) with respect to t once, ¹⁷ we can obtain

$$\{\begin{array}{c}\stackrel{·}{\theta }+\stackrel{·}{\phi }=0\hfill \\ \multicolumn{1}{c}{\stackrel{·}{\phi }=\frac{\pi }{4}\cos \left(\frac{\pi }{6}t\right)}\end{array}$$

(40)

We will represent equations (36) and (40) in the matrix form

$$A(q,\stackrel{·}{q},t)q=d(q,t)$$

(41)

$$A(q,\stackrel{·}{q},t)\stackrel{·}{q}=c(q,t)$$

(42)

where

$$\begin{array}{l}A=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right],q=\left[\begin{array}{c}\theta \\ \phi \end{array}\right],\stackrel{.}{q}=\left[\begin{array}{c}\stackrel{.}{\theta }\\ \stackrel{.}{\phi }\end{array}\right],\\ c=\left[\begin{array}{c}0\\ \frac{\pi }{4}\cos \left(\frac{\pi }{6}t\right)\end{array}\right],d=\left[\begin{array}{c}\frac{\pi }{4}\\ 1.5\sin \left(\frac{\pi }{6}t\right)\end{array}\right]\end{array}$$

Equation (37) can be decomposed as

$$\stackrel{··}{\phi }=-\frac{{\pi }^2}{24}\sin \left(\frac{\pi }{6}t\right)$$

(43)

Integrating equation (43) with respect to t twice, we can obtain

$$\phi =1.5\sin \left(\frac{\pi }{6}t\right)+\mathrm{at}+h$$

(44)

According to Udwadia–Kalaba theory, combining equation (6) with equations (43) and (44), we can know that the control torque $\tau$ can only control the constraint trajectory (43). The zeroth-order constraint trajectory (44) cannot be controlled. When initial conditions have compatibility, if $t=0$ , equation (44) can be solved by the way of multiple integrals and we can obtain $a=0$ and h = 0. Hence, the zeroth-order constraint trajectory is $\phi =1.5\sin (\pi /6t)$ , which is consistent with equation (36). Then, we use the constraint force $Q_c$ which is generated by Udwadia–Kalaba theory to control the constraint trajectory (37), which equals to control the zeroth-order form of constraint trajectory (36) indirectly. Then, the trajectory of the 2-DOF helicopter is controlled. When initial conditions are incompatible, the value of “a” and “h” in equation (44) are not equal to zero. Therefore, by controlling the constraint trajectory (37), it is impossible to guarantee that we obtain the zeroth-order form of constraint trajectory (36) which satisfies our expectation. Then, through the simulation verification and result analysis in section “Simulation test and result analysis,” we will further verify the correctness of the theoretical analysis.

Remark 5

In fact, Udwadia–Kalaba equation only solves the problem by substituting second-order constraint. But the zeroth-order constraint which corresponds to the second-order constraint is not the only one. We can write out its general solution by mathematical calculation. Then, we can determine its specific general solution form by initial conditions.

The solution of incorrect initial conditions

In order to deal with the problem of initial conditions which have incompatibility with the desired constraints, we attempt to modify the controller (39).

Assumption

For each $(q,t)\in R^n\times R$ , $A(q,\stackrel{·}{q},t)$ is a full-rank matrix, it is indicated that $A(q,\stackrel{·}{q},t)A^T(q,\stackrel{·}{q},t)$ is invertible matrix.

On the basis of the Udwadia–Kalaba theory, a modified controller can be given as ²⁴

$${\tau }^{*}=p_1(q,\stackrel{·}{q},t)+p_2(q,\stackrel{·}{q},t)$$

(45)

where

$$\begin{array}{l}{p}_1\left(q,\stackrel{.}{q},t\right):=M^{\frac{1}{2}}\left(q,t\right){\left(A\left(q,\stackrel{.}{q},t\right)M^{-\frac{1}{2}}\left(q,t\right)\right)}^{+}\\ \left(b\left(q,\stackrel{.}{q},t\right)-A\left(q,\stackrel{.}{q},t\right)M^{-1}\left(q,t\right)Q\right)\end{array}$$

(46)

$$\begin{array}{l}{p}_2\left(q,\stackrel{.}{q},t\right):=-kM\left(q,t\right)A^T\left(q,\stackrel{.}{q},t\right)\\ {(A\left(q,\stackrel{.}{q},t\right)A^T\left(q,\stackrel{.}{q},t\right))}^{-1}{P}^{-1}\beta \left(q,\stackrel{.}{q},t\right)\end{array}$$

(47)

k is a positive scalar and P is a positive definite and real symmetric matrix.

Remark 6

$p_1(q,\stackrel{·}{q},t)$ is basically the same as $\tau$ (equation (39); nominal control) which is generated by Udwadia–Kalaba theory. $p_2(q,\stackrel{·}{q},t)$ is mainly used to solve the incompatible problem of initial conditions.

Let

$$\beta (q,\stackrel{·}{q},t):=A(q,\stackrel{·}{q},t)\stackrel{·}{q}-c(q,t)$$

(48)

Differentiating the constraint equation (48) with respect to t, we can obtain

$$\stackrel{·}{\beta }(q,\stackrel{·}{q},t):=A(q,\stackrel{·}{q},t)\stackrel{··}{q}-b(q,\stackrel{·}{q},t)$$

(49)

Theorem

Subject to Assumption, consider system (32). The control (45) renders the following performance.

1. Uniform stability: For each $\zeta >0$ , there exists $\epsilon >0$ such that if $\beta (·)$ is any solution with $\Vert \beta (t_0)\Vert <\epsilon$ , then $\Vert \beta (t)\Vert <\zeta$ for all $t\ge t_0$ .

2. Convergence to zero

$$\underset{t\to \infty }{lim}\beta =0$$

(50)

Proof

We choose a rational Lyapunov function

$$V(\beta ,t)={\beta }^{T}P\beta$$

(51)

By analyzing this Lyapunov function, we can obtain

$${\lambda }_{\min }\left(P\right)\Vert \beta {\Vert }^2\le {\beta }^{T}P\beta \le {\lambda }_{\max }\left(P\right)\Vert \beta {\Vert }^2$$

(52)

where ${\lambda }_{\min }(P)$ denotes the minimum eigenvalue of the positive definite and real symmetric matrix P and ${\lambda }_{\max }(P)$ denotes the maximum eigenvalue of the positive definite and real symmetric matrix P.

Since ${\lambda }_{\min }(P)$ is positive, we can conclude that

$$V(\beta ,t)\ge 0$$

(53)

Differentiating equation (51), we can obtain

$$\begin{array}{l}\stackrel{.}{V}=2{\beta }^{T}P\stackrel{.}{\beta }=2{\beta }^{T}P\left(A\stackrel{··}{q}-b\right)\\ =2{\beta }^{T}P(A\left(M^{-1}Q+M^{-1}\left(p_1+p_2\right)-b\right)\end{array}$$

(54)

Now, we shall analyze each term of equation (54) separately.

First, we know

$$\begin{array}{l}A{M}^{-1}Q+A{M}^{-1}{p}_1-b\\ =A{M}^{-1}Q+A{M}^{-1}\left[M^{\frac{1}{2}}{\left(A{M}^{-\frac{1}{2}}\right)}^{+}\left(b-A{M}^{-1}Q\right)\right]-b\\ =b-b=0\end{array}$$

(55)

Then, combining equations (47) and (54), we can obtain

$$\begin{array}{l}2{\beta }^{T}PA{M}^{-1}{p}_2=2{\beta }^{T}PA{M}^{-1}\left(-kM{A}^T{(A{A}^T)}^{-1}{P}^{-1}\beta \right)\\ =-2k{\Vert \beta \Vert }^2\end{array}$$

(56)

Substituting equations (55) and (56) into equation (54), we can obtain

$$\stackrel{·}{V}(\beta ,t)=-2k\Vert \beta {\Vert }^2$$

(57)

Hence, we have

$$\stackrel{·}{V}(\beta ,t)\le 0$$

(58)

By Lyapunov second method stability criterion, and combining equations (53) and (58), we can obtain the system is stable. In addition, we can conclude the convergence of $\beta$ to 0 as $t\to \infty$ . Hence, by adding the additional term $p_2$ (equation (47)), the incompatible problem of initial conditions is solved perfectly.

Remark 7

$\beta (q,\stackrel{·}{q},t)$ can be expanded to a generalized form $\beta =f(q,\stackrel{·}{q},t)$ . As for zeroth-order trajectory tracking constraints (36), we have redefined $\beta (q,\stackrel{·}{q},t)$ as $\beta (q,\stackrel{·}{q},t)=\stackrel{·}{e}\prime +\gamma e\prime =\left[\begin{array}{c}{\stackrel{·}{e}}_1^2\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\stackrel{·}{e}}_m^2}\end{array}\right]+\gamma \left[\begin{array}{c}{e}_1^2\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{e_m^2}\end{array}\right]$ and $\left[\begin{array}{c}{\stackrel{·}{e}}_1\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\stackrel{·}{e}}_m}\end{array}\right]=:\stackrel{·}{e}=A\stackrel{·}{q}-c,\left[\begin{array}{c}{e}_1\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{e_m}\end{array}\right]=:e=\mathrm{Aq}-d$ , and $\gamma$ is a positive scalar. Then, we can obtain $\beta \to 0$ so that e → 0.

Our aim is to render the movement of 2-DOF helicopter to follow the target constraint trajectory (36). When initial conditions have compatibility, we can use the traditional Udwadia–Kalaba theory to solve the trajectory tracking control problem. When initial conditions have incompatibility, we can use the modified controller ${\tau }^{*}$ . Section “Initial conditions, compatibility and incompatibility” only relates to the theoretical proof. The above conclusions will be further verified by simulation experiments in section “Simulation test and result analysis.”

A specific control flowchart is shown in Figure 3.

Figure 3.

Modeling and control flowchart.

Simulation test and result analysis

In order to accomplish the trajectory tracking control of the simplified model of 2-DOF helicopter, we use MATLAB software for simulation test. Runge–Kutta method is used to solve the system’s ordinary differential equations. ²⁴ According to the actual 2-DOF helicopter, we estimate the relevant parameters and the specific simulation parameters which are shown in Table 2.

Table 2.

Parameters for simulation.

Simulation parameters	Numerical value	Unit
${\mathit{l}}_1$	0.42	m
${\mathit{l}}_2$	0.58	m
${\mathit{l}}_3$	0.29	m
${\mathit{m}}_1$	2.85	kg
${\mathit{m}}_2$	1.42	kg
${\mathit{m}}_3$	0.38	kg
R	0.03	m
g	9.80	m/s2

Case 1. The simulation results and analysis of compatible initial conditions

First of all, according to the actual motion state, initial conditions in the simulation are set. By letting $t=0$ in equations (36) and (40), the values of initial conditions can be calculated. They are given in Table 3.

Table 3.

Initial condition in Case 1.

$\theta$	$\pi /4(\mathrm{rad})$	$\stackrel{·}{\theta }$	$-\pi /4(\mathrm{rad}/\mathrm{s})$
$\phi$	0 (rad)	$\stackrel{·}{\phi }$	$\pi /4(\mathrm{rad}/\mathrm{s})$

Here, we give the desired trajectory of the 2-DOF helicopter as

$${\theta }_d=\frac{\pi }{4}-1.5\sin \left(\frac{\pi }{6}t\right),{\phi }_d=1.5\sin \left(\frac{\pi }{6}t\right),{\theta }_d+{\phi }_d=\frac{\pi }{4}$$

(59)

The control torques are generated by the two motors, which can be obtained from equation (39). The control torque of the main rotor motor is shown in Figure 4 and the control torque of the tail rotor motor is shown in Figure 5. Figures 6 and 7 show the pitch angle $\theta$ and the yaw angle $\phi$ curve as a function of time t of the simplified 2-DOF helicopter under the control torques ${\tau }_1$ and ${\tau }_2$ .

Figure 4.

Case 1: The control torque of the main rotor motor ${\tau }_1$ .

Figure 5.

Case 1: The control torque of the tail rotor motor ${\tau }_2$ .

Figure 6.

Case 1: The curve of the pitch angle $\theta$ .

Figure 7.

Case 1: The change curve of yaw angle $\phi$ .

The plot of error $\theta$ as a function of time t is presented in Figure 8, where error $\theta$ is denoted as the error between the actual $\theta$ and the desired ${\theta }_d:\theta -{\theta }_d$ . The plot of error $\phi$ as a function of time t is presented in Figure 9, where error $\phi$ is denoted as the error between the actual $\phi$ and the desired ${\phi }_d:\phi -{\phi }_d$ . ¹⁸ Figure 10 shows the composite trajectory change curve $(\theta +\phi )$ of the 2-DOF helicopter.

Figure 8.

Case 1: Error $\theta$ $(\theta -{\theta }_d)$ curve of 2-DOF helicopter.

Figure 9.

Case 1: Error $\phi (\phi -{\phi }_d)$ curve of 2-DOF helicopter.

Figure 10.

Case 1: The composite trajectory change curve of 2-DOF helicopter $(\theta +\phi )$ .

From Figures 6 and 7, we can see that there is no divergence phenomenon. From Figures 8 and 9, we can know that the final error value is of the order of ${10}^{-4}$ . The error is very small so that it is negligible.

Therefore, when initial conditions are compatible, the simplified model of the 2-DOF helicopter is able to achieve the required trajectory tracking constraint under the nominal control $\tau =p_1$ which is directly obtained by solving Udwadia–Kalaba equation.

Case 2: the simulation results and analysis of incompatible initial conditions with different control methods

Now, we give a set of initial conditions which are shown in Table 4. It is obvious to find that initial conditions do not satisfy the constraints (36) and (40).

Table 4.

Initial condition in Case 2.

$\theta$	$\pi /8$ (rad)	$\stackrel{·}{\theta }$	$-\pi /3$ (rad/s)
$\phi$	0.5 (rad)	$\stackrel{·}{\phi }$	$\pi /3$ (rad/s)

Aiming at the incompatible initial conditions which are mentioned above, we use the modified controller ${\tau }^{*}$ which is introduced in section “Initial conditions, compatibility and incompatibility” and is shown in equation (45).

Here, we still give the same incompatible initial values as in Table 4 and choose the control parameters as $k=10;\gamma =2;P=[10;01]$ .

Compared with the newly developed control method in Yu et al. and Sun et al

We chose the nominal control $\tau =p_1$ , which is a relatively new approach^16,18 in the field of control to make simulation contrast. The simulation results are shown in Figures 11–21. The desired trajectory (59) is represented by red solid line and the actual trajectory is represented by color line other than red.

Figure 11.

Case 2: Comparison of the tracking performance $\theta$ with nominal control $p_1$ .

Figure 12.

Case 2: Comparison of the tracking error $\theta$ with nominal control $p_1$ .

Figure 13.

Case 2: Comparison of the tracking performance $\phi$ with nominal control $p_1$ .

Figure 14.

Case 2: Comparison of the tracking error $\phi$ with nominal control $p_1$ .

Figure 15.

Case 2: Comparison of the composite trajectory change curve of 2-DOF helicopter $(\theta +\phi )$ with nominal control $p_1$ .

Figure 16.

Case 2: Error $(\theta +\phi )$ curve of 2-DOF helicopter with nominal control $p_1$ .

Figure 17.

Case 2: Comparison of the accumulative tracking error $(\theta +\phi )$ with nominal control $p_1$ .

Figure 18.

Case 2: The control torque of the main rotor motor ${\tau }_1$ .

Figure 19.

Case 2: Comparison of the control cost ${\tau }_1$ with nominal control $p_1$ .

Figure 20.

Case 2: The control torque of the tail rotor motor ${\tau }_2$ .

Figure 21.

Case 2: Comparison of the control cost ${\tau }_2$ with nominal control $p_1$ .

From Figures 11, 13, and 15, we can see that when initial conditions have incompatibility, the actual trajectory of the 2-DOF helicopter under the nominal control $p_1$ is obviously divergent, which is not in line with the desired trajectory. Then, we can come to a conclusion that the nominal control $\tau$ directly obtained by Udwadia–Kalaba theory cannot solve the trajectory constraint problem with incompatible initial conditions.

Form Figures 11 to 17, we can also see that the trajectory tracking error which is controlled by modified controller ${\tau }^{*}$ is obviously smaller than nominal control $p_1$ . The modified controller ${\tau }^{*}=p_1(q,\stackrel{·}{q},t)+p_2(q,\stackrel{·}{q},t)=[{\tau }_1{\tau }_2{]}^T$ which has smoother variation trend and smaller control cost than the nominal control $\tau =p_1$ is shown in Figures 18–21.

Compared with standard control methods

For further comparison, we select the standard LQR and sliding mode control methods which are usually used in the 2-DOF helicopter model to make simulation contrast. The simulation results are shown in Figures 22–32. The desired trajectory (59) is represented by red solid line and the actual trajectory is represented by other colored line.

Figure 22.

Case 2: Comparison of the tracking performance $\theta$ under different control.

Figure 23.

Case 2: Comparison of the tracking error $\theta$ under different control.

Figure 24.

Case 2: Comparison of the tracking performance $\phi$ under different control.

Figure 25.

Case 2: Comparison of the tracking error $\phi$ under different control.

Figure 26.

Case 2: Comparison of the composite trajectory change curve of 2-DOF helicopter $(\theta +\phi )$ under different control methods.

Figure 27.

Case 2: Error $(\theta +\phi )$ curve of 2-DOF helicopter under different control methods.

Figure 28.

Case 2: Comparison of the accumulative tracking error $(\theta +\phi )$ under different control methods.

Figure 29.

Case 2: The control torque of the main rotor motor ${\tau }_1$ under different control.

Figure 30.

Case 2: Comparison of the control cost ${\tau }_1$ of the different control methods.

Figure 31.

Case 2: The control torque of the tail rotor motor ${\tau }_2$ under different control.

Figure 32.

Case 2: Comparison of the control cost ${\tau }_2$ of the different control methods.

From Figures 22, 24, and 26, we can see that the actual trajectory of the 2DOF helicopter can perfectly follow the desired trajectory after a short period of vibration by using modified controller ${\tau }^{*}$ . Different control methods are also used to make comparison, which include traditional LQR control and sliding mode control. From Figures 23, 25, and 27, we can see that the trajectory tracking error converges to zero smoothly under the modified controller ${\tau }^{*}$ . While under the LQR and sliding mode control, the trajectory tracking error converges to zero with obvious range of fluctuation. To observe the error more intuitively, we calculate the accumulative trajectory tracking error $(\theta +\phi )$ from 0 to 60 s, which is shown in Figure 28 by the three-dimensional histogram. The accumulative trajectory tracking error $(\theta +\phi )$ which is controlled by ${\tau }^{*}$ is obviously smaller than other control methods from Figure 28.

The required control torques ${\tau }^{*}=p_1(q,\stackrel{·}{q},t)+p_2(q,\stackrel{·}{q},t)=[{\tau }_1{\tau }_2{]}^T$ which have smoother variation trend and smaller control cost than other standard control methods are shown in Figures 29–32. From Figure 30, we can find that the control cost of the main rotor motor ${\tau }_1$ with sliding mode control is nearly 1.61 times and 4.1 times of that with LQR control, modified controller ${\tau }^{*}$ . Form Figure 32, we can find that the control cost of the tail rotor motor ${\tau }_2$ with sliding mode control is nearly 1.54 times and 4.7 times of that with LQR control, modified controller ${\tau }^{*}$ .

Therefore, according to the obtained results, we can make a conclusion that the modified controller ${\tau }^{*}$ can perfectly solve initial conditions of incompatible problem of the trajectory tracking control of 2-DOF helicopter with less control cost and higher control accuracy.

Summary

1. The constraint control based on Udwadia–Kalaba equation can accomplish the trajectory tracking control task of 2-DOF helicopter’s model by theoretical analysis and numerical simulation. Compared to Lagrange method, there are no additional parameters such as Lagrange multipliers or similar variables in the Udwadia–Kalaba equation.

2. Udwadia–Kalaba equation solves the problem based on second-order constraints. Since the zeroth-order constraint that corresponds second-order constraint is not the only solution, the general solution can be described by mathematical calculation. Therefore, we can determine its specific general solution form by initial conditions.

3. When initial conditions have compatibility, we can use the nominal control $\tau =p_1$ to solve the trajectory tracking control problem of 2-DOF helicopter. When initial conditions have incompatibility, we can use the modified controller ${\tau }^{*}$ .

4. Comparing with different control methods, we find that the modified controller ${\tau }^{*}=p_1(q,\stackrel{·}{q},t)+p_2(q,\stackrel{·}{q},t)=[{\tau }_1{\tau }_2{]}^T$ could provide better trajectory control effects and perfectly solve the problem of incompatible initial conditions with less control cost and higher control accuracy.

5. The constraint control methods need to be calculated and the digital error always exist. But the error is very small and it will not have a significant impact on the control effect in practical applications.