|
二、平衡車參數： %常數 g = 9.81; %重力加速度 [m/sec^2] %車身參數 m = 0.039; %車輪重量(個人更改) [kg] R = 0.038; %車輪半徑(個人更改) - Jw = m * R^2 / 2; %車輪慣性力矩 [kgm^2] M = 0.9; %車體重量(個人更改) [kg] W = 0.20; %車體寬度(個人更改) - D = 0.10; %車體深度(個人更改) - H = 0.15; %車體高度(個人更改) - L = 0.04;%H / 2; %車體重心高度，一般為車體高度一半(個人更改) Jpsi = M * L^2 / 3; %車體慣性力矩(取重心高度) [kgm^2] Jphi = M * (W^2 + D^2) / 12; %車體偏航慣性力矩 [kgm^2] fm = 0.0022; %車體與電機之間的摩擦系數(經驗值) fw = 0; %輪胎與地面之間的摩擦系數(經驗值) %電機參數 Jm = 5.7e-7; %電機慣性力矩(經驗值)[kgm^2] Rm = 2.7; %電機阻抗(電機參數) [ohm] Kb = 0.0134; %電機反電動勢(電機參數) [Vsec/rad] Kt = 0.0134; %電機轉矩(電機參數) [Nm/A] n = 64; %齒輪比，減速比(個人更改) 注：Rm,Kb,kt 詳細可參見手冊。 Jm,fm,fw 很難進行測量，可根據經驗值來取。 三、相關方程式(公式) 依據拉格朗日理論得出以下公式： (θ,Φ) = [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) 平動動能T1，旋轉動能T2，重力勢能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得： 系統總動能L = T1+T2-U (3.9) 我們使用下列變量作為廣義坐標。 θ: 左右輪偏移平均角度 Ψ: 車體傾斜角度 Φ: 車體偏移角度 依據拉格朗日理論(系統動力學)得出以下公式： (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) 在考慮直流電機轉矩和摩擦,給出了廣義力為以下: (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為電機電流 我們不能直接用電流來控制系統，因為它是基于PWM(電壓)的控制。因此我們需通過電機方程式計算電流Il,r和電壓Vl,r之間的關系。如果不計電機內部的摩擦力，那么電機的方程式大概可以計算成如下： Lml,r = Vl,r+Kb(-l,r)-RmIl,r (3.20) (其中Lm為電機感應系數) 我們不計電機的感應系數(取近似值0)，得出電流值： Il,r = (Vl,r+Kb (-l,r))/Rm (3.21) 由3.21 得 廣義表達式用電機的電壓表示： 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) 平衡車狀態方程式 我們可以導出狀態方程,基于現代控制理論的線性運動原理 用在系統的平衡點附近。 既考慮極限值Ψ→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) 表達式3.26 和表達式3.27含有θ、Ψ。表達式3.28只有Φ。 這些方程可以表達為： 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 根據以上動力學方程，這里我們考慮以下的變量 x1,x2為狀態變量，分別取值如下。u為輸入變量，取值如下。進行線性化處理。(xT為x的轉置矩陣。) 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 (求行列式的值) 平衡車控制器設計 輸入輸出 通過輸入u(3.31) .PWM控制左右輪 .由傳感器輸出電機傾斜角度θm l,r 和車體偏移角速度。 車輪傾斜角度θ通過傳感器即可獲得，電機角度通過θm l,r可以獲得，有兩種方法可以得到車體傾斜角度Ψ 1、 通過角速度的積分運算 2、 通過測量 文中用的是1 穩定性 自動控制中提供了很多種方法去穩定一個不穩定的系統(自己查) 控制器設計 3.29 模擬的是一個彈簧系統 下圖給出的就等價于一個彈簧系統
我們發現我們可以通過調節;單擺穩定常數和彈簧阻尼常量(摩擦系數？)，使其平衡。 我們使用的是伺服控制系統，并且選擇θ作為伺服控制系統的參數。最重要的是我們除了用“θ”作為參考,不用其他的變量作為參考。因為這個系統是無法控制的。下圖為伺服控制的方塊圖：
(Cθ作用是驅動θ，來自于X1) 我們通過多次計算反饋增益和積分的二次線性型調節器(LQR)的方法，選擇比較合適的矩陣“Q和R“ Q = , R = Q(2,2)對車體偏移角度非常重要，Q(5,5)是相對于不同的平均角度和參考值之間非常重要的時間融合值。 計算線性二次型調節器(LQR)和定義了其他的參數，反饋部分的計算 如下 %計算得到最佳監管伺服系統 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 % 防止車體抖動的濾波函數(抑制增長速度) k_f(3) = k_f(3) * 0.85; 如上返回值為 K_f=[-0.83508 -34.19 -1.0995 -2.8141] K_i=-0.4472 我們可以調節速度增益值k_f(3) ，在經過線性二次型調節器(LQR)計算后。 因為它會是影響到車體抖動的值。 除此外，我們添加了如下的控制 ●給左右電機不同的值從而使車體轉彎 ●用P(比例調節)控制調節平衡車的前進，因為同樣的PWM對于電機來說轉動的角度也不同 所以我們得出平衡車控制器如下：
搞定。
| 
