【Matlab 六自由度机器人】机器人动力学概述

在前几篇运动学文章中，我们聚焦于机器人的位置、速度和加速度等运动特性，尚未探讨驱动这些运动所需的力和扭矩。今天，我们补齐动力学这一关键环节。本篇重点介绍基于拉格朗日方程的机器人动力学推导以及MATLAB实现方法。先回顾系列已有的内容，再进入正题。

近期更新

【汇总】

【Matlab 六自由度机器人】动力学建模与分析系列文章汇总

【主线】

运动学：定义标准型及改进型D-H参数，建立机器人模型。实现运动学正解。运用蒙特卡罗方法构建机器人工作空间。

动力学：（本篇启动，后续持续更新）

【补充说明】

• 灵活工作空间与可达工作空间的概念辨析
• 改进型D-H参数的详细建立步骤解析
• 旋转参数化方法（欧拉角、姿态角、四元数）相关问题讨论
• 双变量函数atan2(x,y)的含义与用法
• 机器人运动学反解常见问题与解决方案

前言

机器人动力学描述了力与运动之间的内在联系，而先前讨论的运动学方程仅刻画了运动本身，未涉及产生运动的力和扭矩。在分析机器人操作的动态数学模型时，主要采用两种经典理论：其一是牛顿‐欧拉方程（基于力与力矩的动态平衡法），其二是拉格朗日力学（特别是二阶拉格朗日方程）。牛顿‐欧拉法需从运动学出发求解加速度，并消除各内作用力，对于复杂系统而言计算较为繁琐。相比之下，拉格朗日方程仅需计算速度，无需处理内作用力，是一种更为直接高效的建模方法。本文重点采用拉格朗日方程来分析与求解机械手的动力学问题，适合六自由度机器人等复杂系统的动力学建模。

正文

一、拉格朗日方程的推导

1. 单自由度系统

我们从最简单的单自由度系统入手。下图展示了一个质点在垂直方向受重力 \(mg\) 和外力 \(f\) 作用的模型，借此可直观理解拉格朗日方程的物理含义。该质点仅在垂直方向运动，构成单自由度系统。

Alt

根据牛顿第二定律，运动方程为：

\[ m\ddot{y} = f - mg \]

左侧项可改写为动能对速度的偏导形式：

\[ m\ddot{y} = \frac{\mathrm{d}}{\mathrm{d}t}\left(m\dot{y}\right) = \frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial}{\partial \dot{y}}\left(\frac{1}{2}m\dot{y}^2\right) = \frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial \mathcal{K}}{\partial \dot{y}}\right) \]

其中动能 \(\mathcal{K} = \frac{1}{2}m\dot{y}^2\)。同样，重力项可表示为势能的梯度：

\[ mg = \frac{\partial}{\partial y}(mgy) = \frac{\partial \mathcal{P}}{\partial y} \]

势能 \(\mathcal{P} = mgy\)。定义拉格朗日算子 \(\mathcal{L} = \mathcal{K} - \mathcal{P} = \frac{1}{2}m\dot{y}^2 - mgy\)。利用偏导关系，可将运动方程写成：

\[ f = \frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial \dot{y}} - \frac{\partial \mathcal{L}}{\partial y} \]

至此单自由度系统的拉格朗日方程推导完成。推广到 \(n\) 自由度系统，广义坐标设为 \(q_1,\dots,q_n\)，运动方程为：

\[ \frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial \dot{q}_k} - \frac{\partial \mathcal{L}}{\partial q_k} = \tau_k, \quad k=1,\dots,n \]

其中 \(\tau_k\) 为对应的广义力。欧拉-拉格朗日方程不仅导出一组耦合的二阶常微分方程，还提供了一种等效于牛顿第二定律的动力学构造方法。对于多连杆机器人等复杂系统，拉格朗日方法展现出显著的建模优势。

下面分别介绍单连杆和双连杆机械臂系统的拉格朗日方程推导过程。

2. 单连杆机械臂系统

下图中单连杆机器人由一个刚性连杆通过齿轮系连接到直流电机。设 \(\theta_l\) 和 \(\theta_m\) 分别为连杆和电机轴的转角，且 \(\theta_m = r\theta_l\)（\(r:1\) 为齿轮变速比）。该系统仅有一个自由度，选取 \(\theta_l\) 为广义坐标。

请添加图片描述

系统动能：

\[ \mathcal{K} = \frac{1}{2}J_m\dot{\theta}_m^2 + \frac{1}{2}J_\ell\dot{\theta}_\ell^2 = \frac{1}{2}\left(r^2J_m + J_\ell\right)\dot{\theta}_\ell^2 = \frac{1}{2}J\dot{\theta}_\ell^2 \]

其中 \(J = r^2J_m + J_\ell\)。势能：

\[ \mathcal{P} = Mg\ell(1-\cos\theta_\ell) \]

拉格朗日算子：

\[ \mathcal{L} = \frac{1}{2}J\dot{\theta}_\ell^2 - Mg\ell(1-\cos\theta_\ell) \]

代入拉格朗日方程，得：

\[ J\ddot{\theta}_\ell + Mg\ell\sin\theta_\ell = \tau_\ell \]

广义力 \(\tau_\ell\) 包括电机输入力矩 \(u = r\tau_m\) 以及阻尼力矩（电机阻尼 \(B_m\)、连杆阻尼 \(B_\ell\) 反映到连杆上为 \(B = rB_m + B_\ell\)）。故完整动力学方程为：

\[ u = J\ddot{\theta}_\ell + B\dot{\theta}_\ell + Mg\ell\sin\theta_\ell \]

3. 双连杆机械臂系统

下图为双连杆机械臂，属于开式运动链结构，运动特性类似于复摆。符号说明：\(T_1,T_2\) 为施加的转矩；\(m_1,m_2\) 为连杆1、2的质量（以末端质点简化表示）；\(d_1,d_2\) 为连杆长度；\(\theta_1,\theta_2\) 为广义坐标；\(g\) 为重力加速度。

请添加图片描述

首先计算连杆1的动能和势能：

\[ v_1 = d_1\dot{\theta}_1,\quad K_1 = \frac12 m_1 d_1^2 \dot{\theta}_1^2,\quad P_1 = -m_1 g d_1 \cos\theta_1 \]

连杆2的动能计算：

\[ v_2^2 = \dot{x}_2^2 + \dot{y}_2^2,\quad x_2 = d_1\sin\theta_1 + d_2\sin(\theta_1+\theta_2),\quad y_2 = -d_1\cos\theta_1 - d_2\cos(\theta_1+\theta_2) \]

求导后得到速度平方表达式：

\[ v_2^2 = d_1^2\dot{\theta}_1^2 + d_2^2(\dot{\theta}_1+\dot{\theta}_2)^2 + 2d_1 d_2\cos\theta_2(\dot{\theta}_1^2 + \dot{\theta}_1\dot{\theta}_2) \]

因此：

\[ K_2 = \frac12 m_2 d_1^2 \dot{\theta}_1^2 + \frac12 m_2 d_2^2 (\dot{\theta}_1+\dot{\theta}_2)^2 + m_2 d_1 d_2\cos\theta_2(\dot{\theta}_1^2 + \dot{\theta}_1\dot{\theta}_2) \]

势能：

\[ P_2 = -m_2 g d_1\cos\theta_1 - m_2 g d_2\cos(\theta_1+\theta_2) \]

总动能和总势能：

\[ K = K_1+K_2 = \frac12(m_1+m_2)d_1^2\dot{\theta}_1^2 + \frac12 m_2 d_2^2(\dot{\theta}_1+\dot{\theta}_2)^2 + 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+m_2)g d_1\cos\theta_1 - m_2 g d_2\cos(\theta_1+\theta_2) \]

二、MATLAB实例推导

1. 机器人模型的建立

依据上述双连杆机械臂进行建模。单连杆机械臂系统代码如下：

%% 单杆机械臂系统
clc;clear;close all;warning off
%% MOD-DH参数
d1 = 0; d2 = 100; a1 = 0; a2 = 0; alpha1 = pi/2; alpha2 = 0;
L(1)=Link([0 d1 a1 alpha1 0 ],'modified');
L(2)=Link([0 d2 a2 alpha2 0 ],'modified');
Single_Robot = SerialLink(L,'name','SingleRobot');
Single_Robot.teach()
% 限制关节空间
theta1min = -180; theta1max = 180;
theta2min = -180; theta2max = 180;
L(1).qlim = [theta1min theta1max]*pi/180;
L(2).qlim = [theta2min theta2max]*pi/180;

双连杆机械臂系统代码如下：

%% 双连杆机械臂系统
clc;clear;close all;warning off
%% MOD-DH参数
d1 = 0; d2 = 0; d3 = 0;
a1 = 0; a2 = 100; a3 = 100;
alpha1 = pi/2; alpha2 = 0; alpha3 = 0;
L(1)=Link([0 d1 a1 alpha1 0 ],'modified');
L(1).offset = -pi/2;
L(2)=Link([0 d2 a2 alpha2 0 ],'modified');
L(3)=Link([0 d3 a3 alpha3 0 ],'modified');
Double_Robot = SerialLink(L,'name','SingleRobot');
Double_Robot.display()
Double_Robot.teach()
theta1min = -180; theta1max = 180;
theta2min = -180; theta2max = 180;
theta3min = -180; theta3max = 180;
L(1).qlim = [theta1min theta1max]*pi/180;
L(2).qlim = [theta2min theta2max]*pi/180;
L(3).qlim = [theta3min theta3max]*pi/180;

接下来进行运动学轨迹规划。单杆系统轨迹规划如下：

n = 1:100;
q0 = [0 0];
q1 = [30 30];
[q,qd,qdd] = jtraj(q0,q1,n);
figure(2)
subplot(3,1,1); plot(n,q)
subplot(3,1,2); plot(n,qd)
subplot(3,1,3); plot(n,qdd)

请添加图片描述

双连杆系统轨迹规划如下：

n = 1:100;
q0 = [0 0 0];
q1 = [30 60 90];
[q,qd,qdd] = jtraj(q0,q1,n);
figure(2)
subplot(3,1,1); plot(n,q)
subplot(3,1,2); plot(n,qd)
subplot(3,1,3); plot(n,qdd)

请添加图片描述

关于 jtraj() 和笛卡尔空间轨迹规划 ctraj() 函数，将在后续独立篇章中详细讨论。运动学的前期准备已经完成，下面进入机器人动力学拉格朗日方程的MATLAB实现部分。

2. 动力学代码

% 查看robot机器人所有的连杆的动力学参数
robot.dyn;
% 查看robot机器人第n根连杆的动力学参数
robot.dyn(n);
% 对Single_Robot进行动力学参数的设置

总结

本篇对机器人动力学进行了系统概述。第一章详细推导了拉格朗日方程，从单自由度系统入手，逐步引入单连杆和双连杆机械臂的动力学建模，清晰展示了动能与势能的计算方法。第二章给出了上述理论的MATLAB代码实现，完成了从建模到轨迹规划的完整流程。至此，本文完成了动力学概述的核心内容，后续将继续深入探讨六自由度机器人的动力学仿真与实际应用案例。

参考文献

机器人学、机器视觉与控制：MATLAB算法基础

机器人学 机器人建模和控制

MATLAB机器人工具箱（四）动力学

MATLAB机器人工具箱【3】—— 动力学相关函数及用法