找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 3265|回复: 0
收起左侧

平衡车建模

[复制链接]
ID:80436 发表于 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)
= -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)的方法,选择比较合适的矩阵“QR
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对于电机来说转动的角度也不同
所以我们得出平衡车控制器如下:

搞定。

回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表