Gazebo 跟踪8字形和U形轨迹(1) — 错误处理

整个过程还是比较曲折的，主要都是一些细小的问题，跑了很多遍模型才发现

参考轨迹生成问题不大，主要是参考横摆角和参考曲率部分有问题

atan和atan2

首先看下两者的区别

atan 函数：

• atan 函数是简单的反正切函数，接受一个参数（通常是 y/x）
• 在 MATLAB 中，atan 函数的语法是 atan(Y/X)，其中 Y 和 X 是两个数值
• 返回值的范围是 (-π/2, π/2)

atan2 函数：

• atan2 函数是带有两个参数的反正切函数，接受两个参数（y 和 x）
• 在 MATLAB 中，atan2 函数的语法是 atan2(Y, X)，其中 Y 和 X 是两个数值
• 返回值的范围是 (-π, π]，因此它能够提供更广泛的角度范围，而且能够正确处理 x 和 y 同时为零的情况

---

四象限反正切 atan2(Y,X) 基于图形中所示的 Y 和 X 的值返回闭区间 [-pi,pi] 中的值

之前的路径规划时用 atan 计算参考路径点的横摆角，以圆形轨迹为例

clc;
clear all;

% [xr, yr, thetar, kr] = buildDoubleLane(150);
% [xr, yr, thetar, kr] = buildPassLane(20,2,30);
% [xr, yr, thetar, kr] = buildCurveEight(1000,10,10);
% [xr, yr, thetar, kr] = buildUShapePath(10,5,300);
[xr, yr, thetar, kr] = buildCircle(10,1000);

%% 圆形轨迹
function [xr, yr, thetar, kr] = buildCircle(radius,nums)
    t = linspace(0,2*pi,nums);
    xr = radius*cos(t);
    yr = radius*sin(t);
    dx_dt = -radius*sin(t);
    dy_dt = radius*cos(t);
    % 计算偏航角
    thetar = atan2(dy_dt,dx_dt);
    % 计算曲率
    kr = abs(dx_dt.*dy_dt)./(dx_dt.^2 + dy_dt.^2).^(3/2);
end

用 atan 函数所有点的参考横摆角如下图

用 atan2 函数所有点的参考横摆角如下图

考虑到移动机器人实际运动中横摆角范围就是[-pi,pi]，因此参考路径也采用 atan2 函数

另一方面，Pure-Pursuit 中计算 alpha 时需要减去当前横摆角，因此当前位置与目标点间夹角的计算也应采用 atan2 函数

alpha = atan2((ty-y),(tx-x))-phi;       %该处定义向左转为alpha=beta-Fai，所以向右转就输出-alpha

单位换算

中午测试 stanley 测试出来的，真的是很细小的问题，感觉之前 LQR 跟踪超车轨迹跟这个也有关系

PP、Stanley、LQR 都进行了 km/h → m/s，以及 deg→rad，但是 Gazebo 输出的状态信息都是换算过的，再换算相当于进行了很大的缩小，导致跟踪效果变差甚至跟踪失败