移动机器人动力学

移动机器人动力学

拉格朗日动力学方程

方程如下：
$$\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q_i}}\right)-\frac{\partial L}{\partial q_i}=Q$$其中，L=系统的动能(T)-势能(U)，$q$ 为广义坐标，$Q$ 是作用于系统的广义力。

举个例子，推导一下差速机器人的动力学模型：

  机器人在平面上移动，可以认为势能为零，动能为：
$$T=T_v+T_{\omega }+T_{wr}$$其中，$T_v$是平移动能，$T_{\omega }$是旋转动能，$T_{wr}$是两个轮子旋转的动能。
$$\begin{gathered} T_v=\frac{1}{2}M{v}^2 \\ {T}_{\omega }=\frac{1}{2}I{\omega }^2 \\ {T}_{wr}=\frac{1}{2}{I}_o{\omega }_r^2+\frac{1}{2}{I}_o{\omega }_l^2 \end{gathered}$$
  广义坐标选取为 ${\theta }_r$和 ${\theta }_l$，所以把 $v$和 $\omega$用 ${\dot{\theta }}_r$和 ${\dot{\theta }}_l$代替，所以 $L$为：
$$L=\frac{1}{8}M{r}^2({\dot{\theta }}_r+{\dot{\theta }}_l{)}^2+\frac{I{r}^2}{2L^2}({\dot{\theta }}_r-{\dot{\theta }}_l{)}^2+\frac{I_o^2}{2}({\dot{\theta }}_r^2+{\dot{\theta }}_l^2)$$其中，$I$是机器人相对于自身重心的转动惯量，$r$为轮子半径，$I_o$为轮子的转动惯量。
  广义力为电机的电磁转矩减去阻转矩。
$$Q=\tau -K\dot{\theta }$$其中 $K$是粘性摩擦系数，因此最后方程为：
$$\begin{gathered} (\frac{1}{4}M{r}^2+\frac{I{r}^2}{L^2}+I_o){\ddot{\theta }}_r+(\frac{1}{4}M{r}^2-\frac{I{r}^2}{L^2}){\ddot{\theta }}_l+K{\dot{\theta }}_r={\tau }_r \\ (\frac{1}{4}M{r}^2+\frac{I{r}^2}{L^2}+I_o){\ddot{\theta }}_l+(\frac{1}{4}M{r}^2-\frac{I{r}^2}{L^2}){\ddot{\theta }}_r+K{\dot{\theta }}_l={\tau }_l \end{gathered}$$

  这个方程有啥用呢，其实就是得到的加速度的计算方式，通过后面的电机建模可以知道，电磁转矩 $\tau$可以通过电流和电压得到，这样通过这两个方程就可以计算出任意时刻的加速度。有了完整的加速度信息，在里程计计算、定位、控制方面等方面的精度都大大的提高了。
  所有移动机器人动力学方程的推导都是这个套路，没有势能，动能包括平移动能、旋转动能和轮子转动的动能。

直流电机建模

  直流电机可以写出两个方程，电压方程：
$$u=Ri+L\frac{di}{dt}+K_e\dot{\theta }$$
  转矩方程：
$$J\ddot{\theta }=K_{T}i-K\dot{\theta }$$
  取 $i$和 $\dot{\theta }$为状态变量：则联立上述两式子得到电机的状态空间方程为：
$$\left[\begin{array}{c}\dot{i}\\ \ddot{\theta }\end{array}\right]=\left[\begin{array}{cc}-\frac{R}{L}& -\frac{K_e}{L}\\ K_T& -K\end{array}\right]\left[\begin{array}{c}i\\ \dot{\theta }\end{array}\right]+\left[\begin{array}{c}\frac{1}{L}\\ 0\end{array}\right]u$$其中 $K$是粘性摩擦系数，$K_e$和$K_T$是电机电动势和电磁转矩计算时需要的公式。标准的电动势和电磁转矩计算公式为：
$$\begin{gathered} E=C_e\Phi n \\ \tau =C_T\Phi I \end{gathered}$$其中n为转速，所以:
$$\begin{gathered} K_e=\frac{C_e\Phi }{2\pi } \\ {K}_T=C_T\Phi \end{gathered}$$这里的$C_e$、$C_T$和 $\Phi$就可以从电机的参数上找到了。