[TOC]

“在你的专业领域里，你要像一个坚韧的农夫一样，一遍又一遍，向深处耕耘”

已购课程：

【史上最强机械臂教程】机械臂运动学、轨迹规划和动力学控制仿真-AC今夜有雨（付费课程）语雀 https://www.yuque.com/dashboard/books#collab

Matlab机器人工具箱入门指南 • 汪迪 https://class.guyuehome.com/p/t_pc/course_pc_detail/column/p_615fb0e4e4b0dfaf7faa9725

学习计划

1.数理基础和编程：

线性代数：极其重要，是理解和表示向量、空间变换、矩阵运算等的基础。

Python：具身智能研究和开发中最主流的编程语言，拥有丰富的库和社区支持。

Matlab机器人工具箱: 主要用于验证机械臂的算法，对于从零开始学习机器人的运动学和动力学小白来说极易上手，里面有基本的机器人运动学和动力学函数，可通过查看底层函数，加强理论知识的学习，还可以在此基础上快速开发自己的算法。

ROS+Moveit+OMPL:OMPL集成了最先进的运动规划、操作、3D感知、运动学、控制与导航算法。再结合Rviz和Gazebo等工具，就十分完美了。

Linux:如果完全没有Linux开发经验，可以先安装Ubuntu系统，然后看UNIX Tutorial for Beginners，熟悉基本的Linux使用方法
C++: 在对性能要求极高的场景（如底层机器人控制、实时系统）时会用到，且ROS的主要代码都是C++实现的，学C++的时候就可以在Ubuntu下进行，安装一个vscode。
对于ROS最好的教程就是官网的教程ROS Tutorials。首先通过Beginner Level和Intermediate Level了解ROS基本的通讯机制，学会使用catkin,roslaunch,Rviz等基本工具。
理论上在ROS环境下可以从事绝大多数与实时控制无关的研究，如Motion Planning等。若从事的是更加底层的工作，（如控制器设计），目前ROS还无法胜任。
【机器人操作系统 ROS 快速入门教程】 https://www.bilibili.com/video/BV1BP4y1o7pw/?share_source=copy_web&vd_source=4eaa4760820b4cda603e510d556578ac
古月居21讲（归纳性较强），深蓝学院moveit
赵虚左ROS（强推！）【【Autolabor初级教程】ROS机器人入门】 https://www.bilibili.com/video/BV1Ci4y1L7ZZ/?share_source=copy_web&vd_source=4eaa4760820b4cda603e510d556578ac

2.机器人理论：

目标：扎扎实实打好空间变换、DH建模、正逆运动学、轨迹规划、雅可比矩阵等动力学（补理论力学）的基础。

行动：重新过一遍理论知识，用一个专门的本子做笔记、推导，对于逆运动学要自己手算一遍，不能只会调库。

1)齐次变换：用一个4x4的齐次变换矩阵来描述两个坐标系之间的关系，

熟悉坐标表示方式：坐标系{B}在{A}下的位姿如何表示。
左乘和右乘的区别。
了解旋转矩阵每一列的含义，学会如何通过目测写出两个坐标系之间的旋转矩阵。
姿态的表示方式：RPY角，各种欧拉角、轴角表示。

2)运动学：

为了方便计算两个连杆之间的相对位姿，需要学习DH建模方法

建立坐标系
计算DH参数
计算变换矩阵
正解：知道每个连杆两两之间的坐标变换，就可以通过矩阵乘法计算出最后的末端位姿了。
逆解：闭式解法（解析）、数值解法（如Jacobian迭代）。通过不断调整上面几个矩阵的位置，尝试找到可以单独求解的未知数即可。虽然有些繁琐，但是对于初学者来说一定要亲手推一推六轴机械臂的运动学逆解公式，并编程实现。重点是解析解（特殊构型的封闭解析解，解析解需要满足Pieper准则，否则只能用数值解）

3)轨迹规划：

用MATLAB实现直线，圆弧，梯形，三次五次多项式，SLERP，SQUAD，NURBS。

任务空间（笛卡尔空间）控制更贴近目标

4)雅可比计算：

线速度/角速度与关节速度之间的映射关系
用于控制与奇异性分析

5)动力学建模：

Lagrangian（拉格朗日）法：基于能量
Newton-Euler法：基于力和加速度
获取运动方程：M(q)q¨+C(q,q˙)q˙+G(q)=τM(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = \tau
了解阻抗控制、导纳控制、参数辨识

学习资源

了解一个领域的常见技巧

谷歌学术搜关键词：找到survey，或者引用数比较高的论文，然后用ChatPaper总结，或者学术版GPT免费翻译，快速阅读。
如果是最新的论文：Arxiv搜关键词。
看paperwithcode的排行榜，比如：https://paperswithcode.com/sota
如果是代码复现，可以Github搜：awesome+xxx,一般会有大佬给你整理好相关的资料。

专栏

Robotics https://blog.csdn.net/jldemanman/category_7433626.html

机械臂速成小指南 https://blog.csdn.net/m0_53966219/category_11761138.html

【Matlab 六自由度机器人】系列文章汇总https://blog.csdn.net/AlbertDS/article/details/123968265?spm=1001.2014.3001.5501**（写的很细致，强推）**

https://blog.csdn.net/vittore_li/category_11556951.html?spm=1001.2014.3001.5482

机器人进阶之路 https://www.zhihu.com/column/c_1241771327444774912

机器人技术基础之运动学 https://www.zhihu.com/column/c_1693921850789429248?utm_campaign=&utm_medium=social&utm_psn=1896366318900467549&utm_source=qq

【史上最强机械臂教程】机械臂运动学、轨迹规划和动力学控制仿真-AC今夜有雨语雀 https://www.yuque.com/dashboard/books#collab

深蓝学院：机器人人抓取与操作 https://www.shenlanxueyuan.com/course/774

具身智能技术指南 Embodied-AI-Guide https://github.com/TianxingChen/Embodied-AI-Guide?tab=readme-ov-file#vla

全网最全具身智能知识库 https://yv6uc1awtjc.feishu.cn/wiki/WPTzw9ON0ivIVrkLjVocNZh8nLf

具身智能入门指南 https://lcn9rz94g08a.feishu.cn/wiki/LhqzwYwcdiuyWvkdmgdc0OQ2nOc

国内首个具身智能算法与实战全栈教程（已购买）https://gkklv.duanshu.com/?#/

具身智能之心知识星球

关于机械臂操纵（抓取、操纵）方向的论文仓库：https://github.com/BaiShuanghao/Awesome-Robotics-Manipulation

MATLAB机器人工具箱

【MATLAB教程_台大郭彦甫（14课）原视频补档】 https://www.bilibili.com/video/BV1GJ41137UH/?share_source=copy_web&vd_source=4eaa4760820b4cda603e510d556578ac

机器人工具箱笔记https://www.yuque.com/office/yuque/0/2022/doc/27569122/1667006650805-39b5fcad-561e-4dc6-bd08-305d0d51b78d.doc?from=https%3A%2F%2Fwww.yuque.com%2Fjinyeyouyu-v4bnu%2Fwbelr0%2Fcs36y2

Matlab机器人工具箱入门指南 • 汪迪 https://class.guyuehome.com/p/t_pc/course_pc_detail/column/p_615fb0e4e4b0dfaf7faa9725

【MATLAB机器人工具箱10.4 机械臂仿真教学（未完结）】 https://www.bilibili.com/video/BV1q44y1x7WC/?share_source=copy_web&vd_source=4eaa4760820b4cda603e510d556578ac

【MATLAB教程_台大郭彦甫（14课）原视频补档】 https://www.bilibili.com/video/BV1GJ41137UH/?share_source=copy_web&vd_source=4eaa4760820b4cda603e510d556578ac

空间描述和变换

具身智能知识库-坐标系与位姿变换https://yv6uc1awtjc.feishu.cn/wiki/CIuTwmBOoip1cQk31wgc81Tanvd

DH建模

PPT-https://wenku.baidu.com/view/001c588602d8ce2f0066f5335a8102d277a26148.html?_wkts_=1746360331828

详细介绍如何使用MATLAB中的机器人工具箱建立机器人模型（DH法建模）（三自由度机械臂）https://blog.csdn.net/qq_44339029/article/details/109708454?ops_request_misc=&request_id=&biz_id=102&utm_term=%E6%9C%BA%E6%A2%B0%E8%87%82dh&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-1-109708454.142

工业六轴机器人常见的MOD(改进)-DH模型建立方法-https://blog.csdn.net/Vittore_Li/article/details/122219529?spm=1001.2014.3001.5502

正逆运动学

如果是自己设计机械臂或者机械臂末端，需要自己推导运动学公式，若是购买的机械臂成品，则可直接调用机械臂厂家提供的API。

想要快速了解什么是IK,FK的同学可以看这个，可以建立一个粗略的认知：https://www.bilibili.com/video/BV18E411v7F9/?spm_id_from=333.337.search-card.all.click&vd_source=b14220472557bfa1918f3d0faa38bdc1

较为简单的过一遍IK和FK的原理可以看这个：

https://blog.csdn.net/Dwzsa/article/details/142386529?spm=1001.2101.3001.6650.3&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ECtr-3-142386529-blog-109314877.235%5Ev43%5Epc_blog_bottom_relevance_base7&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ECtr-3-142386529-blog-109314877.235%5Ev43%5Epc_blog_bottom_relevance_base7&utm_relevant_index=6

puma560运动学 https://blog.csdn.net/howard789/category_11645542.html?fromshare=blogcolumn&sharetype=blogcolumn&sharerId=11645542&sharerefer=PC&sharesource=qq_52769953&sharefrom=from_link

3、4自由度机械臂正逆解推导与代码实现 • 吴健新 https://class.guyuehome.com/p/t_pc/course_pc_detail/column/p_61839a62e4b0ac1ecd65bf47

正运动学

手推正运动学-https://blog.csdn.net/AlbertDS/article/details/110631364

puma560 手推正运动学https://blog.csdn.net/howard789/article/details/123142492?fromshare=blogdetail&sharetype=blogdetail&sharerId=123142492&sharerefer=PC&sharesource=qq_52769953&sharefrom=from_link

较为详细的视频课（南科大贾振中）：

FK1 https://www.bilibili.com/video/BV1Ve4y127Uf/?spm_id_from=333.788.recommend_more_video.0&vd_source=b14220472557bfa1918f3d0faa38bdc1

FK2 https://www.bilibili.com/video/BV1a14y157uL/?spm_id_from=333.788.videopod.sections&vd_source=b14220472557bfa1918f3d0faa38bdc1

逆运动学

求解方法：数值解和解析解

手推逆运动学-https://blog.csdn.net/AlbertDS/article/details/123679114

手推puma 560 逆运动学https://blog.csdn.net/howard789/article/details/123150942?fromshare=blogdetail&sharetype=blogdetail&sharerId=123150942&sharerefer=PC&sharesource=qq_52769953&sharefrom=from_link

MATLAB机器人工具箱中机器人逆解是如何求出来的？https://www.zhihu.com/question/41673569/answer/129670927?share_code=IhuO3vQR063S&utm_psn=1907032115301823715

【一起用Matlab从零手写六轴机械臂正逆解和轨迹规划】 https://www.bilibili.com/video/BV1UY411N7Xr/?share_source=copy_web&vd_source=4eaa4760820b4cda603e510d556578ac （不调用机器人工具箱）

机器人厂商的逆解是怎么做的 https://www.zhihu.com/question/380943528/answer/1095992504?share_code=10Gaq9dtXhHIi&utm_psn=1907032724964882059

较为详细的视频课（南科大贾振中）：

IK1 https://www.bilibili.com/video/BV1PD4y1t7xP/?spm_id_from=333.337.search-card.all.click&vd_source=b14220472557bfa1918f3d0faa38bdc1

IK2 https://www.bilibili.com/video/BV1Tt4y1T79Z/?spm_id_from=333.788.recommend_more_video.0&vd_source=b14220472557bfa1918f3d0faa38bdc1

Book：

较为详细的IK理论 https://motion.cs.illinois.edu/RoboticSystems/InverseKinematics.html

轨迹规划

具身智能知识库-机器人轨迹规划https://yv6uc1awtjc.feishu.cn/wiki/Zmk2we1JQiv9gBkyk0ZcMXgSnMd

机械臂速成小指南（十三）：轨迹规划概述https://blog.csdn.net/m0_53966219/article/details/125828466?fromshare=blogdetail&sharetype=blogdetail&sharerId=125828466&sharerefer=PC&sharesource=qq_52769953&sharefrom=from_link

机器人控制与轨迹规划https://blog.csdn.net/qq_37469992/article/details/112131666?fromshare=blogdetail&sharetype=blogdetail&sharerId=112131666&sharerefer=PC&sharesource=qq_52769953&sharefrom=from_link

【NUBUS五次非均匀B样条轨迹规划及机械臂仿真】 https://www.bilibili.com/video/BV1gX4y1u7gY/?share_source=copy_web&vd_source=4eaa4760820b4cda603e510d556578ac

六自由度机器人（机械臂）运动学建模及运动规划系列（四）——轨迹规划https://blog.csdn.net/weixin_47849087/article/details/126143953?fromshare=blogdetail&sharetype=blogdetail&sharerId=126143953&sharerefer=PC&sharesource=qq_52769953&sharefrom=from_link

机器人基础-机器人轨迹规划 https://yv6uc1awtjc.feishu.cn/wiki/Zmk2we1JQiv9gBkyk0ZcMXgSnMd

【适用所有机械臂!轨迹跟踪仿真!matlab算法!手把手教程|源码公开】 https://www.bilibili.com/video/BV12q4y1A7fi/?share_source=copy_web&vd_source=4eaa4760820b4cda603e510d556578ac

【《路径规划入门教程》——第一个专注于机械臂的路径规划课程】 https://www.bilibili.com/video/BV1G24y1M7ko/?share_source=copy_web&vd_source=4eaa4760820b4cda603e510d556578ac

再就是买的一些课程里涉及到的轨迹规划的章节

参考书籍：Principles of Robot Motion Theory,Algorithms,and Implementations. Planning Algorithms

项目1：机械臂直线规划+梯形速度规划

任务要求

使用MATLAB软件对珞石XB7进行建模并实现直线规划+梯形速度规划。

DH建模

D-H约定定义了一个机械臂的正向运动学方程，也就是从关节变量到末端执行器位置和姿态的映射，由上图可以得到DH各参数以及关节角限制。

建立机器人坐标系

改进DH（MDH）建模

MATLAB对机器人DH建模

%% MDH参数
theta1 = 0;    d1 = 380;     a1 = 0;    alpha1 = 0;    
theta2 = 0;    d2 = 0;       a2 = 30;   alpha2 = pi/2;        
theta3 = 0;    d3 = 0;       a3 = 340;  alpha3 = 0;    
theta4 = 0;    d4 = 335;     a4 = 35;   alpha4 = pi/2;    
theta5 = 0;    d5 = 0;       a5 = 0;    alpha5 = -pi/2;     
theta6 = 0;    d6 = 83;      a6 = 0;    alpha6 = pi/2;     

L(1) = Link([theta1, d1, a1, alpha1], 'modified');
L(2) = Link([theta2, d2, a2, alpha2], 'modified');L(2).offset=-pi/2;
L(3) = Link([theta3, d3, a3, alpha3], 'modified');
L(4) = Link([theta4, d4, a4, alpha4], 'modified');
L(5) = Link([theta5, d5, a5, alpha5], 'modified');
L(6) = Link([theta6, d6, a6, alpha6], 'modified');

% 限制关节角度
L(1).qlim = [-170*pi/180, 170*pi/180];
L(2).qlim = [-135*pi/180, 96*pi/180];
L(3).qlim = [-45*pi/180, 187*pi/180];
L(4).qlim = [-180*pi/180, 180*pi/180];
L(5).qlim = [-68*pi/180, 68*pi/180];
L(6).qlim = [-180*pi/180, 180*pi/180];


% 构建机器人模型
robot = SerialLink(L, 'name', '珞石XB-7');
robot.display();  % 显示DH参数
robot.teach();    % 交互式示教界面

直线轨迹规划+梯形速度规划

直线轨迹规划

ctraj函数用于生成两点之间的平滑轨迹。

ctraj的主要返回值是位姿轨迹，通常是一个4x4xN的矩阵，其中每个4x4切片表示一个齐次变换矩阵，描述机器人末端执行器在笛卡尔空间中该点的位姿（速度+方向），N是轨迹点的数量，由输入参数（如时间步长或插值点数）决定。

TC=ctraj(T0,T1,N)，是一个从T0姿态到T1姿态的四维笛卡尔轨迹，其中有N个点沿着路径遵循梯形速度曲线。

TC=ctraj(T0,T1,S),S的各元素指定了沿路径的分段距离，这些值处于[0,1]范围内，第i个点对应于沿路径的距离s(i)。

T1=transl(-280,-300, 450);	%起始点齐次变换矩阵 transl函数用于创建一个表示平移的仿射变换矩阵
T2=transl(-280,300, 450);	%终止点齐次变换矩阵
N = 50;                     %插值点数
Tc = ctraj(T1,T2,N);  	    %直线规划，得到每一步的T阵
q=robot.ikine(Tc);		    %运动学逆解
plot3(squeeze(Tc(1,4,:)),squeeze(Tc(2,4,:)),squeeze(Tc(3,4,:)));%输出末端轨迹。
title('直线轨迹');
robot.plot(q);	 %动画演示
%Tc(1,4,:)：提取所有时间步的 x 坐标（第 1 行第 4 列的所有元素）。
%Tc(2,4,:)：提取所有时间步的 y 坐标（第 2 行第 4 列的所有元素）。
%Tc(3,4,:)：提取所有时间步的 z 坐标（第 3 行第 4 列的所有元素）。
%sqeeze函数用于移除数组中长度为 1 的冗余维度，从而简化数组的维度结构（仅压缩长度为 1 的维度，其他维度不变）。直接提取 Tc(1,4,:) 会得到一个 1×1×N 的三维数组，而 plot3 需要 N×1 的向量。squeeze 移除长度为 1 的维度，将 1×1×N 转换为 N×1 的列向量。  
%向量和矩阵数据  plot3(X,Y,Z) 绘制三维空间中的坐标。要绘制由线段连接的一组坐标，应将 X、Y、Z 指定为相同长度的向量。

梯形速度规划

function [t, s] = trapezoid_vel_profile(dist, v_max, a_max)
    % 输入检查
    if v_max <= 0 || a_max <= 0
        error('速度和加速度必须为正数');
    end
    if dist <= 0
        t = 0; s = 0; return;
    end
    
    % 计算关键参数
    t_acc = v_max / a_max;          % 加速时间(s)
    s_acc = 0.5 * a_max * t_acc^2;  % 加速距离(m)
    
    % 判断运动类型
    if 2*s_acc > dist
        % 三角形速度曲线
        t_acc = sqrt(dist / a_max);
        t_total = 2*t_acc;
        t_const = 0;
        s_const = 0;
    else
        % 梯形速度曲线
        s_const = dist - 2*s_acc;   % 匀速距离(m)
        t_const = s_const / v_max;  % 匀速时间(s)
        t_total = 2*t_acc + t_const;% 总时间(s)
    end
    
    % 生成时间序列
    t = linspace(0, t_total, round(t_total/0.01))';%将连续的总时间t_total分割为离散的时间点，步长为0,01s,用于后续计算每个时刻对应的位置s。
    %linspace(start,end,n_points)生成从start到end的等间隔n_points个点，返回的是一个行向量
    %round为四舍五入函数
    s = zeros(size(t));%初始化与时间序列对应的0数组s，避免在循环中动态扩展数组，显著提升代码运行效率
    
    % 计算归一化位置
    for i = 1:length(t)
        if t(i) <= t_acc%加速阶段
            s(i) = 0.5 * a_max * t(i)^2;
        elseif t(i) <= t_acc + t_const%匀速阶段
            s(i) = s_acc + v_max*(t(i)-t_acc);
        else
            t_dec = t(i) - t_acc - t_const;%减速阶段
            s(i) = (s_acc + s_const) + v_max*t_dec - 0.5*a_max*t_dec^2;
        end
    end
    
    % 确保归一化在[0,1]范围内
    s = min(max(s/dist, 0), 1);%s为与时间序列对应的数组，s中每个元素为当前离散时间点对应的距离
    %s/dist即将位置归一化到[0,1],max(a,b)即取a和b之间的最大值
end

画矩形案例

%% 机器人建模（Modified DH参数）(长度mm)
theta1 = 0;    d1 = 380;     a1 = 0;    alpha1 = 0;    
theta2 = 0;    d2 = 0;       a2 = 30;   alpha2 = pi/2;        
theta3 = 0;    d3 = 0;       a3 = 340;  alpha3 = 0;    
theta4 = 0;    d4 = 335;     a4 = 35;   alpha4 = pi/2;    
theta5 = 0;    d5 = 0;       a5 = 0;    alpha5 = -pi/2;     
theta6 = 0;    d6 = 83;      a6 = 0;    alpha6 = pi/2;     

L(1) = Link([theta1, d1, a1, alpha1], 'modified');L(1).offset=pi;
L(2) = Link([theta2, d2, a2, alpha2], 'modified');L(2).offset=pi/2;
L(3) = Link([theta3, d3, a3, alpha3], 'modified');
L(4) = Link([theta4, d4, a4, alpha4], 'modified');
L(5) = Link([theta5, d5, a5, alpha5], 'modified');
L(6) = Link([theta6, d6, a6, alpha6], 'modified');

% 构建机器人模型
robot = SerialLink(L, 'name', 'XB-7');
robot.display();  % 显示DH参数
robot.teach();    % 交互式示教界面

%% 轨迹规划参数（单位转换为米）
scale = 0.001;  % mm转m
v_max = 0.2;    % 最大速度(m/s)
a_max = 0.2;    % 最大加速度(m/s²)

%% 直线轨迹规划 
% 各直线起点和终点（单位：mm）
T_1 = transl(-280,-300, 450);  
T_2 = transl(-280,300, 450);      
T_3 = transl(-450,300, 880); 
T_4 = transl(-450,-300, 880); 
T_5 = transl(-280,-300, 450); 
% 计算直线距离（单位：m）
delta_pos1 = norm(T_2(1:3,4) - T_1(1:3,4)) * scale;%1:3选择矩阵的1到三行 4：选择第四列 T_2(1:3,4)即提取第4列的前3个元素
delta_pos2 = norm(T_3(1:3,4) - T_2(1:3,4)) * scale;
delta_pos3 = norm(T_4(1:3,4) - T_3(1:3,4)) * scale;
delta_pos4 = norm(T_5(1:3,4) - T_4(1:3,4)) * scale;
%% 执行轨迹规划
[t_1, s_1] = trapezoid_vel_profile(delta_pos1, v_max, a_max);
[t_2, s_2] = trapezoid_vel_profile(delta_pos2, v_max, a_max);
[t_3, s_3] = trapezoid_vel_profile(delta_pos3, v_max, a_max);
[t_4, s_4] = trapezoid_vel_profile(delta_pos4, v_max, a_max);
Ts1 = ctraj(T_1, T_2, s_1);  % 笛卡尔空间轨迹
Ts2 = ctraj(T_2, T_3, s_2);  % 笛卡尔空间轨迹
Ts3 = ctraj(T_3, T_4, s_3);  % 笛卡尔空间轨迹
Ts4 = ctraj(T_4, T_5, s_4);  % 笛卡尔空间轨迹

% 逆运动学求解
q1 = robot.ikine(Ts1,'mask', [1 1 1 1 1 1],'qlim', true);  % 全自由度求解
q2 = robot.ikine(Ts2,'mask', [1 1 1 1 1 1],'qlim', true);  % 全自由度求解
q3 = robot.ikine(Ts3,'mask', [1 1 1 1 1 1],'qlim', true);  % 全自由度求解
q4 = robot.ikine(Ts4,'mask', [1 1 1 1 1 1],'qlim', true);  % 全自由度求解
q_total=[q1;q2;q3;q4];%q_total 是一个包含机器人所有关节角度轨迹的 N×6 矩阵（N 为总时间步数，6 对应 6 个关节）
%每一行表示一个时间步的关节角度值 q_total(i,:) = [θ₁, θ₂, θ₃, θ₄, θ₅, θ₆]   第i个时间点的6个关节角度
%[q1; q2; q3; q4] 是纵向拼接（垂直拼接），而 [q1, q2, q3, q4] 是横向拼接（水平拼接）

%% 画出矩形轨迹
x=squeeze(Ts1(1,4,:)); y=squeeze(Ts1(2,4,:)); z=squeeze(Ts1(3,4,:));
x1=squeeze(Ts2(1,4,:)); y1=squeeze(Ts2(2,4,:)); z1=squeeze(Ts2(3,4,:));
x2=squeeze(Ts3(1,4,:)); y2=squeeze(Ts3(2,4,:)); z2=squeeze(Ts3(3,4,:));
x3=squeeze(Ts4(1,4,:)); y3=squeeze(Ts4(2,4,:)); z3=squeeze(Ts4(3,4,:));
x_all=[x;x1;x2;x3];y_all=[y;y1;y2;y3];z_all=[z;z1;z2;z3];

plot3(x_all,y_all,z_all);

%% 实时动画（精确时间控制）
robot.plot(q_total, 'fps', 100, 'trail', {'r','LineWidth',2}, 'nobase', 'notiles');
%roboy.plot按顺序读取每一行（从第 1 行到第 N 行），每行对应一个时间点的关节角度 [θ₁, θ₂, θ₃, θ₄, θ₅, θ₆]。对每一行数据，调用正运动学计算（fkine）并更新3D模型的位姿。不需要自己写 for 循环，robot.plot 内部已经实现了这一逻辑。

%% 计算并绘制末端速度曲线
% 合并各段时间并生成总时间轴
current_time = 0;
% 第一段
t1 = t_1 + current_time;
current_time = current_time + t_1(end);
% 第二段
t2 = t_2 + current_time;
current_time = current_time + t_2(end);
% 第三段
t3 = t_3 + current_time;
current_time = current_time + t_3(end);
% 第四段
t4 = t_4 + current_time;
% 总时间轴
t_total = [t1; t2; t3; t4];%t_total 是一个包含所有时间点的向量，表示机械臂运动的每个离散时刻。

% 计算速度分量（注意单位转换）
dx = diff(x_all) * scale; % mm转m
dy = diff(y_all) * scale; %diff用于计算数组中相邻元素之间的差值（即差分）,对于数组 X = [x₁, x₂, x₃, ..., xn]，diff(X) 返回：[x₂ - x₁, x₃ - x₂, ..., xn - xₙ₋₁]
dz = diff(z_all) * scale;
dt = diff(t_total);

vx = dx ./ dt;%"./"运算符：点除，表示对两个数组的对应元素逐个相除。
vy = dy ./ dt;
vz = dz ./ dt;
speed = sqrt(vx.^2 + vy.^2 + vz.^2);

% 计算每个时间区间的中点时间，用于对齐通过差分计算得到的速度值
time_mid = t_total(1:end-1) + dt/2;%时间轴数据，通常是一个向量（数组），表示每个速度值对应的时间点
%t_total(1:end-1):取时间轴的前 n-1 个点（因为差分会减少一个数据点）。 例如：t_total = [0, 1, 3, 6] → t_total(1:end-1) = [0, 1, 3]。
%dt/2:计算每个时间区间长度的一半。例如：dt = diff([0, 1, 3, 6]) = [1, 2, 3] → dt/2 = [0.5, 1, 1.5]。
%相加操作:将每个区间的起始时间点 t_total(1:end-1) 加上半区间长度 dt/2，得到区间中点：[0, 1, 3] + [0.5, 1, 1.5] = [0.5, 2.0, 4.5]。差分计算的速度是区间内的平均速度，自然应对应区间的中点时刻（而非起点或终点）。这样，速度值 speed 可以正确对齐到区间中点时间 [0.5, 2.0, 4.5] 绘图。

% 绘制总速度曲线
figure('Name','End-Effector Speed Profile');
plot(time_mid, speed, 'LineWidth', 2);
xlabel('Time (s)');
ylabel('Speed (m/s)');
title('End-Effector Linear Speed');
grid on;
ylim([0 v_max*1.1]);

% 绘制各方向速度分量
figure('Name','Velocity Components')
subplot(3,1,1);%subplot(3,1,1) 是 MATLAB 中用于创建子图（subplot）的函数，它的作用是将当前图形窗口分割成多个小区域，并在指定的区域中绘制图形。
%subplot(m, n, p): m：子图的行数（垂直方向分成几块) n：子图的列数（水平方向分成几块) p：当前操作的子图位置（按从左到右、从上到下的顺序编号)
plot(time_mid, vx, 'r');
ylabel('Vx (m/s)');
grid on;

subplot(3,1,2); 
plot(time_mid, vy, 'g');
ylabel('Vy (m/s)');
grid on;

subplot(3,1,3);
plot(time_mid, vz, 'b'); 
ylabel('Vz (m/s)');
xlabel('Time (s)');
grid on;

%% 梯形速度规划（修正版）
function [t, s] = trapezoid_vel_profile(dist, v_max, a_max)
    % 输入检查
    if v_max <= 0 || a_max <= 0
        error('速度和加速度必须为正数');
    end
    if dist <= 0
        t = 0; s = 0; return;
    end
    
    % 计算关键参数
    t_acc = v_max / a_max;          % 加速时间(s)
    s_acc = 0.5 * a_max * t_acc^2;  % 加速距离(m)
    
    % 判断运动类型
    if 2*s_acc > dist
        % 三角形速度曲线
        t_acc = sqrt(dist / a_max);
        t_total = 2*t_acc;
        t_const = 0;
        s_const = 0;
    else
        % 梯形速度曲线
        s_const = dist - 2*s_acc;%const为匀速阶段
        t_const = s_const / v_max;
        t_total = 2*t_acc + t_const;
    end
    
    % 生成时间序列
    t = linspace(0, t_total, round(t_total/0.01))';%将连续的总时间t_total分割为练得时间点，用于后续计算每个时刻对应的位置s
    %linspace(start,end,n_points)生成从start到end的等间隔n_points个点，返回的是一个行向量
    s = zeros(size(t));%初始化与时间序列登场的0数组s，避免在循环中动态扩展数组，显著提升代码运行效率
    
    % 计算归一化位置
    for i = 1:length(t)
        if t(i) <= t_acc
            s(i) = 0.5 * a_max * t(i)^2;
        elseif t(i) <= t_acc + t_const
            s(i) = s_acc + v_max*(t(i)-t_acc);
        else
            t_dec = t(i) - t_acc - t_const;
            s(i) = (s_acc + s_const) + v_max*t_dec - 0.5*a_max*t_dec^2;
        end
    end
    
    % 确保归一化在[0,1]范围内
    s = min(max(s/dist, 0), 1);%s/dist即将位置归一化到[0,1],max(a,b)即取a和b之间的最大值
end

trail

工作空间可视化

%% 机器人建模（Modified DH参数）(长度mm)
theta1 = 0;    d1 = 380;     a1 = 0;    alpha1 = 0;    
theta2 = 0;    d2 = 0;       a2 = 30;   alpha2 = pi/2;        
theta3 = 0;    d3 = 0;       a3 = 340;  alpha3 = 0;    
theta4 = 0;    d4 = 335;     a4 = 35;   alpha4 = pi/2;    
theta5 = 0;    d5 = 0;       a5 = 0;    alpha5 = -pi/2;     
theta6 = 0;    d6 = 83;      a6 = 0;    alpha6 = pi/2;     

L(1) = Link([theta1, d1, a1, alpha1], 'modified');L(1).offset=pi;
L(2) = Link([theta2, d2, a2, alpha2], 'modified');L(2).offset=pi/2;
L(3) = Link([theta3, d3, a3, alpha3], 'modified');
L(4) = Link([theta4, d4, a4, alpha4], 'modified');
L(5) = Link([theta5, d5, a5, alpha5], 'modified');
L(6) = Link([theta6, d6, a6, alpha6], 'modified');

L(1).qlim = [-170*pi/180, 170*pi/180];
L(2).qlim = [-135*pi/180, 96*pi/180];
L(3).qlim = [-45*pi/180, 187*pi/180];
L(4).qlim = [-180*pi/180, 180*pi/180];
L(5).qlim = [-68*pi/180, 68*pi/180];
L(6).qlim = [-180*pi/180, 180*pi/180];

robot = SerialLink(L, 'name', 'XB-7');
robot.display();  % 显示DH参数

num=30000;%循环次数，生成3万个随机位置点
P=zeros(num,3);%% 创建一个num行×3列的全零矩阵P,存储随时间或步骤变化的 三维数据（如机械臂末端的位置坐标 [x, y, z]）。
%生成机械臂末端执行器在三维空间中的随机位置
for i=1:num
    q1=L(1).qlim(1)+rand*(L(1).qlim(2)-L(1).qlim(1));
    q2=L(2).qlim(1)+rand*(L(2).qlim(2)-L(2).qlim(1));
    q3=L(3).qlim(1)+rand*(L(3).qlim(2)-L(3).qlim(1));
    q4=L(4).qlim(1)+rand*(L(4).qlim(2)-L(4).qlim(1));
    q5=L(5).qlim(1)+rand*(L(5).qlim(2)-L(5).qlim(1));
    q6=L(6).qlim(1)+rand*(L(6).qlim(2)-L(6).qlim(1));
    q=[q1 q2 q3 q4 q5 q6];
    T=robot.fkine(q);
    P(i, :)=transl(T);%从齐次变换矩阵T中提取平移部分（即末端执行器的 [x, y, z] 坐标）,将当前随机位置的坐标保存到 P 的第 i 行。
end
plot3(P(:,1),P(:,2),P(:,3), 'b.', 'markersize',1);%P(:,1)：矩阵 P 的第一列（所有点的 x 坐标）。
xlabel('X (mm)'); ylabel('Y (mm)'); zlabel('Z (mm)');
title('机器人工作空间可视化');
axis equal;
grid on

robot.teach();    % 交互式示教界面

附录：MATLAB常用函数

位姿描述

齐次变换矩阵，是一个包括姿态和位置信息的4x4矩阵。每个T都看成转动+移动，只有移动则转动对应部分为单位向量，只有转动则移动部分对应向量为0。

transl(translation)

transl函数用于创建一个表示平移的仿射变换矩阵

transl( [x, y, z] );						% 平移变换（4x4）,为该坐标系相对于世界坐标系的x,y,z坐标

eg:
T1=transl(-100,-100,300);
display(T1);
->T1 =
     1     0     0  -100
     0     1     0  -100
     0     0     1   300
     0     0     0     1

trotx,troty,trotz

trotx用于创建一个表示绕 x 轴旋转的仿射变换矩阵

trotx(theta), troty(theta), trotz(theta);	% 只有旋转的绕xyz轴旋转的齐次变换矩阵（4x4）

eg:
T1=trotx(180);
display(T1);
->T1 =

     1     0     0     0
     0    -1     0     0
     0     0    -1     0
     0     0     0     1

移动+转动

eg:
T1=transl(-100,-100,300)*trotx(180);
display(T1);

T1 =

     1     0     0  -100
     0    -1     0  -100
     0     0    -1   300
     0     0     0     1

eul2r

欧拉角，绕自身坐标系旋转，右乘。

x为横滚，y为俯仰，z为偏航

rpy2r

运动学

建立机器人模型

Link

Link类函数，基于DH法建模，建立其相关关系，DH法建模分改进型和标准型，Link类函数的一种用法是 R = Link([theta,d,a, alpha])，其中参数theta代表DH建模的关节角、参数d代表DH建模的连杆偏距、参数a代表DH建模的连杆长度、参数alpha代表DH建模的连杆转角。例如：L(1)=Link([1,2,3,4],‘modified’)，其中modified表示用改进型DH法建模

%% MDH参数
theta1 = 0;    d1 = 380;     a1 = 0;    alpha1 = 0;    
theta2 = 0;    d2 = 0;       a2 = 30;   alpha2 = -pi/2;        
theta3 = 0;    d3 = 0;       a3 = 340;  alpha3 = 0;    
theta4 = 0;    d4 = 335;     a4 = 35;   alpha4 = -pi/2;    
theta5 = 0;    d5 = 0;       a5 = 0;    alpha5 = pi/2;     
theta6 = 0;    d6 = 83;      a6 = 0;    alpha6 = -pi/2;     

L(1) = Link([theta1, d1, a1, alpha1], 'modified');L(1).offset=pi;
L(2) = Link([theta2, d2, a2, alpha2], 'modified');L(2).offset=pi/2;
L(3) = Link([theta3, d3, a3, alpha3], 'modified');
L(4) = Link([theta4, d4, a4, alpha4], 'modified');
L(5) = Link([theta5, d5, a5, alpha5], 'modified');
L(6) = Link([theta6, d6, a6, alpha6], 'modified');

关节角限制：

SerialLink

一个表示串联机械臂机器人的具体类，其中每个连杆和关节都通过使用DH参数的Link类对象进行描述。

通过在MATLAB命令行串口输出’doc SerialLink’可查看其详细内容。

以下是其常见的一些函数：

1.teach示教

1	Six_link.teach

2.plot

3.plot3d 三维模型展示(需导入STL文件)

只能用于标准型的DH参数建模。

1 2	mdl_puma560; p560.plot3d(qz,'view',[0,0]);

绝对路径

L(i) = Link( [theta, d, a, alpha, sigma]，'modified' ) % 定义关节，DH参数：关节角、连杆偏距、连杆长度、连杆转角，sigma=1为移动副, 改进型DH

L(i).qlim = [ min, max ]						  % 关节角度限制

Six_link = SerialLink( L, 'name', 'Sixlink' )	    % 将Link连接成一个机械臂

Six_link.plot( [theta_1, theta_2, ..., theta_n] ) 	% 画出机械臂，theta为关节初始角度

Six_link.display								 % 输出机械臂信息

Six_link.teach									 % 机械臂操控交互界面(示教)通过执行 robot.teach()，系统会进入示教模式，允许用户通过手动移动机器												人的关节或末端执行器来记录示教数据。这些数据可以用于后续的路径规划、运动控制或其他任务。

运动学

fkine正向运动学

接受关节角度作为输入参数，并返回对应的末端执行器位置和姿态矩阵。

ikine逆向运动学

接受末端执行器位置和姿态矩阵作为输入参数，并返回对应的关节角度。

1	q1=Five_dof.ikine(T,'mask',[1 1 1 1 1 0]);//当用于欠自由度（小于6）时，需用mask

ikine用于求数值解，不考虑关节限制

ikine6s用于求6轴机械臂的解析解

1	q2=Five_dof.ikunc(T);//当用于欠自由度（小于6）时，不需用mask

close all;
%% 建立机器人DH参数,初始姿态为竖直。
L(1)=Link('d',105.03,'a',0,'alpha',pi/2,'standard'); 
L(2)=Link('d',0,'a',-174.42,'alpha',0,'offset',-pi/2,'standard');
L(3)=Link('d',0,'a',-174.42,'alpha',0,'standard');
L(4)=Link('d',75.66,'a',0,'alpha',pi/2,'offset',-pi/2,'standard');
L(5)=Link('d',80.09,'a',0,'alpha',-pi/2,'standard');
L(6)=Link('d',44.36,'a',0,'alpha',0,'standard');  
robot=SerialLink(L(1:6),'name','Gluon_6L3', 'manufacturer','innfos')
%% 正解，给定关节角，求末端位姿
Theta=15*ones(1,6);
%% 验证正逆解的结果
Theta=Theta/180*pi;                         %换算成弧度
T=robot.fkine(Theta)             %求正解的齐次变换矩阵
q1=robot.ikine(T)*180/pi      %求逆解验证关节角

轨迹规划

一次多项式：首末速度突变，加速度为无穷大；

三次多项式：首末速度为0，加速度有突变；

五次多项式：首末速度、加速度为0，无突变。

五次多项式轨迹tpoly

1
2
3

t=linspace(0,2,51)%0-2s内插值51次，等效于0:0.04:2
[P,dP,ddP]=tpoly(0,3,t);%对位置0-3进行51次插值
指定初末速度：[P,dP,ddP]=tpoly(0,3,t,0.02,0.01)%t也可以直接写51，一样的

混合曲线轨迹lspb

看起来感觉和梯形速度规划差不多。

1 2	t=linspace(0,2,51); [p,dP,ddP]=lspb(0,3,t);

多维轨迹mtraj

1 2	t=linspace(0,2,51); [p,dP,ddP]=mtraj(@tpoly,[0,0],[3,4],t);%第一个参数为轨迹规划方式，用哪种就@哪种，二三参数为起点和终点

%% 机器人建模（Modified DH参数）(长度mm)
theta1 = 0;    d1 = 380;     a1 = 0;    alpha1 = 0;    
theta2 = 0;    d2 = 0;       a2 = 30;   alpha2 = pi/2;        
theta3 = 0;    d3 = 0;       a3 = 340;  alpha3 = 0;    
theta4 = 0;    d4 = 335;     a4 = 35;   alpha4 = pi/2;    
theta5 = 0;    d5 = 0;       a5 = 0;    alpha5 = -pi/2;     
theta6 = 0;    d6 = 83;      a6 = 0;    alpha6 = pi/2;     

L(1) = Link([theta1, d1, a1, alpha1], 'modified');L(1).offset=pi;
L(2) = Link([theta2, d2, a2, alpha2], 'modified');L(2).offset=pi/2;
L(3) = Link([theta3, d3, a3, alpha3], 'modified');
L(4) = Link([theta4, d4, a4, alpha4], 'modified');
L(5) = Link([theta5, d5, a5, alpha5], 'modified');
L(6) = Link([theta6, d6, a6, alpha6], 'modified');

L(1).qlim = [-170*pi/180, 170*pi/180];
L(2).qlim = [-135*pi/180, 96*pi/180];
L(3).qlim = [-45*pi/180, 187*pi/180];
L(4).qlim = [-180*pi/180, 180*pi/180];
L(5).qlim = [-68*pi/180, 68*pi/180];
L(6).qlim = [-180*pi/180, 180*pi/180];

robot = SerialLink(L, 'name', 'XB-7');
robot.display();  % 显示DH参数

P1=[-280,-300, 450];
P2=[-280,300, 450];

t=linspace(0,2,51);
Traj=mtraj(@tpoly,P1,P2,t);

n=size(Traj,1);
T=zeros(4,4,n);

for i=1:n
    T(:,:,i)=transl(Traj(i, :))*trotx(180);
end

Qtraj=robot.ikunc(T);
robot.plot(Qtraj,'movie','trail.gif','trail', {'r','LineWidth',2});