【Unity学习笔记】第十九 · 物理引擎约束求解解惑（LCP，最优，拉格朗日乘数法，SI，PGS，基于冲量法）

转载请注明出处: https://blog.csdn.net/weixin_44013533/article/details/140309494

作者：CSDN@|Ringleader|

在学习物理引擎过程中，有几大问题一直困扰着我：

1. 约束求解到底是LCP还是带约束最优问题？
2. 约束求解过程中拉格朗日乘数法和带约束优化中的拉格朗日乘数法是一个东西吗？
3. 基于力的约束求解、基于冲量、基于位置的约束求解区别？
4. SI和PGS的区别？GS与PGS区别？

最优问题

上图给出了约束的两种形式，碰撞约束（不等式约束）和关节约束（等式约束）。解决约束的方式都是沿着碰撞法线或者连杆方向施加力或者冲量，使之满足约束。

现在的问题是，满足约束的方式有很多，为什么一定要沿着约束函数梯度方向？除非你认为沿着梯度方向解决约束最快或者说引入能量最小！

这意味着你默认这就是最优问题！

那么可能的最优目标是什么呢？如果从解决约束移动距离最小来看目标函数$f(x)$就是$\Delta x$，如果是从引入能量最小来看目标函数$f(x)$就是$\frac{1}{2}m\Delta v^2$，写成$\Delta x$形式就是$\frac{1}{2h^2}m\Delta x^2$（$h$为时间步长），写成矩阵的二次型形式就是$\frac{1}{2h^2}\Delta x^{T}M\Delta x$。（注：本文不讨论旋转）

因为有基于冲量和基于位置解决约束思路，我这里统一用能量最小约化目标。

那么约束求解就变成了带约束的二次规划问题了（先讨论等式约束），即：
$$\begin{gathered} minimizef(\Delta x)=\frac{1}{2h^2}\Delta x^{T}M\Delta x \\ s.t.C_i(x+\Delta x)=0,i\in N \end{gathered}$$

或者
$$\begin{gathered} minimizef(\Delta v)=\frac{1}{2}\Delta v^{T}M\Delta v \\ s.t.C_i(v+\Delta v)=0,i\in N \end{gathered}$$

基于位置的约束求解

线性化处理约束
$C_i(x+\Delta x)\approx C_i(x)+\nabla C_i(x)\Delta x$

那么 $C_i(x)+\nabla C_i(x)\Delta x=0$，即$\nabla C_i(x)\Delta x=-C_i(x)$

联立所有约束，
令 $J=[\nabla C_1(x),\nabla C_2(x),...,\nabla C_n(x){]}^T$，
令 $b=[-C_1(x),-C_2(x),...,-C_n(x){]}^T$,

那么原二次规划问题就变为：

$$\begin{gathered} minimizef(\Delta x)=\frac{1}{2h^2}\Delta x^{T}M\Delta x \\ s.t.J\Delta x=b \end{gathered}$$

使用拉格朗日乘数法求解，将带约束问题变为无约束问题：

拉格朗日函数：$L(\Delta x,\lambda )=\frac{1}{2h^2}\Delta x^{T}M\Delta x+{\lambda }^T(J\Delta x-b)$。

解
$$\{\begin{array}{l}\frac{\partial \mathcal{L}}{\partial \Delta x}=\frac{1}{h^2}M\Delta x+J^T\lambda =0(1)\\ \frac{\partial \mathcal{L}}{\partial \lambda }=J\Delta x-b=0(2)\end{array}$$

将第一项$\Delta x=-h^2{M}^{-1}{J}^T\lambda$ 带入第二项，得：$-h^{2}J{M}^{-1}{J}^T\lambda =b$，令$A=-h^{2}J{M}^{-1}{J}^T$
则解线性方程组$A\lambda =b$ 就可以得到λ，然后$\Delta x=-h^2{M}^{-1}{J}^T\lambda$就可求解。

如果A是可逆的，我们可以通过求逆来求解；如果不可逆，我们可以使用数值方法来求解，如高斯赛德尔、雅可比法、牛顿法等。

基于冲量的约束求解

$$\begin{gathered} minimizef(\Delta v)=\frac{1}{2}\Delta v^{T}M\Delta v \\ s.t.C_i(v+\Delta v)=0,i\in N \end{gathered}$$

同样，线性化约束 $C_i(v+\Delta v)\approx C_i(v)+\nabla C_i(v)\Delta v=0$，原问题转为：
$$\begin{gathered} minimizef(\Delta v)=\frac{1}{2}\Delta v^{T}M\Delta v \\ s.t.J\Delta v=b \end{gathered}$$
其中 $J=[C_1(v),C_2(v),...,C_n(v){]}^T$，$b=[-C_1(v),-C_2(v),...,-C_n(v){]}^T$，$n$为约束个数。

利用拉格朗日乘数法，$L(\Delta v,\lambda )=\frac{1}{2}\Delta v^{T}M\Delta v+{\lambda }^T(J\Delta v-b)$
解
$$\{\begin{array}{l}\frac{\partial \mathcal{L}}{\partial \Delta v}=M\Delta v+J^T\lambda =0(3)\\ \frac{\partial \mathcal{L}}{\partial \lambda }=J\Delta v-b=0(4)\end{array}$$
得$-J{M}^{-1}{J}^T\lambda =b$，求解线性方程组可得λ和Δv。

不等式约束情况

以基于冲量的约束求解为例，
$$\begin{gathered} minimizef(\Delta v)=\frac{1}{2}\Delta v^{T}M\Delta v \\ s.t.J\Delta v\le b \end{gathered}$$
运用KKT条件下的拉格朗日乘数法，

$L(\Delta v,\lambda )=\frac{1}{2}\Delta v^{T}M\Delta v+{\lambda }^T(J\Delta v-b)$
满足：
$$KKT条件\{\begin{array}{l}\frac{\partial \mathcal{L}}{\partial \Delta v}=M\Delta v+J^T\lambda =0(5)\\ J\Delta v-b\le 0(6)\\ \lambda \ge 0(7)\\ {\lambda }^T(J\Delta v-b)=0(8)\end{array}$$
第（8）项的条件称作互补松弛条件。

分两种情况，第一种就是λ=0，即f(x)本身极值就在约束内，类似于碰撞约束中已经分离的情况，此时Δv=0，无需施加冲量；
第二种就是λ>0，因为松弛互补约束，必须$J\Delta v-b=0$，这和等式约束相同。

所以对于不等式约束，同样可以使用类似等式约束的拉格朗日乘数法，但需要对λ值进行钳值，保证λ≥0即可（约束≤0则λ≥0，约束≥0则λ≤0）。

对于其中的数学解释，可以参考下面文章：

寻找“最好”（4）——不等约束和KKT条件
Karush-Kuhn-Tucker (KKT)条件

物理上的理解可以参考《基于物理的建模与动画10.3节》碰撞约束的例子，其中$a^{-}$是碰撞前的加速度，$a^{+}$是碰撞后的加速度。地面反作用力$f$和$a^{+}$就有类似的互补性，如果$f=J\lambda$就要求λ≥0，让对象至少不会往地面方向钻。

线性互补问题

从上面的分析可以看出，约束求解就是最优问题，很多文章虽然没有明确指出，但默认使用了上面的结论，也就是沿约束梯度方向，可以使约束求解引入的动能最小。

但为啥很多书籍和文章又称约束解决本质上是线性互补问题呢。
从前面不等约束求解使用KKT条件，其中的互补松弛条件能看出一些端倪。

我们先看一看在不使用优化思想下是怎么求解约束的。

约束力法

根据$\dot{C}=J\dot{q}=0$，以及约束力于速度正交（理想约束下约束力不做功）→$F_c^T\dot{q}=0$，得到$F_c$与$J$平行，所以$F_c=J^T\lambda$。
其中$F_c=J^T\lambda =\nabla C_1{\lambda }_1+\nabla C_2{\lambda }_2+\dots +\nabla C_m{\lambda }_m$，m是梯度的维度。

同时根据$\ddot{C}=0$得到：

$\ddot{C}=\dot{J}\dot{q}+J\ddot{q}=\dot{J}\dot{q}+J{M}^{-1}(F_{ext}+F_c)=\dot{J}\dot{q}+J{M}^{-1}(F_{ext}+J^T\lambda )=0$

则，$J{M}^{-1}{J}^T\lambda =-\dot{J}\dot{q}-J{M}^{-1}{F}_{ext}$

只有λ是未知的，由此解这个线性方程组就能得到约束力$F_c$。那么对应速度和位置通过积分就能获得。

这就是约束力法解约束的思路。

但这个只是无摩擦力的理想约束下，且为等式约束得出的公式，其他复杂情况是否能用类似拉格朗日乘数法暂未可知。

而且基于力的方式求解可能不适合接触约束，因为两个物体的碰撞时，互相之间的力变化很快，而且有一个很大的峰值；而且存在摩檫力约束时，循环求解时可能导致一个无穷大的值。（这段话出自游戏物理引擎(四) 约束）

基于冲量法

类似的。
施加额外冲量$I=M\Delta V={\int }_t^{t+\Delta t}Fdt$，半隐式欧拉法的话，力认为是恒定的，那么

$M\Delta V=F\Delta t=J^T\lambda \Delta t$，$\Delta V=M^{-1}{J}^T\lambda \Delta t$

根据$\dot{C}=J\dot{q}=JV=0$，其中$V=$

$\dot{C}=J(V_{init}+\Delta V)=J(V_{init}+M^{-1}{J}^T\lambda \Delta t)=0$ 推导出

$J{M}^{-1}{J}^T\lambda \Delta t=-J{V}_{init}$

解这个线性方程组就能得到λ和Δv了，对应Δx也能得到。

如果只是通过上面解得出Δv，仅仅保证速度不会违反约束，而要保证位置同样不会违反约束，可以在速度约束加上Baumgarte 项，即$JV+b=0$，其中 $b=-\beta \frac{e}{\Delta t}(0<\beta <1)$，e代表误差项，例如碰撞约束中就是穿透深度，β代表约束违反修正速度。

基于位置法

$C(x+\Delta x)\approx C(x)+\nabla C(x)\Delta x=0$，那么$J\Delta x=-C(x)$，其中$J=\nabla C(x)$
如果使用半隐式欧拉法，$\Delta x=v^{{}^{\prime }}\Delta t=(v_{init}+a\Delta t)\Delta t=(v_{init}+M^{-1}{J}^T\lambda \Delta t)\Delta t$,

带入上面得到：$J{v}_{init}\Delta t+J{M}^{-1}{J}^T\lambda \Delta t^2=-C(x)$，则

$J{M}^{-1}{J}^T\lambda \Delta t^2=-J{v}_{init}\Delta t-C(x)$，解这个线性方程组就能得到λ，然后Δx亦可得到。

赵航在基于约束的物理说$v_{init}=0$，我不知道是不是PBD不考虑速度还是怎么造成的，暂不做深究，思想都差不多。

比较约束求解中最优法和普通物理分析法

分别使用最优和不适用最优的方式，都独立给出了约束求解的方法。
比较一下，例如基于冲量的方法。
$$最优法\{\begin{array}{l}\frac{\partial \mathcal{L}}{\partial \Delta v}=M\Delta v+J^T\lambda =0\\ \frac{\partial \mathcal{L}}{\partial \lambda }=J\Delta v-b=0\end{array}$$
和
$$物理分析法\{\begin{array}{l}M\Delta V=F\Delta t=J^T\lambda \Delta t\\ \dot{C}=J(V_{init}+\Delta V)=0\end{array}$$
在不考虑 $b$ 项差异情况下，差别仅在λ，可以发现最优方式中的λ会暗含Δt项；

而基于位置的
$$\{\begin{array}{l}最优法h^{2}J{M}^{-1}{J}^T\lambda =C(x)\\ 物理分析法J{M}^{-1}{J}^T\lambda \Delta t^2=-J{v}_{init}\Delta t-C(x)\end{array}$$
可以看到更是非常接近。（不知道是不是推导的问题，感觉两个λ符号相反？问题不大，思路对的就行）

可以看出，用最优方法和物理分析法，得到的结果是类似的，但数学最优方式推导简洁优美，而且很容易处理不等约束，拉格朗日乘数法数学意义明确。

相反，普通物理推导方式，对于拉格朗日乘数法使用的正确性含糊其辞，且推导方式较复杂。

所以这波啊，是数学完胜！！！

问题解答

通过上面的研究，问题基本明晰，约束求解本质上就是最优问题，拉格朗日乘数法的使用，就是求解这种带约束的优化问题，而所谓的线性互补问题，就是KKT条件中的互补松弛条件，两种方法使用的拉格朗日乘数法思路都一致，可能在λ的取值上存在差异。

基于力、基于冲量、基于位置求解思路上都很相近，主要区别就是算 $C$ 、 $\dot{C}$还是$\ddot{C}$，然后用$\lambda$替换里面的Δx、Δv或者Fc，最后求解含唯一未知数$\lambda$的线性方程组即可。

SI、PGS本文没涉及，但已知结论就是它俩思想上一致，前者分别逐个求解单个λ，后者对每个λ矩阵中的一行也就是${\lambda }_i$求解，数学上效果是一致的，可能在收敛性上存在差异。

而PGS比GS多一个限制λ范围大于等于0，也就是能够求解非等式约束罢了。

完毕！

参考目录

1. Position Based Dynamics与二次规划
2. 基于约束的物理
3. PhysX原理与实现：一般约束
4. 物理引擎之约束求解(一)——线性互补问题
5. Video Game Physics Tutorial - Part III: Constrained Rigid Body Simulation
6. Game Physics Series 周明倫
7. Karush-Kuhn-Tucker (KKT)条件