Pure-Pursuit 跟踪双移线 Gazebo 仿真

news2024/11/8 17:41:47

Pure-Pursuit 跟踪双移线 Gazebo 仿真

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

自动驾驶规划控制(A*、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.

注意处理接近终点的情况,不加限制的话容易出现绕着终点转圈的现象

在这里插入图片描述

限制后整体的跟踪效果尚可,但在终点处仍旧会出现异常的偏航,仍有较大的优化空间

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/1164623.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

如何在 Endless OS 上安装 ONLYOFFICE 桌面编辑器 7.5

ONLYOFFICE 桌面编辑器是一款基于依据 AGPL v.3 许可进行分发的开源办公套件。使用这款应用&#xff0c;您无需保持网络连接状态即可处理存储在计算机上的文档。本指南会向您介绍&#xff0c;如何在 Endless OS 上安装 ONLYOFFICE 桌面编辑器 7.5。 ONLYOFFICE 桌面版是什么 O…

Ansible中的任务执行控制

循环 简单循环 {{item}} 迭代变量名称 loop: - value1 - value2 - ... //赋值列表{{item}} //迭代变量名称循环散列或字典列表 - name: create filehosts: host1tasks:- name: file moudleservice:name: "{{ item.name }}"state: "{{…

实验记录之——git push

平时做开发的时候经常push代码不成功&#xff0c;如下图 经好友传授经验&#xff0c;有如下方法 Win cmd使用Clash&#xff08;端口是7890&#xff09;代理操作&#xff0c;在cmd中输入&#xff1a; set http_proxy127.0.0.1:7890 set https_proxy127.0.0.1:7890Linux export …

防火墙日志记录和分析

防火墙监控进出网络的流量&#xff0c;并保护部署防火墙的网络免受恶意流量的侵害。它是一个网络安全系统&#xff0c;它根据一些预定义的规则监控传入和传出的流量&#xff0c;它以日志的形式记录有关如何管理流量的信息&#xff0c;日志数据包含流量的源和目标 IP 地址、端口…

可视化流程编排(Bpmn.js)介绍及实践

作者:罗强 为将内部系统打通并规范流程定义&#xff0c;基于统一的平台实现工单自动化流转,从而使用无界山工单系统帮助公司内部人员统一管理和处理来自企业内部提交的工单需求。而在系统中流程编排及节点设计主要是使用bpmn.js实现精细化配置。从而满足各种复杂的业务需求。目…

MapStruct的用法

提示&#xff1a;文章写完后&#xff0c;目录可以自动生成&#xff0c;如何生成可参考右边的帮助文档 文章目录 前言一、依赖导入二、简单使用2.1、定义转换的接口2.2、创建实体类2.3、测试2.4、底层实现 三、常用的注解3.1、Mapping(target "xxx1",source "x…

你还在用System.currentTimeMillis()打印代码执行时间?

文章目录 前言一、开发环境二、使用步骤1. 创建Springboot项目2. 引入hutool3. 使用TimeInterval 总结 前言 Hutool是一个小而全的Java工具类库&#xff0c;里面集成了很多实用的工具类&#xff0c;比如文件、流、加密解密、转码、正则、线程、XML等&#xff0c;通过这些工具类…

探索 Java 8 中的 Stream 流:构建流的多种方式

人嘛&#xff0c;要懂得避嫌… 开篇引入 Java 8引入了Stream流作为一项新的特性&#xff0c;它是用来处理集合数据的一种函数式编程方式。Stream流提供了一种更简洁、高效和易于理解的方法来操作集合数据&#xff0c;同时也能够实现并行处理&#xff0c;以提高性能。 以下是St…

在 GORM 中定义模型

为实现与数据库的无缝交互而打造有效模型的全面指南 在使用 GORM 进行数据库管理时&#xff0c;定义模型是基础。模型是您的应用程序的面向对象结构与数据库的关系世界之间的桥梁。本文深入探讨了在 GORM 中打造有效模型的艺术&#xff0c;探讨如何创建结构化的 Go 结构体&…

第十二章,集合类例题

例题1 package 例题;import java.util.*;public class 例题 {public static void main(String[] args) {// TODO Auto-generated method stub//实例化集合类对象Collection<String> list new ArrayList<>();//调用方法&#xff0c;向集合添加数据list.add("…

Java——java.time包使用方法详解

Java——time包使用方法详解 java.time 包是 Java 8 引入的新日期和时间 API&#xff08;JSR 310&#xff09;&#xff0c;用于替代旧的 java.util.Date 和 java.util.Calendar 类。它提供了一组全新的类来处理日期、时间、时间间隔、时区等&#xff0c;具有更好的设计和易用性…

什么是消息队列

什么是消息队列 消息队列是一种通信机制&#xff0c;用于在不同的应用程序或组件之间传递消息。它允许应用程序之间异步地发送和接收消息&#xff0c;而无需直接依赖彼此的可用性或性能。消息队列通常用于解耦不同组件&#xff0c;提高系统的可伸缩性和可维护性&#xff0c;以…

【Python入门一】Python及PyCharm安装教程

Python及PyCharm安装教程 1 Python简介1.1 Python下载及安装 2 PyCharm简介2.1 PyCharm下载及安装 参考 1 Python简介 Python是一种开源的高级编程语言&#xff0c;由Guido van Rossum于1991年创建。Python易于学习、阅读和编写&#xff0c;具有丰富的标准库和第三方模块&…

智慧校园管理云平台电子班牌系统源码,数据管理、信息发布、家校互通、物联控制、教务管理、日常办公、人脸识别

智慧校园平台源码 智慧校园云平台电子班牌系统源码 一款全功能智慧校园管理云平台电子班牌系统源码&#xff0c;融合了多媒体信息发布、家校互通、物联控制、教务管理、日常办公等一系列应用&#xff0c;是校园管理的现代手段。 电子班牌系统的主要功能包括&#xff1a;数据信…

电源管理(PMIC)MAX20428ATIA/VY、MAX20428ATIC/VY、MAX20428ATIE/VY适合汽车ADAS应用的开关稳压器

一、概述 MAX20428是一款高效率、八路输出、低压PMIC。OUT1将输入电源升压至5V&#xff0c;电流高达500mA&#xff0c;而三个同步降压转换器的输入电压范围为3.0V至4.2V&#xff0c;输出电压范围为0.8V至3.9875V&#xff0c;峰值电流分别高达1.3A、1.3A和3.5A。三个300mA pMOS…

c++实现建造者模式

代码 director.h #pragma once #include<vector> #include<string> #include<iostream>class Product { private:std::vector<std::string> _parts;public:void Add(const std::string& part) {_parts.push_back(part);}void show(){std::cout &…

L1和L2正则化通俗理解

机器学习中&#xff0c;如果参数过多&#xff0c;模型过于复杂&#xff0c;容易造成过拟合&#xff08;overfit&#xff09;。即模型在训练样本数据上表现的很好&#xff0c;但在实际测试样本上表现的较差&#xff0c;不具备良好的泛化能力。为了避免过拟合&#xff0c;最常用的…

在Linux上通过NTLM认证连接到AD服务器(未完结)

这篇文章目前还没有实现具体的功能&#xff0c;只实现了明文登录&#xff0c;因为我缺少一些数据&#xff0c;比如通过密码生成hash&#xff0c;以及通过challenge生成response&#xff0c;我不知道怎么实现&#xff0c;因此这篇文章也是一个交流的文章&#xff0c;希望大佬看见…

Linux的test测试功能

测试文件名的类型&#xff0c;文件是否存在&#xff0c; 文件的权限检测 文件之间的比较 两个整数之间的比较 判断字符串数据 多重条件判定 一个一个来&#xff0c;这个有点多&#xff0c;不过比较有意思&#xff0c;来代码 案例1&#xff0c;判断文件是否存在&#xff…

VS Code提取扩展时出错。XHR failed

需求&#xff1a;想要在扩展中心下载插件&#xff0c;发现报错 原因&#xff1a;vs code之前设置了代理&#xff0c;需要删除即可