Pure-Pursuit 跟踪五次多项式轨迹

考虑双移线轨迹 X 轴方向位移较大，机械楼停车场长度无法满足 100 ~ 120 m，因此采用五次多项式进行轨迹规划，在轨迹跟踪部分也能水一些内容

调整 double_lane.cpp 为 ref_lane.cpp，结合 FrenetPath 类将参考路径的构造抽离为单独的函数

#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>
#include <Eigen/Geometry>#include "cpprobotics_types_double.h"
#include "frenet_path_double.h"
#include "quintic_polynomial_double.h"using namespace std;
using namespace cpprobotics;// 双移线参考路径在 X 方向长度以及参考点的步长
const float length = 120.0;
const float step = 0.1;// 五次多项式轨迹参数
#define DT 0.01
const double T = 50.0; // t-t0经历的时间
const double xend = 25.0;
const double yend = 3.0;array<float, 4> calEulerToQuaternion(const float roll, const float pitch, const float yaw)
{array<float, 4> calQuaternion = {0.0f, 0.0f, 0.0f, 1.0f}; // 初始化四元数// 使用Eigen库来进行四元数计算Eigen::Quaternionf quat;quat = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) *Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());calQuaternion[0] = quat.x();calQuaternion[1] = quat.y();calQuaternion[2] = quat.z();calQuaternion[3] = quat.w();return calQuaternion;
}FrenetPath fp;
void calc_frenet_paths()
{// 起始状态std::array<double, 3> x_start{0.0, 0.0, 0.0};std::array<double, 3> x_end{xend, 0.0, 0.0};// 终点状态std::array<double, 3> y_start{0.0, 0.0, 0.0};std::array<double, 3> y_end{yend, 0.0, 0.0};// 纵向QuinticPolynomial lon_qp(x_start[0], x_start[1], x_start[2], x_end[0],x_end[1], x_end[2], T);// 横向QuinticPolynomial lat_qp(y_start[0], y_start[1], y_start[2], y_end[0],y_end[1], y_end[2], T, xend);for (double t = 0; t < T; t += DT){double x = lon_qp.calc_point_x(t);double xd = lon_qp.calc_point_xd(t);double xdd = lon_qp.calc_point_xdd(t);fp.t.emplace_back(t);fp.x.emplace_back(x);fp.x_d.emplace_back(xd);fp.x_dd.emplace_back(xdd);double y_x_t = lat_qp.calc_point_y_x(x);double y_x_d = lat_qp.calc_point_y_x_d(x);double y_x_t_d = lat_qp.calc_point_y_t_d(y_x_d, xd);double y_x_dd = lat_qp.calc_point_y_x_dd(x);double y_x_t_dd = lat_qp.calc_point_y_t_dd(y_x_dd, xd, y_x_d, xdd);fp.y.emplace_back(y_x_t);fp.y_d.emplace_back(y_x_t_d);fp.y_dd.emplace_back(y_x_t_dd);// 压入航向角// fp.threat.emplace_back(lat_qp.calc_point_thetar(y_x_t_d, xd));// 压入曲率fp.k.emplace_back(lat_qp.calc_point_k(y_x_dd, y_x_d));// fp.k.emplace_back(lat_qp.calc_point_k(y_x_t_dd, y_x_t_d, xdd, xd));}int num = fp.x.size();for (int i = 0; i < num; i++){double dy = fp.y[i + 1] - fp.y[i];double dx = fp.x[i + 1] - fp.x[i];fp.threat.emplace_back(lat_qp.calc_point_thetar(dy, dx));}// 最后一个道路航向角和前一个相同// fp.threat.push_back(fp.threat.back());
}void double_lane_path()
{// 双移线构造的参数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;int points_size = length / step;fp.x.resize(points_size);fp.y.resize(points_size);fp.threat.resize(points_size);fp.k.resize(points_size);for (int i = 0; i <= points_size; ++i){// 计算参考路径点信息float ref_x = i * step;float z1 = shape / dx1 * (ref_x - Xs1) - shape / 2.0;float z2 = shape / dx2 * (ref_x - Xs2) - shape / 2.0;float ref_y = dy1 / 2.0 * (1 + tanh(z1)) - dy2 / 2.0 * (1 + tanh(z2));float ref_phi = atan(pow(dy1 * (1 / cosh(z1)), 2) * (1.2 / dx1) - pow(dy2 * (1 / cosh(z2)), 2) * (1.2 / dx2));float y_dot = dy1 / 2 * pow(1 / cosh(z1), 2) * (shape / dx1) - dy2 / 2 * pow(1 / cosh(z2), 2) * (shape / dx2);float y_ddot = -2 * dy1 * ((cosh(z1) - 1) / pow(cosh(z1), 3)) * pow(shape / dx1, 2) + 2 * dy2 * ((cosh(z2) - 1) / pow(cosh(z2), 3)) * pow(shape / dx2, 2);float ref_k = abs(y_ddot) / pow(1 + y_dot * y_dot, 1.5);fp.x[i] = ref_x;fp.y[i] = ref_y;fp.threat[i] = ref_phi;fp.k[i] = ref_k;}
}int main(int argc, char *argv[])
{ros::init(argc, argv, "ref_lane");ros::NodeHandle nh;ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("/ref_path", 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";// 五次多项式轨迹calc_frenet_paths();// 双移线轨迹// double_lane_path();int points_size = fp.x.size();reference_path.poses.resize(points_size);for (int i = 0; i < points_size; ++i){        cout << fp.x[i] << "," << fp.y[i] << "," << fp.threat[i] << endl;// 计算四元数位姿array<float, 4> calQuaternion = calEulerToQuaternion(0.0, 0.0, fp.threat[i]);pose.pose.position.x = fp.x[i];pose.pose.position.y = fp.y[i];pose.pose.position.z = 0.0;pose.pose.orientation.x = calQuaternion[0];pose.pose.orientation.y = calQuaternion[1];pose.pose.orientation.z = calQuaternion[2];pose.pose.orientation.w = calQuaternion[3];reference_path.poses[i] = pose;}ros::Rate loop(10);while (ros::ok()){path_pub.publish(reference_path);ros::spinOnce();loop.sleep();}return 0;
}

跟踪过程如下

跟踪效果如下

这里只分析出横向跟踪误差，因为 y 与 x 的关系可以直接获得，yaw 与 x 的关系没有直接的表达式

y = 0.00000000 + 0.00000000 * x + 0.00000000 * x^2 + 0.00192000 * x^3 + -0.00011520 * x^4 + 0.00000184 * x^5

💡 需要完善对于五次多项式的学习