Mathematical Modeling and Control Architecture of the Autonomous Lower Body of a Humanoid Robot

The control requires the knowledge of the mathematical model and some sort of intelligence. Whereas the required mathematical modelling is obtained from basic physical laws governing robot dynamics and associated devices [13]. A robot performs the specified tasks in its environment, which can be divided into two classes: Contact type tasks and non-contact type tasks. The non-contact type tasks involved manipulation of the end–effector in space to do desired work. While in contact-type tasks the end effector interacts with the environment. The foot of the lower body of the humanoid robot comes in contact with the ground and exerts a force on the ground. The ground force i.e. the combination of friction force and normal reaction force is applied to the body of the humanoid robot. The friction force is responsible to cause the motion in a forward direction and the body can move on the ground [14]. An individual joint of a manipulator is powered and driven by actuators that apply a force or a torque to cause the motion of the links. The actuator commands that move the manipulator and achieve the specified end effector motion, are provided by the manipulator control system [15]. These commands are based on the control set points generated from the set of joint torque time histories obtained by the trajectory planner [16]. The actual joint and/or end effector positions and their derivatives can be fed back to the control system to get accurate motion. The schematic diagram of a manipulator control system is shown in Figure 1. The dotted lines for the feedback indicate that the control system may employ feedback on the actual joint locations and velocities. The parameter $q$, $\dot{q}$ and $\ddot{q}$ and $\tau$ and so on.

1.png

Figure 1. Block diagram of lower body control system

The dynamic control of a manipulator's motions and /or interaction forces requires knowledge of forces or torque that must be exerted on the manipulator's joints to move the kinks and the end effector from the present location to the desired location, with or without the constraints of a particular planned end-effector trajectory, and or planned end –effector force/torque.

In both the situations of the desired end–effector location and the desired end effector force, the control of the individual joint’s location is important. Hence it is an obvious requirement that each joint is controlled by a position servo. If the lower body of the robot is to move very slowly or to move one joint at a time, then the control is simple because coupled dynamics forces are negligible. 

Each joint can be controlled independently by using a simple control system, which produces a joint actuator forces/torque proportional to the required change in the joint variable. This is the proportional control algorithm. If the motion is fast, which are must for effective robot applications, all joints must move simultaneously. In this situation, the coupled dynamics forces are significant, and it is known that dynamics are nonlinear and complex. Joints cannot move independently, and a complex control algorithm will be required. The typical robot control architecture for an n-Degree of freedom manipulator (DOF) consists of a master control system to control and synchronize n-joints [17]. This master control is responsible for sending “set point” information to command to each of the joint controllers. The n-joints controllers use the set point information to command the joint actuator to move the joint. The joint controller may employ feedback to the master controller. The schematic of a typical control architecture for a manipulator control system is illustrated in Figure 2.

2.png

Figure 2. Control architecture for 5-Dof manipulator

2.1 Manipulator control problem

The task to be performed by a manipulator may require either the execution of specific motions of the end effector in free space and/or the application of specific contact forces at the end effector if the end effector is constrained by the environment. For a given manipulator, many techniques can be used to control it to perform in the desired manner. The performance and range of application of a manipulator are significantly influenced by the control scheme followed, as well as the way it is implemented [18]. The mechanical design and configuration design also greatly influence the control technique utilized and the performance of the manipulator. Depending on the control law employed to compute the joint torques, the control system is classified as either linear or nonlinear [19]. The design of the linear controllers based on the approximate linear model of the manipulator will be examined. 

The effects of time-varying inertia, interaction torques, and nonlinear torques, of the dynamics model are ignored to simplify the control system. Servo control strategies such as proportional derivatives (PD) are considered. Finally, the method of computed torque control, which is based on a nonlinear control law will be considered.

2.2 Effect of damping factor on the system

The control of a system consists of varying the inputs so that the closed loop dynamics of the systems have the desired properties; say the output $y$ follows the desired trajectory $y_d$. Feedback control explicitly uses the error signal, $\mathrm{e}=y-y_d$, the difference between the measured output $y$ and the desired output $y_d$, as a part of control law, to reduce the error and system sensitivity to the parameters in the dynamic model. The block diagram of this system with unity negative feedback is drawn in Figure 3.

3.png

Figure 3. Block diagram of a second-order linear system with unity feedback

If R(s)=Yd(s) is the Laplace transform of a set-point input, the closed-loop transfer function of the control system is 

$\frac{Y(s)}{R(s)}=\frac{G(s)}{1+G(s)}=\frac{b_0 \omega_n^2}{s^2+2 \zeta \omega_n s+\left(1+b_0\right) \omega_n^2}$   (1)

The second-order linear characteristics equation of the closed-loop system is written in standard form as

$s^2+2 \xi \omega_n s+\omega_n^2=0$    (2)

where, ξ and ωn is the damping factor and natural frequency of the system.

The roots of the characteristics equation, r1, r2 which characterize the system’s time response to a given input, are

$r_1, r_2=-\xi \omega_n \pm \omega_n \sqrt{\xi^2-1}$      (3)

These roots determine the nature of time response of the system. If roots are real, then the response of the system is sluggish and non-oscillatory. If roots are complex conjugate, then the response of the system is oscillatory. The system response analysis helps in fixing the controller parameters that ensure closest possible trajectory tracking performance of a joint [20]. Depending on the values of the damping ratio, these roots can be real and unequal or real and equal or complex conjugate. 

These three classes of the roots characterize the three types of time response of the systems. These three classes of roots arise with the value of damping ratio greater than unity, equal to unity and less than unity.

Case(i) Damping ratio greater than unity

A damping ratio greater than unity (ξ>1) gives the roots r1 and r2 as real and unequal. The roots r1 and r2 are

$r_1=-\xi \omega_n+\omega_n \sqrt{\xi^2-1}, r_2=-\xi \omega_n-\omega_n \sqrt{\xi^2-1}$

The time response is obtained by taking the inverse Laplace transform of the equation, which gives

$s^2+2 \xi \omega_n s+\omega_n^2=0$   (4)

$y(t)=1-\frac{e^{-\xi \omega_n \, t}}{2 \sqrt{\xi^2-1}}$$\left[\left(\sqrt{\xi^2-1}-\xi\right) e^{-\omega_n \sqrt{\left(\xi^2-1\right)} \, t}+\left(\sqrt{\xi^2-1}+\xi\right) e^{\omega_n \sqrt{\left(\xi^2-1\right)} \, t}\right]$       (5)

In this case, the system has sluggish and non-oscillatory behaviour and the system is overdamped.

Case(ii) Damping ration equal to unity

For a damping ratio equal to unity (ξ=1), both r1 and r2 are real and equal. The roots r1 and r2 are

$r_{1=}-\omega_n, r_{2=}-\omega_n$

and the time response of the system is 

$y(t)=1-\left[1-e^{-\omega_n t}-\omega_n t e^{-\omega_n t}\right]$    (6)

Under this condition, the system exhibits the fastest possible non-oscillatory, overshoot-free response and it is critically damped.

Case(iii) Damping ratio less than unity

A damping ratio greater than unity (ξ<1) gives the roots r1 and r2 complex conjugates of each other. 

The time response is obtained by taking the inverse Laplace transform of the equation, which gives

$y(t)=1-\frac{e^{-\xi \omega_n t}}{2 \sqrt{\xi^2-1}}$$\sin \left(\omega_n \sqrt{\left(1-\xi^2\right)} t+\tan ^{-1} \frac{\sqrt{\left(1-\xi^2\right)}}{\xi}\right)$       (7)

In this case, the system has an oscillatory behaviour and is underdamped.

From the above analysis, it is observed that the linear second order system is stable for all values of damping ratio, although the behaviour may be oscillatory or non-oscillatory.

A humanoid control system cannot allow an oscillatory response for obvious reasons. For instance, in a pick and place operation, an oscillating end effector may strike against the object before picking it to manipulate. Hence the highest possible speed of response and yet non oscillatory response dictates that the controller design parameters shall be chosen to have the damping ratio ξ=1 or at least close to it but less than unity.

2.3 Model of a manipulator joint

A common drive system for many industrial robots consists of an electric motor as the actuator and provide the torque to the joint. The schematic of an actuator gear link assembly of a rotary joint shown in Figure 4.

The gear ratio ‘η’ is defined as:

η = number of teeth on the gear of link shaft/number of teeth on the gear of actuator shaft. The gear ratio η is greater than 1 for a reduction gear. The gear ratio causes a reduction of the actuator speed $\dot{\theta}_a$ to $\dot{\theta}$, the speed of the link and increase of the actuator torque τ_a to τ, the torque applied to the link (the load) as given by

$\tau=\eta \tau_a$   (8)

$\dot{\theta}=\frac{1}{\eta} \dot{\theta}_a$     (9)

$\theta=\frac{1}{\eta} \theta_a$   (10)

where, Ɵ_a is the angular displacement of the actuator shaft and Ɵ is the joint angular displacement.

4.png

Figure 4. Actuator-gear-link mechanism

The link torque, when reflected to the actuator side, is scaled down by a factor η, the gear ratio. If I_a is the inertia of rotor of the actuator shaft and B_a is the viscous friction coefficient at the actuator bearings, for the rotational mass-damper system, the torque required at the actuator shaft is given by

$\tau_a=I_a \ddot{\theta}_a+B_a \dot{\theta}_a+\frac{\tau}{\eta}$    (11)

$\tau_a=I_a \ddot{\theta}_a+B_a \dot{\theta}_a+\frac{(I \ddot{\theta}+B \dot{\theta})}{\eta}$     (12)

where, I is the inertia of the load and B is the viscous friction coefficient for load. The Eq. (11) can be written in terms of the actuator shaft variable as.

$\tau_a=\left(I_a+\frac{I}{\eta^2}\right) \ddot{\theta}_a+\left(B_a+\frac{B}{\eta^2}\right) \dot{\theta}_a$      (13)

Or in terms of link variables as

$\tau=\left(I+\eta^2 I_a\right) \ddot{\theta}+\left(B+\eta^2 B_a\right) \dot{\theta}$    (14)

The nonlinearity term like viscous force, backlash, joint damping and air damping were discarded. The reason behind this, the system will work inside the closed room, hence there is no such effect disturb the system behavior.

The link torque τ is obtained from the dynamic model of the joint. The dynamic equation of the joint i  is rewritten as below

$\tau=\sum_{j=1}^n M_{i j}(q) \ddot{q}_j+\sum_{\substack{j=1 \\ \text { for } i=1,2, \ldots, n}}^n \sum_{k=1}^n h_{i j k} \dot{q}_j \dot{q}_k+G_i$      (15)

In above equation, gravity loading G is significant in gravity environment only and comes into effect whenever motion is against gravity. The equation does not include the effect of friction, backlash, and elastic deformation. Backlash is difficult to model and elastic deformation and static friction are highly nonlinear and complex to model. The viscous friction is directly proportional to the velocity and can be included in the model. Hence the equation is modified as below to include the contribution of viscous friction.

$\tau=\sum_j M_j \ddot{\theta}_j+\sum_i \sum_k h_{j k} \dot{\theta}_j \dot{\theta}_k+B \dot{\theta}+G$    (16)

where, B is the viscous friction coefficient at the link bearings.

The term in equation is split into linear decoupled and nonlinear interacting terms.

The two linear decoupled terms are $M \ddot{\theta}$ from the first summation term for link i and the third term $B \dot{\theta}$. The remaining terms in equation are nonlinear interacting term and grouped together and denoted as ‘d’ i.e.

$d=\sum_{j \neq i, j=1}^n M_j \ddot{\theta}_j+\sum_j \sum_k h_{j k} \dot{\theta}_j \dot{\theta}_k+G$    （17）

Thus the equation can be written as

$\tau=M \ddot{\theta}+B \dot{\theta}+d$      （18）

Substituting τ from equation and rearranging, the actuator torque is obtained as

$\left(I_a+\frac{M}{\eta^2}\right) \ddot{\theta}_a+\left(B_a+\frac{B}{\eta^2}\right) \dot{\theta}_a+\frac{d}{\eta}$      （19）

In the case of highly geared manipulators (η≫1), it is quite reasonable to ignore the contribution of the nonlinear interacting terms d and still guarantee a good trajectory tracking performance by the controller.

Neglecting the nonlinear terms, the actuator torque reduces to the simple form

$\tau_a=\left(I_{\text {eff }}\right) \ddot{\theta}_a+\left(B_{\text {eff }}\right) \dot{\theta}_a$      （20）

where, I_eff is the effective inertia and B_eff is the effective damping with

$I_{\text {eff }}=\left(I_a+\frac{M}{\eta^2}\right), B_{\text {eff }}=\left(B_a+\frac{B}{\eta^2}\right)$      

Because the configuration dependent terms M is reduced by a factor η², with η≫1), I_eff is almost constant for highly geared manipulators.

2.4 Model of a DC motor

The schematic diagram of an armature controlled permanent magnet DC motor is shown in Figure 5. Motors with field excitation provided by permanent magnets are preferred for robotics applications. Such motors are labelled permanent magnet DC motors.

5.png

Figure 5. Permanent magnet DC motor

With references to the schematic diagram, the control signal to the DC motor is applied at the armature terminals in the form of armature voltage e_a. The motor torque $\tau_a$ is related to the armature current i_a as

$\tau_a=K_T i_a$     （21）

where, K_T is the motor torque constant. The armature current directly controls the torque generated by the actuator. The back emf $e_b$ induced in the armature winding is given by

$e_b=K_b \dot{\theta}_a$      （22）

where, K_b is the back emf constant. Applying the Kirchoff’s voltage law for the circuit of figure gives

$e_a=e_b+i_a R_a$    （23）

where, e_a is the emf across armature and R_a is the armature resistance. Substituting e_b from equation gives

$e_a=K_b \dot{\theta}_a+i_a R_a$    （24)

Thus, the armature voltage is adjusted, depending on the commands generated by the manipulator control system, which specify the torque required from the actuator. The continuous adjustment of the armature voltage e_a is carried out by the motor driver circuitry so that the desired current $i_a$ flows through the armature windings. The schematic of a motor driver circuitry is shown in Figure 6.

2.5 Partitioned Proportional Derivative (PPD) control scheme

Since the system is nonlinear due to presence of second order derivative in the mathematical modelling. Linear PID control system would not capable to handle the nonlinear system. A nonlinear control theory were adopted for joint control system.

A linear controller based on a partitioned proportional derivative control strategy is slightly different from a simple PD controller. It is useful for systems that are more complex. The controller is partitioned into two parts; a model-based portion and a servo portion, such that the joint parameters (I_eff and B_eff appear only in the model-based portion [21]. The approximate model of the manipulator joint developed in previous section is used to implement this controller. The block diagram of this controller is shown in Figure 7.

The portion to the right of the partitioned line is physical system, the model-based portion and to the left of the partitioned line is the servo based control system, usually implemented in a computer or microcontroller.

7.png

Figure 7. Partitioned PD controller for trajectory control

The model-based portion of the control law computes the command torque based on the system parameter I_eff and B_eff. The Laplace of the approximate model of the joint is

$\tau_a(s)=\left(I_{e f f} s^2+B_{e f f} s\right) \theta_a(s)$     (25)

The model-based control law defines the actuator toque using a structure similar to that of equation, i.e.

$\tau_a(s)=I_{e f f} \hat{\tau}_a(s)+B_{e f f} s \theta_a(s)$    (26)

where, $\grave{\tau}_a(s)$ is the torque value specified by the servo portion to control a unit inertia.

From Eq. (24) and Eq. (25)

$\grave{\tau}_a(s)=s^2 \theta_a(s)$   (27)

This is the equation of motion for a system with unit inertia. Thus, the model based law effectively reduced the system to that of unit inertia system. In other words, the physical system appears to be a unit-inertia system to the servo portion [22].

8.png

Figure 8. Simplified PPD controller for control of unit-inertia system

The servo portion of the controller is based on the error in actuator position and its derivative. These servo errors are computed as

$E(s)=\theta_d(s)-\theta_a(s)$     (28)

and $s E(s)=s \theta_d(s)-s \theta_a(s)$     (29)

where, Ɵ_d(s) is the desired joint displacement of the actuator shaft. The servo portion of the control law makes use of the feedback to modify the system behaviour. Because the model –based portion of the control law design becomes very simple. The controller is simply required to control a system of unit inertia with no friction and stiffness [16]. Assuming that the sensors are capable of detecting position and velocity, a proportional derivative control scheme can be used to control the system of mass /inertia. The control system using the PD control scheme with position and velocity feedback for the control of unit inertia is shown in Figure 8.

The control law for the servo portion is specified with two controller gain parameters, the proportional constant K_p, and the derivative constants K_d as

$\grave{\tau}_a(s)=s^2 \theta_d(s)+K_d s E(s)+K_p E(s)$    (30)

Combining Eqns. (28-30), the control law becomes

$s^2 \theta_a(s)=s^2 \theta_d(s)+K_d s E(s)+K_p E(s)$    (31)

The error dynamics of the PPD controller using equation, is given by

$s^2 E(s)+K_d s E(s)+K_0 E(s)=0$      (32)

Eq. (31) is identical to the characteristic equation of the second order linear system. Thus, the partitioned PD controller reduces the system to that of a linear second –order systems. Comparing equation with equation, gives the natural frequency of the system as;

$\omega_n=\sqrt{K_p}$         (33)

and the damping ratio as

$\xi=\frac{K_d}{2 \sqrt{K_p}}$        (34)

From equation, it is clear that any desired second –order performance of the control system is possible by the choice of proper control gains, K_p and K_d.

The controller for the manipulator should be critically damped, i.e., ξ=1.

Hence for critically damped performance

$K_d=2 \sqrt{K_p}=2 \omega_n$      (35)

2.6 Setting of gain parameter

The equation specifies the methodology for setting the methodology for setting the control gains K_p and K_d. A direct consequence of control law, equation, is that the servo error at any instant of time is zero provided there is no initial error and the computation time for the computer is zero, i.e., the actuator torque is computed as a continuous function of time.

In reality, the time taken to compute the servo error, the PD law control gains and to command a new value of torque, is nonzero and is known as the cycle time. This the resulting command torque $\grave{\tau}_a(s)$ is a staircase function and the servo error are non-zero at the beginning of each cycle.

The controller will reduce this nonzero servo error to zero during each cycle. Hence the actual trajectory tracked will be close to, but not exactly the same as desired trajectory. Apart from a damping ratio of unity, another factor that constraints the selection of control gains in the flexibility of links, which are assumed to be rigid bodies in the development of the joint model. The un-modelled structural flexibility of the link and other mechanical elements produces resonance at frequencies other than natural frequency. Because these structural flexibilities have been ignored, the controller must be designed so as not to excite these un-modelled resonances.

The lowest un-modelled resonance, which corresponds to the maximum value of the effective inertia seen by the actuator, I_max has a resonance frequency

$\omega_{\text {res }}=\omega_o \sqrt{\frac{I_0}{I_{\max }}}$      (36)

where, ω_o is the structural frequency when the effective inertia is I_o.

To prevent exciting these structural oscillations and also ensure structural stability, the controller natural frequency ω_n must be limited to 0.5ω_res, i.e.

$\omega_n \leq 0.5 \omega_{\text {res }}$ and $K_p \leq\left(0.5 \omega_o\right)^2 \frac{I_o}{I_{\max }}$     （37）

The natural frequency of individual link were obtained as follows

$\omega_n=\sqrt{\frac{3 E I}{L^3 M}}$   （38）

Based on these parameters the control gain K_p and K_d are computed.

Table 1 shows the approximate gain values for the different joint controller.

Table 1. Tuning parameter of the joint controller

Gain parameter of controller improves the stability of the system. Gain helps the controller to reach to the desired position.

Figure 10 shows the schematic Simulink model of control system. The Coriolis and centripetal torque do not allow the link to reach the desired position of the robot. Computed torque control measures the inertia of the system and sense the signal to different motors controlling the humanoid feet to reach the desired position to reach to the desired position of humanoid feet to at actual position.

The input of the computed torque control is the input trajectory of joint. The nonlinear term of the CTC does not allow the link to reach the desired position. The CTC controller measure the error position and velocity of the joint. The PPD controller adjusted the torque and bring the joint into the desired position [24]. The Coriolis and the centripetal torque are nonlinear in nature. The CTC measured this variation and modifying the controller parameter based on the unit inertia system. The Simulink signal interacting with the physical system. So that one converter used for the interface. For this purpose, the SPS converter used to understand the interface the system. Figure 11 shows the interaction of the computed control to the lower body of the humanoid robot.

11.png

Figure 11. Interfacing of computed torque control to lower body of Humanoid

The interfacing of the CTC controller to the proposed model, the leg movement occurred for the plan trajectory and gait time has been noted. For a simple walk, the first, fourth and fifth joint trajectory has been implemented. During walk, the sequence of trajectory of hip joint, knee joint and ankle joint trajectory implemented, and movement of the leg observed for a gait period of 0.8 seconds [25]. A successful walk obtained. For the stable sudden turn, the first, second, fourth and fifth joint of the system comes into the action. During sudden turn, the second joint motion produced for the momentarily and it will stop. Afterward the same trajectory repeated as usual for the normal walk [1, 2].