Pure-Pursuit 跟踪双移线 Gazebo 仿真

主要参考学习下面的博客和开源项目

自动驾驶规划控制（Ａ*、pure pursuit、LQR算法，使用c++在ubuntu和ros环境下实现）

https://github.com/NeXTzhao/planning

Pure-Pursuit 的理论基础见今年六月份的笔记

对参考轨迹进行调整，采用双移线轨迹

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include <fstream>
using namespace std;

// 双移线构造的参数
const float shape = 2.4;
const float dx1 = 25.0, dx2 = 21.95;
const float dy1 = 4.05, dy2 = 5.7;
const float Xs1 = 27.19, Xs2 = 56.46;
// 参考路径在 X 方向长度以及参考点的步长
const float length = 120.0;
const float step = 0.1;

// 计算 Y 轴参考位置
inline float calculate_reference_y(const float ref_x)
{
    float z1 = shape / dx1 * (ref_x - Xs1) - shape / 2.0;
    float z2 = shape / dx2 * (ref_x - Xs2) - shape / 2.0;
    return dy1 / 2.0 * (1 + tanh(z1)) - dy2 / 2.0 * (1 + tanh(z2));
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "DoubleLane");

    ros::NodeHandle nh;
    ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("/double_lane", 1000, true);

    nav_msgs::Path reference_path;
    reference_path.header.frame_id = "world";
    reference_path.header.stamp = ros::Time::now();

    geometry_msgs::PoseStamped pose;
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "world";

    int points_size = length / step;
    reference_path.poses.resize(points_size + 1);
    for (int i = 0; i <= points_size; ++i)
    {
        float ref_x = i * step;
        float ref_y = calculate_reference_y(ref_x);
        // cout << ref_x << "\t" << ref_y << endl;
        pose.pose.position.x = ref_x;
        pose.pose.position.y = ref_y;
        pose.pose.position.z = 0.0;
        pose.pose.orientation.x = 0.0;
        pose.pose.orientation.y = 0.0;
        pose.pose.orientation.z = 0.0;
        pose.pose.orientation.w = 0.0;
        reference_path.poses[i] = pose;
    }

    ros::Rate loop(10);
    while (ros::ok())
    {
        path_pub.publish(reference_path);
        ros::spinOnce();
        loop.sleep();
    }

    return 0;
}

编程方面进行了一些简单的优化，轨迹跟踪的算法在 poseCallback 中实现，和博主有所区别

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include "cpprobotics_types.h"
#include "cubic_spline.h"
#include "geometry_msgs/PoseStamped.h"

#define PREVIEW_DIS 1.5 // 预瞄距离

#define Ld 1.868 // 轴距

using namespace std;
using namespace cpprobotics;

ros::Publisher purepersuit_;
ros::Publisher path_pub_;
nav_msgs::Path path;

float carVelocity = 0;
float preview_dis = 0;
float k = 0.1;

// 计算四元数转换到欧拉角
std::array<float, 3> calQuaternionToEuler(const float x, const float y,
                                          const float z, const float w)
{
    std::array<float, 3> calRPY = {(0, 0, 0)};
    // roll = atan2(2(wx+yz),1-2(x*x+y*y))
    calRPY[0] = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
    // pitch = arcsin(2(wy-zx))
    calRPY[1] = asin(2 * (w * y - z * x));
    // yaw = atan2(2(wx+yz),1-2(y*y+z*z))
    calRPY[2] = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));

    return calRPY;
}

cpprobotics::Vec_f r_x_;
cpprobotics::Vec_f r_y_;

int pointNum = 0; // 保存路径点的个数
int targetIndex = pointNum - 1;

// 计算发送给模型车的转角
void poseCallback(const geometry_msgs::PoseStamped &currentWaypoint)
{
    auto currentPositionX = currentWaypoint.pose.position.x;
    auto currentPositionY = currentWaypoint.pose.position.y;
    auto currentPositionZ = 0.0;

    auto currentQuaternionX = currentWaypoint.pose.orientation.x;
    auto currentQuaternionY = currentWaypoint.pose.orientation.y;
    auto currentQuaternionZ = currentWaypoint.pose.orientation.z;
    auto currentQuaternionW = currentWaypoint.pose.orientation.w;

    std::array<float, 3> calRPY =
        calQuaternionToEuler(currentQuaternionX, currentQuaternionY,
                             currentQuaternionZ, currentQuaternionW);

    cout << currentPositionX << "\t" << currentPositionY << "\t" << calRPY[2] << endl;

    for (int i = pointNum - 1; i >= 0; --i)
    {
        float distance = sqrt(pow((r_x_[i] - currentPositionX), 2) +
                              pow((r_y_[i] - currentPositionY), 2));
        if (distance < preview_dis)
        {
            targetIndex = i + 1;
            break;
        }
    }

    if (targetIndex >= pointNum)
    {
        targetIndex = pointNum - 1;
    }

    float alpha =
        atan2(r_y_[targetIndex] - currentPositionY, r_x_[targetIndex] - currentPositionX) -
        calRPY[2];

    // 当前点和目标点的距离Id
    float dl = sqrt(pow(r_y_[targetIndex] - currentPositionY, 2) +
                    pow(r_x_[targetIndex] - currentPositionX, 2));

    // 发布小车运动指令及运动轨迹
    if (targetIndex == pointNum - 1 && dl < 0.2) // 离终点很近时停止运动
    {
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0;
        vel_msg.angular.z = 0;
        purepersuit_.publish(vel_msg);
    }
    else
    {
        float theta = atan(2 * Ld * sin(alpha) / dl);
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 3;
        vel_msg.angular.z = theta;
        purepersuit_.publish(vel_msg);
        // 发布小车运动轨迹
        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped.pose.position.x = currentPositionX;
        this_pose_stamped.pose.position.y = currentPositionY;

        geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(theta);
        this_pose_stamped.pose.orientation.x = currentQuaternionX;
        this_pose_stamped.pose.orientation.y = currentQuaternionY;
        this_pose_stamped.pose.orientation.z = currentQuaternionZ;
        this_pose_stamped.pose.orientation.w = currentQuaternionW;

        this_pose_stamped.header.stamp = ros::Time::now();

        this_pose_stamped.header.frame_id = "world";
        path.poses.push_back(this_pose_stamped);
    }

    path_pub_.publish(path);
}

void velocityCall(const geometry_msgs::TwistStamped &carWaypoint)
{
    carVelocity = carWaypoint.twist.linear.x;
    // 预瞄距离计算
    preview_dis = k * carVelocity + PREVIEW_DIS;
}

void pointCallback(const nav_msgs::Path &msg)
{
    // 避免参考点重复赋值
    if (pointNum != 0)
    {
        return;
    }

    // geometry_msgs/PoseStamped[] poses
    pointNum = msg.poses.size();

    // 提前开辟内存
    r_x_.resize(pointNum);
    r_y_.resize(pointNum);
    for (int i = 0; i < pointNum; i++)
    {
        r_x_[i] = msg.poses[i].pose.position.x;
        r_y_[i] = msg.poses[i].pose.position.y;
    }
}
int main(int argc, char **argv)
{
    // 创建节点
    ros::init(argc, argv, "pure_pursuit");

    // 创建节点句柄
    ros::NodeHandle n;
    // 创建Publisher，发送经过pure_pursuit计算后的转角及速度
    purepersuit_ = n.advertise<geometry_msgs::Twist>("/smart/cmd_vel", 20);

    path_pub_ = n.advertise<nav_msgs::Path>("/rvizpath", 100, true);
    // ros::Rate loop_rate(10);

    path.header.frame_id = "world";
    // 设置时间戳
    path.header.stamp = ros::Time::now();
    geometry_msgs::PoseStamped pose;
    pose.header.stamp = ros::Time::now();
    // 设置参考系
    pose.header.frame_id = "world";

    ros::Subscriber splinePath = n.subscribe("/double_lane", 20, pointCallback);
    ros::Subscriber carVel = n.subscribe("/smart/velocity", 20, velocityCall);
    ros::Subscriber carPose = n.subscribe("/smart/rear_pose", 20, poseCallback);
    ros::spin();
    return 0;
}

这里和 CarSim-Simulink 联合仿真的代码类似

function [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
%   该函数是写的第3个S函数控制器(MATLAB版本：R2011a)
%   限定于车辆运动学模型，控制量为速度和前轮偏角，使用的QP为新版本的QP解法
%   [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
%
% is an S-function implementing the MPC controller intended for use
% with Simulink. The argument md, which is the only user supplied
% argument, contains the data structures needed by the controller. The
% input to the S-function block is a vector signal consisting of the
% measured outputs and the reference values for the controlled
% outputs. The output of the S-function block is a vector signal
% consisting of the control variables and the estimated state vector,
% potentially including estimated disturbance states.

switch flag,
 case 0
  [sys,x0,str,ts] = mdlInitializeSizes; % Initialization
  
 case 2
  sys = mdlUpdates(t,x,u); % Update discrete states
  
 case 3
  sys = mdlOutputs(t,x,u); % Calculate outputs

 case {1,4,9} % Unused flags
  sys = [];
  
 otherwise
  error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.

%==============================================================
% Initialization
%==============================================================

function [sys,x0,str,ts] = mdlInitializeSizes
% Call simsizes for a sizes structure, fill it in, and convert it 
% to a sizes array.
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 4; % this parameter doesn't matter
sizes.NumOutputs     = 1;
sizes.NumInputs      = 5;
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes); 
x0 =[0.00001;0.00001;0.00001;0.00001];   
global U; % store current ctrl vector:[vel_m, delta_m]
U=[0];

global cx;
cx = 0:0.01:160;
global cy;
shape=2.4;%参数名称，用于参考轨迹生成
dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称
dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称
Xs1=27.19;Xs2=56.46;%参数名称
for i = 1:length(cx)                      %全局路径c(y)生成 路径初始化
    z1=shape/dx1*(cx(i)-Xs1)-shape/2;
    z2=shape/dx2*(cx(i)-Xs2)-shape/2;
    cy(i) = dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2));
end

% Initialize the discrete states.
str = [];             % Set str to an empty matrix.
ts  = [0.05 0];       % sample time: [period, offset]
%End of mdlInitializeSizes
		      
%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u)
  sys = x;
%End of mdlUpdate.

%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs(t,x,u)
    global U; %store chi_tilde=[vel-vel_ref; delta - delta_ref]
    global cx;
    global cy;
    pi = 3.1415926;
    
    tic
    fprintf('Update start, t=%6.3f\n',t);
    x = u(1);
    y = u(2);
    yaw_angle =u(3)*pi/180;%CarSim输出的Yaw angle为角度，角度转换为弧度
    v = u(4) / 3.6;

    k = 0.1;                                  % look forward gain  前向预测距离所用增益
    Lfc = 3;                                  % 基础预瞄距离
    L = 2.7;                                % [m] wheel base of vehicle
    Ld = k * v + Lfc;
    
    N =  length(cx);
    ind = N;
    for i = N : -1 : 1
        distance = sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
        if distance < Ld
            ind = i + 1;
            break;
        end
    end
    
    if ind > N
       ind = N; 
    end
   
    tx = cx(ind);
    ty = cy(ind);
    Ld = sqrt((tx-x)^2 + (ty-y)^2);
    alpha = atan((ty-y)/(tx-x))-yaw_angle;       %该处定义向左转为alpha=beta-Fai，所以向右转就输出-alpha
    delta = atan(2*L * sin(alpha)/Ld);                %前轮转角
    U = delta;
    sys= U; % vel, steering, x, y
    toc
% End of mdlOutputs.