牽著老婆滿街逛

動力學分析基礎——雅可比矩陣

代碼編寫,資料整理——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文件下載