Project

General

Profile

Actions

Lego NXTway robot

Documentation

Initial model + controller can be found at
- http://lejos-osek.sourceforge.net/nxtway_gs.htm
- http://www.mathworks.com/matlabcentral/fileexchange/19147 for the Simulink files

Plant


Continuous, non-linear equations

The equations of motions derived from the lagrange equations are:

\begin{equation}
[(2m+M)R^2+2J_w+2n^2J_m]\ddot{\theta} + (MLR\mathrm{cos}\psi-2n^2J_m)\ddot{\psi} - MLR\dot{\psi}^2\mathrm{sin}\psi = \frac{nK_t}{R_m}(\nu_l+\nu_r)-2(\frac{nK_tK_b}{R_m} + f_m + f_w)\dot{\theta}+2(\frac{nK_tK_b}{R_m} + f_m)\dot{\psi}
\end{equation}
\begin{equation}
(MLR\mathrm{cos}\psi-2n^2J_m)\ddot{\theta} +(ML^2+J_\psi + 2n^2J_m)\ddot{\psi} - MgL\mathrm{sin}\psi - ML^2\dot{\phi}^2\mathrm{sin}\psi\mathrm{cos}\psi = -\frac{nK_t}{R_m}(v_l+v_r)+2(\frac{nK_tK_b}{R_m} + f_m)\dot{\theta}-2(\frac{nK_tK_b}{R_m} + f_m)\dot{\psi}
\end{equation}

\begin{equation}
\left [ \frac{1}{2}mW^2 + J_\phi + \frac{W^2}{2R^2}(J_w+n^2J_m)+ ML^2\mathrm{sin}^2\psi \right ]\ddot{\phi} +2ML^2\dot{\psi}\dot{\phi}\mathrm{sin}\psi\mathrm{cos}\psi= \frac{WnK_t}{2RR_m}(\nu_r-\nu_l) - \frac{W^2}{2R^2}(\frac{nK_tK_b}{R_m} + f_m + f_w)\dot{\phi}
\end{equation}

where $\theta = \frac{1}{2}(\theta_r+\theta_l)$ and $\phi = \frac{R}{W}(\theta_r - \theta_l)$, and $\nu_r$, $\nu_l$ are the values of the voltage applied to the right and left motors respectively. The values and meaning of the parameters can all be found in the documentation.

Linerized equation

The litteral expressions of the following matrices can be obtained in the attached matlab script.

\begin{equation}
\begin{pmatrix}
\dot{\theta} \\
\dot{\psi} \\
\dot{\phi} \\
\ddot{\theta} \\
\ddot{\psi} \\
\ddot{\phi}
\end{pmatrix} =
\begin{pmatrix}
0&0&0&1&0&0 \\
0&0&0&0&1&0 \\
0&0&0&0&0&1 \\
0&-409.718&0&-162.127&162.127&0 \\
0&269.627&0&78.1496&-78.1496&0 \\
0&0&0&0&0&-95.5684
\end{pmatrix}
\begin{pmatrix}
\theta \\
\psi \\
\phi \\
\dot{\theta} \\
\dot{\psi} \\
\dot{\phi}
\end{pmatrix}
+
\begin{pmatrix}
0&0\\
0&0\\
0&0\\
157.580&-157.580\\
-75.9576&-75.9576\\
53.0787&-53.0787
\end{pmatrix}
\begin{pmatrix}
\nu_r \\
\nu_l
\end{pmatrix}
\end{equation}

Controller

initial controller enhanced with
- anti windup
- wheel control

Building instruction

The controller has been written in Lustre. The following steps allow to compile it to C and upload it on the robot.
TODO

Updated by Romain Jobredeaux almost 9 years ago ยท 29 revisions