最优控制求解停车问题

1、停车问题1 问题描述如下图所示，考虑一个 10m*7m 大小的停车场，将车从如下初始位置停到停车场中任意位置，如图停车位置 1 4 所示。65停车位置2停车位置143)m(y停车位置3停车位置4210初始位置-1-4-3-2-1012345-5x(m)图 1 问题描述示意图2 数学模型车的模型示意图如下所示，则可以得到如下数学模型方程式：x1tx5 cos x3x2tx5 sinx3x3tx5 tanx4（ 1）x4tu1x5tu2x5x4x2x3x1图 2 车的模型示意图3 问题求解3.1 变分法求解最优控制定义如下性能指标函数：t f 12（ 2）Ju t dtt 0 2通过构建 Hami

2、ltonian 求解，并采用数值法求解两点边值问题。（ 1）停车位置1：6543)m(y210-1-4-3-2-1012345-5x(m)21.5u1u21.51x3- 航 向 角x4- 前 轮 转 向 角1x5- 速 度0.50.500-0.5-0.5-1-1012345678012345678t(s)t(s)图 3 变分法求解结果（停车位置1）（ 2）停车位置36543)m(y210-1-5-4-3-2-1012345x(m)22u1x3- 航 向 角u2x4- 前 轮 转 向 角1.51.5x5- 速 度110.50.500-0.5-0.5-11234567-101234560t(s)t

3、(s)图 4 变分法求解结果（停车位置3）3.2 动态规划法求解最优控制定义如下性能指标函数：Jbx t fxf2t f2t012u t dt（ 3）2采用离散动态规划求解， 分别将时间、状态量、控制量、 状态方程和性能指标函数离散化。分别尝试不同的离散化程度。（ 1）第一次离散化求解：运行时间（1120s）离散化后的维度为：时间（150）；状态（ 4*10*2*3*6 ）；控制（ 6*6 ）1.510.50-0.1-0.08-0.06-0.04-0.0200.02-0.121.81x3u11.6x40.8u2x50.61.41.20.410.20.80-0.20.6-0.40.4-0.60.

4、2-0.8020406080100120140160-10050100150图 5 动态规划求解结果（第一次离散化）（ 2）第二次离散化求解：运行时间（ 70291s）离散化后的维度为：时间（30）；状态（ 10*25*12*16*15）；控制（ 15*15 ）图 6 动态规划求解结果（第二次离散化）3.3 直接打靶法求解最优控制定义如下性能指标函数：t fJt012u t dt（ 4）2采用 SQP方法求解，结果如下：（ 1）停车位置11.61.41.216543)m(y210-1-4-3-2-1012345-5x(m)0.60.4x3- 航 向 角x4- 前 轮 转向 角0.2x5- 速

5、度u1u20.800.60.4-0.20.2-0.40-0.6-0.2-0.4-0.8012345678012345678t(s)t(s)图 7 直接打靶法求解结果（停车位置1）（ 2）停车位置36543)m(y210-1-5-4-3-2-1012345x(m)1.60.8x3- 航 向 角u11.4x4- 前 轮 转 向 角0.6u2x5- 速 度1.210.40.80.20.600.4-0.20.20-0.4-0.2-0.6-0.4-0.8012345670123456t(s)t(s)图 8 直接打靶法求解结果（停车位置3）3.4 模型预测控制求解最优控制定义如下性能指标函数：5q xi

6、k : k Np 1 xi ref22r uj k : k2Jk : k Np 1Np1i 12Qj 1 2R（ 5）预测步长 N p =20 ，控制步长 Nc1。（ 1）停车位置 11.61.41.210.80.60.40.26543)m(y210-1-4-3-2-1012345-5x(m)1.5x3- 航 向 角x4- 前 轮 转 向 角1x5- 速 度0.50u1u20-0.5-0.2-0.4246810121416-124681012141600t(s)t(s)图 9 模型预测控制求解结果（停车位置1）（ 2）停车位置36543)m(y210-1-5-4-3-2-1012345x(m)

7、22x3- 航 向 角u1x4- 前 轮 转 向 角u21.5x5- 速 度1.5110.50.500-0.5-0.52468101214-1024681012140t(s)t(s)图 10 模型预测控制求解结果（停车位置3）3.5 自适应动态规划求解最优控制（尝试）首先采用经典的HDP92 方法进行尝试，但多次试验的效果都不好；之后尝试每次用数值法求解最优的控制量， 但效果依旧很差； 然后采用值迭代的方法， 并尝试用二次型近似值函数和神经网络近似值函数两种方式， 但是最后的效果依旧很差 （包括一般值迭代和广义值迭代）。（上述方法对 PPT上的例子，效果还不错）4 结果分析变分法求解最优控制：

8、 针对本问题这样一个相对复杂的模型， 变分法无法很好地处理控制量和状态量的约束。 而且在求解过程中， 由于无法得到解析解， 因而需要采用数值法求解一个两点边值问题。 而在采用 BVP4C求解时， 需要经过多次初值选择试凑， 才能保证可解，否则 BVP4C 无法求解。因此只能采用自己编写打靶法程序求解，致使最后的求解结果不是特别好（速度在刚开始一下增加到很大，即加速度过大；针对停车位置3，路径曲线不是很光滑）。另外，所得结果是一个开环控制。离散动态规划求解最优控制：如果对时间、状态、控制的离散化程度太低，则求解结果很差； 当增加离散化程度，求解结果有一定的改善，但还远远不够，需要继续提高离散化程

9、度。然而， 由于本问题状态量的维数较大，此时将导致维数灾难问题。第二次离散化求解的运行时间为 70291s（ 19 个多小时），如果继续增加离散化程度，虽然求解结果会更加好，但求解时间会更长。而针对本问题，如此长的求解时间是没有太多价值的。直接打靶法求解最优控制：相比变分法， 求解结果更加好。而且可以比较好地处理控制量和状态量的约束。但是，最后时刻的控制并不是0，而在实际停车过程中最后时刻控制应该为 0。另外，所得结果是一个开环控制。模型预测控制求解最优控制：同样可以比较好地处理控制量和状态量的约束。而且相比直接打靶法求解，最后时刻的控制能缓慢变为0。另外，所得结果是一个近似闭环控制，这在实际停车过程中是很有必要的，因为车的每一步运动都会有误差。自适应动态规划求解最优控制：我们尝试了几种自适应动态规划的方法，但效果都不是很好。针对PPT上的例子，这些方法的效