平衡小车动力学建模

前言

两轮自平衡小车由车体和双轮两部分组成，可以看成一个移动的倒立摆，分别对车轮和车体进行力学分析，建立动力学模型，最后，通过对两者的分析给出系统的状态空间表达式。

车轮模型

以右轮为例进行受力分析，如图所示

将车轮的运动分解为平动和转动，
由牛顿第二定律可得
$$m\ddot{x_R}=H_{fR}-H_R(1)$$

由刚体定轴转动定律可得
$$I\dot{{\omega }_R}=T_R-H_{fR}r(2)$$

其中，

物理量	描述	单位
$m$	车轮的质量	$kg$
$r$	车轮的半径	$m$
$x_R$	右轮的水平位移	$m$
$H_{fR}$	右轮受到地面的摩擦力的大小	$N$
$H_R$	右轮受到车体作用力的水平分力的大小	$N$
$T_R$	右轮电机输出的转矩的大小	$N\cdot m$
$I$	车轮的转动惯量	$kg\cdot m^2$
${\omega }_R$	右轮的角速度的大小	$rad/s$

联立$(1)(2)$，消去$H_{fR}$，可得

$$m\ddot{x_R}=\frac{T_R-I\dot{{\omega }_R}}{r}-H_R(3)$$

在车轮不打滑的情况下，车轮的移动速度的大小和转动速度的大小成比例关系，即
$$\{\begin{array}{l}{\omega }_R=\frac{\dot{x_R}}{r}\\ \dot{{\omega }_R}=\frac{\ddot{x_R}}{r}\end{array}(4)$$
将方程$(4)$代入$(3)$中，消去$\dot{{\omega }_R}$，可得
$$(m+\frac{I}{r^2})\ddot{x_R}=\frac{T_R}{r}-H_R(5)$$
由于左右轮的参数相同，对左轮有类似的结果，即
$$(m+\frac{I}{r^2})\ddot{x_L}=\frac{T_L}{r}-H_L(6)$$

车轮模型

车体的运动也可以分解为正向运动（前向、俯仰）和侧向运动（转向、偏航）。

正向运动

小车的正向运动可以分解为前向运动和绕车体质心$P$的相对转动（俯仰）。小车底盘中心$O$的水平位移为$x=\frac{x_L+x_R}{2}(7)$

将方程$(5)(6)$相加，等式两边同时除以$2$可得
$$(m+\frac{I}{r^2})\frac{\ddot{x_L}+\ddot{x_R}}{2}=\frac{T_L+T_R}{2r}-\frac{H_L+H_R}{2}(8)$$
将$(7)$代入$(8)$可得
$$(m+\frac{I}{r^2})\ddot{x}=\frac{T_L+T_R}{2r}-\frac{H_L+H_R}{2}(9)$$

对车体，应用牛顿第二定律可得

在水平方向上，有
$$M\frac{d(x+lsin{\theta }_P)}{d{t}^2}=H_L+H_R(10)$$
在竖直方向上，有
$$M\frac{d(lcos{\theta }_P)}{d{t}^2}=V_L+V_R-Mg(11)$$
对车体，由刚体定轴转动定律可得
$$J_P\ddot{{\theta }_P}=(V_L+V_R)lsin{\theta }_P-(H_L+H_R)lcos{\theta }_P-(T_L+T_R)(12)$$
其中，

物理量	描述	单位
$M$	车体的质量	$kg$
$l$	质心距底盘中心的距离	$m$
$J_P$	车体绕质心转动时的转动惯量	$kg\cdot m^2$
${\theta }_P$	车体与竖直方向所成的夹角	$rad$
$V_L$	车体受到左轮作用力的竖直分力的大小	$N$

联立方程$(9)(10)$，将$H_L+H_R$项消去，可得
$$(M+2m+\frac{2I}{r^2})\ddot{x}-\frac{T_L+T_R}{r}+Ml\ddot{{\theta }_P}cos{\theta }_P-Ml{\dot{{\theta }_P}}^{2}sin{\theta }_P=0(13)$$
其中，$\frac{sin\theta }{dt}=\dot{\theta }cos\theta$，$\frac{d(\dot{\theta }cos\theta )}{dt}=\ddot{\theta }cos\theta -{\dot{\theta }}^{2}sin\theta$

方程含有非线性项，因此，进行线性化，考虑到车体的倾角比较小，通常情况下$-10^{\circ }\le {\theta }_P\le 10^{\circ }$，则可以认为
$$\{\begin{array}{l}cos{\theta }_P=1\\ sin{\theta }_P={\theta }_P\\ {\dot{{\theta }_P}}^2=0\end{array}$$
故方程$(13)$变为
$$\ddot{x}=\frac{T_L+T_R}{(M+2m+\frac{2I}{r^2})r}-\frac{Ml}{(M+2m+\frac{2I}{r^2})}\ddot{{\theta }_P}(14)$$
将方程$(10)(11)$代入方程$(12)$中，消去$H_L+H_R$以及$V_L+V_R$可得
$$(\frac{J_P}{Ml}+l)\ddot{{\theta }_P}+\ddot{x}cos{\theta }_P-gsin{\theta }_P+\frac{T_L+T_R}{Ml}=0(15)$$
对方程$(15)$进行线性化可得
$$\ddot{{\theta }_P}=\frac{Mlg}{(J_P+M{l}^2)}{\theta }_P-\frac{Ml}{(J_P+M{l}^2)}\ddot{x}-\frac{T_L+T_R}{(J_P+M{l}^2)}(16)$$
将方程$(16)$代入方程$(14)$中，消去$\ddot{{\theta }_P}$，可得
$$\ddot{x}=-\frac{M^2{l}^{2}g}{Q_{eq}}{\theta }_P+\frac{J_P+M{l}^2+Mlr}{Q_{eq}r}(T_L+T_R)(17)$$
其中$Q_{eq}=J_{P}M+(J_P+M{l}^2)(2m+\frac{2I}{r^2})$

这里我尝试用matlab的Symbolic Math Toolbox进行符号推导：

syms M m I r
den = M + 2 * m + 2 * I / r^2
syms T_L T_R l theta_P(t) x(t)
syms x_ddot theta_P_ddot theta_P % 可以定义上下标
eqn_1 = (T_L + T_R) / (den * r) - M * l / den * theta_P_ddot == x_ddot
syms J_P g
den_2 = J_P + M * l^2
eqn_2 = theta_P_ddot == (M * l * g) / den_2 * theta_P - M * l / den_2 * x_ddot - (T_L + T_R) / den_2


% 利用subs函数进行进行代换
eqn_3 = subs(eqn_1, theta_P_ddot, (M * l * g) / den_2 * theta_P - M * l / den_2 * x_ddot - (T_L + T_R) / den_2)
% eqn_ddx = simplify(eqn_ddx)   % 利用数学表达式化简
eqn_ddx = isolate(eqn_ddx, x_ddot)  % 将x_ddot分离到等式一侧
latex(eqn_ddx)
% 化简为x_ddot=(a/b)theta_P_ddot+(c/d)(T_L+T_R)的形式

结果如下，和我的预期有一定差距，不知道如何通分继续化简了。
$$\ddot{x}=-\frac{\frac{T_L+T_R}{r\left(M+2m+\frac{2I}{r^2}\right)}+\frac{Ml\left(\frac{T_L+T_R}{M{l}^2+J_P}-\frac{Mgl{\theta }_P}{M{l}^2+J_P}\right)}{M+2m+\frac{2I}{r^2}}}{\frac{M^2{l}^2}{\left(M{l}^2+J_P\right)\left(M+2m+\frac{2I}{r^2}\right)}-1}$$

将方程$(14)$代入方程$(16)$中，消去$\ddot{x}$，可得
$$\ddot{{\theta }_P}=\frac{Mlg(M+2m+\frac{2I}{r^2})}{Q_{eq}}{\theta }_P-\frac{(\frac{Ml}{r}+M+2m+\frac{2I}{r^2})}{Q_{eq}}(T_L+T_R)(18)$$

综上所述，对于正向运动有
$$\{\begin{array}{l}\ddot{x}=-\frac{M^2{l}^{2}g}{Q_{eq}}{\theta }_P+\frac{J_P+M{l}^2+Mlr}{Q_{eq}r}(T_L+T_R)\\ \\ \ddot{{\theta }_P}=\frac{Mlg(M+2m+\frac{2I}{r^2})}{Q_{eq}}{\theta }_P-\frac{(\frac{Ml}{r}+M+2m+\frac{2I}{r^2})}{Q_{eq}}(T_L+T_R)\end{array}(19)$$

转向运动

转向运动时由左右两车轮从水平方向上施加给车体的反作用力的大小$H_L$和$H_R$不相等引起的，由刚体定轴转动定律可得
$$J_{\delta }\ddot{\delta }=\frac{d}{2}(H_L+H_R)(20)$$
其中，

物理量	描述	单位
$d$	轮距	$m$
$J_{\delta }$	车体绕$y$轴转动时的转动惯量	$kg\cdot m^2$
$\delta$	小车的偏航角	$rad$

将方程$(5)(6)$相减得
$$(m+\frac{I}{r^2})(\ddot{x_L}-\ddot{x_R})=\frac{T_L-T_R}{r}-(H_L-H_R)(21)$$

当左右两轮运动速度不相等时，车身转向，如上图所示。由几何关系可得
$$\{\begin{array}{l}\dot{x_L}=\dot{\delta }{r}_L\\ \dot{x_R}=\dot{\delta }{r}_R\\ r_L=r_R+d\end{array}(22)$$
解得
$$\dot{\delta }=\frac{\dot{x_L}-\dot{x_R}}{d}(23)$$
进一步可得
$$\ddot{\delta }=\frac{\ddot{x_L}-\ddot{x_R}}{d}(23)$$
联立$(20)(21)(24)$可得
$$\ddot{\delta }=\frac{T_L-T_R}{r(md+\frac{Id}{r^2}+\frac{2J_{\delta }}{d})}(25)$$

状态方程

由方程$(19)(25)$可得系统的状态方程为
$$\left(\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{{\theta }_P}\\ \ddot{{\theta }_P}\\ \dot{\delta }\\ \ddot{\delta }\end{array}\right)=\left(\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& A_{23}& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& A_{43}& 0& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right)\left(\begin{array}{c}x\\ \dot{x}\\ {\theta }_P\\ \dot{{\theta }_P}\\ \delta \\ \dot{\delta }\end{array}\right)+\left(\begin{array}{cc}0& 0\\ B_{21}& B_{22}\\ 0& 0\\ B_{41}& B_{42}\\ 0& 0\\ B_{61}& B_{62}\end{array}\right)\left(\begin{array}{c}{T}_L\\ T_R\end{array}\right)(26)$$
状态变量$(x\dot{x}{\theta }_P\dot{{\theta }_P}\delta \dot{\delta }{)}^T$分别表示小车的位移，前进速度，车体的倾角，车体的角速度，小车的转向角以及转向速度。由于电机输出扭矩的大小不好直接控制，通过刚体定轴转动定律将其转化为两个车轮的加速度。
$$\{\begin{array}{l}\dot{v_{LO}}=\frac{r{T}_L}{I}\\ \dot{v_{RO}}=\frac{r{T}_R}{I}\end{array}(27)$$
其中$v_{LO}$为左轮无摩擦时线速度的大小，单位为$rad/s$

故系统的状态空间表达式变为
$$\{\begin{array}{l}\dot{x}=Ax+Bu\\ y=Cx+Du\end{array}(28)$$
式中
$$A=\left(\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& A_{23}& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& A_{43}& 0& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right)B=\frac{I}{r}\left(\begin{array}{cc}0& 0\\ B_{21}& B_{22}\\ 0& 0\\ B_{41}& B_{42}\\ 0& 0\\ B_{61}& B_{62}\end{array}\right)$$

$$C=\left(\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right)D=0$$

$$x=\left(\begin{array}{c}x\\ \dot{x}\\ {\theta }_P\\ \dot{{\theta }_P}\\ \delta \\ \dot{\delta }\end{array}\right)u=\left(\begin{array}{c}\dot{v_{LO}}\\ \dot{v_{RO}}\end{array}\right)$$

reference
轮趣科技——基于LQR控制的平衡小车