Home
build details

Show: section status errors & todos local changes recent changes last change in-page changes feedback controls

Duckiebot modeling

Modified 2018-06-24 by Andrea Censi

Obtaining a mathematical model of the Duckiebot is important in order to (a) understand its behavior and (b) design a controller to obtain desired behaviors and performances, robustly.

The Duckiebot uses an actuator (DC motor) for each wheel. By applying different torques to the wheels a Duckiebot can turn, go straight (same torque to both wheels) or stay still (no torque to both wheels). This driving configuration is referred to as differential drive.

In this section we will derive the model of a differential drive wheeled robot. The Duckiebot model will receive voltages as input (to the DC motors) and produce a configuration, or pose, as output. The pose describes unequivocally the position and orientation of the Duckiebot with respect to some Newtonian “world” frame.

Different methods can be followed to obtain the Duckiebot model, namely the Lagrangian or Newton-Euler, we choose to describe the latter as it arguably provides a clearer physical insight. Showing the equivalence of these formulations is an interesting exercises that the interested reader can take as a challenge. A useful resource for modeling of a Duckiebot may be found here [10].

Requires:k:reference_frames (inertial, body), k:intro-transformations (Cartesian, polar)

Suggested: k:intro-ODEs-to-LTIsys

k:diff-drive-robot-model

Preliminaries

Modified 2018-06-24 by Andrea Censi

TODO: relabel inertial frame -> local frame; $(\cdot)^I \rightarrow (\cdot)^L$

previous task next (46 of 51) index
for:Liam Paulltask

The following was marked as "todo".

TODO: relabel inertial frame -> local frame; $(\cdot)^I \rightarrow (\cdot)^L$

File book/learning_materials/71_duckiebot_modeling/10_basic_car_modeling-kin-and-dyn.md.

File book/learning_materials/71_duckiebot_modeling/10_basic_car_modeling-kin-and-dyn.md
in repo duckietown/docs-learning_materials branch master commit ab91feef
last modified by Andrea Censi on 2018-06-24 17:48:00

Created by function create_notes_from_elements in module mcdp_docs.task_markers.

We first briefly recapitulate on the reference frames that we will use to model the Duckiebot, with the intent of introducing the notation used throughout this chapter. It is important to note that we restrict the current analysis to the plane, so all of the following in defined in $\reals^2$.

To describe the behavior of a Duckiebot three reference frames will be used:

  • A “world” frame: a right handed fixed reference system with origin in some arbitrary point $O$. We will indicate variables expressed in this frame with a superscript $W$, e.g., $X^W$, unless there is no risk of ambiguity, in which case no superscript will be used.

  • An “inertial” frame: a fixed reference system parallel to the “world” frame, that spans the plane on which the Duckiebot moves. We will denote its axis as $\{X_I, Y_I\}$, and it will have origin in point $A=(x_A, y_A)$, i.e., the midpoint of the robot’s wheel axle. We will indicate variables expressed in this frame with a superscript $I$, e.g., $X^I$, unless there is no risk of ambiguity, in which case no superscript will be used.

  • A body (or “robot”) frame: a local reference frame fixed with respect to the robot, centered in $A$ as well. The $x$ axis points in the direction of the front of the robot, and the $y$ axis lies along the axis between the wheels, so to form a right handed reference system. We denote the robot body frame with $\{X_R, X_R\}$. The same superscript convention as above will be used. The wheels will have radius $R$.

The robot is assumed to be a rigid body, symmetric, and $X_r$ coincides with axis of symmetry. Moreover, the wheels are considered identical and at the same distance, $L$, from the axle midpoint $A$.

Moreover:

  • The center of mass $C^W = (x_c, y_c)$ of the robot is on the $x_r$ axis, at a distance $c$ from $A$, i.e., ($C^R = (c, 0)$);

  • $X^r$ forms an orientation angle $\theta$ with the local horizontal plane.

These notations are summarized in Figure 1.2.

Relevant notations for modeling a differential drive robot

Kinematics

Modified 2018-06-24 by Andrea Censi

In this section we derive the kinematic model of a differential drive mobile platform under the assumptions of (a) no lateral slipping and (b) pure rolling of the wheels. We refer to these two assumptions as kinematic constraints.

Differential drive robot kinematic constraints

Modified 2018-06-22 by Andrea Censi

The kinematic constraints are derived from two assumptions:

  • No lateral slipping motion: the robot cannot move sideways, but only in the direction of motion, i.e., its lateral velocity in the robot frame is zero: $$ \label{eq:mod-no-lat-slip-constraint-r}\tag{4} \dot y_A^r = 0. $$

By inverting \eqref{eq:mod-rotation-r2i}, this constraint can be expressed through the inertial frame variables, yielding:

$$ \label{eq:mod-no-lat-slip-constraint-i}\tag{5} \dot y_A(t) \cos \theta(t) -\dot x_A(t) \sin \theta(t) = 0. $$

Imposing \eqref{eq:mod-no-lat-slip-constraint-i} on \eqref{eq:mod-A-dot-polar} results in:

$$ \label{eq:mod-no-lat-slip-constraint-v_w}\tag{6} v_w(t) = \dot y_C^I(t) \cos\theta(t) - \dot x_C^I(t) \sin\theta(t), $$

and by recalling that $C^R = (c,0)$:

$$ \label{eq:mod-no-lat-slip-constraint-C}\tag{7} \dot y_C^I(t) \cos\theta(t) - \dot x_C^I(t) \sin\theta(t) = c\dot\theta(t). $$

Hence, we obtain the strongest expression of this constraint:

$$ \label{eq:mod-no-lat-slip-final}\tag{8} v_w(t) = c\dot\theta(t), $$

and therefore:

$$ \label{eq:mod-no-lat-slip-final-dot}\tag{9} \dot v_w(t) = c\ddot\theta(t). $$

a simpler way of deriving \eqref{eq:mod-no-lat-slip-final-dot} is noticing, from \eqref{eq:mod-v_A^R}, that $\dot y_A^R = v_w(t) - c\dot\theta(t)$.

Pure rolling kinematic constraint

\begin{align} \label{eq:mod-pure-rolling}\tag{26} \left\{ \begin{array}{ll} v_{P,r} &= R \dot \varphi_{r} \\ v_{P,l} &= R \dot \varphi_{l} \end{array} \right.. \end{align}

Another notable consequence of this assumption is that, always in the robot frame, the full power of the motor can be translated into a propelling force for the vehicle in the longitudinal direction. Or, more simply, it allows to write:

$$ \label{eq:mod-force-and-torque}\tag{10} F_{u, (\cdot)}(t)R = \tau_{(\cdot)}(t), $$

where $\tau_{(\cdot)}(t)$ is the torque exerted by each motor on its wheel $(\cdot) = {l,r}$.

Differential drive robot kinematic model

Modified 2018-06-22 by Andrea Censi

In a differential drive robot, controlling the wheels at different speeds generates a rolling motion of rate $\omega = \dot \theta$. In a rotating field there always is a fixed point, the center of instantaneous curvature (ICC), and all points at distance $d$ from it will have a velocity given by $\omega d$, and direction orthogonal to that of the line connecting the ICC and the wheels (i.e., the axle). Therefore, by looking at Figure 1.2, we can write:

\begin{align} \label{eq:mod-kin-1}\tag{27} \left\{ \begin{array}{l} \dot \theta (d-L) &= v_l \\ \dot \theta (d+L) &= v_r \end{array} \right., \end{align}

from which:

\begin{align} \label{eq:mod-kin-2}\tag{28} \left\{ \begin{array}{l} d &= L \frac{v_r + v_l}{v_r - v_l} \\ \dot \theta &= \frac{v_r - v_l}{2L} \end{array} \right.. \end{align}

A few observations stem from \eqref{eq:mod-kin-2}:

  • If $v_r = v_l$ the bot does not turn ($\dot \theta = 0$), hence the ICC is not defined;
  • If $v_r = - v_l$, then the robot “turns on itself”, i.e., $d=0$ and $ICC \equiv A$;
  • If $v_r = 0$ (or $v_l = 0$), the rotation happens around the right (left) wheel and $d = 2L$ ($d = L$).

Moreover, a differential drive robot cannot move in the direction of the ICC, it is a singularity.

By recalling the no lateral slipping motion \eqref{eq:mod-no-lat-slip-constraint-r} hypothesis and the pure rolling constraint \eqref{eq:mod-pure-rolling}, and noticing that the translational velocity of $A$ in the robot frame is $v_A = \dot \theta d = (v_r+v_l)/2$ we can write:

\begin{align} \label{eq:mod-kin-3}\tag{29} \left\{ \begin{array}{l} \dot x_A^R &= R (\dot \varphi_R +\dot \varphi_L)/2 \\ \dot y_A^R &= 0 \\ \dot \theta &= \omega = R(\dot \varphi_R - \dot \varphi_L)/(2L) \end{array} \right., \end{align}

which in more compact yields the simplified forward kinematics in the robot frame:

\begin{align} \label{eq:mod-forward-kinematics-robot-frame}\tag{30} \left[ \begin{array}{c} \dot x_A^R \\ \dot y_A^R \\ \dot \theta \end{array} \right] = \left[ \begin{array}{cc} \frac{R}{2} & \frac{R}{2} \\ 0 & 0 \\ \frac{R}{2L} & -\frac{R}{2L} \\ \end{array} \right] \left[ \begin{array}{c} \dot \varphi_R \\ \dot \varphi_L \end{array} \right]. \end{align}

Finally, by using \eqref{eq:mod-rot-mat}, we can recast \eqref{eq:mod-forward-kinematics-robot-frame} in the inertial frame.

The simplified forward kinematics model of a differential drive vehicle is given by: \begin{align} \label{eq:mod-forward-kinematics-inertial-frame}\tag{31} \displaystyle \avec{\dot q}^I = \amat{R}(\theta) \left[ \begin{array}{c} \dot x_A^r \\ \dot y_A^r \\ \dot \theta \end{array} \right] = \left[ \begin{array}{cc} \frac{R}{2} \cos \theta & \frac{R}{2} \cos \theta \\ \frac{R}{2} \sin \theta & \frac{R}{2} \sin \theta \\ \frac{R}{2L} & -\frac{R}{2L} \\ \end{array} \right] \left[ \begin{array}{c} \dot \varphi_R \\ \dot \varphi_L \end{array} \right] = \left[ \begin{array}{cc} \cos \theta & 0 \\ \sin \theta & 0 \\ 0 & 1 \\ \end{array} \right] \left[ \begin{array}{c} v_A \\ \omega \end{array} \right]. \end{align}

Simplified dynamic model

Modified 2018-06-22 by Andrea Censi

By implementing the kinematic constraints formulations derived above, i.e., the no lateral slipping (\eqref{eq:mod-no-lat-slip-final-dot}) and pure rolling (\eqref{eq:mod-force-and-torque}) in the general dynamic model, it is straightforward to obtain:

\begin{align} \label{eq:mod-dyn-model-a}\tag{32} \dot{v}_u (t) &= c \dot \theta^2(t) + \frac{1}{RM} (\tau_R(t)+\tau_L(t)) \\ \dot v_w(t) &= c \dot\theta(t) \\ \ddot \theta &= - \frac{Mc}{Mc^2+J} \dot\theta(t)v_u(t) + \frac{L}{R(Mc^2+J)}(\tau_R(t)-\tau_L(t)) \end{align}

DC motor dynamic model

Modified 2018-06-22 by Andrea Censi

The equations governing the behavior of a DC motor are driven by an input armature voltage $V(t)$:

\begin{align} V(t) &= Ri(t) + L \frac{di}{dt} + e(t) \\ e(t) &= K_b \dot\varphi(t) \\ \tau_m(t) &= K_t i(t) \\ \tau(t) &= N \tau_m(t), \end{align}

where $(K_b, K_t)$ are the back emf and torque constants respectively and $N$ is the gear ratio ($N=1$ in the Duckiebot).

Figure 1.8 shows a diagram of a typical DC motor.

Diagram of a DC motor

Having a relation between the applied voltage and torque, in addition to the dynamic and kinematic models of a differential drive robot, allows us to determine all possible state variables of interest.

torque disturbances acting on the wheels, such as the effects of friction, can be modeled as additive terms (of sign opposite to $\tau$) in the DC motor equations.

Conclusions

Modified 2018-06-22 by Andrea Censi

In this chapter we derived a model for a differential drive robot. Although several simplifying assumption were made, e.g., rigid body motion, symmetry, pure rolling and no lateral slipping - still the model is nonlinear.

Regardless, we now have a sequence of descriptive tools that receive as input the voltage signal sent by the controller, and produce as output any of the state variables, e.g., the position, velocity and orientation of the robot with respect to a fixed inertial frame.

Several outstanding questions remain. For example, we need to determine what is the best representation for our robotic platform - polar coordinates, Cartesian with respect to an arbitrary reference point? Or maybe there is a better choice?

Finally, the above model assumes the knowledge of a number of constants that are characteristic of the robot’s geometry, materials, and the DC motors. Without the knowledge of those constant the model could be completely off. Determination of these parameters in a measurement driven way, i.e., the “system identification” of the robot’s plant, is subject of the odometry class.

Because of mathjax bug

No questions found. You can ask a question on the website.