5.4.2. The Point Forward Steering Controller

The two other controllers discussed, Linear Time-Invariant Control Theory and The PID Controller, are very powerful, general purpose controllers. Unfortunately, they both require tuning that is difficult to do and is part science, art form and luck. Fortunately, for one of the main problems that we need to solve with mobile robots, there is a clever trick of trigonometry that creates a controller that works fairly well and is not difficult to tune. This controller may be used to provide Go To Angle behavior for a mobile robot using Differential Drive.

The Go To Angle behavior is one of the most basic mobile robot behaviors. It is a major component the Go To Goal behavior, but is useful for other objectives also. If the pose of the robot is oriented at \theta, but the The Unicycle Model calls for the robot to turn to angle \phi, the Go To Angle controller will move the robot forward while rotating it in the direction of \phi. The rotation needed is \alpha = \phi - \theta.

Rather than tracking the robot’s orientation and adjusting the rotational velocity (\omega) with a PID controller, this controller performs a direct transformation of the unicycle model so that the desired v and \omega can be directly calculated with a simple equation. The trick of this controller is that it considers a new point on the robot to calculate the needed Jacobian, and thus the needed wheel velocities. When we calculate the robot’s pose, we compute the position and orientation of the center of the robot X, which is the point half way between the two drive wheels. Because the robot can not move perpendicular to its wheels, the second term of The Jacobian is always zero. The robot moves in the direction of its orientation, \theta, but The Unicycle Model requires movement in the direction of \phi. This constraint limits our ability to directly apply The Unicycle Model in our control algorithm. We would like to take the derivative of X with respect to time and set it equal to the velocity vector \mathcal{U} from The Unicycle Model, but no v and \omega can satisfy such an equation.

\mathcal{U} = \left[ \begin{array}{c} u_x \\ u_y \\ \end{array} \right] = \left[ \begin{array}{l} V\,\cos \phi \\ V\,\sin \phi \end{array} \right] \neq \dot{X} = \left[ \begin{array}{l} v\,\cos \theta \\ v\,\sin \theta \end{array} \right]

However, such is not the case for a new point \tilde{X} = (\tilde{x}, \tilde{y}), which is positioned a small distance, l, directly in front of X. The point \tilde{X} can simultaneously move forward, perpendicular and rotate. X is close to \tilde{X} and once we align the robot with our target destination (\theta = \phi) then X will pass over the location of \tilde{X} shortly after \tilde{X} is at that location. Thus, as a close approximation to the ideal, we will design a controller to satisfy \mathcal{U} = \dot{\tilde{X}}.

../../_images/pointFwd_model.png

For now, do not worry about the value of l. After we complete the analysis, reasonable ranges for l will be obvious and computer simulations, can help us tune l. Note that this controller has only one variable to tune, which makes it easier to use than other controllers.

Warning

Beware of Angles

Angles can be tricky because they are linear between -\pi and \pi, yet they are circular in that -\pi and \pi are in fact the same angle. Another issue with angles is that the atan function (arctangent) requires division dy/dx, which is not defined when dx = 0. The solution is this is to use atan2 to compute angles and to wrap any linear angle calculations to the range \{-\pi \ to \ \pi\}.

To prevent a division by zero, atan2 takes two parameters: alpha = atan2(dy, dx)

Angle wrapping may be performed in either of two methods in traditional programming languages. This is what it would look like in Python:

alpha = math.atan2(math.sin(alpha), math.cos(alpha))

alpha = ((math.pi + alpha) % 2*math.pi) - math.pi

In MATLAB:

alpha = wrapToPi( alpha );

Note

Two things to never do in robot code with angles

  1. Use angles in degrees. All of the math functions return and expect angles in radians. As a general rule, only use degrees to accept and display angle measurements in the user interface. If degrees are used in the user interface, the user interface portion of the code should convert the angles to or from radians so that the robot code always works with angles in radians.
  2. Use the range of an angle as \{0 \ to \ 2\,\pi\}. The range of angles should always be \{-\pi \ to \ \pi\}.

5.4.2.1. Controller Derivation

Let us use X to define the location of \tilde{X}. Then we can take the derivative of \tilde{X} and set it equal to the desired velocity vector U from the unicycle model. We will use rectangular coordinates for the initial calculations.

\begin{array}{l} \tilde{x} = x + l\,\cos\,\theta \\ \tilde{y} = y + l\,\sin\,\theta \end{array}

Note

Derivatives of trigonometry functions where the angle is a function of time:

\begin{array}{l} \frac{d}{dt}\cos\,\theta(t) = -\frac{d\theta(t)}{dt}\,\sin\,\theta(t) \\ \\ \frac{d}{dt}\sin\,\theta(t) = \frac{d\theta(t)}{dt}\,\cos\,\theta(t) \end{array}

\begin{array}{l} \dot{\tilde{x}} = \dot{x} - l\,\dot{\theta}\,\sin\,\theta \\ \dot{\tilde{y}} = \dot{y} + l\,\dot{\theta}\,\cos\,\theta \end{array}

From Using Odometry to Track Robot Movement, we know that:

\begin{array}{l} \dot{x} = v\,\cos\,\theta \\ \dot{y} = v\,\sin\,\theta \\ \dot{\theta} = \omega \end{array}

By substitution:

\begin{array}{l} \dot{\tilde{x}} = v\,\cos\,\theta - l\omega\,\sin\,\theta \\ \dot{\tilde{y}} = v\,\sin\,\theta + l\omega\,\cos\,\theta \\ \end{array}

Note

Don’t confuse the names of the velocity variables. V_x is the forward velocity calculated from our odometry measurements. V is a constant value from the unicycle model. If the robot is driving straight, it is the robot’s velocity. With other controllers used to set the orientation angle, the forward velocity is not changed. With this controller, the forward velocity changes with time, so we call it v to indicate that it is a function of time (v(t)).

Now, we equate to \dot{\tilde{X}} to the desired velocity vector. We will also separate v and \omega into a matrix of it’s own since they are the variables that we want to solve for.

(5.3)\begin{array}{ll} \mathcal{U} & = \left[ \begin{array}{c} u_x \\ u_y \\ \end{array} \right] = \left[ \begin{array}{l} V\,\cos \phi \\ V\,\sin \phi \end{array} \right] \\ \\ & = \dot{\tilde{X}} = \underbrace{\left[ \begin{array}{rl} \cos\,\theta & -\sin\,\theta \\ \sin\,\theta & \cos\,\theta \end{array} \right] }_{R(\theta)} \left[ \begin{array}{cc} 1 & 0 \\ 0 & l \end{array} \right] \left[ \begin{array}{c} v \\ \omega \end{array} \right] \end{array}

To solve for v and \omega, we need to take the inverse of two of the above matrices to isolate [ v, \omega] on one side of the equality.

The matrix identified as R(\theta) is a known matrix with some special properties. It is called the rotation matrix. To rotate any point [x, y] by \theta radians counter clockwise, simply multiply the point by R(\theta). The inverse of R(\theta) has a nice property, R^{-1}(\theta) = R(-\theta). If multiplying R(\theta) rotates a point counter clockwise, then multiplying by R(-\theta) will do the inverse and rotate a point the same amount clockwise.

See also

Here is an easy to follow explanation of how to compute the inverse of a matrix: http://www.mathsisfun.com/algebra/matrix-inverse.html

\left[ \begin{array}{c} v \\ \omega \\ \end{array} \right] = \left[ \begin{array}{rr} \cos\,-\theta & -\sin\,-\theta \\ \sin\,-\theta & \cos\,-\theta \end{array} \right] \left[ \begin{array}{cc} 1 & 0 \\ 0 & 1/l \end{array} \right] \left[ \begin{array}{c} u_x \\ u_y \end{array} \right]

Note

Here are some trigonometry identities that the following equations will use:

\begin{array}{l} \cos\,-a = \cos\,a \\ \sin\,-a = -\sin\,a \\ \sin\,a\,\sin\,b = \frac{1}{2}(\cos(a-b) - \cos(a+b)) \\ \\ \cos\,a\,\cos\,b = \frac{1}{2}(\cos(a-b) + \cos(a+b)) \\ \\ \sin\,a\,\cos\,b = \frac{1}{2}(\sin(a-b) + \sin(a+b)) \\ \\ \cos\,a\,\sin\,b = \frac{1}{2}(\sin(a+b) - \sin(a-b)) \end{array}

By multiplying the above matrices and applying trigonometry identities, we get:

\begin{array}{l} v = u_x \cos\,\theta + u_y \sin\,\theta \\ \\ \omega = \frac{1}{l}(u_y \cos\,\theta - u_x \sin\,\theta) \end{array}

Replacing u_x and u_y with their values from the unicycle model gives us equations in terms of V, l, \theta and \phi. The trigonometry identities allow us to simplify the equations.

\begin{array}{rl} v & = V (\cos\,\phi\,\cos\,\theta + \sin\,\phi\,sin\,\theta) \\ & = \frac{V}{2}(2\,\cos(\phi-\theta) + \cos(\phi+\theta) - \cos(\phi+\theta)) \\ \\ \omega & = \frac{V}{l}(\sin\,\phi\,\cos\,\theta - \cos\,\phi\,\sin\,\theta) \\ & = \frac{V}{2l}(2 \sin(\phi-\theta) + \sin(\phi+\theta) - \sin(\phi+\theta)) \end{array}

Note

Finally:

(5.4)\begin{array}{l} v = V \cos(\phi - \theta) \\ \\ \omega = \frac{V}{l} \sin(\phi - \theta) \end{array}

The variables of the final control parameters resolve to \phi-\theta, which is the error between current orientation and the desired orientation angle. For convenience, we can call the error angle, \alpha = \phi-\theta. As always, when dealing with angles, take care to wrap \alpha to between -\pi and \pi.

5.4.2.2. Observations on the Controller

It is curious that this controller makes changes to v as well as to \omega. The PID controller for Go To Angle behavior only adjusts \omega. When v or \omega is zero is some concern to us.

  • When \alpha \in \{ 0, \pi \}, \omega = 0 and v = V or v = -V. At \alpha = 0, the robot is at the desired angle and it is driving forward as desired. At \alpha = \pi, the robot is driving in reverse. In some cases, it may be desired to drive in reverse to a goal. If it drove very long in reverse, it would likely eventually get off its reverse course and turn around. Depending on the application, it may be desired to force a turn around by putting tighter limits on the range of \alpha, perhaps between -3\pi/4 and 3\pi/4.
  • When \alpha \in \{ -\pi/2, \pi/2 \}, v = 0 and \omega = -V/l or \omega = V/l. Here, the robot will briefly pivot in place, causing \alpha to quickly change such that the robot will begin to move forward while continuing to rotate.

5.4.2.3. Wheel Velocities

From Calculating Wheel Velocities, we know how to calculate the velocities from v and \omega:

v_r = \frac{2\,v + \omega\,L}{2\,R}

v_l = \frac{2\,v - \omega\,L}{2\,R}

Substituting equation (5.4), we get:

v_r = \frac{V}{R}(\cos \alpha + \frac{L}{2\,l}\sin \alpha)

v_l = \frac{V}{R}(\cos \alpha - \frac{L}{2\,l}\sin \alpha)

Note

V has units of meters per second. v_r and v_l are in radians per second.

Note

  • In our programs it may be desired for wheel velocities to be a linear velocity, such as meters / second. So we may want to account for the wheel radius (R) in lower portions of the code, so that the R term may be removed from the equations.

  • The term \frac{L}{2\,l} can be regarded as a single tunable parameter, K, which acts as a turning gain constant in our programs. 1/2 \leq K \leq 1 seem reasonable.

  • In the global coordinate frame, \alpha = \phi-\theta. In the local coordinate frame, \alpha is the desired heading.

  • In our programs, our wheel velocity equations for the Go To Angle controller become:

    V_r(t) = V \, (\cos \alpha(t) + K\,\sin \alpha(t))

    V_l(t) = V \, (\cos \alpha(t) - K\,\sin \alpha(t))

The following graph shows the wheel velocities for K = 1 and V/R = 1. The only variable to tune is K (or l, if you prefer). Changes to K only changes the maximum velocity for each wheel and the exact angle for which a wheel might cross stop and change directions. The basic properties of the plots do not change.

../../_images/wheel_speeds.png

From the graph of the wheel velocities, a few observations can be made.

  • Except at the exact values of \alpha \in \{-\pi, 0, \pi\}, the robot is always turning towards the desired angle. (\theta \Rightarrow \phi and \alpha \Rightarrow 0).

    • v_r < v_l when -\pi < \alpha < 0
    • v_r > v_l when 0 < \alpha < \pi

  • The robot rotates around a stationary wheel at \alpha \in \{-3\pi/4, -\pi/4, \pi/4, 3\pi/4\}

  • The robot pivots, v_r = -v_l, at \alpha \in \{-\pi/2, \pi/2\}