两轮车建模

本文使用拉格朗日方程组动力学分析，进行 两轮车建模。首先列出计算过程，然后使用mathematica直接求解。因为我实际使用的两轮车电机使用的是速度控制模式，而不是通常使用的转矩控制模式，所以最后还将求解系统状态与电机速度之间的关系。

1 计算过程

由几何关系得
\begin{cases}x_b=l sin\theta_b+r\theta_w\\y_b=l cos\theta_b\\x_w=r \theta_w\\y_w=r\end{cases}\tag{1.1}
其中x_b、y_b、x_w、y_w分别是车体和轮子的横纵坐标系，\theta_b、\theta_w分别是车体和轮子的角度，m_b和m_w分别为车体和轮子的质量，l为车体的质心高度，r为轮子半径。
\begin{cases}\dot{x_b}=l  cos\theta_b \dot{\theta}_b+r\dot{ \theta}_w\\\dot{y_b}=-l sin\theta_b \dot{\theta}_b\\\dot{x_w}=r \dot{ \theta}_w\\\dot{y_w}=0\end{cases}\tag{1.2}

将（1.1）和（1.2）带入动能V=\frac{1}{2}  m  x^2+\frac{1}{2}  I \dot{\theta}^2)和势能(U=m g h)的定义式中得
\begin{cases}V_b=\frac{1}{2}  m_b ( l^2  \dot{\theta}_b^2 + 2l  r cos\theta_b \dot{\theta}_b  \dot{\theta}_w +r^2\dot{\theta}_w ^2) + \frac{1}{2} I_b \dot{\theta}_b^2\\V_w=\frac{1}{2} m_w r^2  \dot{\theta}_w^2+\frac{1}{2} I_w \dot{\theta_w}^2\\U_b=m_b  g  l  cos\theta_b\\U_w=m_w  g  r\end{cases}\tag{1.3}

将（1.3）带入拉格朗日算子(L=T-V)中

然后计算几个拉格朗日方程中的中间变量
\begin{cases}\frac{\partial L}{\partial \dot{\theta}_b} = (m_b  l^2 + I_b) \dot{\theta}_b+m_b l  rcos\theta_b  \dot{\theta}_w\\\frac{\mathrm{d}}{\mathrm{d}t} (\frac{\partial L}{\partial \dot{\theta}_b})=I_b \ddot{\theta}_b+m_b l ( l  \ddot{\theta}_b + r cos\theta_b  \ddot{\theta}_w - r sin\theta_b \dot{\theta}_b  \dot{\theta}_w)\\\frac{\partial L}{\partial \theta_b}=m_b l (sin\theta_b  g - r  sin\theta_b  \dot{\theta}_b  \dot{\theta}_w)\\\frac{\partial L}{\partial \dot{\theta}_w}=m_b  l  r  cos\theta_b \dot{\theta}_b + r^2  m_w  \dot{\theta}_w + I_w  \dot{\theta}_w + r^2  m_b  \dot{\theta}_w\\\frac{\mathrm{d}}{\mathrm{d}t} \frac{\partial L}{\partial \dot{\theta}_w}=m_b   l   r   cos\theta_b  \ddot{\theta}_b-m_b  l  rsin\theta_b \dot{\theta}_b^2 +r^2 (m_b+m_w)  \ddot{\theta}_w +I_w  \ddot{\theta}_w\\\frac{\partial L}{\partial \theta_w}=0\end{cases}
其中L为拉格朗日算子(L=T-V)

列写拉格朗日方程(\frac{\mathrm{d}}{\mathrm{d}t} \frac{\partial L}{\partial \dot{q}} - \frac{\partial L}{\partial q} = F)得到
\begin{cases}\frac{\mathrm{d}}{\mathrm{d}t} (\frac{\partial L}{\partial \dot{\theta}_b})-\frac{\partial L}{\partial \theta_b}&=(I_b+m_b l ^2 ) \ddot{\theta}_b +m_bl  r  cos\theta_b  \ddot{\theta}_w \\&\quad -m_b l  sin\theta_b  g \\&=-\tau\\\frac{\mathrm{d}}{\mathrm{d}t} \frac{\partial L}{\partial \dot{\theta}_w}-\frac{\partial L}{\partial \theta_w}&=m_b  l r cos\theta_b \ddot{\theta}_b-m_b l  r sin\theta_b  \dot{\theta}_b^2 \\&\quad +(r^2  m_b+ r^2 m_w +I_w) \ddot{\theta}_w\\&=\tau\end{cases}\tag{1.4}
其中\tau为电机输出转矩。

最后求解（1.4），得到
\begin{cases}\ddot{\theta}_b=\frac{-(i_w +r^2 (m_b +m_w) +m_b  r  lcos\theta_b ) \tau+l r^2 m_b^2  g  sin\theta_b - m_b^2 l^2  r^2 sin\theta_b cos\theta_b  \dot{\theta}_b+m_b m_w  g  l  r^2  sin\theta_w +i_w m_b g  l  sin\theta_b}{i_b(i_w+r^2(m_b+m_w))+l^2m_b(i_w+r^2(sin^2\theta_bm_b+m_w))}\\ \ddot{\theta}_w=\frac{i_b\tau +l^2rsin\theta_bm_b^2(-gcos\theta_b+l\dot{\theta_b}^2)+lm_b((l+rcos\theta_b)\tau+rsin\theta_bi_b\dot{\theta}_b^2)}{i_b(i_w+r^2(m_b+m_w))+lm_b(i_w+r^2(sin\theta_bm_b+m_w))}\end{cases}

2 使用mathematica进行计算

前面列写几何关系，计算车体和轮子的动能和势能是不可避免的，但是后面带入拉格朗日方程以及求解过程可以交给mathematica来完成。

3 以电机角加速度作为输入

上节中视\theta_b和\theta_w为变量，求得了他们与转矩\tau之间的关系。这节将计算\ddot{\theta}_b与电机角加速度\ddot{\theta}_m之间的关系。如果直接控制转矩，则需要考虑电机摩擦力等其他干扰因素。控制电机加速度则可以在设计控制器时忽略这些干扰，将他们交给底层的电机驱动器来解决。

为求解\theta_b和\theta_w之间关系，这里视\theta_w和\tau为变量，这样就可以在\theta_w的解中去除\tau。再带入\ddot{\theta}_m=\ddot{\theta}_w-\ddot{\theta}_b就可以得到电机角加速度与车体角加速度之间的关系。

如果需要用控制器输出预测车体角度变化，则需要对（3.1）求逆

4 源代码

两轮车模型.nb