寫在前面的話

軌跡跟蹤 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()

演示視頻