想法来源

在设计机械臂控制算法的时候，需要机械臂科氏力矩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∑nk=1∑i−1[tr(∂qj∂qk∂2TiIi∂qp∂TiT)q˙k]
可将其简化表示为
Cpj=∑k=1napjkq˙k\boldsymbol{C}_{pj}=\sum_{k=1}^n{a_{pjk}\dot{q}_k} Cpj=k=1∑napjkq˙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=1na11kq˙k∑k=1na21kq˙k⋮∑k=1nan1kq˙k∑k=1na12kq˙k∑k=1na22q˙k⋮⋯⋯⋯⋱⋯∑k=1na1nkq˙k⋮⋮∑k=1nannkq˙k⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎡q˙1q˙2q˙3⋮q˙n−1q˙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=⎣⎢⎢⎢⎡a111a211⋮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+a122a211+a212+a221+a222⋮an11+an12+an21+an22⎦⎥⎥⎥⎤=τ1+τ2+⎣⎢⎢⎢⎡a112+a121a212+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 ⎣⎢⎢⎢⎡a112a212⋮an12⎦⎥⎥⎥⎤q˙2=2τ12−τ1−τ2q˙2
可知，上式为第一列所有元素的第二项ap12q2˙a_{p12}\dot{q_2}ap12q2˙分量组成的列向量。
同样，可得到第二列所有元素的第一项ap21q1˙a_{p21}\dot{q_1}ap21q1˙分量组成的列向量。
进行循环，遍历j至n，可得到第一列所有元素的 ap1jqj˙a_{p1j}\dot{q_j}ap1jqj˙组成的列向量。将其累加，得到与第一列所有由于牵连运动导致的分量之和。同时，可得到第j列所有元素的apj1q1˙a_{pj1}\dot{q_1}apj1q1˙ 分量组成的列向量。
进行循环，遍历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

MATLAB Robotic System Toolbox 机械臂科氏矩阵算法相关推荐

matlab length_《Matlab - Robotics System Toolbox》学习笔记(2)
写作说明: 1. 本文主要记录学习 Matlab - Robotics System Toolbox[1]的过程,就其中的一些重要知识点做相关记录.方便后期供自己与他人进行学习. 2. 由于 Matl ...
二关节机械臂matlab控制,二连杆机械臂阻抗控制模拟（一）
在学习机器人动力学相关内容时看到MATLAB论坛上一个有意思的仿真项目Impedance Control for a 2-Link Robot Arm - User-interactive,一个用MA ...
matlab机械臂dh仿真,基于MATLAB Robotics Tools的机械臂仿真
基于MATLAB Robotics Tools的机械臂仿真 [摘要]在MATLAB环境下,对puma560机器人进行运动学仿真研究,利用Robotics Toolbox工具箱编制了简单的程序语句,建立 ...
adams matlab 联仿 5DOF机械臂建模以及联合仿真
adams matlab 联仿 5DOF机械臂建模以及联合仿真模型搭建人类手臂活动限制 Adams 力矩设置控制模型联仿之坑 1. 初始化错误 2. S-function 错误 3. 仿真到 ...
matlab - 3 自由度的机械臂
matlab - 3 自由度的机械臂实验说明 matlab环境代码实验说明你的控制器应该能够控制一个 3 自由度的机械臂到达给定的目的地. 代码有注释. 可视化 matlab环境 matlab ...
《机器人动力学与控制》第五章——速度运动学之机械臂的雅各比矩阵 5.0 导言
文章目录 <机器人动力学与控制>第五章--速度运动学之机械臂的雅各比矩阵 5.0 导言参考文献 <机器人动力学与控制>第五章--速度运动学之机械臂的雅各比矩阵 5.0 导言 ...
matlab阻抗控制仿真,机械臂阻抗控制与仿真研究.PDF
机械臂阻抗控制与仿真研究机械臂阻抗控制与仿真研究 Manipulator impedance control and simulation 高添泉* 乔枫周旋赵雨沈阳建筑大学信息与控制工程学 ...
matlab机器人雅可比矩阵实验,机械臂通过雅可比矩阵实现正运动学及逆运动学迭代解（工具：matlab）...
1.利用工具箱建模,供验证用选用二轴平面机械臂,两个关节角分别是theta1和theta2. view(3); Lnk1 = Link([ 0 0 100 0 0]);%theta,d,a,alph ...
matlab实现三自由度机械臂旋转
matlab实现三自由度的机械臂旋转 1 内容与要求根据表一机械臂长度,以及图一机械臂配置为机械臂开发一个控制器,使其能够执行特定的任务.更具体地说,让机械臂的末端执行器移动到特定的点.项目要求: ...

MATLAB Robotic System Toolbox 机械臂科氏矩阵算法