MD-H模型运动学正解、逆解及姿态角的解算验证

最新推荐文章于 2023-10-25 19:59:54 发布

原创

最新推荐文章于 2023-10-25 19:59:54 发布 · 5.4k 阅读

81 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #矩阵 #几何学 #算法

本文详细介绍了MD-H模型的正运动学验证，通过MATLAB代码展示了如何计算杆件位置和姿态；接着进行了RPY姿态角的解算，并与MATLAB工具箱结果对比；最后进行逆运动学计算，确保初始设定角度与计算结果一致。

文章目录

前言
一、MD-H模型正运动学验证
二、RPY姿态角解算验证
三、MD-H模型逆运动学验证

前言

记录修正D-H模型正运动学、RPY、逆运动学解算的验证过程

一、MD-H模型正运动学验证

%% 正运动学验证
clear;clc;close all;
%
%杆长参数设置
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
d1=L2;a2=L1;a3=L3;a4=L4;d4=L5;d6=L6;
%初始角设置
theta1=10;theta2=20;theta3=30;theta4=40;theta5=50;theta6=60;
T1=[cosd(theta1) -sind(theta1) 0 0; sind(theta1) cosd(theta1) 0 0; 0 0 1 d1; 0 0 0 1]*...
  [sind(theta2) cosd(theta2) 0 a2; 0 0 1 0; cosd(theta2) -sind(theta2) 0 0; 0 0 0 1]*...
  [cosd(theta3) -sind(theta3) 0 a3; sind(theta3) cosd(theta3) 0 0; 0 0 1 0; 0 0 0 1]*...
  [cosd(theta4) -sind(theta4) 0 a4; 0 0 1 d4; -sind(theta4) -cosd(theta4) 0 0; 0 0 0 1]*...
  [cosd(theta5) -sind(theta5) 0 0; 0 0 -1 0; sind(theta5) cosd(theta5) 0 0;0 0 0 1]*...
  [cosd(theta6) -sind(theta6) 0 0; 0 0 1 d6; -sind(theta6) -cosd(theta6) 0 0; 0 0 0 1]

s1=sind(theta1);s2=sind(theta2);s3=sind(theta3);s4=sind(theta4);s5=sind(theta5);s6=sind(theta6);
c1=cosd(theta1);c2=cosd(theta2);c3=cosd(theta3);c4=cosd(theta4);c5=cosd(theta5);c6=cosd(theta6);
s23=s2*c3+c2*s3;c23=c2*c3-s2*s3;
nx=c1*