|
pinocchio
1.2.6-7-g6de3e-dirty
|
The model used for this non linear version of the DC motor is defined by the following state vector \( \mathbf{x} = [ x_0, x_1, x_2 ]^{\top} = [ \theta_m, \dot{\theta}_m , T]^{\top} \) with \(\theta_m\) the motor angle, and \( T \) the temperature.
The ODE function \( \dot{\bf{x}}= f(\bf{x},\bf{u},\bf{c},\bf{f}_{ext}) \) describing the dynamics of the motor for a set of parameters \( \bf{c} \), with control vector \( \bf{u} \) and submitted to external forces \( \bf{f}_{ext} \) by the multi-body rigid dynamics is:
\[ \left( \begin{matrix} \dot{\theta}_m \\ \ddot{\theta}_m \\ \dot{T} \\ \end{matrix} \right) = \left( \begin{matrix} \dot{\theta}_m \\ \frac{K_m}{B_m R_{TA}(1-\alpha_{Cu} ( T-T_{A}))}V - \left(\frac{K_b}{B_mR_{TA}(1-\alpha_{Cu} ( T-T_{A}))} + \frac{D_m}{B_m}\right) \dot{\theta}_m -\frac{\tau_l}{B_m} \\ \frac{(R_{th1} + R_{th2})(V-K_b \dot{\theta}_m)^2}{(1+\alpha_{Cu}(T-T_{A})) R_{TA}-\alpha_{cu}(R_{th1}+R_{th2})(V-K_b \dot{\theta}_m)^2} \end{matrix} \right) \]
with \( B_m\) the rotor inertia, \( K_m \) the motor torque constant, \( R \) the armature resistance, \( i_a \) the current armature, \( \tau_m \) the motor torque, \( V \) the armature voltage, \( V_b \) the back electromotive force (E.M.F.), \( D_m \) the motor friction and includes friction in the brushes and gears, the robot position \( \theta_m \), \( \tau_l \) the motor load, \( K_b \) the back E.M.F. constant.
Note that the winding resistor is increasing linearly with the temperature and is given by:
\[ R = R_{TA} (1 + \alpha_{Cu} (T -T_{A}))\]
and the current is given by:
\[ i_a = K_m \frac{V}{R} \]
As the dynamics can be also written:
\[ \left( \begin{matrix} \dot{x}_0 \\ \dot{x}_1 \\ \dot{T} \\ \end{matrix} \right) = \left( \begin{matrix} x_1 \\ \frac{c_{0}}{c_1 +c_2 T} V + \frac{c_{3} + c_4 T }{c_1 + c_2 T} \dot{\theta}_m + c_5 \tau_l \\ \frac{c_6 (V- c_7 \dot{\theta}_m)^2}{c_8 + c_{9} T + c_{10} (V- c_7 \dot{\theta}_m)^2} \\ \end{matrix} \right) = \left( \begin{matrix} x_1 \\ \frac{c_{0}}{c_1 +c_2 x_2} u + \frac{c_{3} + c_4 x_2 }{c_1 + c_2 x_2} x_1 + c_5 \bf{S}\bf{f}_{ext} \\ \frac{c_6 (u- c_7 x_1)^2}{c_8 + c_{9} x_2 + c_{10} (u- c_7 x_1)^2} \\ \end{matrix} \right) \]
Thus
\[ \begin{array}{rcl} c_1 &= & K_m\\ c_2 &= & B_m R_{TA} (1+T_{A}\alpha_{Cu})\\ c_3 &= & -B_m R_{TA} \alpha_{Cu} \\ c_4 &= & K_B + D_m R_{TA} + D_m R_{TA} \alpha_{Cu} T_{A} \\ c_5 &= & -D_m R_{TA} \alpha_{Cu} \\ c_6 &= & -\frac{1}{B_m} \\ c_7 &= & (R_{th1}+R_{th2})\\ c_8 &= & -K_b\\ c_9 &= & R_{TA} - \alpha_{Cu} T_A R_{TA} \\ c_{10} &=& \alpha_{Cu} R_{TA} \\ c_{11} &=& -\alpha_{Cu}(R_{th1}+R_{th2}) \\ \end{array} \]
The control vector \( \bf{u} \) is the voltage \( V \).
\( \bf{S} \) is the selection matrix projecting the spatial force \(\bf{f}_{ext}\) expressed in the motor coordinates.
The observation function \(h(\bf{x},\bf{u},\bf{c},\bf{f}_{ext})\) is defined as:
\[ \left( \begin{matrix} i_a \\ \tau_m\\ V_b \\ \end{matrix} \right) = \left( \begin{matrix} \frac{V}{R} - \frac{K_b}{R}\dot{\theta}_m \\ \frac{V K_m}{R} - \frac{K_b K_m}{R}\dot{\theta}_m\\ K_b \dot{\theta}_m\\ \end{matrix} \right) \]