对于昨天的matlab程序，又进行了优化，直接将3*3姿态矩阵转为欧拉角(ZYX)显示。优化后的程序如下：
T = robot.fkine([0 0 0 0 0 0]).T
%T = robot.fkine([-0.081522 0.590650 2.562666 0.439001 1.575855 -0.840727]).T
%robot.plot([0 0 0 0 0 0])%转出来不一致，后续有机会再研究
R = T(1:3, 1:3)
[yaw pitch roll]= dcm2angle(R, 'ZYX')
rad2deg(yaw)
rad2deg(pitch)
rad2deg(roll)RPY = rotm2eul(R, 'ZYX')
rad2deg(RPY(1))
rad2deg(RPY(2))
rad2deg(RPY(3))在昨天基础上，优化注释(对D-H参数注释)，如何调整零位姿态。
添加示教界面，通过控制关节，直接显示逆解后位置和姿态。
优化UI视野及坐标轴范围。
最终效果如下：
1、零位姿态调整
调整前如图：
调整后如下：
旋转1、2、4关节，使得零位姿态与aubo i5机械臂一致。
2、示教正解验证
Matlab机械臂1、3关节旋转90度，如下
Aubo 机械臂做同样操作，如下：
3、Matlab程序如下
% Link函数调用格式： L（i）=Link( [theta，D，A，alpha，sigma]，‘convention’)
% 其参数与D-H参数相对应
% 前四个参数依次表示：参数‘theta’代表关节角，参数‘D’代表横距，
% 参数‘A’代表杆件长度，参数‘alpha’代表扭转角，参数‘sigma’代表关节类型：0代表旋转关节，非0代表移动关节，默认值为0。
% 参数 ‘convention’ 表示使用D-H参数法创建机器人模型的类型：
% ‘standard’表示采用标准D-H参数法创建机器人模型；‘modified’表示采用该改进D-H参数法创建机器人模型，默认值为'standard'。
clear;
clc;
%建立机器人模型
% theta d a alpha sigma offset
L1=Link([0 0.0985 0 0 0 pi ],'modified'); %定义连杆的D-H参数
L2=Link([0 0.1215 0 -pi/2 0 -pi/2 ],'modified');
L3=Link([0 0 0.408 pi 0 0 ],'modified');
L4=Link([0 0 0.376 pi 0 -pi/2 ],'modified');
L5=Link([0 0.1025 0 -pi/2 0 0 ],'modified');
L6=Link([0 0.094 0 pi/2 0 0 ],'modified');%轴的旋转限度
L(1).qlim =[-2*pi, 2*pi];
L(2).qlim =[-2*pi, 2*pi];
L(3).qlim =[-2*pi, 2*pi];
L(4).qlim =[-2*pi, 2*pi];
L(5).qlim =[-2*pi, 2*pi];
L(6).qlim =[-2*pi, 2*pi];
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','auboi5'); %连接连杆，机器人取名auboi5
%robot =SerialLink(L,'name','aubo_i5')
title("aubo_i5")figure(1);
robot.display();
robot.teach('rpy/zyx');%调整坐标轴及视野
set(gca,'XLim',[-1.121, 1.121]); %将X轴范围设定
set(gca,'YLim',[-1.121, 1.121]); %将X轴范围设定
set(gca,'ZLim',[0, 1.121]); %将Z轴最小值设定为0，可以消除模型下面的长杆
%set(gca,'XDir','reverse'); %将x轴方向设置为反向
%set(gca,'YDir','reverse'); %将Y轴方向设置为反向
%set(gca,'View',[-85,10]); %设定视野方向角和俯仰角写在后面：
程序在网上都找的到，最后是拼接成一个具象的东西。
正解的原理可以简化成6个4*4矩阵连乘，逆解那个看起来就头大，不过如果最终捋顺后也就是被封装成了一个函数。
如需程序可公众号后台留言“Matlab机械臂运动学示教演示”。
欢迎关注公众号：
