stanley 路径跟踪控制算法

写在前面的话

轨迹跟踪 Trajectory Tracking 和 路径跟踪 Path Following 是机器人控制和自动驾驶领域中的两个核心概念，尽管它们都涉及让系统沿预定路径运动，但目标和方法存在显著差异。以下是它们的区别及实际应用场景的分析：

本文介绍的 stenley 控制算法是一个路径跟踪算法，是根据前轮的转向来控制车子按指定路径行驶，下图来自国防科技大学的论文《Ribbon model based path tracking method for autonomous ground vehicles》。

算法思路

上图是我按自己思路画出的示意图

图案	作用
黑色小箭头	发布的路径信息（posestamped数据，包括坐标和方向）
白色大箭头	航向对齐后前轮方向
绿色大箭头	最终的前轮方向
蓝色大箭头	初始前轮方向
θ1	小横向偏角
θ2	大横向偏角

理解 stanley 控制算法最核心的是分为两步，一个是摆正航向，一个是贴近路径。摆正航向可以理解车子可以跟着路径平行运动，贴近路径可以理解为让两个路径的平行的运行重合。

参考链接：Stanley理论篇

核心代码

1 路径发布

这里路径的发布很简单，路径点的数据格式用的是 PoseStamped ，需要点的坐标和方向（四元数表示），下面代码设置的是一个直线路径。

注意：要设置参考系是 world，因为本文的环境是在 gazebo 的 world 中进行仿真

import rclpy
import csv
import os
import numpy as np
from rclpy.node import Node
from launch_ros.substitutions import FindPackageShare
from tf_transformations import quaternion_from_euler
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path

class PathPublisher(Node):
    def __init__(self):
        super().__init__('path_publisher')
        # Initializing the titles and rows list
        YY = -9.5
        self.path = [[5,YY,0,0,0,0],
                     [7,YY,0,0,0,0], 
                     [9,YY,0,0,0,0], 
                     [11,YY,0,0,0,0], 
                     [13,YY,0,0,0,0]]
        self.direction = 0
        self.path_points = 1

        # Create path publisher
        self.publisher_ = self.create_publisher(Path, '/path_points', 10)
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)

        self.i = 0
    def timer_callback(self):
        path = Path()

        for i in range(len(self.path)):
            points = PoseStamped()
            points.header.frame_id = 'world'
            row = self.path[i]#[(self.i + i) % self.path_points]
            points.pose.position.x = float(row[0])
            points.pose.position.y = float(row[1])
            points.pose.position.z = float(self.direction)
            x,y,z,w = quaternion_from_euler(row[3],row[4],row[5])
            points.pose.orientation.x = float(x)
            points.pose.orientation.y = float(y)
            points.pose.orientation.z = float(z)
            points.pose.orientation.w = float(w)
            path.poses.append(points)

        path.header.frame_id = 'world'
        self.publisher_.publish(path)
        self.get_logger().info('Publishing point no %f: %f'%(self.i % self.path_points, float(self.path[self.i % self.path_points][0])))

def main(args=None):
    rclpy.init(args=args)
    minimal_subscriber = PathPublisher()
    rclpy.spin(minimal_subscriber)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2 获取车子当前位置

在之前的车子控制中选用的阿克曼前轮转向的设置，在gazebo中会发布模型的 tf 话题信息，话题里面包含 tf/odom_demo/base_footprint 信息，可以查看车子的 pose 参数（ xyz 和 rpy ）

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.timer = self.create_timer(1.0, self.lookup_transform)
  =======================================================================================     
   def lookup_transform(self):
       try:
           self.last_time = self.curr_time
           trans = self.tf_buffer.lookup_transform(
               'odom_demo', 'base_footprint', rclpy.time.Time())
           # Orientation
           self.curr_qw = trans.transform.rotation.w
           self.curr_qx = trans.transform.rotation.x
           self.curr_qy = trans.transform.rotation.y
           self.curr_qz = trans.transform.rotation.z
           # Position
           R = quaternion_rotation_matrix([self.curr_qw, self.curr_qx, self.curr_qy, self.curr_qz])
           forward_dir = R[:2, 0]  # 提取X轴（前进方向）
           self.curr_x = trans.transform.translation.x #+ 0.5*forward_dir[0] #位置提前可以提前转弯
           self.curr_y = trans.transform.translation.y #+ 0.5*forward_dir[1]
           self.curr_z = trans.transform.translation.z
           self.get_logger().info(f"Translation: {trans.transform.translation}")
           self.get_logger().info(f"Rotation: {trans.transform.rotation}")
           # Time
           self.curr_time = time.time()

       except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
           self.get_logger().warn("Could not find global pose of car!!")

3 预瞄路径点

代码原理是计算路径所有点的坐标和车子的坐标的欧式距离，最小的就是预备的预瞄路径点 ref_point
如果预设的最大预瞄距离（self.lookahead_distance）小于预备的预瞄路径点到车子的距离，就选用当前的预瞄路径点；
但是如果预测的最大预瞄距离大于预备的预瞄路径点到车子的距离，就选择路径的下一个点，如果都大于
就选择路径最后的一个点。

     ref_points = np.array([[p[0], p[1]] for p in self.ref_path])
     curr_pose = np.array([self.curr_x, self.curr_y])
     nearest_idx = np.argmin(np.linalg.norm(ref_points - curr_pose, axis=1))
     # Find the lookahead point on the reference trajectory 找最近点前面的点
     for i in range(nearest_idx, len(self.ref_path)):
         ref_point = np.array([self.ref_path[i][0], self.ref_path[i][1]])
         if np.linalg.norm(ref_point - curr_pose) > self.lookahead_distance:
             lookahead_point = ref_point
             lpn_idx = i
             break
     else:
         lookahead_point = ref_points[-1]
         lpn_idx = -1

4 计算航向误差

航向误差就是车子当前的航向和当前预瞄路径点的方向之间的偏差，通过向量叉积的符号明确车辆与路径的相对位置，适用于任意方向的航向和路径指向。

    def _yaw(self, q_vehicle, q_path):# 转换为方向向量
        vehicle_yaw = self.quat_to_yaw(q_vehicle)
        path_yaw = self.quat_to_yaw(q_path)

        vehicle_dir = np.array([np.cos(vehicle_yaw), np.sin(vehicle_yaw)])
        path_dir = np.array([np.cos(path_yaw), np.sin(path_yaw)])
        # 计算有符号偏角
        dot = np.dot(vehicle_dir, path_dir)
        cross = vehicle_dir[0] * path_dir[1] - vehicle_dir[1] * path_dir[0]
        alpha = np.arctan2(cross, dot)
        # print(f"偏角: {np.degrees(alpha):.1f} 度")
        return alpha
        
     def quat_to_yaw(self, q):
        [qw, qx, qy, qz] = q
        """四元数转偏航角（弧度）"""
        t3 = 2.0 * (qw * qz + qx * qy)
        t4 = 1.0 - 2.0 * (qy**2 + qz**2)
        return np.arctan2(t3, t4)
 ===========================================================================
R_trackj = [self.ref_path[lpn_idx][5], self.ref_path[lpn_idx][2], 
            self.ref_path[lpn_idx][3], self.ref_path[lpn_idx][4]]#轨迹方向
R_car = [self.curr_qw, self.curr_qx, self.curr_qy, self.curr_qz]
yaw = self._yaw(R_car, R_trackj)

5 计算横向误差

这个横向误差是车子到预瞄路径点的航向的距离，简单理解就是点到线的距离，进一步可以理解成车子相对于预瞄路径点航向坐标系的y轴坐标，航向误差最终要得到转向角delta

参考链接： 利用向量推导坐标旋转公式(方案一)

    def calculate_lateral_error(self, x, y, path_x, path_y, path_yaw):
        # 计算到路径点切线的垂直距离
        dx = path_x - x
        dy = path_y - y
        return dx * np.sin(path_yaw) + dy * np.cos(path_yaw)
===========================================================================================
ex =  self.calculate_lateral_error(self.curr_x, self.curr_y, lookahead_point[0], lookahead_point[1],
                                   quat2eulers(R_trackj)[-1]) # 横向误差
self.speed = 0.2
delta2 = np.arctan2(ex, self.lookahead_distance) #       

完整控制代码

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, Twist,  Point, TransformStamped
import tf2_ros
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from nav_msgs.msg import Path
import math
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, QoSDurabilityPolicy
import numpy as np
import time
from tf_transformations import quaternion_from_euler


def quat2eulers(q) -> tuple:
    """
    Compute yaw-pitch-roll Euler angles from a quaternion.
    @author: michaelwro
    Args
    ----
        q0: Scalar component of quaternion. [qw]
        q1, q2, q3: Vector components of quaternion. [qx, qy, qz]
    
    Returns
    -------
        (roll, pitch, yaw) (tuple): 321 Euler angles in radians
    """
    [q0, q1, q2, q3] = q
    roll = math.atan2(
        2 * ((q2 * q3) + (q0 * q1)),
        q0**2 - q1**2 - q2**2 + q3**2
    )  # radians
    pitch = math.asin(2 * ((q1 * q3) - (q0 * q2)))
    yaw = math.atan2(
        2 * ((q1 * q2) + (q0 * q3)),
        q0**2 + q1**2 - q2**2 - q3**2
    )
    return (roll, pitch, yaw)
def quaternion_rotation_matrix(Q):
    """
    Covert a quaternion into a full three-dimensional rotation matrix.
 
    Input
    :param Q: A 4 element array representing the quaternion (q0,q1,q2,q3) 
 
    Output
    :return: A 3x3 element matrix representing the full 3D rotation matrix. 
             This rotation matrix converts a point in the local reference 
             frame to a point in the global reference frame.
    """
    # Extract the values from Q
    q0 = Q[0]
    q1 = Q[1]
    q2 = Q[2]
    q3 = Q[3]
     
    # First row of the rotation matrix
    r00 = 2 * (q0 * q0 + q1 * q1) - 1
    r01 = 2 * (q1 * q2 - q0 * q3)
    r02 = 2 * (q1 * q3 + q0 * q2)
     
    # Second row of the rotation matrix
    r10 = 2 * (q1 * q2 + q0 * q3)
    r11 = 2 * (q0 * q0 + q2 * q2) - 1
    r12 = 2 * (q2 * q3 - q0 * q1)
     
    # Third row of the rotation matrix
    r20 = 2 * (q1 * q3 - q0 * q2)
    r21 = 2 * (q2 * q3 + q0 * q1)
    r22 = 2 * (q0 * q0 + q3 * q3) - 1
     
    # 3x3 rotation matrix
    rot_matrix = np.array([[r00, r01, r02],
                           [r10, r11, r12],
                           [r20, r21, r22]])
                            
    return rot_matrix
    
def quaternion_multiply(q1, q2):
    """四元数乘法（Hamilton约定）"""
    [w1, x1, y1, z1] = q1
    [w2, x2, y2, z2] = q2
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    ])

def rotation_angles(matrix, order):
    """
    input
        matrix = 3x3 rotation matrix (numpy array)
        oreder(str) = rotation order of x, y, z : e.g, rotation XZY -- 'xzy'
    output
        theta1, theta2, theta3 = rotation angles in rotation order
    """
    r11, r12, r13 = matrix[0]
    r21, r22, r23 = matrix[1]
    r31, r32, r33 = matrix[2]

    if order == 'xzx':
        theta1 = np.arctan(r31 / r21)
        theta2 = np.arctan(r21 / (r11 * np.cos(theta1)))
        theta3 = np.arctan(-r13 / r12)

    elif order == 'xyx':
        theta1 = np.arctan(-r21 / r31)
        theta2 = np.arctan(-r31 / (r11 *np.cos(theta1)))
        theta3 = np.arctan(r12 / r13)

    elif order == 'yxy':
        theta1 = np.arctan(r12 / r32)
        theta2 = np.arctan(r32 / (r22 *np.cos(theta1)))
        theta3 = np.arctan(-r21 / r23)

    elif order == 'yzy':
        theta1 = np.arctan(-r32 / r12)
        theta2 = np.arctan(-r12 / (r22 *np.cos(theta1)))
        theta3 = np.arctan(r23 / r21)

    elif order == 'zyz':
        theta1 = np.arctan(r23 / r13)
        theta2 = np.arctan(r13 / (r33 *np.cos(theta1)))
        theta3 = np.arctan(-r32 / r31)

    elif order == 'zxz':
        theta1 = np.arctan(-r13 / r23)
        theta2 = np.arctan(-r23 / (r33 *np.cos(theta1)))
        theta3 = np.arctan(r31 / r32)

    elif order == 'xzy':
        theta1 = np.arctan(r32 / r22)
        theta2 = np.arctan(-r12 * np.cos(theta1) / r22)
        theta3 = np.arctan(r13 / r11)

    elif order == 'xyz':
        theta1 = np.arctan(-r23 / r33)
        theta2 = np.arctan(r13 * np.cos(theta1) / r33)
        theta3 = np.arctan(-r12 / r11)

    elif order == 'yxz':
        theta1 = np.arctan(r13 / r33)
        theta2 = np.arctan(-r23 * np.cos(theta1) / r33)
        theta3 = np.arctan(r21 / r22)

    elif order == 'yzx':
        theta1 = np.arctan(-r31 / r11)
        theta2 = np.arctan(r21 * np.cos(theta1) / r11)
        theta3 = np.arctan(-r23 / r22)

    elif order == 'zyx':
        theta1 = np.arctan(r21 / r11)
        theta2 = np.arctan(-r31 * np.cos(theta1) / r11)
        theta3 = np.arctan(r32 / r33)

    elif order == 'zxy':
        theta1 = np.arctan(-r12 / r22)
        theta2 = np.arctan(r32 * np.cos(theta1) / r22)
        theta3 = np.arctan(-r31 / r33)

    theta1 = theta1 * 180 / np.pi
    theta2 = theta2 * 180 / np.pi
    theta3 = theta3 * 180 / np.pi

    return (theta1, theta2, theta3)

class StanleyControllerNode(Node):
    def __init__(self):
        super().__init__('stanley_controller')
        self.get_logger().info('Stanley Controller start!')
        # Current pose
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.timer = self.create_timer(1.0, self.lookup_transform)
        # self.current_pose_subscription = self.create_subscription(PoseStamped,'/ground_truth/pose', self.current_pose_listener_callback, 10)
        # Position
        self.curr_x = None
        self.curr_y = None
        self.curr_z = None
        # Orientation
        self.curr_qw = None
        self.curr_qx = None
        self.curr_qy = None
        self.curr_qz = None
        # Time
        self.curr_time = None
        # Last position and time
        self.last_time = None
        self.last_x = None
        self.last_y = None
        # 
        # Reference trajectory
        self.reference_trajectory_subscription = self.create_subscription(Path, '/path_points', self.reference_trajectory_listener_callback, 10)
        # Position
        self.ref_path = None
        self.ref_side = None
        # Control publisher
        self.control_publisher = self.create_publisher(Twist, '/car_nav2/cmd_demo', 10)
        control_publisher_timer_period = 1/50  # seconds
        self.control_publisher_timer = self.create_timer(control_publisher_timer_period, self.control_publisher_timer_callback)
        control_timer = 0.1 # seconds
        self.control_timer = self.create_timer(control_timer, self.control_timer_callback)
        # Steering angle
        self.theta = None 
        # speed
        self.speed = None 
        # Controller parameter
        self.K = 0.5
        self.last_u = 0
        self.lookahead_distance = 1.0
    
    def lookup_transform(self):
        try:
            self.last_time = self.curr_time
            trans = self.tf_buffer.lookup_transform(
                'odom_demo', 'base_footprint', rclpy.time.Time())
            # Orientation
            self.curr_qw = trans.transform.rotation.w
            self.curr_qx = trans.transform.rotation.x
            self.curr_qy = trans.transform.rotation.y
            self.curr_qz = trans.transform.rotation.z
            # Position
            R = quaternion_rotation_matrix([self.curr_qw, self.curr_qx, self.curr_qy, self.curr_qz])
            forward_dir = R[:2, 0]  # 提取X轴（前进方向）
            self.curr_x = trans.transform.translation.x #+ 0.5*forward_dir[0] #位置提前可以提前转弯
            self.curr_y = trans.transform.translation.y #+ 0.5*forward_dir[1]
            self.curr_z = trans.transform.translation.z
            self.get_logger().info(f"Translation: {trans.transform.translation}")
            self.get_logger().info(f"Rotation: {trans.transform.rotation}")
            # Time
            self.curr_time = time.time()
 
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            self.get_logger().warn("Could not find global pose of car!!")

    # def current_pose_listener_callback(self, msg:PoseStamped):
    #     # Previous
    #     self.last_time = self.curr_time
    #     self.last_x = self.curr_x
    #     self.last_y = self.curr_y
    #     # Position
    #     self.curr_x = msg.pose.position.x
    #     self.curr_y = msg.pose.position.y
    #     self.curr_z = msg.pose.position.z
    #     # Orientation
    #     self.curr_qw = msg.pose.orientation.w
    #     self.curr_qx = msg.pose.orientation.x
    #     self.curr_qy = msg.pose.orientation.y
    #     self.curr_qz = msg.pose.orientation.z
    #     # Time
    #     self.curr_time = msg.header.stamp.nanosec

    def reference_trajectory_listener_callback(self, msg:Path):
        self.ref_path = []
        for pose in msg.poses:
            x = pose.pose.position.x
            y = pose.pose.position.y
            self.ref_side = pose.pose.position.z
            qx = pose.pose.orientation.x
            qy = pose.pose.orientation.y
            qz = pose.pose.orientation.z
            qw = pose.pose.orientation.w
            self.ref_path.append([x, y, qx, qy, qz, qw])

    def publish_control(self, theta, speed):
        vel_msg = Twist()   
        vel_msg.linear.x = speed
        vel_msg.angular.z = theta #左转是正数，车轮右转是负数
        self.control_publisher.publish(vel_msg)

    def control_publisher_timer_callback(self):
        if (self.theta is not None) and (self.speed is not None):
            self.publish_control(self.theta, self.speed)
            self.get_logger().info(f'Controller output: theta: {self.theta}, speed: {self.speed}')
        else:
            self.get_logger().info(f'Stanley Controller wrong control!')

    def control_timer_callback(self):
        # Calculate control
        if (self.ref_path is not None) and (self.curr_x is not None) and (self.last_time is not None):

            # If the robot reaches the goal, reset is_goal_pose_received 到达目标（距离小于0.05）
            goal_point = np.array([self.ref_path[-1][0], self.ref_path[-1][1]])
            curr_xy = np.array([self.curr_x, self.curr_y])
            if np.linalg.norm(curr_xy - goal_point) < 0.1:
                self.theta = 0.0
                self.speed = 0.0
                self.get_logger().info(f'Reached goal. Goal pose reset')
                return
            
            # 横向误差
            ref_points = np.array([[p[0], p[1]] for p in self.ref_path])
            curr_pose = np.array([self.curr_x, self.curr_y])
            nearest_idx = np.argmin(np.linalg.norm(ref_points - curr_pose, axis=1))
            # Find the lookahead point on the reference trajectory 找最近点前面的点
            for i in range(nearest_idx, len(self.ref_path)):
                ref_point = np.array([self.ref_path[i][0], self.ref_path[i][1]])
                if np.linalg.norm(ref_point - curr_pose) > self.lookahead_distance:
                    lookahead_point = ref_point
                    lpn_idx = i
                    break
            else:
                lookahead_point = ref_points[-1]
                lpn_idx = -1

            R_trackj = [self.ref_path[lpn_idx][5], self.ref_path[lpn_idx][2], 
                        self.ref_path[lpn_idx][3], self.ref_path[lpn_idx][4]]#轨迹方向
            R_car = [self.curr_qw, -self.curr_qx, -self.curr_qy, -self.curr_qz]#车辆当前方向 四元数共轭
            W = quaternion_multiply(R_car,  R_trackj)[0]
            yaw = -2*np.arccos(np.clip(abs(W), 0.0, 1.0))

            ex =  self.calculate_lateral_error(self.curr_x, self.curr_y, lookahead_point[0], lookahead_point[1],
                                               quat2eulers(R_trackj)[-1]) # 横向误差
            self.speed = 0.2
            delta2 = np.arctan2(ex, self.lookahead_distance) #           
            
            self.get_logger().info(f'e: {ex}, delta: {delta2}, yaw: {yaw}')
            self.theta = np.clip((yaw + delta2), -0.5, 0.5)
    
    def calculate_lateral_error(self, x, y, path_x, path_y, path_yaw):
        # 计算到路径点切线的垂直距离
        dx = path_x - x
        dy = path_y - y
        return -dx * np.sin(path_yaw) + dy * np.cos(path_yaw)
            
def main(args=None):
    rclpy.init(args=args)
    StanleyController = StanleyControllerNode()
    rclpy.spin(StanleyController)
    StanleyController.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

演示视频

stanley 控制算法（阿克曼前轮转向小车）