Optimization control method for shock absorber

Abstract
A control system for optimizing the performance of a vehicle suspension system by controlling the damping factor of one or more shock absorbers is described. The control system uses a fitness (performance) function that is based on the physical laws of minimum entropy. The control system uses a fuzzy neural network that is trained by a genetic analyzer. The genetic analyzer uses a fitness function that maximizes information while minimizing entropy production. The fitness function uses a difference between the time differential of entropy from a control signal produced in a learning control module and the time differential of the entropy calculated by a model of the suspension system that uses the control signal as an input. The entropy calculation is based on a dynamic model of an equation of motion for the suspension system such that the suspension system is treated as an open dynamic system.
Description




REFERENCE TO RELATED APPLICATION




The present application claims priority benefit of Japanese Application No. Hei 11-009954, filed Jan. 18, 1999, the disclosure of which is hereby included by reference in its entirety.




BACKGROUND OF THE INVENTION




1. Field of the Invention




This invention relates to an optimization control method for a shock absorber having a non-linear kinetic characteristic.




2. Description of the Related Art




Feedback control systems are widely used to maintain the output of a dynamic system at a desired value in spite of external disturbance forces that would move the output away from the desired value. For example, a household furnace controlled by a thermostat is an example of a feedback control system. The thermostat continuously measures the air temperature of the house, and when the temperature falls below a desired minimum temperature, the thermostat turns the furnace on. When the furnace has warmed the air above the desired minimum temperature, then the thermostat turns the furnace off. The thermostat-furnace system maintains the household temperature at a constant value in spite of external disturbances such as a drop in the outside air temperature. Similar types of feedback control are used in many applications.




A central component in a feedback control system is a controlled object, otherwise known as a process “plant,” whose output variable is to be controlled. In the above example, the plant is the house, the output variable is the air temperature of the house, and the disturbance is the flow of heat through the walls of the house. The plant is controlled by a control system. In the above example, the control system is the thermostat in combination with the furnace. The thermostat-furnace system uses simple on-off feedback control to maintain the temperature of the house. In many control environments, such as motor shaft position or motor speed control systems, simple on-off feedback control is insufficient. More advanced control systems rely on combinations of proportional feedback control, integral feedback control, and derivative feedback control. Feedback that is the sum of proportional plus integral plus derivative feedback is often referred to as PID control.




The PID control system is a linear control system that is based on a dynamic model of the plant. In classical control systems, a linear dynamic model is obtained in the form of dynamic equations, usually ordinary differential equations. The plant is assumed to be relatively linear, time invariant, and stable. However, many real-world plants are time varying, highly nonlinear, and unstable. For example, the dynamic model may contain parameters (e.g., masses, inductances, aerodynamic coefficients, etc.) which are either poorly known or depend on a changing environment. Under these conditions, a linear PID controller is insufficient.




Evaluating the motion characteristics of a nonlinear plant is often difficult, in part due to the lack of a general analysis method. Conventionally, when controlling a plant with nonlinear motion characteristics, it is common to find certain equilibrium points of the plant and the motion characteristics of the plant are linearized in a vicinity near an equilibrium point. Control is then based on evaluating the pseudo (linearized) motion characteristics near the equilibrium point. This technique works poorly, if at all, for plants described by models that are unstable or dissipative. The optimization control for a non-linear kinetic characteristic of a controlled process has not been well developed. A general analysis method for non-linear kinetic characteristic has not been previously available, so a control device suited for the linear-kinetic characteristic is often substituted. Namely, for the controlled process with the non-linear kinetic characteristic, a suitable balance point for the kinetic characteristic is picked. Then, the kinetic characteristic of the controlled process is linearized in a vicinity of the balance point, whereby the evaluation is conducted relative to pseudo-kinetic characteristics.




However, this method has several disadvantageous. Although the optimization control may be accurately conducted around the balance point, its accuracy decreases beyond this balance point. Further, this method cannot typically keep up with various kinds of environmental changes around the controlled process.




Shock absorbers used for automobiles and motor cycles are one example of a controlled process having the non-linear kinetic characteristic. The optimization of the non-linear kinetic characteristic has been long sought because vehicle's turning performances and ride are greatly affected by the damping characteristic and output of the shock absorbers.




SUMMARY OF THE INVENTION




The present invention solves these and other problems by providing a new control system for optimizing a shock absorber having a non-linear kinetic characteristic. The new AI control system uses a fitness (performance) function that is based on the physical laws of minimum entropy. This control system can be used to control complex plants described by nonlinear, unstable, dissipative models. The control system is configured to use smart simulation techniques for controlling the shock absorber (plant).




In one embodiment, the control system comprises a learning system, such as a neural network that is trained by a genetic analyzer. The genetic analyzer uses a fitness function that maximizes sensor information while minimizing entropy production.




In one embodiment, a suspension control uses a difference between the time differential (derivative) of entropy from the learning control unit and the time differential of the entropy inside the controlled process (or a model of the controlled process) as a measure of control performance. In one embodiment, the entropy calculation is based on a thermodynamic model of an equation of motion for a controlled process plant that is treated as an open dynamic system.




The control system is trained by a genetic analyzer. The optimized control system provides an optimum control signal based on data obtained from one or more sensors. For example, in a suspension system, a plurality of angle and position sensors can be used. In an off-line (laboratory) learning mode, fuzzy rules are evolved using a kinetic model (or simulation) of a vehicle and is suspension system. Data from the kinetic model is provided to an entropy calculator which calculates input and output entropy production of the model. The input and output entropy productions are provided to a fitness function calculator that calculates a fitness function as a difference in entropy production rates for the genetic analyzer. The genetic analyzer uses the fitness function to develop a training signal for the off-line control system. Control parameters from the off-line control system are then provided to an online control system in the vehicle.




In one embodiment, the invention includes a method for controlling a nonlinear object (a plant) by obtaining an entropy production difference between a time differentiation (dS


u


/dt) of the entropy of the plant and a time differentiation (dS


c


/dt) of the entropy provided to the plant from a controller. A genetic algorithm that uses the entropy production difference as a fitness (performance) function evolves a control rule in an off-line controller. The nonlinear stability characteristics of the plant are evaluated using a Lyapunov function. The genetic analyzer minimizes entropy and maximizes sensor information content. Control rules from the off-line controller are provided to an online controller to control suspension system. In one embodiment, the online controller controls the damping factor of one or more shock absorbers (dampers) in the vehicle suspension system.




In some embodiments, the control method also includes evolving a control rule relative to a variable of the controller by means of a genetic algorithm. The genetic algorithm uses a fitness function based on a difference between a time differentiation of the entropy of the plant (dS


u


/dt) and a time differentiation (dS


c


/dt) of the entropy provided to the plant. The variable can be corrected by using the evolved control rule.




In another embodiment, the invention comprises an AI control system adapted to control a nonlinear plant. The AI control system includes a simulator configured to use a thermodynamic model of a nonlinear equation of motion for the plant. The thermodynamic model is based on a Lyapunov function (V), and the simulator uses the function V to analyze control for a state stability of the plant. The AI control system calculates an entropy production difference between a time differentiation of the entropy of said plant (dS


u


/dt) and a time differentiation (dS


c


/dt) of the entropy provided to the plant by a low-level controller that controls the plant. The entropy production difference is used by a genetic algorithm to obtain an adaptation function in which the entropy production difference is minimized. The genetic algorithm provides a teaching signal to a fuzzy logic classifier that determines a fuzzy rule by using a learning process. The fuzzy logic controller is also configured to form a control rule that sets a control variable of the controller in the vehicle.




In yet another embodiment, the invention comprises a new physical measure of control quality based on minimum production entropy and using this measure for a fitness function of genetic algorithm in optimal control system design. This method provides a local entropy feedback loop in the control system. The entropy feedback loop provides for optimal control structure design by relating stability of the plant (using a Lyapunov function) and controllability of the plant (based on production entropy of the control system). The control system is applicable to all control systems, including, for example, control systems for mechanical systems, bio-mechanical systems, robotics, electro-mechanical systems, etc.











BRIEF DESCRIPTION OF THE DRAWINGS




The advantages and features of the disclosed invention will readily be appreciated by persons skilled in the art from the following detailed description when read in conjunction with the drawings listed below.





FIG. 1

is a block diagram illustrating a control system for a shock absorber.





FIG. 2A

is a block diagram showing a fuzzy control unit that estimates an optimal throttle amount for each shock absorber and outputs signals according to the predetermined fuzzy rule based on the detection results.





FIG. 2B

is a block diagram showing a learning control unit having a fuzzy neural network.





FIG. 3

is a schematic diagram of a four-wheel vehicle suspension system showing the parameters of the kinetic models for the vehicle and suspension system.





FIG. 4

is a detailed view of the parameters associated with the right-front wheel from FIG.


3


.





FIG. 5

is a detailed view of the parameters associated with the left-front wheel from FIG.


3


.





FIG. 6

is a detailed view of the parameters associated with the right-rear wheel from FIG.


3


.





FIG. 7

is a detailed view of the parameters associated with the left-rear wheel from











DETAILED DESCRIPTION





FIG. 1

is a block diagram illustrating one embodiment of an optimization control system


100


for controlling one or more shock absorbers in a vehicle suspension system.




This control system


100


is divided in an actual (online) control module


102


in the vehicle and a learning (offline) module


101


. The learning module


101


includes a learning controller


118


, such as, for example, a fuzzy neural network (FNN). The learning controller (hereinafter “the FNN


118


”) can be any type of control system configured to receive a training input and adapt a control strategy using the training input. A control output from the FNN


118


is provided to a control input of a kinetic model


120


and to an input of a first entropy production calculator


116


. A sensor output from the kinetic model is provided to a sensor input of the FNN


118


and to an input of a second entropy production calculator


114


. An output from the first entropy production calculator


116


is provided to a negative input of an adder


119


and an output from the second entropy calculator


114


is provided to a positive input of the adder


119


. An output from the adder


119


is provided to an input of a fitness (performance) function calculator


112


. An output from the fitness function calculator


112


is provided to an input of a genetic analyzer


110


. A training output from the genetic analyzer


110


is provided to a training input of the FNN


118


.




The actual control module


102


includes a fuzzy controller


124


. A control-rule output from the FNN


118


is provided to a control-rule input of a fuzzy controller


124


. A sensor-data input of the fuzzy controller


124


receives sensor data from a suspension system


126


. A control output from the fuzzy controller


124


is provided to a control input of the suspension system


126


. A disturbance, such as a road-surface signal, is provided to a disturbance input of the kinetic model


120


and to the vehicle and suspension system


126


.




The actual control module


102


is installed into a vehicle and controls the vehicle suspension system


126


. The learning module


101


optimizes the actual control module


102


by using the kinetic model


120


of the vehicle and the suspension system


126


. After the learning control module


101


is optimized by using a computer simulation, one or more parameters from the FNN


118


are provided to the actual control module


101


.




In one embodiment, a damping coefficient control-type shock absorber is employed, wherein the fuzzy controller


124


outputs signals for controlling a throttle in an oil passage in one or more shock absorbers in the suspension system


126


.





FIGS. 2A and 2B

illustrate one embodiment of a fuzzy controller


200


suitable for use in the FNN


118


and/or the fuzzy controller


124


. In the fuzzy controller


200


, data from one or more sensors is provided to a fuzzification interface


204


. An output from the fuzzification interface


204


is provided to an input of a fuzzy logic module


206


. The fuzzy logic module


206


obtains control rules from a knowledge-base


202


. An output from the fuzzy logic module


206


is provided to a de-fuzzification interface


208


. A control output from the de-fuzzification interface


208


is provided to a controlled process


210


(e.g. the suspension system


126


, the kinetic model


120


, etc.).




The sensor data shown in

FIGS. 1 and 2

, can include, for example, vertical positions of the vehicle z


0


, pitch angle β, roll angle α, suspension angle η for each wheel, arm angle θ for each wheel, suspension length z


6


for each wheel, and/or deflection z


12


for each wheel. The fuzzy control unit estimates the optimal throttle amount for each shock absorber and outputs signals according to the predetermined fuzzy rule based on the detection results.




The learning module


101


includes a kinetic model


120


of the vehicle and suspension to be used with the actual control module


101


, a learning control module


118


having a fuzzy neural network corresponding to the actual control module


101


(as shown in FIG.


2


B), and an optimizer module


115


for optimizing the learning control module


118


.




The optimizer module


115


computes a difference between a time differential of entropy from the FNN


118


(dSc/dt) and a time differential of entropy inside the subject process (i.e., vehicle and suspensions) obtained from the kinetic model


120


. The computed difference is used as a performance function by a genetic optimizer


110


. The genetic optimizer


110


optimizes (trains) the FNN


118


by genetically evolving a teaching signal. The teaching signal is provided to a fuzzy neural network in the FNN


118


. The genetic optimizer


110


optimizes the fuzzy neural network (FNN) such that an output of the FNN, when used as an input to the kinetic module


120


, reduces the entropy difference between the time differentials of both entropy values.




The fuzzy rules from the FNN


118


are then provided to a fuzzy controller


124


in the actual control module


102


. Thus, the fuzzy rule (or rules) used in the fuzzy controller


124


(in the actual control module


101


), are determined based on an output from the FNN


118


(in the learning control unit), that is optimized by using the kinetic model


120


for the vehicle and suspension.




The genetic algorithm


110


evolves an output signal a based on a performance function ƒ. Plural candidates for α are produced and these candidates are paired according to which plural chromosomes (parents) are produced. The chromosomes are evaluated and sorted from best to worst by using the performance function ƒ. After the evaluation for all parent chromosomes, good offspring chromosomes are selected from among the plural parent chromosomes, and some offspring chromosomes are randomly selected. The selected chromosomes are crossed so as to produce the parent chromosomes for the next generation. Mutation may also be provided. The second-generation parent chromosomes are also evaluated (sorted) and go through the same evolutionary process to produce the next-generation (i.e., third-generation) chromosomes. This evolutionary process is continued until it reaches a predetermined generation or the evaluation function ƒ finds a chromosome with a certain value. The outputs of the genetic algorithm are the chromosomes of the last generation. These chromosomes become input information α provided to the FNN


118


.




In the FNN


118


, a fuzzy rule to be used in the fuzzy controller


124


is selected from a set of rules. The selected rule is determined based on the input information α from the genetic algorithm


110


. Using the selected rule, the fuzzy controller


124


generates a control signal C


dn


for the vehicle and suspension system


126


. The control signal adjusts the operation (damping factor) of one or more shock absorbers to produce a desired ride and handling quality for the vehicle.




The genetic algorithm


110


is a nonlinear optimizer that optimizes the performance function ƒ. As is the case with most optimizers, the success or failure of the optimization often ultimately depends on the selection of the performance function ƒ.




The fitness function


112


ƒ for the genetic algorithm


110


is given by









f
=

min








S



t







(
1
)













where












S



t


=

(









S
c




t


-








S
u




t



)





(
2
)













The quantity dS


u


/dt represents the rate of entropy production in the output x(t) of the kinetic model


120


. The quantity dS


c


/dt represents the rate of entropy production in the output C


dn


of the FNN


118


.




Entropy is a concept that originated in physics to characterize the heat, or disorder, of a system. It can also be used to provide a measure of the uncertainty of a collection of events, or, for a random variable, a distribution of probabilities. The entropy function provides a measure of the lack of information in the probability distribution. To illustrate, assume that p(x) represents a probabilistic description of the known state of a parameter, that p(x) is the probability that the parameter is equal to z. If p(x) is uniform, then the parameter p is equally likely to hold any value, and an observer will know little about the parameter p. In this case, the entropy function is at its maximum. However, if one of the elements of p(z) occurs with a probability of one, then an observer will know the parameter p exactly and have complete information about p. In this case, the entropy of p(x) is at its minimum possible value. Thus, by providing a measure of uniformity, the entropy function allows quantification of the information on a probability distribution.




It is possible to apply these entropy concepts to parameter recovery by maximizing the entropy measure of a distribution of probabilities while constraining the probabilities so that they satisfy a statistical model given measured moments or data. Though this optimization, the distribution that has the least possible information that is consistent with the data may be found. In a sense, one is translating all of the information in the data into the form of a probability distribution. Thus, the resultant probability distribution contains only the information in the data without imposing additional structure. In general, entropy techniques are used to formulate the parameters to be recovered in terms of probability distributions and to describe the data as constraints for the optimization. Using entropy formulations, it is possible to perform a wide range of estimations, address ill-posed problems, and combine information from varied sources without having to impose strong distributional assumptions.




Entropy-based optimization of the FNN is based on obtaining the difference between a time differentiation (dS


u


/dt) of the entropy of the plant and a time differentiation (dS


c


/dt) of the entropy provided to the kinetic model from the FNN


118


controller that controls the kinetic model


120


, and then evolving a control rule using a genetic algorithm. The time derivative of the entropy is called the entropy production rate. The genetic algorithm


110


minimizes the difference between the entropy production rate of the kinetic model


120


(that is, the entropy production of the controlled process) (dS


u


/dt) and the entropy production rate of the low-level controller (dS


c


/dt) as a performance function. Nonlinear operation characteristics of the kinetic model (the kinetic model represents a physical plant) are calculated by using a Lyapunov function.




The dynamic stability properties of the model


120


near an equilibrium point can be determined by use of Lyapunov functions as follows. Let V(x) be a continuously differentiable scalar function defined in a domain D⊂R


n


that contains the origin. The function V(x) is said to be positive definite if V(0)=0 and V(x)>0 for x≠0. The function V(x) is said to be positive semidefinite if V(x)≧0 for all x. A function V(x) is said to be negative definite or negative semidefinite if −V(x) is positive definite or positive semidefinite, respectively. The derivative of V along the trajectories {dot over (x)}=ƒ(x) is given by:











V
.



(
x
)


=





i
=
1

n










V




x
i






x
.

i



=




V



x




f


(
x
)








(
3
)













where ∂V/∂x is a row vector whose ith component is ∂V/∂x


i


and the components of the n-dimensional vector ƒ(x) are locally Lipschitz functions of x, defined for all x in the domain D. The Lyapunov stability theorem states that the origin is stable if there is a continuously differentiable positive definite function V(x) so that {dot over (V)}(x) is negative definite. A function V(x) satisfying the conditions for stability is called a Lyapunov function.




The genetic algorithm realizes


110


the search of optimal controllers with a simple structure using the principle of minimum entropy production.




The fuzzy tuning rules are shaped by the learning system in the fuzzy neural network


118


with acceleration of fuzzy rules on the basis of global inputs provided by the genetic algorithm


110


.




In general, the equation of motion for non-linear systems is expressed as follows by defining “q” as generalized coordinates, “f” and “g” random functions, “Fe” as control input.









q
=


f


(


q
.

,
q

)


+

g


(
q
)


-

F
e






(
a
)













In the above equation, when the dissipation term and control input in the second term are multiplied by a speed, the following equation can be obtained for the time differentials of the entropy.












S



t


=




f


(


q
.

,




q

)




q
.


-
Feq

=









S
u




t


-








S
c




t








(
b
)













dS/dt is a time differential of entropy for the entire system. dS


u


/dt is a time differential of entropy for the plant, that is the controlled process. dS


c


/dt is a time differential of entropy for the control system for the plant.




The following equation is selected as Lyapunov function for the equation (a).









V
=



(




q
2


+

S
2


)

/
2

=


(




q
2


+


(


S
u

-

S
c


)

2


)

/
2






(
c
)













The greater the integral of the Lyapunov function, the more stable the kinetic characteristic of the plant.




Thus, for the stabilization of the systems, the following equation is introduced as a relationship between the Lyapunov function and entropy production for the open dynamic system.












V

/


t


=





q





q


+


(


S
u

-

S
c


)







(





S
u


/


t


-




S
c


/


t



)



<
0





(
d
)









q





q


<


(


S
u

-

S
c


)







(





S
c


/


t


-




S
u


/


t



)






(
e
)













A Duffing oscillator is one example of a dynamic system. In the Duffing oscillator, the equation of motion is expressed as:











x
¨

+

x
.

+
x
+

x
3


=
0




(
f
)













the entropy production from this equation is calculated as:












S

/


t


=

x
3





(
g
)













Further, Lyapunov function relative to the equation (f) becomes:










V
=



(

1
/
2

)







x
2


+

U


(
x
)




,


U


(
x
)


=



(

1
/
4

)







x
4


-


(

1
/
2

)







x
2








(
h
)













If the equation (f) is modified by using the equation (h), it is expressed as:











x
¨

+
x
+




U


(
x
)





x



=
0




(
i
)













If the left side of the equation (i) is multiplied by x as:








x
¨

+
x
+





U


(
x
)





x



x


=
0










Then, if the Lyapunov function is differentiated by time, it becomes:









V

/


t


=


x





x

+


(




U


(
x
)



/


x


)


x












If this is converted to a simple algebra, it becomes:












V

/


t


=


(

1
/
T

)







(



S

/


t


)






(
j
)













wherein “T” is a normalized factor.




dS/dt is used for evaluating the stability of the system. dS


u


/dt is a time change of the entropy for the plant. −dS


c


/dt is considered to be a time change of negative entropy given to the plant from the control system.




The present invention calculates waste such as disturbances for the entire control system of the plant based on a difference between the time differential dS


u


/dt of the entropy of the plant that is a controlled process and time differential dSu/dt of the entropy of the plant. Then, the evaluation is conducted by relating to the stability of the controlled process that is expressed by Lyapunov function. In other words, the smaller the difference of both entropy, the more stable the operation of the plants.




Suspension Control




In one embodiment, the control system


100


of

FIGS. 1-2

is applied to a suspension control system, such as, for example, in an automobile, truck, tank, motorcycle, etc.





FIG. 3

is a schematic diagram of an automobile suspension system. In

FIG. 3

, a right front wheel


301


is connected to a right arm


313


. A spring and damper linkage


334


controls the angle of the arm


313


with respect to a body


310


. A left front wheel


302


is connected to a left arm


323


and a spring and damper


324


controls the angle of the arm


323


. A front stabilizer


330


controls the angle of the left arm


313


with respect to the right arm


323


. Detail views of the four wheels are shown in

FIG. 4-7

. Similar linkages are shown for a right rear wheel


303


and a left rear wheel


304


. The




In one embodiment of the suspension control system, the learning module


101


uses a kinetic model


120


for the vehicle and suspension.

FIG. 3

illustrates each parameter of the kinetic models for the vehicle and suspensions.

FIGS. 4-7

illustrate exploded views for each wheel as illustrated in FIG.


3


.




A kinetic model


120


for the suspension system in the vehicle


300


shown in

FIGS. 3-7

is developed as follows.




1. Description of Transformation Matrices




1.1 A Global Reference Coordinate x


r


, y


r


, z


r


{r} is Assumed to be at the Geometric Center P


r


of the Vehicle Body


310






The following are the transformation matrices to describe the local coordinates for:




{2} is a local coordinate in which an origin is the center of gravity of the vehicle body


310


;




{7} is a local coordinate in which an origin is the center of gravity of the suspension;




{10n} is a local coordinate in which an origin is the center of gravity of the n'th arm;




{12n} is a local coordinate in which an origin is the center of gravity of the n'th wheel;




{13n} is a local coordinate in which an origin is a contact point of the n'th wheel relative to the road surface; and




{14} is a local coordinate in which an origin is a connection point of the stabilizer.




Note that in the development that follows, the wheels


302


,


301


,


304


, and


303


are indexed using “i”, “ii”, “iii”, and “iv”, respectively.




1.2 Transformation Matrices




As indicated, “n” is a coefficient indicating wheel positions such as i, ii, iii, and iv for left front, right front, left rear and right rear respectively. The local coordinate systems x


0


, y


0


, and z


0


{0} are expressed by using the following conversion matrix that moves the coordinate {r} along a vector (0,0,z


0


)










0
r


T
=

[



1


0


0


0




0


1


0


0




0


0


1



z
0





0


0


0


1



]











Rotating the vector {r} along y


r


with an angle β makes a local coordinate system x


0c


, y


0c


, z


0c


{0r} with a transformation matrix


0c




0


T.














0

c

0


T
=

[




cos





β



0



sin





β



0




0


1


0


0






-
sin






β



0



cos





β



0




0


0


0


1



]





(
4
)













Transferring {0r} through the vector (a


1n


, 0, 0) makes a local coordinate system x


0f


, y


0f


, z


0f


{0f} with a transformation matrix


0r




0f


T.














0

n


0

c



T
=

[



1


0


0



a

1

n






0


1


0


0




0


0


1


0




0


0


0


1



]





(
5
)













The above procedure is repeated to create other local coordinate systems with the following transformation matrices.















1

n


0

n



T

=

[



1


0


0


0




0



cos





α





-
sin






α



0




0



sin





α




cos





α



0




0


0


0


1



]





(
6
)























2

1

i



T
=

[



1


0


0



a
0





0


1


0



b
0





0


0


1



c
0





0


0


0


1



]





(
7
)













1.3 Coordinates for the Wheels (Index n: i for the Left Front, ii for the Right Front, etc.) are Generated as Follows




Transferring {1n} through the vector (0, b


2n


, 0) makes local coordinate system x


3n


, y


3n


, z


3n


{3n} with transformation matrix


1f




3n


T.














3

n


1

n



T
=

[



1


0


0


0




0


1


0



b

2

n






0


0


1


0




0


0


0


1



]





(
8
)

























4

n


3

n



T

=

[



1


0


0


0




0



cos






γ
n






-
sin







γ
n




0




0



sin






γ
n





cos






γ
n




0




0


0


0


1



]





(
9
)
























5

n


4

n



T
=

[



1


0


0


0




0


1


0


0




0


0


1



c

1

n






0


0


0


1



]





(
10
)

























6

n


5

n



T

=

[



1


0


0


0




0



cos






η
n






-
sin







η
n




0




0



sin






η
n





cos






η
n




0




0


0


0


1



]





(
11
)
























7

n


6

n



T
=

[



1


0


0


0




0


1


0


0




0


0


1



z

6

n






0


0


0


1



]





(
12
)
























8

n


4

n



T
=

[



1


0


0


0




0


1


0


0




0


0


1



c

2

n






0


0


0


1



]





(
13
)

























9

n


8

n



T

=

[



1


0


0


0




0



cos






θ
n






-
sin







θ
n




0




0



sin






θ
n





cos






θ
n




0




0


0


0


1



]





(
14
)
























10

n


9

n



T
=

[



1


0


0


0




0


1


0



e

1

n






0


0


1


0




0


0


0


1



]





(
15
)























11

n


9

n



T

=

[



1


0


0


0




0


1


0



e

3

n






0


0


1


0




0


0


0


1



]





(
16
)























12

n


11

n



T

=

[



1


0


0


0




0




cos

ζ

n




-


sin

ζ

n




0




0




sin

ζ

n





cos

ζ

n



0




0


0


0


1



]





(
17
)























13

n


12

n



T

=

[



1


0


0


0




0


1


0


0




0


0


1



z

12

n






0


0


0


1



]





(
18
)























14

n


9

n



T

=

[



1


0


0


0




0


1


0



e

0

n






0


0


1


0




0


0


0


1



]





(
19
)













1.4 Some Matrices are Sub-Assembled to Make the Calculation Simpler














1

n

r


T

=




0
r


T






0

n


0

c



T






1

n


0

n



T












=




[



1


0


0


0




0


1


0


0




0


0


1



z
0





0


0


0


1



]



[




cos

β



0



sin

β



0




0


1


0


0





-

sin

β




0



cos

β



0




0


0


0


1



]




[



1


0


0



a

1

n






0


1


0


0




0


0


1


0




0


0


0


1



]




[



1


0


0


0




0



cos

α




-

sin

α




0




0



sin

α




cos

α



0




0


0


0


1



]








=


[




cos

β



0



sin

β





a

1

n




cos

β






0


1


0


0





-

sin

β




0



cos

β





z
0

-


a
1



sin

β







0


0


0


1



]



[



1


0


0


0




0



cos

α




-

sin

α




0




0



sin

α




cos

α



0




0


0


0


1



]








=

[




cos

β




sin

βsinα




sin

βcosα





a

1

n




cos

β






0



cos

α




-

sin

α




0





-

sin

β





cos

βsinα




cos

βcosα





z
0

-


a

1

n




sin

β







0


0


0


1



]









(
20
)
























4

n

r


T

=





1

n

r


T






3

n


1

n



T






4

n


3

n



T












=



[




cos

β




sin

βsinα




sin

βcosα





a

1

n




cos

β






0



cos

α




-

sin

α




0





-

sin

β





cos

βsinα




cos

βcosα





z
0

-


a

1

n




sin

β







0


0


0


1



]



[



1


0


0


0




0


1


0



b

2

n






0


0


1


0




0


0


0


1



]


·

[



1


0


0


0




0



cosγ
n




-

sinγ
n




0




0



sinγ
n




cosγ
n



0




0


0


0


1



]








=

[




cos

β





sin

βsin



(

α
+

γ
n


)






sin

βcos



(

α
+

γ
n


)







b

2

n




sin

βsinα


+


a

1

n




cos

β







0



cos


(

α
+

γ
n


)





-

sin


(

α
+

γ
n


)







b

2

n




cos

α







-

sin

β






cos

βsin



(

α
+

γ
n


)






cos

βcos



(

α
+

γ
n


)






z
0

-


b

2

n




c

os

βsinα


-


a

1

n




sin

β







0


0


0


1



]









(
21
)
























7

n


4

n



T

=





5

n


4

n



T






6

n


5

n



T






7

n


6

n



T












=



[



1


0


0


0




0


1


0


0




0


0


1



c

1

n






0


0


0


1



]



[



1


0


0


0




0



cosη
n




-

sinη
n




0




0



sinη
n




cosη
n



0




0


0


0


1



]




[



1


0


0


0




0


1


0


0




0


0


1



z

6

n






0


0


0


1



]








=


[



1


0


0


0




0



cosη
n




-

sinη
n




0




0



sinη
n




cosη
n




c

1

n






0


0


0


1



]



[



1


0


0


0




0


1


0


0




0


0


1



z

6

n






0


0


0


1



]








=

[



1


0


0


0




0



cosη
n




-

sinη
n






-

z

6

n





sinη
n






0



sinη
n




cosη
n





c

1

n


+


z

6

n




cosη
n







0


0


0


1



]









(
22
)

































10

n


4

n




T

=






T

9

n


8

n



8

n


4

n




T

10

n


9

n



T







=







[



1


0


0


0




0


1


0


0




0


0


1



c

2

n






0


0


0


1



]



[



1


0


0


0




0



cos






θ
n






-
sin







θ
n




0




0



sin






θ
n





cos






θ
n




0




0


0


0


1



]




[



1


0


0


0




0


1


0



e

1

n






0


0


1


0




0


0


0


1



]








=






[



1


0


0


0




0



cos






θ
n






-
sin







θ
n




0




0



sin






θ
n





cos






θ
n





c

2

n






0


0


0


1



]



[



1


0


0


0




0


1


0



e

1

n






0


0


1


0




0


0


0


1



]








=





[



1


0


0


0




0



cos






θ
n






-
sin







θ
n






e

1

n



cos






θ
n






0



sin






θ
n





cos






θ
n






c

2

n


+


e

1

n



sin






θ
n







0


0


0


1



]








(
23
)

































12

n


4

n




T

=






T

9

n


8

n



8

n


4

n




T

11

n


9

n




T

12

n


11

n



T







=








[



1


0


0


0




0


1


0


0




0


0


1



c

2

n






0


0


0


1



]



[



1


0


0


0




0



cos






θ
n






-
sin







θ
n




0




0



sin






θ
n





cos






θ
n




0




0


0


0


1



]




[



1


0


0


0




0


1


0



e

3

n






0


0


1


0




0


0


0


1



]




[



1


0


0


0




0



cos






ζ
n






-
sin







ζ
n




0




0



sin






ζ
n





cos






ζ
n




0




0


0


0


1



]








=







[



1


0


0


0




0



cos






θ
n






-
sin







θ
n




0




0



sin






θ
n





cos






θ
n





c

2

n






0


0


0


1



]



[



1


0


0


0




0


1


0



e

3

n






0


0


1


0




0


0


0


1



]




[



1


0


0


0




0



cos






ζ
n






-
sin







ζ
n




0




0



sin






ζ
n





cos






ζ
n




0




0


0


0


1



]








=






[



1


0


0


0




0



cos






θ
n






-
sin







θ
n






e

3

n



cos






θ
n






0



sin






θ
n





cos






θ
n






c

2

n


+


e

3

n



sin






θ
n







0


0


0


1



]



[



1


0


0


0




0



cos






ζ
n






-
sin







ζ
n




0




0



sin






ζ
n





cos






ζ
n




0




0


0


0


1



]








=





[



1


0


0


0




0



cos






(


θ
n

+

ζ
n


)





-

sin


(


θ
n

+





ζ
n


)







e

3

n



cos






θ
n






0



sin






(


θ
n

+

ζ
n


)





cos


(


θ
n

+





ζ
n


)






c

2

n


+


e

3

n



sin






θ
n







0


0


0


1



]








(
24
)













2. Description of All the Parts of the Model Both in Local Coordinate Systems and Relations to the Coordinate {r} or {1n} Referenced to the Vehicle Body


310






2.1 Description in Local Coordinate Systems










P
body
2

=


P

susp
.
n


7

n


=


P

arm
.
n


10

n


=


P

wheel
.
n


12

n


=


P

touchpoint
.
n


13

n


=


P

stab
.
n


14

n


=

[



0




0




0




1



]










(
25
)













2.2 Description in Global Reference Coordinate System {r}













P
body
r

=






T
2

1

i



1

i

r



TP
body
2








=







[




cos





β




sin





β





sin





α




sin





β





cos





α





a


1

i








cos





β





0



cos





α





-
sin






α



0






-
sin






β




cos





β





sin





α




cos





β





cos





α





z
0

-


a

1

i







sin





β






0


0


0


1



]



[



1


0


0



a
0





0


1


0



b
0





0


0


1



c
0





0


0


0


1



]




[



0




0




0




1



]








=





[















a
0






cos





β

+


b
0






sin





β





sin





α

+


c
0






sin





β





cos





α

+


a

1

i







cos





β










b
0






cos





α

-


c
0






sin





α









-

a
0







sin





β

+


b
0






cos





β





sin





α

+


c

0







cos





β





cos





α

-


a

1

i







sin





β






1



]








(
26
)























P

susp
.
n

r

=






T

7

n


4

n



4

n

r



TP
suspn

7

n









=






[




cos





β




sin





β





sin






(

α
+

γ
n


)





sin





βcos






(

α
+

γ
n


)







b


2

n








sin





β





sin





α

+


a

1

n







cos





β






0



cos






(

α
+

γ
n


)





-

sin


(

α
+

γ
n


)







b

2

n







cos





α







-
sin






β




cos





βsin






(

α
+

γ
n


)





cos





β






cos


(

α
+

γ
n


)







z
0

+


b

2

n







cos





βsin





α

-


a

1

n



sin





β






0


0


0


1



]

·













[



1


0


0


0




0



cos






η
n






-
sin







η
n






-

z

6

n




sin






η
n






0



sin






η
n





cos






η
n






c

1

n


+


z

6

n







cos






η
n







0


0


0


1



]



[



0




0




0




1



]








=





[










{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n








cos


(

α
+

γ
n


)



+


b

2

n







sin





α


}


sin





β

+


a

1

n







cos





β









-

z

6

n









sin


(

α
+

γ
n

+

η
n


)



-


c

1

n








sin


(

α
+

γ
n


)



+


b

2

n







cos





α









{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n








cos


(

α
+

γ
n


)



+


b

2

n







sin





α


}






cos





β

-


a

1

n







sin





β






1



]








(
27
)


























P

arm
.
n

r

=






T

10

n


4

n



4

n

r



TP
armn

10

n









=






[




cos





β




sin





β





sin






(

α
+

γ
n


)





sin





βcos






(

α
+

γ
n


)







b


2

n








sin





β





sin





α

+


a

1

n







cos





β






0



cos






(

α
+

γ
n


)





-

sin


(

α
+

γ
n


)







b

2

n







cos





α







-
sin






β




cos





βsin






(

α
+

γ
n


)





cos





β






cos


(

α
+

γ
n


)







z
0

+


b

2

n







cos





βsin





α

-


a

1

n



sin





β






0


0


0


1



]

·


[



1


0


0


0




0



cos






θ
n






-
sin







θ
n






e

3

n



cos






θ
n






0



sin






θ
n





cos






θ
n






c

2

n


+


e

1

n







sin






θ
n







0


0


0


1



]



[



0




0




0




1



]









=





[










{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n








cos


(

α
+

γ
n


)



+


b

2

n







sin





α


}


sin





β

+


a

1

n







cos





β









e

1

n








cos


(

α
+

γ
n

+

θ
n


)



-


c

2

n








sin


(

α
+

γ
n


)



+


b

2

n







cos





α









{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n








cos


(

α
+

γ
n


)



+


b

2

n







sin





α


}






cos





β

-


a

1

n







sin





β






1



]








(
28
)


































P

wheel
.
n

r

=






T

12

n


4

n



4

n

r



TP

wheel
.
n


12

n









=







[




cos





β




sin





β





sin






(

α
+

γ
n


)





sin





βcos






(

α
+

γ
n


)







b


2

n








sin





β





sin





α

+


a

1

n







cos





β






0



cos






(

α
+

γ
n


)





-

sin


(

α
+

γ
n


)







b

2

n







cos





α







-
sin






β




cos





βsin






(

α
+

γ
n


)





cos





β






cos


(

α
+

γ
n


)








b

2

n







cos





βsin





α

-


a

1

n



sin





β






0


0


0


1



]



[



1


0


0


0




0



cos






(


θ
n

+

ζ
n


)





-

sin


(


θ
n

+

ζ
n


)







e

3

n



cos






θ
n






0



sin






(


θ
n

+

ζ
n


)





cos






(


θ
n

+

ζ
n


)






c

2

n


+


e

3

n







sin






θ
n







0


0


0


1



]




[



0




0




0




1



]








=





[










{



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n








cos


(

α
+

γ
n


)



+


b

2

n







sin





α


}


sin





β

+


a

1

n







cos





β









e

3

n








cos


(

α
+

γ
n

+

θ
n


)



-


c

2

n








sin


(

α
+

γ
n


)



+


b

2

n







cos





α







{


z
0

+


{



e

3

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n








cos


(

α
+

γ
n


)



+


b

2

n







sin





α


}






cos





β

-


a

1

n







sin





β







1



]








(
29
)































P

touchpoint
.
n

r

=






T

12

n


4

n



4

n

r



T

13

n


12

n




TP

touchpoint
.
n


13

n









=





[




cos





β




sin





β





sin






(

α
+

γ
n


)





sin





βcos






(

α
+

γ
n


)







b


2

n








sin





β





sin





α

+


a

1

n







cos





β






0



cos






(

α
+

γ
n


)





-

sin


(

α
+

γ
n


)







b

2

n







cos





α







-
sin






β




cos





βsin






(

α
+

γ
n


)





cos





β






cos


(

α
+

γ
n


)







z
0

+


b

2

n







cos





βsin





α

-


a

1

n



sin





β






0


0


0


1



]














[



1


0


0


0




0



cos


(


θ
n

+

ζ
n


)





-

sin


(


θ
n

+

ζ
n


)







e

3

n



cos






θ
n






0



sin


(


θ
n

+

ζ
n


)





cos


(


θ
n

+

ζ
n


)






c

2

n


+


e

3

n







sin






θ
n







0


0


0


1



]



[



1


0


0


0




0


1


0


0




0


0


1



z

12

n






0


0


0


0



]




[



0




0




0




1



]








=





[










{



z

12

n



cos





α

+


e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n








cos


(

α
+

γ
n


)



+


b

2

n







sin





α


}


sin





β

+


a

1

n







cos





β









-

z

12

n








sin





α

+


e

3

n








cos


(

α
+

γ
n

+

θ
n


)



-


C

2

n




sin


(

α
+

γ
n


)



+


b

2

n







cos





α








z
0

+


{



z

12

n



cos





α

+


e

3

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n








cos


(

α
+

γ
n


)



+


b

2

n







sin





α


}






cos





β

-


a

1

n







sin





β






1



]








(
30
)













where ζ is substituted by,







ζ
n

=


-

γ
n


-

θ
n












because of the link mechanism to support a wheel at this geometric relation.




2.3 Description of the Stabilizer Linkage Point in the Local Coordinate System {1n}




The stabilizer works as a spring in which force is proportional to the difference of displacement between both arms in a local coordinate system {1n} fixed to the body


310


.













P

stab
.
n


1

n


=






T

4

n


3

n



3

n


1

n




T

8

n


4

n




T

9

n


8

n




T

14

n


9

n




TP

stab
.
n


14

n









=







[



1


0


0


0




0


1


0




b

2

n











0


0


1


0




0


0


0


1



]



[



1


0


0


0




0



cos






γ
n






-
sin







γ
n




0




0



sin






γ
n





cos






γ
n




0




0


0


0


1



]




[



1


0


0


0




0


1


0


0




0


0


1



c

2

n






0


0


0


1



]















[



1


0


0


0




0



cos






θ
n






-
sin







θ
n




0




0



sin






θ
n





cos






θ
n




0




0


0


0


1



]



[



1


0


0


0




0


1


0



e

0

n






0


0


1


0




0


0


0


1



]




[



0




0




0




1



]








=





[







0







e

0

n








cos


(


γ
n

+

θ
n


)



-


c

2

n



sin






γ
n


+

b

2

n










e

0

n








sin


(


γ
n

+

θ
n


)



+


c

2

n



cos






γ
n







0



]








(
31
)













3. Kinetic Energy, Potential Energy and Dissipative Functions for the <Body>, <Suspension>, <Arm>, <Wheel> and <Stabilizer>




Kinetic energy and potential energy except by springs are calculated based on the displacement referred to the inertial global coordinate {r}. Potential energy by springs and dissipative functions are calculated based on the movement in each local coordinate.




<Body>










T
b
tr

=


1
2




m
b



(



x
.

b
2

+


y
.

b
2

+


z
.

b
2


)







(
32
)













where











x
b

=



(


a
0

+

a

1

n



)






cos





β

+


(



b
0






sin





α

+


c
0






cos





α


)






sin





β










y
b

=



b
0






cos





α





-


c
0






sin





α










z
b

=


z
0

-


(


a
0

+

a

1

n



)






sin





β

+


(



b
0






sin





α

+


c
0






cos





α


)






cos





β







(
33
)













and














q

j
,
k


=




β

,
α
,

z
0











x
b




β


=







-

(


a
0

+

a

1

n



)



sin





β

+


(



b
0


sin





α

+


c
0






cos





α


)


cos





β












x
b




α


=






(



b
0






cos





α

-


c
0






sin





α


)


sin





β











y
b




β


=









x
b





z
0



=





y
b





z
0



=
0












y
b




α


=







-

b
0







sin





α

-


c
0






cos





α












z
b




β


=







-

(


a
0

+

a

1

n



)



cos





β

-


(



b
0






sin





α

+


c
0






cos





α


)


sin





β












z
b




α


=






(



b
0






cos





α

-


c
0






sin





α


)


cos





β











z
b





z
0



=




1







(
34
)













and thus













T
b
tr

=






1
2




m
b



(



x
.

b
2

+


y
.

b
2

+


z
.

b
2


)









=






1
2



m
b






j
,
k




(






x
b





q
j








x
b





q
k






q
.

j




q
.

k


+





y
b





q
j








y
b





q
k






q
.

j




q
.

k


+




















z
b





q
j








z
b





q
k






q
.

j




q
.

k


)







=






1
2



m
b








β
.

2




{



-

(


a
0

+

a
1


)



sin





β

+


(



b
0






sin





α

+


c
0






cos





α


)


cos





β


}

2


+

















α
.

2




{


(



b
0






cos





α

-


c
0






sin





α


)


sin





β

}

2


+




α
.

2



(



-

b
0







sin





α

-


c
0






cos





α


)


2

+















β
.

2




{



-

(


a
0

+

a
1


)



cos





β

-


(



b
0






sin





α

+


c
0






cos





α


)


sin





β


}

2


+















α
.

2




{


(



b
0






cos





α

-


c
0






sin





α


)


cos





β

}

2


+


z
.

0
2

+

2



α





.



β
.

[

{

-

(


a
0

+






















a
1

)



sin





β

+


(



b
0






sin





α

+


c
0






cos





α


)


cos





β


}



(



b
0






cos





α

-

















c
0






sin





α

)



sin





β

+

{



-

(


a
0

+

a
1


)



cos





β

-

(



b
0






sin





α

+






















c
0






cos





α

)



sin





β

}



(



b
0






cos





α

-


c
0






sin





α


)


cos





β

]

-

2


β
.








z
.

0



{

(


a
0

+




















a

1

n


)



cos





β

+


(



b
0






sin





α

-


c
0






cos





α


)


sin





β


}

+

2


α
.









z
.

0

(



b
0






cos





α

-


















c
0






sin





α

)



cos





β








=






1
2



m
b








α
.

2



(


b
0
2

+

c
0
2


)


+



β
.

2



{



(


a
0

+

a

1

i



)

2

+


(



b
0






sin





α

+

cos





α


)

2


}


+
















z
.

0
2

-

2


α
.








β
.



(


a
0

+

a

1

i



)




(



b

0







cos





α

-


c
0






sin





α


)


-

2



β





.




z
.

0



{

(


a
0

+



















a

1

i


)







cos





β

+


(



b
0






sin





α

-


c
0






cos





α


)


sin





β

+

2


α
.









z
.

0

(



b

0







cos





α

-


















c
0






sin





α

)







cos





β









(
35
)




















T
b
ro

=


1
2



(



I
bx



ω
bx
2


+


I
by



ω
by
2


+


I
bz



ω
bz
2



)






(
36
)













where






&AutoLeftMatch;





ω
bx

=

α
.








ω
by

=

β
.








ω
bz

=
0


























T
b
ro

=






1
2



(



I
bx




α
.

2


+


I
by




β
.

2



)









U
b

=






m
b


g






z
b








=






m
b


g


{



-

(


a
0

+

a

1

n



)



sin





β

+


(



b
0






sin





α

+


c
0






cos





α


)


cos





β


}















<Suspension>










T
sn
tr

=






1
2




m
sn



(



x
.

sn
2

+


y
.

sn
2

+


z
.

sn
2


)







(
37
)













where










x
sn

=





{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n








cos


(

α
+

γ
n


)



+

















b

2

n







sin





α

}



sin





β

+


α

1

n







cos





β








y
sn

=







-

z

6

n





sin


(

α
+

γ
n

+

η
n


)



-


c

1

n




sin


(

α
+

γ
n


)



+


b

2

n







cos





α









z
sn

=






z
0

+

{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n








cos


(

α
+

γ
n


)



+


















b

2

n







sin





α

}



cos





β

-


α

1

n







sin





β

























q

j
,
k


=





z

6

n



,

η
n

,
α
,
β
,

z
0











x
sn





z

6

n




=






cos


(

α
+

γ
n

+

η
n


)



sin





β











x
sn





η
n



=






-

z

6

n





sin


(

α
+

γ
n

+

η
n


)



sin





β











x
sn




α


=





{



-

z

6

n





sin


(

α
+

γ
n

+

η
n


)



-


c

1

n



sin






(

α
+

γ
n


)


+
















b

2

n



cos





α

}



sin





β










x
sn




β


=





{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n




cos


(

α
+

γ
n


)



+

















b

2

n



sin





α

}



cos





β

-


a

1

n



sin





β











y
sn





z

6

n




=





-

sin


(

α
+

γ
n

+

η
n


)













y
sn





η
n



=






-

z

6

n









cos


(

α
+

γ
n

+

η
n


)













y
sn




α


=







-

z

6

n









cos


(

α
+

γ
n

+

η
n


)



-


c

1

n








cos


(

α
+

γ
n


)



-


b

2

n



sin





α












y
sn




β


=









x
sn





z
0



=





y
sn





z
0



=
0












z
sn





z
0



=




1







(
38
)


























z
sn





z

6

n




=






cos


(

α
+

γ
n

+

η
n


)



cos





β











z
sn





η
n



=






-

z

6

n





sin


(

α
+

γ
n

+

η
n


)



cos





β











z
sn




α


=





{



-

z

6

n





sin


(

α
+

γ
n

+

η
n


)



-


c

1

n



sin






(

α
+

γ
n


)


+















b

2

n



cos





α

}


cos





β










z
sn




β


=





-

{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n




cos


(

α
+

γ
n


)



+

















b

2

n



sin





α

}


sin





β

-


a

1

n



cos





β








(
39
)

























T
sn
tr


=






1
2




m
sn



(



x
.

sn
2

+


y
.

sn
2

+


z
.

sn
2


)









=






1
2



m
sn






j
,
k




(






x
sn





q
j








x
sn





q
k






q
.

j




q
.

k


+





y
sn





q
j








y
sn





q
k






q
.

j




q
.

k


+



















z
sn





q
j








z
sn





q
k






q
.

j




q
.

k


)







(
40
)



























=






1
2



m
sn







z
.


6

n

2

+



η
.

n
2







z

6

n

2


+



α
.

2

[


z

6

n

2

+

c

1

n

2

+

b

2

n

2

+


















2






{



z

6

n








c

1

n







cos






η
n


-


z

6

n








b

2

n



sin






(


γ
n

+

η
n


)


-


c

1

n








b

2

n



sin






γ
n



}


]

+













β
.

2

[

{

(



z

6

n







cos






(

α
+

γ
n

+

η
n


)


+


c

1

n







cos






(

α
+

γ
n


)


+





















b

2

n







sin





α

)

}

2

+

a

1

n

2


]

+


z
.

0
2

+

2







z
.


6

n








α
.







{



c

1

n







sin






η
n


+
















b

2

n







cos






(


γ
n

+

η
n


)


}

-

2



z
.


6

n








β
.







a

1

n







cos






(

α
+

γ
n

+

η
n


)


+












2







η
.

n







α
.







z

6

n








{


z

6

n


+


c

1

n







cos






η
n


-


b

2

n







sin






(


γ
n

-

η
n


)



}


+













2



η
.

n







β
.







z

6

n








a

1

n







sin






(

α
+

γ
n

+

η
n


)


+












2






α
.







β
.







a

1

n








{



z

6

n







sin






(

α
+

γ
n

+

η
n


)


+


c

1

n







sin






(

α
+

γ
n


)


-
















b

2

n



cos





α

}

+

2







z
.


6

n









z
.

0






cos






(

α
+

γ
n

+

η
n


)






cos





β

-












2







η
.

n








z
.

0







z

6

n







sin






(

α
+

γ
n

+

η
n


)






cos





β

+












2






α
.








z
.

0







{



z

6

n



sin






(

α
+

γ
n

+

η
n


)


-


c

1

n







sin






(

α
+

γ
n


)


+

















b

2

n







cos





α

}






cos





β

+

2






β
.








z
.

0







{



z

6

n







cos






(

α
+

γ
n

+

η
n


)


+




















c

1

n







sin






(

α
+

γ
n


)


+


b

2

n







cos





α


}






sin





β

+


α

1

n







cos





β


]









(
41
)























T
sn
ro






0







U
sn

=







m
sn


g






z
sn


+


1
2







k
sn








(


z

6

n


-

l
sn


)

2









=






m
sn



g
[


z
0

+

{



z

6

n







cos






(

α
+

γ
n

+

η
n


)


+


c

1

n



cos






(

α
+

γ
n


)


+





















b

2

n







sin





α

}






cos





β

-


a

1

n







sin





β


]

+


1
2







k
sn








(


z

6

n


-

l
sn


)

2









F
sn

=






-

1
2








c
sn








z
.


6

n

2









(
42
)













<Arm>










T
an
tr

=


1
2




m
an



(



x
.

an
2

+


y
.

an
2

+


z
.

an
2


)







(
43
)













where













x
an

=





{



e

1

n







sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n







cos






(

α
+

γ
n


)


+
















b

2

n







sin





α

}


sin





β

+


a

1

n







cos





β








y
an

=







e

1

n







cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n







sin






(

α
+

γ
n


)


+


b

2

n







cos





α









z
an

=






z
0

+

{



e

1

n







sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n







cos






(

α
+

γ
n


)


+

















b

2

n







sin





α

}






cos





β

-


a

1

n







sin





β








(
44
)













and














q

j
,
k


=





θ
n


,
α
,
β
,

z
0











x
an





θ
n



=






e

1

n








cos


(

α
+

γ
n

+

θ
n


)



sin





β











x
an




α


=





{



e

1

n



cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n



sin






(

α
+

γ
n


)


+
















b

2

n



cos





α

}



sin





β










x
an




β


=





{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+

















b

2

n



sin





α

}



cos





β

-


a

1

n



sin





β











y
an





θ
n



=






-

e

1

n









sin


(

α
+

γ
n

+

θ
n


)













y
an




α


=







-

e

1

n









sin


(

α
+

γ
n

+

θ
n


)



-


c

2

n








cos


(

α
+

γ
n


)



-


b

2

n



sin





α












y
an




β


=









x
an





z
0



=





y
an





z
0



=
0












z
an





θ
n



=






e

1

n








cos


(

α
+

γ
n

+

θ
n


)



cos





β











z
an




α


=





{



e

1

n



cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n



sin






(

α
+

γ
n


)


+















b

2

n



cos





α

}


cos





β










z
an




β


=





-

{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+

















b

2

n



sin





α

}


sin





β

-


a

1

n



cos





β











z
an





z
0



=




1







(
45
)













thus













T
an
tr

=






1
2




m
an



(



x
.

an
2

+


y
.

an
2

+


z
.

an
2


)









=






1
2



m
an






j
,
k




(






x
an





q
j








x
an





q
k






q
.

j




q
.

k


+





y
an





q
j








y
an





q
k






q
.

j




q
.

k


+



















z
an





q
j








z
an





q
k






q
.

j




q
.

k


)







(
46
)



























=






1
2



m
an








θ
.

n
2







e

1

n

2


+



α
.

2

[


e

1

n

2

+

c

2

n

2

+

b

2

n

2

-


















2






{



e

1

n








c

2

n







sin






θ
n


+


e

1

n








b

2

n



cos






(


γ
n

+

θ
n


)


+


c

2

n








b

2

n



sin






γ
n



}


]

+













β
.

2

[

{



e

1

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n







cos






(

α
+

γ
n


)


+




















b

2

n







sin





α

)

}

2

+

a

1

n

2


]

+


z
.

0
2

+

2


θ
.



α
.







e

1

n








{


e

1

n


-


c

2

n







sin






θ
n


+
















b

2

n







cos






(


γ
n

+

θ
n


)


}

-

2



θ
.

n







β
.







e

1

n




a

1

n







cos






(

α
+

γ
n

+

θ
n


)


-











2






α
.







β
.







a

1

n








{



e

1

n



cos






(

α
+

γ
n

+

θ
n


)


-


c

1

n







sin






(

α
+

γ
n


)


+
















b

2

n







cos





α

}

-

2



θ
.

n








z
.

0







e

1

n







cos






(

α
+

γ
n

+

θ
n


)






cos





β

+











2






α
.








z
.

0







{



e

1

n







cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n







sin






(

α
+

γ
n


)


+

















b

2

n



cos





α

}


cos





β

+

2






β
.









z
.

0





[

{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+





















c

2

n



cos






(

α
+

γ
n


)


+


b

2

n







sin





α


}






sin





β

+


α

1

n



cos





β


]









(
47
)























T
an
ro

=






1
2



I
ax



ω
ax
2








=






1
2





I
ax



(


α
.

+


θ
.

n


)


2









U
an

=






m
an


g






z
an








=






m
an



g
[


z
0

+

{



e

1

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n



cos






(

α
+

γ
n


)


+




















b

2

n







sin





α

}






cos





β

-


a

1

n







sin





β


]







(
48
)













<Wheel>










T
wn
tr

=






1
2




m
wn



(



x
.

wn
2

+


y
.

wn
2

+


z
.

wn
2


)







(
49
)













where













x
wn

=





{



e

3

n







sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n







cos






(

α
+

γ
n


)


+
















b

2

n







sin





α

}


sin





β

+


a

1

n







cos





β








y
wn

=







e

3

n







cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n







sin






(

α
+

γ
n


)


+


b

2

n







cos





α









z
wn

=






z
0

+

{



e

3

n







sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n







cos






(

α
+

γ
n


)


+

















b

2

n







sin





α

}






cos





β

-


a

1

n







sin





β








(
50
)













Substituting m


an


with m


wn


and e


1n


with e


3n


in the equation for the arm, yields an equation for the wheel as:













T
wn
tr

=






1
2



m
wn








θ
.

n
2







e

3

n

2


+



α
.

2

[


e

3

n

2

+

c

2

n

2

+

b

2

n

2

-


















2






{



e

3

n








c

2

n







sin






θ
n


+


e

3

n








b

2

n



cos






(


γ
n

+

θ
n


)


+


c

2

n








b

2

n



sin






γ
n



}


]


+













β
.

2

[

{



e

3

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n







cos






(

α
+

γ
n


)


+




















b

2

n







sin





α

}


2

+

a

1

n

2


]

+


z
.

0
2

+

2


θ
.



α
.







e

3

n








{


e

3

n


-


c

2

n







sin






θ
n


+

















b

2

n







cos






(


γ
n

+

θ
n


)


}


-

2



θ
.

n







β
.







e

3

n




a

1

n







cos






(

α
+

γ
n

+

θ
n


)


-











2






α
.







β
.







a

1

n








{



e

3

n



cos






(

α
+

γ
n

+

θ
n


)


-


c

1

n







sin






(

α
+

γ
n


)


+
















b

2

n







cos





α

}

+

2



θ
.

n








z
.

0







e

3

n







cos






(

α
+

γ
n

+

θ
n


)






cos





β

+











2






α
.








z
.

0







{



e

3

n







cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n







sin






(

α
+

γ
n


)


+


















b

2

n



cos





α

}



cos





β

-

2






β
.









z
.

0





[

{



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+






















c

2

n




sin


(

α
+

γ
n


)



+


b

2

n







sin





α


}







sin





β

+


α

1

n



cos





β


]









(
51
)























T
wn
ro

=




0







U
wn

=







m
wn


g






z
wn


+


1
2







k
wn








(


z

12

n


-

l
wn


)

2









=






m
wn



g
[


z
0

+

{



e

3

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n



cos






(

α
+

γ
n


)


+





















b

2

n







sin





α

}






cos





β

-


a

1

n







sin





β


]

+


1
2







k
wn








(


z

12

n


-

l
wn


)

2









F
wn

=






-

1
2








c
wn








z
.


12

n

2









(
52
)













<Stabilizer>










T
zn
tr


0




(
53
)







T
zn
ro


0




(
54
)























U

zi
,
ii









1
2





k
zi



(


z
zi

-

z
zii


)


2








=






1
2








k
zi

[


{



e

0

i



sin






(


γ
i

+

θ
i


)


+


c

2

i



cos






γ
i



}

-















{



e

0

ii



sin






(


γ
ii

+

θ
ii


)


+


c

2

ii



cos






γ
ii



}

]

2






=






1
2



k
zi



e

0

i

2




{


sin






(


γ
i

+

θ
i


)


+

sin






(


γ
ii

+

θ
ii


)



}

2















where






e

0

ii



=

-

e

0

i




,


c

2

ii


=

c

2

i



,


γ
ii

=

-

γ
i











U

ziii
,
iv









1
2





k
ziii



(


z
ziii

-

z
ziv


)


2








=






1
2








k
ziii

[


{



e

0

iii



sin






(


γ
iii

+

θ
iii


)


+


c

2

iii



cos






γ
iii



}

-















{



e

0

iv



sin






(


γ
iv

+

θ
iv


)


+


c

2

iv



cos






γ
iv



}

]

2






=






1
2



k
ziii



e

0

iii

2




{


sin






(


γ
iii

+

θ
iii


)


+

sin






(


γ
iv

+

θ
iiv


)



}

2















where






e

0

ii



=



-

e

0

iii









c

2

iv



=

c

2

iii




,


γ
iv

=

-

γ
iii











(
55
)




















F
zn


0




(
56
)













Therefore the total kinetic energy is:










T
tot

=


T
b
tr

+




n
=
i

iv



&LeftBracketingBar;


T
sn
tr

+

T
an
tr

+

T
wn
tr

+

T
b
ro

+

T
an
ro


&RightBracketingBar;







(
57
)























T
tot

=






T
b
tr

+




n
=
i

iv



&LeftBracketingBar;


T
sn
tr

+

T
an
tr

+

T
wn
tr

+

T
b
ro

+

T
an
ro


&RightBracketingBar;









=






1
2



m
b








α
.

2



(


b
0
2

+

c
0
2


)


+



β
.

2



{



(


a
0

+

a

1

i



)

2

+


(



b
0






sin





α

+

cos





α


)

2


}


+
















z
.

0
2

-

2


α
.








β
.



(


a
0

+

a

1

i



)




(



b

0







cos





α

-


c
0






sin





α


)


-

2



β





.




z
.

0



{

(


a
0

+





















a

1

i


)







cos





β

+


(



b
0






sin





α

+


c
0






cos





α


)


sin





β


}

+

2


α
.









z
.

0

(



b

0







cos





α

-



















c
0






sin





α

)







cos





β



+




n
=
i

iv



&LeftBracketingBar;


1
2



m
sn







z
.


6

n

2

+



η
.

n
2







z

6

n

2


+



α
.

2

[


z

6

n

2

+

c

1

n

2

+



















b

2

n

2

+

2






{



z

6

n








c

1

n







cos






η
n


-


z

6

n








b

2

n



sin






(


γ
n

+

η
n


)


-



















c

1

n








b

2

n



sin






γ
n


}


]

+



β
.

2

[

{

(



z

6

n







cos






(

α
+

γ
n

+

η
n


)


+






















c

1

n







cos






(

α
+

γ
n


)


+


b

2

n







sin





α


}


2

+

a

1

n

2


]

+


z
.

0
2

+












2







z
.


6

n








α
.







{



c

1

n







sin






η
n


+


b

2

n







cos






(


γ
n

+

η
n


)



}


-













2



z
.


6

n








β
.







a

1

n







cos






(

α
+

γ
n

+

η
n


)


+













2







η
.

n







α
.







z

6

n








{


z

6

n


+


c

1

n







cos






η
n


-


b

2

n







sin






(


γ
n

-

η
n


)



}


+













2



η
.

n







β
.







z

6

n








a

1

n







sin






(

α
+

γ
n

+

η
n


)


+












2






α
.







β
.







a

1

n








{



z

6

n







sin






(

α
+

γ
n

+

η
n


)


+


c

1

n







sin






(

α
+

γ
n


)


-

















b

2

n



cos





α

}


+

2







z
.


6

n









z
.

0






cos






(

α
+

γ
n

+

η
n


)






cos





β

-












2







η
.

n








z
.

0







z

6

n







sin






(

α
+

γ
n

+

η
n


)






cos





β

+












2






α
.








z
.

0







{



-

z

6

n




sin






(

α
+

γ
n

+

η
n


)


-


c

1

n







sin






(

α
+

γ
n


)


+


















b

2

n







cos





α

}







cos





β

-

2






β
.









z
.

0





[

{



z

6

n







cos






(

α
+

γ
n

+

η
n


)


-






















c

1

n







cos






(

α
+

γ
n


)


+


b

2

n







sin





α


}


+


α

1

n







cos





β


]



+












1
2



m
an








θ
.

n
2







e

1

n

2


+



α
.

2

[


e

1

n

2

+

c

2

n

2

+

b

2

n

2

-


















2






{



e

1

n








c

2

n







sin






θ
n


+


e

1

n








b

2

n



cos






(


γ
n

+

θ
n


)


+


c

2

n








b

2

n



sin






γ
n



}


]


+













β
.

2

[

{



e

1

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n







cos






(

α
+

γ
n


)


+













(









b

2

n







sin





α

)


}

)

2

+

a

1

n

2


]

+


z
.

0
2

+

2


θ
.



α
.







e

1

n








{


e

1

n


-


c

2

n







sin






θ
n


+

















b

2

n







cos






(


γ
n

+

θ
n


)


}


-

2



θ
.

n







β
.







e

1

n




a

1

n







cos






(

α
+

γ
n

+

θ
n


)


-











2






α
.







β
.







a

1

n








{



e

1

n



cos






(

α
+

γ
n

+

θ
n


)


-


c

1

n







sin






(

α
+

γ
n


)


+

















b

2

n







cos





α

}


+

2



θ
.

n








z
.

0







e

1

n







cos






(

α
+

γ
n

+

θ
n


)






cos





β

+











2






α
.








z
.

0







{



e

1

n







cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n







sin






(

α
+

γ
n


)


+


















b

2

n



cos





α

}



cos





β

-

2






β
.









z
.

0





[

{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+





















c

2

n



cos






(

α
+

γ
n


)


+


b

2

n







sin





α


}


+


α

1

n



cos





β




+












1
2



m
wn








θ
.

n
2







e

3

n

2


+



α
.

2

[


e

3

n

2

+

c

2

n

2

+

b

2

n

2

-


















2






{



e

3

n








c

2

n







sin






θ
n


-


e

3

n








b

2

n



cos






(


γ
n

+

θ
n


)


+


c

2

n








b

2

n



sin






γ
n



}


]


+













β
.

2

[

{



e

3

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n







cos






(

α
+

γ
n


)


+




















b

2

n







sin





α

}


2

+

a

1

n

2


]

+


z
.

0
2

+

2


θ
.



α
.







e

3

n








{


e

3

n


-


c

2

n







sin






θ
n


+

















b

2

n







cos






(


γ
n

+

θ
n


)


}


-

2



θ
.

n







β
.







e

3

n




a

1

n







cos






(

α
+

γ
n

+

θ
n


)


-











2






α
.







β
.







a

1

n








{



e

3

n



cos






(

α
+

γ
n

+

θ
n


)


-


c

1

n







sin






(

α
+

γ
n


)


+

















b

2

n







cos





α

}


+

2



θ
.

n








z
.

0







e

3

n







cos






(

α
+

γ
n

+

η
n


)






cos





β

+











2






α
.








z
.

0







{



e

3

n







cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n







sin






(

α
+

γ
n


)


+


















b

2

n



cos





α

}



cos





β

-

2






β
.









z
.

0





[

{



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



-

















c

2

n




cos


(

α
+

γ
n


)



+



b

2

n







sin





α

}

+



α

1

n



cos





β



+















1
2



(



I
bx




α
.

2


+


I
by




β
.

2



)


+


1
2





I
anx



(


α
.

+


θ
.

n


)


2



&RightBracketingBar;








(
58
)



























=







1
2




α
.

2



m
bbI


+



β
.

2



{


m
baI

+



m
b



(



b
0






sin





α

+


c
0


cos





α


)


2


}


+



z
.

0
2



m
b


-














2


α
.







(



β
.



m
ba


-



z
.

0



m
b


cos





β


)



(



b

0







cos





α

-


c
0






sin





α


)


-














2



β





.




z
.

0



{



m
ba






cos





β

+



m
b



(



b
0






sin





α

+


c
0






cos





α


)



sin





β


}


]


+












1
2






n
=
i

iv



&LeftBracketingBar;



m
sn



(



z
.


6

n

2

+



η
.

n
2







z

6

n

2



)


+



θ
.

n
2



m
aw2In


+


z
0
2



m
sawn


+

















α
.

2






m
sawIn

+


m
sn



z

6

n


[


z

6

n


+

2


m
sn



{



c

1

n



cos






η
n


-























b

2

n



sin






(


γ
n

+

η
n


)


}


]

-

2


m
aw1n



{



c

2

n



sin






θ
n


-


















b

2

n



cos






(


γ
n

+

θ
n


)


}




+



β
.

2






m
saw2n

+















m
sn



{



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+


c

1

n



cos






(

α
+

γ
n


)


+


















b

2

n



sin





α

}


2

+


m
an



{



e
1


sin






(

α
+

γ
n

+

θ
n


)


+



















c

2

n



cos






(

α
+

γ
n


)


+


b

2

n



sin





α


}


2

+


m
wn



{



e
3


sin






(

α
+

γ
n

+

θ
n


)


+




















c

2

n



cos






(

α
+

γ
n


)


+


b

2

n



sin





α


}


2



+

2



z
.


6

n




α
.



m
sn



{



c

1

n



sin






η
n


+

















b

2

n



cos






(


γ
n

+

η
n


)


}


-

2







z
.


6

n








β
.


m






a

1

n







cos






(

α
+

γ
n

+

η
n


)


+












2







η
.

n







α
.







m
sn







z

6

n








{


z

6

n


+


c

1

n







cos






η
n


-


b

2

n



sin






(


γ
n

+

η
n


)



}


+













2







η
.

n







β
.



m
sn



z

6

n








a

1

n



sin






(

α
+

γ
n

+

η
n


)


+













2






θ
.








α
.



[


m
aw2In

-


m
aw1n



{



c

2

n







sin






θ
n


-


b

2

n



cos






(


γ
n

+

θ
n


)



}



]



-













2






θ
.







β
.



m
aw1n







a

1

n



cos






(

α
+

γ
n

+

θ
n


)


+












2






α
.







β
.







a

1

n








{



m
sawcn






sin






(

α
+

γ
n


)


-


m
sawbn


cos





α

+


















m
sn



z

6

n



sin






(

α
+

γ
n

+

η
n


)


-


m
aw1n


cos






(

α
+

γ
n

+

θ
n


)



}


+












2







z
.


6

n









z
.

0







m
sn






cos






(

α
+

γ
n

+

η
n


)


cos





β

-













2


(


α
.

+


η
.

n


)




z
.

0



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


cos





β

+













2



θ
.

n








z
.

0



m
awln


cos






(

α
+

γ
n

+

θ
n


)


cos





β

+












2






α
.




z
.

0



{



m
awln






sin






(

α
+

γ
n

+

θ
n


)


-


m
sawcn






sin






(

α
+

γ
n


)


+


















m
sawbn


cos





α

}







cos





β

-

2






β
.




z
.

0

[

{



z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


-
















m
awln



sin


(

α
+

γ
n


)



+


m
sawcn


cos






(

α
+

γ
n


)


+



















m
sawbn


sin





α

}



sin





β

+


m
sawan


cos





β


]



&RightBracketingBar;







(
59
)













where











m
ba

=


m
b



(


a
0

+

a

1

i



)










m
bbI

=



m
b



(


b
0
2

+

c
0
2


)


+

I
bx










m
baI

=




m
b



(


a
0

+

a

1

i



)


2

+

I
by










m
sawn

=


m
sn

+

m
an

+

m
wn










m
sawan

=


(


m
sn

+

m
an

+

m
wn


)



a

1

n











m
sawbn

=


(


m
sn

+

m
an

+

m
wn


)



b

2

n











m
sawcn

=



m
sn



c

1

n



+


(


m
an

+

m
wn


)



c

2

n












m
saw2n

=


(


m
sn

+

m
an

+

m
wn


)



a

1

n

2













m
sawIn

=







m
an



e

1

n

2


+


m
wn



e

3

n

2


+


m
sn



(


c

1

n

2

+

b

2

n

2

-

2


c

1

n




b

2

n



sin






γ
n



)


+














(


m
an

+

m
wn


)



(


c

2

n

2

+

b

2

n

2

-

2


c

2

n




b

2

n



sin






γ
n



)


+

I
axn













m
aw2In

=



m
an



e

1

n

2


+


m
wn



e

3

n

2


+

I
axn










m
aw1n

=



m
an



e

1

n



+


m
wn



e

3

n












m
aw2n

=



m
an



e

1

n

2


+


m
wn



e

3

n

2








(
60
)













Hereafter variables and coefficients which have index “n” implies implicit or explicit that they require summation with n=i, ii, iii, and iv.




Total potential energy is:










U
tot

=


U
b

+




n
=
i

iv



&LeftBracketingBar;


U
sn

+

U
an

+

U
wn

+

U
zn


&RightBracketingBar;







(
61
)



























=







m
b


g


{


z
0

-


(


a
0

+

a

1

n



)


sin





β

+


(



b
0






sin





α

+


c
0






cos





α


)


cos





β


}


+
















n
=
i

iv



&LeftBracketingBar;


m
sn



g
[


z
0

+

{



z

6

n







cos






(

α
+

γ
n

+

η
n


)


+
























c

1

n



cos






(

α
+

γ
n


)


+


b

2

n







sin





α


}






cos





β

-


a

1

n







sin





β


]

+













1
2







k
sn








(


z

6

n


-

l
sn


)

2


+


m
an



g
[


z
0

+

{



e

1

n








sin


(

α
+

γ
n

+

θ
n


)



+























c

2

n



cos






(

α
+

γ
n


)


+


b

2

n







sin





α


}






cos





β

-


a

1

n







sin





β


]

+












m
wn



g
[


z
0

+

{



e

3

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n



cos






(

α
+

γ
n


)


+























b

2

n







sin





α

}






cos





β

-


a

1

n







sin





β


]

+


1
2







k
wn








(


z

12

n


-

l
wn


)

2



&RightBracketingBar;

+













1
2



k
zi



e
oi
2




{


sin


(


γ
i

+

θ
i


)


+

sin


(


γ
ii

+

θ
ii


)



}

2


+













1
2



k
ziii



e
oiii
2




{


sin


(


γ
iii

+

θ
iii


)


+

sin


(


γ
iv

+

θ
iv


)



}

2









(
62
)



























=






g


{



z
0



m
b


-


m
ba


sin





β

+



m
b



(



b
0






sin





α

+


c
0






cos





α


)



cos





β


}


+
















n
=
i

iv





g
[

{



z
0



m
sawn


+


m
sn



z

6

n







cos






(

α
+

γ
n

+

η
n


)


+


















m
aw1n







sin


(

α
+

γ
n

+

θ
n


)



+


m
sawcn


cos






(

α
+

γ
n


)


+

















m
sawbn






sin





α

}






cos





β

-


m
sawan






sin





β


]

+














1
2







k
sn








(


z

6

n


-

l
sn


)

2


+


1
2







k
wn








(


z

12

n


-

l
wn


)

2





+













1
2



k
zi



e

0

i

2




{


sin


(


γ
i

+

θ
i


)


+

sin


(


γ
ii

+

θ
ii


)



}

2


+













1
2



k
ziii



e
oiii
2




{


sin


(


γ
iii

+

θ
iii


)


+

sin


(


γ
iv

+

θ
iv


)



}

2









(
63
)













where











m
ba

=


m
b



(


a
0

+

a

1

i



)










m
sawan

=


(


m
sn

+

m
an

+

m
wn


)



a

1

n











m
sawbn

=


(


m
sn

+

m
an

+

m
wn


)



b

2

n











m
sawcn

=



m
sn



c

1

n



+


(


m
an

+

m
wn


)



c

2

n












γ
ii

=

-

γ
i







(
64
)













4. Lagrange's Equation




The Lagrangian is written as:












L
=






T
tot

-

U
tot








=






1
2

[




α
.

2



m
bbI


+



β
.

2



{


m
baI

+



m
b



(



b
0






sin





α

+


c
0


cos





α


)


2


}


+



z
.

0
2



m
b


-















(


2


α
.







β
.



m
ba


-



z
.

0



m
b


cos





β


)



(



b

0







cos





α

-


c
0






sin





α


)


]

-












2



β





.




z
.

0



{



m
ba






cos





β

+



m
b



(



b
0






sin





α

+


c
0






cos





α


)



sin





β


}


]

+












1
2






n
=
i

iv



&LeftBracketingBar;



m
sn



(



z
.


6

n

2

+



η
.

n
2







z

6

n

2



)


+



θ
.

n
2



m
aw2In


+


z
0
2



m
sawn


+

















α
.

2






m
sawIn

+


m
sn




z

6

n


[


z

6

n


+

2


{



c

1

n



cos






η
n


-























b

2

n



sin






(


γ
n

+

η
n


)


}

]

-

2


m
aw1n



{



c

2

n



sin






θ
n


-

















b

2

n



cos






(


γ
n

+

θ
n


)


}



+



β
.

2






m
saw2n

+















m
sn



{



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+


c

1

n



cos






(

α
+

γ
n


)


+

















b

2

n



sin





α

}

2

+


m
an



{



e
1


sin






(

α
+

γ
n

+

θ
n


)


+


















c

2

n



cos






(

α
+

γ
n


)


+


b

2

n



sin





α


}

2

+


m
wn



{



e
3


sin






(

α
+

γ
n

+

θ
n


)


+



















c

2

n



cos






(

α
+

γ
n


)


+


b

2

n



sin





α


}

2



+

2



z
.


6

n




α
.



m
sn



{



c

1

n



sin






η
n


+
















b

2

n



cos






(


γ
n

+

η
n


)


}

-

2







z
.


6

n








β
.



m
sn



a

1

n







cos






(

α
+

γ
n

+

η
n


)


+












2







η
.

n







α
.







m
sn







z

6

n








{


z

6

n


+


c

1

n







cos






η
n


-


b

2

n



sin






(


γ
n

+

η
n


)



}


+













2







η
.

n







β
.



m
sn



z

6

n








a

1

n



sin






(

α
+

γ
n

+

η
n


)


+













2






θ
.








α
.



[


m
aw2In

-


m
aw1n



{



c

2

n







sin






θ
n


-


b

2

n



cos






(


γ
n

+

θ
n


)



}



]



-













2






θ
.







β
.



m
aw1n







a

1

n



cos






(

α
+

γ
n

+

θ
n


)


+












2






α
.







β
.







a

1

n








{



m
sawcn






sin






(

α
+

γ
n


)


-


m
sawbn


cos





α

+

















m
sn



z

6

n



sin






(

α
+

γ
n

+

η
n


)


-


m
aw1n


cos






(

α
+

γ
n

+

θ
n


)



}

+











2







z
.

0



{




z
.


6

n








m
sn






cos






(

α
+

γ
n

+

η
n


)


+
















(


α
.

+


θ
.

n


}



m
awln


cos






(

α
+

γ
n

+

θ
n


)


-














(


α
.

+


η
.

n


)



z

6

n




m
sn






sin






(

α
+

γ
n

+

η
n


)


-


α
.



m
sawcn






sin






(

α
+

γ
n


)


+
















α
.







m
sawbn


cos





α

-


β
.



m
sawcn



}


cos





β

-











2


β
.





z
.

0

[

{



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-


m
awln


sin






(

α
+

γ
n

+

θ
n


)


+





















m
sawcn


cos






(

α
+

γ
n


)


+


m
sawbn


sin





α


}


sin





β

}

&RightBracketingBar;

-












g


{



z
0



m
b


-


m
ba


sin





β

+



m
b



(



b
0






sin





α

+


c
0






cos





α


)



cos





β


}


-














1
2



k
zi



e

0

i

2




{


sin


(


γ
i

+

θ
i


)


+

sin


(


γ
ii

+

θ
ii


)



}

2


-














1
2



k
ziii



e

0

iii

2




{


sin


(


γ
iii

+

θ
iii


)


+

sin


(


γ
iv

+

θ
iv


)



}

2


-















n
=
i

iv





g
[



z
0



m
sawn


+

{



m
sn



z

6

n







cos






(

α
+

γ
n

+

η
n


)


+



















m
aw1n







sin


(

α
+

γ
n

+

θ
n


)



+


m
sawcn


cos






(

α
+

γ
n


)


+

















m
sawbn






sin





α

}






cos





β

-


m
sawan






sin





β


]

+













1
2







k
sn








(


z

6

n


-

l
sn


)

2


+


1
2







k
wn








(


z

12

n


-

l
wn


)

2











(
65
)



















L




z
0



=

-

g


(


m
b

+

m
sawn


)






















L





z
.

0



=








z
.

0



m
b


+


α
.



m
b


cos






β


(



b
0


cos





α

-


c
0


sin





α


)



-


β
.



{



m
ba


cos





β

+


















m
b



(



b
0


sin





α

+


c
0


cos





α


)



sin





β

}

+



z
.

0



m
sawn


+











{



z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


+


(


α
.

+


θ
.

n


)



m

aw





ln




cos


(

α
+

γ
n

+

θ
n


)



-















(


α
.

+


η
.

n


)



z

6

n




m
sn


sin






(

α
+

γ
n

+

θ
n


)


-



















α

.



m
sawcn



sin


(

α
+

γ
n


)



+


α
.



m
sawbn


cos





α

-















β
.



m
sawan


}


cos





β

-


β
.



{



m

aw





ln




sin


(

α
+

γ
n

+

θ
n


)



-
















z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


+


m
sawcn


cos






(

α
+

γ
n


)


+














m
sawbn


sin





α

}


sin





β
























t




(



L





z
.

0



)


=








z
¨

0



(


m
b

+

m
sawn


)


+


α
¨




m
b



(



b
0


cos





α

-


c
0


sin





α


)



-














β
.



α
.



m
b


sin






β


(



b
0


cos





α

-


c
0


sin





α


)



+















α
.

2



m
b


cos






β


(



b
0


sin





α

+


c
0


cos





α


)



-














β
¨



{



m
ba


cos





β

+



m
b



(



b
0


sin





α

+


c
0


cos





α


)



sin





β


}


+













β
.



{



β
.



m
ba


sin





β

+


α
.




m
b



(



b
0


cos





α

-


c
0


sin





α


)



sin





β

+
















β
.




m
b



(



b
0


sin





α

+


c
0


cos





α


)



cos





β

}

+











{




z
¨


6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


-















(


α
.

+


η
.

n


)




z
.


6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


}

-













(


α
¨

+


η
¨

n


)



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-














(


α
.

+


η
.

n


)




z
.


6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-















(


α
.

+


η
.

n


)

2



z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


-














α
¨



m
sawcn


sin






(

α
+

γ
n


)


-



α
.

2



m
sawcn


sin






(

α
+

γ
n


)


+














α
¨



m
sawbn


cos





α

-



α
.

2



m
sawbn


sin





α

-















β
¨



m
sawan


}


cos





β

-


β
.



{




z
.


6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


-
















(


α
.

+


θ
.

n


)



m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


-














(


α
.

+


η
.

n


)



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-














α
.



m
sawcn


sin






(

α
+

γ
n


)


-


α
.



m
sawbn


cos





α

-















β
.



m
sawan


}


sin





β

-


β
¨



{



m

aw





ln



sin






(

α
+

γ
n

+

θ
n


)


+
















z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


+


m
awcn


cos






(

α
+

γ
n


)


+















m
sawbn


sin





α

}






sin





β

-












β
.



{



(


α
.

+


θ
.

n


)



m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


+
















z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


-














(


α
.

+


η
.

n


)



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-
















α
.



m
sawcn


sin






(

α
+

γ
n


)


+


α
.



m
sawbn


cos





α


}






sin





β

-












β
2



{



m

aw





ln



sin






(

α
+

γ
n

+

θ
n


)


-
















z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


+


m
sawcn


cos






(

α
+

γ
n


)


+














m
sawbn


sin





α

}


cos





β

























L



β


=







-

α
.





z
.

0



m
b


sin





β






(



b
0


cos





α

-


c
0


sin





α


)


+













β
.




z
.

0



{



m
ba


sin





β

-


m
b

(



b
0


sin





α

-





















c
0


cos





α

)


cos





β

}

)






g


{



m
ba


cos





β

+
















m
b



(



b
0


sin





α

+


c
0


cos





α


)



sin





β

}

+













g
[

{



m
sn



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+

















m

aw





ln



sin






(

α
+

γ
n

+

θ
n


)


+
















m
sawcn


cos






(

α
+

γ
n


)


+


m
sawbn


sin





α


}


sin





β

+













m
sawan


cos





β

]

-



z
.

0



{




z
.


6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


+
















(


α
.

+


θ
.

n


)



m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


-














(


α
.

+


η
.

n


)



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-














α
.



m
sawcn


sin






(

α
+

γ
n


)


+


α
.



m
sawbn


cos





α

-















β
.



m
sawan


}


sin





β

+


β
.




z
.

0



{



m

aw





ln



sin






(

α
+

γ
n

+

θ
n


)


+
















z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


+
















m
sawcn


cos






(

α
+

γ
n


)


+


m
sawbn


sin





α


}


cos





β









(
66
)

























L



α


=






{




β
.

2




m
b



(



b
0


cos





α

-


c
0


sin





α


)



+


α
.



β
.



m
ba



}







(



b
0


sin





α

+
















c
0


cos





α

)

-


α
.




z
.

0



m
b


cos






β
(



b
0


sin





α

+
















c
0


cos





α

)

-


β
.




z
.

0




m
b



(



b
0


cos





α

-


c
0


sin





α


)



sin





β

+











&LeftBracketingBar;



β
.

2






m
sn



{



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+


c

1

n



cos






(

α
+

γ
n


)


+



















b

2

n



sin





α

}



{



-

z

6

n




sin






(

α
+

γ
n

+

η
n


)


-
















c

1

n



sin






(

α
+

γ
n


)


+


b

2

n



cos





α


}

+












m
an



{



e

1

n



sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n



cos






(

α
+

γ
n


)


+
















b

2

n



sin





α

}



{



e
1


cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n



sin






(

α
+

γ
n


)


+















b

2

n



cos





α

}

+


m
wn



{



e

3

n



sin






(

α
+

γ
n

+

θ
n


)


+

















c

2

n



cos






(

α
+

γ
n


)


+


b

2

n



sin





α


}



{



e
3


cos






(

α
+

γ
n

+

θ
n


)


-

















c

2

n



sin






(

α
+

γ
n


)


+


b

2

n



cos





α


}



+














z
.


6

n




β
.



m
sn



a
ln


sin






(

α
+

γ
n

+

η
n


)


+















η
.

n



β
.







m
sn



z

6

n




a
ln


cos






(

α
+

γ
n

+

η
n


)


+














θ
.



β
.



m

aw





ln




a

1
n



sin






(

α
+

γ
n

+

θ
n


)


+













α
.



β
.



a
ln



{



m
sawcn


cos






(

α
+

γ
n


)


+


m
sawbn


sin





α

+

















m
sn



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+


m

aw





ln



sin






(

α
+

γ
n

+

θ
n


)



}

-













z
.

0

(




z
.


6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


+















(


α
.

+


θ
.

n


)



m

aw





ln



sin






(

α
+

γ
n

+

θ
n


)


+














(


α
.

+


η
.

n


)



z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


+
















α
.



m
sawcn


cos






(

α
+

γ
n


)


+


α
.



m
sawbn


sin





α


}


cos





β

-












β
.





z
.

0

[

{



m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


-






z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-




















m
sawcn


sin






(

α
+

γ
n


)


+


m
sawbn


cos





α


}


sin





β

&RightBracketingBar;

-














gm
b



(



b
0


cos





α

-


c
0


sin





α


)



cos





β

+

g


{



m
sn



z

6

n



sin






(

α
+

γ
n

+

η
n


)


-

















m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


+















m
sawcn


sin






(

α
+

γ
n


)


-


m
sawbn


cos





α


}


cos





β







(
67
)

























L




η
n



=








α
.

2



m
sn



z

6

n




{



-

c

1

n




sin






η
n


-


b

2

n



cos






(


γ
n

+

η
n


)



}


+














β
.

2



m
sn



{



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+


















c

1

n



cos






(

α
+

γ
n


)


+


b

2

n



sin





α


}



{


-

z

6

n




sin






(

α
+

γ
n

+

η
n


)


}


+













+


z
.


6

n





α
.



m
sn



{



c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


+















z
.


6

n




β
.



m
sn



a

1

n



sin






(

α
+

γ
n

+

η
n


)


-















η
.

n



α
.



m
sn



z

6

n




{



c

1

n



sin






η
n


+


b

2

n



cos






(


γ
n

+

η
n


)



}


+















η
.

n



β
.



m
sn



z

6

n




a

1
n



cos






(

α
+

γ
n

+

η
n


)


+














α
.



β
.



a

1

n




m
sn



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+














gm
sn



z

6

n



sin






(

α
+

γ
n

+

η
n


)


cos





β

-














z
.

0



{




z
.


6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


+

















(


α
.

+


η
.

n


)



z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


}


cos





β

+












β
.




z
.

0



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


sin





β








(
68
)

























L








θ
n



=






-

k
zi




e

0

i

2



{


sin






(


γ
i

+

θ
i


)


+

sin






(


γ
ii

+

θ
ii


)



}



{


cos






(


γ
i

+

θ
i


)


+















cos


(


γ
ii

+

θ
ii


)


}

-


k
ziii



e

0

iii

2



{


sin






(


γ
iii

+

θ
iii


)


+
















sin






(


γ
iv

+

θ
iv


)


}







{


cos






(


γ
iii

+

θ
iii


)


+

cos






(


γ
iv

+

θ
iv


)



}


-














α
.

2



m

aw





ln




{



c

2

n



cos






θ
n


+


b

2

n



sin






(


γ
n

+

θ
n


)



}


+














β
.

2






m
an



{



e

1

n



sin






(

α
+

γ
n

+

θ
n


)



c

2

n



cos






(

α
+

γ
n


)


+



















b

2

n



sin





α

}



e

1

n



cos






(

α
+

γ
n

+

θ
n


)


+












m
wn



{



e

3

n



sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n



cos






(

α
+

γ
n


)


+


















b

2

n



sin





α

}



e

3

n



cos






(

α
+

γ
n

+

θ
n


)




-













θ
.



α
.



m
aw1n



{



c

2

n



cos






θ
n


+


b

2

n



sin






(


γ
n

+

θ
n


)



}


+














θ
.



β
.



m
aw1n



a

1

n



sin






(

α
+

γ
n

+

θ
n


)


+














α
.



β
.



a

1

n




m
aw1m


sin






(

α
+

γ
n

+

θ
n


)


-














gm
aw1n


cos






(

α
+

γ
n

+

θ
n


)


cos





β

-
















z
.

0



(


α
.

+


θ
.

n


)




m

aw





ln



sin






(

α
+

γ
n

+

θ
n


)


cos





β

-













β
.




z
.

0



m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


sin





β








(
69
)

























L




z

6

n




=







m
sn




η
.

n
2



z

6

n



+



α
.

2




m
sn

[


z

6

n


+

{



c

1

n



cos






η
n


-




















b

2

n



sin






(


γ
n

+

η
n


)


}

]

+













β
.

2



m
sn



{



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+


















c

1

n



cos






(

α
+

γ
n


)


+


b

2

n



sin





α


}


cos






(

α
+

γ
n

+

η
n


)


+



















η

.

n



α
.



m
sn



{


2


z

6

n



+


c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


+















η
.

n



β
.



m
sn



a

1

n



sin






(

α
+

γ
n

+

η
n


)


+














α
.



β
.



a

1

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-














gm
sn


cos






(

α
+

γ
n

+

η
n


)


cos





β

-


k
sn



(


z

6

n


-

l
sn


)


-














(


α
.

+


η
.

n


)




z
.

0



m
sn


sin






(

α
+

γ
n

+

η
n


)


cos





β

-













β
.




z
.

0



m
sn


cos






(

α
+

γ
n

+

η
n


)


sin





β








(
70
)






















L




z

12

n




=

-


k
wn



(


z

12

n


-

l
wn


)







(
71
)

























L




β
.



=






β
.






m
saw2n

+

m
baI

+



m
b



(



b
0


sin





α

+


c
0


cos





α


)


2

+















m
sn



{



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+


c

1

n




cos


(

α
+

γ
n


)



+

















b

2

n



sin





α

}

2

+


m
an



{



e

1

n



sin






(

α
+

γ
n

+

θ
n


)


+


















c

2

n



cos






(

α
+

γ
n


)


+


b

2

n



sin





α


}

2

+












m
wn



{



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


















b

2

n



sin





α

}

2



-


α
.




m
ba



(



b
0


cos





α

-


c
0


sin





α


)



-














z
.


6

n




m
sn



a

1

n



cos






(

α
+

γ
n

+

η
n


)


+















η
.

n



m
sn



z

6

n




a

1

n



sin






(

α
+

γ
n

+

η
n


)


-














θ
.



m
aw1n



a

1

n



cos






(

α
+

γ
n

+

θ
n


)


+













α
.



a

1

n




{



m
sawcn


sin






(

α
+

γ
n


)


-


m
sawbn


cos





α

+
















m
sn



z

6

n



sin






(

α
+

γ
n

+

η
n


)


-














m
aw1n


cos






(

α
+

γ
n

+

θ
n


)


}

-



z
.

0

[

{



m
b



b
0


sin





α

+
















c
0


cos





α

)

+


m

aw





ln



sin






(

α
+

γ
n

+

η
n


)


+













z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


+
















m
sawcn


cos






(

α
+

γ
n


)


+


m
sawbn


sin





α


}


sin





β

+












(


m
ba

+

m
sawcn


)


cos





β

]







(
72
)



























t




(



L




β
.



)


=






β
¨






m
saw2n

+

m
baI

+



m
b



(



b
0


sin





α

+


c
0


cos





α


)


2

+















m
sn



{



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+


c

1

n



cos






(

α
+

γ
n


)


+

















b

2

n



sin





α

}

2

+


m
an



{



e

1

n



sin






(

α
+

γ
n

+

θ
n


)


+


















c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}

2

+












m
wn



{



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


















b

2

n



sin





α

}

2



+

2


β
.






α
.




m
b

(



b
0


sin





α

+



















c
0


cos





α

)







(



b
0


cos





α

-


c
0


sin





α


)


+












m
sn



{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n



cos






(

α
+

γ
n


)


+
















b

2

n



sin





α

}







{




z
.


6

n



cos






(

α
+

γ
n

+

η
n


)


-















(


α
.

+


η
.

n


)



z

6

n



sin






(

α
+

γ
n

+

η
n


)


-














α
.



[



c

1

n



sin






(

α
+

γ
n


)


-


b

2

n



cos





α


]


}

+












m
an



{



e

1

n



sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n



cos






(

α
+

γ
n


)


+
















b

2

n



sin





α

}







{



(


α
.

+


θ
.

n


)



e

1

n



cos






(

α
+

γ
n

+

θ
n


)


-















α
.



[



c

2

n



sin






(

α
+

γ
n


)


-


b

2

n



cos





α


]


}

+












m
wn



{



e

3

n



sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n



cos






(

α
+

γ
n


)


+
















b

2

n



sin





α

}







{



(


α
.

+


θ
.

n


)



e

3

n



sin






(

α
+

γ
n

+

θ
n


)


-
















α
.



[



c

2

n



sin






(

α
+

γ
n


)


-


b

2

n



cos





α


]


}



-













α
¨




m
ba



(



b
0


cos





α

-


c
0


sin





α


)



+















α
.

2




m
ba



(



b
0


sin





α

+


c
0


cos





α


)



-















z
¨


6

n




m
sn



a

1

n



cos






(

α
+

γ
n

+

η
n


)


+
















z
.


6

n




(


α
.

+


η
.

n


)




m
sn



a

1

n



sin






(

α
+

γ
n

+

η
n


)


+















η
¨

n



m
sn



z

6

n




a

1

n



sin






(

α
+

γ
n

+

η
n


)


+















η
.

n



m
sn




z
.


6

n




a

1

n



sin






(

α
+

γ
n

+

η
n


)


+
















η
.

n



(


α
.

+


η
.

n


)




m
sn



z

6

n




a

1

n



cos






(

α
+

γ
n

+

η
n


)


-















θ
¨

n



m
aw1n



a

1

n



cos






(

α
+

γ
n

+

θ
n


)


+
















θ
.

n



(


α
.

+


θ
.

n


)




m
aw1n



a

1

n



sin






(

α
+

γ
n

+

θ
n


)


+













α
¨



a

1

n




{



m
sawcn


sin






(

α
+

γ
n


)


-
















m
sawbn


cos





α

+


m
sn



z

6

n



sin






(

α
+

γ
n

+

η
n


)


-














m
aw1n


cos






(

α
+

γ
n

+

θ
n


)


}

+












α
.



a

1

n




{



α
.



m
sawcn


cos






(

α
+

γ
n


)


+


α
.



m
sawbn


sin





α

+
















(


α
.

+


η
.

n


)



m
sm



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+














m
sn




z
.


6

n



sin






(

α
+

γ
n

+

η
n


)


+














(


α
.

+


θ
.

n


)



m
aw1n


sin






(

α
+

γ
n

+

θ
n


)


}

-













z
¨

0

[

{



m
b



(



b
0


sin





α

+


c
0


cos





α


)


+
















m
aw1n


sin






(

α
+

γ
n

+

θ
n


)


+














z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)



m
sawcn


cos






(

α
+

γ
n


)


+















m
sawbn


sin





α

}


sin





β

+

(


m
ba

+


m
sawan


cos





β


]

-













z
.

0

[

{



α
.




m
b



(



b
0


cos





α

-


c
0


sin





α


)



-
















(


α
.

+


θ
.

n


)



m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


+















z
.


6

n





m
sn



(

α
+

γ
n

+

η
n


)



-














(


α
.

+


η
.

n


)



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-
















α
.



m
sawcn


sin






(

α
+

γ
n


)


+


α
.



m
sawbn


cos





α


}


sin





β

+












β
.



{



m
b



(



b
0


sin





α

+


c
0


cos





α


)


+
















m

aw





ln



sin






(

α
+

γ
n

+

θ
n


)


+


z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


+
















m
sawcn


cos






(

α
+

γ
n


)


+


m
sawbn


sin





α


}


cos





β

-













β
.



(


m
ba

+

m
sawan


)



sin





β

}







(
73
)

























L




α
.



=







α
.



m
bbI


-


β
.




m
ba



(



b
0


cos





α

-


c
0


sin





α


)



+















z
.

0



m
b


cos






β


(



b
0


cos





α

-


c
0


sin





α


)



+













α
.






m
sawIn

+


m
sn




z

6

n


[


z

6

n


+

2


{



c

1

n



cos






η
n


-























b

2

n



sin






(


γ
n

+

η
n


)


}

]

-












2


m
aw1n



{



c

2

n



sin






θ
n


-


b

2

n



cos






(


γ
n

+

θ
n


)



}




+














z
.


6

n




m
sn



{



c

1

n



sin






η
n


+


b

2

n



cos






(


γ
n

+

η
n


)



}


+















η
.

n



m
sn



z

6

n




{


z

6

n


+


c

1

n



cos






η
n


-






b

2

n



sin






(


γ
n

+

η
n


)



}


+














θ
.



[


m
aw2In

-


m
aw1n



{



c

2

n



sin






θ
n


-


b

2

n



cos






(


γ
n

+

θ
n


)



}



]


+













β
.



a

1

n




{



m
sawcn


sin






(

α
+

γ
n


)


-


m
sawbn


cos





α

+

















m
sn



z

6

n



sin






(

α
+

γ
n

+

η
n


)


-


m
aw1n


cos






(

α
+

γ
n

+

θ
n


)



}

+













z
.

0



{



m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


-


z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-

















m
sawcn


sin






(

α
+

γ
n


)


+


m
sawbn


cos





α


}


cos





β







(
74
)



























t




(



L




α
.



)


=








β






m
ba


¨



(



b
0


cos





α

-


c
0


sin





α


)


+


β
.



α
.




m
ba

(



b
0


sin





α

+

















c
0


cos





α

)

+



z
¨

0



m
b


cos






β


(



b
0


cos





α

-


c
0


sin





α


)



-













β
.




z
.

0



m
b


sin






β


(



b
0






cos





α

-


c
0


sin





α


)



-














α
.




z
.

0



m
b


cos






β


(



b
0


sin





α

+


c
0


cos





α


)



+













α
¨






m
bb1

+

m
sawIn

+


m
sn




z

6

n


[


z

6

n


+

2


{



c

1

n



cos






η
n


-























b

2

n




sin


(


γ
n

+

η
n


)



}

]

-

2


m
aw1n



{



c

2

n



sin






θ
n


-

















b

2

n



cos






(


γ
n

+

θ
n


)


}



+

α





m
sn





z
.


6

n


[


z

6

n


+

2


{



c

1

n



cos






η
n


-






















b

2

n



sin






(


γ
n

+

η
n


)


}

]

+


m
sn




z

6

n


[



z
.


6

n


-

2



η
.

n



{



c

1

n



sin






η
n


+




















b

2

n



cos






(


γ
n

+

η
n


)


}

]

-

2



θ
.

n



m
aw1n



{



c

2

n



cos






θ
n


+

















b

2

n



sin






(


γ
n

+

θ
n


)


}



+



z
¨


6

n




m
sn



{



c

1

n



sin






η
n


+


b

2

n




cos
(


γ
n

+



















η
n

)

}

+



z
.


6

n





η
.

n



m
sn



{



c

1

n



cos






η
n


-


b

2

n



sin






(


γ
n

+

η
n


)



}


+














η
¨

n



m
sn



z

6

n




{


z

6

n


+


c

1

n



cos






η
n


-


b

2

n



sin






(


γ
n

+

η
n


)



}


+















η
.

n



m
sn




z
.


6

n




{


z

6

n


+


c

1

n



cos






η
n


-


b

2

n



sin






(

γ
+

η
n


)



}


+















η
.

n



m
sn



z

6

n




{



z
.


6

n


-



η
.

n



[



c

1

n



sin






η
n


+


b

2

n




cos


(


γ
n

+

η
n


)




]



}


+














θ
¨



[


m
aw2In

-


m
aw1n



{



c

2

n



sin






θ
n


-


b

2

n



cos






(


γ
n

+

θ
n


)



}



]


-















θ
.

n
2



m
aw1n



{



c

2

n



cos






θ
n


+


b

2

n



sin






(


γ
n

+

θ
n


)



}


]

+












β
¨



a

1

n




{



m
sawcn


sin






(

α
+

γ
n


)


-


m
sawbn


cos





α

+

















m
sn



z

6

n



sin






(

α
+

γ
n

+

η
n


)


-


m
aw1n


cos






(

α
+

γ
n

+

θ
n


)



}

+












β
.



a

1

n




{



α
.



[



m
sawcn



cos


(

α
+

γ
n


)



+


m
sawbn


sin





α


]


+
















m
sn




z
.


6

n



sin






(

α
+

γ
n

+

η
n


)


+














(


α
.

+


η
.

n


)



m
sn



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+














(


α
.

+


θ
.

n


)



m
aw1n



sin


(

α
+

γ
n

+

θ
n


)



}

-













z
¨

0



{



m
aw1n



cos


(

α
+

γ
n

+

θ
n


)



+


z

6

n




m
sn



sin
(

α
+

γ
n

+





















η
n

)

-


m
sawcn



sin


(

α
+

γ
n


)



+


m
sawbn


cos





α


}


cos





β

-













z
.

0



{



-

(


α
.

+


θ
.

n


)




m

aw





ln



sin






(

α
+

γ
n

+

θ
n


)


+

















z
.


6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-














(


α
.

+


η
.

n


)



z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



-
















α
.



m
sawcn


cos






(

α
+

γ
n


)


-


α
.



m
sawbn


sin





α


}


cos





β

-












β
.




z
.

0



{



m

aw





ln




cos


(

α
+

γ
n

+

θ
n


)



-
















z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


-















m
sawcn



sin


(

α
+

γ
n


)



+


m
sawbn


cos





α


}


sin





β







(
75
)

























L





η
.

n



=







m
sn




η
.

n



z

6

n

2


+


α
.



m
sn



z

6

n




{


z

6

n


+


c

1

n



cos






η
n


-

















b

2

n



sin






(


γ
n

+

η
n


)


}

+


β
.



m
sn



6

6

n




a

1

n



sin






(

α
+

γ
n

+

η
n


)


-













z
.

0



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


cos





β








(
76
)



























t




(



L





η
.

n



)


=







m
sn




η
¨

n



z

6

n

2


+

2


m

2

n





η
.

n




z
.


6

n




z

6

n



+














α
¨



m
sn



z

6

n




{


z

6

n


+


c

1

n



cos






η
n


-


b

2

n



sin






(


γ
n

+

η
n


)



}


+














α
¨



m
sn



z

6

n




{


z

6

n


+


c

1

n



cos






η
n


-


b

2

n



sin






(


γ
n

+

η
n


)



}


+














α
.



m
sn



z

6

n




{



z
.


6

n


-



η
.

n



[



c

1

n



sin






η
n


+


b

2

n



cos






(


γ
n

+

η
n


)



]



}


+














β
¨



m
sn



z

6

n




a

1

n




sin


(

α
+

γ
n

+

η
n


)



+














β
¨



m
sn




z
.


6

n




a

1

n




sin


(

α
+

γ
n

+

η
n


)



+















β
.



(


α
.

+


η
.

n


)




m
sn



z

6

n




a

1

n



cos






(

α
+

γ
n

+

η
n


)


-















z
¨

0



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


cos





β

-















z
.

0



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


cos





β

-














(


α
.

+


η
.

n


)




z
.

0



z

6

n




m
sn


cos






(

α
+

γ
n

+

η
n


)


cos





β

-













β
.




z
.

0



z

6

n




m
sn


sin






(

α
+

γ
n

+

η
n


)


cos





β








(
77
)

























L





θ
.

n



=








θ
.

n



m
aw2In


+


α
.

[


m
aw2In

-


m
aw1n



{



c

2

n



sin






θ
n


-




















b

2

n



cos






(


γ
n

+

θ
n


)


}

]

-


β
.



m
aw1n



a

1

n




cos


(

α
+

γ
n

+

θ
n


)



+













z
.

0



m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


cos





β








(
78
)



























t




(



L





θ
.

n



)


=








θ
¨

n



m
aw2In


+


α
¨

[


m
aw2In

-


m
aw1n



{



c

2

n



sin






θ
n


-




















b

2

n



cos






(


γ
n

+

θ
n


)


}

]

-


α
.




θ
.

n



m
aw1n



{



c

2

n



cos






θ
n


+
















b

2

n



sin






(


γ
n

+

θ
n


)


}

-


β
¨



m
aw1n



a

1

n



cos






(

α
+

γ
n

+

θ
n


)


+














β
.



(


α
.

+


θ
.

n


)




m
aw1n



a

1

n



sin






(

α
+

γ
n

+

θ
n


)


+















z
¨

0



m

aw





ln



cos






(

α
+

γ
n

+

θ
n


)


cos





β

-














(


α
.

+


θ
.

n


)




z
.

0



m
aw1n


sin






(

α
+

γ
n

+

θ
n


)


cos





β

-













β
.




z
.

0



m
aw1n


cos






(

α
+

γ
n

+

θ
n


)


sin





β








(
79
)

























L





z
.


6

n




=







m
sn




z
.


6

n



+


α
.



m
sn



{



c

1

n



sin






η
n


+


b

2

n



cos






(


γ
n

+

η
n


)



}


-














β
.



m
sn



a

1

n



cos






(

α
+

γ
n

+

η
n


)


+














z
.

0



m
sn


cos






(

α
+

γ
n

+

η
n


)


cos





β








(
80
)



























t




(



L





z
.


6

n




)


=







m
sn




z
¨


6

n



+


α
¨



m
sn



{



c

1

n



sin






η
n


+


b

2

n



cos






(


γ
n

+

η
n


)



}


+














α
.




η
.

n



m
sn







{


cos






η
n


-


b

2

n



sin






(


γ
n

+

η
n


)



}


-














β
¨



m
sn



a

1

n



cos






(

α
+

γ
n

+

η
n


)


+















β
.



(


α
.

+


η
.

n


)




m
sn



a

1

n



sin






(

α
+

γ
n

+

η
n


)


+















z
¨

0



m
sn


cos






(

α
+

γ
n

+

η
n


)


cos





β

-














(


α
.

+


η
.

n


)




z
.

0



m
sn


sin






(

α
+

γ
n

+

η
n


)


cos





β

-













β
.




z
.

0



m
sn


cos






(

α
+

γ
n

+

η
n


)


sin





β








(
81
)






















L





z
.


12

n




=
0




(
82
)
























t




(



L





z
.


12

n




)


=
0




(
83
)













The dissipative function is:










F
tot

=


-

1
2




(



c
sn




z
.


6

n

2


+


c
wn




z
.


12

n

2



)






(
84
)













The constraints are based on geometrical constraints, and the touch point of the road and the wheel. The geometrical constraint is expressed as














e

2

n



cos






θ

n







=


-

(


z

6

n


-

d

1

n



)



sin






η
n











e

2

n







sin






θ
n


-


(


z

6

n


-

d

1

n



)






cos






η
n



=


c

1

n


-

c

2

n










(
85
)













The touch point of the road and the wheel is defined as













z
tn

=





z

p
touchpointn
r








=






z
0

+

{



z

12

n







cos





α

+


e

3

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n








cos


(

α
+

γ
n


)



+


















b

2

n







sin





α

}







cos





β

-


a

1

n







sin





β







=






R
n



(
t
)









(
86
)













where R


n


(t) is road input at each wheel.




Differentials are:

















θ
.

n



e

2

n







sin






θ
n


-



z
.


6

n







sin






η
n


-




η
.

n



(


z

6

n


-

d

1

n



)







cos






η
n



=
0










θ
.

n



e

2

n







cos






θ
n


-



z
.


6

n







cos






η
n


+




η
.

n



(


z

6

n


-

d

1

n



)







sin






η
n



=
0













z
.

0

+


{




z
.


12

n







cos





α

-


α
.



z

12

n







sin





α

+


(


α
.

+


θ
.

n


)







e

3

n







cos






(

α
+

γ
n

+

θ
n


)


-


α
.



c

2

n



sin






(

α
+

γ
n


)


+


α
.



b

2

n







cos





α


}






cos





β

-


β
.



[



{



z

12

n







cos





α

+


e

3

n







sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n







cos






(

α
+

γ
n


)


+


b

2

n







sin





α


}






sin





β

+


a

1

n







cos





β


]


-



R
.

n



(
t
)



=
0





(
87
)













Since the differentials of these constraints are written as













j




a
lnj






q
.

j




+


a
lnt




t



=

0






(


l
=
1

,
2
,


3





n

=
i

,
ii
,
iii
,
iv

)






(
88
)













then the values a


lnj


are obtained as follows.













a

1

n0


=
0







a

2

n0


=
0







a

3

n0


=
1







(
89
)









a

1

n1


=
0

,






a

1

n2


=
0

,






a

1

n3


=


-

(


z

6

n


-

d

1

n



)







cos






η
n



,






a

1

n4


=


e

2

n







sin






θ
n



,






a

1

n5


=


-
sin







η
n



,






a

1

n6


=
0







a

2

n1


=
0

,






a

2

n2


=
0

,






a

2

n3


=


(


z

6

n


-

d

1

n



)






sin






η
n



,






a

2

n4


=


e

2

n







cos






θ
n



,






a

2

n5


=


-
cos







η
n



,






a

2

n6


=
0








a

3

n1


=



-

{



z

12

n







cos





α

+


e

3

n







sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n







cos






(

α
+

γ
n


)


+


b

2

n







sin





α


}







sin





β

+


a

1

n







cos





β



,






a

3

n2


=


{



-

z

12

n








sin





α

+


e

3

n







cos






(

α
+

γ
n

+

θ
n


)


-


c

2

n







sin






(

α
+

γ
n


)


+


b

2

n







cos





α


}






cos





β


,






a

3

n3


=
0

,






a

3

n4


=


e

3

n







cos






(

α
+

γ
n

+

θ
n


)






cos





β


,






a

3

n5


=
0

,






a

3

n6


=

cos





α





cos





β

















From the above, Lagrange's equation becomes















t




(



L





q
.

j



)


-



L




q
j




=


Q
j

+





l
,
n














λ
ln



a

1

nj









(
90
)













where













q
0

=

z
0









q
1

=
β

,


q
2

=
α

,











q

3

i


=

η
i


,






q

4

i


=

θ
i


,






q

5

i


=

z

6

i



,





q

6

i


=

z

12

i










q

3

ii


=

η
ii


,






q

4

ii


=

θ
ii


,






q

5

ii


=

z

6

ii



,





q

6

ii


=

z

12

ii










q

3

iii


=

η
iii


,






q

4

iii


=

θ
iii


,






q

5

iii


=

z

6

iii



,





q

6

iii


=

z

12

iii










q

3

iv


=

η
iv


,






q

4

iv


=

θ
iv


,






q

5

iv


=

z

6

iv



,





q

6

iv


=

z

12

iv












(
91
)























t




(



L





z
.

0



)


-



L




z
0




=





F





z
.

0



+




l
,
n





λ

l





n




α
ln0






l



=
1


,
2
,


3





n

=
i

,
ii
,
iii
,
iv

















z
¨

0



(


m
b

+

m
sawn


)


+


a
¨



m
b


cos






β


(



b
0


cos





α

-


c
0


sin





α


)



-


β
.



a
.



m
b


sin






β


(



b
0


cos





α

-


c
0


sin





α


)



-



a
.

2



m
b


cos






β


(



b
0


sin





α

-


c
0


cos





α


)



-


β
¨



{



m
ba


cos





β

+



m
b



(



b
0


sin





α

-


c
0


cos





α


)



sin





β


}


+


β
.



{



β
.



m
ba


sin





β

+


a
.




m
b



(



b
0


cos





α

-


c
0


sin





α


)



sin





β

+


β
.




m
b



(



b
0


sin





α

-


c
0


cos





α


)



cos





β


}


+


{



z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



-


(


α
.

+


η
.

n


)




z
.


6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



+


(


α
¨

+


θ
¨

n


)



m
awln



cos


(

α
+

γ
n

+

θ
n


)



-



(


α
.

+


θ
.

n


)

2



m
awln



sin


(

α
+

γ
n

+

θ
n


)



+


(


α
¨

+


θ
¨

n


)



z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-



(


α
.

+


η
.

n


)

2



z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



+



(


α
.

+


θ
.

n


)

2



z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



-


α
¨



m
sawcn



sin


(

α
+

γ
n


)



-



α
.

2



m
sawcn



cos


(

α
+

γ
n


)



+


α
¨



m
sawbn


cos





α

-



α
.

2



m
sawbn


sin





α

-


β
¨



m
sawan



}


cos





β

-



β
.

(




z
.


6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



+


(


α
.

+


θ
.

n


)



m
awln



cos


(

α
+

γ
n

+

θ
n


)



-


(


α
.

+


η
.

n


)



z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-


α
.







m
sawcn



sin


(

α
+

γ
n


)







+


α
.



m
sawbn


cos





α

-


β
.



m
sawan



}


sin





β

-



β
¨

(



m
awln



sin


(

α
+

γ
n

+

θ
n


)



-


z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



+






m
sawcn



cos


(

α
+

γ
n


)



+


m
sawbn


sin





α


}


sin





β

-


β
.



{



(


α
.

+


θ
.

n


)



m
awln



cos


(

α
+

γ
n

+

θ
n


)



-



z
.


6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



-






(


α
.

+


η
.

n


)



z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-


α
.



m
sawcn


sin






(

α
+

γ
n


)


+


α
.



m
sawbn






cos





α


}


sin





β

-



β
.

2



{



m
awln


sin






(

α
+

γ
n

+

θ
n


)


+


z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



+






m
sawcn



cos


(

α
+

γ
n


)



+


m
sawbn


sin





α


}


cos





β

+

g


(


m
b

+

m
sawn


)



=

λ

3

n



















z
¨

0



(


m
b

+

m
sawn


)


+


a
¨



m
b


cos






β


(



b
0


cos





α

-


c
0


sin





α


)



-



a
.

2



m
b


cos






β


(



b
0


sin





α

-


c
0


cos





α


)



-


β
¨



{



m
ba


cos





β

+



m
b



(



b
0


sin





α

-


c
0


cos





α


)



sin





β


}


+


β
.



{




β
.



(


m
ba

+

m
sawan


)



sin





β

+


β
.




m
b



(



b
0


sin





α

-


c
0


cos





α


)



cos





β


}






+


{




z
¨


6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



-

2


(


α
.

+


η
.

n


)




z
.


6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



+


(


α
¨

+


θ
¨

n


)



m
awln



cos


(

α
+

γ
n

+

θ
n


)



-



(


α
.

+


θ
.

n


)

2



m
awln



sin


(

α
+

γ
n

+

θ
n


)



-


(


α
¨

+


θ
¨

n


)



z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-



(


α
.

+


θ
.

n


)

2



z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



-


α
¨



m
sawcn



sin


(

α
+

γ
n


)



-



α
.

2



m
sawcn



cos


(

α
+

γ
n


)



+


α
¨



m
sawbn


cos





α

-



α
.

2



m
sawbn


sin





α

-


β
¨



m
sawan



}


cos





β

-

2



β
.

(




z
.


6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



+


(


α
.

+


θ
.

n


)



m
awln



cos


(

α
+

γ
n

+

θ
n


)



-


(


α
.

+


η
.

n


)



z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-


α
.







m
sawcn



sin


(

α
+

γ
n


)







+


α
.



m
sawbn


cos





α


}


sin





β

-


(



β
¨


sin





β

+


β
2


cos





β


)



{



m
awln


sin






(

α
+

γ
n

+

θ
n


)


+


z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



+






m
sawcn



cos


(

α
+

γ
n


)



+


m
sawbn


sin





α


}


+

g


(


m
b

+

m
sawn


)



=

λ

3

n

















z
¨

0

=


λ

3

n


-
g
-

&AutoLeftMatch;






















a
¨



m
b



C
β



A
2


-



a
.

2



m
b



A
1


-


β
¨



{



m
ba



C
β


+


m
b



A
1



S
β



}


+








β
.



{



m
ba



S
β


+


β
.



m
b



A
1



C
β



}


+

{




z
¨


6

n




m
sn



C

α





γ





η



-

2


(


α
.

+


η
.

n


)
















z
.


6

n




m
sn



S

α





γ





η



+


(


α
¨

+


θ
¨

n


)



m
awln



C

α





γ





η



-



(


α
.

+


θ
.

n


)

2



m
awln



S

α





γ





η



-











(


α
¨

+


η
¨

n


)



z

6

n




m
sn



S

α





γ





η



-



(


α
.

+


η
.

n


)

2



z

6

n




m
sn



C

α





γ





η



-











a
¨



m
sawcn



S

α





γ





η



-



a
.

2



m
sawcn



C
αγη


+


α
¨



m
sawcn



C
α


-












α
.

2



m
sawbn



S
α


-


β
¨







m
sawan



}



C
β


-

2






β
.



{




z
.


6

n




m
sn



C
αγη


+














(


α
.

-


θ
.

n


)



m
awln



C

α





γη



-


(


a
.

+


η
.

n


)



z

6

n




m
sn



S
αγη


-











α
.



m
sawcn



S
αγη


+


α
.



m
sawbn



C
α


-


β
.




m
sawan

/
2



}


S





β

-







(



β
¨







S
β


+



β
.

2



C
β



)



{



m
awln



S

α





γ





η



+


z

6

n




m
sn



C


α





γ





η








+














m
sawcn



C
αγη


+


m
sawbn



S
a



}





&AutoLeftMatch;

m
bsawn

&AutoRightMatch;


&AutoRightMatch;

























t








(



L




β
.



)


-



L



β



=




F




β
.



+




l
,
n





λ

l





n




a

l





n1

















l
=
1

,
2
,


3





n

=
i

,
ii
,
iii
,
iv





(
92
)





















β
¨






m
saw2n

+

m
baI

+



m
b



(



b
0


sin





α

+


c
0


cos





α


)


2

+


m
sn




{



z

6

n



cos






(

α
+

γ
n

+

η
n


)


+


c

1

n



cos






(

α
+

γ
n


)


+


b

2

n



sin





α


}

2


+


m
an




{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}

2


+


m
wn




{



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}

2






+

2


β
.







α
.




m
b



(



b
0


sin





α

+


c
0


cos





α


)




(



b
0


cos





α

-


c
0


sin





α


)


+


m
sn



{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}



{




z
.


6

n




cos


(

α
+

γ
n

+

η
n


)



-


(


α
.

+


η
.

n


)



z

6

n




sin


(

α
+

γ
n

+

η
n


)



-


α
.



[



c

1

n




sin


(

α
+

γ
n


)



-


b

2

n



cos





α


]



}


+


m
an



{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}



{



(


α
.

+


θ
.

n


)



e

1

n




cos


(

α
+

γ
n

+

θ
n


)



-


α
.



[



c

2

n




sin


(

α
+

γ
n


)



-


b

2

n



cos





α


]



}


+


m
wn



{



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}



{



(


α
.

+


θ
.

n


)



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



-


α
.



[



c

2

n




sin


(

α
+

γ
n


)



-


b

2

n



cos





α


]



}






-


α
¨




m
ba



(



b
0


cos





α

-


c
0


sin





α


)



+



α
.

2




m
ba



(



b
0


sin





α

+


c
0


cos





α


)



-



z
¨


6

n








m
sn







a

1

n








cos


(

α
+

γ
n

+

η
n


)



+




z
.


6

n




(


α
.

+

η
.


)








m
sn







a

1

n








sin


(

α
+

γ
n

+

η
n


)



+



η
¨

n







m
sn







z

6

n








a

1

n








sin


(

α
+

γ
n

+

η
n


)



+



η
.

n



m
sn








z
.


6

n








a

1

n








sin


(

α
+

γ
n

+

η
n


)



+




η
.

n



(


α
.

+


η
.

n


)








m
sn







z

6

n








a

1

n








cos


(

α
+

γ
n

+

η
n


)



-



θ
¨

n



m
aw1n



a

1

n




cos


(

α
+

γ
n

+

θ
n


)



+




θ
.

n



(


α
.

+


θ
.

n


)




m
aw1n



a

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


α
¨



a

1

n








{



m
sawcn



sin


(

α
+

γ
n


)



-


m
sawbn


cos





α

+


m
sn



z

6

n




sin


(

α
+

γ
n

+

η
n


)



-


m
aw1n



cos


(

α
+

γ
n

+

θ
n


)




}


+


α
.



a

1

n








{



α
.



m
sawcn



cos


(

α
+

γ
n


)



+


α
.



m
sawbn


sin





α

+


(


α
.

+


η
.

n


)



m
sn







z

6

n








cos


(

α
+

γ
n

+

η
n


)



+


m
sn




z
.


6

n




sin


(

α
+

γ
n

+

η
n


)



+


(


α
.

+


θ
.

n


)



m
aw1n



sin


(

α
+

γ
n

+

θ
n


)




}






(
93
)





















-



z
¨

0



[



{



m
b



(



b
0


sin





α

+


c
0


cos





α


)


+


m

aw





ln




sin


(

α
+

γ
n

+

θ
n


)



+


z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



+


m
sawcn



cos


(

α
+
γ

)



+


m
sawbn


sin





α


}






sin





β

+

(


m
ba

+


m
sawan


cos





β


)


]



-



z
.

0



[



{



α
.




m
b



(



b
0


cos





α

-


c
0


sin





α


)



+


(


α
.

+


θ
.

n


)



m

aw





ln




cos


(

α
+

γ
n

+

θ
n


)



+



z
.


6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



-


(


α
.

+


η
.

n


)



z

6

n




m
sn







sin


(

α
+

γ
n

+

η
n


)



-


α
.



m
sawcn



sin


(

α
+

γ
n


)



+


α
.



m
sawbn


cos





α


}


sin





β

+


β
.




z
.

0



{



m
b



(



b
0


sin





α

+


c
0


cos





α


)


+


m

aw





ln




sin


(

α
+

γ
n

+

θ
n


)



+


z

6

n








m
sn







cos
(





α
+

γ
n

+

η
n


)


+






m
sawcn



cos


(

α
+

γ
n


)



+


m
sawbn


sin





α


}






cos





β

-





(


m
ba

+


m
sawan


sin





β


)


]


+


α
.




z
.

0



m
b


sin






β


(



b
0


cos





α

-


c
0


sin





α


)



-


β
.








z
.

0







{



m
ba


sin





β

-



m
b



(



b
0


sin





α

+


c
0


cos





α


)



cos





β


}


-

g






{



m
ba


cos





β

+



m
b



(



b
0


sin





α

+


c
0


cos





α


)



sin





β


}


-




g


[



{



m
sn



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


m
aw1n



sin


(

α
+

γ
n

+

θ
n


)



+


m
sawcn



cos


(

α
+

γ
n


)



+


m
sawbn


sin





α


}


sin





β

+


m
sawan


cos





β


]


-



z
.

0



{




z
.


6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



+


(


α
.

+


θ
.

n


)



m
aw1n



cos


(

α
+

γ
n

+

θ
n


)



-


(


α
.

+


η
.

n


)



z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-


α
.



m
sawcn



sin


(

α
+

γ
n


)



+


α
.



m
sawbn


cos





α

-


β
.



m
sawan



}


sin





β




}

-


β
.




z
.

0



{



m

aw





ln




sin


(

α
+

γ
n

+

θ
n


)



+


z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



+


m
sawcn



cos


(

α
+

γ
n


)



+


m
sawbn


sin





α


}


cos





β




=


λ

3

n




[



-

{



z

12

n



cos





α

+


e

3

n



sin






(

α
+

γ
n

+

θ
n


)


+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}







β

+


a

1

n







cos





β


]





















β
¨



(


m
saw2n

+

m
baI

+


m
b



A
1
2


+


m
sn



B
1
2


+


m
an



B
2
2


+


m
wn



B
3
2



)


+

2



β
.



[



α
.



m
b



A
1



A
2


+


m
sn



B
1



{




z
.


6

n




C

α





γ





η





m



-


(


α
.

+

η
.


)



z

6

n




S

αγη





m



-


α
.



A
4



}


+


m
an



B
2



{



(


α
.

+


θ
.

n


)



e

1

n




C

αγθ





n



-


α
.



A
6



}


+


m
wn



B
3



{



(


α
.

+


θ
.

n


)



e

3

n




S

αγθ





n



-


α
.



A
6



}



]



-


α
¨



m
ba



A
2


+



α
.

2



m
ba



A
1


-



z
¨


6

n




m
sn



a

1

n




C

αγη





m



+

2




z
.


6

n




(


α
.

+


η
.

n


)




m
sn



a

1

n




S

αγη





n



+



η
¨

n



m
sn



z

6

n




S

αγη





m



+




η
.

n



(


2


α
.


+


η
.

n


)




m
sn



z

6

n




a

1

n




C

αγη





m



-



θ
¨

n



m
aw1n



a

1

n




C

αγθ





n



+




θ
.

n



(


2


α
.


+


θ
.

n


)




m
aw1n



a

1

n




S

αγθ





n



+


α
¨



a

1

n




{



m
sawcn



S

αγ





n



-


m
sawbn



C
α


+


m
sn



z

6

n




S

αγη





n



-


m
aw1n



C

αγθ





n




}


+



α
.

2



a

1

n




{



m
sawcn



C

αγ





n



+


m
sawbn



S
α


+


m
sn



z

6

n




C

αγη





n



+


m
aw1n



S

αγθ





n




}


-



z
¨

0



[



{



m
b



(



b
0



S
α


+


c
0



C
α



)


+


m

aw





ln




S

αγη





n



+


z

6

n




m
sn



C

αγη





n



+


m
sawcn



C

αγ





n



+


m
sawbn



S
α



}



S
β


+


(


m
ba

+

b
sawan


)



C
β



]


+




z
.

0



(

1
-

β
.


)




(


m
ba

+

m
sawan


)


sin





β

-

g


[



m
ba



C
β


+


m
b



A
1



S
β


+


{



m
sn



z

6

n




C

αγη





n



+


m
aw1n



S

αγθ





n



+


m
sawcn



C

αγ





n



+


m
sawbn



S
α



}



S
β


+


m
sawan



C
β



]



=


λ

3

n




[



-

{



z

12

n




C
α


+


e

3

n




S

αγθ





n



+


c

2

n




C

αγ





n



+


b

2

n




S
α



}




S
β


+


a

1

n




C
β



]






(
94
)























2


β
.

[



α
.



m
b



A
1



A
2


+


m
sn



B
1

[




z
.


6

n




C

αγη





n



-


(


α
.

+


η
.

n


)



z

6

n




S

αγη







-


α
.



A
4






}

+


m
an



B
2



{



(


α
.

+


θ
.

n


)



e

1

n




C

αγθ





n



-


α
.



A
6



}


+


m
wn



B
3



{



(


α
.

+


θ
.

n


)



e

3

n




S

αγθ





n



-


α
.



A
6



}



]

-


α
¨



m
ba



A
2


+



α
.

2



m
ba



A
1


-



z
¨


6

n




m
sn



a

1

n




C

αγη





n



+

2




z
.


6

n




(


α
.

+


η
.

n


)




m
sn



a

1

n




S

αγ





ηn



+



η
¨

n



m
sn



z

6

n




a

1

n




S

αγη





n



+




η
.

n



(


2


α
.


+


η
.

n


)




m
sn



z

6

n




a

1

n




C

αγη





n



-



θ
¨

n



m
aw1n



a

1

n




C

αγθ





n



+




θ
.

n



(


2


α
.


+


θ
.

n


)




m
aw1n



a

1

n




S

αγθ





n



+


α
¨



a

1

n




{



m
sawcn



S

αγ





n



-


m
sawbn



C
α


+


m
sn



z

6

n




S

αγη





n



-


m
aw1n



C

αγθ





n




}


+



α
.

2



a

1

n




{



m
sawcn



C

αγ





n



+


m
sawbn



S
α


+


m
sn



z

6

n




C

αγη





n



+


m
aw1n



S

αγθ





n




}


-



z
¨

0



[



{



m
b



(



b
0



S
α


+


c
0



C
α



)


+


m
aw1n



S

αγη





n



+


z

6

n




m
sn



C

αγη





n



+


m
sawcn



C

αγ





n



+


m
sawbn



S
α



}



S
β


+


(


m
ba

+

m
sawan


)



C
β



]


+




z
.

0



(

1
-

β
.


)




(


m
ba

+

m
sawan


)


sin





β

-

g


[



m
ba



C
β


+


m
b



A
1



S
β


+


{



m
sn



z

6

n




C

αγη





n



+


m
aw1n



S

αγθ





n



+


m
sawcn



C

αγ





n



+


m
sawbn



S
α



}



S
β


+


m
sawan



C
b



]






(
95
)







β
¨

=



+

λ

3

n





{



(



z

12

n




C
α


+


e

3

n




S

αγθ





n



+


c

2

n




C

αγ





n



+


b

2

n




S
α



)



S
β


-


a

1

n




C
β



}



-

(


m
saw2n

+

m
baI

+


m
b



A
1
2


+


m
sn



B
1
2


+


m
an



B
2
2


+


m
wn



B
3
2



)


































t




(



L




α
.



)


-



L



α



=




F




α
.



+




l
,
n





λ
ln



a

ln





2













l
=
1

,
2
,


3





n

=
i

,
ii
,
iii
,
iv





(
96
)





















β
¨




m
ba



(



b
0


cos





α

-


c
0


sin





α


)



+


β
.



α
.




m
ba



(



b
0


sin





α

+


c
0


cos





α


)



+



z
¨

0



m
b


cos






β


(



b
0


cos





α

-


c
0


sin





α


)



-


β
.




z
.

0



m
b


sin






β


(



b
0


cos





α

-


c
0


sin





α


)



-


α
.




z
.

0



m
b


cos






β


(



b
0


sin





α

-


c
0


cos





α


)



+


α
¨






m
bbI

+

m
sawIn

+


m
sn




z

6

n




[


z

6

n


+

2


{



c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}



]



-

2


m
aw1n



{



c

2

n



sin






θ
n


-


b

2

n




cos


(


γ
n

+

θ
n


)




}






+


α
.







m
sn





z
.


6

n




[


z

6

n


+

2


{



c
1


cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}



]



+


m
sn




z

6

n




[



z
.


6

n


-

2



η
.

n



{




c
.


1

n



sin






η
n


+


b

2

n




cos


(


γ
n

+

η
n


)




}



]



-

2



θ
.

n



m
aw1n



{



c

2

n



cos






θ
n


+


b

2

n



sin






(


γ
n

+

θ
n


)



}






+



z
¨


6

n




m
sn



{



c

1

n



sin






η
n


+


b

2

n



cos






(


γ
n

+

η
n


)



}


+



z
.


6

n





η
.

n



m
sn



{



c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


+



η
¨

n



m
sn



z

6

n




{


z

6

n


+


c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


+



η
.

n



m
sn




z
.


6

n




{


z

6

n


+


c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


+



η
.

n



m
sn



z

6

n




{



z
.


6

n


-



η
.

n



[



c

1

n



sin






η
n


+


b

2

n




cos


(


γ
n

+

η
n


)




]



}


+


θ
¨



[


m
aw2In

-


m
aw1n



{



c

2

n



sin






θ
n


-


b

2

n




cos


(


γ
n

+
θ

)




}



]


-




θ
.

n
2



m
aw1n



{



c

2

n



cos






θ
n


+


b

2

n




sin


(


γ
n

+

θ
n


)




}


]

+


β
¨



a

1

n




{



m
sawcn



sin


(

α
+

γ
n


)



-


m
sawbn


cos





α

+


m
sn



z

6

n




sin


(

α
+

γ
n

+

η
n


)



-


m
aw1n



cos


(

α
+

γ
n

+

θ
n


)




}


+


β
.




α
.


1

n




{



α
.



[



m
sawcn



cos


(

α
+

γ
n


)



+


m
sawbn


sin





α


]


+


m
sn




z
.


6

n




sin


(

α
+

γ
n

+

η
n


)



+


(


α
.

+


η
.

n


)



m
sn



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


(


α
.

+


θ
.

n


)



m
aw1n



sin


(

α
+

γ
n

+

θ
n


)




}


+




z
¨

0



{



m

aw





ln




cos


(

α
+

γ
n

+

θ
n


)



-


z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-


m
sawcn



sin


(

α
+

γ
n


)



+


m
sawbn


cos





α


}


cos





β

}

+



z
.

0



{



-

(


α
.

+


θ
.

n


)




m

aw





ln




sin


(

α
+

γ
n

+

θ
n


)



-


z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-


(


α
.

+


θ
.

n


)



z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



-


α
.



m
sawcn



cos


(

α
+

γ
n


)



-


α
.



m
sawbn


sin





α


}


cos





β

-



β
.




z
.

0



{



m

aw





ln




cos


(

α
+

γ
n

+

θ
n


)



-


z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-


m
sawcn



sin


(

α
+

η
n


)



+


m
sawbn


cos





α


}


cos





β

}

-


{




β
.

2




m
b



(



b
0


cos





α

-


c
0


sin





α


)



+


α
.



β
.



m
ba



}







(



b
0


sin





α

+






c
0


cos





α


)


-

&LeftBracketingBar;





β
.

2











m
sn







{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}







{



-

z

6

n









sin
(





α
+

γ
n

+

η
n


)


-


c

1

n




sin


(

α
+

γ
n


)



+


b

2

n



cos





α


}


+


m
an



{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}



{



e
1



cos


(

α
+

γ
n

+

θ
n


)



-


c

2

n




sin


(

α
+

γ
n


)



+


b

2

n



cos





α


}


+


m
wn



{



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}



{



e
3



cos


(

α
+

γ
n

+

θ
n


)



-


c

2

n




sin


(

α
+

γ
n


)



+


b

2

n



cos





α


}






+



z
.


6

n




β
.



m
sn



a

1

n




sin


(

α
+

γ
n

+

η
n


)



+



η
.

n












β

.







m
sn







z

6

n








a

1

n








cos




(

α
+

γ
n

+

η
n


)


+


θ
.







β
.







m
aw1n







a

1

n








sin


(

α
+

γ
n

+

θ
n


)



+


α
.







β
.







a

1

n








{



m
sawcn



cos


(

α
+

γ
n


)



+


m
sawbn


sin





α

+


m
sn



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


m
aw1n



sin


(

α
+

γ
n

+

θ
n


)




}


-



z
.

0



{




z
.


6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



+


(


α
.

+


θ
.

n


)



m

aw





ln




sin


(

α
+

γ
n

+

θ
n


)



+


(


α
.

+


η
.

n


)



z

6

n




m
sn



cos


(

α
+

γ
n

+

θ
n


)



+


α
.



m
sawcn


cos






(

α
+

γ
n


)


+


α
.



m
sawbn


sin





α


}






cos





β






β
.









z
.

0





[






{



m

aw





ln




cos


(

α
+

γ
n

+

θ
n


)



-


z

6

n




m
sn







sin


(

α
+

γ
n

+

η
n


)



-


m
sawn



sin


(

α
+

γ
n


)



+


m
sawbn


cos





α


}


sin





β

&RightBracketingBar;


+



gm
b



(



b
0


cos





α

-


c
0


sin





α


)



cos





β

-

g






{



m
sn







z

6

n








sin


(

α
+

γ
n

+

η
n


)



-


m
aw1n







cos


(

α
+

γ
n

+

θ
n


)



+


m
sawcn







sin


(

α
+

γ
n


)



-


m
sawbn






cos





α


}






cos





β






=






λ

3

n








{



-

z

12

n




sin





α

+


e

3

n








cos
(





α
+

γ
n

+

θ
n


)


-


c

2

n








sin
(





α
+

γ
n


)


+






b


2

n












cos





α


}


cos





β







(
97
)























z
¨

0



{



m
b



A
2


+


m

aw





ln




C

αγθ





n



-


z

6

n




m
sn



S

αγη





n



-


m
sawcn



S

αγ





n



+


m
sawbn



C
α



}



C
β


-


β
¨



m
ba



A
2


+


α
¨



{


m
bbI

+

m
sawIn

+


m
sn




z

6

n




(


z

6

n


+

2


E

1

n




)



-

2


m
aw1n



H

1

n




}


+

2


α
.



{



m
sn





z
.


6

n




(


z

6

n


+

E

1

n



)



-


m
sn



z

6

n





η
.

n



E

2

n



-



θ
.

n



m
aw1n



H

2

n




}


+



z
¨


6

n




m
sn



E

2

n



+



z
.


6

n





η
.

n



m
sn



E

1

n



+



η
¨

n



m
sn



z

6

n




{


z

6

n


+

E

1

n



}


+



η
.

n



m
sn




z
.


6

n




{


2


z

6

n



+

E

1

n



}


-



η
.

n
2



m
sn



z

6

n




E

2

n



+


θ
¨



(


m
aw2In

-


m
aw1n



H

1

n




)


-



θ
.

n
2



m
aw1n



H

2

n



+


B
¨




a

1

n




(



m
sawcn



S

αγ





n



-


m
sawbn



C
α


+


m
sn



z

6

n




S

αγη





n



-


m
aw1n



C

αγθ





n




)



+


β
.



a

1

n




{



α
.



(



m
sawcn



C

αγ





n



+


m
sawbn



S
α



)


+


m
sn




z
.


6

n




S

αγη





n



+


(


α
.

+


η
.

n


)



m
sn



z

6

n




C

αγη





n



+


(


α
.

+


θ
.

n


)



m
aw1n



S

αγθ





n




}


-



β
.

2



m
b



A
2



A
1


-

[



β
.



{



m
sn




B
1



(



-

z

6

n





S

αγη





n



-

A
4


)



+


m
an




B
2



(



e
1



C

αγθ





n



-

A
6


)



+


m
wn




B
3



(



e
3



C

αγθ





n



-

A
6


)




}


+



z
.


6

n




β
.



m
sn



a

1

n




S

αγη





n



+



η
.

n



β
.



m
sn



z

6

n




a

1

n




C

αγη





n



+


θ
.



β
.



m
aw1n



a

1

n




S

αγθ





n



+


α
.



β
.



a

1

n




{



m
sawcn



C

αγ





n



+


m
sawbn



S
α


+


m
sn



z

6

n




C

αγη





n



+


m
aw1n



S

αγθ





n




}



]

+


gm
b



A
2



C
β


-

g


{



m
sn



z

6

n




S

αγη





n



-


m
aw1n



C

αγθ





n



+


m
sawcn



S

αγ





n



-


m
sawbn



C
α



}



C
β



=


λ

3

n




{



-

z

12

n





S
α


+


e

3

n




C

αγθ





n



-


c

2

n




S

αγ





n



+


b

2

n




C
α



}



C
β






(
98
)























z
¨

0



{



m
b



A
2


+


m

aw





ln




C

αγθ





n



-


z

6

n




m
sn



S

αγη





n



-


m
sawcn



S

αγ





n



+


m
sawbn



C
α



}



C
β


-


β
¨



m
ba



A
2


+


α
¨



{


m
bbI

+

m
sawIn

+


m
sn




z

6

n




(


z

6

n


+

2


E

1

n




)



-

2


m
aw1n



H

1

n




}


+



m
sn



(


2


α
.




z
.


6

n



+



η
¨

n



z

6

n



+

2



η
.

n




z
.


6

n




)




(


z

6

n


+

E

1

n



)


-

2



α
.



(



m
sn



z

6

n





η
.

n



E

2

n



+



θ
.

n



m
aw1n



H

2

n




)



+



z
¨


6

n




m
sn



E

2

n



-



η
.

n
2



m
sn



z

6

n




E

2

n



+


θ
¨



(


m
aw2In

-


m
aw1n



H

1

n




)


-



θ
.

n
2



m
aw1n



H

2

n



+


β
¨




a

1

n




(



m
sawcn



S

αγ





n



-


m
sawbn



C
α


+


m
sn



z

6

n




S

αγη





n



-


m
aw1n



C

αγθ





n




)



-



β
.

2



{



m
b



A
2



A
1


+


m
sn




B
1



(



-

z

6

n





S

αγη





n



-

A
4


)



+


m
an




B
2



(



e
1



C

αγθ





n



-

A
6


)



+


m
wn




B
3



(



e
3



C

αγθ





n



-

A
6


)




}


+


gm
b



A
2



C
β


-

g


{



m
sn



z

6

n




S

αγη





n



-


m
aw1n



C

αγθ





n



+


m
sawcn



S

αγ





n



-


m
sawbn



C
α



}



C
β



=




λ

3

n




(



-

z

12

n





S
α


+


e

3

n




C

αγθ





n



-


c

2

n




S

αγ





n



+


b

2

n




C
α



)




C
β












(
99
)






















z
¨

0



{



m
b



A
2


+


m

aw





ln




C

αγ





θ





n



-


z

6

n




m
sn



S

αγη





n



-


m
sawcn



S

αγ





n



+


m
sawbn



C
α



}



C
β




m
sn



(


2


α
.




z
.


6

n



+



η
¨

n



z

6

n



+

2



η
.

n




z
.


6

n




)




(


z

6

n


+

E

1

n



)


-

2



α
.



(



m
sn



z

6

n





η
.

n



E

2

n



+



θ
.

n



m
aw1n



H

2

n




)



+



z
¨


6

n




m
sn



E

2

n



-



η
.

n
2



m
sn



z

6

n




E

2

n



+


θ
¨



(


m
aw2In

-


m
aw1n



H

1

n




)


-



θ
.

n
2



m
aw1n



H

2

n



+


β
¨



a

1

n



+


β
¨




a

1

n




(



m
sawcn



S

αγ





n



-


m
sawbn



C
α


+


m
sn



z

6

n




S

αγη





n



-


m
aw1n



C

αγθ





n




)



-



β
.

2



{



m
b



A
2



A
1


+


m
sn




B
1



(



-

z

6

n





S

αγη





n



-

A
4


)



+


m
an




B
2



(



e
1



C

αγθ





n



-

A
6


)



+


m
wn




B
3



(



e
3



C

αγθ





n



-

A
6


)




}


+


gm
b



A
2



C
β


-

g


{



m
sn



z

6

n




S

αγη





n



-


m
aw1n



C

αγθ





n



+


m
sawcn



S

αγ





n



-


m
sawbn



C
α



}



C
β


-


β
¨



m
ba



A
2






(
100
)







α
¨

=



+


λ

3

n




(



z

12

n




S
α


-


e

3

n




C

αγθ





n



+


c

2

n




S

αγ





n



-


b

2

n




C
α



)





C
β



-

{


m
bbI

+

m
sawbn

+


m
sn




z

6

n




(


z

6

n


+

2


E

1

n




)



-

2


m
aw1n



H

1

n




}


































t




(



L





η
.

n



)


-



L




η
n




=




F





η
.

n



+




l
,
n





λ
ln



a
ln3












l
=
1

,
2
,


3





n

=
i

,
ii
,
iii
,
iv





(
101
)






















m
sn




η
¨

n



z

6

n

2


+

2


m
sn




η
.

n




z
.


6

n




z

6

n



+


α
¨



m
sn



z

6

n




{


z

6

n


+


c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


+


α
.



m
sn




z
.


6

n




{


z

6

n


+


c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


+


α
.



m
sn



z

6

n




{



z
.


6

n


-



η
.

n



[



c

1

n



sin






η
n


+


b

2

n




cos


(


γ
n

+

η
n


)




]



}


+


β
¨



m
sn



z

6

n




a

1

n




sin


(

α
+

γ
n

+

η
n


)



+


β
.



m
sn




z
.


6

n




a

1

n




sin


(

α
+

γ
n

+

η
n


)



+



β
.



(


α
.

+


η
.

n


)




m
sn



z

6

n




a

1

n




cos


(

α
+

γ
n

+

η
n


)



-



z
¨

0



z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



cos





β

-



z
.

0




z
.


6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



cos





β

-


(


α
.

+


η
.

n


)




z
.

0



z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)



cos





β

-


β
.




z
.

0




z
.


6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



sin





β

-






α
.

2



m
sn



z

6

n




{



-

c

1

n




sin






η
n


-


b

2

n




cos


(


γ
n

+

η
n


)




}


+



β
.

2



m
sn



{



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


c

1

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}



{


-

z

6

n




sin






(

α
+

γ
n

+

η
n


)


}


+



z
.


6

n




α
.



m
sn



{



c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

-

η
n


)




}


+



z
.


6

n




β
.



m
sn



a

1

n




sin


(

α
+

γ
n

+

η
n


)



-



η
.

n



α
.



m
sn



z

6

n




{



c

1

n



sin






η
n


+


b

2

n




cos


(


γ
n

+

η
n


)




}


+



η
.

n



β
.



m
sn







z

6

n








a

1

n








cos


(

α
+

γ
n

+

η
n


)



+


α
.



β
.



a

1

n




m
sn



z

6

n




cos


(

α
+

γ
n

+

η
n


)



+


gm
sn



z

6

n




sin


(

α
+

γ
n

+

η
n


)



cos





β

-



z
.

0



{



z

6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



+


(


α
.

+


η
.

n


)




z
.

0



z

6

n




m
sn



cos


(

α
+

γ
n

+

η
n


)




}


cos





β

+


β
.




z
.

0




z
.


6

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



sin





β





=



-


λ

1

n




(


z

6

n


-

d

1

n



)




cos






η
n


+



λ

2

n




(


z

6

n


-

d

1

n



)



sin






η
n







(
102
)






















m
sn




η
¨

n



z

6

n

2


+

2


m
sn




η
.

n




z
.


6

n




z

6

n



+


α
¨



m
sn



z

6

n




{


z

6

n


+

E
1


}


+


α
.



m
sn




z
.


6

n




{


2


z

6

n



+

E
1


}


-


α
.



m
sn



z

6

n





η
.

n



E
2


+


β
¨



m
sn



z

6

n




a

1

n




S

αγη





n



+


β
.



m
sn




z
.


6

n




a

1

n




S

αγη





n



+



β
.



(


α
.

+


η
.

n


)




m
sn



z

6

n




a

1

n




C

αγη





n



-



z
¨

0



z

6

n




m
sn



S

αγη





n




C
β


+



α
.

2



m
sn



z

6

n




E
2


+



β
.

2



m
sn



B
1



z

6

n




S

αγη





n



-



z
.


6

n




α
.



m
sn



E
1


-



z
.


6

n




β
.



m
sn



a

1

n




S

αγη





n



+



η
.

n



α
.



m
sn



z

6

n




E
2


-



η
.

n



β
.



m
sn



z

6

n




a

1

n




C

αγη





n



-


α
.



β
.



a

1

n




m
sn



z

6

n




C

αγη





n



-


gm
sn



z

6

n




S

αγη





n




C
β



=



-


λ

1

n




(


z

6

n


-

d

1

n



)





C

η





n



+



λ

2

n




(


z

6

n


-

d

1

n



)




S

η





n








(
103
)





















m
sn



z

6

n




{




η
¨

n



z

6

n



+

2



η
.

n




z
.


6

n



+


α
¨



(


z

6

n


+

E
1


)


+

2


α
.




z
.


6

n



+


β
¨



a

1

n




S

αγη





n



-



z
¨

0



S

αγη





n




C
β


+



α
.

2



E
2


+



β
.

2



B
1



S

αγη





n



-


gS

αγη





n




C
β



}


=




-


λ

1

n




(


z

6

n


-

d

1

n



)





C

η





n



+



λ

2

n




(


z

6

n


-

d

1

n



)




S

η





n














(
104
)




















λ

1

n


=






m
sn



z

6

n




{




η
¨

n



z

6

n



+

2



η
.

n




z
.


6

n



+


α
¨



(


z

6

n


+

E
1


)


+

2


α
.




z
.


6

n



+










β
¨



a

1

n




S

αγη





n



-



z
¨

0



S

αγη





n




C
β


+



α
.

2



E
2


+



β
.

2



B
1



S

αγη





n



-









gS

αγη





n




C
β


}

-



λ

2

n




(


z

6

n


-

d

1

n



)




S

η





n









-

(


z

6

n


-

d

1

n



)




C

η





n








(
105
)


























t




(







L





θ
.

n



)


-



L




θ
n




=




F





θ
.

n



+




l
,
n





λ
ln



a
ln4












l
=
1

,
2
,


3





n

=
i

,
ii
,
iii
,
iv





(
106
)






















θ
¨

n



m
aw2In


+


α
¨

[


m
aw2In

-


m
aw1n



{



c

2

n



sin






θ
n


-


b

2

n




cos


(


γ
n

+

θ
n


)




}


-


α
.




θ
.

n



m
aw1n



{



c

2

n



cos






θ
n


+


b

2

n




sin


(


γ
n

+

θ
n


)




}


-


β
¨



m
aw1n



a

1

n




cos


(

α
+

γ
n

+

θ
n


)



+



β
.



(


α
.

+


θ
.

n


)




m
aw1n







a

1

n








sin


(

α
+

γ
n

+

θ
n


)



+



z
¨

0



m
aw1n



cos


(

α
+

γ
n

+

θ
n


)



cos





β

-


(


α
.

+


θ
.

n


)








z
.

0







m
aw1n







sin


(

α
+

γ
n

+

θ
n


)



cos





β

-


β
.




z
.

0







m
aw1n







cos


(

α
+

γ
n

+

θ
n


)



sin





β

-

&AutoLeftMatch;


[



-

k
zi








e

0

i

2







{


sin


(


γ
i

+

θ
i


)


-

sin


(


γ
ii

+

θ
ii


)



}







cos
(






γ
n

+

θ
n


)







X
s


-






k
ziii







e

0

iii

2







{


sin


(


γ
iii

+

θ
iii


)


-

sin
(






γ
iv

+





θ
iv


)


}







cos
(






γ
n

+

θ
n


)







X
s


-



α
.

2



m
aw1n



{



c

2

n



cos






θ
n


+


b

2

n




sin


(


γ
n

+

θ
n


)




}


+



β
.

2







m
an



{



e

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}







e

1

n








cos


(

α
+

γ
n

+

θ
n


)



+


m
wn



{



e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}







e

3

n








cos


(

α
+

γ
n

+

θ
n


)







-


θ
.



α
.



m
aw1n



{



c

2

n



cos






θ
n


+


b

2

n




sin


(


γ
n

+

θ
n


)




}


+


θ
.



β
.



m
aw1n



a

1

n




sin


(

α
+

γ
n

+

θ
n


)



+


α
.



β
.



a

1

n




m
aw1n



sin


(

α
+

γ
n

+

θ
n


)



-


gm
aw1n



cos


(

α
+

γ
n

+

θ
n


)



cos





β

-




z
.

0



(


α
.

+


θ
.

n


)




m
aw1n



sin


(

α
+

γ
n

+

θ
n


)



cos





β

-


β
.




z
.

0



m
aw1n



cos


(

α
+

γ
n

+

θ
n


)



sin





β


]

=



λ

1

n




e

2

n



sin






θ
n


+


λ

2

n




e

2

n



cos






θ
n


+


λ

3

n




e

3

n




cos


(

α
+

γ
n

+

θ
n


)



cos





β










(
107
)






















θ
¨

n



m
aw2In


+


α
¨



(


m
aw2In

-


m
aw1n



H
1



)


-


α
.




θ
.

n



m
aw1n



H
2


-


β
¨



m
aw1n



a

1

n




C

αγθ





n



+



β
.



(


α
.

+


θ
.

n


)




m
aw1n



a

1

n




S

αγθ





n



+



z
¨

0



m
aw1n







C

αγθ





n








C
β


-

&AutoLeftMatch;


[



-

k
zi








e

0

i

2







{


sin


(


γ
i

+

θ
i


)


+

sin


(


γ
ii

+

θ
ii


)



}







X
s


-






k
ziii







e

0

iii

2







{


sin


(


γ
iii

+

θ
iii


)


+

sin


(


γ
iv

+

θ
iv


)



}







cos


(


γ
n

+

θ
n


)




X
s


-



α
.

2



m
aw1n



H
2


+



β
.

2



(



m
an



B
2



e

1

n




C

αγθ





n



+


m
wn



B
3



e

3

n




C

αγθ





n




)


-


θ
.



α
.



m
aw1n



H
2


+


θ
.



β
.



m
aw1n



a

1

n




S

αγθ





n



+


α
.



β
.



a

1

n




m
aw1n



S

αγθ





n



-


gm
aw1n



C

αγθ





n




C
β



]

=



λ

1

n




e

2

n




S
θn


+


λ

2

n




e

2

n




C

θ





n



+


λ

3

n




e

3

n




C

αγθ





n




C
β









(
108
)























θ
¨

n



m
aw2In


+


α
¨



(


m
aw2In

-


m
aw1n



H
1



)


-


β
¨



m
aw1n



a

1

n




C

αγθ





n



+



z
¨

0



m

aw





ln




C

αγθ





n




C
β


+



α
.

2



m
aw1n



H
2


-



β
.

2



(



m
an



B
2



e

1

n




C

αγθ





n



+


m
wn



B
3



e

3

n




C

αγθ





n




)


+


gm
aw1n



C

αγθ





n




C
β


+


k
zi



e

0

i

2



{


sin


(


γ
i

+

θ
i


)


+

sin


(


γ
ii

+

θ
ii


)



}



cos


(


γ
n

+

θ
n


)



+


k
ziii



e

0

iii

2



{


sin


(


γ
iii

+

θ
iii


)


+

sin


(


γ
iv

+

θ
iv


)



}



cos


(


γ
n

+

θ
n


)




=




λ

1

n




e

2

n




S
θn


+


λ

2

n




e

2

n




C
θn


+


λ

3

n




e

3

n




C

αγθ





n




C
β













(
109
)





















θ
¨

n

=







α
¨



(


m
aw2In

-


m
aw1n



H
1



)


-


β
¨



m
aw1n



a

1

n




C

αγθ





n



+









z
¨

0



m

aw





ln




C

αγθ





n




C
β


+



α
.

2



m
aw1n



H
2


-









β
.

2



(



m
an



B
2



e

1

n




C

αγθ





n



+


m
wn



B
3



e

3

n




C

αγθ





n




)


+








gm
aw1n



C

αγθ





n




C
β


-


λ

1

n




e

2

n




S

θ





n



-


λ

2

n




e

2

n




C

θ





n



-








λ

3

n




e

3

n




C

αγθ





n




C
β


+


k
zi



e

0

i

2



{


sin


(


γ
i

+

θ
i


)


+












sin


(


γ
ii

+

θ
ii


)


}



cos


(


γ
n

+

θ
n


)



+


k
ziii



e

0

iii

2



{


sin


(


γ
iii

+

θ
iii


)


+











sin


(


γ
iv

+

θ
iv


)


}



cos


(


γ
n

+

θ
n


)







-

m
aw2In







(
110
)


























t




(



L





z
.


6

n




)


-



L




z

6

n





=




F





z
.


6

n




+




l
,
n





λ
ln



a
ln5












l
=
1

,
2
,


3





n

=
i

,
ii
,
iii
,
iv





(
111
)






















m
sn




z
¨


6

n



+


α
¨



m
sn



{



c

1

n



sin






η
n


+


b

2

n




cos


(


γ
n

+

η
n


)




}


+


α
.




η
.

n



m
sn



{



c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


-


β
¨



m
sn



a

1

n




cos


(

α
+

γ
n

+

η
n


)



+



β
.



(


α
.

+


η
.

n


)




m
sn



a

1

n




sin


(

α
+

γ
n

+

η
n


)



+



z
¨

0



m
sn



cos


(

α
+

γ
n

+

η
n


)



cos





β

-


(


α
.

+


η
.

n


)




z
.

0



m
sn



sin


(

α
+

γ
n

+

η
n


)



cos





β

-


β
.




z
.

0



m
sn



cos


(

α
+

γ
n

+

η
n


)



sin





β

-





m
sn




η
.

n
2



z

6

n



+



α
.

2




m
sn



[


z

6

n


+

{



c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


]



+







β
.

2







m
sn







{



z

6

n








cos
(





α
+

γ
n

+

η
n


)


+


c

1

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}







cos
(





α
+

γ
n

+

η
n


)


+



η
.

n



α
.



m
sn



{


2


z

6

n



+


c

1

n



cos






η
n


-


b

2

n




sin


(


γ
n

+

η
n


)




}


+



η
.

n



β
.



m
sn



a

1

n




sin


(

α
+

γ
n

+

η
n


)



+


α
.



β
.



a

1

n




m
sn



sin


(

α
+

γ
n

+

η
n


)



-


gm
sn



cos


(

α
+

γ
n

+

η
n


)



cos





β

-


k
sn



(


z

6

n


-

l
sn


)


+




z
.

0



(


α
.

+


η
.

n


)




m
sn



sin


(

α
+

γ
n

+

η
n


)



cos





β

-


β
.




z
.

0



m
sn



cos


(

α
+

γ
n

+

η
n


)



sin





β





=



-

c
sn





z
.


6

n



-


λ

1

n



sin






η
n


-


λ

2

n



cos






η
n







(
112
)






















m
sn



{



z
¨


6

n


+


α
¨



E
2


-


β
¨



a

1

n




C

αγη





n



-



η
.

n
2



z

6

n



-



α
.

2



(


z

6

n


+

E
1


)


-



β
.

2



B
1



C

αγη





n



-

2



η
.

n



α
.



z

6

n



+


gC

αγη





n




C
β



}


+


k
sn



(


z

6

n


-

l
sn


)



=




-

c
sn





z
.


6

n



-


λ

1

n




S

η





n



-


λ

2

n




C

η





n














(
113
)




















λ

2

n


=






m
sn



{



z
¨


6

n


+


α
¨



E
2


-


β
¨



a

1

n



-



η
.

n
2



z

6

n



-



α
.

2



(


z

6

n


+

E
1


)


-













β
.

2



B
1



C

αγη





n



-

2



η
.

n



α
.



z

6

n



+


gC

αγη





n




C
β



}

+








k
sn



(


z

6

n


-

l
sn


)


+


c
sn




z
.


6

n



+


λ

1

n




S

η





n








-

C

η





n








(
114
)






















t




(



L





z
.


12

n




)


-



L




z

12

n





=




F





z
.


12

n




+




l
,
n





λ
ln



a
ln6









l
=
1

,
2
,


3





n

=
i

,
ii
,
iii
,
iv






















k
wn



(


z

12

n


-

l
wn


)


=



-

c
wn





z
.


12

n



+


λ

3

n



cos





αcosβ








=



-

c
wn





z
.


12

n



+


λ

3

n




C
α



C
β
















(
115
)




















λ

3

n


=




c
wn




z
.


12

n



+


k
wn



(


z

12

n


-

l
wn


)




C
α






(
116
)













From the differentiated constraints it follows that:















θ
¨

n



e

2

n




S

θ





n



+



θ
.

n
2



e

2

n




C

θ





n



-



z
¨


6

n




S

η





n



-



z
.


6

n





η
.

n



C

η





n



-











η
¨

n



(


z

6

n


-

d

1

n



)








C

η





n



-



η
.

n




z
.


6

n




C

η





n



+




η
.

n
2



(


z

6

n


-

d

1

n



)








S

η





n




=
0









θ
¨

n



e

2

n




C

θ





n



-



θ
.

n
2



e

2

n




S

θ





n



-



z
¨


6

n




C

η





n



+



z
.


6

n





η
.

n



S

η





n



+











η
¨

n



(


z

6

n


-

d

1

n



)








S

η





n



+



η
.

n




z
.


6

n




S

η





n



+




η
.

n
2



(


z

6

n


-

d

1

n



)








C

η





n




=
0












(
117
)





















η
¨

n

=








θ
¨

n



e

2

n




S

θ





n



+







θ
.

n
2







e

2

n








C

θ





n



-







z
¨


6

n








S

η





n



-







2



η
.

n




z
.


6

n




C

η





n



+




η
.

n
2



(


z

6

n


-

d

1

n



)




S

η





n









(


z

6

n


-

d

1

n



)



C

η





n








(
118
)





















z
¨


6

n


=








θ
¨

n



e

2

n




C

θ





n



-



θ
.

n
2



e

2

n




S

θ





n



+




η
¨

n



(


z

6

n


-

d

1

n



)




S

η





n



+







2



η
.

n




z
.


6

n




S

η





n



+




η
.

n
2



(


z

6

n


-

d

1

n



)




C

η





n








C

η





n







(
119
)













and











z
.


12

n


=




{



α
.







z

12

n




S
α


-


(


α
.

+


θ
.

n


)



e

3

n




C

αγθ





n



+


α
.







c

2

n




S

αγ





n



-


α
.







b

2

n




C
α



}



C
β


-






z
.

0

+






β
.

[







{



z

12

n




C
α


+


e

3

n




S

αγθ





n



+


c

2

n




C

αγ





n



+


b

2

n




S
α



}



S
β


+


a

1

n




C
β



]

+



R
.

n



(
t
)





C
α



C
β







(
120
)













Supplemental differentiation of equation (116) for the later entropy production calculation yields:











k
wn




z
.


12

n



=



-

c
wn





z
¨


12

n



+



λ
.


3

n




C
α



C
β


-


α
.



λ

3

n




S
α



C
β


-


β
.



λ

3

n




C
α



S
β







(
121
)













therefore











z
¨


12

n


=





λ
.


3

n




C
α



C
β


-


α
.



λ

3

n




S
α



C
β


-


β
.



λ

3

n




C
α



S
β


-


k
wn




z
.


12

n





c
wn






(
122
)













or from the third equation of constraint:














z
¨

0

+

{




z
¨


12

n



cos





α

-



z
.


12

n




α
.






cos





α

-


α
¨



z

12

n







sin





α

-


α
.








z
.


12

n







sin





α

-











α
.

2



z

12

n



cos





α

+


(


α
¨

+


θ
.

n


)



e

3

n








cos


(

α
+

γ
n

+

θ
n


)



-









(


α
.

+


θ
.

n


)

2



e

3

n








sin


(

α
+

γ
n

+

θ
n


)



-


α
¨







c

2

n




sin


(

α
+

γ
n


)



-












α
.

2



c

2

n








cos


(

α
+

γ
n


)



+


α
¨







b

2

n



cos





α

-



α
.

2



b

2

n



sin





α


}


cos





β

-







β
.



{




z
.


12

n



cos





α

-


α
.







z

12

n







sin





α

+


(


α
.

+


θ
.

n


)



e

3

n








cos


(

α
+

γ
n

+

θ
n


)



-













α
.







c

2

n




sin


(

α
+

γ
n


)



+


α
.







b

2

n



cos





α


}


sin





β

-


β
¨

[

{



z

12

n



cos





α

+














e

3

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}


sin





β

+









a

1

n



cos





β

]

-


β
.

[

{




z
.


12

n



cos





α

-


α
.







z

12

n



sin





α

+











(


α
.

+


θ
.

n


)



e

3

n




cos


(

α
+

γ
n

+

θ
n


)



-


(


α
.

+


γ
.

n


)



c

2

n




sin


(

α
+

γ
n


)



+










α
.







b

2

n



cos





α

}


sin





β

+


β
.



{



z

12

n







cos





α

+


e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+

















c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}


cos





β

-


β
.







a

1

n



sin





β


]

-



R
¨

n



(
t
)



=
0







(
123
)





















z
¨


12

n


=







z
¨

0

+

{



-


z
.


12

n





α
.






cos





α

-


α
¨



z

12

n







sin





α

-


α
.








z
.


12

n







sin





α

-











α
.

2



z

12

n



cos





α

+


(


α
¨

+


θ
.

n


)



e

3

n








cos


(

α
+

γ
n

+

θ
n


)



-









(


α
.

+


θ
.

n


)

2



e

3

n








sin


(

α
+

γ
n

+

θ
n


)



-


α
¨







c

2

n




sin


(

α
+

γ
n


)



-












α
.

2



c

2

n








cos


(

α
+

γ
n


)



+


α
¨







b

2

n



cos





α

-



α
.

2



b

2

n



sin





α


}


cos





β

-







β
.



{




z
.


12

n



cos





α

-


α
.







z

12

n







sin





α

+










(


α
.

+


θ
.

n


)



e

3

n








cos


(

α
+

γ
n

+

θ
n


)



-


α
.







c

2

n




sin


(

α
+

γ
n


)



+










α
.







b

2

n



cos





α

}


sin





β

-


β
¨

[

{



z

12

n



cos





α

+











e

3

n








sin


(

α
+

γ
n

+

θ
n


)



+


c

2

n




cos


(

α
+

γ
n


)



+












b

2

n



sin





α

}


sin





β

+


a

1

n



cos





β


]

-


β
.

[

{




z
.


12

n



cos





α

-











α
.







z

12

n



sin





α

+


(


α
.

+


θ
.

n


)



e

3

n




cos


(

α
+

γ
n

+

θ
n


)



-











(


α
.

+


γ
.

n


)



c

2

n




sin


(

α
+

γ
n


)



+


α
.







b

2

n



cos





α


}


sin





β

+







β
.



{



z

12

n







cos





α

+


e

3

n




sin


(

α
+

γ
n

+

θ
n


)



+















c

2

n




cos


(

α
+

γ
n


)



+


b

2

n



sin





α


}


cos





β

-


β
.







a

1

n



sin





β


]

-



R
¨

n



(
t
)








-
cos






α





cos





β






(
124
)













IV. Equations for Entropy Production




Minimum entropy production (for use in the fitness function of the genetic algorithm) is expressed as:













β


S



t


=






-
2





β
.

2

[



α
.







m
b



A
1



A
2


+


m
sn



B
1



{




z
.


6

n




C

αγη





n



-















(


α
.

+


η
.

n


)



z

6

n




S

αγη





n



-


α
.







A
4



}

+








m
an



B
2



{



(


α
.

+


θ
.

n


)



e

1

n




C

αγθ





n



-


α
.







A
6



}


+








m
wn



B
3



{



(


α
.

+


θ
.

n


)



e

3

n




S

αγθ





n



-


α
.







A
6



}


-










z
.

0



(


m
ba

+

m
sαwan


)





S
β

/
2


]






m
sαw2n

+

m
baI

+


m
b



A
1
2


+


m
sn



B
1
2


+


m
an



B
2
2


+


m
wn



B
3
2








(
125
)























α


S



t


=






-
2




α
.

2



{



m
sn



α
.









z
.


6

n




(


z

6

n


+

E

1

n



)



+











m
sn



z

6

n





η
.

n



E

2

n



+



θ
.

n



m

α





wln




H

2

n




}






m
bbI

+

m
sαwIn

+


m
sn




z

6

n




(


z

6

n


+

2


E

1

n




)



-

2


m

α





wIn




H

1

n









(
126
)
























η
n



S



t


=




η
.

n
3


t





g






η
n


-


2



η
.

n
2




z
.


6

n





z

6

n


-

d

1

n









(
127
)
























z

6

n




S



t


=

2



η
.

n




z
.


6

n

2


t





g






η
n






(
128
)
























z

12

n




S



t


=



z
.


12

n

2



(


α
.

+


α
.


t





g





α

+

2


β
.


t





g





β


)






(
129
)













The learning module


101


gains pseudo-sensor signals based on the kinetic models of the vehicle and suspensions obtained by the above-described methods. Then, the learning module


101


directs the learning control unit to operate based on the pseudo-sensor signals. Further, at the optimized part, the learning module


101


calculates the time differential of the entropy from the learning control unit and time differential of the entropy inside the controlled process. In this embodiment, the entropy inside the controlled processes is obtained from the kinetic models as described above. This embodiment utilizes the time differential of the entropy dS


cs


/dt (where S


cs


is S


c


for the suspension) relative to the vehicle body and dS


s


/dt to which time differential of the entropy dS


ss


/dt (where the subscript ss refers to the suspension) relative to the suspension is added. Further, this embodiment employs the damper coefficient control type shock absorber. Since the learning control unit (control unit of the actual control module


101


) controls the throttle amount of the oil passage in the shock absorbers, the speed element is not included in the output of the learning control unit. Therefore, the entropy of the learning control unit is reduced, and tends toward zero.




The optimized part defines the performance function as a difference between the time differential of the entropy from the learning control unit and time differential of the entropy inside the controlled process. The optimized part genetically evolves teaching signals (input/output values of the fuzzy neural network) in the learning control unit with the genetic algorithm so that the above difference (i.e., time differential of the entropy for the inside of the controlled process in this embodiment) becomes small. The learning control unit is optimized based on the learning of the teaching signals.




Then, the parameters (fuzzy rule based in the fuzzy reasoning in this embodiment) for the control unit at the actual control module


101


are determined based on the optimized learning control unit. Thereby, the optimal regulation of the suspensions with nonlinear characteristic can be allowed.




Although the foregoing has been a description and illustration of specific embodiments of the invention, various modifications and changes can be made thereto by persons skilled in the art, without departing from the scope and spirit of the invention as defined by the following claims.



Claims
  • 1. A method for control of a plant comprising the steps of: calculating a first entropy production rate corresponding to an entropy production rate of a control signal provided to a model of said plant; calculating a second entropy production rate corresponding to an entropy production rate of said model of said plant; determining a fitness function for a genetic optimizer using said first entropy production rate and said second entropy production rate; providing said fitness function to said genetic optimizer; providing a teaching output from said genetic optimizer to a learning input of a first control system, said first control system configured to control said model of said plant; and providing a control parameter from said first control system to parameter input of said second control system.
  • 2. The method of claim 1, wherein said first control system comprises a neural network.
  • 3. The method of claim 1, wherein said second control system comprises a neural network.
  • 4. The method of claim 1, wherein said model of said plant comprises a model of a suspension system.
  • 5. The method of claim 1, wherein said second control system is configured to control a physical plant.
  • 6. The method of claim 1, wherein said second control system is configured to control a shock absorber.
  • 7. The method of claim 1, wherein said second control system is configured to control a damping rate of a shock absorber.
  • 8. The method of claim 1, wherein said second control system receives sensor input data from one or more sensors that monitor a vehicle suspension system.
  • 9. The method of claim 8, wherein at least one of said sensors is a height sensor that measures a vehicle height.
  • 10. The method of claim 8, wherein at least one of said sensors is a length sensor that measures a change in length of at least a portion of said suspension system.
  • 11. The method of claim 8, wherein at least one of said sensors is an angle sensor that measures an angle of at least a portion of said suspension system with respect to said vehicle.
  • 12. The method of claim 8, wherein at least one of said sensors is an angle sensor that measures an angle of a first portion of said suspension system with respect to a second portion of said suspension system.
  • 13. The method of claim 1, wherein said second control system is configured to control a throttle valve in a shock absorber.
  • 14. A control apparatus comprising: off-line optimization means for determining a control parameter from an entropy production rate; online control means for using said control parameter to control a plant.
Non-Patent Literature Citations (31)
Entry
Bose Bimal K., (1994), “Expert System, Fuzzy Logic, and Neural Network Applications in Power Electric and Motion Control”, Processing of the IEEE, vol. 82, No. 8, pp. 1303-1323, Aug. 1994.
Feng Q. and Yamafuji K., (1988), “Design and simulation of control system of an inverted pendulum”, Robotica, vol. 6, No. 3, pp. 235-241, No Month.
Gradetsky V.G. and Ulyanov S.V., (1993), “Mobile system with wall climbing robots”, Intern. J. of Computer and Systems Sciences, vol. 31, No. 1, pp. 126-142, (no month).
Johansson R., Magnusson M. and Fransson P.A., (1995), “Galvanic vestibular stimulation for analysis of postural adaptation and stability”, IEEE Transactions on Biomedical Engineering, vol. 42, No. 3, pp. 282-292, (no month).
Ju M.-S., Yi S.-G., Tsuei Y.-G, and Chou Y.-L., (1995), “Fuzzy control of electrohydraulic above-knee prostheses”, JSME International Journal, vol. 38, No. 1, pp. 78-85, (no month).
Lee Y.N., Kim T.W. and Suh I.H., (1994), “A look-up table-based self-organizing fuzzy plus linear controller”, Mechatronics, vol. 4, No. 1, pp. 71-90, Apr. 19, 1993.
Liu T.S. and Wu J.C., (1993), “A model for rider-motorcycle system using fuzzy control”, IEEE Transactions on Systems, Man and Cybernetics, vol. 23-SMC, No. 1, pp. 267-276, No Month.
Mendel, Jerry M., (1995), “Fuzzy Logic Systems for Engineering: A Tutorial”, Proceeding of the IEEE, vol. 83, No. 3, pp. 345-377, (no month).
Nakajima R., Tsubouchi T., Yuta S. and Koyanagi E., (1997), “A development of a new mechanism of an autonomous unicycle”, Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '97), vol. 2, Grenoble, France, pp. 906-912, No Month.
Peround M. and Saucier A., (1987), “Thermodynamics of dissipative systems”, Helvetica Physica, vol. 60, No. 8, pp. 1038-1051, No Month.
Sheng Z.Q. and Yamafuji K., (1995), “Realization of a human riding a unicycle by a robot”, Proc. of '95 IEEE Intern. Conf. on Robotics & Automation, Japan, vol. 2, pp. 1319-1326, Jun. 1995.
Sheng Z.Q. and Yamafuji K., (1997), “Postural stability of a human riding a unicycle and its emulation by a robot”, IEEE Trans. on Robotics and Automation, vol. 13, No. 5, pp. 709-720, No Month.
Sheng, Z.Q. and Yamafuji K., (1995), “Study on the stability and motion control of a unicyle, 1st Report: Dynamics of a human riding a unicycle and its modeling by link mechanism”, JSME International Journal, vol. 38C, No. 2, pp. 249-259,1996, No Month.
Sheng Z.Q., Yamafuji K. and Ulyanov S.V., (1996), “Study on the stability and motion control of a unicycle. Pts 3,” JSME International Journal, vol 39, No. 3, pp. 560-568, No Month.
Tsai, C.H. and Li, T.H.S., (1996) “Design of Lyapunov Function Based Fuzzy Logic Controller”, IEEE, pp. 396-401, 1992, No Month.
Ulyanov S.V., (1992), “Fuzzy models of Intelligent control systems: Theoretical and applied aspects” Soviet Journal of Computer and Systems Sciences (Engineering Cybernetics), vol. 30, No. 4, pp. 1-22, No Month.
Ulyanov S.V., Sheng Z.Q., Yamafuji K., Watanabe S. and Ohkura T., (1995), “Self-organization fuzzy chaos Intelligent controller for a robotic unicyle: A New benchmark in AI control”, Proc. of 5th Intelligent System Symposium: Fuzzy, Al and Neural Network Applications Technologies (FAN Symp. '95), Tokyo, pp. 41-46, Sep. 1996.
Ulyanov S.V. and Yamafuji K., (1996), “Fuzzy Intelligent emotion and instinct control of a robotic unicycle”, 4th Intern. Workshop on Advanced Motion Control, Mie, Japan, vol. 1, pp. 127-132, 1998.
Ulyanov S.V., Feng Q., Yamafuji K. and Ulyanov V.S., (1998), “Stochastic analysis of nonlinear dynamic system with time-dependent structure, Pts. 1, 2”, Probabilistic Engineering Mechanics (published), no month.
Ulyanov, S.V., Yamafuji, K., Gradetsky V.G., and Pagni, A., “Expert Fuzzy-Neuro Controller Desing for Wall Climbing Robot for Decontamination of Nuclear-Power Station”, Journal of Robotics and Mechatronics, vol. 7, No. 1, 1995, pp. 75-86, Dec. 13, 1994.
Ulyanov, S.V., Yamafuji, K., Ulyanov, V.S., Kurawaki, I., Hagiwara, T. and Panfilov, S.A., “Computational Intelligence for Robust Control Algorithms of Complex Dynamic Systems with Minimum Entropy Production Part 1: Simulation of Entropy-Like Dynamic Behavior and Lyapunov Stability”, Journal of Advanced Computational Intelligence, vol. 3, No. 2, 1999, pp. 82-98, Dec. 1, 1998.
Ulyanov, V.S., Yamafuji, K., Ulyanov, S.V., and Tanaka K., “Computational Intelligence with New Physical Controllability Measure sure Robust Control Algorithm of Extension-Cableless Robotic Unicycle”, Journal of Advanced Computational Intelligence, vol. 3, No. 2, 1999, pp. 136-147, Apr. 1999.
V.S. Ulyanov, Watanabe, S. Yamafuji, K., Ulyanov, S.V., Litvintseva, L.V., and Kurawaki, I. “Intelligent robust control of a robotic unicycle based on a new physical measure for mechanical controllability” Advanced Robotics, vol. 12, No. 4 1998, pp. 455-481, No Month.
Ulyanov, S.V., Yamafuji, K., Miyagawa, K., Tanaka, T. and Fukuda T., “Intelligent Fuzzy Motion Control of Mobile Robot for Service Use”, IEEE/RSJ International Conference on Intelligent Robots and Systems, Human Robot Interaction and Cooperative Robots, vol. 3, Aug. 5-9, 1995 pp.486-491.
Ulyanov, S.V., Yamafuji, K., Arai, F., and Fukuda T., “Modeling of Micro-Nano-Robots and Physical Limit of Micro Control” Journal of the Robotics Society of Japan, vol. 14, No. 8, Nov. 1996, pp.18-21.
van Rooij A.J.F., Jain L.C. and Johnson R.P., (1997), “Neural Network Training Using Genetic Algorithms” (Machine Perception and Artificial Intelligence, vol. 26). World Scientific Pub., SGP, No Month.
Vasilieva O.I., Ionov I.P. and Ulyanov S.V., (1989), “Dual control of the artificial ventilation process with use of a fuzzy controller in the feedback circuit”, Biomedical Eng., vol. 23, No. 1, pp. 7-16.
Wu J.C. and Llu T.S., (1995), “Fuzzy control of rider-motorcycle system using genetic algorithm and auto-tuning”, Mechatronics, vol. 4, No. 4, pp. 441-455, No Month.
Wu J.C. and Llu T.S., (1996), “Fuzzy control stabilization with applications to motorcycle control”, IEEE Transactions on Systems, Man and Cybernetics, vol. 26, No. 6, pp. 836-847, No Month.
Zhakharov V.N. and Ulyanov S.V., (1994), “Fuzzy models of intelligent industrial controllers and control systems. Pts 1”, Journal of Computer and Systems Sciences International, vol. 32, No. 1, pp. 123-144, No Month.
Zhakharov V.N. and Ulyanov S.V., (1995), “Fuzzy models of intelligent industrial controllers and control systems. Pts 2, 3”, Journal of Computer and Systems Sciences International, vol. 33, No. 2, pp. 94-108; 117-136, Mar.-Apr.