目錄

一、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++代碼）