写在前面的话

轨迹跟踪 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 Pathclass PathPublisher(Node):def __init__(self):super().__init__('path_publisher')# Initializing the titles and rows listYY = -9.5self.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 = 0self.path_points = 1# Create path publisherself.publisher_ = self.create_publisher(Path, '/path_points', 10)timer_period = 0.1self.timer = self.create_timer(timer_period, self.timer_callback)self.i = 0def 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_timetrans = self.tf_buffer.lookup_transform('odom_demo', 'base_footprint', rclpy.time.Time())# Orientationself.curr_qw = trans.transform.rotation.wself.curr_qx = trans.transform.rotation.xself.curr_qy = trans.transform.rotation.yself.curr_qz = trans.transform.rotation.z# PositionR = 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.zself.get_logger().info(f"Translation: {trans.transform.translation}")self.get_logger().info(f"Rotation: {trans.transform.rotation}")# Timeself.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_pointlpn_idx = ibreakelse: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 alphadef 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 - xdy = path_y - yreturn 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_eulerdef quat2eulers(q) -> tuple:"""Compute yaw-pitch-roll Euler angles from a quaternion.@author: michaelwroArgs----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] = qroll = math.atan2(2 * ((q2 * q3) + (q0 * q1)),q0**2 - q1**2 - q2**2 + q3**2)  # radianspitch = 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 Qq0 = Q[0]q1 = Q[1]q2 = Q[2]q3 = Q[3]# First row of the rotation matrixr00 = 2 * (q0 * q0 + q1 * q1) - 1r01 = 2 * (q1 * q2 - q0 * q3)r02 = 2 * (q1 * q3 + q0 * q2)# Second row of the rotation matrixr10 = 2 * (q1 * q2 + q0 * q3)r11 = 2 * (q0 * q0 + q2 * q2) - 1r12 = 2 * (q2 * q3 - q0 * q1)# Third row of the rotation matrixr20 = 2 * (q1 * q3 - q0 * q2)r21 = 2 * (q2 * q3 + q0 * q1)r22 = 2 * (q0 * q0 + q3 * q3) - 1# 3x3 rotation matrixrot_matrix = np.array([[r00, r01, r02],[r10, r11, r12],[r20, r21, r22]])return rot_matrixdef quaternion_multiply(q1, q2):"""四元数乘法（Hamilton约定）"""[w1, x1, y1, z1] = q1[w2, x2, y2, z2] = q2return 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):"""inputmatrix = 3x3 rotation matrix (numpy array)oreder(str) = rotation order of x, y, z : e.g, rotation XZY -- 'xzy'outputtheta1, 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.pitheta2 = theta2 * 180 / np.pitheta3 = theta3 * 180 / np.pireturn (theta1, theta2, theta3)class StanleyControllerNode(Node):def __init__(self):super().__init__('stanley_controller')self.get_logger().info('Stanley Controller start!')# Current poseself.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)# Positionself.curr_x = Noneself.curr_y = Noneself.curr_z = None# Orientationself.curr_qw = Noneself.curr_qx = Noneself.curr_qy = Noneself.curr_qz = None# Timeself.curr_time = None# Last position and timeself.last_time = Noneself.last_x = Noneself.last_y = None# # Reference trajectoryself.reference_trajectory_subscription = self.create_subscription(Path, '/path_points', self.reference_trajectory_listener_callback, 10)# Positionself.ref_path = Noneself.ref_side = None# Control publisherself.control_publisher = self.create_publisher(Twist, '/car_nav2/cmd_demo', 10)control_publisher_timer_period = 1/50  # secondsself.control_publisher_timer = self.create_timer(control_publisher_timer_period, self.control_publisher_timer_callback)control_timer = 0.1 # secondsself.control_timer = self.create_timer(control_timer, self.control_timer_callback)# Steering angleself.theta = None # speedself.speed = None # Controller parameterself.K = 0.5self.last_u = 0self.lookahead_distance = 1.0def lookup_transform(self):try:self.last_time = self.curr_timetrans = self.tf_buffer.lookup_transform('odom_demo', 'base_footprint', rclpy.time.Time())# Orientationself.curr_qw = trans.transform.rotation.wself.curr_qx = trans.transform.rotation.xself.curr_qy = trans.transform.rotation.yself.curr_qz = trans.transform.rotation.z# PositionR = 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.zself.get_logger().info(f"Translation: {trans.transform.translation}")self.get_logger().info(f"Rotation: {trans.transform.rotation}")# Timeself.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.nanosecdef reference_trajectory_listener_callback(self, msg:Path):self.ref_path = []for pose in msg.poses:x = pose.pose.position.xy = pose.pose.position.yself.ref_side = pose.pose.position.zqx = pose.pose.orientation.xqy = pose.pose.orientation.yqz = pose.pose.orientation.zqw = pose.pose.orientation.wself.ref_path.append([x, y, qx, qy, qz, qw])def publish_control(self, theta, speed):vel_msg = Twist()   vel_msg.linear.x = speedvel_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 controlif (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.0self.speed = 0.0self.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_pointlpn_idx = ibreakelse:lookahead_point = ref_points[-1]lpn_idx = -1R_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.2delta2 = 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 - xdy = path_y - yreturn -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()

演示视频