二连杆机器人动力学——机械臂动力学计算实例

1. 机器人系统模型

假设一个二连杆机械臂结构如图所示。

其中，

机械臂的质量主要富集于机器人关节处，分别为： $m_1$ 、 $m_2$ ；机械臂连杆长度分别为： $d_1$ 、 $d_2$ ；机械臂关节运动为纯旋转，所以仅存在为角度的广义坐标： $\theta _1$ 、 $\theta _2$ ；重力加速度为： $g$ 。

根据定义

拉格朗日方程：

$F_{i}=\frac{d}{dt} \frac{\partial L }{\partial \dot{q}_{i}} - \frac{\partial L }{\partial \dot{q}_{i}},i=1,2,...,n$

其中，

L为拉格朗日函数 $L=K-P$ ，而K为系统广义动能，P为系统广义位能；

$i$ 为连杆子系统索引号（ $i=1,2,...$ ）； $q_i$ 为第 $i$ 个子系统的动能和位能的坐标（位移和角度）， $\dot{q}_i$ 为相应的广义速度（线速度和角速度）； $F_{i}$ 为第 $i$ 个子系统的广义力（力和力矩）。

2. 计算各个连杆的动能及位能

该二连杆机器人系统可将机器人拆解为两个单独连杆（连杆1和连杆2 ）的子系统。

2.1 连杆1的动能

$K_1 \\= \frac{1}{2} m_1 v_{1}^{2} \\=\frac{1}{2} m_1 \left ( \left (\frac{d}{dt}(d_1 sin \theta_1) \right )^2 + \left (\frac{d}{dt}(-d_1 cos \theta_1) \right )^2 \right ) \\ = \frac{1}{2} m_1 \left ( \left ( d_1 cos \theta_1 \dot{\theta}_1 \right )^2 + \left ( d_1 sin \theta_1 \dot{\theta}_1\right )^2 \right ) \\=\frac{1}{2} m_1 d_1^{2} \left ( cos^2 \theta_1+sin^2 \theta_1 \right ) \dot{\theta}_{1}^{2} \\ = \frac{1}{2} m_1 d_1^{2} \dot{\theta}_{1}^{2}$

2.2 连杆1的位能

$P_{1}\\=m_{1}g h_{1} \\= m_{1}g(-d_{1} cos \theta _{1}) \\= - m_{1} g d_{1} cos \theta_{1}$

2.3 连杆2的动能

$K_2 \\= \frac{1}{2} m_2 v_{2}^{2}\\=\frac{1}{2} m_2 \left ( (\frac{d }{d t} (d_1 sin \theta _1 + d_2 sin(\theta _1 + \theta _2)))^{2}+ (\frac{d }{d t} (-d_1 cos \theta _1 - d_2 cos(\theta _1 + \theta _2)))^{2}\right ) \\=\frac{1}{2} m_2 \left ( \left ( d_1 cos \theta_1 \dot{\theta}_1 + d_2 cos (\theta_1 + \theta_2)(\dot{\theta}_1 + \dot{\theta}_2) \right )^{2} +\left(d_1 sin \theta_1 \dot{\theta}_1 + d_2 sin(\theta_1 + \theta_2)(\dot{\theta_1}+\dot{\theta_2})\right)^2 \right ) \\=\frac{1}{2} m_2\left ( d_1^2 \dot{\theta}_{1}^{2} + d_{2}^{2}\left ( \dot{\theta}_1^{2}+2\dot{\theta}_1 \dot{\theta}_2 + \dot{\theta}_2^2 \right ) +2d_1 d_2 cos \theta_2 (\dot{\theta}_1^2 + \dot{\theta}_1 \dot{\theta}_2)\right ) \\=\frac{1}{2} m_2 d_1^2 \dot{\theta}_{1}^{2} + \frac{1}{2} m_2 d_{2}^{2}\left ( \dot{\theta}_1^{2}+2\dot{\theta}_1 \dot{\theta}_2 + \dot{\theta}_2^2 \right ) +m_2 d_1 d_2 cos \theta_2 (\dot{\theta}_1^2 + \dot{\theta}_1 \dot{\theta}_2)$

2.4 连杆2的位能

$P_{2}\\=m_{2}g h_{2} \\= m_{2}g(-d_{1} cos \theta _{1} - d_2 cos(\theta_1 + \theta_2)) \\= - m_{2} g d_{1} cos \theta_{1} - m_{2} g d_{1} cos(\theta_1 + \theta_2)$

2.5 系统总能量

总动能：

$K \\= K_1 + K_2 \\=\frac{1}{2} m_1 d_1^{2} \dot{\theta}_{1}^{2}+\frac{1}{2} m_2 d_1^2 \dot{\theta}_{1}^{2} + \frac{1}{2} m_2 d_{2}^{2}\left ( \dot{\theta}_1^{2}+2\dot{\theta}_1 \dot{\theta}_2 + \dot{\theta}_2^2 \right ) +m_2 d_1 d_2 cos \theta_2 (\dot{\theta}_1^2 + \dot{\theta}_1 \dot{\theta}_2) \\=\frac{1}{2} \left (m_1 +m_2 \right ) d_1^{2} \dot{\theta}_{1}^{2} + \frac{1}{2} m_2 d_{2}^{2}\left ( \dot{\theta}_1^{2}+2\dot{\theta}_1 \dot{\theta}_2 + \dot{\theta}_2^2 \right ) +m_2 d_1 d_2 cos \theta_2 (\dot{\theta}_1^2 + \dot{\theta}_1 \dot{\theta}_2)$

总位能

$P\\=P_1 + P_2 \\=- m_{1} g d_{1} cos \theta_{1}- m_{2} g d_{1} cos \theta_{1} - m_{2} g d_{1} cos(\theta_1 + \theta_2) \\ = - \left (m_{1} +m_2 \right ) g d_{1} cos \theta_{1}- m_{2} g d_{1} cos(\theta_1 + \theta_2)$

3. 拉格朗日平衡法求动力学方程

二连杆机械臂的拉格朗日函数L可以计算得：

$L \\=K-P \\=\frac{1}{2} \left (m_1 +m_2 \right ) d_1^{2} \dot{\theta}_{1}^{2} + \frac{1}{2} m_2 d_{2}^{2}\left ( \dot{\theta}_1^{2}+2\dot{\theta}_1 \dot{\theta}_2 + \dot{\theta}_2^2 \right ) +m_2 d_1 d_2 cos \theta_2 (\dot{\theta}_1^2 + \dot{\theta}_1 \dot{\theta}_2)\\+ \left (m_{1} +m_2 \right ) g d_{1} cos \theta_{1}+ m_{2} g d_{1} cos(\theta_1 + \theta_2)$

让L对分别对 $\theta _1, \theta_2, \dot{\theta}_1, \dot{\theta}_2$ 进行求导：

$\frac{\partial L}{\partial \theta_1} \\ = -(m_1 + m_2) g d_1 sin \theta_1 - m_2 g d_1 sin(\theta_1 + \theta_2)$ ；

$\frac{\partial L }{\partial \theta_2} \\= -m_2 d_1 d_2 sin \theta_2 (\dot{\theta}_1^2 + \dot{\theta}_1 \dot{\theta}_2)- m_2 g d_1 sin(\theta_1+\theta_2)$ ；

$\frac{\partial L}{\partial \dot{\theta}_1} \\= (m_1 + m_2) d_1^2 \dot{\theta}_1 + m_2 d_2^2 \dot{\theta}_1 + m_2 d_2^2 \dot{\theta}_2 + 2 m_2 d_1 d_2 cos \theta_2 \dot{\theta}_1 + m_2 d_1 d_2 cos \theta_2 \dot{\theta}_2$

$\frac{\partial L}{\partial \dot{\theta}_2} \\=m_2 d_2 ^2 \dot{\theta}_1+m_2 d_2 ^2 \dot{\theta}_2 + m_2 d_1 d_2 cos \theta_2 \dot{\theta}_1$

$\frac{d}{dt} \frac{\partial L }{\partial \dot{\theta}_1} \\= (m_1 + m_2) d_1^2 \ddot{\theta}_1 + m_2 d_2^2 \ddot{\theta}_1 + m_2 d_2^2 \ddot{\theta}_2 - 2m_2 d_1 d_2 sin \theta _2 \dot{\theta}_1 \dot{\theta}_2 + 2m_2 d_1 d_2 cos \theta_2 \ddot{\theta}_1 - m_2 d_1 d_2 sin \theta_2 \dot{\theta}_2^2 + m_2 d_1 d_2 cos \theta_2 \ddot{\theta}_2 \\= \left [ (m_1 + m_2) d_1^2 + m_2 d_2^2 + 2m_2 d_1 d_2 cos \theta_2 \right ] \ddot{\theta}_1 + \left ( m_2 d_2^2 + m_2 d_1 d_2 cos \theta_2 \right ) \ddot{\theta}_2 \\- 2 m_2 d_1 d_2 sin \theta_2 \dot{\theta}_1 \dot{\theta}_2 - m_2 d_1 d_2 sin \theta_2 \dot{\theta}_2^2$

$\frac{d}{dt} \frac{\partial L }{\partial \dot{\theta}_2} \\= m_2 d_2^2 \ddot{\theta}_1 + m_2 d_2^2 \ddot{\theta}_2 + m_2 d_1 d_2 cos \theta_2 \ddot{\theta}_1 - m_2 d_1 d_2 sin \theta_2 \dot{\theta}_1 \dot{\theta}_2$

机械臂各关节力矩分别为 $T_1,T_2$ ，根据拉格朗日方程可以得到：

$T_1 \\ = \frac{d}{dt} \frac{\partial L }{\partial \dot{\theta}_1}-\frac{\partial L}{\partial \theta_1} \\=\left [ (m_1 + m_2) d_1^2 + m_2 d_2^2 + 2m_2 d_1 d_2 cos \theta_2 \right ] \ddot{\theta}_1 + \left ( m_2 d_2^2 + m_2 d_1 d_2 cos \theta_2 \right ) \ddot{\theta}_2 \\- 2 m_2 d_1 d_2 sin \theta_2 \dot{\theta}_1 \dot{\theta}_2 - m_2 d_1 d_2 sin \theta_2 \dot{\theta}_2^2 +(m_1 + m_2) g d_1 sin \theta_1 + m_2 g d_1 sin(\theta_1 + \theta_2)$

$T_2 \\ = \frac{d}{dt} \frac{\partial L }{\partial \dot{\theta}_2}-\frac{\partial L}{\partial \theta_2} \\= m_2 d_2^2 \ddot{\theta}_1 + m_2 d_2^2 \ddot{\theta}_2 + m_2 d_1 d_2 cos \theta_2 \ddot{\theta}_1+m_2 d_1 d_2 sin \theta_2 \dot{\theta}_1^2 + m_2 g d_1 sin(\theta_1+\theta_2)$

整理成动力学方程，

$\left\{\begin{matrix} T_1 = \left [ (m_1 + m_2) d_1^2 + m_2 d_2^2 + 2m_2 d_1 d_2 cos \theta_2 \right ] \ddot{\theta}_1 + \left ( m_2 d_2^2 + m_2 d_1 d_2 cos \theta_2 \right ) \ddot{\theta}_2 \\- 2 m_2 d_1 d_2 sin \theta_2 \dot{\theta}_1 \dot{\theta}_2 - m_2 d_1 d_2 sin \theta_2 \dot{\theta}_2^2 +(m_1 + m_2) g d_1 sin \theta_1 + m_2 g d_1 sin(\theta_1 + \theta_2) \\ T_2 = \left (m_2 d_2^2 + m_2 d_1 d_2 cos \theta_2 \right )\ddot{\theta}_1 + m_2 d_2^2 \ddot{\theta}_2 +m_2 d_1 d_2 sin \theta_2 \dot{\theta}_1^2 + m_2 g d_1 sin(\theta_1+\theta_2)\end{matrix}\right.$

写成矩阵形式

$\begin{bmatrix} T_1\\ T_2 \end{bmatrix} \\=\begin{bmatrix} (m_1 + m_2) d_1^2 + m_2 d_2^2 + 2m_2 d_1 d_2 cos \theta_2 & m_2 d_2^2 + m_2 d_1 d_2 cos \theta_2\\ m_2 d_2^2 + m_2 d_1 d_2 cos \theta_2 & m_2 d_2^2 \end{bmatrix} \begin{bmatrix} \ddot{\theta}_1\\ \ddot{\theta}_2 \end{bmatrix} \\+ \begin{bmatrix} 0 & -2m_2 d_1 d_2 sin \theta_2 \dot{\theta}_1-m_2 d_1 d_2 sin \theta_2 \dot{\theta}_2\\ m_2 d_1 d_2 sin \theta_2 \dot{\theta}_1 & 0 \end{bmatrix} \begin{bmatrix} \dot{\theta}_1\\ \dot{\theta}_2 \end{bmatrix} \\+\begin{bmatrix} (m_1 + m_2)gd_1 sin \theta_1 + m_2 g d_1 sin(\theta_1 +\theta_2)\\ m_2 g d_1 sin(\theta_1 + \theta_2) \end{bmatrix}$

通常上述矩阵形式可以用二阶非线性微分方程描述：

$\tau =M(q) \ddot{q}+C(q,\dot{q}) \dot{q}+G(q)$

其中， $q,\dot{q},\ddot{q}$ 分别为机器人广义坐标、广义速度、广义加速度； $M(q)$ 为机械臂惯性矩阵； $C(q,\dot{q})$ 表示离心力和科氏力矩阵； $G(q)$ 表示机器人重力矩阵； $\tau$ 表示机器人广义力。

常用性质：

性质1： $M(q)-2C(q,\dot{q})$ 是一个斜对称矩阵；

性质2： $M(q)$ 是一个对称正定矩阵；

二连杆机器人动力学——机械臂动力学计算实例相关推荐

matlab adams 机械臂,基于ADAMS与MATLAB/SIMULINK的二杆机械臂动力学仿真与验证
川七一 , , , 乓基于与的二杆机械臂动力学仿真与验证贾雪峰' , 宋洪军' , 荣学文 , 李贻斌山东农业大学机械与电子工程学院 , 山东泰安 , 一 ] 」山东大学控制科学与工程学 ...
机械臂动力学--加速度计算
#机械臂动力学–加速度线加速在博客<速度与矢量的微分>的式(5-12)描述了坐标系{A}下的速度矢量BQ^B QBQ,当坐标系{A}的原点与坐标系{B}的原点重合时,速度矢量BQ^BQ ...
Matlab机械臂综合仿真平台，包含运动学、动力学和控制。 MATLAB机器人仿真正逆运动学simulink轨迹规划机械臂动力学控制等
Matlab机械臂综合仿真平台,包含运动学.动力学和控制. MATLAB机器人仿真正逆运动学simulink轨迹规划机械臂动力学控制等 gui控制仿真平台PUMA机器人 robotics toolb ...
机械臂动力学——动力学建模
一.动力学基础概念基本动力学模型 τ＝D(q)q¨+C(q,q˙)+G(q)\tau ＝ D(q)\ddot{q}+C(q,\dot{q})+G(q) τ ＝ D(q)q¨+C(q,q˙)+G( ...
【机械臂算法】机械臂动力学参数辨识仿真
本文以puma560m机械臂为例子进行动力学参数辨识的讲解,puma560m可以在robotic toolbox中找到,这里以它真实机械臂对他的动力学参数进行辨识. 此外这里还有要说的是,机械臂参数辨 ...
中正平和的机器人学笔记——5. 机械臂动力学
0. 基础知识 0.1 线加速度上一篇中我们讲到了在坐标系{A}和坐标系{B}原点重合时,BQ^BQBQ的速度矢量的表示方式: AVQ^AV_QAVQ = BARBVQ^A_BR^BV_QBAR ...
matlab 机械臂动力学
机械臂动力学解决的问题: 1. 正动力学: 已知一个力矩τ,计算出操作臂的运动θ dθ ddθ ,用于模型仿真有用. 2. 逆动力学:已知轨迹点θ/dθ/ddθ ,求出期望的关节力矩矢量τ,用于机械臂 ...
Udacity机器人软件工程师课程笔记（十六）-机械臂仿真控制实例（其一）-Gazebo、RViz和Moveit！
机械臂仿真控制实例目录环境设置项目工具介绍 Gazebo (1)Gazebo组件 (2)Gazebo界面统一机器人描述格式(URDF) RViz Moveit! 1.环境设置对于此项目,使用 ...
Udacity机器人软件工程师课程笔记（十八）-机械臂仿真控制实例（其三）-KR210机械臂反向运动学
机械臂仿真控制实例(其二)-KR210正向运动学目录反向运动学概述为Kuka KR210创建IK解算器 1.反向运动学概述 KR210的最后三个关节是满足三个相邻的关节轴线在单点处相交的旋转关节 ...

二连杆机器人动力学——机械臂动力学计算实例

二连杆机器人动力学——机械臂动力学计算实例相关推荐

最新文章

热门文章