基于分段五次多项式对路径平滑

文章预览

点击上方链接，关注公众号，了解更多信息。 全局路径分段参数化表示 为了对全局路径进行分段优化，我们将全局路径划分为𝑛段（图 1.1），各段路径的路点(𝑥, 𝑦)分别由五次参数样条表示： 图1.1 全局路径   其中，𝑠表示路段的弧长变量（𝑠 ∈ [0, 𝑠𝑖]，𝑠𝑖为各段路径的弧长），𝑎𝑖,𝑗、𝑏𝑖,𝑗表示路段𝑖的第𝑗个多项式系数（𝑖 ∈ {1,2, … , 𝑛}，𝑗 ∈ {0,1,2,3,4,5}）。由上式可知，当各段路径的多项式确定后,即可得到全局路径的参数化表示。将式(1.1)、(1.2)转换为矩阵形式： 令：    则各段路径组合得到的全局路径参数样条函数状态变量𝑢为： （2）定义优化目标函数 为了达到使全局路径趋于平滑的目的，我们通过控制各参数样条函数的一阶 导数、二阶导数实现路径平滑的效果，因此，对各路段的参数样条建立如下优化 ………………………………