[欠驱动机器人]4,动态规划(Dynamic Programming)

前言

之前的章节中，我们考虑了单摆的相平面，并且把控制单摆的问题解释如下：我们能否做很少的操作就可以改变相平面的向量流，从而达到把不稳定不动点变成稳定点的不动点？在输入力矩不限制的条件下这个很好完成（反转重力方向），但是这样输入的力矩太大了，没有必要。通过控制能量的方法可以避免这个无意义的耗费，但是这个对模型的需求比较大，对于复杂模型未必还能使用类似的方法。本节我们介绍其他的优化方法来完成反馈控制，来解决这个问题。

控制问题变成优化问题

本节介绍一种优化控制即使用优化的方法来实现控制。这种方法的优点有以下几点：

• 普遍，对于欠驱动系统、全驱动系统，线性系统、非线性系统都可以使用优化方法来求解
• 这种方法可以把复杂的问题简化成一个目标函数，一个优化问题
• 优化问题可以使用数值方法求解
  控制问题变成优化问题的基本思想是把控制达成的目标变成优化中的罚函数（cost function），接下来使用一个双积分器的例子来说明这个思想

例子4.1
考虑输入受限的双积分器，动力学方程如下：
$$\begin{gathered} \ddot{q}=u, \\ |u|<1 \end{gathered}$$
可以把上边的非线性方程理解成1kg的质点在无摩擦的x轴上运动，输入的力是u，本次控制的目标是设置一个控制函数u，使得直线在x=0处静止。
为了把这个问题变成一个优化问题，我们需要定义一个价值函数，这个价值函数给控制策略进行打分。对于本例这个问题，我们可以定义很多种价值函数，比如：
1,最短时间：$$\underset{\pi }{\min }{t}_f,subjectto\mathbf{x}\left(t_0\right)={\mathbf{x}}_0,\mathbf{x}\left(t_f\right)=0.$$
2,二次代价函数：$$\underset{\pi }{\min }{\int }_{t_0}^{\infty }\left[{\mathbf{x}}^T(t)\mathbf{Q}\mathbf{x}(t)\right]dt,\mathbf{Q}\succ 0.$$
第一个是优化的时间，会得到时间最短的值，第二个则惩罚每个x不为0的点，因此促使轨迹更加直接的接近0，这样可能会花费更多时间（事实上，第二种控制方法会导致指数逼近0，严格来讲达到0需要无穷长时间，而第一种则会在有限的时间内达到目标点。）需要注意的是，只有当系统能够到达原点附近时，这两个优化问题才会有解，否则对于最小时间问题，系统无法满足终止条件，对于二次代价函数问题，代价函数不能收敛到一个定值。
还要注意我们要求输入力小于1也要进入到约束中，否则优化策略会返回一个无限大的力，使得质量块十分迅速的到达原点，除了限制输入的幅值，还有一种方法来减小输入的方法是对控制也加一个罚函数，比如：$$\int \left[{\mathbf{u}}^T(t)\mathbf{R}\mathbf{u}(t)\right]dt$$

优化控制对机器人控制来说已经有很长的历史了，例如，在拾取-放置机器人操作机的最短时间问题上已经有了很长时间的发展。而LQR算法对很多控制机器来说更是基础算法。

新增成本(Additive cost)

当我们为了优化控制发展理论和算法工具是，我们会发现有一些公式比其它的更加好解。举一个例子就是可以将优化目标函数用新增成本的方法来建模，这样他们通常可以导致一个递归解，在新增函数中，轨迹的分数可以表示成：
$${\int }_0^T\ell (x(t),u(t))dt$$
式中l是瞬时成本，T可以是有限量可以是无穷量。当T是无穷量时，整个解法就十分的简洁，但是这个需要保证积分最后时收敛的。
上边提到的二次代价成本显然时新增成本形式的，而最小时间成本看起来不是，但是我们可以将之改写成以下形式：$$\ell (x,u)=\{\begin{array}{ll}0& \text{ if }x=0\\ 1& \text{ otherwise }\end{array}$$
我们会在接下来的几章来寻找解决优化问题的方法，本节中，我们使用动态规划的方法来解决新增成本的优化问题

图搜索的最优控制

对于连续系统，动态规划是一套理论思想，然而对于有限离散系统，动态规划是一种可以使用最优反馈控制的数值算法，可以是图搜索的一个工具。

例如上图，有N个节点s1-sN，而控制的行为是a1…，假设每个边（控制行为）都有成本l(s,a)，s是当前节点，a是所采取的行动，而且我们可以将系统动力学表示为：
$$s[n+1]=f(s[n],a[n])$$
在离散的图上，有很多种算法都可以找到从起始点到目标点的最优路径，核心思想是对于每个节点，我们都可以通过递归计算最优成本找到最优路径。假设所有的节点的成本都是0，而后使用如下的递归算法：
$${\hat{J}}^{\ast }\left(s_i\right)\Leftarrow \underset{a\in A}{\min }\left[\ell \left(s_i,a\right)+{\hat{J}}^{\ast }\left(f\left(s_i,a\right)\right)\right]\text{\hspace{1em}(1)}$$
如果f(s,a)对每个状态-行为都有定义，那么算法会更加简单。我们可以把目标状态的成本定义为0。J可以表示成一个离散状态的个数维的向量。这个算法也叫数值迭代，会收敛到最优解加上一个常数，而且在实操中，收敛的速度非常快。并且有同步和异步两种方法，同步就是把图中所有的节点成本都算一遍然后更新数值，异步就是没算一次刷新一次，异步也是收敛的。假设图有一个目标状态的成本为0，那么这个代价去函数表示到目标的加权最短距离。
数值迭代是一种非常简单的算法，但是能做的事情很多，它能够高效的计算每个离散状态的最小成本，一旦我们知道了最优成本控制的策略也很容易得出了：
$${\pi }^{\ast }\left(s_i\right)={\mathrm{argmin}}_a\left[\ell \left(s_i,a\right)+J^{\ast }\left(f\left(s_i,a\right)\right)\right].\text{\hspace{1em}(2)}$$
需要注意的是，我们在使用数值迭代时，对连续的世界进行了离散化，因此使用数值迭代算法得到的最优解和真正的最优解之间是有误差的。但是这种解法也有一个好处，它可以轻松的适应所有的同维度系统而基本不需要改变程序。

连续动力学方程

图搜索算法是个非常好的算法，但是随着系统维度的上升，这个算法的计算复杂度是指数上升的。在很多情况中，我们使用别的算法，不离散系统，在连续的空间完成计算。需要记住，我们一直要做的事情是，对于指定的动力学系统，自定义给出顺势成本函数，然后就可以得到控制策略，下边的结构有一些绕，但是有这个目标的指引，还是不会迷失的。

HJB 方程

上边我们提到了数值迭代的方法，但是1和2式不单单是一个优化算法，还是优化的一个充分条件，如果我们能找到J和Π满足这两个式子，那么Π就一定是一个优化控制器。连续系统有类似的表达形式，就是下面这个吓人的(Hamilton-Jacobi-Bellman Equation)HJB方程(我觉得这个方程可以别太在意怎么推出来的，会用就好，需要在意的是，f是系统动力学方程，l是我们人为加入的瞬时成本，类似离散版本的，J是一个从状态空间到成本函数的映射)：
0 = min ⁡ u [ ℓ ( x ,