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˙​，却不能计算矩阵CCC，因此产生了自己写一个算法的想法。

算法简介

参考

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

算法思路

科氏力矩矩阵元素的表达式为

Cpj=∑i=1n∑k=1i−1[tr(∂2Ti∂qj∂qkIi∂TiT∂qp)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]}} Cpj​=i=1∑n​k=1∑i−1​[tr(∂qj​∂qk​∂2Ti​​Ii​∂qp​∂TiT​​)q˙​k​]

可将其简化表示为

Cpj=∑k=1napjkq˙k\boldsymbol{C}_{pj}=\sum_{k=1}^n{a_{pjk}\dot{q}_k} Cpj​=k=1∑n​apjk​q˙​k​

那么，科氏力矩可表示为如下较为直观的形式

[∑k=1na11kq˙k∑k=1na12kq˙k⋯∑k=1na1nkq˙k∑k=1na21kq˙k∑k=1na22q˙k⋯⋮⋮⋮⋱⋮∑k=1nan1kq˙k⋯⋯∑k=1nannkq˙k][q˙1q˙2q˙3⋮q˙n−1q˙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=1n​a11k​q˙​k​∑k=1n​a21k​q˙​k​⋮∑k=1n​an1k​q˙​k​​∑k=1n​a12k​q˙​k​∑k=1n​a22​q˙​k​⋮⋯​⋯⋯⋱⋯​∑k=1n​a1nk​q˙​k​⋮⋮∑k=1n​annk​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=[a111a211⋮an11]\boldsymbol{\tau }_1=\left[ \begin{array}{c} a_{111}\\ a_{211}\\ \vdots\\ a_{n11}\\ \end{array} \right] τ1​=⎣⎢⎢⎢⎡​a111​a211​⋮an11​​⎦⎥⎥⎥⎤​

可知，为第一列所有元素ap11a_{p11}ap11​组成的列向量。

通过循环，便可得到所有元素中的apjja_{pjj}apjj​分量，组成矩阵 CsqC_{sq}Csq​，此分量是由于连杆的离心运动形成的。

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

令关节角速q˙=ei˙+ej˙\bm{\dot{q}}=\dot{e_i}+\dot{e_j}q˙​=ei​˙​+ej​˙​ (第i, j 个元素为1，其余都为0)，计算此时的科氏力矩。

以i=1,j=2时情况进行举例分析，通过InverseDynamic函数计算得到的科氏力矩为

τ12=[a111+a112+a121+a122a211+a212+a221+a222⋮an11+an12+an21+an22]=τ1+τ2+[a112+a121a212+a221⋮an12+an21]\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] τ12​=⎣⎢⎢⎢⎡​a111​+a112​+a121​+a122​a211​+a212​+a221​+a222​⋮an11​+an12​+an21​+an22​​⎦⎥⎥⎥⎤​=τ1​+τ2​+⎣⎢⎢⎢⎡​a112​+a121​a212​+a221​⋮an12​+an21​​⎦⎥⎥⎥⎤​

注意此时，τ1τ2\tau_1 \tau_2τ1​τ2​已经在上一步骤中计算得到。

同时，通过分析C矩阵元素的表达式，可知apjk=apkja_{pjk}=a_{pkj}apjk​=apkj​ ，进而得到

[a112a212⋮an12]q˙2=τ12−τ1−τ22q˙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 ⎣⎢⎢⎢⎡​a112​a212​⋮an12​​⎦⎥⎥⎥⎤​q˙​2​=2τ12​−τ1​−τ2​​q˙​2​

可知，上式为第一列所有元素的第二项ap12q2˙a_{p12}\dot{q_2}ap12​q2​˙​分量组成的列向量。

同样，可得到第二列所有元素的第一项ap21q1˙a_{p21}\dot{q_1}ap21​q1​˙​分量组成的列向量。

进行循环，遍历j至n，可得到第一列所有元素的 ap1jqj˙a_{p1j}\dot{q_j}ap1j​qj​˙​组成的列向量。将其累加，得到与第一列所有由于牵连运动导致的分量之和。同时，可得到第j列所有元素的apj1q1˙a_{pj1}\dot{q_1}apj1​q1​˙​ 分量组成的列向量。

进行循环，遍历i至n，得到所有列由于牵连运动导致的分量之和，组成矩阵C2C_2C2​ 。

分量叠加

将离心运动导致的分量与牵连运动导致的分量叠加，得到最后的矩阵C：

C=C2+Csq∗diag(q˙)\boldsymbol{C}=\boldsymbol{C}_2+\boldsymbol{C}_{sq}*\text{diag}\left( \boldsymbol{\dot{q}} \right) C=C2​+Csq​∗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^2for j=1:NQD = 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^2for j=1:Nfor k=j+1:Nif k>Nbreak;end% find a product term qd_j * qd_kQD = 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;endendC = C + Csq * diag(qd);end