机器人动力学--拉格朗日公式

机器人动力学–拉格朗日公式

摘要

机器人动力学研究机器人运动所需的力和力矩。动力学方程，也成运动方程，是一系列二阶微分方程的形式
$$\tau =M(\theta )\ddot{\theta }+h(\theta ,\dot{\theta }),\text{\hspace{1em}(8.1)}$$
这里$\theta \in {\mathbb{R}}^n$是关节变量的矢量，$\tau \in {\mathbb{R}}^n$是关节力和力矩的矢量，$M(\theta )\in {\mathbb{R}}^{n\times n}$是对称正定矩阵，$h(\theta ,\dot{\theta })\in {\mathbb{R}}^n$是包括向心力，科氏力，重力和摩擦力的总和，这些力域$\theta ,\dot{\theta }$相关.

机器人正动力学问题是给定状态$(\theta ,\dot{\theta })$和关节力及力矩，确定机器人的加速度$\ddot{\theta }$,
$$\ddot{\theta }=M^{-1}(\theta )(\tau -h(\theta ,\dot{\theta })),\text{\hspace{1em}(8.2)}$$
机器人动力学逆问题是寻找关节力和关节力矩，使机器人达到对应的状态和期望的加速度，如式(8.1). 推导机器人动力学的两种经典方法是牛顿-欧拉公式和拉格朗日公式。

引例

动力学拉格朗日公式的第一步是选择相互独立的坐标$q\in {\mathbb{R}}^n$描述系统的构型，这些坐标$q$称为广义坐标。第二步是定义广义力$f\in {\mathbb{R}}^n$。广义力和坐标变化率$\dot{q}$是对偶的，它们的内积$f^T\dot{q}$对应功率。然后拉格朗日函数$\mathcal{L}(q,\dot{q})$定义为系统的动能$\mathcal{K}(q,\dot{q})$减去势能$\mathcal{P}(q)$,
$$\mathcal{L}(q,\dot{q})=\mathcal{K}(q,\dot{q})-\mathcal{P}(q).$$
然后依据拉格朗日公式，运动方程可表达为
$$f=\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}}-\frac{\partial \mathcal{L}}{\partial q},\text{\hspace{1em}(8.3)}$$
该方程也称为带外力的欧拉-拉格朗日方程。
首先考虑一个简单例子，以质量为$m$，限制再竖直方向运动的质点为例。该质点的构型空间是一条竖直直线。很自然选择高度作为广义坐标，用标量$x\in \mathbb{R}$表示.假设重力$mg$竖直向下，依据牛顿第二定律，那么运动方程为
$$f-mg=m\ddot{x}.\text{\hspace{1em}(8.4)}$$
使用拉格朗日公式可以推导得到同样结果。
质点的动能为$\frac{1}{2}m{\dot{x}}^2$,势能为$mgx$,那么拉格朗日函数为
$$\mathcal{L}(x,\dot{x})=\mathcal{K}(x,\dot{x})-\mathcal{P}(x)=\frac{1}{2}m{\dot{x}}^2-mgx,\text{\hspace{1em}(8.5)}$$
运动方程为
$$f=\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{x}}-\frac{\partial \mathcal{L}}{\partial x}=m\ddot{x}+mg,\text{\hspace{1em}(8.6)}$$
以下面的2R机械臂为例，用拉格朗日公式推导其运动方程。

机械臂在xy平面内，重力沿-y方向。推导动力学方程，首先要定义连杆的质量和惯量特性。假设连杆质量集中在连杆末端，分别为$m_1,m_2$,连杆1的位置和速度为
$$\begin{gathered} \left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]=\left[\begin{array}{c}{L}_{1}cos{\theta }_1\\ L_{1}sin{\theta }_1\end{array}\right], \\ \left[\begin{array}{c}{\dot{x}}_1\\ {\dot{y}}_1\end{array}\right]=\left[\begin{array}{c}-L_{1}sin{\theta }_1\\ L_{1}cos{\theta }_1\end{array}\right]{\dot{\theta }}_1, \end{gathered}$$
连杆2末端的位置和速度为
$$\begin{gathered} \left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]=\left[\begin{array}{c}{L}_{1}cos{\theta }_1+L_{2}cos({\theta }_1+{\theta }_2)\\ y_{1}sin{\theta }_1+L_{2}sin({\theta }_1+{\theta }_2)\end{array}\right], \\ \left[\begin{array}{c}{\dot{x}}_2\\ {\dot{y}}_2\end{array}\right]=\left[\begin{array}{cc}-L_{1}sin{\theta }_1-L_{2}sin({\theta }_1+{\theta }_2)& -L_{2}sin({\theta }_1+{\theta }_2)\\ L_{1}cos+L_{2}cos({\theta }_1+{\theta }_2)& L_{2}cos({\theta }_1+{\theta }_2)\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right] \end{gathered}$$
选择关节坐标$\theta =({\theta }_1,{\theta }_2)$作为广义坐标,广义力$\tau =({\tau }_1,{\tau }_2)$对应关节力矩。拉格朗日函数为
$$\mathcal{L}(\theta ,\dot{\theta })=\sum _{i=1}^2({\mathcal{K}}_i-{\mathcal{P}}_i)\text{\hspace{1em}(8.7)}$$
连杆的动能为${\mathcal{K}}_1,{\mathcal{K}}_2$为
$$\begin{gathered} {\mathcal{K}}_1=\frac{1}{2}{m}_1({\dot{x}}_1^2+{\dot{y}}_1^2)=\frac{1}{2}{m}_1{L}_1^2{\dot{\theta }}_1^2 \\ {\mathcal{K}}_2=\frac{1}{2}({\dot{x}}_2^2+{\dot{y}}_2^2) \\ =\frac{1}{2}{m}_2((L_1^2+2L_1{L}_{2}cos{\theta }_2+L_2^2){\dot{\theta }}_1+2(L_2^2+L_1{l}_{2}cos{\theta }_2){\dot{\theta }}_1{\dot{\theta }}_2+L_2^2{\dot{\theta }}_2^2) \end{gathered}$$
连杆的势能${\mathcal{P}}_1,{\mathcal{P}}_2$ 为
$$\begin{gathered} {\mathcal{P}}_1=m_{1}g{y}_1=m_{1}g{L}_{1}sin{\theta }_1 \\ {\mathcal{P}}_2=m_{2}g{y}_2=m_{2}g(L_{1}sin{\theta }_1+L_{2}sin({\theta }_1+{\theta }_2)) \end{gathered}$$
欧拉-拉格朗日方程（8.3）应用在该例子为
$${\tau }_i=\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial {\dot{\theta }}_i}-\frac{\partial \mathcal{L}}{{\theta }_i},i=1,2.\text{\hspace{1em}(8.4)}$$
计算整理可得
$$\begin{array}{c}{\tau }_1=(m_1{L}_1^2+m_2(L_1^2+2L_1{L}_{2}cos{\theta }_2+L_2^2)){\ddot{\theta }}_1\\ +m_2(L_1{L}_{2}cos{\theta }_2+L_2^2){\ddot{\theta }}_2-m_2{L}_1{L}_{2}sin{\theta }_2(2{\dot{\theta }}_1{\dot{\theta }}_2+{\dot{\theta }}_2^2)\\ +(m_1+m_2)L_{1}gcos{\theta }_1+m_{2}g{L}_{2}cos({\theta }_1+{\theta }_2)\\ \\ {\tau }_2=m_2(L_1{L}_{2}cos{\theta }_2+L_2^2){\ddot{\theta }}_1+m_2{L}_2^2{\ddot{\theta }}_2\\ +m_{2}g{L}_{2}cos({\theta }_1+{\theta }_2)\end{array}\text{\hspace{1em}(8.9)}$$
上面的方程合并可以写成
$$\tau =M(\theta )\ddot{\theta }+\underset{h(\theta ,\dot{\theta })}{c(\theta ,\dot{\theta })+g(\theta )}\text{\hspace{1em}(8.10)}$$
其中
$$\begin{gathered} M(\theta )=\left[\begin{array}{cc}{m}_1{L}_1^2+m_2(L_1^2+2L_1{L}_{2}cos{\theta }_2+L_2^2)& m_2(L_1{L}_{2}cos{\theta }_2+L_2^2)\\ m_2(L_1{L}_{2}cos{\theta }_2+L_2^2)& m_2{L}_2^2\end{array}\right], \\ c(\theta ,\dot{\theta })=\left[\begin{array}{c}-m_2(L_1{L}_{2}sin{\theta }_2(2{\dot{\theta }}_1{\dot{\theta }}_2)\\ m_2{L}_1{L}_2{\dot{\theta }}_1^{2}cos{\theta }_2\end{array}\right], \\ g(\theta )=\left[\begin{array}{c}(m_1+m_2)L_{1}gcos{\theta }_1+m_{2}g{L}_{2}cos({\theta }_1+{\theta }_2)\\ m_{2}g{L}_{2}cos({\theta }_1+{\theta }_2)\end{array}\right] \end{gathered}$$
由上式可以发现，运动方程与$\ddot{\theta }$是线性关系，与$\dot{\theta }$是二次方关系，与$\theta$是三角函数关系。

在惯性坐标系中（$x_2,y_2$)，加速度可简单写成坐标的二阶导数（${\ddot{x}}_2,{\ddot{y}}_2$）的线性函数。而关节坐标$({\theta }_1,{\theta }_2)$不在惯性坐标系，因此加速度与坐标二阶导数的线性关系，与一阶导数的平方关系。包含二次项${\dot{\theta }}_i^2$的项称为向心项，包含二次项${\dot{\theta }}_i{\dot{\theta }}_j,i̸=j$的项称为科氏项。

拉格朗日公式一般形式

下面推导一般n个连杆的开链机构的拉格朗日动力学公式。第一步选择广义坐标$\theta \in {\mathbb{R}}^n$描述系统的构型空间。广义力表示为$\tau \in {\mathbb{R}}^n$,如果${\theta }_i$是旋转关节，那么${\tau }_i$对应力矩，如果${\theta }_i$是移动关节，那么${\tau }_i$对应力。

一旦选择了广义坐标和确定了广义力，下一步是计算拉格朗日函数
$$\mathcal{L}(\theta ,\dot{\theta })=\mathcal{K}(\theta ,\dot{\theta })-\mathcal{P}(\theta )\text{\hspace{1em}(8.13)}$$
这里$\mathcal{K}(\theta ,\dot{\theta })$是系统总动能，$\mathcal{P}(\theta )$是系统的总势能。对于刚性连杆，动能总可以写成如下形式
$$\mathcal{K}(\theta )=\frac{1}{2}\sum _{i=1}^n\sum _{j=1}^n{m}_{ij}(\theta ){\dot{\theta }}_i{\dot{\theta }}_j=\frac{1}{2}{\dot{\theta }}^{T}M(\theta )\dot{\theta },\text{\hspace{1em}(8.14)}$$
这里$m_{ij}$是$n\times n$的质量矩阵的第$(i,j)$个元素。

动力学方程的解析式可以计算下式的右边的值得到
$${\tau }_i=\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial {\dot{\theta }}_i}-\frac{\partial \mathcal{L}}{\partial {\theta }_i},i=1,2,\dots ,n.\text{\hspace{1em}(8.15)}$$

动能使用式（8.14）表示，动力学方程可以写成
$${\tau }_i=\sum _{j=1}^n{m}_{ij}(\theta ){\ddot{\theta }}_j+\sum _{j=1}^n\sum _{k=1}^n{\Gamma }_{ijk}(\theta ){\dot{\theta }}_j{\dot{\theta }}_k+\frac{\partial \mathcal{P}}{\partial {\theta }_i},i=1,\dots ,n,\text{\hspace{1em}(8.16)}$$
这里${\Gamma }_{ijk}(\theta )$称为第一类Christoffel符号，它的定义如下
$${\Gamma }_{ijk}(\theta )=\frac{1}{2}(\frac{\partial m_{ij}}{\partial {\theta }_k})+\frac{\partial m_{ik}}{\partial {\theta }_j}-\frac{\partial m_{jk}}{\partial {\theta }_i}).\text{\hspace{1em}(8.17)}$$

参考文献

[1] Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017