想法来源

在设计机械臂控制算法的时候，需要机械臂科氏力矩$C(\mathbf{q},\dot{\mathbf{q}})$的信息，但是MATLAB官方的Robotic System Toolbox工具箱只能计算科氏力矩$C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}$，却不能计算矩阵$C$，因此产生了自己写一个算法的想法。

算法简介

参考

计算过程参考自澳大利亚学者Peter Corke的工具箱Robotics Toolbox for MATLAB 中的科氏矩阵函数SerialLink.coriolis()。而Peter Corke是参考的论文《Efficient Dynamic Computer Simulation of Robotic Mechanisms》中的算法。

算法思路

科氏力矩矩阵元素的表达式为
$$C_{pj}=\sum _{i=1}^n\sum _{k=1}^{i-1}\left[tr\left(\frac{{\partial }^2{\mathbf{T}}_i}{\partial q_j\partial q_k}{I}_i\frac{\partial {\mathbf{T}}_i^{\text{T}}}{\partial q_p}\right){\dot{q}}_k\right]$$
可将其简化表示为
$${\mathbf{C}}_{pj}=\sum _{k=1}^n{a}_{pjk}{\dot{q}}_k$$
那么，科氏力矩可表示为如下较为直观的形式
$$\left[\begin{array}{cccc}{\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 & ⋮\\ ⋮& ⋮& \ddots & ⋮\\ {\sum }_{k=1}^n{a}_{n1k}{\dot{q}}_k& \cdots & \cdots & {\sum }_{k=1}^n{a}_{nnk}{\dot{q}}_k\end{array}\right]\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\\ ⋮\\ {\dot{q}}_{n-1}\\ {\dot{q}}_n\end{array}\right]$$
可以看到，只要将所有的元素 计算出来，就可以计算矩阵C。
刚性机械臂的动力学方程为
$$\mathbf{M}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\mathbf{C}\left(\mathbf{q},\dot{\mathbf{q}}\right)\dot{\mathbf{q}}+\mathbf{G}\left(\mathbf{q}\right)=\mathbf{\tau }$$
我们手头可用的工具为MATLAB自带的机器人工具箱，其中的inverseDynamics函数可由等式左边(关节角位置、速度)，计算等式右边，即需要的外部力矩。
我们将关节角加速度 以及重力力矩 以及摩擦力矩等外部力矩置为零， inverseDynamics函数的输出便为科氏力矩 (这也是工具箱中计算科氏力矩的方法)，根据科氏力矩，我们便可以计算科氏矩阵 。
接下来便详细介绍如何通过inverseDynamics函数计算科氏矩阵。

计算过程

计算向心运动导致的分量

首先，令关节角速度$\dot{\mathbf{q}}=\dot{e}$ (第i个元素置为1，其余都为0)，计算此时的科氏力矩。
对i=1时进行分析：
此时的通过InverseDynamic函数计算得到的科氏力矩为
$${\mathbf{\tau }}_1=\left[\begin{array}{c}{a}_{111}\\ a_{211}\\ ⋮\\ a_{n11}\end{array}\right]$$
可知，为第一列所有元素$a_{p11}$组成的列向量。
通过循环，便可得到所有元素中的$a_{pjj}$分量，组成矩阵 $C_{sq}$，此分量是由于连杆的离心运动形成的。

计算由牵连运动导致的分量

令关节角速$\dot{\mathbf{q}}=\dot{e_i}+\dot{e_j}$ (第i, j 个元素为1，其余都为0)，计算此时的科氏力矩。
以i=1,j=2时情况进行举例分析，通过InverseDynamic函数计算得到的科氏力矩为
$${\mathbf{\tau }}_{12}=\left[\begin{array}{c}{a}_{111}+a_{112}+a_{121}+a_{122}\\ a_{211}+a_{212}+a_{221}+a_{222}\\ ⋮\\ a_{n11}+a_{n12}+a_{n21}+a_{n22}\end{array}\right]={\mathbf{\tau }}_1+{\mathbf{\tau }}_2+\left[\begin{array}{c}{a}_{112}+a_{121}\\ a_{212}+a_{221}\\ ⋮\\ a_{n12}+a_{n21}\end{array}\right]$$
注意此时，${\tau }_1{\tau }_2$已经在上一步骤中计算得到。
同时，通过分析C矩阵元素的表达式，可知$a_{pjk}=a_{pkj}$ ，进而得到
$$\left[\begin{array}{c}{a}_{112}\\ a_{212}\\ ⋮\\ a_{n12}\end{array}\right]{\dot{q}}_2=\frac{{\mathbf{\tau }}_{12}-{\mathbf{\tau }}_1-{\mathbf{\tau }}_2}{2}{\dot{q}}_2$$
可知，上式为第一列所有元素的第二项$a_{p12}\dot{q_2}$分量组成的列向量。
同样，可得到第二列所有元素的第一项$a_{p21}\dot{q_1}$分量组成的列向量。
进行循环，遍历j至n，可得到第一列所有元素的 $a_{p1j}\dot{q_j}$组成的列向量。将其累加，得到与第一列所有由于牵连运动导致的分量之和。同时，可得到第j列所有元素的$a_{pj1}\dot{q_1}$ 分量组成的列向量。
进行循环，遍历i至n，得到所有列由于牵连运动导致的分量之和，组成矩阵$C_2$ 。

分量叠加

将离心运动导致的分量与牵连运动导致的分量叠加，得到最后的矩阵C：
$$\mathbf{C}={\mathbf{C}}_2+{\mathbf{C}}_{sq}\ast \text{diag}\left(\dot{\mathbf{q}}\right)$$
计算结束。

代码

简单起见，直接调用了工具箱的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