机器人工具箱逆运动学求解ikine6s函数为何只有姿态正确，而位置不对呢？

Question

代码思路是首先建立SDH模型，因为ikine6s只能对SDH模型求解。
以下是我的机器人和SDH模型，有没有建错呢？

然后给定一个位姿，正运动学fkine函数得到齐次变换矩阵T_0。
再使用ikine6s求8组逆解。求出的逆解对应的齐次变换矩阵T_1-T_8。
Matlab代码：

%% ikine6s

clear all
close all
clc

%% SDH方法

L(1)=Link('revolute','alpha',-pi/2,  'a',350,  'offset',0,  'd',750,  'standard');
L(2)=Link('revolute','alpha',0,      'a',1250, 'offset',0,  'd',0,    'standard');
L(3)=Link('revolute','alpha',pi/2,   'a',55,   'offset',0,  'd',0,    'standard');
L(4)=Link('revolute','alpha',-pi/2,  'a',0,    'offset',0,  'd',1100, 'standard');
L(5)=Link('revolute','alpha',pi/2,   'a',0,    'offset',0,  'd',0,    'standard');
L(6)=Link('revolute','alpha',0,      'a',0,    'offset',0,  'd',0,    'standard');

L(1).qlim=[-185,185]/180*pi;
L(2).qlim=[-146,0]/180*pi;
L(3).qlim=[-29,245]/180*pi;
L(4).qlim=[-350,350]/180*pi;
L(5).qlim=[-125,125]/180*pi;
L(6).qlim=[-350,350]/180*pi;

KUKA_KR210_2=SerialLink(L,'name','KUKA KR210-2');
KUKA_KR210_2.display

%% 给定位姿求逆解

T_0=KUKA_KR210_2.fkine([-pi/6,-pi/3,5*pi/6,-pi/6,pi/3,-pi/6])

qrad_1=KUKA_KR210_2.ikine6s(T_0,'luf');
qrad_2=KUKA_KR210_2.ikine6s(T_0,'lun');
qrad_3=KUKA_KR210_2.ikine6s(T_0,'ldf');
qrad_4=KUKA_KR210_2.ikine6s(T_0,'ldn');
qrad_5=KUKA_KR210_2.ikine6s(T_0,'ruf');
qrad_6=KUKA_KR210_2.ikine6s(T_0,'run');
qrad_7=KUKA_KR210_2.ikine6s(T_0,'rdf');
qrad_8=KUKA_KR210_2.ikine6s(T_0,'rdn');

T_1=KUKA_KR210_2.fkine(qrad_1)
T_2=KUKA_KR210_2.fkine(qrad_2)
T_3=KUKA_KR210_2.fkine(qrad_3)
T_4=KUKA_KR210_2.fkine(qrad_4)
T_5=KUKA_KR210_2.fkine(qrad_5)
T_6=KUKA_KR210_2.fkine(qrad_6)
T_7=KUKA_KR210_2.fkine(qrad_7)
T_8=KUKA_KR210_2.fkine(qrad_8)

但是这样只有前3*3代表的腕部姿态部分是对的，第4列对应的腕点位置却与初始给定的不同，且8组逆解位置也各有不同。
不明白这里是什么原因？我的DH模型建错了吗？

Answer 1

在构建SDH模型的过程中，需要注意以下几点：

关节坐标系的选择：根据机器人的实际结构，选择合适的关节坐标系。在这个例子中，你选择了标准的SDH模型，但是需要注意的是，SDH模型只是一种理论模型，实际机器人的结构并不一定符合SDH模型，因此需要根据具体情况进行选择。

关节角度的定义：关节角度的定义需要与机器人的实际运动一致，例如在旋转关节中，一般采用右手法则定义旋转方向；在平移关节中，一般采用正方向为关节向外伸出的方向。

齐次变换矩阵的计算：齐次变换矩阵需要按照正确的顺序进行乘法运算，例如，在计算从基坐标系到末端执行器坐标系的变换矩阵时，需要按照从基坐标系到第一个关节坐标系、从第一个关节坐标系到第二个关节坐标系，以此类推，直到从倒数第二个关节坐标系到末端执行器坐标系的顺序进行乘法运算。

逆解的计算：ikine6s函数只能计算六自由度的机器人，而在这个例子中，机器人有六个旋转关节，因此可以直接使用ikine函数进行求解，例如ikine函数可以求解出所有逆解，但是需要注意，ikine函数求解的逆解不一定是最优解，因此需要根据具体情况进行选择。

根据你提供的信息，无法确定问题出现的具体原因，可能是SDH模型建立有误，也可能是计算逆解的方法有误。建议检查代码中的细节，尤其是关节坐标系的选择和逆解的计算方法，以确定问题所在。