#### 13.3.4. Feedback Control for Nonholonomic Mobile Robots

This video introduces feedback stabilization of a planned trajectory for a nonholonomic wheeled mobile robot.

Once we've planned a trajectory for a nonholonomic wheeled mobile robot, we need a feedback controller to track the trajectory. Although feedback control to a stationary configuration requires a control law that's time-varying, or, discontinuous in the configuration, as we learned in an earlier video, feedback control to a trajectory is "easier."

Feedback control requires an estimate of the configuration of the chassis. Such an estimate can be maintained using odometry, as discussed in the next video. But typically odometry is augmented with external sensors, like laser rangefinders, cameras, cameras with depth sensors, or GPS. This video does not address the estimation process but assumes that a configuration estimate is available.

The configuration of the robot is represented by its heading angle phi and the position (x,y) of a point midway between the wheels in the case of a diff-drive robot and a point midway between the rear wheels in the case of a car. Since there are three configuration variables but only two controls, it's not possible to independently control the rate of change of all three configuration variables. Instead, we could choose a point P fixed to the chassis and use the two controls to control the velocity of this point.

The position of the point P in the space frame is given by the (x,y) position of the chassis plus the vector to the point P expressed in the space frame. The velocity of the point P is simply the time derivative of its position. To control the motion of the point P to follow a desired trajectory, we could use a proportional controller, which says that the velocity of the point is proportional to the position error. The commanded velocity of the point, (x_P-dot, y_P-dot), can be converted to the linear velocity v and the angular velocity omega of the chassis. Note that x_r, the x-position of the point P in the body frame, must be nonzero to be able to move the point P in arbitrary linear directions.

The constraints on v and omega depend on whether the robot is a unicycle, diff-drive, or car. When initially planning the trajectory, the planner should use only a subset of the possible controls, such as those shown here. By doing so, the feedback controller has some extra control authority to make corrections to errors in trajectory tracking.

Assuming a point P on the mid-line of the robot, this is an example straight-ahead trajectory of the chassis, and the corresponding trajectory followed by the point P. At any given point on the trajectory, the robot can satisfy the desired location of P with different orientations of the chassis, but only one of the orientations shown is consistent with the path before and after this point. Therefore, even though the robot only follows the trajectory of the point P, this motion will often tend to align the robot's orientation to the desired orientation.

For example, if this is the initial configuration of the robot, then proportional control of the point P will drive the full configuration of the robot to the desired trajectory. Notice that the robot's final configuration is a little behind the desired configuration. To reduce that error, we could add an integral term or a feedforward term, as discussed in Chapter 11.

Here is another planned trajectory and the actual initial configuration of the robot. Proportional control of the point P causes the point to converge toward the planned trajectory, but the controller causes the robot to execute a direction reversal. So even though the point P converges to the desired trajectory, the final orientation of the chassis is off by nearly 180 degrees. In other words, just tracking the planned trajectory of a point P does not always result in good tracking of the full chassis trajectory.

To track the full configuration of the chassis, we define the configuration of the frame {b}, midway between the wheels of the robot, as phi, x, y, and the desired configuration at any instant is given by the frame {d}. The error coordinates are phi_e, x_e, and y_e. With these error coordinates, we have a number of potential choices for the control law; one example is the feedforward plus feedback nonlinear controller shown here. It's called a nonlinear controller because it's nonlinear in the error coordinates. The derivation of this controller and the choice of the control gains k_1, k_2, and k_3 are beyond the scope of this video, and I suggest you consult the references in the book. But notice that if the error reduces to zero, the commanded controls reduce to the feedforward controls. Also, examining the control law shows that the heading error should be less than pi over 2 and the linear velocity of the planned motion should be nonzero. In other words, this control law is not a good choice for stabilizing a trajectory that simply spins in place.

Here is a planned trajectory for the robot, and this is the actual initial configuration, with nonzero error. The controller brings the robot to the desired trajectory, as shown in these error plots as a function of time. The error in all configuration variables is driven to zero.

In the next video we'll discuss how to maintain an estimate of the chassis configuration using wheel encoder data. This is called odometry.