【Roban教程】双足步行基础——机器人运动学2

逆运动学

正运动学是已知机器人运动学模型的情况下，根据机器人各关节角度求各连杆的位姿。逆运动学则相反，是确定想要某连杆达到的预期位姿，根据预期位姿求解该状态下机器人各关节角度。一般是给出机器人脚掌或手掌连杆的预期位姿，然后求解各个关节角度。

1.逆运动学的解析方法

在机器人鹘关节轴相交、踝关节两轴相交的情况下，可以比较简单地用解析法求解腿上各个关节的转角。如图1所示，定义从躯干坐标系的原点到鹘关节的距离为D，大腿长为A，小腿长为B。给定躯干和右脚的位姿分别为 (p1，R1)和（p7，R7）。

图1  解析法求解逆运动学

此时依次求得鹘关节位置为

式（1）

踝关节坐标系下的鹘关节位置矢量为

式(2)

踝关节与鹘关节之间的距离C为

式(3)

根据三角形余弦定理，有

式(4)

从而求得膝关节角q5为

式(5)

基于求得的q5和r_x，r_y，r_z，可以再次根据三角形原理求得踝关节的关节角度q6、q7分别为

式(6)

其中

式(7)

最后再求得鹘关节q2，q3，q4 分别为

式(8)

其中R_ij为如下矩阵的元素，其中为了简写，c2=cosq2, s2=sinq2。

式（9）

式（10)

以上即为逆运动学的解析解法，需要注意的是该样例下机器人鹘关节三轴相交、踝关节两轴相交，这会大大减小解析法计算的复杂程度。实际的机器人运动控制过程中，一般使用数值解法进行逆运动学求解。

2.逆运动学的数值解法

解析解法求解逆运动学原理简单计算量小，但其应用的局限性较大。比如对于一些特殊构型的机器人，逆运动学可能得不到解析解。数值解法则适用范围更广，虽然迭代计算求解需要更大的计算 ，但现有的计算芯片可以轻易满足逆运动学数值解法的计算需求。

首先考虑一六自由度的运动机构，因为不管是机械臂还是机械腿，在逆解时都可以视为同样的对象。简单的逆解情况，可以认为逆解时机构的端固定另一端运动。固定的一端称为基座，运动的一端称为末端。末端在三维空间中运动时，其位姿也在变化，我们称末端在“笛卡儿空间”中运动。随着末端在空间中运动，机构的各个关节的转动角度也在变化。末端的每一个空间姿态，对应有相应的各个关节转动角度，我们称由各个关节转动角度构成的矢量在“关节空间”中运动。于是正解是把关节空间的关节转角矢量转化为笛卡儿空间的末端位姿，逆解则是反过来从末端位姿求解关节转角矢量，数学式的表达则为：

式（11）

其中x为末端的空间位置和朝向角，q为关节转角矢量。

式（12）

遗憾的是没有办法具体地写出上式中函数f（）的形式，所以不能直接使用函数计算正逆解。不过可以通过另一个可求得的矩阵，即雅可比矩阵，来进行逆解的求解。雅可比矩阵的含义是把运动链末端点的笛卡尔空间速度，映射到关节空间的关节转角速度，其数学形式为：

式（13）

根据上式可以认为末端点在笛卡尔空间中有一个位移Δx时，对应的关节转角矢量有一个变化的差值 Δq。当位移Δx越小，其与Δq的关系就越符合上式。图2展示了在位移大小变化时，由Δq产生的实际末端轨迹与理想位移轨迹的偏差，可以看出位移越小时二者轨迹偏差越小。

图2  笛卡尔空间位移与关节位移

利用该关系来进行逆解求解的过程为，首先计算运动机构的当前位姿与预期位姿的差值dx和机构当前位姿下的雅可比矩阵J，然后利用式（13）计算对应的关节转角增量dq，接着把增量dq加到当前关节转角上重新计算新的运动机构位姿。显然新的位姿会比原先的位姿更接近预期位姿。以上流程经过多次迭代，使最后求得的位姿偏差接近到符合预先设定的精度要求时，即认为求解成功。

其中根据式（13）计算关节转角增量dq时，自然而然地做法是求取雅可比矩阵J的逆，从而得到 dq。但在许多情况下，因为运动机构构型或者机构位姿的差异，雅可比矩阵是不可逆的。为了处理该问题，根据不同的处理手段又引申出了雅可比转置法、伪逆法、奇异值分解法等逆解的数值解法，本节不再详细介绍。

下一篇来以 matlab 软件的机器人工具箱中的 ikine( ）逆解函数为例，说明上述计算方法的编程实现流程，具体见下一篇。