【MATLAB源码-第431期】基于MATLAB的六自由度机械臂正逆运动学建模、阻尼逆解与多目标平滑轨迹仿真。

操作环境：

MATLAB 2024a

1 、算法描述

摘要

移动底盘机械臂将移动平台的空间覆盖能力与多自由度机械臂的操作能力结合起来，在智能制造、物流搬运、巡检维护和柔性装配等场景中具有较强应用价值。针对移动底盘六自由度机械臂的建模、求解与轨迹生成问题，本文建立了一种基于MATLAB环境的仿真方法。系统首先依据六关节串联机械臂的结构特点设置连杆参数、关节限位和底盘高度，并利用齐次变换描述各连杆坐标系之间的位置与姿态关系。在此基础上，系统通过正运动学计算由关节空间到末端空间的映射关系，并以多组目标位姿作为任务点，采用阻尼最小二乘逆运动学方法求解对应关节角。为了提高求解稳定性，逆运动学迭代过程中引入数值雅可比矩阵、位置误差、姿态误差和关节限位约束，避免普通伪逆方法在接近奇异构型时出现过大的关节修正量。完成多目标关节角求解后，系统在关节空间内采用五次多项式轨迹规划方法生成连续运动过程，使关节位移、速度和加速度能够保持较好的平滑性。仿真结果通过机械臂三维模型、正逆运动学位姿对比、末端位置曲线、关节位移曲线、关节速度曲线、关节加速度曲线、逆运动学误差收敛曲线以及运动过程动图进行展示。整体结果表明，该方法能够较完整地描述移动底盘六自由度机械臂从建模、位姿求解到轨迹生成的基本流程，并为后续加入动力学控制、避障规划和真实机器人控制接口提供基础。

关键词：移动底盘机械臂；六自由度机械臂；正运动学；逆运动学；阻尼最小二乘；轨迹规划；MATLAB仿真

一、引言

机器人技术的发展正在从单一固定工位操作向移动化、协作化和柔性化方向延伸。传统工业机械臂通常安装在固定基座上，适合执行重复性强、工作空间相对稳定的生产任务。移动底盘机械臂在此基础上增加了底盘平台，能够扩大作业范围，并适应更加复杂的空间环境。对于工程应用来说，移动底盘机械臂的核心问题不只是让机械结构"动起来"，而是要保证末端执行器能够按照给定任务到达指定位置和姿态，同时使关节运动过程稳定、连续、可观察、可验证。如果只关注运动结果而忽略关节限位、奇异构型、速度突变和加速度突变，仿真结果可能表面可行，但实际控制中会暴露出较大的工程风险。

六自由度串联机械臂是机械臂研究和应用中较典型的结构形式。六个转动关节理论上可以满足末端执行器在三维空间中的位置和姿态控制要求，因此常用于搬运、装配、焊接、喷涂和抓取等任务。对于这类机械臂，正运动学用于根据各关节角计算末端位姿，逆运动学用于根据目标位姿反求关节角。二者是轨迹规划和运动控制的基础。正运动学通常具有明确的计算流程，而逆运动学往往更复杂。原因在于机械臂结构存在非线性耦合，同一个末端位姿可能对应多组关节解，也可能因为目标点超出工作空间而不存在可行解。当机械臂接近奇异构型时，普通雅可比伪逆方法还可能产生较大的关节速度或迭代抖动。因此，在教学仿真和工程原型设计阶段，采用具有一定鲁棒性的数值逆解方法具有现实意义。

本文围绕移动底盘六自由度机械臂开展仿真研究。系统以MATLAB作为实现平台，建立兼容不同机器人建模后端的仿真框架。当环境中存在Peter Corke Robotics Toolbox时，系统优先使用该工具箱完成机械臂模型绘制和轨迹函数调用；当环境中不存在该工具箱但存在MATLAB Robotics System Toolbox时，系统可以切换到rigidBodyTree后端，并用自定义绘图函数实现简化机械臂显示。这种处理方式提高了工程代码的可迁移性，也避免了仿真强依赖单一工具箱。本文重点不在于建立真实底盘的轮式运动模型，而是在固定底盘高度条件下研究六自由度机械臂的正逆运动学、关节空间轨迹规划和可视化验证流程。该边界必须明确，否则容易把移动底盘机械臂误解为同时包含底盘导航、全局路径规划和机械臂操作控制的完整系统。本文所实现的是机械臂本体运动学层面的仿真框架，底盘部分主要体现为基座高度和空间安装平台。

二、系统建模方法

机械臂运动学建模的首要任务是建立各连杆之间的坐标变换关系。本文根据六自由度串联机械臂的结构特点设置六组连杆参数，包括连杆长度、连杆扭角、连杆偏距和关节偏置。底盘部分以固定高度的基座变换表示，使机械臂安装点相对于地面具有一定高度。该处理方式虽然没有描述底盘的平面移动过程，但能够反映机械臂安装在移动平台上的空间关系，适合用于机械臂本体运动学验证。

系统通过齐次变换矩阵描述相邻坐标系之间的相对关系。每一个关节角输入后，都可以得到当前关节对应的局部变换。将六个关节的局部变换依次累乘，就能得到末端执行器相对于基座坐标系的整体位姿。该过程构成正运动学求解。正运动学的特点是输入明确、结果唯一，只要机械臂参数和关节角确定，末端位置和姿态即可直接计算。因此，本文先利用初始关节角计算初始末端位姿，再利用多组参考关节角生成多个目标末端位姿，为后续逆运动学求解提供目标。

为了提高程序结构的清晰度，系统将主流程与本地函数分开。主函数负责工程初始化、参数设置、多目标求解、轨迹拼接和图像输出；函数文件夹负责工具箱检测、机器人建模、正运动学计算、逆运动学求解、数值雅可比计算、轨迹规划、坐标变换、绘图和图像保存等具体功能。这种结构的优点是主流程较容易阅读，后续修改某一个算法模块时不需要改动整段程序。例如，如果后续需要把阻尼最小二乘逆解替换为粒子群优化、遗传算法或深度学习逆解，只需要重点调整逆运动学求解函数，而不必重写图像输出和轨迹规划部分。

关节限位是机械臂建模中容易被忽略但非常重要的约束。理想数学模型可以给出任意角度，但真实机械臂受机械结构、电缆布置、减速器范围和安全策略限制，不可能无限转动。本文为每个关节设置角度范围，并在逆运动学迭代过程中对关节角进行限幅处理。该处理虽然不能完全等价于真实控制器中的速度、加速度和力矩约束，但可以避免数值迭代得到明显不合理的关节角，从而提高仿真结果的工程合理性。

三、正逆运动学求解策略

正运动学求解是本文系统的基础环节。系统根据当前关节角依次计算连杆坐标变换，并提取末端执行器的位置向量和姿态矩阵。对于多目标任务，系统先通过参考关节角生成目标位姿，再要求逆运动学算法从上一目标点对应的关节构型出发，迭代求解下一个目标位姿。这种连续求解策略比每次都从固定初值开始搜索更合理，因为实际机械臂在多段任务中通常也是从当前位姿移动到下一位姿，而不是每次回到初始姿态。

逆运动学部分采用阻尼最小二乘方法。该方法本质上属于基于雅可比矩阵的数值迭代方法。系统在每一次迭代中先计算当前末端位姿，再计算当前位姿与目标位姿之间的位置误差和姿态误差，然后通过数值雅可比矩阵建立末端误差与关节修正量之间的近似关系。与普通伪逆方法相比，阻尼项能够降低雅可比矩阵病态时的放大效应，使关节修正量不至于过大。这一点对六自由度机械臂很关键，因为机械臂在伸展、折叠或腕部对齐时可能接近奇异状态。如果不加阻尼，算法可能在误差下降过程中出现关节角大幅跳变，甚至导致轨迹不连续。

本文使用的位置误差由目标末端位置与当前末端位置之间的差值表示，姿态误差由当前坐标轴与目标坐标轴之间的方向差异构成。两类误差组合后形成六维误差向量，既包含空间位置偏差，也包含姿态方向偏差。系统设置误差收敛阈值和最大迭代次数，当误差低于阈值时停止迭代；如果达到最大迭代次数仍未满足阈值，则保留最后一次迭代结果并记录状态。该处理方式比盲目迭代更稳妥，也便于通过误差收敛曲线检查算法是否可靠。

数值雅可比矩阵的优点是实现简单，对机械臂解析模型依赖较小。系统通过对每个关节角施加微小扰动，观察末端位置和姿态变化，从而近似获得该关节对末端位姿的影响。对于教学仿真和一般工程验证来说，这种方法具有较好的通用性。它的不足是计算量高于解析雅可比，并且数值微分步长选择会影响精度和稳定性。本文采用较小扰动步长，以兼顾近似精度和数值稳定性。严格来说，如果后续需要部署到实时控制系统，仍建议进一步推导解析雅可比，或使用经过验证的机器人控制库，以减少实时计算压力。

四、轨迹规划与运动连续性设计

完成目标关节角求解后，系统需要把离散目标点连接成连续运动过程。若机械臂直接从一个关节角跳到另一个关节角，仿真图像虽然可以显示目标位姿变化，但这种运动不具备真实可执行性。真实机械臂的关节电机需要按照连续的位移、速度和加速度变化运行，否则会引起冲击、振动、跟踪误差和结构磨损。因此，轨迹规划是连接逆运动学结果与运动控制的关键环节。

本文在关节空间中采用五次多项式轨迹规划方法。五次多项式轨迹能够在起点和终点处同时约束位置、速度和加速度，使每一段关节运动具有较平滑的起止过程。对于多目标任务，系统把初始关节角到第一个目标关节角作为第一段轨迹，再依次把上一目标关节角到下一目标关节角作为后续轨迹。拼接时去除重复端点，避免时间轴和轨迹数据出现重复采样。最终得到完整的关节位移、速度和加速度序列。

关节空间轨迹规划的优势是实现直接，能够保证每个关节的运动曲线可控，也便于检查是否存在关节突变。其不足是末端空间轨迹不一定严格沿某条预设几何曲线运动。如果任务要求末端执行器沿直线、圆弧或复杂曲面运行，则需要在笛卡尔空间进行路径插值，再对每个插值点求逆运动学。本文任务更强调多目标点之间的平滑过渡，因此采用关节空间规划是合理的。该选择降低了实现复杂度，也符合机械臂点到点运动的常见仿真流程。

系统还输出关节速度曲线和关节加速度曲线。速度曲线用于观察各关节在运动中是否存在突变，加速度曲线用于观察轨迹平滑性和潜在冲击风险。若某个关节速度或加速度明显异常，说明目标点设置、逆解选择或运动时间参数可能需要重新调整。本文没有进一步引入力矩约束和电机动力学模型，因此加速度曲线只能作为运动平滑性的运动学指标，不能直接等价为电机实际负载。这个边界需要明确，否则容易把运动学仿真误认为动力学仿真。

五、仿真结果分析

系统输出的三维模型图展示了移动底盘、六自由度机械臂和末端轨迹之间的空间关系。底盘被简化为固定支撑平台，机械臂末端轨迹以曲线形式显示，起点和终点通过不同标记区分。该图的价值在于快速判断机械臂的整体工作空间、末端运动范围和轨迹是否存在明显异常。对于工程说明来说，三维模型图比单纯的数字矩阵更直观，也更适合向非算法背景人员解释系统实现内容。

正逆运动学位姿对比图用于检查逆运动学求解是否能够达到由参考关节角生成的目标位姿。图中同时显示初始位姿、多个逆解位姿和目标点位置。若逆解结果与目标位姿明显偏离，则说明逆运动学误差未收敛，或者目标点超出当前机械臂构型可达范围。本文通过记录每个目标点的误差序列，并输出逆运动学误差曲线，使收敛过程可以被直接观察。这种做法比只看最终姿态图更可靠，因为最终图像可能因为视角和尺度原因掩盖小误差，而误差曲线能够反映迭代过程是否稳定。

末端位置曲线展示了末端执行器在全任务时间内的空间坐标变化。由于系统采用多段关节空间轨迹拼接，末端位置通常表现为连续曲线，而不是突变折线。该曲线可以用于判断末端运动是否存在明显跳变，也可以辅助分析不同目标点之间的运动趋势。关节位移曲线展示六个关节角随时间的变化情况，是检查关节运动范围和连续性的主要依据。若关节位移曲线在段与段拼接处不连续，则说明轨迹拼接或逆解选择存在问题。本文通过连续使用上一目标解作为下一段初值，有助于减少逆解分支跳变带来的不连续问题。

关节速度曲线和加速度曲线进一步验证了轨迹规划的平滑性。五次多项式轨迹的优势在于起止阶段速度和加速度变化较平缓，适合点到点运动。对于机械臂系统而言，速度突变会增加控制跟踪难度，加速度突变则可能引发振动和冲击。本文通过速度、加速度曲线把这些风险显性化。逆运动学误差曲线则从算法角度评价各目标点的求解质量。若误差曲线能够随着迭代次数下降并接近设定阈值，说明阻尼最小二乘方法在当前目标设置下能够获得有效解。若误差曲线下降缓慢或停止在较大误差水平，则需要检查目标位姿、关节限位、阻尼系数和初始值选择。

运动过程动图补充了静态图像难以表达的连续过程。通过动图可以观察机械臂从起点依次经过多个目标点的整体姿态变化，也可以检查运动过程中是否出现明显穿模、姿态突变或视觉不合理现象。动图不能替代数值指标，但它对于发现直观问题非常有效。综合各图可知，本文系统已经形成了从模型建立、目标位姿生成、逆运动学求解、轨迹规划到结果可视化的完整闭环。需要注意的是，本文结果主要证明运动学仿真流程成立，并不代表真实机器人在相同轨迹下必然满足电机力矩、碰撞安全和控制稳定性要求。

六、工程价值与不足

本文方法的工程价值主要体现在三个方面。第一，系统结构清晰，主函数和功能函数分工明确，便于后续维护和扩展。第二，逆运动学求解考虑了阻尼项和关节限位，比简单伪逆迭代更稳健。第三，结果展示覆盖模型、位姿、末端轨迹、关节运动和误差收敛，能够从多个角度验证仿真是否合理。这些特点使该系统适合作为移动底盘机械臂运动学仿真的基础版本，也适合作为课程设计、毕业设计或后续算法研究的起点。

本文也存在明确不足。首先，底盘运动没有被真正建模，系统没有考虑轮式底盘的平面位姿、运动约束、定位误差和路径规划问题。其次，机械臂只进行了运动学层面的分析，没有建立动力学模型，也没有考虑关节力矩、电机饱和、负载变化和控制器跟踪误差。再次，轨迹规划采用关节空间点到点方式，末端路径没有被约束为特定几何形状，因此不适合直接用于要求末端严格沿直线或曲面运动的加工任务。最后，逆运动学采用数值雅可比，通用性较好，但实时性和精度仍可能受步长、阻尼系数和初始值影响。以上不足不是程序错误，而是当前模型边界。只有把边界讲清楚，仿真结果才不会被过度解释。

后续研究可以从三个方向扩展。第一，可以引入底盘平面运动模型，将底盘位置和机械臂关节统一到移动操作机器人的整体运动学框架中。第二，可以加入障碍物、工作空间约束和碰撞检测，使轨迹不只满足关节连续性，还能满足环境安全性。第三，可以进一步加入动力学模型和控制器模型，分析关节力矩、跟踪误差和外部扰动对运动精度的影响。如果面向真实机器人部署，还需要增加传感器反馈、坐标标定、控制周期、通信延迟和安全停机机制。

七、结论

本文完成了移动底盘六自由度机械臂的正逆运动学建模与多目标轨迹仿真研究。系统通过连杆参数和固定底盘高度建立机械臂模型，利用正运动学计算末端位姿，并采用阻尼最小二乘方法求解多目标逆运动学问题。在轨迹生成阶段，系统通过五次多项式方法完成关节空间平滑规划，使机械臂能够在多个目标点之间连续运动。通过三维模型图、位姿对比图、末端位置曲线、关节位移曲线、关节速度曲线、关节加速度曲线、逆运动学误差曲线和运动动图，系统从空间结构、运动过程和算法收敛三个层面对结果进行了验证。

总体来看，该仿真框架能够较好地支撑六自由度机械臂运动学分析和基础轨迹规划研究。它的结论应限定在运动学仿真层面，不能直接等同于真实机械臂的动力学控制效果。若后续继续加入底盘导航、碰撞检测、动力学约束和闭环控制，该系统可以进一步扩展为更完整的移动操作机器人仿真平台。

参考文献

Agustian, Indra, Novalio Daratha, Ruvita Faurina, Agus Suandi, and Sulistyaningsih. "Robot Manipulator Control with Inverse Kinematics PD-Pseudoinverse Jacobian and Forward Kinematics Denavit Hartenberg." arXiv, 2021.

Ahmed, Abubaker, Meng Yu, and Feifei Chen. "Inverse Kinematic Solution of 6-DOF Robot-Arm Based on Dual Quaternions and Axis Invariant Methods." Arabian Journal for Science and Engineering, vol. 47, 2022, pp. 15915--15930.

Analooee, Ali, Reza Kazemi, and Shahram Azadi. "SCR-Normalize: A Novel Trajectory Planning Method Based on Explicit Quintic Polynomial Curves." Proceedings of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering, vol. 234, no. 4, 2020.

Ibarra-Pérez, Teodoro, José Manuel Ortiz-Rodríguez, Fernando Olivera-Domingo, Héctor A. Guerrero-Osuna, Hamurabi Gamboa-Rosales, and Ma. del Rosario Martínez-Blanco. "A Novel Inverse Kinematic Solution of a Six-DOF Robot Using Neural Networks Based on the Taguchi Optimization Technique." Applied Sciences, vol. 12, no. 19, 2022, article 9512.

Kuo, Yong-Lin, Chun-Chen Lin, and Zheng-Ting Lin. "Dual-Optimization Trajectory Planning Based on Parametric Curves for a Robot Manipulator." International Journal of Advanced Robotic Systems, 2020.

Lu, Song, Bingxiao Ding, and Yangmin Li. "Minimum-Jerk Trajectory Planning Pertaining to a Translational 3-Degree-of-Freedom Parallel Manipulator through Piecewise Quintic Polynomials Interpolation." Advances in Mechanical Engineering, 2020.

Luo, Sha, Dianming Chu, Qingdang Li, and Yan He. "Inverse Kinematics Solution of 6-DOF Manipulator Based on Multi-Objective Full-Parameter Optimization PSO Algorithm." Frontiers in Neurorobotics, vol. 16, 2022, article 791796.

Villalobos, Jessica, Irma Y. Sanchez, and Fernando Martell. "Singularity Analysis and Complete Methods to Compute the Inverse Kinematics for a 6-DOF UR/TM-Type Robot." Robotics, vol. 11, no. 6, 2022, article 137.

Wang, Xiaoqi, Xing Liu, Lerui Chen, and Heyu Hu. "Deep-Learning Damped Least Squares Method for Inverse Kinematics of Redundant Robots." Measurement, vol. 171, 2021, article 108821.

Wu, Guanglei, Wenkang Zhao, and Xuping Zhang. "Optimum Time-Energy-Jerk Trajectory Planning for Serial Robotic Manipulators by Reparameterized Quintic NURBS Curves." Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, 2021.