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

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.


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][2][3][4][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][9][10][11][12][13][14][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][20][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 2 Mathematical Problems in Engineering The control system of the method for constrained stabilization. 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 Section 2 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.

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 where M > 0 is the  ×  mass matrix, Q is the generalized force for unconstrained system, and Q c 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 of  generalized coordinates, and there are  independent movements at the position level, the constraint equation can be written as where q = [q 1 , q 2 , . . ., q  ] T is the generalized coordinate matrix.Formula ( 2) is differentiated twice with respect to time; the constraint equation at acceleration level is where A is the  ×  Jacobian matrix and b = − Ȧ q is a  × 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] in 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 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 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.
Combining (3) and ( 6) and arranging them, finishing can obtain where b  = b −  Φ − Φ.The corrected equation for constraint violation stabilization can be written as In order to meet the needs of the corrected trajectory, the additional generalized constraint force on the system should be reexpressed as

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 firstorder integral method is used, the numerical solution of the equation yields where the subscript represents the numerical solution at the corresponding time step and Δ is the integration time step.Since (10) is a difference equation of the discrete data function, the  transform of ( 10) can obtain where  0 is the  transform variable.Rearranging (11) yields and employing Laplace transform to the first integral equation yields where  is the operator of Laplace domain.Analyzing the similarities between ( 12) and ( 13), the relationship between  and  0 can be obtained as Then, based on the first-order integral method, substituting (14) in any ()/() yields a ( 0 )/( 0 ).The Laplace transform of ( 6) results in 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  0 -plane.On the other hand, the position on the  0 -plane should be found out, which corresponds to the left half plane of the -plane, the region of system stability.According to the definition of  transform where  is the sample period.From Laplace transform,  =  + , so it is possible to write 0     =   , ∠ 0 = .
From ( 18), it can be seen that if  = 0 on the -plane, | 0 | = 1 on the  0 -plane; all advisable  0 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 -plane obtained by (17) will all mapped to the interior of a unit circle with the center at the origin on the  0 -plane.Therefore, from the mapping relation between the -plane and the  0 -plane, it is shown that, on the  0 -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  0 can be obtained as Letting roots of (19) fall in the unit circle, the area of parameters  and  can be obtained, when Δ the time step for integration is determined.Letting 1 = Δ and 1 = Δ 2 , the areas of values of 1 and 1 are shown in Figure 2 when  0 locate in the unit circle.
The stable boundary of a system is a unit circle | 0 | = 1, Φ and Φ will converge to 0, if | 0 | < 1.The smaller | 0 |, the faster Φ and Φ converge to 0, and the smaller ∠ 0 , 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.

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  =  (q) ,  =  (q) ,  = ℎ (q) (20) and the velocity mapping between the generalized coordinate system and the Cartesian coordinate system is ẋ = ḟ (q) q , ẏ = ġ (q) q , ż = ḣ (q) q .
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 Thus, Instantaneous errors of velocities and positions between integral calculation values and the given theorem values in the Cartesian space can be defined as Further, the numerical index for verifying stability parameters, the positional constraint error, and the velocity constraint error can be defined as If selected values of parameters could make  1 and  2 smaller, parameter values are better.But it is difficult to select the appropriate  and , satisfying  1 and  2 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 where  is the position weighting factor and  is the velocity weighting factor. is used as the final criteria to select  and .Let   = sin ,  1 = sin  1 ,  2 = sin  2 ,  3 = sin  3 ,  12 = sin( 1 +  2 ),  13 = sin( 1 +  3 ),  23 = sin( 2 +  3 ),   = cos ,  1 = cos  1 ,  2 = cos  2 ,  3 = cos  3 ,  12 = cos( 1 +  2 ),  13 = cos( 1 +  3 ),  23 = cos( 2 +  3 ), for ease of writing.According to the general method of manipulator dynamics equation, if there is no external constraint,

Numerical Examples
Therefore, in (1) of Section 2, According to the basic method of Lagrange dynamics modeling, the inertial matrix for the manipulator can be obtained by ] .
The components of M are given by The matrix of Coriolis and centrifugal forces for the manipulator are given by The nonzero values of Γ  are given by The gravity terms are given by 4.2.The Object Trajectory.From Figure 2, 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 From Figure 2 and the forward kinematics of the manipulator, the position mapping relations between the Cartesian coordinate and the generalized coordinate can be obtained as Plugging (34a), (34b), and (34c) into (35a), (35b), and (35c), respectively, and arranging them yields 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 and items in b  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 b  have been obtained.

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.
Figure 4 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.
According to the determination method of parameter values in Section 3.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  =  = 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 realtime performance of the robot control because it is only the preparation stage of the control process.
Putting  = 9.5 and  = 4.5 into b  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.
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.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 14 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.

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.

5 1Figure 2 :
Figure 2: Stability region in the 1-1 plane for the first-order integral method.

( 21 )
The obtained position and velocity by the dynamic equation corrected with constraint violation stabilization are defined as   ,   ,   and ẋ  , ẏ  , ż  on the direction of , ,  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 Δ =  −   , Δ =  −   , Δ =  −   .

4. 1 .
The System Model.A 3-link spatial manipulator, as shown in Figure3, is selected to analyze the selection of stabilizing parameters.In Figure3,  −1 is the length of the  link of the manipulator,  −1 is the distance from the gravity center of the  link to the end of the joint, and   indicates the generalized position of the  joint.Suppose link masses of the 3-link spatial manipulator are  1 =  2 =  3 = 1 kg, lengths are  1 =  2 =  3 = 1 m, distances of gravity centers are  0 =  1 =  2 = 0.5 m, moments of inertia of  axis are  1 =  2 =  3 = 1, moments of inertia of  axis are  1 =  2 =  3 = 1, and moments of inertia of  axis are  1 =  2 =  3 = 1.

Figure 8 :Figure 9 :
Figure 8: Velocity violation errors in directions of coordinate axes for the end point of the manipulator.

Figure 10 :Figure 11 :
Figure 10: Position violation errors in directions of coordinate axes for the end point of the manipulator.

Figure 12 :
Figure 12: Comparison of the velocity violation error.

Figure 13 :
Figure 13: Comparison of the position violation error.

Figure 14 :
Figure 14: Comparison of required torques for each joint.