用matlab的m函数编写机器人的正运动学计算函数输入为各关节角度 输出为末端的其次矩阵进一步转换为位置与姿态

假设你的机器人是一个n自由度的机器人，那么机器人的正运动学可以这样计算：

function Tn = ForwardKinematics(theta)
% theta是n维列向量，表示各个关节的角度
% Tn是机器人末端的齐次矩阵

%% 定义机器人的DH参数
% 可以根据实际情况进行修改
a = [0.5, 0.2, 0.1, 0.3];
alpha = [pi/2, 0, pi/2, 0];
d = [0.2, 0, 0, 0];
theta0 = [0, pi/2, 0, 0];

%% 计算各个关节坐标系的齐次矩阵
T_i = cell(1, n);
for i = 1:n
    T_i{i} = DH(a(i), alpha(i), d(i), theta(i)+theta0(i));
end

%% 计算机器人末端的齐次矩阵
Tn = eye(4);
for i = 1:n
    Tn = Tn * T_i{i};
end

%% 辅助函数DH
function T = DH(a, alpha, d, theta)
    T = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
         sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
         0, sin(alpha), cos(alpha), d;
         0, 0, 0, 1];
end

end

这个函数中，我们首先定义了机器人的DH参数，然后根据输入的关节角度计算出各个关节坐标系的齐次矩阵，最后将它们连乘起来得到机器人末端的齐次矩阵。

如果你想要得到机器人末端的位置和姿态，可以将机器人末端的齐次矩阵转换为旋转矩阵和位移向量，然后用`tr2eul`函数将旋转矩阵转换为欧拉角。

Tn = ForwardKinematics(theta);
R = Tn(1:3, 1:3);
p = Tn(1:3, 4);
[roll, pitch, yaw] = tr2eul(R);

其中，`R`是机器人末端的旋转矩阵，`p`是机器人末端的位移向量，`roll`、`pitch`和`yaw`分别是机器人末端的欧拉角。