動(dòng)力學(xué)分析基礎(chǔ)——雅可比矩陣

代碼編寫(xiě),資料整理——ZH1110

動(dòng)力學(xué)仿真計(jì)算歸結(jié)為對(duì)典型的常微分方程組的初值問(wèn)題。在解上述的初值問(wèn)題時(shí)，除了應(yīng)用常微分方程初值問(wèn)題的數(shù)值積分外，還將用到求解線(xiàn)性代數(shù)方程組的數(shù)值方法,所以首先我們必須先研究這兩個(gè)常用的計(jì)算機(jī)算法,已便于后面的計(jì)算.

高斯消去法求解線(xiàn)性代數(shù)方程組(直接法,即消去法)，已在線(xiàn)性代數(shù)課程中有詳細(xì)的討論，在此給出些說(shuō)明以及具體的算法描述。

大致可以分為以下兩步。
1．將系數(shù)矩陣經(jīng)過(guò)一系列的初等行變換（歸一化）

在變換過(guò)程中，采用原地工作，即經(jīng)變換后的元素仍放在原來(lái)的位置上。

2.消去。它的作用是將主對(duì)角線(xiàn)以下的均消成0，而其它元素與向量中的元素也應(yīng)作相應(yīng)的變換

最后，進(jìn)行回代依次解出

如:我們要解如下方程組：

初等行變換:

回代得到結(jié)果:

龍格-庫(kù)塔算法求解常微分方程

用歐拉算法、改進(jìn)歐拉算法以及經(jīng)典龍格-庫(kù)塔算法對(duì)常微分方程的初值問(wèn)題進(jìn)行數(shù)值求解算法。
動(dòng)力學(xué)仿真計(jì)算最后會(huì)出現(xiàn)一加速度，速度，坐標(biāo)的兩階微分方程組，其積分需要這種計(jì)算方法。

一、 使用歐拉算法及其改進(jìn)算法（梯形算法）進(jìn)行求解
　　所謂的微分方程數(shù)值求解，就是求問(wèn)題的解y(x)在一系列點(diǎn)上的值y(xi)的近似值yi。歐拉(Euler)算法是其實(shí)現(xiàn)的依據(jù)是用向前差商來(lái)近似代替導(dǎo)數(shù)。對(duì)于常微分方程：
　　dy/dx=f(x,y)，x∈[a,b]
　　y(a)=y0
　　可以將區(qū)間[a,b]分成n段，那么方程在第xI點(diǎn)有y'(xI)=f(xI,y(xI))，再用向前差商近似代替導(dǎo)數(shù)則為：(y(xI+1)-y(xI))/h= f(xI,y(xI))，因此可以根據(jù)xI點(diǎn)和yI點(diǎn)的數(shù)值計(jì)算出yI+1來(lái).由此可以看出，常微分方程數(shù)值解法的基本出發(fā)點(diǎn)就是計(jì)算離散化點(diǎn)。
　　yI+1= yI+h*f(xI ,yI)

　下面就舉一個(gè)簡(jiǎn)單的常微分方程
　　y'=x-y+1,x∈[0,0.5]
　　y(0)=1                      （人工計(jì)算后的解析式為: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

由于方程曲線(xiàn)是內(nèi)凹的所以無(wú)論如何減少步距,得到的結(jié)果都小于真實(shí)值，有必要采取措施來(lái)抑制、減少誤差，盡量使結(jié)果精確。在構(gòu)造歐拉公式時(shí)采取的一個(gè)重要步驟--用向前差商來(lái)代替導(dǎo)數(shù)，如將其改為向后差商也是行的通的。此時(shí)的歐拉公式就變成了：yI+1= yI+h*f(xI+1,yI+1)，由于該式是一個(gè)隱式公式，所以可用迭代法進(jìn)行計(jì)算，直至獲取到滿(mǎn)足精度要求的yI+1。從數(shù)學(xué)上可以證明，該式的局部截?cái)嗾`差和前面的歐拉公式的截?cái)嗾`差在主部上之相差正負(fù)號(hào)，所以只要將顯示和隱式的兩個(gè)歐拉公式相加后再行求解會(huì)大大減少誤差。可以解得改進(jìn)后的歐拉公式的表達(dá)式為：

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

　
　　從下表得出的實(shí)驗(yàn)數(shù)據(jù)可以看出，這種經(jīng)過(guò)改進(jìn)的歐拉算法所存在的誤差已大為減少，可以直接單獨(dú)應(yīng)用于實(shí)際的工程計(jì)算。誤差的減少主要是由于先利用了歐拉公式對(duì)yI+1的值進(jìn)行了預(yù)估，然后又利用梯形公式對(duì)預(yù)估值作了校正，從而在預(yù)估--校正的過(guò)程中減少了誤差。

 使用經(jīng)典龍格-庫(kù)塔算法進(jìn)行高精度求解
對(duì)于一階精度的歐拉公式有：
　　yi＋１＝y(tǒng)i＋h*K1
　　K1=f(xi,yi)
　　當(dāng)用點(diǎn)xi處的斜率近似值K1與右端點(diǎn)xi＋１處的斜率K2的算術(shù)平均值作為平均斜率K*的近似值，那么就會(huì)得到二階精度的改進(jìn)歐拉公式：
　　yi＋１＝y(tǒng)i＋h*( K1+ K2)/2
　　K1=f(xi,yi)
　　K2=f(xi+h,yi+h*K1)
　　依次類(lèi)推，如果在區(qū)間[xi,xi+1]內(nèi)多預(yù)估幾個(gè)點(diǎn)上的斜率值K1、K2、……Km，并用他們的加權(quán)平均數(shù)作為平均斜率K*的近似值，顯然能構(gòu)造出具有很高精度的高階計(jì)算公式。經(jīng)數(shù)學(xué)推導(dǎo)、求解，可以得出四階龍格－庫(kù)塔公式，也就是在工程中應(yīng)用廣泛的經(jīng)典龍格－庫(kù)塔算法：
　　yi＋１＝y(tǒng)i＋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)

龍格 -庫(kù)塔算法代碼清單：
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
　　從下表記錄的程序運(yùn)行結(jié)果來(lái)看，在步長(zhǎng)為0.1的情況下所計(jì)算出來(lái)的常微分方程的數(shù)值解是非常精確的，用浮點(diǎn)數(shù)進(jìn)行運(yùn)算時(shí)由近似所引入的誤差幾乎不會(huì)對(duì)計(jì)算結(jié)果產(chǎn)生影響。　

一般來(lái)說(shuō)經(jīng)典龍格－庫(kù)塔算法精確度高又利于計(jì)算機(jī)編程實(shí)現(xiàn)，穩(wěn)定性也很好，可以考慮作為首選實(shí)現(xiàn)算法。

求解兩階微分方程組的龍格—庫(kù)塔法:

對(duì)于兩階微分方程組:

利用雅可比矩陣分析動(dòng)力學(xué)

系統(tǒng)約束方程的概念：

對(duì)于剛體系，剛體間存在鉸(或運(yùn)動(dòng)副)。在一個(gè)鉸的鄰接剛體中，一個(gè)剛體的運(yùn)動(dòng)將部分地牽制了另一剛體的運(yùn)動(dòng)。在一般情況下，描述系統(tǒng)位形的坐標(biāo)并不完全獨(dú)立，在運(yùn)動(dòng)過(guò)程中，它們之間存在某些關(guān)系。這些關(guān)系的解析表達(dá)式構(gòu)成約束方程

將約束方程求導(dǎo)有

這即雅可比(C.G.J. Jacobi)矩陣，或簡(jiǎn)稱(chēng)約束方程的雅可比。

體系通用的動(dòng)力學(xué)模型(具體可參考分析力學(xué)著作)即:

它不是典型的常微分方程組，故仿真計(jì)算不是一般的常微分方程組初值問(wèn)題 。為此定義變量陣,

將方程動(dòng)力學(xué)改寫(xiě)為

上所述，經(jīng)過(guò)上述變換，動(dòng)力學(xué)仿真計(jì)算歸結(jié)為對(duì)典型的常微分方程組的初值問(wèn)題。在對(duì)上述初值問(wèn)題進(jìn)行數(shù)值積分的過(guò)程中方程之右函數(shù)中的值不能直接得到，需通過(guò)解代數(shù)方程得到。此時(shí)拉格朗日乘子的值也同時(shí)得到。由此可知，在解上述的初值問(wèn)題時(shí)，除了應(yīng)用常微分方程初值問(wèn)題的數(shù)值積分外，還將用到求解線(xiàn)性代數(shù)方程組的數(shù)值方法。

仿真計(jì)算的步驟:

(1)  將時(shí)間離散,根據(jù)式計(jì)算A，b,利用高斯消元法解代數(shù)方程；                

(2) 進(jìn)行數(shù)值積分得,返回(1)

例1:圖示一雙質(zhì)點(diǎn)擺，擺球P₁與P₂的質(zhì)量為m₁=m₂=1，擺長(zhǎng)分別為l₁=1與l₂=1,兩球開(kāi)始位置在正右方。試?yán)?font lang=ZH-CN face=宋體 size=2>雅可比矩陣建立該雙質(zhì)點(diǎn)擺的動(dòng)力學(xué)方程。

解：如圖建立慣性基。雙質(zhì)點(diǎn)擺為兩質(zhì)點(diǎn)系，系統(tǒng)的坐標(biāo)陣為

約束方程為

可見(jiàn)坐標(biāo)數(shù)為4，約束方程數(shù)為2，故系統(tǒng)自由度為2。引入拉格朗日乘子陣。約束方程(1)的雅可比為

(每種約束方程都有其偏導(dǎo)數(shù)方程,如果你不了解如何求二階導(dǎo)數(shù)直接帶公式即可)

主動(dòng)力只有重力，主動(dòng)力陣為

將上述分析的結(jié)果代入動(dòng)力學(xué)模型，有動(dòng)力學(xué)方程

將式(1)對(duì)時(shí)間求二階導(dǎo)數(shù)，得到加速度約束方程，即

將后兩個(gè)方程與動(dòng)力學(xué)方程合并，有完整的動(dòng)力學(xué)方程:

編寫(xiě)代碼：
Const m As Single = 1 '質(zhì)量
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()
'開(kāi)始位置在右方
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))
'重新設(shè)置初始值
INI
Next
End Sub

例2:利用拉格朗日第一類(lèi)方程建立所示均質(zhì)桿封閉的動(dòng)力學(xué)方程,桿開(kāi)始角度為Pi/6。

解：如圖所示，在質(zhì)心C建立桿的連體基,該桿關(guān)于質(zhì)心C的轉(zhuǎn)動(dòng)慣量為J_C=ml²/12。根據(jù)已知條件，桿AB在運(yùn)動(dòng)過(guò)程中位形坐標(biāo)間有如下兩個(gè)獨(dú)立的約束方程(s=2)

約束方程的雅可比與加速度約束方程的右項(xiàng)分別為

引入兩個(gè)拉格朗日乘子l＝(l₁ l₂)^T。系統(tǒng)受到的主動(dòng)力為重力，由增廣主動(dòng)力陣的定義，有。根據(jù)上面分析，可得動(dòng)力學(xué)方程

或由式(9.1-25)可得到封閉的動(dòng)力學(xué)方程

　代碼（略）：

優(yōu)化與精度的思考:

優(yōu)化：動(dòng)力學(xué)仿真主要是對(duì)雅可比矩陣不斷的跌代,其計(jì)算量隨剛體數(shù)目非線(xiàn)形增長(zhǎng),因此如何加快跌代就成為非常重要的問(wèn)題了,如——1.在用龍格-庫(kù)塔算法時(shí)可根據(jù)斜率動(dòng)態(tài)改變計(jì)算步長(zhǎng) 2.專(zhuān)門(mén)對(duì)鏈條狀剛體系優(yōu)化求解線(xiàn)性代數(shù)方程組過(guò)程,等等.

精度：如圖一單擺,我們用歐拉算法常微分方程的初值問(wèn)題進(jìn)行數(shù)值求解,球會(huì)逐漸出現(xiàn)'下垂',這是由歐拉算法的誤差引起的,當(dāng)然用龍格-庫(kù)塔算法可以極大程度上消除誤差,但會(huì)加大計(jì)算,這也是值得平衡的一個(gè)問(wèn)題.

　DOWNLOAD文件下載