ETH机器人动力学讲义：基于零空间方法的全身运动控制（WBC）-1

基本知识

雅可比

零空间与优先级

多任务逆运动学控制

多任务同等优先级

多任务优先级控制

matlab计算实例

题中单任务的求解

多任务无优先级

多任务优先级控制

基本知识

雅可比

雅可比矩阵 J 可以完成由关节角速度 q_dot 向终端速度 w 的映射

但是在大多数情况下，我们更倾向于通过规划得到的终端速度 w 去规划关节速度 q_dot ，如果矩阵 J 可逆的情况下，通过简单的对在两侧左乘 J的逆矩阵即可。不过在大多数情况下，J 是不可逆的通常通过伪逆来实现：

零空间与优先级

对于有冗余的系统（如上式）通常有无穷多的解，加入零空间矩阵 N , 其满足 $J_{e0}N = 0$ ，便可以通过实现如下公式实现任意改变 $\dot{q_{0}}$ ，也不会影响上式中的控制中对终端运动 w 的控制。也就是说下式中 $\dot{q}$ 与 $w_e$ 的控制关系不会受到 $\dot{q_{0}}$ 影响，所以相比之下 $\dot{q}$ 拥有更高的优先级。

最简单的零空间矩阵 N 的可以选取为 $N = \mathbb{I} - J_{e0}^{+}J_{e0}$

多任务逆运动学控制

多任务同等优先级

当存在有 $n_{t}$ 个任务具有相同的优先级，通常采用如下方法求取广义速度

多任务优先级控制

上面讨论过了据有更高优先级的控制矩阵，下面是第一优先级任务的控制

为了保证第一个任务的优先级，第二个任务由下式计算得：

计算出 $\dot{q_{0}}$ 并带入第一个任务

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.5945J_E * q_dot1 =1.00001.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]/9q_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动力学讲义

(10条消息) ETH：RobotDynamicsLectureNotes-智慧交通文档类资源-CSDN文库