# Determination of Stability Correction Parameters for Dynamic Equations of Constrained Multibody Systems

Authors: Lyu Guizhi; Liu Rong
Journal: Mathematical Problems in Engineering
 (2018)

---

## Abstract

When analyzing mechanical systems with numerical simulation by the Udwadia and Kalaba method, numerical integral results of dynamic equations will gradually deviate from requirements of constraint equations and eventually lead to constraint violation. It is a common method to solve the constraint violation by using constraint stability to modify the constraint equation. Selection of stability parameters is critical in the particular form of the corrected equation. In this paper, the method of selecting and determining of stability parameters is given, and these parameters will be used to correct the Udwadia-Kalaba basic equation by the Baumgarte constraint stability method. The selection domain of stability parameters is further reduced in view of the singularity of the constraint matrix during the integration procedure based on the selection domain which is obtained by the system stability analysis method. Errors of velocity violation and position violation are defined in the workspace, so as to determine the parameter values. Finally, the 3-link spatial manipulator is used to verify stability parameters of the proposed method. Numerical simulation results verify the effectiveness of the proposed method.


---

## Body

## 1. Introduction

The theory, which was proposed for modeling and analysis of the constrained system dynamics by Udwadia and Kalaba, has been applied more and more in mechanical systems [1–5]. However, when the mechanical system is analyzed and simulated with Udwadia and Kalaba method, the results of numerical integration of system dynamics equation will deviate from requirements of constraint equations over time and eventually lead to constraint violation. In order to solve the constraint violation problem, Udwadia has proposed a tracking control method for nonlinear systems based on the Lyapunov stability principle [6]. The asymptotic stability control of the system is realized by modifying the constraint equations. The basic idea of the method is essentially consistent with the Baumgarte stabilization method for constraint violations [7] and can be widely used in various working environment [8–15].The stability parameters are important for the Baumgarte stabilization method to modify constraint violations, and they are often selected by the experience method. Although it has been mentioned that different values of stability parameters should be chosen for different constraint equations [16], few people further research on this. A variety of methods have been studied in order to select appropriate parameters to keep the system stable after constraint equations modified. The Taylor expansion comparison method can directly calculate values of stability parameters [17, 18]. However, the choice of stability parameters is strictly related to the integral time step. Stability parameters will be too large if the time step is too small, which will make dynamic equations distorted and the system unstable. The method of system stability analysis is used to select stable parameters, in which different numerical integration methods [19–21] and different integration time step [22, 23] would be considered to affect the selection area of stability parameters. But parameters, selected from the area of stability parameters with the method of system stability analysis, could not guarantee the continuous stability of the system in the numerical integration process. This paper focuses on further reducing the area of stability parameters on the basis of system stability analysis through the singularity determination for the constraint matrix and selecting specific stability parameters according to engineering requirements.The outline of the paper is presented as follows: in Section2 the form of modified Udwadia-Kalaba dynamic equation with constraint stability is presented. In Section 3 the selectable region of stability parameters and the determination method of stability parameters are given. In Section 4, the proposed method is verified and discussed; after the circular trajectory is defined as constraint, a dynamic model of the spatial 3-link manipulators is established and the numerical simulation is completed. Finally, conclusions are provided in Section 5.


## 2. Udwadia-Kalaba Equation with Violation Stability

Basing on the modeling idea of multibody dynamics system, the equation of motion for a constrained mechanical system can be described by(1)Mq¨=Q+Qc,where M>0 is the n×n mass matrix, Q is the generalized force for unconstrained system, and Qc is the generalized force needed to be applied in the system in order to satisfy a given constraint.Each part of the mechanical system needs to move along a specific trajectory, which can be regarded as a constraint in order to accomplish a given task. If the multibody system is composed ofn generalized coordinates, and there are m independent movements at the position level, the constraint equation can be written as (2)Φ≡Φq=0,where q=q1,q2,…,qnT is the generalized coordinate matrix. Formula (2) is differentiated twice with respect to time; the constraint equation at acceleration level is (3)Φ¨≡Aq¨-b=0,where A is the m×n Jacobian matrix and b=-A˙q˙ is a m×1 vector.According to the Udwadia-Kalaba equation, if the initial condition of the system satisfies the constraint Eq. (2), then the closed solution of the generalized constraint force on the system can be obtained from [24](4)Qc=M1/2AM-1/2+b-AM-1Qin which “+” represents the Moore–Penrose generalized inverse.Eq. (4) indicates the control force that the system needs to be applied, when the unconstrained mechanical system was required to move along the given constraint trajectory by (2). Substitute the expression item of (4) into (1) and rewrite the formulation in a more visible way; the fundamental equation of Udwadia-Kalaba can be obtained as (5)q¨=M-1Q+M-1/2AM-1/2+b-AM-1Q.The constraints represented by (2) should be satisfied by the control force that gained from (4). However, in the simulation process, the integral error of q¨ in formula (5) increases with the time, and the motion trajectory of the mechanical system obtained eventually deviates from the given trajectory from (2). Therefore, the method of Baumgarte constraint violation stability can be used to correct (3). The corrected constraint equation can be written as (6)Φ¨+αΦ˙+βΦ=0.This formula is the differential equation of the closed loop system of the constrained equation, in which α and β can be considered as control parameters. From the formula (6), it can be found that the fundamental principle of the correction equation is to correct the acceleration by the feedback of position and velocity. The structure of the control system of closed loop can be considered as Figure 1.Figure 1
The control system of the method for constrained stabilization.Combining (3) and (6) and arranging them, finishing can obtain (7)Aq¨=bB,where bB=b-αΦ˙-βΦ. The corrected equation for constraint violation stabilization can be written as (8)q¨=M-1Q+M-1/2AM-1/2+bB-AM-1Q.In order to meet the needs of the corrected trajectory, the additional generalized constraint force on the system should be reexpressed as (9)Qc=M1/2AM-1/2+bB-AM-1Q.


## 3. Methods for Selection and Determination of Stability Parameters

### 3.1. The Selection Scope of Stability Parameters

The method of system stability analysis mainly is used to determine the selection area of stability parameter. It is necessary to change the integral equation into a difference equation when solving the motion with numerical method. When the first-order integral method is used, the numerical solution of the equation yields(10)yn+1=yn+fnΔt,where the subscript represents the numerical solution at the corresponding time step and Δt is the integration time step. Since (10) is a difference equation of the discrete data function, the Z transform of (10) can obtain(11)z0Yz0=Yz0+Fz0Δt,where z0 is the Z transform variable. Rearranging (11) yields(12)Fz0Yz0=z0-1Δtand employing Laplace transform to the first integral equation yields(13)Ys=1sFs,where s is the operator of Laplace domain. Analyzing the similarities between (12) and (13), the relationship between s and z0 can be obtained as(14)s=z0-1Δt.Then, based on the first-order integral method, substituting (14) in any Fs/Ys yields a Fz0/Yz0.The Laplace transform of (6) results in(15)s2+αs+β=0.From (15), the necessary and sufficient condition for system asymptotically stable is that roots of the characteristic equation of the system all have negative real parts. That means α>0 and β>0; the system is asymptotic stability. However, the selection scope is too large to select appropriate values of α and β. The selection scope of α and β values can be further reduced by the position of the roots of (15) in the z0-plane. On the other hand, the position on the z0-plane should be found out, which corresponds to the left half plane of the s-plane, the region of system stability. According to the definition of Z transform (16)z0=eTs,where T is the sample period. From Laplace transform, s=σ+jω, so it is possible to write(17)z0=eTσ+jω=eTσejTω(18)z0=eTσ,∠z0=ωT.From (18), it can be seen that if σ=0 on the s-plane, z0=1 on the z0-plane; all advisable z0 values correspond to the unit circle with the center at the origin. If the roots of the characteristic (15) all have negative real parts, that is, σ<0, roots on the left half plane of the s-plane obtained by (17) will all mapped to the interior of a unit circle with the center at the origin on the z0-plane. Therefore, from the mapping relation between the s-plane and the z0-plane, it is shown that, on the z0-plane, the inside of the unit circle is a stable region and the outside of the unit circle is an unstable region, and the periphery of the unit circle is the stable boundary.Substituting (14) into (15), the characteristic equation about z0 can be obtained as(19)z02+αΔt-2z0+βΔt2-αΔt+1=0.Letting roots of (19) fall in the unit circle, the area of parameters α and β can be obtained, when Δt the time step for integration is determined. Letting α1=αΔt and β1=βΔt2, the areas of values of α1 and β1 are shown in Figure 2 when z0 locate in the unit circle.Figure 2
Stability region in theα1-β1 plane for the first-order integral method.The stable boundary of a system is a unit circlez0=1, Φ and Φ˙ will converge to 0, if z0<1. The smaller z0, the faster Φ and Φ˙ converge to 0, and the smaller ∠z0, the smaller the frequency of Φ and Φ˙. However, values of α and β, calculated from α1 and β1 selected from the shadow region of Figure 2, could not guarantee the stability of Φ and Φ˙. In order to make values of α and β not affect the final stability of Φ and Φ˙, it is necessary to ensure that the constraint matrix is always a nonsingular matrix in the integral process. Therefore, the area of values for α and β can be further reduced, by analyzing the influence of α and β on the constraint matrix, and the judgment of the singularity for the constraint matrix in the integral calculation.


### 3.2. Determination Values of Stability Parameter

The determination of stability parameters is not only related to the convergence speed of the constrainedΦ and Φ˙, but also related to magnitudes of instantaneous errors of the position constraint and the speed constraint after stabilization. In practical applications, constraints of mechanical system are often defined in the workspace of the Cartesian coordinate system. In order to understand the executing situation of given constraint, errors of position and velocity which generated in the generalized coordinate system can be mapped into the Cartesian coordinate system.The position mapping of the given constraint trajectory from the generalized coordinate system to the Cartesian coordinate system is(20)x=fq,y=gq,z=hqand the velocity mapping between the generalized coordinate system and the Cartesian coordinate system is(21)x˙=f˙qq˙,y˙=g˙qq˙,z˙=h˙qq˙.The obtained position and velocity by the dynamic equation corrected with constraint violation stabilization are defined asxB, yB, zB and x˙B, y˙B, z˙B on the direction of x, y, z coordinate axes of the Cartesian coordinate system. Instantaneous position errors of three axis direction between the integral position and the given position in the Cartesian coordinate system are(22)Δx=x-xB,Δy=y-yB,Δz=z-zB.Instantaneous velocity errors of three axis directions between the integral velocity and the required velocity of the given trajectory in the Cartesian coordinate system are(23)Δx˙=x˙-x˙B,Δy˙=y˙-y˙B,Δz˙=z˙-z˙B.Thus, Instantaneous errors of velocities and positions between integral calculation values and the given theorem values in the Cartesian space can be defined as(24)e1=Δx2+Δy2+Δz2,e2=Δx˙2+Δy˙2+Δz˙2.Further, the numerical index for verifying stability parameters, the positional constraint error, and the velocity constraint error can be defined as(25)E1=1T∫0Te12dt,E2=1T∫0Te22dt.If selected values of parameters could make E1 and E2 smaller, parameter values are better. But it is difficult to select the appropriate α and β, satisfying E1 and E2 as minimum values. Weighting factors can be set, in line with different requirements of the position constraint error and the velocity constraint error, distributing the weight rationally. The error for parameter determination is defined as(26)e=cE12+dE22,where c is the position weighting factor and d is the velocity weighting factor. e is used as the final criteria to select α and β.


## 3.1. The Selection Scope of Stability Parameters

The method of system stability analysis mainly is used to determine the selection area of stability parameter. It is necessary to change the integral equation into a difference equation when solving the motion with numerical method. When the first-order integral method is used, the numerical solution of the equation yields(10)yn+1=yn+fnΔt,where the subscript represents the numerical solution at the corresponding time step and Δt is the integration time step. Since (10) is a difference equation of the discrete data function, the Z transform of (10) can obtain(11)z0Yz0=Yz0+Fz0Δt,where z0 is the Z transform variable. Rearranging (11) yields(12)Fz0Yz0=z0-1Δtand employing Laplace transform to the first integral equation yields(13)Ys=1sFs,where s is the operator of Laplace domain. Analyzing the similarities between (12) and (13), the relationship between s and z0 can be obtained as(14)s=z0-1Δt.Then, based on the first-order integral method, substituting (14) in any Fs/Ys yields a Fz0/Yz0.The Laplace transform of (6) results in(15)s2+αs+β=0.From (15), the necessary and sufficient condition for system asymptotically stable is that roots of the characteristic equation of the system all have negative real parts. That means α>0 and β>0; the system is asymptotic stability. However, the selection scope is too large to select appropriate values of α and β. The selection scope of α and β values can be further reduced by the position of the roots of (15) in the z0-plane. On the other hand, the position on the z0-plane should be found out, which corresponds to the left half plane of the s-plane, the region of system stability. According to the definition of Z transform (16)z0=eTs,where T is the sample period. From Laplace transform, s=σ+jω, so it is possible to write(17)z0=eTσ+jω=eTσejTω(18)z0=eTσ,∠z0=ωT.From (18), it can be seen that if σ=0 on the s-plane, z0=1 on the z0-plane; all advisable z0 values correspond to the unit circle with the center at the origin. If the roots of the characteristic (15) all have negative real parts, that is, σ<0, roots on the left half plane of the s-plane obtained by (17) will all mapped to the interior of a unit circle with the center at the origin on the z0-plane. Therefore, from the mapping relation between the s-plane and the z0-plane, it is shown that, on the z0-plane, the inside of the unit circle is a stable region and the outside of the unit circle is an unstable region, and the periphery of the unit circle is the stable boundary.Substituting (14) into (15), the characteristic equation about z0 can be obtained as(19)z02+αΔt-2z0+βΔt2-αΔt+1=0.Letting roots of (19) fall in the unit circle, the area of parameters α and β can be obtained, when Δt the time step for integration is determined. Letting α1=αΔt and β1=βΔt2, the areas of values of α1 and β1 are shown in Figure 2 when z0 locate in the unit circle.Figure 2
Stability region in theα1-β1 plane for the first-order integral method.The stable boundary of a system is a unit circlez0=1, Φ and Φ˙ will converge to 0, if z0<1. The smaller z0, the faster Φ and Φ˙ converge to 0, and the smaller ∠z0, the smaller the frequency of Φ and Φ˙. However, values of α and β, calculated from α1 and β1 selected from the shadow region of Figure 2, could not guarantee the stability of Φ and Φ˙. In order to make values of α and β not affect the final stability of Φ and Φ˙, it is necessary to ensure that the constraint matrix is always a nonsingular matrix in the integral process. Therefore, the area of values for α and β can be further reduced, by analyzing the influence of α and β on the constraint matrix, and the judgment of the singularity for the constraint matrix in the integral calculation.


## 3.2. Determination Values of Stability Parameter

The determination of stability parameters is not only related to the convergence speed of the constrainedΦ and Φ˙, but also related to magnitudes of instantaneous errors of the position constraint and the speed constraint after stabilization. In practical applications, constraints of mechanical system are often defined in the workspace of the Cartesian coordinate system. In order to understand the executing situation of given constraint, errors of position and velocity which generated in the generalized coordinate system can be mapped into the Cartesian coordinate system.The position mapping of the given constraint trajectory from the generalized coordinate system to the Cartesian coordinate system is(20)x=fq,y=gq,z=hqand the velocity mapping between the generalized coordinate system and the Cartesian coordinate system is(21)x˙=f˙qq˙,y˙=g˙qq˙,z˙=h˙qq˙.The obtained position and velocity by the dynamic equation corrected with constraint violation stabilization are defined asxB, yB, zB and x˙B, y˙B, z˙B on the direction of x, y, z coordinate axes of the Cartesian coordinate system. Instantaneous position errors of three axis direction between the integral position and the given position in the Cartesian coordinate system are(22)Δx=x-xB,Δy=y-yB,Δz=z-zB.Instantaneous velocity errors of three axis directions between the integral velocity and the required velocity of the given trajectory in the Cartesian coordinate system are(23)Δx˙=x˙-x˙B,Δy˙=y˙-y˙B,Δz˙=z˙-z˙B.Thus, Instantaneous errors of velocities and positions between integral calculation values and the given theorem values in the Cartesian space can be defined as(24)e1=Δx2+Δy2+Δz2,e2=Δx˙2+Δy˙2+Δz˙2.Further, the numerical index for verifying stability parameters, the positional constraint error, and the velocity constraint error can be defined as(25)E1=1T∫0Te12dt,E2=1T∫0Te22dt.If selected values of parameters could make E1 and E2 smaller, parameter values are better. But it is difficult to select the appropriate α and β, satisfying E1 and E2 as minimum values. Weighting factors can be set, in line with different requirements of the position constraint error and the velocity constraint error, distributing the weight rationally. The error for parameter determination is defined as(26)e=cE12+dE22,where c is the position weighting factor and d is the velocity weighting factor. e is used as the final criteria to select α and β.


## 4. Numerical Examples

### 4.1. The System Model

A 3-link spatial manipulator, as shown in Figure3, is selected to analyze the selection of stabilizing parameters. In Figure 3, li-1 is the length of the i link of the manipulator, ri-1 is the distance from the gravity center of the i link to the end of the joint, and qi indicates the generalized position of the i joint. Suppose link masses of the 3-link spatial manipulator are m1=m2=m3 = 1 kg, lengths are l1=l2=l3 = 1 m, distances of gravity centers are r0=r1=r2= 0.5 m, moments of inertia of x axis are Ix1=Ix2=Ix3=1, moments of inertia of y axis are Iy1=Iy2=Iy3=1, and moments of inertia of z axis are Iz1=Iz2=Iz3=1.Figure 3
The 3-link spatial manipulator.Letst=sin⁡t, s1=sin⁡q1, s2=sin⁡q2, s3=sin⁡q3, s12=sin⁡q1+q2, s13=sin⁡q1+q3, s23=sin⁡q2+q3, ct=cos⁡t, c1=cos⁡q1, c2=cos⁡q2, c3=cos⁡q3, c12=cos⁡q1+q2, c13=cos⁡q1+q3, c23=cos⁡q2+q3, for ease of writing.According to the general method of manipulator dynamics equation, if there is no external constraint,(27)Mq¨+Cq˙+G=0.Therefore, in (1) of Section 2, (28)Q=-Cq˙-G.According to the basic method of Lagrange dynamics modeling, the inertial matrix for the manipulator can be obtained by(29)M=M11M12M13M21M22M23M31M32M33.The components of M are given by (30)M11=Iy2s22+Iy3s232+Iz1+Iz2c22+Iz3c232+m2r12c22+m3l2c2+r2c232M22=Ix2+Ix3+m3l12+m2r12+m3r22+2m3l1r2c3M33=Ix3+m3r22M12=M21=M13=M31=0M23=M32=Ix3+m3r22+m3l1r2c3.The matrix of Coriolis and centrifugal forces for the manipulator are given by(31)C=∑k=1nΓijkq˙k.The nonzero values of Γijk are given by (32)Γ112=Iy2-Iz2-m2r12c2s2+Iy3-Iz3c23s23-m3l1c2+r2c23l1s2+r2s23Γ113=Iy3-Iz3c23s23-m3r2s23l1c2+r2c23Γ121=Iy2-Iz2-m2r12c2s2+Iy3-Iz3c23s23-m3l1c2+r2c23l1s2+r2s23Γ131=Iy3-Iz3c23s23-m3r2s23l1c2+r2c23Γ211=Iz2-Iy2+m2r12c2s2+Iz3-Iy3c23s23+m3l1c2+r2c23l1s2+r2s23Γ223=-l1m3r2s3Γ232=-l1m3r2s3Γ233=-l1m3r2s3Γ311=Iz3-Iy3c23s23+m3r2s23l1c2+r2c23Γ322=l1m3r2s3.The gravity terms are given by(33)G=∂V∂q=0-m2gr1+m3gl1c2-m3r2c23-m3gr2c23.


### 4.2. The Object Trajectory

From Figure2, it is can be seen that the origin of the Cartesian coordinate system in the workspace of the manipulator is located at the base of the 3-link manipulator. A spatial circle is defined as the motion trajectory of the manipulator end. The coordinate of the circle center in the Cartesian coordinate system is (1,1,1), the radius of the circle is 0.5, and the normal vector of the circle plane is (1,1,1). Parametric equations of the circle can be expressed as(34a)x=1+122ct+126st(34b)y=1-122ct+126st(34c)z=1-226st.From Figure2 and the forward kinematics of the manipulator, the position mapping relations between the Cartesian coordinate and the generalized coordinate can be obtained as(35a)x=l1c2+l2c23c1(35b)y=l1c2+l2c23s1(35c)z=l0-l1s2-l2s23.Plugging (34a), (34b), and (34c) into (35a), (35b), and (35c), respectively, and arranging them yields(36)
Φ
=
l
1
c
2
+
l
2
c
23
c
1
-
1
-
1
2
2
c
t
-
1
2
6
s
t
l
1
c
2
+
l
2
c
23
s
1
-
1
+
1
2
2
c
t
-
1
2
6
s
t
l
0
-
l
1
s
2
-
l
2
s
23
-
1
+
2
2
6
s
t
=
0
.Eq. (36) is taken as a first-order derivative relative to time, yielding Φ˙. Eq. (36) is taken as the second-order derivative relative to time; after arranging, items of (3) in Section 2 can be obtained as(37)
A
=
l
1
c
2
+
l
2
c
23
s
1
l
2
s
23
+
l
1
s
2
c
1
l
2
s
23
c
1
-
l
1
c
2
+
l
2
c
23
c
1
l
2
s
23
+
l
1
s
2
s
1
l
2
s
23
s
1
0
l
1
c
2
+
l
2
c
23
l
2
c
23
b
=
q
˙
1
s
1
l
2
s
23
q
˙
2
+
q
˙
3
+
l
1
q
˙
2
s
2
-
c
1
l
2
c
23
q
˙
2
+
q
˙
3
2
+
l
1
q
˙
2
2
c
2
+
q
˙
1
s
1
l
2
s
23
q
˙
2
+
q
˙
3
+
q
˙
2
l
1
s
2
-
q
˙
1
2
c
1
l
1
c
2
+
l
2
c
23
+
2
c
t
4
+
6
12
s
t
-
s
1
l
2
c
23
q
˙
2
+
q
˙
3
2
+
l
1
q
˙
2
2
c
2
-
q
˙
1
c
1
l
2
s
23
q
˙
2
+
q
˙
3
+
l
1
q
˙
2
s
2
+
q
˙
1
c
1
l
2
s
23
q
˙
2
+
q
˙
3
+
q
˙
2
l
1
s
2
-
q
˙
1
2
s
1
l
1
c
2
+
l
2
c
23
-
2
4
c
t
+
6
12
s
t
l
1
q
˙
2
q
˙
2
s
2
+
l
2
q
˙
2
+
q
˙
3
2
s
23
-
6
6
s
tand items in bB of (7) at Section 2 are all known except for α and β. So, in the integral Eq. (8) of Section 2, all other items except α and β in bB have been obtained.


### 4.3. Simulation Results and Analysis

Taking the step of integral time is 0.1 s, the 3-link spatial manipulator is analyzed by the dynamics model and the given constraint trajectory on the Matlab 2015a software platform by a PC with Intel Core i5 CPU, 3.20 GHz basic frequency, and 4.00 GB RAM. The simulation results are as follows.Figure4 shows the selection area of stable parameter in accordance with the method of Section 3.1, and the program runs 8.6 s. The sparse point area is the selectable area of parameters in which parameters are determined by the method of system stability analysis, while the dense point area is the final selectable area of parameters in which the constraint matrix is a nonsingular matrix should also be guaranteed. From Figure 4, it is can be seen that the final selectable area is much smaller than the area determined by the system stability analysis method. This is because the stability analysis method takes into account given constraints only without parameters of the system structure, which the dense point area considers.Figure 4
The selectable area of stability parameters.According to the determination method of parameter values in Section3.1, and calculating 95.38 s, influence diagrams for α and β values on the constraint error are shown in Figures 5–7. Figure 5 shows the influence of α and β on positional constraint errors. From the figure, it can be seen that at the area which is close to α=5 and β=4, the minimum error of the position constraint can be obtained. Figure 6 shows the influence of α and β on velocity constraint errors. The minimum error of the velocity constraint can be obtained from the area which is close to α=9.5 and β=1. The values of α and β for obtaining the minimum position constraint error are not the same as those obtained for the minimum speed constraint error. Therefore, it is necessary to define the determinant error by weighting factors to select values for stable parameters. By (26), considering weight factors c=d=0.5, the relation between α, β and the parameter determination error can be obtained in Figure 7. From Figure 7, it is possible to obtain the minimum determination error at α=9.5 and β=4.5. Although the selection process takes little long time to calculate, it will not affect the real-time performance of the robot control because it is only the preparation stage of the control process.Figure 5
The influence ofα and β values on the positional constraint error.Figure 6
The influence ofα and β values on the velocity constraint error.Figure 7
The relation betweenα, β and the parameter determination error e.Puttingα=9.5 and β=4.5 into bB of (8), the ordinary differential equation is used to numerically integrate. The simulation time is 40 s. After the program runs 2.88 s, errors between numerical solutions and given theoretical values can be obtained in the Cartesian space, as shown in Figures 8–11. Figure 8 shows velocity violation errors in directions of coordinate axes for the end point of the manipulator, and Figure 9 shows the velocity violation error for the end point of the 3-link spatial manipulator in Cartesian coordinate system. It can be seen that when the 3-link spatial manipulator moves along a given trajectory, velocity violation error is limited to a specific range. Figure 10 shows position violation errors in directions of coordinate axes for the end point of the manipulator, and Figure 11 shows the position violation error for the end point of the 3-link spatial manipulator in Cartesian coordinate system. It can also be seen that the position violation error is also controlled in a specific small range.Figure 8
Velocity violation errors in directions of coordinate axes for the end point of the manipulator.Figure 9
The velocity violation error for the end point of the manipulator in Cartesian coordinate system.Figure 10
Position violation errors in directions of coordinate axes for the end point of the manipulator.Figure 11
The position violation error for the end point of the manipulator in Cartesian coordinate system.If the fundamental equation of dynamics is not corrected by the method of constraint violation stabilization, the numerical integration employs (5) of Section 2 directly. After the simulation of 14 s, the system will eventually drift away due to the constraint violation. Figures 12–14 show comparisons of constraint violation errors before and after the correction of the dynamics equation in the 14 s simulation time after the program runs 1.9 s. Figure 12 presents a comparison of the velocity violation error. As can be seen from the Figure, at the particular moment prior to the 14 s, the velocity violation error increases sharply before the equation is corrected. Figure 13 presents a comparison of the position violation error. It is found that the position error increases with the time before the constraint equation is corrected. But after the 14 s, the violation error will increase rapidly due to the increase of the speed violation error, and the system will drift away because of that.Figure 12
Comparison of the velocity violation error.Figure 13
Comparison of the position violation error.Figure 14
Comparison of required torques for each joint.Figure14 shows comparison of required torques for each joint before and after the correction of dynamic equation, obtained from (4) and (9), respectively. Dashed lines represent generalized torques that need to be imposed on each joint before the correction to meet the given trajectory. Solid lines represent generalized torques applied to each joint after the correction, to meet the requirement of limited trajectory errors in a certain range. It can be seen that the control torque curve is regular after the Baumgarte stabilization; thus the effect of constraint drift could be avoided for the performance of controller.


## 4.1. The System Model

A 3-link spatial manipulator, as shown in Figure3, is selected to analyze the selection of stabilizing parameters. In Figure 3, li-1 is the length of the i link of the manipulator, ri-1 is the distance from the gravity center of the i link to the end of the joint, and qi indicates the generalized position of the i joint. Suppose link masses of the 3-link spatial manipulator are m1=m2=m3 = 1 kg, lengths are l1=l2=l3 = 1 m, distances of gravity centers are r0=r1=r2= 0.5 m, moments of inertia of x axis are Ix1=Ix2=Ix3=1, moments of inertia of y axis are Iy1=Iy2=Iy3=1, and moments of inertia of z axis are Iz1=Iz2=Iz3=1.Figure 3
The 3-link spatial manipulator.Letst=sin⁡t, s1=sin⁡q1, s2=sin⁡q2, s3=sin⁡q3, s12=sin⁡q1+q2, s13=sin⁡q1+q3, s23=sin⁡q2+q3, ct=cos⁡t, c1=cos⁡q1, c2=cos⁡q2, c3=cos⁡q3, c12=cos⁡q1+q2, c13=cos⁡q1+q3, c23=cos⁡q2+q3, for ease of writing.According to the general method of manipulator dynamics equation, if there is no external constraint,(27)Mq¨+Cq˙+G=0.Therefore, in (1) of Section 2, (28)Q=-Cq˙-G.According to the basic method of Lagrange dynamics modeling, the inertial matrix for the manipulator can be obtained by(29)M=M11M12M13M21M22M23M31M32M33.The components of M are given by (30)M11=Iy2s22+Iy3s232+Iz1+Iz2c22+Iz3c232+m2r12c22+m3l2c2+r2c232M22=Ix2+Ix3+m3l12+m2r12+m3r22+2m3l1r2c3M33=Ix3+m3r22M12=M21=M13=M31=0M23=M32=Ix3+m3r22+m3l1r2c3.The matrix of Coriolis and centrifugal forces for the manipulator are given by(31)C=∑k=1nΓijkq˙k.The nonzero values of Γijk are given by (32)Γ112=Iy2-Iz2-m2r12c2s2+Iy3-Iz3c23s23-m3l1c2+r2c23l1s2+r2s23Γ113=Iy3-Iz3c23s23-m3r2s23l1c2+r2c23Γ121=Iy2-Iz2-m2r12c2s2+Iy3-Iz3c23s23-m3l1c2+r2c23l1s2+r2s23Γ131=Iy3-Iz3c23s23-m3r2s23l1c2+r2c23Γ211=Iz2-Iy2+m2r12c2s2+Iz3-Iy3c23s23+m3l1c2+r2c23l1s2+r2s23Γ223=-l1m3r2s3Γ232=-l1m3r2s3Γ233=-l1m3r2s3Γ311=Iz3-Iy3c23s23+m3r2s23l1c2+r2c23Γ322=l1m3r2s3.The gravity terms are given by(33)G=∂V∂q=0-m2gr1+m3gl1c2-m3r2c23-m3gr2c23.


## 4.2. The Object Trajectory

From Figure2, it is can be seen that the origin of the Cartesian coordinate system in the workspace of the manipulator is located at the base of the 3-link manipulator. A spatial circle is defined as the motion trajectory of the manipulator end. The coordinate of the circle center in the Cartesian coordinate system is (1,1,1), the radius of the circle is 0.5, and the normal vector of the circle plane is (1,1,1). Parametric equations of the circle can be expressed as(34a)x=1+122ct+126st(34b)y=1-122ct+126st(34c)z=1-226st.From Figure2 and the forward kinematics of the manipulator, the position mapping relations between the Cartesian coordinate and the generalized coordinate can be obtained as(35a)x=l1c2+l2c23c1(35b)y=l1c2+l2c23s1(35c)z=l0-l1s2-l2s23.Plugging (34a), (34b), and (34c) into (35a), (35b), and (35c), respectively, and arranging them yields(36)
Φ
=
l
1
c
2
+
l
2
c
23
c
1
-
1
-
1
2
2
c
t
-
1
2
6
s
t
l
1
c
2
+
l
2
c
23
s
1
-
1
+
1
2
2
c
t
-
1
2
6
s
t
l
0
-
l
1
s
2
-
l
2
s
23
-
1
+
2
2
6
s
t
=
0
.Eq. (36) is taken as a first-order derivative relative to time, yielding Φ˙. Eq. (36) is taken as the second-order derivative relative to time; after arranging, items of (3) in Section 2 can be obtained as(37)
A
=
l
1
c
2
+
l
2
c
23
s
1
l
2
s
23
+
l
1
s
2
c
1
l
2
s
23
c
1
-
l
1
c
2
+
l
2
c
23
c
1
l
2
s
23
+
l
1
s
2
s
1
l
2
s
23
s
1
0
l
1
c
2
+
l
2
c
23
l
2
c
23
b
=
q
˙
1
s
1
l
2
s
23
q
˙
2
+
q
˙
3
+
l
1
q
˙
2
s
2
-
c
1
l
2
c
23
q
˙
2
+
q
˙
3
2
+
l
1
q
˙
2
2
c
2
+
q
˙
1
s
1
l
2
s
23
q
˙
2
+
q
˙
3
+
q
˙
2
l
1
s
2
-
q
˙
1
2
c
1
l
1
c
2
+
l
2
c
23
+
2
c
t
4
+
6
12
s
t
-
s
1
l
2
c
23
q
˙
2
+
q
˙
3
2
+
l
1
q
˙
2
2
c
2
-
q
˙
1
c
1
l
2
s
23
q
˙
2
+
q
˙
3
+
l
1
q
˙
2
s
2
+
q
˙
1
c
1
l
2
s
23
q
˙
2
+
q
˙
3
+
q
˙
2
l
1
s
2
-
q
˙
1
2
s
1
l
1
c
2
+
l
2
c
23
-
2
4
c
t
+
6
12
s
t
l
1
q
˙
2
q
˙
2
s
2
+
l
2
q
˙
2
+
q
˙
3
2
s
23
-
6
6
s
tand items in bB of (7) at Section 2 are all known except for α and β. So, in the integral Eq. (8) of Section 2, all other items except α and β in bB have been obtained.


## 4.3. Simulation Results and Analysis

Taking the step of integral time is 0.1 s, the 3-link spatial manipulator is analyzed by the dynamics model and the given constraint trajectory on the Matlab 2015a software platform by a PC with Intel Core i5 CPU, 3.20 GHz basic frequency, and 4.00 GB RAM. The simulation results are as follows.Figure4 shows the selection area of stable parameter in accordance with the method of Section 3.1, and the program runs 8.6 s. The sparse point area is the selectable area of parameters in which parameters are determined by the method of system stability analysis, while the dense point area is the final selectable area of parameters in which the constraint matrix is a nonsingular matrix should also be guaranteed. From Figure 4, it is can be seen that the final selectable area is much smaller than the area determined by the system stability analysis method. This is because the stability analysis method takes into account given constraints only without parameters of the system structure, which the dense point area considers.Figure 4
The selectable area of stability parameters.According to the determination method of parameter values in Section3.1, and calculating 95.38 s, influence diagrams for α and β values on the constraint error are shown in Figures 5–7. Figure 5 shows the influence of α and β on positional constraint errors. From the figure, it can be seen that at the area which is close to α=5 and β=4, the minimum error of the position constraint can be obtained. Figure 6 shows the influence of α and β on velocity constraint errors. The minimum error of the velocity constraint can be obtained from the area which is close to α=9.5 and β=1. The values of α and β for obtaining the minimum position constraint error are not the same as those obtained for the minimum speed constraint error. Therefore, it is necessary to define the determinant error by weighting factors to select values for stable parameters. By (26), considering weight factors c=d=0.5, the relation between α, β and the parameter determination error can be obtained in Figure 7. From Figure 7, it is possible to obtain the minimum determination error at α=9.5 and β=4.5. Although the selection process takes little long time to calculate, it will not affect the real-time performance of the robot control because it is only the preparation stage of the control process.Figure 5
The influence ofα and β values on the positional constraint error.Figure 6
The influence ofα and β values on the velocity constraint error.Figure 7
The relation betweenα, β and the parameter determination error e.Puttingα=9.5 and β=4.5 into bB of (8), the ordinary differential equation is used to numerically integrate. The simulation time is 40 s. After the program runs 2.88 s, errors between numerical solutions and given theoretical values can be obtained in the Cartesian space, as shown in Figures 8–11. Figure 8 shows velocity violation errors in directions of coordinate axes for the end point of the manipulator, and Figure 9 shows the velocity violation error for the end point of the 3-link spatial manipulator in Cartesian coordinate system. It can be seen that when the 3-link spatial manipulator moves along a given trajectory, velocity violation error is limited to a specific range. Figure 10 shows position violation errors in directions of coordinate axes for the end point of the manipulator, and Figure 11 shows the position violation error for the end point of the 3-link spatial manipulator in Cartesian coordinate system. It can also be seen that the position violation error is also controlled in a specific small range.Figure 8
Velocity violation errors in directions of coordinate axes for the end point of the manipulator.Figure 9
The velocity violation error for the end point of the manipulator in Cartesian coordinate system.Figure 10
Position violation errors in directions of coordinate axes for the end point of the manipulator.Figure 11
The position violation error for the end point of the manipulator in Cartesian coordinate system.If the fundamental equation of dynamics is not corrected by the method of constraint violation stabilization, the numerical integration employs (5) of Section 2 directly. After the simulation of 14 s, the system will eventually drift away due to the constraint violation. Figures 12–14 show comparisons of constraint violation errors before and after the correction of the dynamics equation in the 14 s simulation time after the program runs 1.9 s. Figure 12 presents a comparison of the velocity violation error. As can be seen from the Figure, at the particular moment prior to the 14 s, the velocity violation error increases sharply before the equation is corrected. Figure 13 presents a comparison of the position violation error. It is found that the position error increases with the time before the constraint equation is corrected. But after the 14 s, the violation error will increase rapidly due to the increase of the speed violation error, and the system will drift away because of that.Figure 12
Comparison of the velocity violation error.Figure 13
Comparison of the position violation error.Figure 14
Comparison of required torques for each joint.Figure14 shows comparison of required torques for each joint before and after the correction of dynamic equation, obtained from (4) and (9), respectively. Dashed lines represent generalized torques that need to be imposed on each joint before the correction to meet the given trajectory. Solid lines represent generalized torques applied to each joint after the correction, to meet the requirement of limited trajectory errors in a certain range. It can be seen that the control torque curve is regular after the Baumgarte stabilization; thus the effect of constraint drift could be avoided for the performance of controller.


## 5. Conclusion

The method of acceleration feedback correction for the constrained equation can suppress the constraint violation in the numerical integral operation and achieve the asymptotic stability when the initial condition does not satisfy the constraint equation. The system stability analysis method is used to obtain the initial area of stability parameter values. The area is further reduced by the requirement of nonsingularity of the corrected constraint matrix. After values which are chosen from the reduced area correct the acceleration term, the final stability of position constraints and velocity constraints could be achieved.Based on the position violation error and the velocity violation error of Cartesian coordinate system in workspace, the determination error with weighting factors is given for the selection of stabilizing parameters. The determination error can be determined by weighting factors according to the different requirements of position constraints and position constraints. Stability parameters are selected by the minimum value of the determination error. The dynamic equation of 3-link spatial manipulator is constructed by Udwadia-Kalaba method, and the equation is corrected by the stability parameter selected by the given method. The error analysis of the simulation results shows the reliability and validity of the given method for stability parameter determination.The application of Baumgarte stabilization method can make the system converge to the desired trajectory asymptotically. The paper focuses on the selection of stability parameters from the perspective of control accuracy, and the combination of control accuracy and convergence speed will be the next research topic.



---

*Source: 8945301-2018-05-06.xml*