Path Tracking

Stanley Method

Daniel Chia introduced this method to me.I implemented the Stanley controller, and it’s so much better at Path Tracking than Pure Pursuit.

Links

• Short video intro by UofT prof https://www.youtube.com/watch?v=9Y7kVIJs_JI&ab_channel=TaHaiDang
• http://ai.stanford.edu/~gabeh/papers/hoffmann_stanley_control07.pdf
• http://robots.stanford.edu/papers/thrun.stanley05.pdf

Implementation

• https://github.com/AtsushiSakai/PythonRobotics#stanley-control

Why is Stanley Method superior to Pure Pursuit?

Stanley takes path curvature into error correction. This is why Stanley control is better suited for higher speed driving when compared to Pure Pursuit.

With Pure Pursuit, you are only considering a single point ahead of you. You don’t consider what comes after that point. You simply care about how to reach that point. However, with Stanley Method, you also worry about the heading. So you compare you car’s heading with the heading of the target trajectory.

This way, you get better stability guarantees.

The equation is given by $\theta (t)=\varphi (t)+{\tan }^{-1}\left(\frac{K_e\ast e(t)}{v(t)}\right)$ where

• $\theta (t)$ is the steering angle
• $\varphi (t)$ is the heading error
  • $\varphi (t)=\text{path heading }-\text{current heading}$
• $e(t)$ is the crosstrack error
  • $K_e$ is the gain for the crosstrack error

My implementation:

...
    def drive_to_target_stanley(self):
        """
        Using the stanley method derivation. 
        
        Might get the best out of both worlds for responsiveness, and less oscillations.
        """
        # calculate curvature/steering angle
        closest_wheelbase_front_point_car, closest_wheelbase_front_point_world = self.pure_pursuit.get_waypoint_stanley(self.current_pose_wheelbase_front)
 
        path_heading = math.atan2(closest_wheelbase_front_point_world[1] - self.closest_wheelbase_rear_point[1], closest_wheelbase_front_point_world[0] - self.closest_wheelbase_rear_point[0])
        current_heading = math.atan2(self.current_pose_wheelbase_front.position.y - self.current_pose.position.y, self.current_pose_wheelbase_front.position.x - self.current_pose.position.x)
        
        if current_heading < 0:
            current_heading += 2 * math.pi
        if path_heading < 0:
            path_heading += 2 * math.pi
 
        # calculate the errors
        crosstrack_error = math.atan2(self.K_E * closest_wheelbase_front_point_car[1], self.target_velocity) # y value in car frame
        heading_error = path_heading - current_heading
        if heading_error > math.pi:
            heading_error -= 2 * math.pi
        elif heading_error < - math.pi:
            heading_error += 2 * math.pi
 
        heading_error *= self.K_H
 
        # Calculate the steering angle using the Stanley controller formula
        angle = heading_error + crosstrack_error
        angle = np.clip(angle, -np.radians(self.steering_limit), np.radians(self.steering_limit))
        velocity = self.target_velocity * self.velocity_percentage