標題: 平衡車建模 [打印本頁]
作者: liuqq 時間: 2015-5-22 00:29
標題: 平衡車建模
二、平衡車參數：
%常數
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對于電機來說轉動的角度也不同
所以我們得出平衡車控制器如下：
搞定。
| 歡迎光臨 (http://www.denmoz.com/bbs/) |
Powered by Discuz! X3.1 |