开源代码参考文章:
Minimum jerk / Minimum snap后端轨迹优化_jerk snap-CSDN博客
Github地址:
GitHub - MEZHANGYUE/MyTrajectoryGeneration
算法原理参考文章:
【附源码和详细的公式推导】Minimum Snap轨迹生成,闭式求解Minimum Snap问题,机器人轨迹优化,多项式轨迹路径生成与优化-CSDN博客
路径规划-Minimum snap轨迹优化_路径规划算法中如何提高路径的平滑度-CSDN博客
一、代码架构详解
代码主体包含了以下文件:
头文件:
planner.h
trajectory_generation.h
cpp文件:
main.cpp
planner.cpp
trajectory_generation.cpp
1、头文件trajectory_generation.h
声明了TrajectoryGenerationTool类,
其中包含了求解轨迹多项式系数矩阵的函数SolveQPClosedForm()。
2、cpp文件trajectory_generation.cpp
主要是求解轨迹多项式系数的函数:
说明注释如下:
最终返回值为轨迹多项式系数矩阵PolyCoeff。
此处可以将轨迹多项式系数矩阵cout出来,或者发布话题。
3、头文件planner.h:
声明了planner类,
类中包含了处理系数矩阵、生成轨迹、发布轨迹话题所需要的函数。
getcoeff(), trajectory_path(), tra_publish(), getparm(), getPosPoly()。
4、cpp文件planner.cpp:
函数tra_publish(),发布轨迹话题。
5、cpp文件main.cpp:
初始化节点,
调用planner,发送出话题。
二、运行方法
编译:
cd MyTrajectoryGeneration
catkin_make
报错:缺少Eigen/Dense
用以下指令安装:
sudo apt install libeigen3-dev
安装完成后,编译器会去 /usr/local/include 或者 /usr/include 目录找头文件,但找到的是eigen3,并没有Eigen和unsupported,因此需要建立一个软连接,链接到这两个文件夹即可
#首先要确定你的Eigen3安装在/usr/local/include还是/usr/include文件夹
cd /usr/include
sudo ln -sf eigen3/Eigen Eigen
sudo ln -sf eigen3/unsupported unsupported
再次编译,成功。
config.yaml文件用于设置轨迹约束(路径点、每段轨迹时间)
运行如下指令运行程序:
roslaunch Trajectory_Planner planner.launch
运行结果:
使用以下指令查看轨迹话题:
rostopic echo /tra_generation