这是逆运动学的奇异之处还是不正确的实现？

我此刻试图计算将我的机器人从此变换矩阵描述的当前状态中移出的Q配置。 />对于这种旋转

0.00549713  0.842013  -0.539429  
0.999983 -0.00362229 0.00453632 
0.00186567 -0.539445 -0.842019
                    

和这个位置

-0.0882761
-0.255069
 0.183645

由于Z方向的急剧变化，我认为我可以将起点和终点之间的路径分成小块通过插值在它们之间创建数据，并为每个小位置计算逆运动学。问题是我得到的输出相当大。这使我怀疑某些输出可能是错误的。我正在使用的模拟将旋转限制为360度。。我认为出了点问题。我以为我遇到了单一性问题。 （UR5））
更新
我正在使用球体来计算所需的变换矩阵。这个想法是，我希望我的手臂位于此球体上，指向内部中心。 pre class =“ lang-cpp prettyprint-override”> setQ
此函数为我提供了球体上的点，该点用于使用std::vector<Transform3D<>> pathPlanning::sphere(double dx, double dy, double dz) { double r = 5.0; // Radius of the sphere - set to 5.0 cm (TODO: has to be checked if that also is accurate) cout << "Create a sphere" << endl; double current_x = this->device->baseTframe(this->toolFrame,this->state).P()[0]; double current_y = this->device->baseTframe(this->toolFrame,this->state).P()[1]; double current_z = this->device->baseTframe(this->toolFrame,this->state).P()[2]; // Formula for sphere (x-x0)²+(y-y0)²+(z-z0)²=r² // x: x = x_0 + rcos(theta)sin(phi) // y: y = y_0 + rsin(theta)sin(phi) // z: z = z_0 + rcos(phi) // Angle range: 0 <= theta <= 2M_PI ; 0 <= phi <= M_PI double obj_x = current_x + dx; double obj_y = current_y + dy; double obj_z = current_z + dz; std::vector<Transform3D<>> out; int count = 32; for(double azimuthal = 0; azimuthal <= M_PI ; azimuthal+=0.01 ) { for(double polar = 0.35; polar <= M_PI-0.35 ; polar+=0.01 ) { double sphere_x = obj_x + r*cos(azimuthal)*sin(polar); double sphere_y = obj_y + r*sin(azimuthal)*sin(polar); double sphere_z = obj_z + + r*cos(polar); //string text = to_string(sphere_x) + " , " + to_string(sphere_y)+ " , " + to_string(sphere_z); //positions << text << endl; Transform3D<> transformation_matrix = transform(obj_x,obj_y,obj_z,sphere_x,sphere_y,sphere_z); if(0.1<(transformation_matrix.P()[0] - current_x) || 0.1<(transformation_matrix.P()[1] - current_y) || 0.1<(transformation_matrix.P()[2] - current_z)) { cout << "Interpolate: " << endl; std::vector<Transform3D<>> transformation_i = invKin_LargeDisplacement(transformation_matrix); out.insert(out.end(),transformation_i.begin(),transformation_i.end()); cout << out.size() << endl; cout << "only returning one interpolation onto the sphere!" << endl; return transformation_i; } else { cout << "OK" << endl; out.push_back(transformation_matrix); } if(count == 32) //TODO: Why...... is this occuring? { //cout << "Theta: " << theta << " Phi: " << phi << endl; //cout << sphere_x << " , " << sphere_y <<" , "<< sphere_z << endl; count = 0; } else { count++; } } } return out; } 创建旋转矩阵。 />变换基本上计算旋转矩阵。数学是基于@Ben的帖子的，这是对我遇到的类似问题的解答。.
更新
旋转矩阵的错误是由于极坐标为0 => sin（0）=0。
我绘制了显示雅各布行列式的行列图，同时计算了大位移时的逆运动学。对于每个逆运动学迭代，我将机器人设置为新的q_i并将其用作当前值，并继续进行计算，直到达到最终配置为止。一般而言，这个数字很小。.再次，我认为奇异性可能是罪魁祸首。并从该状态执行逆运动学。Q是6个关节的关节角度。
更新
插值是通过将起点到终点的路径线性划分为指定数量的数据点来完成的。 br />
该图显示了由插值生成的每个变换矩阵及其位置部分。红点是路径（每1000个位置）。蓝色的球是要跟踪的对象，绿色的点表示球体。由于我仅对球体上的第一个点执行此操作，因此它仅击中球体上的一个点，即最高点图也显示了。
旋转没有显示太大的变化，这也使当前旋转和所需旋转之间存在基于意义的差异。
我的InvKin大位移实现：
如图所示，我很确定我的插值有效。另一方面，我的逆运动学并不十分确定。.
更新
@Chuck在回答中提到，检查核心功能是个好主意，这可能会为将来的工作提供一些启示错误。
我用一个inv.kin函数试了一下，我知道它可以工作，但是没有返回任何结果，这使我怀疑我创建的转换函数是否正确？上面显示的是机器人仿真。.上面显示的transform函数是我用来计算所需的函数并提供我的逆运动学的函数。是否设置不正确？ br /> @ Chuck针对我的问题提出了另一种方法，该职位只有3个自由度。我选择更改轨迹，并给定距离dx，dy，dz来执行简单的逆运动学。出于某种原因，哪个对我不太好？即使是很小的差异，......这是我的代码：

    0  0.942755 -0.333487
    1         0         0
    0 -0.333487 -0.942755

输出此代码

8.66654
19.809
115.771

Transform是将机器人移至所需状态的状态。 。
我的实现方式有问题，或者是一个奇异之处。.
尤其是因为我移动的幅度不大（0.00001）！我已经解决了这个奥秘。.必须是球形函数，该函数创建了机器人无法触及的点。!!