langxin11 · 学习笔记

显式欧拉法#

经典四阶 Runge–Kutta（RK4）#

此前的笔记遗漏了中间两项的系数，并把最后一项误写为 $k_3$；上式为修正后的表达。

隐式中点法#

由于右端含有未知的 $x_{k+1}$，每一步通常需要求解隐式方程。原先写的 $x_{k+1}=x_k+h f(x_{k+1},u_k)$ 是后向欧拉法，不是隐式中点法。

比较积分方法时，应同时考察步长、误差、稳定性和每一步的求解成本。课程材料见 Dynamics Discretization & Stability。

基础概念（梯度，雅可比矩阵，Hessian矩阵）#

1. 标量函数$f:\mathbb{R} ^n→\mathbb{R}$的梯度向量(Gradient)

示例$f(x,y)=x^2+y^3$的梯度为$\nabla{f}=\left[2x,3y^2\right]^\top$

2. 向量值函数$\mathbf{F}(\mathbf{x}):\mathbb{R} ^n→\mathbb{R}^m$的雅可比矩阵(Jacobian)

$\mathbf{F}(x, y) = \begin{bmatrix} x^2 y \\ \sin y \end{bmatrix}$的雅可比矩阵为

3. 标量函数$f:\mathbb{R} ^n→\mathbb{R}$的Hessian 矩阵：$n\times n$对称矩阵

   $$\mathbf{H}_f = \begin{bmatrix} \frac{\partial^2 f}{\partial x_1^2} & \cdots & \frac{\partial^2 f}{\partial x_1 \partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial^2 f}{\partial x_n \partial x_1} & \cdots & \frac{\partial^2 f}{\partial x_n^2} \end{bmatrix}$$
   • 标量函数梯度的雅可比矩阵即是Hessian矩阵
   • $m=1$时，雅可比矩阵退化为梯度的转置
   • 行主导和列主导向量函数的复合求导会导致链式法则的式子不一样，由于矩阵的维度不一致)

HW1-Q1#

Julia 求导————————ForwardDiff.jl

1. 标量函数$f(x)$对标量$x$的导数

1
import ForwardDiff as FD
2
function f(x)
3
    return x^2
4
end
5
x=randn()
6
dx=FD.derivative(f,x)

1. 标量函数$f(x)$对向量$X=[x_1,x_2,...,x_n]$的导数—雅可比矩阵/梯度
2. 向量函数$f(X)=[f_1(X),f_2(X),...,f_m(X),]$对向量$X=[x_1,x_2,...,x_n]$的Jacobian矩阵

求解一个非线性方程$f(x)=0$的方法（求根）#

• 不动点迭代法

  离散系统动力学求平衡点

  $$\begin{gathered} x_{k+1}=f(x_k,u_k)\\ f^*=x-f \end{gathered}$$

• Newton方法

  $$\begin{gathered} f(x+dx)=f(x)+\frac{df}{dx}\Delta x=0\\ \Delta x= -\frac{df}{dx}^{-1}f\\ x \leftarrow x+\Delta x\\ \text{Loop until convergence} \end{gathered}$$

在无约束优化问题中，可以用 Newton 法求解 $\nabla f(x)=0$（一阶必要条件）；驻点是否为局部极小值还需要进一步判断

HW1_S25_Q3 QP求解器（Log-Domain Interior Point Method）#

引入拉格朗日乘子

• $\mu \in \mathcal{R}^p$ 对应等式约束
• $\lambda \in \mathcal{R}^m$对应不等式约束（要求 $\lambda≥ 0$）

拉格朗日函数为：

KKT条件：梯度条件、原始可行性、对偶可行性、互补松弛条件

引入非负松弛变量 $s\geq0$，使得 $Gx-h=s$；内点迭代时取 $s>0$。

新的拉格朗日函数

这里使用 $\rho>0$ 的扰动互补条件；原始 KKT 的右侧应为零。令 $\lambda=\sqrt{\rho}e^{-\sigma},s=\sqrt{\rho}e^{\sigma}$（指数逐元素计算），得到内点中心路径上的方程。需要逐步减小 $\rho$ 才能逼近原问题，不能固定一个正数就宣称解满足原始互补条件。

定义关于 $z=[x;\mu;\sigma]$ 的残差向量 $r_\rho(z)$，及其 Jacobian $D r_\rho(z)$。残差向量不是 Jacobian，下面的分块矩阵也不是目标函数的 Hessian。

每个 Newton 步求解 $D r_\rho(z)\Delta z=-r_\rho(z)$，再配合线搜索和中心路径参数更新。这是算法推导，尚未构成完整求解器；终止时还需检查原始可行性、对偶可行性和互补残差。这里假设 $Q$ 对称半正定；一般非凸 QP 的 KKT 驻点不能直接认定为全局最优解。

这里的$P_0$和$P_1$代表原始KKT和IP_KKT的残差，也就是

砖块掉落仿真#

不考虑砖块的旋转，一个掉落的砖块的动力学方程可以写成

$$\begin{gathered} M \dot{v} + M g = J^T \mu \\ \text{ where } M = mI_{2\times 2}, \; g = \begin{bmatrix} 0 \\ 9.81 \end{bmatrix},\; J = \begin{bmatrix} 0 & 1 \end{bmatrix} \end{gathered}$$

其中，$v=[v_x;v_z]$ 是速度，$q=[q_x;q_z]$ 是位置，竖直向上为正；$\mu$ 是法向接触力。这里忽略旋转、摩擦和反弹，采用非穿透接触与 backward Euler 离散，不能直接当作一般刚体碰撞模型。

$$\begin{bmatrix} v_{k+1} \\ q_{k+1} \end{bmatrix} = \begin{bmatrix} v_k \\ q_k \end{bmatrix}+ \Delta t \cdot \begin{bmatrix} \frac{1}{m} J^T \mu_{k+1} - g \\ v_{k+1} \end{bmatrix}$$

约束

$$\begin{align} J q_{k+1} &\geq 0 &&\text{(不会穿透地面)} \\ \mu_{k+1} &\geq 0 &&\text{(接触力方向只向上)} \\ \mu_{k+1} J q_{k+1} &= 0 &&\text{(没接触无接触力)} \end{align}$$

等价转换成如下的QP问题(可以通过KKT条件证明)

$$\begin{align} &\text{minimize}_{v_{k+1}} && \frac{1}{2} v_{k+1}^T M v_{k+1} + [M (\Delta t \cdot g - v_k)]^Tv_{k+1} \\ &\text{subject to} && J(q_k + \Delta t \cdot v_{k+1}) \geq 0 \\ \end{align}$$

引入拉格朗日乘子$\mu \geq 0$

$$\mathcal{L}=\frac{1}{2} v_{k+1}^T M v_{k+1} + [M (\Delta t \cdot g - v_k)]^Tv_{k+1} -\mu \cdot J(q_k + \Delta t \cdot v_{k+1})$$

KKT 条件

$$\begin{align} Mv_{k+1} + M(\Delta{t}\cdot g-v_k)-J^\mathrm{T}\mu\Delta t&=0\qquad\text{梯度条件}\\ J(q_k + \Delta t \cdot v_{k+1}) &\geq 0\qquad\text{原始可行性}\\ \mu &\geq 0\qquad\text{对偶可行性}\\ \mu \cdot J(q_k + \Delta t \cdot v_{k+1})&=0\qquad\text{互补松弛 } \end{align}$$

引入间隙松弛变量 $s=J(q_k+\Delta t\,v_{k+1})$，并令 $\mu=\sqrt{\rho}e^{-\sigma}$、$s=\sqrt{\rho}e^{\sigma}$，使 $\mu s=\rho$。这是与上文一致的数值参数化；在本例中 $\rho$ 带有接触力乘间隙的单位，实际实现还应考虑尺度归一化。

$$\begin{align} Mv_{k+1} + M(\Delta{t}\cdot g-v_k)-J^\mathrm{T}\sqrt{\rho}e^{-\sigma}\Delta t&=0\\ J(q_k + \Delta t \cdot v_{k+1}) -\sqrt{\rho}e^{\sigma}&= 0\\ \end{align}$$