matlab quadprog_Matlab路径平滑实现

场地自动驾驶一般需要录入地图，地图在采样的过程中难免会存在噪声，不宜直接作为路径输入到自动驾驶控制器中。怎么样将一堆离散的不平滑的采样点转化成平滑的路径是值得研究的问题。

首先我们做出如下建模，假设x和y分别满足下列方程，其中s为路径路径长度和

柳梦璃：MATLAB路径规划算法中提到的FrenetSerretSystem坐标系是一回事。

下面定义一下优化的目标如下，其中l为待优化路径长度，

和

是两个系数。

假设我们采样到到的点坐标为

，在此点的s坐标为

，我们不想让求得的路径离实际采样的点太远，所以我们有以下不等式约束，其中

和

是两个参数。

以及考虑到完整路径一般很长，无法用一段曲线来平滑整个路径，所以要将整个路径分成一段段较短的路径段。在两个段连接的地方我们希望能达到曲率的连续，曲率公式见下：

所以就要达到二阶导数的连续，当然也可以做出更高的要求，例如我们要求三阶导数连续，所得的等式约束可见下式。

至此，我们有了优化目标函数，优化所需的等式约束和不等式约束，下面来展开一下

。

同理可求得：

所有的准备工作都完成了，但是实际上使用这种方法过程中可能存在问题。因为一段路径终点的一阶导数和二阶导数是不受约束的可能产生不合理的情形，带入下一段路径中就可能带来优化效果非常差的结果，而且这会继续传递到下一段，以至于优化失败。可见下图：

由此，在算法中需要作出一些处理，我给出的处理方法比较简单，可见下图（调皮下，如果各位论文中引用记得给我加到引文列表哈）。

使用改进的方法后得到的结果比较好的，有效避免了一段终点的导数不受约束的情况，因为现在一段的终点是上一段的中点，中点两边都会有大量的点来牵制此点的曲率，不让其太过不合理。

那么，我们首先看一下结果，最后我会给出实现的代码。

上图为处理过的采样点，初看并不会觉得太过不平滑，但是我们可以看一下他的航向角情况，下图是它航向角随s的变化情况。

平滑后为5段连接的五次曲线，连接处满足曲率连续。

下面看一下实现代码：

---

samples 

这一段代码生成原始采样点，实际上就是一个圆弧，但是加入了噪声。

ref_points 

接着对原始采样点做出初步处理，第一步是将采样点再采样，原始采样点太过密集，所以再次以0.5的间隔采样一次。接着对所有点进行分段，每段长度为4米。

function

上面为对点再采样的函数。

function

上面是对路径进行分割的函数。

figure

画一下处理过后的采样点和采样点处的航向角。接着我们需要对第一段路径进行平滑，第一段不需要等式约束，注释掉的部分是调用另外一种优化器的代码，但是使用fmincon求解该问题发现其对于约束处理不是太好，所以最后使用了quadprog来进行优化求解。forcesingle会强制使用一段优化法（原始方法），这里禁用了而使用改进的算法。

%% fit every span to 5 order polynominal

这里IneqFcns给出不等式约束：

function

而EqFcns给出的是等式约束，这里也可以看出待优化的参数为[a0,a1,a2,a3,a4,a5,b0,b1,b2,b3,b4,b5]。

function

HMatrix就是二次型的系数矩阵，之前给出了求解方法了，所以这里是直接带入求解的结果的：

function

最后就是循环求解第2段到最后一段的系数了：

pl 

其中QPSolver就是将求解问题的几个必要步骤包装了一下而已。

function

最后是一些无关紧要的绘图代码，给出平滑过后的曲线。

%% 

当然这里给出的代码仅供参考，而且不能选择曲线的次数，做的好一点可以选择平滑曲线的次数，例如不用5次而用6次曲线，也可以选择段之间的连续次数例如要求达到4阶连续而不是这里定死的3阶。