ARTICLE · 1151334

【MATLAB源码-第459期】基于matlab的D-H模型与阻尼最小二乘逆运动学的六自由度机械臂五次轨迹规划及动力学前馈PD控制仿真.

发布时间：2026-10-10 15:46:55 最近访问：2026-10-10 15:46:56

操作环境：

MATLAB 2024a

1、算法描述

摘要

六自由度串联机械臂具有结构灵活、工作空间较大以及末端位姿调节能力强等特点，是工业装配、搬运、焊接、检测和智能制造系统中的典型执行机构。机械臂运动控制过程同时涉及运动学建模、逆运动学求解、轨迹生成、动力学分析以及闭环跟踪控制等多个环节，其中任一环节存在较大误差都可能影响最终的末端定位精度和运动稳定性。针对这一问题，本文以六转动关节串联机械臂为研究对象，在MATLAB R2024a环境下建立一套完整的运动学与动力学仿真系统。

首先采用标准D-H方法描述各连杆之间的空间坐标变换关系，通过逐级坐标变换获得机械臂各关节及末端执行器位姿。在此基础上建立几何雅可比矩阵，并采用具有自适应阻尼、关节限位约束和回溯搜索机制的阻尼最小二乘算法求解机械臂逆运动学，从而改善传统数值逆解在接近奇异位形时容易产生关节增量过大的问题。随后，根据多个任务路径点在关节空间构造分段五次轨迹，使各段连接位置具有连续的关节角、角速度及角加速度特性。动力学部分综合考虑各连杆质量、质心位置、转动惯量、电机等效转子惯量、黏性摩擦及重力影响，并建立动力学前馈与PD反馈相结合的轨迹跟踪控制器。为检验控制器对模型误差和扰动的适应能力，控制模型与实际对象之间设置一定参数偏差，并在运动过程中加入短时外部扰动力矩。

仿真结果表明，四组任务位姿均能完成稳定逆解，最大位置残差为2.109627×10^-9 m。机械臂关节角总体均方根误差为0.433180°，末端位置均方根误差为5.614945 mm，末端姿态均方根误差为1.003337°，最大控制力矩为44.192922 N·m，整个轨迹过程中未出现控制力矩饱和。无量纲雅可比最小奇异值为0.085812，最大条件数为21.111015，最小关节限位余量为15.653%。将仿真步长由0.01 s减小至0.005 s后，主要性能指标最大相对变化仅为0.097021%，说明所得结果具有较好的数值稳定性。研究结果表明，所建立的仿真系统能够较完整地反映六自由度机械臂从运动学规划到动力学闭环控制的基本过程，可为机械臂运动控制算法分析和参数设计提供参考。

关键词： 六自由度机械臂；D-H模型；逆运动学；阻尼最小二乘；五次轨迹规划；动力学控制；PD控制

一、引言

随着自动化生产系统不断向柔性化、数字化和智能化方向发展，工业机械臂承担的任务已经从早期重复性搬运逐步扩展到复杂装配、精密加工、视觉引导抓取和协同操作等领域。对于典型六自由度串联机械臂而言，六个转动关节可以共同决定末端执行器的位置和姿态，因此能够满足大多数空间操作任务的基本自由度需求。但自由度增加后，机械臂关节之间存在明显的非线性耦合关系，末端位置变化通常由多个关节共同作用产生，这使机械臂运动规划和控制问题具有较强的系统性。

机械臂控制首先需要解决关节变量和末端执行器位姿之间的映射关系。正运动学根据已知关节变量计算末端位姿，其计算过程相对明确；逆运动学则需要根据目标位置和姿态反求关节变量，在实际系统中往往更加复杂。对于具有特定几何结构的机械臂，可以推导解析逆解，但对于一般串联机构、带有关节约束的系统或者需要同时考虑多种优化目标的情况，数值逆运动学具有更好的通用性。近年来针对机械臂微分运动学、雅可比矩阵和数值逆解的研究仍在不断完善，雅可比方法依然是机械臂速度控制、数值逆运动学以及运动性能评价的重要基础。

除逆运动学外，任务路径点之间如何生成连续、平滑且容易被执行机构跟踪的运动轨迹也是机械臂系统的重要问题。若参考轨迹在速度或者加速度层面存在较大突变，容易产生较大的瞬态驱动力矩和结构振动。近年来的轨迹规划研究仍普遍将平滑性、运动时间、冲击和能耗作为重要评价指标，多项研究采用高阶多项式、平滑方向规划和多目标优化方法改善机械臂运动过程。

另一方面，仅利用运动学规划得到关节角并不能完整描述实际机械臂的运动过程。高速运动时，连杆惯性、重力、科氏力、离心力以及摩擦都会影响控制效果，因此需要进一步建立动力学模型并设计闭环控制器。本文将运动学、逆解、轨迹规划、动力学建模、前馈补偿和PD闭环控制组织为统一仿真流程，并通过模型自检、外部扰动测试、奇异性评价和步长减半验证提高实验结果的可信度。

二、六自由度机械臂总体仿真方案

本文建立的机械臂由六个串联转动关节组成，各关节按照固定顺序连接形成开放式运动链。整个系统以主程序作为统一入口，按照机械臂模型建立、任务位姿生成、逆运动学求解、参考轨迹规划、动力学跟踪仿真、性能指标统计和结果绘图的顺序运行。

机械臂首先根据D-H参数建立各关节坐标系，并设置关节允许运动范围。为了保证用于验证的任务位姿实际可达，程序预先设置四组位于关节允许范围内的合法构型，再通过正运动学得到相应末端目标位姿。随后将这些位姿作为逆运动学输入，由数值算法重新求解关节角。这种处理方法能够避免由于人为设置不可达目标而导致逆运动学结果失去可比性，同时能够直接判断数值逆解是否可以重新逼近已知合法构型对应的末端位姿。

得到四个关节路径点以后，在相邻路径点之间进行分段轨迹规划，生成完整的关节位置、速度和加速度参考序列。控制系统以该序列作为目标输入，根据当前机械臂状态计算控制力矩，并利用实际动力学模型计算下一时刻关节状态。

为避免只依据跟踪曲线进行主观判断，本文同时建立多项量化指标，包括关节均方根误差、最大关节误差、末端位置误差、末端姿态误差、雅可比最小奇异值、雅可比条件数、关节限位余量、最大驱动力矩和机械功指标等。最终系统共输出八张PNG结果图以及机械臂运动GIF，实现从整体运动状态到局部性能指标的多角度观察。

三、机械臂运动学建模与逆运动学求解

3.1 D-H运动学建模

本文采用标准D-H方法描述六自由度机械臂相邻连杆之间的几何关系。每一组连杆参数包含连杆长度、连杆扭转角、关节偏距和关节转角。六组D-H参数统一存放于机械臂模型函数中，使机械尺寸和后续算法实现相互分离。

在给定关节变量后，程序依次计算从基坐标系到第一个关节坐标系、第二个关节坐标系直至末端坐标系的变换关系。通过逐级相乘，可以得到全部关节点在基坐标系中的位置以及末端执行器的方向。

这种建模方式结构清晰，便于修改机械臂尺寸，同时能够直接服务于机械臂绘图、质心位置计算、雅可比矩阵建立和工作空间采样等后续模块。

3.2 几何雅可比矩阵

机械臂雅可比矩阵用于描述关节速度与末端线速度、角速度之间的局部映射关系，也是本文逆运动学算法的重要基础。程序根据各关节旋转轴方向、关节原点以及末端位置构造六维几何雅可比矩阵。

需要注意的是，雅可比矩阵中平移部分与旋转部分具有不同物理量纲。如果直接将原始矩阵用于条件数计算，所得结果容易受到单位选取影响。因此，在性能评价过程中，本文根据机械臂总特征长度对平移部分进行无量纲化处理，再计算最小奇异值和条件数，从而使奇异性评价更具可比性。Haviland和Corke近年来对机械臂微分运动学的系统整理也说明，雅可比矩阵不仅用于速度映射，也是逆运动学及机械臂性能评价中的核心数学工具。

3.3 阻尼最小二乘逆运动学

普通雅可比逆或者伪逆方法在机械臂接近奇异位形时可能出现很大的关节增量。为提高数值稳定性，本文采用阻尼最小二乘方法进行迭代求解。

在每次迭代过程中，算法首先根据当前关节变量进行正运动学计算，获得当前末端位置和姿态，然后计算其与目标位姿之间的误差。位置误差和姿态误差经过适当权重处理后共同进入逆解过程。

阻尼系数不是完全固定，而是根据当前加权雅可比矩阵的最小奇异值进行调节。当机械臂运动状态相对良好时保持较小阻尼，以保证逆解速度；当最小奇异值下降时逐渐提高阻尼，以减弱接近奇异状态时关节变量过度变化。

同时，每一次计算得到的关节增量均设置最大转角限制。程序还加入回溯搜索机制，不直接接受所有计算结果，而是从完整更新步长开始逐级缩小试探步长，仅在新的关节构型能够降低综合位姿误差时接受该次更新。如果连续缩小步长仍无法改善误差，则停止当前迭代。

此外，每次更新后的关节角均限制在机械臂允许运动范围以内。这使逆运动学求解不仅考虑位姿误差，还避免数值迭代产生明显超出机械结构范围的无效解。现代逆运动学研究仍大量关注奇异性、约束以及关节运动效率等问题，说明单纯追求目标位姿误差最小并不足以覆盖实际机械臂逆解要求。

四、分段五次关节轨迹规划

完成逆运动学以后可以得到四组离散关节路径点，但机械臂不能直接在这些离散角度之间跳变，因此需要进一步生成连续时间轨迹。

本文采用分段五次多项式时间标度方法，在相邻关节路径点之间分别规划运动过程。四个关键路径点对应时间分别为0 s、2.5 s、5.0 s和7.5 s，总运动时间为7.5 s，正常仿真步长设置为0.01 s。

每一段运动均按照起点和终点速度为零、加速度为零的方式生成，因此机械臂到达各关键路径点时会形成短暂停稳状态。五次时间标度能够同时保证位置、速度和加速度连续，结构简单且无需复杂在线优化，适合作为验证控制算法的基础参考轨迹。

程序分别输出参考关节角、参考角速度和参考角加速度，使后续控制器不仅能够根据位置误差进行闭环调节，还能够利用目标加速度进行动力学前馈。

轨迹规划研究的发展表明，机械臂参考信号的连续性会直接影响跟踪性能和机械冲击，因此高阶平滑轨迹仍具有重要工程价值。针对方向轨迹、柔性机械臂和多目标轨迹优化的近期工作也持续关注速度、加速度、冲击及结构振动等问题。

五、机械臂动力学建模及前馈PD控制

5.1 动力学参数建立

本文不仅建立机械臂几何模型，还为六个连杆设置质量、质心位置和转动惯量参数。根据连杆长度及简化几何形状估计惯量张量，同时加入电机转子的等效惯量。

在给定关节状态后，程序计算各连杆质心的线速度雅可比和角速度雅可比，并将各连杆平动和转动部分对应的惯性贡献进行累加，由此得到关节空间质量矩阵。

重力项根据各连杆质心受到的重力作用映射到关节空间获得。科氏力与离心力部分则利用质量矩阵对各关节变量的中心差分结果构造，使整个动力学计算过程能够直接由基础MATLAB函数完成。

模型同时设置各关节黏性摩擦系数和最大允许驱动力矩，因此仿真对象相对于单纯运动学模型更加完整。

5.2 动力学前馈与PD反馈控制

本文采用动力学前馈和PD反馈相结合的控制结构。控制器根据参考关节加速度、关节位置误差和速度误差形成期望加速度，再利用标称质量矩阵、科氏项、重力项和摩擦项计算控制输入。

与理想动力学完全抵消不同，本文有意使控制器采用的标称模型与实际对象存在一定偏差。其中标称质量、转动惯量、转子惯量和黏性摩擦参数均按照实际模型的一定比例缩放。这种设置能够避免控制器和实际对象完全相同后产生过于理想化的跟踪结果，更接近参数辨识存在误差时的情况。

机械臂初始时刻同样设置一定关节角偏差，用于观察闭环系统能否逐渐恢复到参考轨迹附近。

在3.90 s至4.35 s时间范围内，程序对第二、第三和第四关节施加不同幅度的平滑外部扰动力矩。不同关节的扰动幅值并不完全相同，而是考虑机械臂各关节负载能力和等效惯量差异进行设置。该处理能够检验机械臂在运动过程中受到瞬态外部扰动时的恢复能力。

所有关节控制力矩最终均经过饱和限制。如果力矩超过对应关节上限，程序会限制到允许范围，避免数值模型产生不符合执行机构能力的无限控制输入。

六、仿真结果与性能分析

6.1 逆运动学收敛结果

四个目标位姿均由合法关节构型对应的正运动学结果生成。经过阻尼最小二乘逆解后，全部目标位姿均能够达到程序设定的收敛条件。

仿真统计显示，逆运动学最大位置残差为2.109627×10^-9 m，最大姿态残差为1.909310×10^-9 rad。该结果说明在本文给出的四组任务位姿和初始条件下，数值逆运动学能够实现较高精度的位姿逼近。

需要指出的是，这一结果证明的是本文任务点范围内算法的数值收敛能力，并不能据此认为任意空间位姿都存在唯一可行逆解。对于机械臂不可达区域、不同初值以及更接近奇异区域的构型，逆解结果仍可能发生变化。

6.2 关节轨迹跟踪结果

完整运动过程中六个关节总体均方根误差为0.433180°。其中第一至第六关节RMSE分别为0.180556°、0.601036°、0.688716°、0.303532°、0.165054°和0.371917°。

第三关节误差相对较大，其最大绝对误差达到2.774425°，但总体误差仍处于有限范围内。第二关节RMSE为0.601036°，说明承载能力和运动耦合较明显的中部关节对整个系统跟踪误差贡献较高。

从关节角曲线可以观察到，实际运动能够整体跟随五次参考轨迹变化，并在任务路径点附近逐渐回到稳定状态。即使在外扰时间段内出现局部偏差，闭环反馈仍能够继续对误差进行修正。

6.3 末端执行器跟踪结果

末端位置总体RMSE为5.614945 mm，最大位置误差为11.594475 mm；末端姿态RMSE为1.003337°，最大姿态误差为2.416622°。

期望末端路径长度为0.875546 m，实际路径长度为0.877266 m，两者整体较为接近。这表明关节误差经过机械臂运动学映射以后虽然形成一定末端位置和姿态偏差，但实际末端轨迹的整体运动趋势与规划轨迹一致。

由于本文控制器采用有偏标称动力学参数，并加入初始状态误差及外部扰动，因此所得结果并非完全理想模型下的零误差跟踪结果。这种设计更适合观察闭环控制器对模型失配的实际调节过程。

6.4 驱动力矩及约束分析

仿真过程中最大控制力矩为44.192922 N·m，所有关节均未达到设定力矩饱和阈值，力矩饱和样本占比为0%。

系统计算得到的绝对机械功指标为40.852187 J，可用于对当前轨迹和控制策略的总体执行代价进行辅助评价。

关节约束方面，整个运动轨迹的最小关节限位余量为15.653%。由此可见，本次规划过程没有直接触发关节限位，但距离部分关节边界也并非十分宽裕。在进一步扩大工作空间或调整任务路径时，仍需要持续考虑关节限位问题。

6.5 奇异性及数值稳定性分析

在对雅可比矩阵平移部分进行特征长度归一化以后，轨迹中的最小奇异值为0.085812，最大条件数为21.111015。该结果说明本文轨迹没有进入严重奇异状态，但不能据此认为机械臂始终处于远离奇异位形的区域。

模型自检结果显示，质量矩阵最大相对非对称误差为3.476488×10^-19，最小特征值为0.01571092 kg·m²；重力梯度最大差分误差为4.301766×10^-9 N·m，位置雅可比最大差分误差为6.619044×10^-11 m/rad，科氏项功率一致性归一化误差为3.630882×10^-12。

程序还专门构造180°相对姿态测试，所得旋转对数向量模长为3.141592653590 rad，说明姿态误差计算能够正确处理接近180°的特殊情况。

为了进一步判断固定步长是否明显影响结果，将仿真步长由0.01 s减半至0.005 s，并重新计算关节RMSE、末端位置RMSE、末端姿态RMSE和最大控制力矩。四项指标中最大变化为0.097021%，低于程序设置的2%判定阈值。因此，对于本文模型和运动工况，0.01 s步长没有造成明显的离散化结果失真。

七、系统特点与优势

本文建立的六自由度机械臂仿真系统具有以下几个特点。

第一，系统结构完整。仿真并非只完成机械臂绘图或者单独进行轨迹插值，而是覆盖D-H运动学建模、正运动学、逆运动学、轨迹规划、动力学建模、闭环控制、性能评价和结果可视化等多个环节，各模块之间的数据关系较为明确。

第二，逆运动学数值稳定性较强。算法在普通阻尼最小二乘基础上加入奇异性相关阻尼调节、单步角度限制、关节限位和回溯搜索，能够降低数值迭代过程中突然产生大幅关节变化的风险。

第三，轨迹具有较好的连续性。关节路径点之间采用分段五次轨迹，每段首尾速度和加速度均为零，因此能够避免使用简单线性插值时出现明显速度突变和加速度突变。

第四，控制实验具有一定非理想条件。控制器没有直接使用与实际对象完全一致的动力学模型，而是主动引入质量、惯量和摩擦参数偏差，同时增加初始状态误差和短时外扰。这种方式能够更加直观地观察闭环反馈在模型失配条件下的作用。

第五，性能评价维度较完整。除常用关节误差和末端位置误差外，系统还分析末端姿态误差、驱动力矩、工作空间、雅可比最小奇异值、条件数、可操作度和关节限位余量，能够从精度、控制输入和运动安全余量多个角度分析机械臂运动状态。

第六，仿真结果具有可重复性。主程序固定随机种子，工作空间随机采样能够保持一致。输出结果保存为PNG、GIF、CSV、MAT和文本文件，便于后续进一步分析。

第七，程序设置了模型自检和步长收敛检查。质量矩阵、重力项、雅可比和科氏项并非只依赖程序内部计算结果，而是通过独立数值差分进行交叉验证，并进一步通过减小离散步长观察核心指标变化，有利于降低单纯依据曲线形状判断算法有效性的风险。

八、结论

本文针对六自由度串联机械臂建立了一套基于MATLAB R2024a的运动学、轨迹规划及动力学闭环控制仿真系统。首先采用标准D-H方法建立六关节机械臂模型，并通过几何雅可比矩阵描述关节空间和末端任务空间之间的局部运动关系。针对一般数值逆解在奇异区域可能出现不稳定的问题，采用带自适应阻尼、关节限位和回溯搜索的阻尼最小二乘方法求解四组任务位姿。

在逆运动学结果基础上，利用分段五次轨迹连接各关节路径点，并获得连续的参考关节位置、速度和加速度。随后综合考虑机械臂质量、质心、惯量、重力和摩擦等因素建立动力学模型，并设计动力学前馈PD控制器完成轨迹跟踪。通过在控制器标称模型中引入参数偏差、设置初始关节偏差以及加入短时外部扰动，对控制系统进行非理想条件测试。

最终仿真得到关节角总体RMSE为0.433180°，末端位置RMSE为5.614945 mm，末端姿态RMSE为1.003337°，最大驱动力矩为44.192922 N·m。全过程未发生力矩饱和，最小关节限位余量为15.653%。步长减半后核心指标最大变化仅为0.097021%，模型数值自检全部通过。

综合结果表明，本文所建立的系统能够较稳定地实现六自由度机械臂从任务位姿求解、连续轨迹生成到非线性动力学跟踪控制的完整仿真流程，同时能够通过多维性能指标对算法结果进行定量评价，为机械臂运动学分析、轨迹规划及控制算法验证提供一种较完整的数字仿真方法。

参考文献

Haviland, Jesse, and Peter Corke. “Manipulator Differential Kinematics: Part I: Kinematics, Velocity, and Applications.” IEEE Robotics & Automation Magazine, 2023, pp. 2–12. doi:10.1109/MRA.2023.3270228.
Haviland, Jesse, and Peter Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv, 2022, arXiv:2207.01794.
Corke, Peter, and Jesse Haviland. “Not Your Grandmother’s Toolbox—The Robotics Toolbox Reinvented for Python.” 2021 IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2021, pp. 11357–11363. doi:10.1109/ICRA48506.2021.9561366.
Haviland, Jesse, and Peter Corke. “NEO: A Novel Expeditious Optimisation Algorithm for Reactive Motion Control of Manipulators.” IEEE Robotics and Automation Letters, vol. 6, no. 2, 2021, pp. 1043–1050. doi:10.1109/LRA.2021.3056060.
Singletary, Andrew, et al. “Online Active Safety for Robotic Manipulators.” 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE, 2019, pp. 173–178. doi:10.1109/IROS40897.2019.8968231.
Yonezawa, Ansei, Heisei Yonezawa, and Itsuro Kajiwara. “Simple Inverse Kinematics Computation Considering Joint Motion Efficiency.” IEEE Transactions on Cybernetics, vol. 54, no. 9, 2024, pp. 4903–4914. doi:10.1109/TCYB.2024.3372989.
Quan, Yuan, et al. “The Dexterity Capability Map for a Seven-Degree-of-Freedom Manipulator.” Machines, vol. 10, no. 11, 2022, article 1038. doi:10.3390/machines10111038.
Li, Xiao, Hailin Lv, Detian Zeng, and Qi Zhang. “An Improved Multi-Objective Trajectory Planning Algorithm for Kiwifruit Harvesting Manipulator.” IEEE Access, vol. 11, 2023, pp. 65689–65699. doi:10.1109/ACCESS.2023.3289207.
Li, Yuanyuan, Shuzhi Sam Ge, Qingping Wei, Tian Gan, and Xiaolin Tao. “An Online Trajectory Planning Method of a Flexible-Link Manipulator Aiming at Vibration Suppression.” IEEE Access, vol. 8, 2020, pp. 130616–130632. doi:10.1109/ACCESS.2020.3009526.
Tagliavini, Andrea, and Corrado Guarino Lo Bianco. “A Smooth Orientation Planner for Trajectories in the Cartesian Space.” IEEE Robotics and Automation Letters, vol. 8, no. 5, 2023, pp. 2606–2613. doi:10.1109/LRA.2023.3256921.