目录
基本知识
雅可比
雅可比矩阵 J 可以完成由关节角速度 q_dot 向终端速度 w 的映射
但是在大多数情况下，我们更倾向于通过规划得到的终端速度 w 去规划关节速度 q_dot ，如果矩阵 J 可逆的情况下，通过简单的对在两侧左乘 J的逆矩阵即可。不过在大多数情况下，J 是不可逆的通常通过伪逆来实现：
零空间与优先级
对于有冗余的系统（如上式）通常有无穷多的解，加入零空间矩阵 N , 其满足 ，便可以通过实现如下公式实现任意改变
，也不会影响上式中的控制中对终端运动 w 的控制。也就是说下式中
与
的控制关系不会受到
影响，所以相比之下
拥有更高的优先级。
最简单的零空间矩阵 N 的可以选取为
多任务逆运动学控制
多任务同等优先级
当存在有 个任务具有相同的优先级，通常采用如下方法求取广义速度
多任务优先级控制
上面讨论过了据有更高优先级的控制矩阵，下面是第一优先级任务的控制
为了保证第一个任务的优先级，第二个任务由下式计算得：
计算出 并带入第一个任务
matlab计算实例
实例描述了一个平面三自由度机械臂，每段连杆的长度视为1，考虑在某时刻 t 时，三个关节的关节角度分别为 pi/6 , pi/3 , pi/3 ，此时想要给关节末端速度为[1 , 1]，求解此时的关节角速度。
题中单任务的求解
% 目标矩阵，实现目标速度任务
W_E = [1 1]'
% 雅可比矩阵
J_E = 0.5*[0 -sqrt(3) -sqrt(3); -4 -3 -1 ]
q_dot1 = pinv(J_E) * W_E
%将求得的q_dot1结果反推，可以看出是没有误差的
J_E * q_dot1
%以下为计算结果
q_dot1 =
0.0688
-0.5602
-0.5945
J_E * q_dot1 =
1.0000
1.0000多任务无优先级
在上述问题的基础上，额外添加一个任务
此时的问题可以由如下公式求解
% 目标1矩阵，实现目标速度任务
W_E = [1 1]'
% 雅可比矩阵
J_E = 0.5*[0 -sqrt(3) -sqrt(3); -4 -3 -1 ]
q_dot1 = pinv(J_E) * W_E
% 目标2矩阵，实现目标速度任务
W_j = [0 ; 0]
% 为了让关节1和2速度是零
J_j = [1 0 0;
0 1 0]
q_dot2 = pinv([J_E;J_j])*[W_E;W_j]
% 计算两次不同情况的范数判断误差
% 只考虑任务1的计算结果，可见其对任务1进行了完全的满足，误差为0
norm(W_E - J_E*q_dot1,2)^2 = 1.9722e-31
norm(W_j - J_j*q_dot1,2)^2 = 0.3185
% 多任务无优先级的计算结果，总误差相对变小了，说明统筹考虑了两个任务的要求
norm(W_E - J_E*q_dot2,2)^2 = 0.0059
norm(W_j - J_j*q_dot2,2)^2 = 0.0223
多任务优先级控制
在第二问的前提下，如果考虑任务1的优先级高于任务2的优先级，通常采用下述公式
% 目标1矩阵，实现目标速度任务
W_E = [1 1]'
% 雅可比矩阵
J_E = 0.5*[0 -sqrt(3) -sqrt(3); -4 -3 -1 ]
% 目标2矩阵，实现目标速度任务
W_j = [0 ; 0]
% 为了让关节1和2速度是零
J_j = [1 0 0;
0 1 0]
J_1 = J_E;
J_2 = J_j;
W1 = W_E;
W2 = W_j;
% N1 = diag([1 1 1]) - pinv(J_1)*J_1;
N1 = [1 -2 2 ;-2 4 -4 ;2 -4 4]/9
q_dot3 = pinv(J_1)*W1 + N1*pinv(J_2*N1)*(W2 - J_2*pinv(J_1)*W1)
% 最终的计算结果计算误差，可以发现其在前两问的基础上，
% 不仅满足了任务1的要求，同时尽可能的减小了任务二的误差
norm(W_E - J_E*q_dot3,2)^2 = 1.9722e-31
norm(W_j - J_j*q_dot3,2)^2 = 0.0357
参考：ETH动力学讲义