MPC（模型预测控制）基本原理

	MPC实际上将对小车的运动控制问题转化成为了一个带约束的非线性优化问题。

	考虑一个最简化的模型，即将小车看做平面上质点，则小车具有一系列状态组分：位置P，朝向与x轴夹角theta，速度值v。可优化变量：角速度值omega，朝向方向加速度a。

	MPC模型运行的前提是已生成全局路径，且知道当前的上述三个状态。

	定义cost函数。cost组成成分包括：
	1.距离偏差。根据上述状态值预测出的局部路径与全局路径ROI之间每一对应节点的距离。其中全局路径输入时已截取ROI，且已转换到小车坐标系，因此只需要计算y方向差异平方和即可
	2.方向偏差。每一节点theta与路径切线方向角度差平方和。
	3.当前速度v与参考速度ref_v之间差异的平方和。ref_v可以当做实时可控的运动速度上限，模型会尽量使速度接近该值。
	4.当前加速度的平方和。该项旨在尽量降低加速度绝对值，保证运动中速度值平滑。

	定义约束。约束主要包括各状态值与可优化变量的上限与下限，也可自定义约束。通常定义加速度，确保小车实际可达到该速度。

	该非线性优化问题由CppAD库处理得出解。CppAD是COIN-OR组织提供的c++自动微分(Algorithmic Differentiation)库, 提供稀疏矩阵计算、多线程、向量运算、矩阵求导计算等模块，配合同样是该组织的Ipopt非线性优化库进行求解。使用分以下几步：
	1.定义FG_eval_model类用于定义cost函数和预测数据更新规则。使用时创建实例。
	2.定义约束，包括变量上下限。
	3.将小车状态以数组形式按定义的顺序传入Ipopt的solve函数。同时将约束、FG_eval_model的实例传入。
	4.获取解。解析传出的可优化变量omega与a，计算并发布cmd_vel，或根据实际模型转换成需要的参数，例如差速模型则转换成左右轮速度。