MATLAB Robotic System Toolbox 机械臂科氏矩阵算法
想法来源
在设计机械臂控制算法的时候，需要机械臂科氏力矩
C
(
q
,
q
˙
)
C(\bm{q},\dot{\bm{q}})
C
(
q
,
q
˙
)
的信息，但是MATLAB官方的Robotic System Toolbox工具箱只能计算科氏力矩
C
(
q
,
q
˙
)
q
˙
C(\bm{q},\dot{\bm{q}})\dot{\bm{q}}
C
(
q
,
q
˙
)
q
˙
，却不能计算矩阵
C
C
C
，因此产生了自己写一个算法的想法。
算法简介
参考
计算过程参考自澳大利亚学者Peter Corke的工具箱Robotics Toolbox for MATLAB 中的科氏矩阵函数SerialLink.coriolis()。而Peter Corke是参考的论文《Efficient Dynamic Computer Simulation of Robotic Mechanisms》中的算法。
算法思路
科氏力矩矩阵元素的表达式为
C
p
j
=
∑
i
=
1
n
∑
k
=
1
i
−
1
[
t
r
(
∂
2
T
i
∂
q
j
∂
q
k
I
i
∂
T
i
T
∂
q
p
)
q
˙
k
]
C_{pj}=\sum_{i=1}^n{\sum_{k=1}^{i-1}{\left[ tr\left( \frac{\partial ^2\boldsymbol{T}_i}{\partial q_j\partial q_k}I_i\frac{\partial \boldsymbol{T}_{i}^{\text{T}}}{\partial q_p} \right) \dot{q}_k \right]}}
C
p
j
=
i
=
1
∑
n
k
=
1
∑
i
−
1
[
t
r
(
∂
q
j
∂
q
k
∂
2
T
i
I
i
∂
q
p
∂
T
i
T
)
q
˙
k
]
可将其简化表示为
C
p
j
=
∑
k
=
1
n
a
p
j
k
q
˙
k
\boldsymbol{C}_{pj}=\sum_{k=1}^n{a_{pjk}\dot{q}_k}
C
p
j
=
k
=
1
∑
n
a
p
j
k
q
˙
k
那么，科氏力矩可表示为如下较为直观的形式
[
∑
k
=
1
n
a
11
k
q
˙
k
∑
k
=
1
n
a
12
k
q
˙
k
⋯
∑
k
=
1
n
a
1
n
k
q
˙
k
∑
k
=
1
n
a
21
k
q
˙
k
∑
k
=
1
n
a
22
q
˙
k
⋯
⋮
⋮
⋮
⋱
⋮
∑
k
=
1
n
a
n
1
k
q
˙
k
⋯
⋯
∑
k
=
1
n
a
n
n
k
q
˙
k
]
[
q
˙
1
q
˙
2
q
˙
3
⋮
q
˙
n
−
1
q
˙
n
]
\left[ \begin{matrix} \sum_{k=1}^n{a_{11k}\dot{q}_k}& \sum_{k=1}^n{a_{12k}\dot{q}_k}& \cdots& \sum_{k=1}^n{a_{1nk}\dot{q}_k}\\ \sum_{k=1}^n{a_{21k}\dot{q}_k}& \sum_{k=1}^n{a_{22}\dot{q}_k}& \cdots& \vdots\\ \vdots& \vdots& \ddots& \vdots\\ \sum_{k=1}^n{a_{n1k}\dot{q}_k}& \cdots& \cdots& \sum_{k=1}^n{a_{nnk}\dot{q}_k}\\ \end{matrix} \right] \left[ \begin{array}{c} \dot{q}_1\\ \dot{q}_2\\ \dot{q}_3\\ \vdots\\ \dot{q}_{n-1}\\ \dot{q}_n\\ \end{array} \right]
⎣
⎢
⎢
⎢
⎢
⎡
∑
k
=
1
n
a
1
1
k
q
˙
k
∑
k
=
1
n
a
2
1
k
q
˙
k
⋮
∑
k
=
1
n
a
n
1
k
q
˙
k
∑
k
=
1
n
a
1
2
k
q
˙
k
∑
k
=
1
n
a
2
2
q
˙
k
⋮
⋯
⋯
⋯
⋱
⋯
∑
k
=
1
n
a
1
n
k
q
˙
k
⋮
⋮
∑
k
=
1
n
a
n
n
k
q
˙
k
⎦
⎥
⎥
⎥
⎥
⎤
⎣
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎡
q
˙
1
q
˙
2
q
˙
3
⋮
q
˙
n
−
1
q
˙
n
⎦
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎤
可以看到，只要将所有的元素 计算出来，就可以计算矩阵C。
刚性机械臂的动力学方程为
M
(
q
)
q
¨
+
C
(
q
,
q
˙
)
q
˙
+
G
(
q
)
=
τ
\boldsymbol{M}\left( \boldsymbol{q} \right) \boldsymbol{\ddot{q}}+\boldsymbol{C}\left( \boldsymbol{q},\boldsymbol{\dot{q}} \right) \boldsymbol{\dot{q}}+\boldsymbol{G}\left( \boldsymbol{q} \right) =\boldsymbol{\tau }
M
(
q
)
q
¨
+
C
(
q
,
q
˙
)
q
˙
+
G
(
q
)
=
τ
我们手头可用的工具为MATLAB自带的机器人工具箱，其中的inverseDynamics函数可由等式左边(关节角位置、速度)，计算等式右边，即需要的外部力矩。
我们将关节角加速度 以及重力力矩 以及摩擦力矩等外部力矩置为零， inverseDynamics函数的输出便为科氏力矩 (这也是工具箱中计算科氏力矩的方法)，根据科氏力矩，我们便可以计算科氏矩阵 。
接下来便详细介绍如何通过inverseDynamics函数计算科氏矩阵。
计算过程
计算向心运动导致的分量
首先，令关节角速度
q
˙
=
e
˙
\bm{\dot{q}}=\dot{e}
q
˙
=
e
˙
(第i个元素置为1，其余都为0)，计算此时的科氏力矩。
对i=1时进行分析：
此时的通过InverseDynamic函数计算得到的科氏力矩为
τ
1
=
[
a
111
a
211
⋮
a
n
11
]
\boldsymbol{\tau }_1=\left[ \begin{array}{c} a_{111}\\ a_{211}\\ \vdots\\ a_{n11}\\ \end{array} \right]
τ
1
=
⎣
⎢
⎢
⎢
⎡
a
1
1
1
a
2
1
1
⋮
a
n
1
1
⎦
⎥
⎥
⎥
⎤
可知，为第一列所有元素
a
p
11
a_{p11}
a
p
1
1
组成的列向量。
通过循环，便可得到所有元素中的
a
p
j
j
a_{pjj}
a
p
j
j
分量，组成矩阵
C
s
q
C_{sq}
C
s
q
，此分量是由于连杆的离心运动形成的。
计算由牵连运动导致的分量
令关节角速
q
˙
=
e
i
˙
+
e
j
˙
\bm{\dot{q}}=\dot{e_i}+\dot{e_j}
q
˙
=
e
i
˙
+
e
j
˙
(第i, j 个元素为1，其余都为0)，计算此时的科氏力矩。
以i=1,j=2时情况进行举例分析，通过InverseDynamic函数计算得到的科氏力矩为
τ
12
=
[
a
111
+
a
112
+
a
121
+
a
122
a
211
+
a
212
+
a
221
+
a
222
⋮
a
n
11
+
a
n
12
+
a
n
21
+
a
n
22
]
=
τ
1
+
τ
2
+
[
a
112
+
a
121
a
212
+
a
221
⋮
a
n
12
+
a
n
21
]
\boldsymbol{\tau }_{12}=\left[ \begin{array}{c} a_{111}+a_{112}+a_{121}+a_{122}\\ a_{211}+a_{212}+a_{221}+a_{222}\\ \vdots\\ a_{n11}+a_{n12}+a_{n21}+a_{n22}\\ \end{array} \right] =\boldsymbol{\tau }_1+\boldsymbol{\tau }_2+\left[ \begin{array}{c} a_{112}+a_{121}\\ a_{212}+a_{221}\\ \vdots\\ a_{n12}+a_{n21}\\ \end{array} \right]
τ
1
2
=
⎣
⎢
⎢
⎢
⎡
a
1
1
1
+
a
1
1
2
+
a
1
2
1
+
a
1
2
2
a
2
1
1
+
a
2
1
2
+
a
2
2
1
+
a
2
2
2
⋮
a
n
1
1
+
a
n
1
2
+
a
n
2
1
+
a
n
2
2
⎦
⎥
⎥
⎥
⎤
=
τ
1
+
τ
2
+
⎣
⎢
⎢
⎢
⎡
a
1
1
2
+
a
1
2
1
a
2
1
2
+
a
2
2
1
⋮
a
n
1
2
+
a
n
2
1
⎦
⎥
⎥
⎥
⎤
注意此时，
τ
1
τ
2
\tau_1 \tau_2
τ
1
τ
2
已经在上一步骤中计算得到。
同时，通过分析C矩阵元素的表达式，可知
a
p
j
k
=
a
p
k
j
a_{pjk}=a_{pkj}
a
p
j
k
=
a
p
k
j
，进而得到
[
a
112
a
212
⋮
a
n
12
]
q
˙
2
=
τ
12
−
τ
1
−
τ
2
2
q
˙
2
\left[ \begin{array}{c} a_{112}\\ a_{212}\\ \vdots\\ a_{n12}\\ \end{array} \right] \dot{q}_2=\frac{\boldsymbol{\tau }_{12}-\boldsymbol{\tau }_1-\boldsymbol{\tau }_2}{2}\dot{q}_2
⎣
⎢
⎢
⎢
⎡
a
1
1
2
a
2
1
2
⋮
a
n
1
2
⎦
⎥
⎥
⎥
⎤
q
˙
2
=
2
τ
1
2
−
τ
1
−
τ
2
q
˙
2
可知，上式为第一列所有元素的第二项
a
p
12
q
2
˙
a_{p12}\dot{q_2}
a
p
1
2
q
2
˙
分量组成的列向量。
同样，可得到第二列所有元素的第一项
a
p
21
q
1
˙
a_{p21}\dot{q_1}
a
p
2
1
q
1
˙
分量组成的列向量。
进行循环，遍历j至n，可得到第一列所有元素的
a
p
1
j
q
j
˙
a_{p1j}\dot{q_j}
a
p
1
j
q
j
˙
组成的列向量。将其累加，得到与第一列所有由于牵连运动导致的分量之和。同时，可得到第j列所有元素的
a
p
j
1
q
1
˙
a_{pj1}\dot{q_1}
a
p
j
1
q
1
˙
分量组成的列向量。
进行循环，遍历i至n，得到所有列由于牵连运动导致的分量之和，组成矩阵
C
2
C_2
C
2
。
分量叠加
将离心运动导致的分量与牵连运动导致的分量叠加，得到最后的矩阵C：
C
=
C
2
+
C
s
q
∗
diag
(
q
˙
)
\boldsymbol{C}=\boldsymbol{C}_2+\boldsymbol{C}_{sq}*\text{diag}\left( \boldsymbol{\dot{q}} \right)
C
=
C
2
+
C
s
q
∗
diag
(
q
˙
)
计算结束。
代码
简单起见，直接调用了工具箱的velocityproduct函数
function C = CoriolisFcn(TreeStruct, q, qd, N)
% q为关节角位置，qd关节角速度，TreeStruct 为机械臂的RigidBodyTree类，
% N为关节数量(关节数量和刚体数量NumBodies有可能不同)
NumBodies = TreeStruct.NumBodies;
% 根据robotic toolbox for MATLAB中的coriolis函数更改而来
% tree = robotics.manip.internal.RigidBodyTree(NumBodies, TreeStruct);
tree = TreeStruct;
assert( numrows(q) == N, 'Function_by_siyong:coriolisFcn:badarg', 'Cq must have %d rows', N);
assert( numrows(qd) == N, 'Function_by_siyong:coriolisFcn:badarg', 'qd must have %d rows', N);
C = zeros(N);
Csq = cast(zeros(numel(q)), 'like', q);
% find the torques that depend on a single finite joint speed,
% these are due to the squared (centripetal) terms
%
% set QD = [1 0 0 ...] then resulting torque is due to qd_1^2
for j=1:N
QD = zeros(N,1);
QD(j) = 1;
% fExt = zeros(6, NumBodies);
% tau = inverseDynamics(tree, q, QD);
tau = velocityProduct(tree, q, QD);
Csq(:,j) = tau;
end
% find the torques that depend on a pair of finite joint speeds,
% these are due to the product (Coridolis) terms
% set QD = [1 1 0 ...] then resulting torque is due to
% qd_1 qd_2 + qd_1^2 + qd_2^2
for j=1:N
for k=j+1:N
if k>N
break;
end
% find a product term qd_j * qd_k
QD = zeros(N,1);
QD(j) = 1;
QD(k) = 1;
tau = velocityProduct(tree, q, QD);
C(:,k) = C(:,k) + (tau - Csq(:,k) - Csq(:,j)) .* qd(j)/2;
C(:,j) = C(:,j) + (tau - Csq(:,k) - Csq(:,j)) .* qd(k)/2;
end
end
C = C + Csq * diag(qd);
end