4、逆运动学（IK）：使用ikine和ikcon函数求解逆解，讨论多解、奇异位形及求解策略

逆运动学，说白了就是已知机器人手爪要到达的位置和姿态，反推各个关节该转多少角度。这问题听起来简单，做起来可没那么轻松。我刚开始做机器人控制时，觉得正运动学算起来麻烦，逆运动学应该更直接才对——结果第一次跑ikine就给我来了个下马威。

4.1 逆运动学的基本概念

正运动学是已知关节角度求末端位姿，逆运动学正好反过来。用数学语言说：已知末端齐次变换矩阵 T，求关节角度 q，使得 f(q) = T。

这里有个关键问题：正运动学通常有唯一解，但逆运动学往往多解。你想想看，一个六轴机器人从A点走到B点，手肘可以朝上也可以朝下，这不就两种姿势了吗？

4.2 ikine函数：数值法求解

MATLAB Robotics Toolbox提供了ikine函数，它用的是数值迭代法。我个人习惯先试试这个，因为它通用性强，什么构型的机器人都能算。

% 创建一个六轴机器人
robot = loadrobot('kinovaGen3', 'DataFormat', 'row');
% 设定目标位姿
targetPose = trvec2tform([0.5, 0.2, 0.3]) * eul2tform([0, pi/4, 0]);
% 初始猜测（很重要！）
initialGuess = [0, 0, 0, 0, 0, 0];
% 求解逆解
[q_solution, solution_info] = ikine(robot, targetPose, initialGuess, [1 1 1 1 1 1]);

这里有个坑——初始猜测直接影响求解结果。我在项目中遇到过，同样的目标位姿，换个初始值就收敛到不同的解。所以实际使用时，我通常会跑多个初始值，然后选一个最合理的。

4.3 ikcon函数：带约束的求解

ikcon是ikine的升级版，它允许你给关节角度加约束。比如有些关节不能转超过±90°，或者你想让某个关节保持在特定范围内。

% 设置关节约束（弧度）
joint_limits = [-pi/2, pi/2;   % 关节1
                -pi/3, pi/3;   % 关节2
                -pi, pi;       % 关节3
                -pi/2, pi/2;   % 关节4
                -pi/3, pi/3;   % 关节5
                -pi, pi];      % 关节6

% 带约束求解
[q_constrained, info] = ikcon(robot, targetPose, initialGuess, joint_limits);

ikcon内部用的是优化算法，它会尽量让解落在约束范围内。嗯，这里要注意：约束设得太紧可能导致无解，设得太松又失去约束意义。我一般先跑一次ikine看看自然解的范围，再据此设定约束。

4.4 多解问题与选择策略

多解是逆运动学的家常便饭。一个六轴机器人，理论上最多可以有16组逆解。但实际中，我们通常只关心其中几组。

解的类型	特点	适用场景
肘部向上/向下	手肘位置不同	避障时切换
腕部翻转	手腕姿态不同	避免关节限位
肩部左/右	整体构型不同	工作空间边缘

选择策略上，我常用的方法是：

• 最短路径法：选离当前关节角度最近的那组解
• 能量最优法：选关节变化总和最小的解
• 避障优先法：选不与环境碰撞的解

曾经有一次，我调试焊接机器人时发现它总在某个点抖动。后来查出来是每次求解都选了不同的解，导致关节来回切换。解决办法很简单——锁定解的类型，比如强制用肘部向上的解。

4.5 奇异位形：避坑指南

奇异位形是逆运动学的噩梦。当机器人处于奇异位形时，某个方向的运动速度会变得极大，甚至无穷大。说白了，就是关节无论怎么转，末端都动不了某个方向。

常见的奇异位形有三种：

• 边界奇异：手臂完全伸直或折叠
• 腕部奇异：腕部两个关节轴线重合
• 肩部奇异：肩关节与腕关节共线

怎么检测奇异？可以用雅可比矩阵的条件数来判断。条件数越大，越接近奇异。

% 计算雅可比矩阵
J = geometricJacobian(robot, q_current, 'end_effector');
% 计算条件数
cond_num = cond(J);
% 如果条件数很大，说明接近奇异
if cond_num > 1000
    warning('接近奇异位形！');
end

4.6 求解策略总结

说了这么多，实际项目中该怎么选？我总结了几条经验：

1. 优先用ikcon：带约束的求解更安全，不容易出界
2. 多初始值尝试：至少试3-5个不同的初始猜测
3. 实时监测条件数：发现奇异及时调整轨迹
4. 记录历史解：保持解的连续性，避免跳变

你想想看，逆运动学其实是个优化问题。我们不仅要找到解，还要找到好的解。好的解意味着：关节运动平滑、不碰奇异、不超限位。