二、平衡车参数: %常数 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对于电机来说转动的角度也不同 所以我们得出平衡车控制器如下:
搞定。
|