机器人中的数值优化（二）

文档维护：Arvin

网页部署：Arvin

▶

写在前面：本文内容是作者在深蓝学院机器人中的数值优化学习时的笔记，作者按照自己的理解进行了记录，如果有错误的地方还请执政。如涉侵权，请联系删除。

无约束优化

拟牛顿法

为什么要用拟牛顿法？

一般情况下，当函数为曲线平滑的凸函数时，我们使用牛顿法。牛顿法如下：

通过二阶泰勒展开：

$$f(\boldsymbol{x})\approx\hat{f}\left(\boldsymbol{x}\right)\triangleq f(\boldsymbol{x}_k)+\nabla f(\boldsymbol{x}_k)^T(\boldsymbol{x}-\boldsymbol{x}_k)+\frac12(\boldsymbol{x}-\boldsymbol{x}_k)^T\nabla^2f(\boldsymbol{x}_k)(\boldsymbol{x}-\boldsymbol{x}_k) \tag{1}$$

最小化二次近似：

$$\begin{aligned}&\nabla\hat{f}\left(\boldsymbol{x}\right)=\nabla^2f(\boldsymbol{x}_k)(\boldsymbol{x}-\boldsymbol{x}_k)+\nabla f(\boldsymbol{x}_k)=\mathbf{0}\\&\Longrightarrow\boldsymbol{x}=\boldsymbol{x}_k-\left[\nabla^2f(\boldsymbol{x}_k)\right]^{-1}\nabla f(\boldsymbol{x}_k)\end{aligned} \tag{2}$$

规定：

$$\nabla^2f(\boldsymbol{x}_k)\succ\boldsymbol{O} \tag{3}$$

牛顿步长：

$$\boldsymbol{x}_{k+1}=\boldsymbol{x}_k-\left[\nabla^2f(\boldsymbol{x}_k)\right]^{-1}\nabla f(\boldsymbol{x}_k) \tag{4}$$

即：

$$\boldsymbol{x}_{k+1}=\boldsymbol{x}_k-H^{-1}_{t}g_{t} \tag{5}$$

牛顿法的一些问题：

• 当当前点距离极小点距离较远时，使用牛顿法需要计算二阶海森矩阵，造成计算力浪费。
• 海森矩阵需要满足很多条件，当当前点在线性函数上时，海森矩阵值为零，则步长就会变为无穷大。
• 当函数为非凸函数，牛顿法可能会向更高点迭代。

牛顿法近似，海森矩阵需要正定可逆：

$$f(x)-f\left(x_{k}\right) \approx\left(x-x_{k}\right)^{T} g+\frac{1}{2}\left(x-x_{k}\right)^{T} H\left(x-x_{k}\right) \tag{6}$$ $$\text { Solve } H_{k} d_{k}=-g_{k}$$

因为迭代需要沿着梯度下降的方向，所以在几何上搜索方向要与负梯度成锐角：

$$\langle-g,-H^{-1}g\rangle=\langle g,H^{-1}g\rangle=g^TH^{-1}g>0 \tag{8}$$

因此只有海森矩阵正定可逆时，使用牛顿法才不会出现上述的问题（向更高点迭代，步长变为无穷大）。

如何构造拟牛顿法？

拟牛顿法近似

$$f(x)-f(x_k)\approx\left(x-x_k\right)^Tg_k+\frac12\left(x-x_k\right)^TM_k{\left(x-x_k\right)} \tag{9}$$ $$\text { Solve } M^{k} d^{k}=-g^{k}$$

拟牛顿法的目的就是为了避免上述问题，同时避免求逆节约计算资源，加快运算速度。我们构造一个矩阵来逼近海森矩阵或者他的逆。

为了方便推导，我们假设$f(x)$是二次函数，此时海森矩阵$H$是常数矩阵，任意两点$x_k$与$x_{k+1}$处的梯度之差是：

$$\bigtriangledown f(x_{k+1})-\bigtriangledown f(x_{k})=H\cdot(x_{k+1}-x_{k}) \tag{10}$$

即：

$$x_{k+1}-x_{k}=H^{-1}\cdot[\bigtriangledown f(x_{k+1})-\bigtriangledown f(x_{k})] \tag{11}$$

对于非二次型的情况，也近似满足上式关系。

因为：

$$\begin{aligned}\Delta x&=x_{k+1}-x_k\\\Delta g&=\nabla f{\left(x_{k+1}\right)}-\nabla f{\left(x_k\right)}\end{aligned} \tag{12}$$

我们可得：

$$\Delta g\approx M^{k+1}\Delta x$$

or：

$$\Delta x\approx B^{k+1}\Delta g \tag{13}$$

其中，$M_{k+1}B_{k+1}=I$

以上就是拟牛顿条件，不同的拟牛顿法，区别就在于如何逼近不同的矩阵。

DFP法逼近的是$H^{-1}$。

BFGS法逼近的是$H$。

DFP法

DFP是用$D_k$来近似海森矩阵的逆矩阵$H_{k}^{-1}$

推导

现在已知拟牛顿条件：

$$\Delta x_k=D_{k+1}\cdot\Delta g_k \tag{14}$$

假设已知$D_k$，希望用叠加的方式求$D_{k+1}$，即$D_{k+1}=D_k+\Delta D_k$，代入得到：

$$\Delta D_k\Delta g_k=\Delta x_k-D_k\Delta g_k \tag{15}$$

假设满足这个等式的$\Delta D_k$是这样的形式（我也不知道为什么要这么假设，可能是为了保证正定可逆）：

$$\Delta D_k=\Delta x_k\cdot q_k^T-D_k\Delta g_k\cdot w_k^T \tag{16}$$

对照可以得到：

$$q_k^T\cdot\Delta g_k=w_k^T\cdot\Delta g_k=I_n \tag{17}$$

要保证$\Delta D_k$是对称的，参照其表达式，最简单的就是令：

$$\begin{gathered}q_k=\alpha_k\Delta x_k\\w_k=\beta_kD_k\Delta g_k\end{gathered} \tag{18}$$

可得：

$$\begin{gathered}\alpha_k=\frac1{\Delta x_k^T\Delta g_k}\\\beta_k=\frac1{\Delta g_k^TD_k\Delta g_k}\end{gathered} \tag{19}$$

然后带入式16可得：

$$\Delta D_k=\frac{\Delta x_k\Delta x_k^T}{\Delta x_k^T\Delta g_k}-\frac{D_k\Delta g_k\Delta g_k^TD_k}{\Delta g_k^TD_k\Delta g_k} \tag{20}$$

虽然式子看起来很复杂但是并不涉及求逆，第一项仅涉及向量乘法，第二项仅涉及矩阵乘法。

DFP算法步骤

(1) 给定初始点$x_0$，精度$\epsilon$，令$D_0=I$

(2) 计算搜索方向

$$d_{k}=-D_{k}\nabla f(x_{k})$$

(3) 从当前点出发，沿着搜索方向做一维搜索，获得最优步长并更新参数：

$$\lambda_k=\arg\min_\lambda f(x_{k}+\lambda\cdot d_{k})$$ $$x_{k+1}=x_{k}+\lambda_k\cdot d_{k}$$

(4)判断精度，若$|g_{k+1}|<\epsilon$则停止迭代，否则转 (5)

(5) 计算 $\Delta g=g_{k+1}-g_k$, $\Delta x=x_{k+1}-x_{k}$,更新 $D$

$$\begin{aligned}D_{k+1}=D_k+\frac{\Delta x_k\Delta x_k^T}{\Delta x_k^T\Delta g_k}-\frac{D_k\Delta g_k\Delta g_k^TD_k}{\Delta g_k^TD_k\Delta g_k}\end{aligned}$$

(6)$\quad k=k+1$,转(2)

BFGS法

BFGS是最流行的拟牛顿算法。与DFP算法相比，性能更佳。它是构建一个矩阵来逼近海森矩阵$H_k$。

推导

$$\Delta g_t=M_{k+1}\cdot\Delta x_k \tag{21}$$

和DFP类似，我们可以得到

$$\Delta M_k=\frac{\Delta g_k\Delta g_k^T}{\Delta g_k^T\Delta x_k}-\frac{M_k\Delta x_k\Delta x_k^TM_k}{\Delta x_k^TM_k\Delta x_k} \tag{22}$$

下面的推导需要用到Sherman-Morrison公式：

假设$A$是$n$阶可逆矩阵，$t$是常量，$u, v$是$n$维向量，且$A+uv^T$也是可逆矩阵，则

$$(A+\frac{uv^T}{t})^{-1}=A^{-1}-\frac{A^{-1}uv^TA^{-1}}{t+v^TA^{-1}u} \tag{22}$$

应用两次上述公式可以得到：

$$M_{k+1}^{-1}=\left(I-\frac{\Delta x\Delta g^T}{\Delta g^T\Delta x}\right)M_k^{-1}\left(I-\frac{\Delta g\Delta x^T}{\Delta g^T\Delta x}\right)+\frac{\Delta x\Delta x^T}{\Delta g^T\Delta x}$$

且：$B_K = M_k^{-1}$，则有

$$B^{k+1}=\left(I-\frac{\Delta x\Delta g^T}{\Delta g^T\Delta x}\right)B^k\left(I-\frac{\Delta g\Delta x^T}{\Delta g^T\Delta x}\right)+\frac{\Delta x\Delta x^T}{\Delta g^T\Delta x}$$

BFGS算法步骤

严格凸函数

对于严格凸函数（strictly convex），伪代码如下：

不严格凸函数

Wolfe条件

• weak Wolfe conditions $$\begin{array}{c}f(x^k)-f(x^k+\alpha d)\geq-c_1\cdot\alpha d^\mathrm{T}\nabla f(x^k)\\d^\mathrm{T}\nabla f(x^k+\alpha d)\geq c_2\cdot d^\mathrm{T}\nabla f(x^k)\end{array}$$

​ 其中，$0<c_1<c_2<1$，通常$c_1=10^{-4},~c_2=0.9$。

• strong Wolfe conditions $$\begin{array}{c}f(x^k)-f(x^k+\alpha d)\geq-c_1\cdot\alpha d^\mathrm{T}\nabla f(x^k)\\\left|d^\mathrm{T}\nabla f(x^k+\alpha d)\right|\leq\left|c_2\cdot d^\mathrm{T}\nabla f(x^k)\right|\end{array}$$ 其中，$0<c_1<c_2<1$，通常$c_1=10^{-4},\mathrm{~}c_2=0.9$

cautious update

仅仅是wolfe条件无法保证收敛，BFGS算法还需要cautious update

$$B^{k+1}=\begin{cases}\left(I-\frac{\Delta x\Delta g^T}{\Delta g^T\Delta x}\right)B^k\biggl(I-\frac{\Delta g\Delta x^T}{\Delta g^T\Delta x}\biggr)+\frac{\Delta x\Delta x^T}{\Delta g^T\Delta x}&&\mathrm{~if~}\Delta g^T\Delta x>\epsilon||g_k||\Delta x^T\Delta x,&\epsilon=10^{-6}\\B^k&&\mathrm{~otherwise}\end{cases}$$

具体步骤：

LBFGS法

BFGS公式：

$$B^{k+1}=\left(I-\frac{\Delta x\Delta g^T}{\Delta g^T\Delta x}\right)B^k\left(I-\frac{\Delta g\Delta x^T}{\Delta g^T\Delta x}\right)+\frac{\Delta x\Delta x^T}{\Delta g^T\Delta x}$$

我们使用BFGS算法来利用单位矩阵逐步逼近H矩阵，但是每次计算都要储存B矩阵，如果维度特别大，就会占用许多内存。我们可以用时间换空间的方法，我们不储存B矩阵，而是储存$\Delta x$和$\Delta g$。比如数据集有10000个维度，由储存10000×10000的矩阵变为了储存是个1×10000的10个向量，有效储存了空间。

并且有时候需要迭代许多轮，我们规定只存储m个回合的的数据。

• Lewis & Overton line search:

​ weak Wolfe conditions

$$\begin{gathered}S(\alpha):f{\left(x^k\right)}-f{\left(x^k+\alpha d\right)}\geq-c_1\cdot\alpha d^\mathrm{T}\nabla f{\left(x^k\right)}\\C(\alpha):d^\mathrm{T}\nabla f{\left(x^k+\alpha d\right)}\geq c_2\cdot d^\mathrm{T}\nabla f{\left(x^k\right)}\end{gathered}$$

综上对于非凸非光滑函数：

共轭梯度法

作业

BFGS

环境

ROS-Melodic

运行

mkdir catkin_ws
cd catkin_ws
mkdir src
cp gcopter /catkin_ws/src
catkin_make
source ./devel/set.bash
roslaunch gcopter curve_gen.launch

结果

说明

1. 三次样条（Cubic Spline）:

   参考链接：三次样条

   $$\begin{aligned} \rho(s) &= a_i + b_is_i + c_is_i^2 + d_is_i^3,\ s_i\in(0,1)\\ \rho(s)^{'} &=b_i + 2c_is_i + 3d_is_i^2,\\ \rho(s)^{''} &=2c_i + 6d_is_i \end{aligned}$$

   and natural spline is: $\rho(s_0)^{‘’} = \rho(s_N)^{‘’} = 0$.

2. 目标函数:

   $$\begin{aligned} \text{cost}&=\text{Energy}(x_1,...,x_{N-1}) + \text{Potential}(x_1,...,x_{N-1}) \\ &= \sum_{i=0}^{N}(4c_i^2 + 12c_id_i + 12d_i^2)\ +\ 1000\sum_{i=1}^{N-1}\sum_{j=1}^{M}\left(\max(r_j - \|x_i - o_i\|,0)\right) \\ \text{where},&\\ x_i & = [\bar x_i, \bar y_i]_{1\times2}\\ a_i &= x_i\\ b_i &= D_i\\ c_i &=-3(x_i - x_{i+1}) -2D_i - D_{i+1}\\ d_i &= 2(x_i - x_{i+1}) + D_i + D_{i+1}\\ \text{and, here:}&\\ &\begin{bmatrix} D_1\\ D_2\\ ...\\D_{N-1} \end{bmatrix}_{(N-1)\times 2} = \begin{bmatrix} 4 &1 \\ 1 & 4 & 1 \\& ...& ...& ...\\& &1 &4 &1 \\ &&&1 &4 \end{bmatrix}^{-1}_{(N-1)\times(N-1)} \begin{bmatrix} 3(x_2-x_0)\\ 3(x_2-x_0)\\ ...\\3(x_N-x_{N-2}) \end{bmatrix}_{(N-1)\times 2} \end{aligned}$$

3. 计算梯度:

   • Energy Gradient

$$\begin{aligned} \frac{\partial E}{\partial x}|_{x_1,..,x_{N-1}}&= \sum_{i=0}^{N-1}[ (8c_i + 12d_i)\frac{\partial c_i}{\partial x} + (12c_i + 24d_i)\frac{\partial d_i}{\partial x} ]\\ \text{where},&\\ \frac{\partial c_i}{\partial x}|_{x_1,..,x_{N-1}}&= -3\frac{\partial (x_i - x_{i+1})}{\partial x} -2\frac{\partial D_i}{\partial x} - \frac{\partial D_{i+1}}{\partial x}\\ \frac{\partial d_i}{\partial x}|_{x_1,..,x_{N-1}}&= 2\frac{\partial (x_i - x_{i+1})}{\partial x} +\frac{\partial D_i}{\partial x} + \frac{\partial D_{i+1}}{\partial x}\\ \frac{\partial}{\partial x}\begin{bmatrix} x_0 - x_1\\ x_1 -x_2 \\...\\ x_N - x_{N-1}\end{bmatrix}&= \begin{bmatrix} -1 \\ 1 & -1 \\& ...& ...\\& &1 &-1 \\ &&&1 &-1 \end{bmatrix}_{(N-1)\times(N-1)} \end{aligned} \\ \frac{\partial D_i}{\partial x} = \begin{bmatrix} 4 &1 \\ 1 & 4 & 1 \\& ...& ...& ...\\& &1 &4 &1 \\ &&&1 &4 \end{bmatrix}^{-1}_{(N-1)\times(N-1)} \begin{bmatrix} 0 &3 \\ -3 & 0 & 3 \\& ...& ...& ...\\& &-3 &0 &3 \\ &&&-3 &0 \end{bmatrix}_{(N-1)\times(N-1)}$$

• Potential Gradient $$\begin{aligned} \frac{\partial P}{\partial \bar x}|_{\bar x_1,..,\bar x_{N-1}} &= -1000\sum_{j=1}^M\frac{\bar x_i - a_j}{\sqrt{(\bar x_i- a_j)^2+(\bar y_i -b_j)^2}}\\ \frac{\partial P}{\partial \bar y}|_{\bar y_1,..,\bar y_{N-1}} &= -1000\sum_{j=1}^M\frac{\bar y_i - a_j}{\sqrt{(\bar x_i- a_j)^2+(\bar y_i -b_j)^2}} \end{aligned}$$

1. Lewis & Overton line search

参考链接：

拟牛顿法（DFP、BFGS、L-BFGS

牛顿法和拟牛顿法

BFGS算法的迭代公式推导