用MATLAB玩转机器人--第五章 机器人的数学建模

5.2 机器人的机械结构数学建模

利用机器人的运动学和动力学知识，使用拉格朗日法推导机器人的机械结构数学模型。

5.2.1 机器人运动学基础

（1） D-H表示法
D-H法是 Denative 和 Hartenberg 提出的针对机器人运动学进行建模的标准方法。适用于任何机器人构型，而与机器人的结构和复杂程度无关。
D-H法为机器人的每个关节处的杆件坐标系建立一个 4×4 齐次变换矩阵，以此表示此关节处的单杆与前一个单杆坐标系的关系。总体思想是首先给每个关节指定坐标系，然后确定从一个关节到相邻下一个关节的变换矩阵。通过逐次变换，把所有变化结合起来，就确定了机器人的末端关节与基座（固定坐标系）之间的总变化，从而建立运动学方程并求解。
（2） 正运动学
假设有一个构型已知的机器人，它的所有关节角度和单杆长度都是已知的，那么很容易算出机器人的受爪在空间的位置，这一过程称为正运动学分析。换而言之，如果机器人的所有关节角度变量已知，用正运动学方程就能计算任一瞬间机器人手爪的位置。即正运动学是通过已知关节空间的关节角变量，去计算笛卡尔空间手爪位置的分析方法。
（3） 逆运动学
如果机器人的手爪在空间中的位置是已知的，就必须反过来求取机器人的每一个关节的角度。只有在机器人各个关节的角度达到要求的值时，才能保证机器人的手爪定位在所期望的位置。即已知笛卡尔空间中手爪的位置，求取关节空间中各个关节变量的值，这是逆运动学。
简而言之，正运动学是由关节角度求位置，而逆运动学是由位置求关节角度。

5.2.2 机器人坐标变换

在机器人的控制中，经常需要用到不同的坐标系，各种数据需要在不同的坐标系之间进行变换，主要变换为平移变换和旋转变换。
（1） 二维平面上的平移变换

如图5-6，点X在坐标系A（$x_A$,$y_A$）中表示为${}^A\text{X}$=${[}^A\text{x}{,}^A\text{y}{]}^T$，在坐标系B（$x_B$,$y_B$）中表示为向量${}^B\text{X}$=${[}^B\text{x}{,}^B\text{y}{]}^T$。坐标系 A 到 B 的平移向量为${}^A{\text{q}}_B$=$[q_x,q_y{]}^T$，则 ${}^A\text{X}$和${}^B\text{X}$的关系如下表示：
$$\left[\begin{array}{c}{}^A\text{x}\\ {}^A\text{y}\end{array}\right]=\left[\begin{array}{c}{}^B\text{x}\\ {}^B\text{y}\end{array}\right]+\left[\begin{array}{c}{q}_x\\ q_y\end{array}\right]$$
即$${}^A\text{X}{=}^B\text{X}{+}^A{\text{q}}_B\text{\hspace{1em}(5-19)}$$
（2）二维平面上的旋转变换
如图5-7，坐标系B（$x_B$,$y_B$）是由坐标A（$x_A$,$y_A$）旋转角度θ得到的，因此对于点X，${}^A\text{X}$=${[}^A\text{x}{,}^A\text{y}{]}^T$，${}^B\text{X}$=${[}^B\text{x}{,}^B\text{y}{]}^T$之间就是一个旋转变换关系。其中旋转方向以逆时针为正。旋转矩阵：
$${}^A{\text{R}}_B=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\text{\hspace{1em}(5-20)}$$
变换结果为：$${}^A\text{X}{=}^A{\text{R}}_B{}^B\text{X}\text{\hspace{1em}(5-21)}$$
即$$\left[\begin{array}{c}{}^A\text{x}\\ {}^A\text{y}\end{array}\right]=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\left[\begin{array}{c}{}^B\text{x}\\ {}^B\text{y}\end{array}\right]=\left[\begin{array}{c}{}^B\text{x}\times cos\theta {-}^B\text{y}\times sin\theta \\ {}^B\text{x}\times sin\theta {+}^B\text{y}\times cos\theta \end{array}\right]$$
（3）二维平面上的平移、旋转复合变换
对于点X，在坐标系A、B中分别表示为${}^A\text{X}$，${}^B\text{X}$，坐标系 A 到 B 的平移向量为${}^A{\text{q}}_B$=$[q_x,q_y{]}^T$，旋转向量为${}^A{\text{R}}_B$，则复合变换公式为：
$${}^A\text{X}{=}^A{\text{R}}_B{}^B\text{X}{+}^A{\text{q}}_B\text{\hspace{1em}(5-22)}$$
（4）三维空间中的平移、旋转变换
三维空间中的复合变换公式同二维空间为：
$${}^A\text{X}{=}^A{\text{R}}_B{}^B\text{X}{+}^A{\text{q}}_B\text{\hspace{1em}(5-27)}$$
在三维空间中，围绕 x 轴、y轴、z轴旋转 θ 角之后的旋转矩阵分别如下所示：
绕 x 轴旋转：
$${}^A{\text{R}}_B=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & -sin\theta \\ 0& sin\theta & cos\theta \end{array}\right]\text{\hspace{1em}(5-24)}$$
绕 y 轴旋转：
$${}^A{\text{R}}_B=\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]\text{\hspace{1em}(5-25)}$$
绕 z 轴旋转：
$${}^A{\text{R}}_B=\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(5-26)}$$
（5）坐标变换矩阵的整合
建立 4×4矩阵，将点 X在坐标系旋转加平移转换关系整合表示为：$$\left[\begin{array}{c}{}^A\text{X}\\ 1\end{array}\right]=\left[\begin{array}{cc}{}^A{\text{R}}_B& {}^A{\text{q}}_B\\ 0& 1\end{array}\right]\left[\begin{array}{c}{}^B\text{X}\\ 1\end{array}\right]\text{\hspace{1em}(5-30)}$$

5.2.3 利用拉格朗日法导出机械结构模型

拉格朗日法的基础是分析系统能量与系统变量及其微分之间的关系，是将有关运动的描述转化为能量的描述，从而求得运动方程式。
不是一般性，取 n 维力学空间中 Q 点的 坐标为（$q_1$,$q_2$,…,$q_n$）,而$q_i$(i = 1,2,…,n)所受合力为$u_i$。定义拉格朗日函数 L = T - U，T 是动能，U 是势能。荣国公式推导可以得到拉格朗日运动方程为：
$$u_i=\frac{d}{dt}(\frac{\partial L}{\partial {\dot{q}}_i})-\frac{\partial L}{\partial q_i}+\frac{\partial D}{\partial {\dot{q}}_i}\text{\hspace{1em}(5-31)}$$
上式中 D 是损失能量。
考虑到粘性摩擦，造成的能量损失 D 为：
$$D=\frac{1}{2}\sum c_i{\dot{q}}_i^2\text{\hspace{1em}(5-32)}$$
这里的 $c_i$是粘性系数。
综上所述，利用拉格朗日法导出运动方程式的步骤如下：
① 设定系统的 $q_i$和对应的力$u_i$。
② 计算系统的动能 T；势能 U 和能量损失 D。
③ 将第②步的结果带入拉格朗日运动方程（5-31）。
利用拉格朗日法导出单杆的数学模型，坐标系如图5-8所示。为了简化运算，忽略电机和齿轮的质量以及惯性的影响。

以单杆的轴心端(连接电机)为原点，存在两个坐标系。其中，固定坐标系（0）的坐标轴为（${}^0\text{x}{,}^0\text{y}{,}^0\text{z}$），他是世界坐标系（X,Y,Z）在 XY 平面上的一个平移。固定于单杆的坐标系（1）的坐标轴为（${}^1\text{x}{,}^1\text{y}{,}^1\text{z}$），${}^1\text{z}$ 和 ${}^0\text{z}$轴重合。单杆坐标系（1）随着单杆的转动而相对于固定坐标系（0）绕 ${}^0\text{z}$轴旋转。
对于空间中的任一点 X∈$R^3$,在坐标系（0）、（1）中的坐标向量分贝为${}^0\text{X}$ 和 ${}^1\text{X}$，扩展为
$$\left[\begin{array}{c}{}^0\text{X}\\ 1\end{array}\right]=\left[\begin{array}{c}{}^0\text{x}\\ {}^0\text{y}\\ {}^0\text{z}\\ 1\end{array}\right],\left[\begin{array}{c}{}^1\text{X}\\ 1\end{array}\right]=\left[\begin{array}{c}{}^1\text{x}\\ {}^1\text{y}\\ {}^1\text{z}\\ 1\end{array}\right]\text{\hspace{1em}(5-33)}$$
从坐标系（0）变换到坐标系（1），因为是绕 ${}^0\text{z}$ 轴旋转 ${\theta }_1$，所以由公式（5-26）可得坐标变换矩阵 $A_1$ 为：
$${}^A{\text{R}}_B=\left[\begin{array}{cccc}cos{\theta }_1& -sin{\theta }_1& 0& 0\\ sin{\theta }_1& cos{\theta }_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(5-34)}$$
图 5-8中，单杆的重心点 $P_{g1}$在单杆坐标系（1）上的坐标为（$r_1$，0，0），由式（5-30）可得$P_{g1}$点在固定坐标系（0）上有：
$$P_{g1}=A_1\left[\begin{array}{c}{r}_1\\ 0\\ 0\\ 1\end{array}\right]\text{\hspace{1em}(5-35)}$$
将式（5-34）代入式（5-35）可得：
$$P_{g1}=\left[\begin{array}{c}{r}_{1}cos{\theta }_1\\ r_{1}sin{\theta }_1\\ 0\\ 1\end{array}\right]\text{\hspace{1em}(5-36)}$$
式（5-36）为单杆的重心在固定坐标系（0）上你的坐标值，将以上 4×1 的向量去掉扩充的最后一个元素“1”，恢复到 3×1矩阵：
$$P_{g1}=\left[\begin{array}{c}{r}_{1}cos{\theta }_1\\ r_{1}sin{\theta }_1\\ 0\end{array}\right]\text{\hspace{1em}(5-37)}$$
对式（5-37）求时间上额微分，得到速度量$V_{g1}$:
$$V_{g1}=\left[\begin{array}{c}-r_1{\dot{\theta }}_{1}sin{\theta }_1\\ r_1{\dot{\theta }}_{1}cos{\theta }_1\\ 0\end{array}\right]\text{\hspace{1em}(5-38)}$$
因为单杆坐标系（1）是绕固定坐标系（0）以 ${}^0\text{z}$为轴旋转${\theta }_1$角度，因此在${}^0\text{x}$轴和${}^0\text{y}$轴上没有角度变化。所以单杆坐标系（1）相对于固定坐标系（0）的角速度${}^1\text{ω}$为：
$${}^1\text{ω}=\left[\begin{array}{c}0\\ 0\\ {\dot{\theta }}_1\end{array}\right]\text{\hspace{1em}(5-39)}$$
${\dot{\theta }}_1$是单杆坐标系（1）绕固定坐标系${}^0\text{z}$轴（与${}^1\text{z}$轴重合）旋转的角速度。
此外，单杆的动能 T 是单杆运动时的速度与角速度的能量之和：
$$T=\frac{1}{2}{m}_1(V_{g1}{)}^T{V}_{g1}+\frac{1}{2}({}^1\text{ω}{)}^T{I}_{zzg1}{}^1\text{ω}\text{\hspace{1em}(5-40)}$$
式中，$m_1$是单杆的质量，$V_{g1}$是单杆重心的移动速度，${}^1\text{ω}$是单杆旋转的角速度，$I_{zzg1}$是单杆重心在 z 轴方向的转动惯量。
综上得到：
$$T=\frac{1}{2}{\left[\begin{array}{c}-r_1{\dot{\theta }}_{1}sin{\theta }_1\\ r_1{\dot{\theta }}_{1}cos{\theta }_1\\ 0\end{array}\right]}^T\left[\begin{array}{c}-r_1{\dot{\theta }}_{1}sin{\theta }_1\\ r_1{\dot{\theta }}_{1}cos{\theta }_1\\ 0\end{array}\right]+\frac{1}{2}{\left[\begin{array}{c}0\\ 0\\ {\dot{\theta }}_1\end{array}\right]}^T{I}_{zzg1}\left[\begin{array}{c}0\\ 0\\ {\dot{\theta }}_1\end{array}\right]$$
整理后得：
$$T=\frac{1}{2}{m}_1{r}_1^2{\dot{\theta }}_1^2+\frac{1}{2}{I}_{zzg1}{\dot{\theta }}_1^2\text{\hspace{1em}(5-42)}$$
因为哪敢是在 XY 平面内转动，因此将重力和势能忽略不计。力矩$\tau$使单杆旋转的动力，所以$u_1=\tau$，并且$q_1={\theta }_1$。考虑到粘性摩擦，可得：
$$\frac{\partial D}{\partial {\dot{q}}_i}=\frac{\partial D}{\partial {\dot{\theta }}_1}=\partial (\frac{1}{2}{c}_1{\dot{\theta }}_1^2)/\partial {\dot{\theta }}_1=c_1{\dot{\theta }}_1\text{\hspace{1em}(5-43)}$$
下面分别计算拉格朗日运动方程（5-31）中的其他各项：
$$\frac{\partial L}{\partial {\dot{q}}_i}=\frac{\partial T}{\partial {\dot{\theta }}_1}=m_1{r}_1^2{\dot{\theta }}_1+I_{zzg1}{\dot{\theta }}_1\text{\hspace{1em}(5-44)}$$
所以，
$$\frac{d}{dt}(\frac{\partial L}{\partial {\dot{q}}_i})=m_1{r}_1^2{\ddot{\theta }}_1+I_{zzg1}{\ddot{\theta }}_1\text{\hspace{1em}(5-45)}$$
又有
$$\frac{\partial L}{\partial q_1}=\frac{\partial T}{\partial {\theta }_1}=0\text{\hspace{1em}(5-46)}$$
将以上各项带入拉格朗日运动方程，
$$u_i=\frac{d}{dt}(\frac{\partial L}{\partial {\dot{q}}_i})-\frac{\partial L}{\partial q_i}+\frac{\partial D}{\partial {\dot{q}}_i}$$
整理后可得：
$$\tau =(m_1{r}_1^2+I_{zzg1}){\ddot{\theta }}_1+c_1{\dot{\theta }}_1\text{\hspace{1em}(5-47)}$$
即为单杆运动的数学模型。

5.3 机器人的电气结构数学建模

驱动机器人关节运动的为电机，因此建立直流电机和减速器的数学模型。
（1） 直流电机的数学模型
直流电机，是将地哪能转换成机械能的一种装置。直流电机的电磁转矩$T_e$(由电枢电流和磁场相互作用而产生的电磁力形成的转矩)与流经电机转子即电枢的电流$i_a$成正比，其比例常数$K_t$为转矩常数，即
$$T_e=K_t{i}_a\text{\hspace{1em}(5-48)}$$
直流电机的无负载转速与反电动势成正比。直流电机轴在旋转时两个端子之间会产生电压，成为反电动势。反电动势 e 与角速度$\omega =\dot{\theta }$成正比，比例系数是 $K_e$：
$$e=K_e\omega =K_e\frac{d\theta }{dt}\text{\hspace{1em}(5-49)}$$
直流电机在无负载运行时，输入电压等于反电动势，与转动速度成正比。可以认为$K_t$和$K_e$在电学上是同一个量，即$K_e=K_t$。
在电枢等速旋转时，直流电机产生的驱动转矩 $T_e$ 必须要与传递到负载侧的电磁转矩 T 和空载损耗之和想平衡。空载损耗是指电机旋转时需考虑的惯性（转动惯量 J ）和粘滞摩擦（旋转运动对应粘滞摩擦系数 $B_m$）这些因素。
因此，直流电机产生的点此转矩 $T_e$ 与将要传递给负载方的转矩 T 的平衡关系为：
$$T_e=T+(J\ddot{\theta }+B_m\dot{\theta })\text{\hspace{1em}(5-50)}$$
式中，θ 表示旋转角度；$\dot{\theta }$表示旋转角速度 [rad/s]；$\ddot{\theta }$表示旋转角加速度(rad/$s^2$)；J 为转动惯量； $B_m$为旋转运动对应的粘滞摩擦系数。
忽略电机内电刷压降，由基尔霍夫电压定律（在任何一个闭合回路中，各元件上的电压降的代数和等于电动势的代数和，即从一点出发绕回路一周回到该点时，各段电压的代数和恒等于零）可得：
$$v=R_a{i}_a+L_a\frac{d{i}_a}{dt}+K_e\frac{d\theta }{dt}\text{\hspace{1em}(5-51)}$$
其中，v 为输入电压，$R_a$为转子电阻，$i_a$为转子电流，$L_a$是转子电感。
将式（5-51）进行拉普拉斯变换得到：
$$v(s)=R_a{i}_a(s)+L_{a}s{i}_a(s)+K_{e}s\theta (s)\text{\hspace{1em}(5-52)}$$
整理后得到直流电机的模型为：
$$i_a(s)=\frac{1}{L_{a}s+R_a}v(s)-\frac{K_{e}s}{L_{a}s+R_a}\theta (s)\text{\hspace{1em}(5-53)}$$
（2） 减速器的数学模型
假设减速器链接的齿轮 1 和齿轮 2 的齿数分别为 $n_1$ 和 $n_2$，则变速比 N 为：
$$N=n_2/n_1\text{\hspace{1em}(5-54)}$$
当直流电机的旋转角度为 ${\theta }_1$，单杆的旋转角度为 ${\theta }_2$时，
$${\theta }_1=N\times {\theta }_2\text{\hspace{1em}(5-55)}$$
考虑齿轮传动中滑动摩擦等因素，在直流电机侧的输出扭矩，即公式（5-50）中共的 T 传递到负载侧时还损失一部分能量。摩擦损失与电机侧输出扭矩 T 成比例，摩擦损失比例系数为 c，则传到单杆的转轴一侧的单杆扭矩 ${\tau }_2$为
$${\tau }_2=N(T-cT)=N\times (1-c)\times T=N\times E\times T\text{\hspace{1em}(5-56)}$$
式中 E = 1 - c 被称为传导系数。为了突出对比，将电机侧输出扭矩 T 表示为 ${\tau }_1$，则单杆侧扭矩${\tau }_2$为：
$${\tau }_2=N({\tau }_1-c{\tau }_1)=N\times (1-c)\times {\tau }_1=N\times E\times {\tau }_1\text{\hspace{1em}(5-57)}$$