使用SDH参数建立串联机器人模型，设置Z轴方向并确定机器人未端执行器的齐次矩阵，当J1~J6角度分别为45度、60度、30度、15度、-30度和45度时。使用RoboticsToolbox验证结果的正确性。（附MATLAB建模代码）

在使用SDH(Serial Direct Drive)参数构建串联机器人模型时，首先需要了解机器人的关节坐标系以及各关节之间的变换关系。通常，Z轴表示工具坐标系的方向，末端执行器的位置由所有关节的角度决定。

假设我们有一个六关节串联机器人，每个关节对应一个旋转运动。为了计算末端执行器的齐次矩阵，我们需要使用关节变量\( q_1 \)到\( q_6 \)，其中\( J1=45^\circ \), \( J2=60^\circ \), ..., \( J6=45^\circ \)。

对于SDH结构，每个关节的变换可以由旋转矩阵描述，例如\( T_i = R_{z}(q_i)T_{i-1} \)，从第一个关节开始，直到最后一个关节。这里的\( R_z(\theta) \)是绕Z轴的旋转矩阵，\( T_i \)是关节\( i \)后的世界坐标系相对于关节\( i-1 \)的变换。

末端执行器的齐次矩阵\( H \)初始为 Identity 矩阵，然后通过递归乘法得到：

\[
H = R_{z}(q_6) * R_{z}(q_5) * ... * R_{z}(q_1)
\]

你可以用以下MATLAB代码实现这一过程：

% 定义旋转矩阵函数
Rz = @(theta) [cos(theta), -sin(theta); sin(theta), cos(theta)];

% 给定关节角度数组
angles = [45; 60; 30; 15; -30; 45]; % 度

% 初始化齐次矩阵
H = eye(4);

% 逐关节计算并更新齐次矩阵
for i = 1:length(angles)
    H = Rz(deg2rad(angles(i))) * H;
end

% Robotics Toolbox 验证
% 如果有RTB包，你可以使用`transformFromJoint`函数
% 或者手动创建Link对象，然后计算末端位置和姿态，对比两者是否一致
% 例如：
% link = SerialLink('Name', 'SDH Robot');
% for i = 1:length(link.Joints)
%     link.Joints(i).Angle = angles(i);
% end
% toolFrame = transformFromJoint(link);
% isHomogeneousEqual(H, toolFrame); % 判断两个矩阵是否相等

%