|
二、平衡車參數(shù)： %常數(shù) g = 9.81; %重力加速度 [m/sec^2] %車身參數(shù) m = 0.039; %車輪重量(個(gè)人更改) [kg] R = 0.038; %車輪半徑(個(gè)人更改) - Jw = m * R^2 / 2; %車輪慣性力矩 [kgm^2] M = 0.9; %車體重量(個(gè)人更改) [kg] W = 0.20; %車體寬度(個(gè)人更改) - D = 0.10; %車體深度(個(gè)人更改) - H = 0.15; %車體高度(個(gè)人更改) - L = 0.04;%H / 2; %車體重心高度，一般為車體高度一半(個(gè)人更改) Jpsi = M * L^2 / 3; %車體慣性力矩(取重心高度) [kgm^2] Jphi = M * (W^2 + D^2) / 12; %車體偏航慣性力矩 [kgm^2] fm = 0.0022; %車體與電機(jī)之間的摩擦系數(shù)(經(jīng)驗(yàn)值) fw = 0; %輪胎與地面之間的摩擦系數(shù)(經(jīng)驗(yàn)值) %電機(jī)參數(shù) Jm = 5.7e-7; %電機(jī)慣性力矩(經(jīng)驗(yàn)值)[kgm^2] Rm = 2.7; %電機(jī)阻抗(電機(jī)參數(shù)) [ohm] Kb = 0.0134; %電機(jī)反電動(dòng)勢(shì)(電機(jī)參數(shù)) [Vsec/rad] Kt = 0.0134; %電機(jī)轉(zhuǎn)矩(電機(jī)參數(shù)) [Nm/A] n = 64; %齒輪比，減速比(個(gè)人更改) 注：Rm,Kb,kt 詳細(xì)可參見手冊(cè)。 Jm,fm,fw 很難進(jìn)行測(cè)量，可根據(jù)經(jīng)驗(yàn)值來取。 三、相關(guān)方程式(公式) 依據(jù)拉格朗日理論得出以下公式： (θ,Φ) = [1/2(θl+θr), R/W(θr-θl)] (3.1) (xm,ym,zm) = (mdt,mdt,R),(m,m) = (RcosΦ RsinΦ) (3.2) (xl,yl,zl) = (xm-sinΦW/2, ym+cosΦW/2,zm) (3.3) (xr,yr,zr) = (xm+sinΦW/2, ym-cosΦW/2,zm) (3.4) (xb,yb,zb) = (xm+LsinΨcosΦ, ym+LsinΨsinΦ,zm+LcosΨ) (3.5) 平動(dòng)動(dòng)能T1，旋轉(zhuǎn)動(dòng)能T2，重力勢(shì)能U公式表示如下： T1=1/2(xl2+yl2,zl2)m+1/2(xr2+yr2,zr2)m+1/2(xb2+yb2,zb2)M (3.6) T2=(Jwl2)1/2+ (Jwr2) 1 /2+ (JΨ2) 1/2+ (Jφ2) 1/2+ [n2Jm(l-)2]1/2+ [n2Jm(r-)2]1/2 (3.7) U=mgz1+mgz2+Mgzb (3.8) 由3.5 和3.6得： 系統(tǒng)總動(dòng)能L = T1+T2-U (3.9) 我們使用下列變量作為廣義坐標(biāo)。 θ: 左右輪偏移平均角度 Ψ: 車體傾斜角度 Φ: 車體偏移角度 依據(jù)拉格朗日理論(系統(tǒng)動(dòng)力學(xué))得出以下公式： (L/)d/dt-L/θ=Fθ (3.10) (L/)d/dt-L/Ψ=FΨ (3.11) (L/)d/dt-L/Φ=FΦ (3.12) 由3.10 3.11 3.12得： [(2m+M)R2+2Jw+2n2Jm]+(MLRcosΨ-2n2Jm)-MLR2sinΨ= Fθ (3.13) (MLRcosΨ-2n2Jm)+(ML2+JΨ+2n2Jm)-MgLsinΨ-ML22sinΨcosΨ= FΨ (3.14) [1mW2/2+JΦ+(Jw+n2Jm)W2/2R2+ML2sin2Ψ]+2ML2sinΨcosΨ= FΦ (3.15) 在考慮直流電機(jī)轉(zhuǎn)矩和摩擦,給出了廣義力為以下: (Fθ,FΨ,FΦ) = [Fl+Fr ,FΨ,W(Fr -Fl)/2R] (3.16) Fl = nKtil+fm(-l)-fwl (3.17) Fr = nKtir+fm(-r)-fwr (3.18) FΨ = -nKtil- nKtir- fm(-l)- fm(-r) (3.19) 這里的il,r為電機(jī)電流 我們不能直接用電流來控制系統(tǒng)，因?yàn)樗腔?font face="Calibri">PWM(電壓)的控制。因此我們需通過電機(jī)方程式計(jì)算電流Il,r和電壓Vl,r之間的關(guān)系。如果不計(jì)電機(jī)內(nèi)部的摩擦力，那么電機(jī)的方程式大概可以計(jì)算成如下： Lml,r = Vl,r+Kb(-l,r)-RmIl,r (3.20) (其中Lm為電機(jī)感應(yīng)系數(shù)) 我們不計(jì)電機(jī)的感應(yīng)系數(shù)(取近似值0)，得出電流值： Il,r = (Vl,r+Kb (-l,r))/Rm (3.21) 由3.21 得 廣義表達(dá)式用電機(jī)的電壓表示： Fθ = α(Vl+ Vr)-2(β+fw)+2β (3.22) FΨ = -α(Vl+ Vr)+2β-2β (3.23) FΦ = Wα(Vr- Vl)/2R-W2(β+fw)/2R2 (3.24) α = nKt/Rm , β = nKtKb/Rm+fm (3.25) 平衡車狀態(tài)方程式 我們可以導(dǎo)出狀態(tài)方程,基于現(xiàn)代控制理論的線性運(yùn)動(dòng)原理 用在系統(tǒng)的平衡點(diǎn)附近。 既考慮極限值Ψ→0 (sinΨ→Ψ,cosΨ→1),(并且忽略第二命令如 2。?) 方程式3.13 ~3.15 近似得： [(2m+M)R2+2Jw+2n2Jm] +(MLR -2n2Jm) = Fθ (3.26) (MLR -2n2Jm)+(ML2+JΨ+2n2Jm)-MgLΨ= FΨ (3.27) [1mW2/2+JΦ+(Jw+n2Jm)W2/2R2] = FΦ (3.28) 表達(dá)式3.26 和表達(dá)式3.27含有θ、Ψ。表達(dá)式3.28只有Φ。 這些方程可以表達(dá)為： E+F+G = H (3.29) E = F = 2 G = H = I+J = K(Vr-Vl) (3.30) I = 1mW2/2+JΦ+W2(Jw+n2Jm)/2R2 J = W2(β+fw) K = Αw/2R 根據(jù)以上動(dòng)力學(xué)方程，這里我們考慮以下的變量 x1,x2為狀態(tài)變量，分別取值如下。u為輸入變量，取值如下。進(jìn)行線性化處理。(xT為x的轉(zhuǎn)置矩陣。) x1 = [Φ,ΨT, x2 = [ΦT, u = [Vl , Vr]T (3.31) 我們可以從 3.29 3.30 得到： 1 = A1x1+B1u (3.32) 2 = A2x2+B2u (3.33) A1 = , B1 = (3.34) A2 = , B2 = (3.35) A1(3,2) = -gMLE(1,2)/det(E) A1(4,2) = gMLE(1,1)/det(E) A1(3,3) = -2[(β+fw)E(2,2)+ βE(1,2)]/det(E) A1(4,3) = 2[(β+fw)E(1,2)+ βE(1,1)]/det(E) A1(3,4) = 2β[E(2,2)+E(1,2)]/det(E) A1(4,4) = -2β[E(1,1)+E(1,2)]/det(E) B1(3) = α[E(2,2)+E(1,2)]/det(E) B1(4) = -α[E(1,1)+E(1,2)]/det(E) Det(E) = E(1,1)E(2,2)-E(1,2)2 (求行列式的值) 平衡車控制器設(shè)計(jì) 輸入輸出 通過輸入u(3.31) .PWM控制左右輪 .由傳感器輸出電機(jī)傾斜角度θm l,r 和車體偏移角速度。 車輪傾斜角度θ通過傳感器即可獲得，電機(jī)角度通過θm l,r可以獲得，有兩種方法可以得到車體傾斜角度Ψ 1、 通過角速度的積分運(yùn)算 2、 通過測(cè)量 文中用的是1 穩(wěn)定性 自動(dòng)控制中提供了很多種方法去穩(wěn)定一個(gè)不穩(wěn)定的系統(tǒng)(自己查) 控制器設(shè)計(jì) 3.29 模擬的是一個(gè)彈簧系統(tǒng) 下圖給出的就等價(jià)于一個(gè)彈簧系統(tǒng)
我們發(fā)現(xiàn)我們可以通過調(diào)節(jié);單擺穩(wěn)定常數(shù)和彈簧阻尼常量(摩擦系數(shù)？)，使其平衡。 我們使用的是伺服控制系統(tǒng)，并且選擇θ作為伺服控制系統(tǒng)的參數(shù)。最重要的是我們除了用“θ”作為參考,不用其他的變量作為參考。因?yàn)檫@個(gè)系統(tǒng)是無法控制的。下圖為伺服控制的方塊圖：
(Cθ作用是驅(qū)動(dòng)θ，來自于X1) 我們通過多次計(jì)算反饋增益和積分的二次線性型調(diào)節(jié)器(LQR)的方法，選擇比較合適的矩陣“Q和R“ Q = , R = Q(2,2)對(duì)車體偏移角度非常重要，Q(5,5)是相對(duì)于不同的平均角度和參考值之間非常重要的時(shí)間融合值。 計(jì)算線性二次型調(diào)節(jié)器(LQR)和定義了其他的參數(shù)，反饋部分的計(jì)算 如下 %計(jì)算得到最佳監(jiān)管伺服系統(tǒng) A_BAR = [A1, zeros(4, 1); C1(1, :), 0]; B_BAR = [B1; 0, 0]; QQ = [ 1, 0, 0, 0, 0 0, 6e5, 0, 0, 0 0, 0, 1, 0, 0 0, 0, 0, 1, 0 0, 0, 0, 0, 4e2 ]; RR = 1e3 * eye(3); KK = lqr(A_BAR, B_BAR, QQ, RR); k_f = KK(1, 1:4); % feedback gain k_i = KK(1, 5); % integral gain % 防止車體抖動(dòng)的濾波函數(shù)(抑制增長速度) k_f(3) = k_f(3) * 0.85; 如上返回值為 K_f=[-0.83508 -34.19 -1.0995 -2.8141] K_i=-0.4472 我們可以調(diào)節(jié)速度增益值k_f(3) ，在經(jīng)過線性二次型調(diào)節(jié)器(LQR)計(jì)算后。 因?yàn)樗鼤?huì)是影響到車體抖動(dòng)的值。 除此外，我們添加了如下的控制 ●給左右電機(jī)不同的值從而使車體轉(zhuǎn)彎 ●用P(比例調(diào)節(jié))控制調(diào)節(jié)平衡車的前進(jìn)，因?yàn)橥瑯拥腜WM對(duì)于電機(jī)來說轉(zhuǎn)動(dòng)的角度也不同 所以我們得出平衡車控制器如下：
搞定。
| 
