牵着老婆满街逛

动力学分析基础——雅可比矩阵

代码编写,资料整理——ZH1110

动力学仿真计算归结为对典型的常微分方程组的初值问题。在解上述的初值问题时，除了应用常微分方程初值问题的数值积分外，还将用到求解线性代数方程组的数值方法,所以首先我们必须先研究这两个常用的计算机算法,已便于后面的计算.

高斯消去法求解线性代数方程组(直接法,即消去法)，已在线性代数课程中有详细的讨论，在此给出些说明以及具体的算法描述。

大致可以分为以下两步。
1．将系数矩阵经过一系列的初等行变换（归一化）

在变换过程中，采用原地工作，即经变换后的元素仍放在原来的位置上。

2.消去。它的作用是将主对角线以下的均消成0，而其它元素与向量中的元素也应作相应的变换

最后，进行回代依次解出

如:我们要解如下方程组：

初等行变换:

回代得到结果:

龙格-库塔算法求解常微分方程

用欧拉算法、改进欧拉算法以及经典龙格-库塔算法对常微分方程的初值问题进行数值求解算法。
动力学仿真计算最后会出现一加速度，速度，坐标的两阶微分方程组，其积分需要这种计算方法。

一、 使用欧拉算法及其改进算法（梯形算法）进行求解
　　所谓的微分方程数值求解，就是求问题的解y(x)在一系列点上的值y(xi)的近似值yi。欧拉(Euler)算法是其实现的依据是用向前差商来近似代替导数。对于常微分方程：
　　dy/dx=f(x,y)，x∈[a,b]
　　y(a)=y0
　　可以将区间[a,b]分成n段，那么方程在第xI点有y'(xI)=f(xI,y(xI))，再用向前差商近似代替导数则为：(y(xI+1)-y(xI))/h= f(xI,y(xI))，因此可以根据xI点和yI点的数值计算出yI+1来.由此可以看出，常微分方程数值解法的基本出发点就是计算离散化点。
　　yI+1= yI+h*f(xI ,yI)

　下面就举一个简单的常微分方程
　　y'=x-y+1,x∈[0,0.5]
　　y(0)=1                      （人工计算后的解析式为:y(x)=x+e-x）

'欧拉算法
Private Sub Euler()
For x = 0 To 0.5 Step 0.1
y(i + 1) = y(i) + 0.1 * (x - y(i) + 1)
List1.AddItem y(i)
i = i + 1
Next
End Sub

由于方程曲线是内凹的所以无论如何减少步距,得到的结果都小于真实值，有必要采取措施来抑制、减少误差，尽量使结果精确。在构造欧拉公式时采取的一个重要步骤--用向前差商来代替导数，如将其改为向后差商也是行的通的。此时的欧拉公式就变成了：yI+1= yI+h*f(xI+1,yI+1)，由于该式是一个隐式公式，所以可用迭代法进行计算，直至获取到满足精度要求的yI+1。从数学上可以证明，该式的局部截断误差和前面的欧拉公式的截断误差在主部上之相差正负号，所以只要将显示和隐式的两个欧拉公式相加后再行求解会大大减少误差。可以解得改进后的欧拉公式的表达式为：

　　yI+1= yI+h*(f(xI, yI)+f(xI+1, yI+hf(xI,yI)))/2

　
　　从下表得出的实验数据可以看出，这种经过改进的欧拉算法所存在的误差已大为减少，可以直接单独应用于实际的工程计算。误差的减少主要是由于先利用了欧拉公式对yI+1的值进行了预估，然后又利用梯形公式对预估值作了校正，从而在预估--校正的过程中减少了误差。

 使用经典龙格-库塔算法进行高精度求解
对于一阶精度的欧拉公式有：
　　yi＋１＝yi＋h*K1
　　K1=f(xi,yi)
　　当用点xi处的斜率近似值K1与右端点xi＋１处的斜率K2的算术平均值作为平均斜率K*的近似值，那么就会得到二阶精度的改进欧拉公式：
　　yi＋１＝yi＋h*( K1+ K2)/2
　　K1=f(xi,yi)
　　K2=f(xi+h,yi+h*K1)
　　依次类推，如果在区间[xi,xi+1]内多预估几个点上的斜率值K1、K2、……Km，并用他们的加权平均数作为平均斜率K*的近似值，显然能构造出具有很高精度的高阶计算公式。经数学推导、求解，可以得出四阶龙格－库塔公式，也就是在工程中应用广泛的经典龙格－库塔算法：
　　yi＋１＝yi＋h*( K1+ 2*K2 +2*K3+ K4)/6
　　K1=f(xi,yi)
　　K2=f(xi+h/2,yi+h*K1/2)
　　K3=f(xi+h/2,yi+h*K2/2)
　　K4=f(xi+h,yi+h*K3)

龙格 -库塔算法代码清单：
Private Sub Runge_Kutta()
Dim K1 As Single, K2 As Single, K3 As Single, K4 As Single
For x = 0 To 0.5 Step 0.1
K1 = x - y(i) + 1 '求K1
K2 = (x + 0.1 / 2) - (y(i) + K1 * (0.1 / 2)) + 1
K3 = (x + 0.1 / 2) - (y(i) + K2 * (0.1 / 2)) + 1
K4 = (x + 0.1) - (y(i) + K3 * 0.1) + 1
y(i + 1) = y(i) + 0.1 * (K1 + 2 * K2 + 2 * K3 + K4) / 6
List2.AddItem y(i)
i = i + 1
Next
End Sub
　　从下表记录的程序运行结果来看，在步长为0.1的情况下所计算出来的常微分方程的数值解是非常精确的，用浮点数进行运算时由近似所引入的误差几乎不会对计算结果产生影响。　

一般来说经典龙格－库塔算法精确度高又利于计算机编程实现，稳定性也很好，可以考虑作为首选实现算法。

求解两阶微分方程组的龙格—库塔法:

对于两阶微分方程组:

利用雅可比矩阵分析动力学

系统约束方程的概念：

对于刚体系，刚体间存在铰(或运动副)。在一个铰的邻接刚体中，一个刚体的运动将部分地牵制了另一刚体的运动。在一般情况下，描述系统位形的坐标并不完全独立，在运动过程中，它们之间存在某些关系。这些关系的解析表达式构成约束方程

将约束方程求导有

这即雅可比(C.G.J. Jacobi)矩阵，或简称约束方程的雅可比。

体系通用的动力学模型(具体可参考分析力学著作)即:

它不是典型的常微分方程组，故仿真计算不是一般的常微分方程组初值问题 。为此定义变量阵,

将方程动力学改写为

上所述，经过上述变换，动力学仿真计算归结为对典型的常微分方程组的初值问题。在对上述初值问题进行数值积分的过程中方程之右函数中的值不能直接得到，需通过解代数方程得到。此时拉格朗日乘子的值也同时得到。由此可知，在解上述的初值问题时，除了应用常微分方程初值问题的数值积分外，还将用到求解线性代数方程组的数值方法。

仿真计算的步骤:

(1)  将时间离散,根据式计算A，b,利用高斯消元法解代数方程；                

(2) 进行数值积分得,返回(1)

例1:图示一双质点摆，摆球P₁与P₂的质量为m₁=m₂=1，摆长分别为l₁=1与l₂=1,两球开始位置在正右方。试利用雅可比矩阵建立该双质点摆的动力学方程。

解：如图建立惯性基。双质点摆为两质点系，系统的坐标阵为

约束方程为

可见坐标数为4，约束方程数为2，故系统自由度为2。引入拉格朗日乘子阵。约束方程(1)的雅可比为

(每种约束方程都有其偏导数方程,如果你不了解如何求二阶导数直接带公式即可)

主动力只有重力，主动力阵为

将上述分析的结果代入动力学模型，有动力学方程

将式(1)对时间求二阶导数，得到加速度约束方程，即

将后两个方程与动力学方程合并，有完整的动力学方程:

编写代码：
Const m As Single = 1 '质量
Const g As Single = 9.8 '重力
'速度,加速度
Dim X(2) As Single, Y(2) As Single, Vx(2) As Single, Vy(2) As Single
Dim A(1 To 6, 1 To 6) As Single, b(1 To 6) As Single, Fr(1 To 6) As Single

Private Sub Form_Load()
'开始位置在右方
X(1) = 1: Y(1) = 0: X(2) = 2: Y(2) = 0
Vx(1) = 0: Vy(2) = 0
...
End Sub

'初始化
Private Sub INI()
A(1, 1) = m: A(1, 5) = 2 * X(1): A(1, 6) = -2 * (X(2) - X(1))
A(2, 2) = m: A(2, 5) = 2 * Y(1): A(2, 6) = -2 * (Y(2) - Y(1))
A(3, 3) = m:: A(3, 6) = 2 * (X(2) - X(1))
A(4, 4) = m:: A(4, 6) = 2 * (Y(2) - Y(1))
A(5, 1) = X(1): A(5, 2) = Y(1)
A(6, 1) = X(1) - X(2): A(6, 2) = Y(1) - Y(2): A(6, 3) = X(2) - X(1): A(6, 4) = Y(2) - Y(1)
b(2) = m * g: b(4) = m * g: b(5) = -Vx(1) ^ 2 - Vy(1) ^ 2: b(6) = -(Vx(2) - Vx(1)) ^ 2 - (Vy(2) - Vy(1)) ^ 2
End Sub

Private Sub Timer1_Timer()
Const t = 0.03 '步距
'高斯消去法解方程组
GaMss A(), b(), 6, Fr()
For jj = 1 To 2
'积分
Vx(jj) = Vx(jj) + t * Fr(jj * 2 - 1): Vy(jj) = Vy(jj) + t * Fr(jj * 2)
X(jj) = X(jj) + t * Vx(jj): Y(jj) = Y(jj) + t * Vy(jj)
'绘制球,杆
Picture1.Circle (X(jj), Y(jj)), 0.1
Picture1.Line (X(jj - 1), Y(jj - 1))-(X(jj), Y(jj))
'重新设置初始值
INI
Next
End Sub

例2:利用拉格朗日第一类方程建立所示均质杆封闭的动力学方程,杆开始角度为Pi/6。

解：如图所示，在质心C建立杆的连体基,该杆关于质心C的转动惯量为J_C=ml²/12。根据已知条件，杆AB在运动过程中位形坐标间有如下两个独立的约束方程(s=2)

约束方程的雅可比与加速度约束方程的右项分别为

引入两个拉格朗日乘子l＝(l₁ l₂)^T。系统受到的主动力为重力，由增广主动力阵的定义，有。根据上面分析，可得动力学方程

或由式(9.1-25)可得到封闭的动力学方程

　代码（略）：

优化与精度的思考:

优化：动力学仿真主要是对雅可比矩阵不断的跌代,其计算量随刚体数目非线形增长,因此如何加快跌代就成为非常重要的问题了,如——1.在用龙格-库塔算法时可根据斜率动态改变计算步长 2.专门对链条状刚体系优化求解线性代数方程组过程,等等.

精度：如图一单摆,我们用欧拉算法常微分方程的初值问题进行数值求解,球会逐渐出现'下垂',这是由欧拉算法的误差引起的,当然用龙格-库塔算法可以极大程度上消除误差,但会加大计算,这也是值得平衡的一个问题.

　DOWNLOAD文件下载