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

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

img

img

然后给定一个位姿，正运动学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模型建错了吗？

img


img

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

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

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

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

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

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

该回答引用ChatGPT

问题可能在于您的SDH模型定义中出现了错误。根据您提供的代码，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');

这里的参数分别对应了机器人的6个关节，但是这些参数的定义不太符合机器人的实际情况。例如，第一个关节的alpha参数为-pi/2，而第四个关节的alpha参数为-pi/2，这样就导致了机器人的第三个关节和第四个关节重合在一起，这显然是不合理的。

您可以尝试根据机器人的实际参数重新定义SDH模型，并且使用matlab内置的工具（例如robotics.RigidBodyTree）来检验模型的正确性。这样，您就可以确定SDH模型的正确性，从而避免逆解出现问题。

方案来自 梦想橡皮擦 狂飙组基于 GPT 编写的 “程秘”

从你的代码和模型来看，你的SDH模型是正确的，因此可能是代码的问题导致位置不正确。

关于位置错误的问题，可能是以下原因导致：

首先，可能是因为你的初始位置不在机器人的工作空间内。这种情况下，ikine6s函数会找不到逆解。你可以通过调整初始位姿位置或者扩大机器人的工作空间，从而解决这个问题。

另一种可能的原因是ikine6s函数求解逆解时，因为机器人的特殊结构，可能存在奇异点。当机器人处于奇异点附近时，逆解可能不稳定，因此可能会得到错误的逆解。这种情况下，你可以通过增加约束条件，使得求解的逆解更加稳定。

建议你检查一下机器人的工作空间和约束条件，确认它们是否设置正确。


import numpy as np
import matplotlib.pyplot as plt
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar
from matplotlib.widgets import Cursor
from PyQt5.QtWidgets import QApplication, QMainWindow, QVBoxLayout, QSizePolicy, QWidget

class ApplicationWindow(QMainWindow):
    def __init__(self):
        super().__init__()
        self.setWindowTitle("Matplotlib Crosshair Example")

        # Create a figure and add a subplot
        self.fig, self.ax = plt.subplots()
        self.canvas = FigureCanvas(self.fig)
        self.toolbar = NavigationToolbar(self.canvas, self)

        # Add the subplot to the figure
        t = np.arange(0.0, 1.0, 0.01)
        s = np.sin(2 * np.pi * t)
        self.ax.plot(t, s)
        self.ax.set_xlabel('Time (s)')
        self.ax.set_ylabel('Signal')

        # Add the cursor to the subplot
        self.cursor = Cursor(self.ax, useblit=True, color='red', linewidth=1)

        # Set up the layout and show the window
        layout = QVBoxLayout()
        layout.addWidget(self.toolbar)
        layout.addWidget(self.canvas)
        widget = QWidget()
        widget.setLayout(layout)
        self.setCentralWidget(widget)

if __name__ == '__main__':
    app = QApplication([])
    window = ApplicationWindow()
    window.show()
    app.exec_()

该示例代码创建了一个包含一个Matplotlib子图和一个十字光标的PyQt5窗口。在这个例子中，我们首先创建了一个figure并添加了一个subplot。然后，我们在subplot中绘制了一个简单的正弦波，并为x轴和y轴设置了标签。

接下来，我们使用Matplotlib的Cursor工具创建了一个十字光标，并将其添加到subplot中。最后，我们将Matplotlib画布和Matplotlib导航工具栏添加到PyQt5窗口的布局中，并显示窗口。

在这个例子中，我们使用了Matplotlib的默认十字光标样式，将其颜色设置为红色。如果你想自定义十字光标的样式，可以通过调整Cursor工具的参数来实现。