Modelling Robot Manipulator Dynamics

In my research the modelling of robot manipulators (RMs) dynamics, mapping the position, velocity and acceleration of joints to forces and torques exerted to the structure is based on the Lagrange formulation, which ensures the appropriate structure of the dynamic model that is commonly used in control algorithms.

RMs are known to be highly nonlinear multi-input multi-output systems. To preserve the known structure of the Lagrange formulation, common to all system equations of RMs and other dynamic systems such as navigation dynamics of missiles and aeroplanes, I have chosen the grey-box modelling approach.

Forces exerted to joints of the RM are the sum of four components modelling consequently the torque resulting from the inertia (H), the Coriolis effects and centrifugal forces (C), the gravity forces (g) and the viscose friction (f). Individual knowledge of all these components is important for precise, model based robot control algorithms, yet it is impossible to directly, explicitly measure these components in any general real life system. When designing a grey-box model advantage can be taken of other commonly known facts of robotics like H and g are nonlinear functions of joint positions and the driving torque 𝜏 is linear in the joint accelerations. The centrifugal and Coriolis effects are quadratic in the joint velocities and nonlinear in the joint positions and f is linear in joint velocity [80].

The dynamic model identification method uses the measured resultant torque and joint variables along suitably chosen paths for every joint. The application of Lagrange dynamic equations for a robot manipulator in the joint space formulates the resultant torque 𝜏_𝑖 acting on the i^th joint for all the p joints of the RM as a function of following vectors:

∑^𝑝_𝑗=1(𝑫_𝑖𝑗(𝒒) ∙ 𝒒̈_𝑗)+ ∑^𝑝_𝑗=1∑^𝑝_𝑘=1(𝒒̇_𝒋∙ 𝑫_𝑖𝑗𝑘(𝒒) ∙ 𝒒̇_𝑘)+ 𝑫_𝑖(𝒒) + 𝑓_𝑖 = 𝜏_𝑖 (39) where: q is the vector of joint positions; 𝒒̇ are joint velocities; 𝒒̈ are joint accelerations;

∑^𝑝_𝑗=1(𝑫_𝑖𝑗(𝒒) ∙ 𝒒̈_𝑗) is commonly referred to as 𝑯 ∙ 𝒒̈ or 𝕁 ∙ 𝒒̈ the inertia matrix component of the torque; ∑^𝑝_𝑗=1∑^𝑝_𝑘=1(𝒒̇_𝒋∙ 𝑫_𝑖𝑗𝑘(𝒒) ∙ 𝒒̇_𝑘) is commonly referred to as 𝑪 ∙ 𝒒̇

or ℂ ∙ 𝒒̇, describing the centrifugal forces and the Coriolis effects; 𝑫_𝑖(𝒒) is commonly referred to as g the gravitational force component; 𝑓_𝑖 stands for the viscous friction; and 𝜏_𝑖 is the resultant torque acting on the i^th joint – identities in the common robotics notation are:

𝐻_𝑖𝑗 = 𝐷_𝑖𝑗(𝒒), 𝐶_𝑖𝑘 = ∑^𝑝_𝑗=1𝑞̇_𝑗 ∙ 𝐷_𝑖𝑗𝑘(𝒒), 𝑔_𝑖 = 𝐷_𝑖(𝒒), 𝑓_𝑖 = 𝑐𝑜𝑛𝑠𝑡_𝑖, (40)

where: 𝐷_𝑖𝑗, 𝐷_𝑖𝑗𝑘, 𝐷_𝑖 are in general, highly nonlinear scalar functions of q, the joint position vector. They may contain sin(*) and cos(*) functions of joint positions and/or of their products and sums defined by the geometry of the RM.

There are well known general relations that can be used to reduce the number of equation (40) is not possible. The only information on the output of the system joint is the resultant torque (39). The identification of all nonlinear functions under these terms is a considerable problem.

By my Hypothesis III.a this paper will propose and present the validity of a new method that will identify the RM dynamics through finding the 𝐷_𝑖𝑗 nonlinear functions of equation (39) as TSK FLSs, while calculating 𝐷_𝑖𝑗𝑘 nonlinear functions as in equation (41). All linear parameters of the system will be determined by SVD based robust LS method. Nonlinear parameters will be evolved by multi-objective GA and fine-tuned by a gradient descent method.

4.1.2 Modelling Multi-rotor Flight Dynamics

The complete dynamics of an aircraft, taking into account aero-elastic effects, flexibility of wings, internal dynamics of engines, and the whole set of changing environmental variables is quite complex and somewhat unmanageable for the purpose of autonomous control engineering [10]. This paper deals with the flight dynamics of multi-rotors.

Multi-rotor UAV manoeuvres are controlled by varying angular speeds of its propellers.

Each rotor blade produces a thrust and a torque, whose combination generates the main trust, the yaw torque, the pitch torque, and the roll torque acting on the multi-rotor.

Motors produce a force proportional to the square of the angular speed and the angular acceleration of the rotor; the acceleration term is commonly neglected as the speed transients are short thus exerting no significant effects. Motors of a multi-rotor can only turn in a fixed direction, so the produced force can be always presumed positive.

Motors are set up so that opposites form pairs rotating in the same direction (clockwise/counter-clockwise), while their neighbouring motors are rotating in the opposite direction (counter-clockwise/clockwise). This arrangement is chosen so that gyroscopic effects and aerodynamic torques are cancelled in trimmed flight. The main trust is the sum of individual trusts of each motor. The pitch torque is a function of difference in forces produced on one pair of motors, while the roll torque is a function of difference in forces produced on other pair of motors. The yaw torque is sum off all motor reaction torques due to shaft acceleration and blades drag. The motor torque is opposed by a general aerodynamic drag.

For a full navigation dynamic model of a multi-rotor system both (i) the centre of mass position vector of 𝝃 = (𝑥, 𝑦, 𝑧) in fixed frame coordinates and (ii) the orientation Euler angles: roll, pitch, yaw angles (ϕ,θ,ψ) around body axes X, Y, Z are considered. Using the Euler-Lagrange approach it can be shown how the translational forces 𝑭_𝜉, applied to the rotorcraft due to main trust, can be fully decoupled from the yaw, pitch and roll

𝑚 ∙ (𝝃̈ + 𝑔 ∙ [0 0 1]^𝑇) = 𝑭_𝜉 (42) where: 𝝃̈ is the second time derivative (acceleration) of the E-frame coordinates of the rotorcraft centre of mass 𝝃 = (𝑥, 𝑦, 𝑧), 𝑚 is the multi-rotor mass; 𝑔 is the gravitational constant, which is acting only along the third axis 𝑧⃗; 𝑭_𝜉 is the vector of translational forces.

𝕁(𝒒) ∙ 𝒒̈ + ℂ(𝒒, 𝒒̇) ∙ 𝒒̇ = 𝝉 (43)

where: 𝕁 is a 3x3 matrix, called the inertia matrix, ℂ is also a 3x3 matrix that refers to Coriolis, gyroscopic and centrifugal terms, 𝒒 = [𝜙, 𝜃, 𝜓] is the state vector of Euler angles, its time derivatives are 𝑑𝒒/𝑑𝑡 = 𝒒̇ = [𝜙̇, 𝜃̇, 𝜓̇] and 𝑑𝒒̇/𝑑𝑡 = 𝒒̈ = [𝜙̈, 𝜃̈, 𝜓̈].

For the scope of this research we shall address only equation (43) as that is the complex nonlinear challenging part of the multi-rotor flight dynamics model to be identified.

Equation (43) can be analysed as three resultant torques τi acting along the [ϕ, θ, ψ] axes for i,j,k∈(ϕ, θ, ψ) as:

∑ (𝐷_{𝑗 𝑖𝑗}(𝒒) ∙ 𝑞̈_𝑗) + ∑ ∑ (𝑞̇_{𝑗 𝑘 𝑗}∙ 𝐷_𝑖𝑗𝑘(𝒒) ∙ 𝑞̇_𝑘) = 𝜏_𝑖 (44) The similarity with RM dynamics equation (39) is obvious, the first component of equation (44) is the inertia matrix part expansion, the second is the Coriolis matrix term expansion, whose components are highly nonlinear functions containing 𝑠𝑖𝑛(𝒒) and 𝑐𝑜𝑠(𝒒) components, and also their products and sums defined by the rigid body system geometry as described in [67].

There are general relations that can be used for reducing the number of unknown inertia and Coriolis components: 𝕁 is symmetric and ℂ is defined by Christoffel symbols of 𝕁:

𝐷_𝑖𝑗𝑘 = (𝜕𝐷_𝑖𝑗/𝜕𝑞_𝑘+ 𝜕𝐷_𝑖𝑘/𝜕𝑞_𝑗− 𝜕𝐷_𝑗𝑘/𝜕𝑞_𝑖)/2 (45) These properties result in further inherent relations as:

𝐷_𝑖𝑗 = 𝐷_𝑗𝑖, 𝐷_𝑖𝑗𝑘 = 𝐷_𝑖𝑘𝑗, 𝐷_𝑘𝑖𝑗 = −𝐷_𝑗𝑖𝑘, 𝐷_𝑘𝑗𝑘 = 0, ∀𝑖, 𝑘 ≥ 𝑗 (46) It should be noted that direct measurement of any single 𝐷_𝑖𝑗𝑘 or 𝐷_𝑖𝑗 component of equation (44) is not possible. Measurable data vector pairs are (𝒒,̈ 𝝉) angular accelerations as system input and resultant torques proportional to rotation speed of motors as system output.

Determining all 𝐷_𝑖𝑗𝑘 and 𝐷_𝑖𝑗 nonlinear functions is a considerable grey-box identification problem, but when achieved the model is usable in efficient robust and precise model based control implementations, as this model preserves all 𝒒̈, 𝒒̇ state variables in an explicit form.

By my Hypothesis III.b this paper will propose and present the validity of a new method that will identify the multi-rotor flight dynamics equation (44) 𝐷_𝑖𝑗 components by specially constructed continuous and periodic TSK FLSs, while calculating 𝐷_𝑖𝑗𝑘 nonlinear functions as in equation (45). For modelling multi-rotor flight dynamics I propose to extend the definition of TSK FLSs in a way that they become periodic and of continuous output, even for the attitude Euler angle system inputs 0-2π transitions.

4.1.3 Validating Quality of Complex Nonlinear Dynamic System Genetic Fuzzy