目录

一、Python实现（推荐方法）

1.1代码cubic_spline_path.py

1.2使用方法

 二、C++实现

参考博客

---

想让机器人/智能车无人驾驶，要有期望路径，最简单的是一条直线，或者是一条光滑曲线。

生成路径的方法有两种：

1. 手动遥控机器人运动一段路径，并保存轨迹
2. 人为设定几个关键点，拟合出直线或曲线

方法1通常是根据GNSS/INS位姿数据记录轨迹，可参考ROS之rviz显示GNSS/INS运动轨迹_可见一班的博客-CSDN博客

方法2是这篇博客讨论的内容。分别使用C++和Python在ROS下实现。

一、Python实现（推荐方法）

1.1代码cubic_spline_path.py

根据指定的插值算法 alg，创建一个插值函数 cubic_spline。如果 alg 为 “linear”，则使用线性插值函数 interp1d 创建插值函数；如果 alg 为 “cubic”，则使用三次样条插值函数 interp1d 创建插值函数。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-import rospy
import rosparam
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
import sys
import os
import yaml
import numpy as np
from scipy.interpolate import interp1d# def interpolation(waypoint_list, alg="cubic"):
def interpolation(alg="linear"):ix = []iy = []temp_x = np.array([1,5,8,10])temp_y = np.array([1,4,9,10])cubic_spline = Noneif alg == "linear":cubic_spline = interp1d(temp_x, temp_y)elif alg == "cubic":cubic_spline = interp1d(temp_x, temp_y, kind='cubic')waypoint_x_start = temp_x[0]waypoint_x_end = temp_x[-1]length = int(abs(waypoint_x_end - waypoint_x_start) / 0.01)print(length)ix = np.linspace(waypoint_x_start, waypoint_x_end, num=length)iy = cubic_spline(ix)return ix, iyif __name__ == '__main__':rospy.init_node("cubic_spline_path")waypoint_x, waypoint_y = interpolation()path_pub = rospy.Publisher('/spline_path', Path, queue_size=10)path = Path()for index in range(len(waypoint_x)):pose = PoseStamped()pose.pose.position.x = waypoint_x[index]pose.pose.position.y = waypoint_y[index]print(pose)path.poses.append(pose)path.header.frame_id = "map"path.header.stamp = rospy.Time.now()rate = rospy.Rate(10)while not rospy.is_shutdown():path_pub.publish(path)rate.sleep()rospy.spin()

1.2使用方法

1. 在ROS工作空间中新建文件夹scripts
2. 新建.py文件，复制上述代码
3. 更改文件权限允许作为程序执行
4. 终端执行 ./cubic_spline_path.py
5. 打开rviz，订阅spline_path话题显示路径

 

 二、C++实现

 直线插值比较简单，三次样条插值比较复杂。

主函数中调用这个函数即可，传入参数为A、B点x，y坐标值，返回nav_msgs::Path路径。

// 线性插值路径
nav_msgs::Path LinearInterpolation(const double &Ax, const double &Ay, const double &Bx, const double &By, const double &stepsize)
{double x1 = Ax;double y1 = Ay;double z1 = 0;double x2 = Bx;double y2 = By;double z2 = 0;double step_size = stepsize;double dist = std::sqrt(std::pow((x2 - x1), 2) + std::pow((y2 - y1), 2) + std::pow((z2 - z1), 2));int num_steps = dist / step_size;for (int i = 0; i < num_steps; ++i){double ratio = static_cast<double>(i) / num_steps;geometry_msgs::PoseStamped interpolated_pose;interpolated_pose.header.frame_id = "map";interpolated_pose.header.stamp = ros::Time::now();interpolated_pose.pose.position.x = x1 + ratio * (x2 - x1);interpolated_pose.pose.position.y = y1 + ratio * (y2 - y1);interpolated_pose.pose.position.z = z1 + ratio * (z2 - z1);interpolated_pose.pose.orientation.x = 1;interpolated_pose.pose.orientation.y = 0;interpolated_pose.pose.orientation.z = 0;interpolated_pose.pose.orientation.w = 0;scatter_path.poses.push_back(interpolated_pose);}return scatter_path;
}

参考博客

【运动规划算法项目实战】路径规划中常用的插值方法（附ROS C++代码）_路径插值

【运动规划算法项目实战】如何使用Pure Pursuit算法进行路径跟踪（附ROS C++代码）