基于MPC的车辆自动泊车轨迹跟踪控制+代码
基于MPC的车辆自动泊车轨迹跟踪控制+代码
附赠自动驾驶学习资料和量产经验：链接
一、MPC理论推导
参考《无人驾驶模型预测控制》与《模型预测控制》
首先是最优控制，之前总结LQR是一种无约束的线性二次最优控制问题，通过求解Ricatti方程得到最优控制率，而对于非线性系统，求取最优控制的问题被转化为求解对于的Hamilton-Jacobi-Bellman方程，可以转换成求解有约束的优化问题。并且mpc起源于工业应用，就是先在工业界成功应用之后才有了相对应的理论研究。
任何一本mpc的书上基本都会写这段话
模型预测控制的基本思想就是利用已有的模型、 系统当前的状态和未来的控制量去预测系统未来的输出， 通过滚动地求解带约束优化问题来实现控制目的， 具有预测模型、 滚动优化和反馈校正三个特点 。
以及下面这句话
模型预测控制通常将待优化问题转换为二次型规划 ( Quadratic Program-ming， QP) 问题。二次型规划是一个典型的数学优化问题， 它的优化目标为带有线性或者非线性约束的二次型实函数，常用的解法为有效集法或者内点法。
龚建伟老师的书对于mpc的理论推导是按照如下的顺序
模型预测控制器的主体， 主要由线性误差模型、 系统约束以及目标函数组成。 线性误差模型是轨迹跟踪控制系统的数学描述， 也是构建控制算法的基础。 系统约束包括车辆执行机构约束、 控制量平滑约束以及车辆稳定性约束等。 目标函数的设计则综合考虑轨迹跟踪的快速性以及平稳性。
那么首先是线性误差模型，和LQR一样，直接给出线性化和离散化之后的车辆运动学模型，系统的状态变量是线性误差变量
然后龚建伟老师介绍了目标函数和系统约束，如下所示
可以看到，在目标函数中， 求解的变量为控制时域内的控制增量ΔU， 那么约束条件中也只能是以控制增量或者控制增量与转换矩阵相乘的形式出现。也就是说，相比于式LQR给出的目标函数，用控制增量取代控制量，并且加入了松弛因子。 这样不仅能对控制增量进行直接的限制， 也能防止执行过程中出现没有可行解的情况。 我们需要计算的，就是在目标函数中，未来一段时间系统的输出
书上给出的控制量和控制增量的约束如下，分别是对速度输出和前轮转角的约束
我自己理解的是可以把这个的目标函数当作一个新的状态方程的目标函数去看待，那么里面的η（k）和控制增量Δu（k）就对应这个新的状态空间方程的系统变量和输出，接下来的目标就是构建新的一个模型来满足这个目标函数（书中对这部分的推导在第三章）
构建下面这个新的状态变量，把已知的矩阵带进去，右下角就是构建的新的状态变量方程
可以写的清楚一点
里面什么a11，还是b11啥的都是原来的矩阵的符号啥的，Nx和Nu也就是状态量的个数和控制量的个数，Nx=3，Nu=2，A,B,C矩阵分别是我们新构建的状态空间表达式里面的状态矩阵、输入矩阵、输出矩阵
然后对新构建的状态变量kesi（k+1）和系统的输出η（k+1）展开看看有什么规律，这部分叫预测
里面写的Np和Nc也就是预测时域和控制时域了，为了使得关系更加明确。将系统未来时刻的输出写成矩阵的形式，对于输出方程η（k），化简合并得到
对于新构建出来的输出方程，可以看到，在预测时域内的状态量和输出量都可以通过新系统当前的状态和控制时域内的控制增量得到。
接下来把上面得到的输出方程带入到，目标函数里面看看，那么可以定义这个方程的参考值Yr，也就是参考的输出向量（其实是0，因为系统的状态量是误差，所以参考输出是0），化简一下（下面第一张图片的目标函数没有加松弛因子）
这部分我觉得要重点掌握，就是将优化问题转化成二次型规划QP问题（应该是这么个意思）
重新推导一下
书上的过程也贴一下，字母的具体的含义应该很好明白，方框内的可以化简掉
既然成了QP问题，那么就是能够约束控制量和控制量的增量，把控制输入展开写写
这部分的化简我觉得还得在理理思路，总之基于以上的思路，把最优化问题转化成了一个QP问题
二、代码模型
使用的软件版本分别是matlab2020b+carsim8.02
仿真场景是一个侧方停车的工况，在carsim中可以搭建道路环境，如下
由于是泊车，之前的模型是基于自行车模型推导的，所以要将计算出的前轮转角转化为车辆左右前轮的转角，也就是分别以左前轮和右前轮的转角输入，并且设置倒车，也就是倒挡-2，同时在车辆到达指定位置的时候，给一个制动压力Pbk
carsim中设定一个初始的车辆速度-2.88km/h，以及车辆初始位置，前轴的中心位置X0,Y0,YAW
可以把工况保存成一个cpar文件方便后续的使用，simulink模型如下
左边将自行车模型前轮转角转换成左右前轮转角的matlab function如下
function [delta_L,delta_R] = fcn(delta)
l=1.923;
L1=1.2;
tan_delta_L=1/(1/(tan(delta/180*pi))-(L1/2)/l);
tan_delta_R=1/(1/(tan(delta/180*pi))+(L1/2)/l);
delta_L=atan(tan_delta_L)/pi*180;
delta_R=atan(tan_delta_R)/pi*180;
原理如下
这里我不太明白，按公式推应该是和下面一样，L是轴距，L1是左轮到右轮的距离，和代码里面表示的形式差了一个负号，欢迎各位讨论（仿真的时候按代码的进行仿真）
右边的matlab function就是把车辆前轴中心的位置转换成后轴中心的位置，这个比较简单
function [Xr,Yr] = fcn(Xo,Yo,Yaw)
l=1.923;%轴距
Xr=Xo-l*cos(Yaw/180*pi);
Yr=Yo-l*sin(Yaw/180*pi);
下面分别介绍横向控制和纵向控制
Longitudinal control
纵向控制就是速度控制了，通过设定carsim中车辆初始速度来控制车速
就是用后轴中心的位置减去后轴lr减去一个误差e，如果这个值大于等于0，说明还没到规定的位置，那么不给制动压力，否则给一个6的制动压力
Lateral MPC control
横向控制的流程图如下
首先是左边的模块，遍历所有的参考点，找到当前时刻参考点的信息，包括每个参考点的期望位置xd，yd、期望航向角θd，期望输入δd，k_d是下标索引值
function [xd,yd,thetad,deltad,k_d] = fcn(Xr,Yr,k)
px=[7.49700000000000,7.48700000000000,7.47700000000000,7.46700000000000,7.45700000000000,7.44700000000000,7.43700000000000,7.42700000000000,7.41700000000000,7.40700000000000,7.39700000000000,7.38700000000000,7.37700000000000,7.36700000000000,7.35700000000000,7.34700000000000,7.33700000000000,7.32700000000000,7.31700000000000,7.30700000000000,7.29700000000000,7.28700000000000,7.27700000000000,7.26700000000000,7.25700000000000,7.24700000000000,7.23700000000000,7.22700000000000,7.21700000000000,7.20700000000000,7.19700000000000,7.18700000000000,7.17700000000000,7.16700000000000,7.15700000000000,7.14700000000000,7.13700000000000,7.12700000000000,7.11700000000000,7.10700000000000,7.09700000000000,7.08700000000000,7.07700000000000,7.06700000000000,7.05700000000000,7.04700000000000,7.03700000000000,7.02700000000000,7.01700000000000,7.00700000000000,6.99700000000000,6.98700000000000,6.97700000000000,6.96700000000000,6.95700000000000,6.94700000000000,6.93700000000000,6.92700000000000,6.91700000000000,6.90700000000000,6.89700000000000,6.88700000000000,6.87700000000000,6.86700000000000,6.85700000000000,6.84700000000000,6.83700000000000,6.82700000000000,6.81700000000000,6.80700000000000,6.79700000000000,6.78700000000000,6.77700000000000,6.76700000000000,6.75700000000000,6.74700000000000,6.73700000000000,6.72700000000000,6.71700000000000,6.70700000000000,6.69700000000000,6.68700000000000,6.67700000000000,6.66700000000000,6.65700000000000,6.64700000000000,6.63700000000000,6.62700000000000,6.61700000000000,6.60700000000000,6.59700000000000,6.58700000000000,6.57700000000000,6.56700000000000,6.55700000000000,6.54700000000000,6.53700000000000,6.52700000000000,6.51700000000000,6.50700000000000,6.49700000000000,6.48700000000000,6.47700000000000,6.46700000000000,6.45700000000000,6.44700000000000,6.43700000000000,6.42700000000000,6.41700000000000,6.40700000000000,6.39700000000000,6.38700000000000,6.37700000000000,6.36700000000000,6.35700000000000,6.34700000000000,6.33700000000000,6.32700000000000,6.31700000000000,6.30700000000000,6.29700000000000,6.28700000000000,6.27700000000000,6.26700000000000,6.25700000000000,6.24700000000000,6.23700000000000,6.22700000000000,6.21700000000000,6.20700000000000,6.19700000000000,6.18700000000000,6.17700000000000,6.16700000000000,6.15700000000000,6.14700000000000,6.13700000000000,6.12700000000000,6.11700000000000,6.10700000000000,6.09700000000000,6.08700000000000,6.07700000000000,6.06700000000000,6.05700000000000,6.04700000000000,6.03700000000000,6.02700000000000,6.01700000000000,6.00700000000000,5.99700000000000,5.98700000000000,5.97700000000000,5.96700000000000,5.95700000000000,5.94700000000000,5.93700000000000,5.92700000000000,5.91700000000000,5.90700000000000,5.89700000000000,5.88700000000000,5.87700000000000,5.86700000000000,5.85700000000000,5.84700000000000,5.83700000000000,5.82700000000000,5.81700000000000,5.80700000000000,5.79700000000000,5.78700000000000,5.77700000000000,5.76700000000000,5.75700000000000,5.74700000000000,5.73700000000000,5.72700000000000,5.71700000000000,5.70700000000000,5.69700000000000,5.68700000000000,5.67700000000000,5.66700000000000,5.65700000000000,5.64700000000000,5.63700000000000,5.62700000000000,5.61700000000000,5.60700000000000,5.59700000000000,5.58700000000000,5.57700000000000,5.56700000000000,5.55700000000000,5.54700000000000,5.53700000000000,5.52700000000000,5.51700000000000,5.50700000000000,5.49700000000000,5.48700000000000,5.47700000000000,5.46700000000000,5.45700000000000,5.44700000000000,5.43700000000000,5.42700000000000,5.41700000000000,5.40700000000000,5.39700000000000,5.38700000000000,5.37700000000000,5.36700000000000,5.35700000000000,5.34700000000000,5.33700000000000,5.32700000000000,5.31700000000000,5.30700000000000,5.29700000000000,5.28700000000000,5.27700000000000,5.26700000000000,5.25700000000000,5.24700000000000,5.23700000000000,5.22700000000000,5.21700000000000,5.20700000000000,5.19700000000000,5.18700000000000,5.17700000000000,5.16700000000000,5.15700000000000,5.14700000000000,5.13700000000000,5.12700000000000,5.11700000000000,5.10700000000000,5.09700000000000,5.08700000000000,5.07700000000000,5.06700000000000,5.05700000000000,5.04700000000000,5.03700000000000,5.02700000000000,5.01700000000000,5.00700000000000,4.99700000000000,4.98700000000000,4.97700000000000,4.96700000000000,4.95700000000000,4.94700000000000,4.93700000000000,4.92700000000000,4.91700000000000,4.90700000000000,4.89700000000000,4.88700000000000,4.87700000000000,4.86700000000000,4.85700000000000,4.84700000000000,4.83700000000000,4.82700000000000,4.81700000000000,4.80700000000000,4.79700000000000,4.78700000000000,4.77700000000000,4.76700000000000,4.75700000000000,4.74700000000000,4.73700000000000,4.72700000000000,4.71700000000000,4.70700000000000,4.69700000000000,4.68700000000000,4.67700000000000,4.66700000000000,4.65700000000000,4.64700000000000,4.63700000000000,4.62700000000000,4.61700000000000,4.60700000000000,4.59700000000000,4.58700000000000,4.57700000000000,4.56700000000000,4.55700000000000,4.54700000000000,4.53700000000000,4.52700000000000,4.51700000000000,4.50700000000000,4.49700000000000,4.48700000000000,4.47700000000000,4.46700000000000,4.45700000000000,4.44700000000000,4.43700000000000,4.42700000000000,4.41700000000000,4.40700000000000,4.39700000000000,4.38700000000000,4.37700000000000,4.36700000000000,4.35700000000000,4.34700000000000,4.33700000000000,4.32700000000000,4.31700000000000,4.30700000000000,4.29700000000000,4.28700000000000,4.27700000000000,4.26700000000000,4.25700000000000,4.24700000000000,4.23700000000000,4.22700000000000,4.21700000000000,4.20700000000000,4.19700000000000,4.18700000000000,4.17700000000000,4.16700000000000,4.15700000000000,4.14700000000000,4.13700000000000,4.12700000000000,4.11700000000000,4.10700000000000,4.09700000000000,4.08700000000000,4.07700000000000,4.06700000000000,4.05700000000000,4.04700000000000,4.03700000000000,4.02700000000000,4.01700000000000,4.00700000000000,3.99700000000000,3.98700000000000,3.97700000000000,3.96700000000000,3.95700000000000,3.94700000000000,3.93700000000000,3.92700000000000,3.91700000000000,3.90700000000000,3.89700000000000,3.88700000000000,3.87700000000000,3.86700000000000,3.85700000000000,3.84700000000000,3.83700000000000,3.82700000000000,3.81700000000000,3.80700000000000,3.79700000000000,3.78700000000000,3.77700000000000,3.76700000000000,3.75700000000000,3.74700000000000,3.73700000000000,3.72700000000000,3.71700000000000,3.70700000000000,3.69700000000000,3.68700000000000,3.67700000000000,3.66700000000000,3.65700000000000,3.64700000000000,3.63700000000000,3.62700000000000,3.61700000000000,3.60700000000000,3.59700000000000,3.58700000000000,3.57700000000000,3.56700000000000,3.55700000000000,3.54700000000000,3.53700000000000,3.52700000000000,3.51700000000000,3.50700000000000,3.49700000000000,3.48700000000000,3.47700000000000,3.46700000000000,3.45700000000000,3.44700000000000,3.43700000000000,3.42700000000000,3.41700000000000,3.40700000000000,3.39700000000000,3.38700000000000,3.37700000000000,3.36700000000000,3.35700000000000,3.34700000000000,3.33700000000000,3.32700000000000,3.31700000000000,3.30700000000000,3.29700000000000,3.28700000000000,3.27700000000000,3.26700000000000,3.25700000000000,3.24700000000000,3.23700000000000,3.22700000000000,3.21700000000000,3.20700000000000,3.19700000000000,3.18700000000000,3.17700000000000,3.16700000000000,3.15700000000000,3.14700000000000,3.13700000000000,3.12700000000000,3.11700000000000,3.10700000000000,3.09700000000000,3.08700000000000,3.07700000000000,3.06700000000000,3.05700000000000,3.04700000000000,3.03700000000000,3.02700000000000,3.01700000000000,3.00700000000000,2.99700000000000,2.98700000000000,2.97700000000000,2.96700000000000,2.95700000000000,2.94700000000000,2.93700000000000,2.92700000000000,2.91700000000000,2.90700000000000,2.89700000000000,2.88700000000000,2.87700000000000,2.86700000000000,2.85700000000000,2.84700000000000,2.83700000000000,2.82700000000000,2.81700000000000,2.80700000000000,2.79700000000000,2.78700000000000,2.77700000000000,2.76700000000000,2.75700000000000,2.74700000000000,2.73700000000000,2.72700000000000,2.71700000000000,2.70700000000000,2.69700000000000,2.68700000000000,2.67700000000000,2.66700000000000,2.65700000000000,2.64700000000000,2.63700000000000,2.62700000000000,2.61700000000000,2.60700000000000,2.59700000000000,2.58700000000000,2.57700000000000,2.56700000000000,2.55700000000000,2.54700000000000,2.53700000000000,2.52700000000000,2.51700000000000,2.50700000000000,2.49700000000000,2.48700000000000,2.47700000000000,2.46700000000000,2.45700000000000,2.44700000000000,2.43700000000000,2.42700000000000,2.41700000000000,2.40700000000000,2.39700000000000,2.38700000000000,2.37700000000000,2.36700000000000,2.35700000000000,2.34700000000000,2.33700000000000,2.32700000000000,2.31700000000000,2.30700000000000,2.29700000000000,2.28700000000000,2.27700000000000,2.26700000000000,2.25700000000000,2.24700000000000,2.23700000000000,2.22700000000000,2.21700000000000,2.20700000000000,2.19700000000000,2.18700000000000,2.17700000000000,2.16700000000000,2.15700000000000,2.14700000000000,2.13700000000000,2.12700000000000,2.11700000000000,2.10700000000000,2.09700000000000,2.08700000000000,2.07700000000000,2.06700000000000,2.05700000000000,2.04700000000000,2.03700000000000,2.02700000000000,2.01700000000000,2.00700000000000,1.99700000000000,1.98700000000000,1.97700000000000,1.96700000000000,1.95700000000000,1.94700000000000,1.93700000000000,1.92700000000000,1.91700000000000,1.90700000000000,1.89700000000000,1.88700000000000,1.87700000000000,1.86700000000000,1.85700000000000,1.84700000000000,1.83700000000000,1.82700000000000,1.81700000000000,1.80700000000000,1.79700000000000,1.78700000000000,1.77700000000000,1.76700000000000,1.75700000000000,1.74700000000000,1.73700000000000,1.72700000000000,1.71700000000000,1.70700000000000,1.69700000000000,1.68700000000000,1.67700000000000,1.66700000000000,1.65700000000000,1.64700000000000,1.63700000000000,1.62700000000000,1.61700000000000,1.60700000000000,1.59700000000000,1.58700000000000,1.57700000000000,1.56700000000000,1.55700000000000,1.54700000000000,1.53700000000000,1.52700000000000,1.51700000000000,1.50700000000000,1.49700000000000,1.48700000000000,1.47700000000000,1.46700000000000,1.45700000000000,1.44700000000000,1.43700000000000,1.42700000000000,1.41700000000000,1.40700000000000,1.39700000000000,1.38700000000000,1.37700000000000,1.36700000000000,1.35700000000000,1.34700000000000,1.33700000000000,1.32700000000000,1.31700000000000,1.30700000000000,1.29700000000000,1.28700000000000,1.27700000000000,1.26700000000000,1.25700000000000,1.24700000000000,1.23700000000000,1.22700000000000,1.21700000000000,1.20700000000000,1.19700000000000,1.18700000000000,1.17700000000000,1.16700000000000,1.15700000000000,1.14700000000000,1.13700000000000,1.12700000000000,1.11700000000000,1.10700000000000,1.09700000000000,1.08700000000000,1.07700000000000,1.06700000000000,1.05700000000000,1.04700000000000,1.03700000000000,1.02700000000000];
py=[1.10000132390871,1.10003103093403,1.10009720896582,1.10019908067609,1.10033587377205,1.10050682098400,1.10071116005326,1.10094813372010,1.10121698971167,1.10151698072983,1.10184736443921,1.10220740345502,1.10259636533104,1.10301352254745,1.10345815249885,1.10392953748209,1.10442696468426,1.10494972617060,1.10549711887234,1.10606844457472,1.10666300990486,1.10728012631967,1.10791911009379,1.10857928230751,1.10925996883467,1.10996050033060,1.11068021222004,1.11141844468501,1.11217454265279,1.11294785578384,1.11373773845964,1.11454354977072,1.11536465350447,1.11620041813316,1.11705021680176,1.11791342731594,1.11878943212996,1.11967761833457,1.12057737764496,1.12148810638866,1.12240920549345,1.12334008047529,1.12428014142627,1.12522880300247,1.12618548441193,1.12714960940253,1.12812060624991,1.12909790774546,1.13008095118414,1.13106917835244,1.13206203551632,1.13305897340910,1.13405944721940,1.13506291657903,1.13606884555094,1.13707670261713,1.13808596066654,1.13909609698302,1.14010659323321,1.14111693545448,1.14212661404281,1.14313512374077,1.14414196362541,1.14514663709614,1.14614865186273,1.14714751993315,1.14814275760154,1.14913388543610,1.15012042826704,1.15110191517444,1.15207787947626,1.15304785871618,1.15401139465153,1.15496803324124,1.15591732463377,1.15685882315495,1.15779208729599,1.15871667970135,1.15963216715667,1.16053812057666,1.16143411499310,1.16231972954268,1.16319454745490,1.16405815604011,1.16491014667730,1.16575011480209,1.16657765989463,1.16739238546751,1.16819389905369,1.16898181219441,1.16975574042714,1.17051530327344,1.17126012422693,1.17198983074120,1.17270405421770,1.17340242999370,1.17408459733016,1.17475019939971,1.17539888327452,1.17603029991422,1.17664410415385,1.17723995469178,1.17781751407757,1.17837644869995,1.17891642877474,1.17943712833270,1.17993822520755,1.18041940102378,1.18088034118468,1.18132073486016,1.18174027497473,1.18213865819540,1.18251558491959,1.18287075926308,1.18320388904788,1.18351468579019,1.18380286468831,1.18406814461054,1.18431024808314,1.18452890127817,1.18472383400151,1.18489477968071,1.18504147535293,1.18516366165284,1.18526108280059,1.18533348658966,1.18538062437484,1.18540225106010,1.18539812508656,1.18536800842033,1.18531166654054,1.18522886842715,1.18511938654894,1.18498299685140,1.18481947874465,1.18462861509136,1.18441019219469,1.18416399978616,1.18388983101364,1.18358748242918,1.18325675397703,1.18289744898146,1.18250937413474,1.18209233948506,1.18164615842441,1.18117064767654,1.18066562728484,1.18013092060030,1.17956635426939,1.17897175822200,1.17834696565936,1.17769181304197,1.17700614007747,1.17628978970861,1.17554260810116,1.17476444463180,1.17395515187608,1.17311458559629,1.17224260472943,1.17133907137510,1.17040385078342,1.16943681134294,1.16843782456860,1.16740676508960,1.16634351063735,1.16524794203336,1.16411994317721,1.16295940103440,1.16176620562433,1.16054025000817,1.15928143027682,1.15798964553883,1.15666479790825,1.15530679249264,1.15391553738093,1.15249094363137,1.15103292525941,1.14954139922566,1.14801628542381,1.14645750666851,1.14486498868331,1.14323866008860,1.14157845238947,1.13988429996371,1.13815614004966,1.13639391273417,1.13459756094049,1.13276703041622,1.13090226972118,1.12900323021539,1.12706986604697,1.12510213414001,1.12309999418254,1.12106340861445,1.11899234261538,1.11688676409267,1.11474664366925,1.11257195467156,1.11036267311750,1.10811877770432,1.10584024979655,1.10352707341392,1.10117923521926,1.09879672450646,1.09637953318833,1.09392765578458,1.09144108940970,1.08891983376089,1.08636389110597,1.08377326627133,1.08114796662980,1.07848800208861,1.07579338507730,1.07306413053560,1.07030025590143,1.06750178109873,1.06466872852543,1.06180112304137,1.05889899195621,1.05596236501731,1.05299127439772,1.04998575468405,1.04694584286441,1.04387157831631,1.04076300279459,1.03762016041936,1.03444309766387,1.03123186334247,1.02798650859851,1.02470708689227,1.02139365398887,1.01804626794619,1.01466498910280,1.01124988006585,1.00780100569903,1.00431843311046,1.00080223164059,0.997252472850196,0.993669230508209,0.990052580579681,0.986402601213703,0.982719372731306,0.979002977613400,0.975253500488673,0.971471028121527,0.967655649399987,0.963807455323621,0.959926538991465,0.956012995589935,0.952066922380740,0.948088418688820,0.944077585890253,0.940034527400165,0.935959348660665,0.931852157128762,0.927713062264266,0.923542175517729,0.919339610318356,0.915105482061917,0.910839908098674,0.906543007721298,0.902214902152784,0.897855714534376,0.893465569913482,0.889044595231594,0.884592919312206,0.880110672848729,0.875597988392424,0.871055000340300,0.866481844923052,0.861878660192965,0.857245586011851,0.852582764038939,0.847890337718825,0.843168452269373,0.838417254669636,0.833636893647780,0.828827519668996,0.823989284923423,0.819122343314070,0.814226850444728,0.809302963607891,0.804350841772680,0.799370645572750,0.794362537294224,0.789326680863603,0.784263241835681,0.779172387381474,0.774054286276135,0.768909108886865,0.763737027160845,0.758538214613145,0.753312846314648,0.748061098879966,0.742783150455358,0.737479180706657,0.732149370807175,0.726793903425633,0.721412962714079,0.716006734295795,0.710575405253237,0.705119164115933,0.699638200848416,0.694132706838129,0.688602874883367,0.683048899181165,0.677470975315243,0.671869300243913,0.666244072288000,0.660595491118756,0.654923757745788,0.649229074504972,0.643511645046371,0.637771674322154,0.632009368574516,0.626224935323602,0.620418583355409,0.614590522709729,0.608740964668044,0.602870121741466,0.596978207658637,0.591065437353663,0.585132026954023,0.579178193768489,0.573204156275058,0.567210134108846,0.561196348050032,0.555163020011757,0.549110373028057,0.543038631241776,0.536948019892484,0.530838765304397,0.524711094874297,0.518565237059446,0.512401421365516,0.506219878334492,0.500020839532605,0.493804537538245,0.487571205929876,0.481321079273963,0.475054393112884,0.468771383952852,0.462472289251834,0.456157347407470,0.449826797744989,0.443480880505131,0.437119836832062,0.430743908761304,0.424353339207635,0.417948371953025,0.411529251634544,0.405096223732289,0.398649534557293,0.392189431239456,0.385716161715454,0.379229974716662,0.372731119757072,0.366219847121212,0.359696407852066,0.353161053738990,0.346614037305631,0.340055611797852,0.333486031171643,0.326905550081043,0.320314423866062,0.313712908540591,0.307101260780332,0.300479737910709,0.293848597894789,0.287208099321203,0.280558501392061,0.273900063910873,0.267233047270469,0.260557712440915,0.253874320957433,0.247183134908322,0.240484416922875,0.233778430159296,0.227065438292621,0.220345705502638,0.213619496461804,0.206887076323163,0.200148710708264,0.193404665695091,0.186655207805962,0.179900603995464,0.173141121638365,0.166377028517537,0.159608592811867,0.152836083084184,0.146059768269178,0.139279917661310,0.132496800902739,0.125710687971237,0.118921849168113,0.112130555106122,0.105337076697396,0.0985416851413541,0.0917446519126228,0.0849462487489558,0.0781467476391562,0.0713464208109886,0.0645455407191045,0.0577443800329558,0.0509432116247182,0.0441423085572046,0.0373419440717926,0.0305423915763305,0.0237439246330717,0.0169468169465792,0.0101513423516526,0.00335777480124588,-0.00343361164561395,-0.0102225428359126,-0.0170087445347207,-0.0237919424372837,-0.0305718621810975,-0.0373482293579923,-0.0441207695262118,-0.0508892082224961,-0.0576532709741640,-0.0644126833111889,-0.0711671707782851,-0.0779164589469883,-0.0846602734277346,-0.0913983398819425,-0.0981303840340941,-0.104856131683818,-0.111575308717969,-0.118287641122707,-0.124992854995582,-0.131690676557613,-0.138380832165372,-0.145063048323059,-0.151737051694590,-0.158402569115673,-0.165059327605895,-0.171707054380797,-0.178345476863957,-0.184974322699073,-0.191593319762045,-0.198202196173051,-0.204800680308632,-0.211388500813774,-0.217965386613986,-0.224531066927385,-0.231085271276772,-0.237627729501719,-0.244158171770645,-0.250676328592903,-0.257181930830852,-0.263674709711947,-0.270154396840819,-0.276620724211351,-0.283073424218762,-0.289512229671692,-0.295936873804274,-0.302347090288226,-0.308742613244924,-0.315123177257487,-0.321488517382856,-0.327838369163878,-0.334172468641385,-0.340490552366274,-0.346792357411593,-0.353077621384615,-0.359346082438927,-0.365597479286505,-0.371831551209798,-0.378048038073809,-0.384246680338174,-0.390427219069247,-0.396589395952179,-0.402732953302997,-0.408857634080691,-0.414963181899287,-0.421049341039937,-0.427115856462993,-0.433162473820093,-0.439188939466238,-0.445195000471877,-0.451180404634986,-0.457144900493150,-0.463088237335642,-0.469010165215509,-0.474910434961648,-0.480788798190888,-0.486645007320077,-0.492478815578154,-0.498289977018236,-0.504078246529700,-0.509843379850259,-0.515585133578048,-0.521303265183702,-0.526997533022442,-0.532667696346148,-0.538313515315448,-0.543934751011794,-0.549531165449547,-0.555102521588055,-0.560648583343736,-0.566169115602159,-0.571663884230123,-0.577132656087743,-0.582575199040525,-0.587991281971452,-0.593380674793063,-0.598743148459534,-0.604078474978762,-0.609386427424439,-0.614666779948144,-0.619919307791415,-0.625143787297832,-0.630339995925102,-0.635507712257138,-0.640646716016137,-0.645756788074667,-0.650837710467742,-0.655889266404910,-0.660911240282328,-0.665903417694846,-0.670865585448087,-0.675797531570531,-0.680699045325592,-0.685569917223703,-0.690409939034393,-0.695218903798374,-0.699996605839615,-0.704742840777430,-0.709457405538554,-0.714140098369226,-0.718790718847272,-0.723409067894184,-0.727994947787200,-0.732548162171389,-0.737068516071728,-0.741555815905188,-0.746009869492808,-0.750430486071784,-0.754817476307546,-0.759170652305838,-0.763489827624803,-0.767774817287060,-0.772025437791790,-0.776241507126811,-0.780422844780665,-0.784569271754695,-0.788680610575130,-0.792756685305162,-0.796797321557031,-0.800802346504101,-0.804771588892949,-0.808704879055437,-0.812602048920803,-0.816462932027731,-0.820287363536443,-0.824075180240773,-0.827826220580250,-0.831540324652182,-0.835217334223732,-0.838857092744002,-0.842459445356117,-0.846024238909300,-0.849551321970958,-0.853040544838761,-0.856491759552723,-0.859904819907286,-0.863279581463397,-0.866615901560590,-0.869913639329073,-0.873172655701799,-0.876392813426556,-0.879573977078044,-0.882716013069956,-0.885818789667061,-0.888882176997285,-0.891906047063790,-0.894890273757056,-0.897834732866965,-0.900739302094878,-0.903603861065718,-0.906428291340053,-0.909212476426174,-0.911956301792176,-0.914659654878044,-0.917322425107729,-0.919944503901230,-0.922525784686678,-0.925066162912414,-0.927565536059072,-0.930023803651659,-0.932440867271638,-0.934816630569007,-0.937150999274381,-0.939443881211074,-0.941695186307177,-0.943904826607646,-0.946072716286373,-0.948198771658278,-0.950282911191383,-0.952325055518894,-0.954325127451284,-0.956283051988375,-0.958198756331417,-0.960072169895167,-0.961903224319977,-0.963691853483868,-0.965437993514617,-0.967141582801834,-0.968802562009043,-0.970420874085767,-0.971996464279607,-0.973529280148322,-0.975019271571911,-0.976466390764695,-0.977870592287397,-0.979231833059224,-0.980550072369949,-0.981825271891988,-0.983057395692488,-0.984246410245401,-0.985392284443569,-0.986494989610807,-0.987554499513980,-0.988570790375084,-0.989543840883334,-0.990473632207236,-0.991360148006673,-0.992203374444988,-0.993003300201060,-0.993759916481390,-0.994473217032178,-0.995143198151407,-0.995769858700924,-0.996353200118520,-0.996893226430011,-0.997389944261322,-0.997843362850563,-0.998253494060116,-0.998620352388710,-0.998943954983509,-0.999224321652187,-0.999461474875014,-0.999655439816932,-0.999806244339641,-0.999913919013678,-0.999978497130499,-1.00000001471456];
ptheta=[-0.00297069379334702,-0.00661770657172625,-0.0101868186464428,-0.0136784564510091,-0.0170930562934526,-0.0204310636161949,-0.0236929323061108,-0.0268791240095772,-0.0299901074804960,-0.0330263579706422,-0.0359883566126258,-0.0388765898669493,-0.0416915489554345,-0.0444337293568122,-0.0471036302867366,-0.0497017542474503,-0.0522286065513688,-0.0546846948992889,-0.0570705289757496,-0.0593866200560522,-0.0616334806366709,-0.0638116240934167,-0.0659215643519761,-0.0679638155720271,-0.0699388918690325,-0.0718473070273903,-0.0736895742485936,-0.0754662059180512,-0.0771777133707530,-0.0788246066849911,-0.0804073944955412,-0.0819265837992183,-0.0833826798051546,-0.0847761857674740,-0.0861076028549043,-0.0873774300162440,-0.0885861638624397,-0.0897342985691545,-0.0908223257743859,-0.0918507344904137,-0.0928200110397514,-0.0937306389842303,-0.0945830990614140,-0.0953778691481935,-0.0961154242101948,-0.0967962362773640,-0.0974207744152023,-0.0979895047045035,-0.0985028902338788,-0.0989613910934494,-0.0993654643760297,-0.0997155641818827,-0.100012141631977,-0.100255644887687,-0.100446519167211,-0.100585206775074,-0.100672147135908,-0.100707776819670,-0.100692529593786,-0.100626836450246,-0.100511125662278,-0.100345822828935,-0.100131350922938,-0.0998681303500131,-0.0995565789997769,-0.0991971123109497,-0.0987901433245342,-0.0983360827523958,-0.0978353390370166,-0.0972883184236089,-0.0966954250182375,-0.0960570608639076,-0.0953736260058769,-0.0946455185708367,-0.0938731348213463,-0.0930568692443942,-0.0921971146178637,-0.0912942620805526,-0.0903487012100772,-0.0893608200987852,-0.0883310054166468,-0.0872596424947479,-0.0861471154011001,-0.0849938070011574,-0.0838000990435773,-0.0825663722252296,-0.0812930062675330,-0.0799803799840008,-0.0786288713565846,-0.0772388575969987,-0.0758107152251342,-0.0743448201312827,-0.0728415476435623,-0.0713012725962594,-0.0697243693964218,-0.0681112120773994,-0.0664621743763943,-0.0647776297818149,-0.0630579515995081,-0.0613035130113084,-0.0595146871279005,-0.0576918470459553,-0.0558353658978347,-0.0539456169140330,-0.0520229734542935,-0.0500678090729941,-0.0480804975537115,-0.0460614129636299,-0.0440109296826052,-0.0419294224544948,-0.0398172664208873,-0.0376748371550857,-0.0355025107020535,-0.0333006636031289,-0.0310696729295385,-0.0288099163128166,-0.0265217719630827,-0.0242056186968817,-0.0218618359568398,-0.0194908038288137,-0.0170929030628383,-0.0146685150789981,-0.0122180219860600,-0.00974180658825460,-0.00724025239033408,-0.00471374360532247,-0.00216266515470719,0.000412597331446577,0.00301165751699568,0.00563412836340585,0.00827962213786162,0.0109477504257049,0.0136381241348762,0.0163503535171479,0.0190840481824403,0.0218388171133205,0.0246142686959192,0.0274100107290248,0.0302256504635971,0.0330607946198450,0.0359150494216819,0.0387880206292076,0.0416793135682078,0.0445885331715906,0.0475152840106150,0.0504591703433417,0.0534197961454712,0.0563967651648754,0.0593896809602725,0.0623981469476417,0.0654217664540580,0.0684601427658014,0.0715128791777377,0.0745795790499307,0.0776598458644397,0.0807532832761475,0.0838594951770798,0.0869780857501578,0.0901086595315972,0.0932508214761127,0.0964041770125854,0.0995683321128228,0.102742893354399,0.105927467986605,0.109121663997174,0.112325090174957,0.115537356187523,0.118758072637837,0.121986851141239,0.125223304390050,0.128467046225030,0.131717691707665,0.134974857183383,0.138238160360678,0.141507220372795,0.144781657853350,0.148061095006467,0.151345155672838,0.154633465403430,0.157925651528909,0.161221343223523,0.164520171582556,0.167821769681543,0.171125772649724,0.174431817734671,0.177739544369210,0.181048594234675,0.184358611330911,0.187669242032304,0.190980135156248,0.194290942024595,0.197601316520658,0.200910915150848,0.204219397103887,0.207526424306748,0.210831661479478,0.214134776192631,0.217435438917593,0.220733323078742,0.224028105104789,0.227319464476604,0.230607083775071,0.233890648725675,0.237169848244616,0.240444374478090,0.243713922847942,0.246978192083224,0.250236884267432,0.253489704865395,0.256736362763015,0.259976570296738,0.263210043285224,0.266436501057838,0.269655666481492,0.272867265986460,0.276071029589093,0.279266690913013,0.282453987211757,0.285632659383322,0.288802451988574,0.291963113264391,0.295114395139523,0.298256053240977,0.301387846907354,0.304509539194102,0.307620896880696,0.310721690473073,0.313811694208838,0.316890686055115,0.319958447709002,0.323014764596834,0.326059425865479,0.329092224383033,0.332112956725742,0.335121423173618,0.338117427697892,0.341100777949329,0.344071285247061,0.347028764563287,0.349973034505586,0.352903917302996,0.355821238784775,0.358724828362673,0.361614519008661,0.364490147233217,0.367351553062547,0.370198580012783,0.373031075066149,0.375848888643643,0.378651874576172,0.381439890077626,0.384212795715793,0.386970455379373,0.389712736248441,0.392439508763081,0.395150646588069,0.397846026580369,0.400525528755476,0.403189036250697,0.405836435290005,0.408467615147903,0.411082468111621,0.413680889444108,0.416262777345441,0.418828032914606,0.421376560110575,0.423908265711258,0.426423059276311,0.428920853104239,0.431401562193320,0.433865104199873,0.436311399398847,0.438740370639231,0.441151943305427,0.443546045274490,0.445922606873099,0.448281560837361,0.450622842269231,0.452946388594373,0.455252139520611,0.457540036995103,0.459810025161693,0.462062050319309,0.464296060878878,0.466512007321347,0.468709842156445,0.470889519878769,0.473050996927709,0.475194231644516,0.477319184230951,0.479425816708001,0.481514092874382,0.483583978265534,0.485635440113048,0.487668447302987,0.489682970337635,0.491678981292799,0.493656453779750,0.495615362905834,0.497555685233172,0.499477398742505,0.501380482792214,0.503264918082260,0.505130686613152,0.506977771652425,0.508806157693449,0.510615830421056,0.512406776674390,0.514178984410515,0.515932442668685,0.517667141535233,0.519383072108603,0.521080226464877,0.522758597623016,0.524418179511535,0.526058966935989,0.527680955543902,0.529284141795016,0.530868522926429,0.532434096923347,0.533980862486110,0.535508819000252,0.537017966506107,0.538508305667738,0.539979837745525,0.541432564564708,0.542866488488555,0.544281612388818,0.545677939618518,0.547055473984658,0.548414219720592,0.549754181460228,0.551075364211568,0.552377773330301,0.553661414496020,0.554926293685690,0.556172417150392,0.557399791390570,0.558608423132674,0.559798319305823,0.560969487018810,0.562121933538211,0.563255666265263,0.564370692715629,0.565467020496667,0.566544657288089,0.567603610819477,0.568643888852734,0.569665499159818,0.570668449505351,0.571652747626160,0.572618401213976,0.573565417895792,0.574493805217364,0.575403570624766,0.576294721447362,0.577167264881159,0.578021207972326,0.578856557600862,0.579673320464980,0.580471503065348,0.581251111690393,0.582012152401113,0.582754631016677,0.583478553100453,0.584183923945945,0.584870748563250,0.585539031665909,0.586188777658067,0.586819990621651,0.587432674304441,0.588026832107274,0.588602467073276,0.589159581875764,0.589698178807328,0.590218259768892,0.590719826259167,0.591202879364608,0.591667419748662,0.592113447642763,0.592540962836550,0.592949964668689,0.593340452017446,0.593712423293083,0.594065876428244,0.594400808870559,0.594717217574378,0.595015098993215,0.595294449072471,0.595555263241493,0.595797536408271,0.596021262951131,0.596226436713073,0.596413050995494,0.596581098552479,0.596730571584686,0.596861461733996,0.596973760078780,0.597067457128313,0.597142542818235,0.597199006506304,0.597236836967709,0.597256022391514,0.597256550376558,0.597238407928076,0.597201581453920,0.597146056762648,0.597071819059043,0.596978852943099,0.596867142406560,0.596736670831200,0.596587420987082,0.596419375030840,0.596232514503741,0.596026820331519,0.595802272822696,0.595558851668067,0.595296535940416,0.595015304094330,0.594715133966263,0.594396002774642,0.594057887120710,0.593700762989441,0.593324605750226,0.592929390158721,0.592515090358035,0.592081679881062,0.591629131652274,0.591157417990238,0.590666510610527,0.590156380628544,0.589626998562736,0.589078334338372,0.588510357291158,0.587923036171548,0.587316339149307,0.586690233817812,0.586044687199644,0.585379665751522,0.584695135370539,0.583991061399411,0.583267408633489,0.582524141327179,0.581761223200820,0.580978617448238,0.580176286744496,0.579354193253553,0.578512298637131,0.577650564063447,0.576768950215867,0.575867417303046,0.574945925068657,0.574004432801412,0.573042899346188,0.572061283114815,0.571059542097699,0.570037633875873,0.568995515633227,0.567933144169175,0.566850475912197,0.565747466933435,0.564624072960462,0.563480249392401,0.562315951314800,0.561131133514823,0.559925750497917,0.558699756503833,0.557453105523878,0.556185751318219,0.554897647434392,0.553588747225320,0.552259003868738,0.550908370386874,0.549536799666473,0.548144244479614,0.546730657505178,0.545295991350391,0.543840198573919,0.542363231708086,0.540865043283412,0.539345585852387,0.537804812014353,0.536242674441298,0.534659125903824,0.533054119298060,0.531427607672754,0.529779544258040,0.528109882493424,0.526418576057762,0.524705578899189,0.522970845265778,0.521214329737273,0.519435987257071,0.517635773165162,0.515813643231755,0.513969553691562,0.512103461278794,0.510215323262987,0.508305097485477,0.506372742396519,0.504418217093429,0.502441481359150,0.500442495701843,0.498421221394941,0.496377620518214,0.494311655999510,0.492223291657136,0.490112492243186,0.487979223487465,0.485823452142260,0.483645146027837,0.481444274078767,0.479220806390764,0.476974714268730,0.474705970274921,0.472414548278458,0.470100423505280,0.467763572588698,0.465403973620982,0.463021606205493,0.460616451509510,0.458188492317785,0.455737713086808,0.453264099999676,0.450767641021759,0.448248325956824,0.445706146503861,0.443141096314784,0.440553171052121,0.437942368448032,0.435308688363202,0.432652132846820,0.429972706196736,0.427270415020110,0.424545268294823,0.421797277431019,0.419026456333131,0.416232821462468,0.413416391899860,0.410577189408862,0.407715238499230,0.404830566490368,0.401923203575465,0.398993182885322,0.396040540552679,0.393065315776371,0.390067550885820,0.387047291405246,0.384004586118033,0.380939487130854,0.377852049937901,0.374742333484632,0.371610400231656,0.368456316217696,0.365280151123180,0.362081978332247,0.358861874995448,0.355619922091070,0.352356204486275,0.349070810997609,0.345763834450531,0.342435371738601,0.339085523881431,0.335714396081895,0.332322097782483,0.328908742720337,0.325474448981486,0.322019339053728,0.318543539878208,0.315047182899846,0.311530404116179,0.307993344124960,0.304436148169970,0.300858966185325,0.297261952838151,0.293645267569348,0.290009074632668,0.286353543131699,0.282678847055005,0.278985165309191,0.275272681749687,0.271541585209415,0.267792069525320,0.264024333562202,0.260238581234406,0.256435021524949,0.252613868502041,0.248775341332904,0.244919664295129,0.241047066785039,0.237157783323368,0.233252053557978,0.229330122263773,0.225392239339445,0.221438659801293,0.217469643774036,0.213485456478292,0.209486368214952,0.205472654346571,0.201444595274995,0.197402476416321,0.193346588172043,0.189277225897148,0.185194689864619,0.181099285226939,0.176991321973705,0.172871114886265,0.168738983488711,0.164595251995525,0.160440249255902,0.156274308694567,0.152097768249280,0.147910970304937,0.143714261624390,0.139507993275919,0.135292520557425,0.131068202917476,0.126835403872923,0.122594490923733,0.118345835464346,0.114089812692296,0.109826801513687,0.105557184445891,0.101281347517182,0.0969996801640079,0.0927125751251326,0.0884204283336023,0.0841236388059469,0.0798226085290410,0.0755177423448202,0.0712094478325607,0.0668981351892487,0.0625842171079547,0.0582681086541947,0.0539502271408191,0.0496309920010801,0.0453108246601792,0.0409901484056875,0.0366693882564872,0.0323489708306943,0.0280293242127327,0.0237108778193738,0.0193940622652791,0.0150793092278537,0.0107670513118657,0.00645772191357317,0.00215175508499321,0];
PR=[0,0.611592139287632,0.601460208438458,0.591237768692544,0.580928183400868,0.570534887646886,0.560061387914724,0.549511255446579,0.538888123097702,0.528195683393216,0.517437679356342,0.506617905316897,0.495740196434316,0.484808429093865,0.473826509780353,0.462798376580670,0.451727986237062,0.440619313368672,0.429476344082458,0.418303068696963,0.407103476340343,0.395881550384101,0.384641261533596,0.373386561202402,0.362121379056727,0.350849613091044,0.339575127145042,0.328301746004426,0.317033247351445,0.305773358958469,0.294525754771871,0.283294046559355,0.272081785042018,0.260892450433148,0.249729452938688,0.238596126343181,0.227495725637625,0.216431425758627,0.205406316082532,0.194423398766897,0.183485589279853,0.172595711628753,0.161756496529603,0.150970583648112,0.140240516500235,0.129568745212733,0.118957623919728,0.108409410875507,0.0979262696961677,0.0875102687075626,0.0771633817671669,0.0668874886135927,0.0566843764075883,0.0465557411267104,0.0365031873255538,0.0265282312310399,0.0166323023526755,0.00681674281694039,-0.00291718634769286,-0.0125683087161814,-0.0221355253847223,-0.0316178157395177,-0.0410142351794893,-0.0503239111692653,-0.0595460430211476,-0.0686798972537560,-0.0777248079223921,-0.0866801716665730,-0.0955454473350340,-0.104320151550808,-0.113003859256010,-0.121596198183837,-0.130096848771093,-0.138505539416278,-0.146822049013849,-0.155046198429279,-0.163177852566162,-0.171216917624296,-0.179163337590344,-0.187017092608380,-0.194778199413967,-0.202446705579776,-0.210022689053123,-0.217506259101479,-0.224897549865233,-0.232196722394342,-0.239403960854320,-0.246519472448649,-0.253543484197310,-0.260476244062260,-0.267318016336748,-0.274069083066554,-0.280729741669120,-0.287300303379286,-0.293781092216409,-0.300172445972759,-0.306474710732812,-0.312688244956303,-0.318813414991370,-0.324850595290303,-0.330800168296046,-0.336662522646337,-0.342438053432781,-0.348127159049119,-0.353730245580590,-0.359247720062964,-0.364679994834364,-0.370027483338626,-0.375290603562574,-0.380469773519849,-0.385565413436940,-0.390577944969007,-0.395507789514798,-0.400355370043137,-0.405121108987821,-0.409805428131428,-0.414408749962930,-0.418931495535141,-0.423374085222516,-0.427736938570466,-0.432020473162046,-0.436225106504658,-0.440351253506221,-0.444399327879299,-0.448369741944476,-0.452262905762492,-0.456079227887541,-0.459819114959573,-0.463482971708021,-0.467071200747611,-0.470584202884962,-0.474022377414950,-0.477386120552904,-0.480675827657253,-0.483891891846083,-0.487034703718916,-0.490104653524173,-0.493102127591271,-0.496027512609851,-0.498881192004956,-0.501663548273332,-0.504374962436514,-0.507015813367926,-0.509586479325869,-0.512087336238638,-0.514518760022189,-0.516881123814061,-0.519174801242029,-0.521400163942205,-0.523557582524990,-0.525647427489588,-0.527670068254212,-0.529625873211286,-0.531515210641720,-0.533338448624853,-0.535095954089676,-0.536788094641601,-0.538415236981045,-0.539977748058912,-0.541475995407695,-0.542910345748676,-0.544281166794137,-0.545588826382645,-0.546833692894887,-0.548016135350238,-0.549136522843104,-0.550195226625503,-0.551192617217764,-0.552129067256238,-0.553004949811605,-0.553820639387087,-0.554576512082896,-0.555272944414125,-0.555910315571367,-0.556489005106434,-0.557009394934768,-0.557471868682447,-0.557876811233032,-0.558224609891002,-0.558515653914772,-0.558750333892676,-0.558929043729003,-0.559052178302310,-0.559120135555106,-0.559133315448585,-0.559092120437788,-0.558996955141785,-0.558848227434246,-0.558646346725226,-0.558391725834750,-0.558084780227441,-0.557725927604111,-0.557315588726484,-0.556854187289036,-0.556342149679499,-0.555779905034166,-0.555167885800535,-0.554506526985747,-0.553796266618959,-0.553037545854400,-0.552230808712086,-0.551376502297002,-0.550475076574509,-0.549526984794323,-0.548532682780080,-0.547492630011156,-0.546407287924251,-0.545277122256593,-0.544102600495010,-0.542884193831559,-0.541622375977225,-0.540317623664872,-0.538970416420731,-0.537581236538699,-0.536150569136411,-0.534678901924900,-0.533166725189085,-0.531614532170035,-0.530022818137223,-0.528392081092071,-0.526722821215676,-0.525015541406749,-0.523270746140206,-0.521488942576050,-0.519670639594626,-0.517816348204792,-0.515926581025363,-0.514001852797069,-0.512042679490769,-0.510049578783072,-0.508023070002644,-0.505963673129883,-0.503871910197474,-0.501748303468240,-0.499593376727618,-0.497407654223565,-0.495191660857043,-0.492945922317460,-0.490670964562181,-0.488367313530632,-0.486035495716940,-0.483676037178136,-0.481289464058402,-0.478876301972308,-0.476437076100833,-0.473972311051679,-0.471482530436694,-0.468968257148972,-0.466430012861758,-0.463868317794775,-0.461283690972273,-0.458676649890789,-0.456047709933528,-0.453397384827858,-0.450726186344250,-0.448034623686516,-0.445323203844780,-0.442592431378011,-0.439842807915488,-0.437074832320941,-0.434289000485553,-0.431485805016992,-0.428665735283230,-0.425829277106539,-0.422976912740226,-0.420109120683676,-0.417226375291028,-0.414329147245533,-0.411417902640313,-0.408493103445949,-0.405555207030181,-0.402604666405352,-0.399641929373851,-0.396667439308402,-0.393681634448312,-0.390684947762734,-0.387677807270817,-0.384660635444336,-0.381633849322792,-0.378597860503155,-0.375553074852282,-0.372499892437601,-0.369438707583646,-0.366369908557730,-0.363293877637513,-0.360210991129229,-0.357121618873377,-0.354026124728387,-0.350924866050803,-0.347818193845099,-0.344706452685013,-0.341589980596306,-0.338469109046727,-0.335344162929200,-0.332215460314640,-0.329083312891390,-0.325948025217088,-0.322809895346594,-0.319669214558967,-0.316526266997779,-0.313381330385519,-0.310234675224829,-0.307086565485352,-0.303937257785110,-0.300787002478616,-0.297636042431089,-0.294484614006378,-0.291332946592942,-0.288181262670115,-0.285029777866058,-0.281878701007088,-0.278728234100354,-0.275578572371368,-0.272429904182769,-0.269282411222178,-0.266136268595362,-0.262991644403319,-0.259848700534902,-0.256707591854670,-0.253568467045859,-0.250431468015629,-0.247296730344711,-0.244164383202349,-0.241034549225264,-0.237907345074265,-0.234782880795615,-0.231661260530375,-0.228542582086363,-0.225426937295583,-0.222314411999362,-0.219205085992845,-0.216099033311055,-0.212996322127069,-0.209897014762504,-0.206801168101816,-0.203708833168827,-0.200620055645462,-0.197534875645564,-0.194453327943331,-0.191375441986724,-0.188301241963310,-0.185230746984686,-0.182163970908784,-0.179100922806368,-0.176041606586033,-0.172986021541129,-0.169934161875691,-0.166886017495840,-0.163841573288462,-0.160800809906660,-0.157763703307341,-0.154730225268875,-0.151700343044104,-0.148674019876257,-0.145651214715556,-0.142631882465051,-0.139615974062813,-0.136603436532979,-0.133594213032511,-0.130588242988088,-0.127585462097922,-0.124585802525249,-0.121589192844769,-0.118595558191581,-0.115604820356594,-0.112616897795136,-0.109631705735867,-0.106649156269844,-0.103669158423444,-0.100691618188881,-0.0977164386912508,-0.0947435200767484,-0.0917727598612255,-0.0888040527452172,-0.0858372908271365,-0.0828723636266810,-0.0799091581737926,-0.0769475591139809,-0.0739874486232967,-0.0710287067114400,-0.0680712111251942,-0.0651148374709572,-0.0621594592016730,-0.0592049479319860,-0.0562511731472135,-0.0532980025809391,-0.0503453021038668,-0.0473929358629054,-0.0444407663529473,-0.0414886543376378,-0.0385364592940190,-0.0355840389651569,-0.0326312498037685,-0.0296779469034166,-0.0267239841162014,-0.0237692140134525,-0.0208134880364457,-0.0178566566205142,-0.0148985690879337,-0.0119390738405688,-0.00897801843608681,-0.00601524954355870,-0.00305061315276314,-8.39545161633327e-05,0.00288488170920404,0.00585605149562475,0.00882971111885340,0.0118060175387317,0.0147851278166074,0.0177671994766569,0.0207523901977125,0.0237408577870167,0.0267327601461869,0.0297282552738035,0.0327275009549175,0.0357306549672683,0.0387378748661744,0.0417493179147896,0.0447651410351185,0.0477855007360596,0.0508105530723163,0.0538404534937421,0.0568753567987356,0.0599154171363169,0.0629607877849752,0.0660116212230195,0.0690680689090391,0.0721302813309685,0.0751984078540592,0.0782725966195180,0.0813529945203107,0.0844397471001951,0.0875329984241410,0.0906328910646898,0.0937395659497852,0.0968531622833016,0.0999738175429573,0.103101667234541,0.106236844952580,0.109379482131918,0.112529708158548,0.115687650046749,0.118853432488619,0.122027177735016,0.125209005443432,0.128399032600678,0.131597373490042,0.134804139454022,0.138019438867123,0.141243377101001,0.144476056241555,0.147717575104916,0.150968029142499,0.154227510218665,0.157496106613054,0.160773902841495,0.164060979537612,0.167357413376554,0.170663276954561,0.173978638604623,0.177303562336130,0.180638107759740,0.183982329826367,0.187336278816450,0.190700000242048,0.194073534567996,0.197456917264942,0.200850178571185,0.204253343437498,0.207666431292897,0.211089456078829,0.214522425984216,0.217965343363848,0.221418204644225,0.224881000158648,0.228353714008819,0.231836324014937,0.235328801439889,0.238831111091220,0.242343210929163,0.245865052155281,0.249396579013672,0.252937728622768,0.256488430926911,0.260048608528277,0.263618176642382,0.267197042823039,0.270785107071616,0.274382261520926,0.277988390435804,0.281603370099457,0.285227068648282,0.288859346039087,0.292500053909829,0.296149035487093,0.299806125502057,0.303471150101565,0.307143926746890,0.310824264144424,0.314511962180539,0.318206811806061,0.321908595015222,0.325617084738036,0.329332044825544,0.333053229954720,0.336780385591194,0.340513247962659,0.344251543992547,0.347994991289544,0.351743298109511,0.355496163337992,0.359253276464909,0.363014317613339,0.366778957468183,0.370546857376665,0.374317669269841,0.378091035725223,0.381866590013554,0.385643956085743,0.389422748639136,0.393202573168786,0.396983026018148,0.400763694439371,0.404544156675532,0.408323982020088,0.412102730931476,0.415879955117154,0.419655197600682,0.423427992919019,0.427197867133232,0.430964338054815,0.434726915299361,0.438485100479036,0.442238387350673,0.445986261931471,0.449728202708675,0.453463680800434,0.457192160110874,0.460913097558075,0.464625943233963,0.468330140607543,0.472025126770230,0.475710332583120,0.479385182965876,0.483049097069825,0.486701488548687,0.490341765754700,0.493969332021493,0.497583585885955,0.501183921359625,0.504769728154622,0.508340391983075,0.511895294772321,0.515433815017464,0.518955327920531,0.522459205825700,0.525944818338125,0.529411532719761,0.532858714115215,0.536285725833681,0.539691929666580,0.543076686116209,0.546439354731582,0.549779294371992,0.553095863475442,0.556388420378283,0.559656323557074,0.562898931931656,0.566115605150793,0.569305703847167,0.572468589935094,0.575603626853975,0.578710179868065,0.581787616316649,0.584835305869073,0.587852620799941,0.590838936223063,0.593793630363738,0.596716084783757,0.599605684622806,0.602461818850353,0.605283880483248,0.608071266789966,0.610823379552299,0.613539625240600,0.616219415228539,0.618862165998738,0.621467299343649,0.624034242524855,0.626562428481642,0.629051295989363,0.631500289835848,0.633908860966372,0.636276466658876,0.638602570657506,0.640886643302684,0.643128161687684,0.645326609778349,0.647481478502507,0.649592265926580,0.651658477287111,0.653679625150368,0.655655229471784,0.657584817710878,0.659467924866775,0.661304093612536,0.663092874314412,0.664833825121854,0.666526512019326,0.668170508868434,0.669765397468035,0.671310767587513,0.672806217008563,0.674251351549882,0.675645785098731,0.676989139633890,0.678281045232808,0.679521140113736,0.680709070602484,0.681844491184575,0.682927064472421,0.683956461225408,0.684932360332096,0.685854448829311,0.686722421849720,0.687535982660834,0.688294842605180,0.688998721114606,0.689647345685354,0.690240451840654,0.690777783136320,0.691259091117682,0.691684135297144,0.692052683146198,0.692364510037769,0.692619399241990,0.692817141900684,0.692957536971106,0.693040391224383,0.693065519205745,0.693032743191178,0.692941893179876,0.692792806841886,0.692585329502726,0.692319314093209,0.691994621143981,0.691611118725062,0];
l=1.923;
e_min=1;
k_d=k;
kend=k+50;
if kend>=length(px)
kend=length(px);
end
for i=k:kend
e=sqrt((Xr-px(i))^2+(Yr-py(i))^2);
if e<e_min
e_min=e;
k_d=i;
end
end
xd=px(k_d);
yd=py(k_d);
thetad=ptheta(k_d);
deltad=PR(k_d);
然后是MPC控制器，输入为自车信息，参考点的4个信息，以及期望速度（-2.88/3.6=-0.8m/s），不去控制速度，直接输出，同时对计算出的前轮转角做一个最后回正的switch判断。规划出的泊车路径如下，总共有648个离散轨迹点
MPC具体的代码如下
首先定义初始参数
tstart=tic;%开始计时
Nx=3;%状态量的个数（X，Y，Yaw）
Nu=2;%控制量的个数（v，delta）
Np=30;%预测步长Np
Nc=1;%控制步长Nc
Row=10;%松弛因子
t_d =Yaw*pi/180;%角度转弧度
r=zeros(3,1);%目标点位置，姿态向量
r(1)=xd;
r(2)=yd;
r(3)=thetad;
vd1=V;%参考速度、转角
vd2=deltad;
预测时域Np=30，控制时域Nc=1，松弛因子row=10，当然这个是人为设置的，然后是系统运动学方程的3个状态变量和两个输入
然后是我们构建的新的状态空间的变量
kesi=zeros(Nx+Nu,1);%状态向量，控制向量
kesi(1)=Xr-r(1);%Xr==X(1)
kesi(2)=Yr-r(2);%Yr==X(2)
kesi(3)=t_d-r(3);%Yaw==X(3)
kesi(4)=U1;
kesi(5)=U2;
T=0.02;%仿真步长
L=1.923;%车辆轴距
然后是系统的状态矩阵
a=[1 0 -vd1*sin(t_d)*T;
0 1 vd1*cos(t_d)*T;
0 0 1];
b=[cos(t_d)*T 0;
sin(t_d)*T 0;
tan(vd2)*T/L vd1*T/(cos(vd2)^2)];
A=[a,b;
zeros(Nu,Nx),eye(Nu)];
B=[b;eye(Nu)];
C=[1 0 0 0 0;
0 1 0 0 0;
0 0 1 0 0;];
a和b是线性离散化之后的车辆运动学模型，A和B和C是构建的新的状态空间方程的状态矩阵，输入矩阵和输出矩阵
然后是是预测，就是将优化问题转化为QP问题
PHI=[];
THETA=[];
for j=1:1:Np
PHI=[PHI;C*A^j];
for k=1:1:Nc
THETA=[THETA;C*A^(j-k)*B];
end
end
error=PHI*kesi;
Q=100*eye(Nx*Np,Nx*Np);
R=5*eye(Nu*Nc);
H=[THETA'*Q*THETA+R,zeros(Nu*Nc,1);
zeros(1,Nu*Nc),Row];
f=[2*error'*Q*THETA,0];
PHI和THETA分别是输出方程的系数，error就是E，Q,R是人为调试的参数，H和f就是QP问题中的参数
然后是处理约束
%% 约束
%不等式约束
A_t=zeros(Nc,Nc);
for p=1:1:Nc
for q=1:1:Nc
if q<=p
A_t(p,q)=1;
else
A_t(p,q)=0;
end
end
end
A_I=kron(A_t,eye(Nu));
Ut=kron(ones(Nc,1),[U1;U2]);
umin=[-0.1;-0.08];
umax=[0.1;0.08];
delta_umin=[-0.02;-0.002;];
delta_umax=[0.02;0.002];
Umin=kron(ones(Nc,1),umin);
Umax=kron(ones(Nc,1),umax);
A_cons=[A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1)];
b_cons=[Umax-Ut;-Umin+Ut];
%状态量约束
M=10;
delta_Umin=kron(ones(Nc,1),delta_umin);
delta_Umax=kron(ones(Nc,1),delta_umax);
lb=[delta_Umin;0];
ub=[delta_Umax;M];
使用matlab自带的quadprog求解器求解QP问题
%% 求解二次规划问题
opts = optimoptions('quadprog','Algorithm','active-set');
x_start=zeros(2*Nc+1,1);
[X,fval,exitflag]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,x_start,opts);
最后得到输出
%% 计算输出
U1_=kesi(4)+X(1);%上一时刻输出误差加优化结果得到本时刻误差
U2_=kesi(5)+X(2);
v_des=U1_+vd1;%误差加参考值得到最终输出值
delta_des=U2_+vd2;
delta_t=toc(tstart);%计时结束
最后的输出=上一时刻的偏差【U（k-1）-Ur（k-1）】+优化求得的结果ΔU+参考值Ur（k）
三、仿真验证
车辆初始位置：SV_XO 9.423；SV_YO 1.1
仿真时间设置为15s，仿真效果如下，贴个视频(背景音乐有点吵)
关于参数的说明
Np越大，系统的稳定性越高，响应速度慢；Nc越大，控制灵敏度越高，但系统的稳定性和鲁棒性会下降；Q和R矩阵越大，说明误差状态量和输入的约束权重越大。
参数的调整是很重要的，需要更多的经验来体会！
