最近一个师弟问我关于机器人路径生成的问题,我也考虑这个问题很长时间了。去年做机器人比赛时就把机器人路径生成规划和存储跟随等这些功能实现了,但是当时因为没接触到三次样条曲线,所以路径函数的生成是用了比较笨的方法。最近接触到了三次样条曲线,刚好实现机器人路径生成的要求。正好师弟他们也要用,写出来也许有用。
我是根据李庆阳的《数值分析》这本教材中的讲解编写的程序,使用的是第一边界条件,用追赶法求解了M矩阵。
为了调用方便,我将整个函数所有的信息构造成一个结构体,输入插值点的坐标和数量,定义边界条件后,将这个结构体的指针作为参数传给Spline3()函数,就完成了函数计算,计算结果也存储在该结构体中。
程序如下:
/*=======================================================================*/
//=====================三次样条插值的函数S(x)实现=============================
// 创 建 人: 汪雅楠
// 北京交通大学 QQ312818820
// 说 明: 根据研究生教材《数值分析》(李庆杨)第51页~第56页编写
/* 初始条件: 1. 已知两端的一阶导数值
2. 已知两端的二阶导数值