组合体惯量法A: matlab程序—机械臂动力学建模
机械臂为典型的多体系统，针对机械臂的动力学建模可以采用传统的多刚体系统建模原理，但是针对机械臂，由于其一般为串行链结构，针对其特殊性，可以采用有别于传统动力学建模原理的特殊方法。机械臂的实时动力学按照计算原则不同分为单处理器串行计算以及多处理器并行计算方法，本文研究的为单处理器串行计算的机械臂实时动力学。
对于多自由度的机械臂的逆向动力学一般采用Luh等改进的牛顿欧拉方程递推得到，该算法是机械臂逆向动力学算法中在单处理器上运行速度最快。对于机械臂的正向动力学建模，主要应用于机械臂的仿真模拟。其实时计算最主要由基于关节空间惯量矩阵的算法以及正向动力学递推算法。
基于关节空间惯量矩阵的动力学算法,该方法中关键是求出机器人系统的关节空间惯量矩阵，再求出其离心力项，进而根据机器人的动力学普遍方程求出关节角加速度。而求解关节空间惯量矩阵的方法有很多种，Walker和Orin在其论文中给出了三种求解关节空间惯量矩阵的方法，但是其中计算效率最高的是基于组合体求解惯量矩阵的方法[1]。
组合体惯量法建立机械臂的正向动力学是非常重要的方法，其可以大大减小动力学建模的时间并且提高计算效率，尤其针对多自由度机械臂。
%这部分的函数可以分解为主函数（调用别的子函数）、状态保存、adams 数据加载与处理、图像处理
%数值积分_目前用到的函数是：
基于关节空间惯量矩阵的算法实现流程如下
主函数如下：
tic
close all;
clc;clear;
theta=[0 ;-90 ;0; 0 ; 0 ;0; 0;0]*(pi/180);
theta0=[0 ;-90 ;0; 0 ; 0 ;0; 0;0]*(pi/180);%做图像时备用 %%注意此处只要修改一个值就可以了
dtheta=zeros(8,1);
ddtheta=zeros(8,1);
h=0.01;
H=10;
i=0:h:H;
N=length(i);
j=1;
for i=0:h:H
format bank;
% t=[1*sin(i);0.5*cos(i);0.5*sin(i);0.1*cos(i);0.1*sin(i);0.05*sin(i);0.01*sin(i)].*0.1;%此处力矩的单位为Nm
t=[0.01*sin(i);0*cos(i);0*sin(i);0*cos(i);0*sin(i);0*sin(i);0.01*sin(i)].*0.1;%此处力矩的单位为Nm
a1=theta(1)-theta0(1);%关节坐标系下的角度
a2=theta(2)-theta0(2);
a3=theta(3)-theta0(3);
a4=theta(4)-theta0(4);
a5=theta(5)-theta0(5);
a6=theta(6)-theta0(6);
a7=theta(7)-theta0(7);
CRBA_qq(j,1:7)=180/pi*[a1,a2,a3,a4,a5,a6,a7];
CRBA_dqq(j,1:7)=180/pi*dtheta(1:7);
cg=dynamic_model1008_01(theta,dtheta);
Hq=dynamic_model1008_02(theta);
ddtheta(1:7,1)= inv(Hq)*(t-cg); %k时刻的角加速度
dtheta(1:7,1)=dtheta(1:7,1)+h*ddtheta(1:7,1);
theta(1:7,1)=theta(1:7,1)+dtheta(1:7,1)*h+ddtheta(1:7,1)*(h^2/2);
cctheta(j,1:7)=180/pi*theta(1:7);
ccaa(j,1:7)=180/pi*ddtheta(1:7);
j=j+1;
end
toc
i=0:h:H;dynamic_model1008_01.m的函数具体内容如下所示
function cg0=dynamic_model1008_01(theta,dtheta)
% %预处理加模型建立模块
format bank;
format long
p=(10^(-3))*[0 0 0 0 0 0 0;
0 250 0 150 0 200 0 ;
150 0 150 0 135 0 100];
rc=(10^(-3))*[0 0 0 0 0 0 0;
0 125 0 75 0 100 0 ;
/*
* 提示：该行代码过长，系统自动注释不进行高亮。一键复制会移除系统注释
* 75 0 75 0 75 0 50];
*/
w=zeros(3,8);%第2:8代表第1:7，第一位是空位
dw=zeros(3,8);
v=zeros(3,8);
dv=zeros(3,8);
dvc=zeros(3,8);
ddtheta=zeros(8,1); %第1:7代表第1:7 第8位是空位
% 注意一下编程时设计到w,dw，v,dv和theta以及dtheta的角标区别
for i=2:8 %2:8 实际代表的是第1:7
rm=dynamic_bianhuanjuzhen(i-1,theta(i-1),1);%此处为从i-1到i的变换，故j=1
dq=[0 ;0 ;1]*dtheta(i-1);
ddq=[0 ;0 ;1]*ddtheta(i-1);
w(1:3,i)=rm*(w(1:3,i-1)+dq); %for w1 w2 w3 w4 w5 w6 w7
antis_w0=dynamic_antisymmetric(w(1:3,i-1));
dw(1:3,i)=rm*(dw(1:3,i-1)+antis_w0*[0;0;1]*dtheta(i-1)+ddq); %for dw1 dw2 dw3 dw4 dw5 dw6 dw7
antis_w1=dynamic_antisymmetric(w(1:3,i));
antis_dw=dynamic_antisymmetric(dw(1:3,i));
dv(1:3,i)=rm*(dv(1:3,i-1))+antis_dw*p(1:3,i-1)+antis_w1*(antis_w1*p(1:3,i-1)); %for dv 1:7
dvc(1:3,i)=dv(1:3,i)+antis_dw*rc(1:3,i-1)+antis_w1*(antis_w1*rc(1:3,i-1));
end
% n到1的动力学部分,且w,dw,v,dv,dvc这些参数是需要在计算完微分方程之后实时更新的
FF=zeros(3,8);%2*8
f=zeros(3,9);%3*9
NN=zeros(3,8);
n=zeros(3,9);%3*9 2:8代表实际的1:7
cg=[0;0 ;0 ;0; 0; 0; 0];%1*7
m=[5 18 10 12 8.6 16 10];
Ic=dy_IccCRBA();%调用转动惯量矩阵!!!
for i=8:-1:2 %8:2 实际代表的是第7:1
rm0=dynamic_bianhuanjuzhen(i,theta(i),0) ;%感觉此处有错,从第i到第1-1
rm=dynamic_bianhuanjuzhen(i-1,theta(i-1),1); %i=8 对应第7个杆件 对应着第case=7变换
FF(1:3,i)=m(i-1).*dvc(1:3,i);
f(1:3,i)=FF(1:3,i)+rm0*f(1:3,i+1);
antis_w0=dynamic_antisymmetric(w(1:3,i));
NN(1:3,i)=Ic(1:3,3*i-5:3*i-3)*dw(1:3,i)+antis_w0*(Ic(1:3,3*i-5:3*i-3)*w(1:3,i));
antis_p=dynamic_antisymmetric(p(1:3,i-1));
antis_rc=dynamic_antisymmetric(rc(1:3,i-1));
n(1:3,i)=NN(1:3,i)+rm0*n(1:3,i+1)+antis_p*f(1:3,i)+antis_rc*FF(1:3,i);
cg(i-1)=((rm*[0;0;1]).')*n(1:3,i);%2:8 of 'n' map 1:7 of 'cg'
end
cg0=cg;
end
% g=cg;%以上已经计算出c（q，dq）+G(q)的值dynamic_model1008_02.m的具体函数内容如下所示：
function Hq0=dynamic_model1008_02(theta)
format bank;
format long
p=(10^(-3))*[0 0 0 0 0 0 0;
0 250 0 150 0 200 0 ;
150 0 150 0 135 0 100];
%
% p=(10^(-3))*[0 0 0 0 0 0 0 ;
% 0 0 250 0 150 0 200 ;
% 0 150 0 150 0 135 0 ];
%
rc=(10^(-3))*[0 0 0 0 0 0 0;
0 125 0 75 0 100 0 ;
75 0 75 0 75 0 50];
% p=(10^(-3))*[0 0 -2080 0 0 0 0;0 430 0 -2080 -430 430 0 ; 0 0 727 0 0 0 700];% 将坐标点建立在相交电处
% rc=(10^(-3))*[0 0 1120.53 1037.7 0 0 0; 0 0 0 0 168.4 -168.4 0;168.4
% 168.4 -273.3 0 0 0 -347.5];
m=[5 18 10 12 8.6 16 10];
Ic=dy_IccCRBA();%调用转动惯量矩阵!!!
Icrba1=zeros(3,3);
Icrba=[Icrba1,Icrba1,Icrba1,Icrba1,Icrba1,Icrba1,Icrba1,Icrba1]; %初始化组合体惯量阵
mc=[0 0 0 0 0 0 0 0];%1*8
for i=7:-1:1 %计算组合体质量
mc(i)=m(i)+mc(i+1);
end
cj=zeros(3,8);%3*8 其第1:7代表第1:7，第8列空
for i=7:-1:1 %组合体c(i)的值
rm2=dynamic_bianhuanjuzhen(i+1,theta(i+1),0); %从i+1到i系的变换
cj(1:3,i)=(m(i)*(p(1:3,i)+rc(1:3,i))+mc(i+1)*(p(1:3,i)+rm2*cj(1:3,i+1)))/mc(i);
end
for i=7:-1:1 %=求解组合体转动惯量
rm=dynamic_bianhuanjuzhen(i,theta(i),1); %从i-1到i的变换
rm1=dynamic_bianhuanjuzhen(i+1,theta(i+1),0); %从i+1到i系的变换
a=p(1:3,i)+rc(1:3,i)-cj(1:3,i);
aa=(a.')*a*eye(3)-a*(a.');
b=p(1:3,i)+rm1*cj(1:3,i+1)-cj(1:3,i);%此处为rm1
bb=(b.')*b*eye(3)-b*(b.');
Icrba(1:3,3*i-2:3*i)=Ic(1:3,3*i-2:3*i)+m(i).*aa+(rm1)*Icrba(1:3,3*i+1:3*i+3)*(rm1.')+mc(i+1).*bb; %注意这里的惯量矩阵变换
end
% the solution of H(q)
f=zeros(3,7);
n=zeros(3,7);
Hq=zeros(7,7);%7*7
for j=7:-1:1
for i=j:-1:1
z0=[0;0;1]; % 实时角度值 待完善
rm=dynamic_bianhuanjuzhen(i,theta(i),1); %从i-1到i的变换
if i==j
antis_cj=dynamic_antisymmetric(cj(1:3,i));
f(1:3,i)=mc(i)*(-antis_cj*(rm*z0));
n(1:3,i)=Icrba(1:3,3*i-2:3*i)*(rm*z0)+antis_cj*f(1:3,i);
Hq(i,j)=((rm*z0).')*n(1:3,i);
else if i<j
rm1=dynamic_bianhuanjuzhen(i+1,theta(i+1),0); %从i+1到i系的变换
f(1:3,i)=rm1*f(1:3,i+1);
antis_p=dynamic_antisymmetric(p(1:3,i));
n(1:3,i)=rm1*n(1:3,i+1)+antis_p*f(1:3,i);
Hq(i,j)=((rm*z0).')*n(1:3,i);
end
end
end
for i=2:7
for j=i-1:-1:1
Hq(i,j)=Hq(j,i);
end
end
Hq0=Hq;
enddynamic_bianhuanjuzhen .m的具体内容如下所示
%预处理函数
%变换矩阵，使用函数
%此处需包含从i到i-1的变换，也包含i-1到i的变换
%i为当前需要变换的坐标，j=0为从i至i-1的变换，j=1为从i-1至i的变换，theta为对应的关节角度
function r=dynamic_bianhuanjuzhen(i,theta,j)
switch i
case 1
m=[cos(theta) -sin(theta) 0 ;
sin(theta) cos(theta) 0 ;
0 0 1 ]；
case 2
m=[cos(theta) -sin(theta) 0;
0 0 1;
-sin(theta) -cos(theta) 0 ];
case 3
m=[cos(theta) -sin(theta) 0;
0 0 1;
-sin(theta) -cos(theta) 0 ];
case 4
m=[cos(theta) -sin(theta) 0;
0 0 -1;
sin(theta) cos(theta) 0 ];
case 5
m=[cos(theta) -sin(theta) 0;
0 0 1;
-sin(theta) -cos(theta) 0 ];
case 6
m=[cos(theta) -sin(theta) 0;
0 0 -1;
sin(theta) cos(theta) 0 ];
case 7
m=[cos(theta) -sin(theta) 0;
0 0 1;
-sin(theta) -cos(theta) 0 ];
case 8
m=[0 0 0;
0 0 0;
0 0 0]；
end
% if j==1 %j=1为从i-1至i的变换
% r=m;
% else if j==0%j=0为从i至i-1的变换
% r=(m.');
%2013年10月15日修改了此处
if j==1 %j=1为从i-1至i的变换
r=m';
else if j==0%j=0为从i至i-1的变换
r=m;
end
enddynamic_antisymmetric.m的具体内容如下所示：
%此函数属于预处理函数
%求解向量的反对称阵
function antis=dynamic_antisymmetric(v)
n=length(v);
if n==3
antis=[0 0 0;0 0 0 ;0 0 0];
antis(1,2)=-v(3);
antis(1,3)=v(2);
antis(2,1)=v(3);
antis(2,3)=-v(1);
antis(3,1)=-v(2);
antis(3,2)=v(1);
else
error('Wrong number of input arguments');
end
end质量属性函数dy_IccCRBA.m
%inertia
function Ic=dy_IccCRBA()
Ic1=[0.15 0 0;
0 0.15 0;
0 0 0.08];
Ic2=[0.5 0 0;
0 0.2 0;
0 0 0.5];
Ic3=[0.35 0 0;
0 0.35 0;
0 0 0.15];
Ic4=[0.4 0 0;
0 0.15 0;
0 0 0.4];
Ic5=[0.38 0 0;
0 0.38 0;
0 0 0.16];
Ic6=[0.4 0 0;
0 0.16 0;
0 0 0.4];
Ic7=[0.23 0 0;
0 0.23 0;
0 0 0.17];
Ic8=[0 0 0 ;0 0 0 ;0 0 0];
Ic=[Ic1,Ic2,Ic3,Ic4,Ic5,Ic6,Ic7,Ic8]; %元胞数组,相对于的转动惯量
end本文机械臂的具体模型如下所示：
组合体惯量法A: matlab程序—机械臂动力学建模
[1]Walker M W, Orin D E. Efficient dynamic computer simulation of robotic mechanisms[J]. Journal of Dynamic Systems, Measurement, and Control, 1982, 104(3): 205-211.
[2]Jin M, Zhou C, Liu Y, et al. Dynamics modeling and simulation of space manipulator based on spatial vector[C]//2014 IEEE International Conference on Mechatronics and Automation. IEEE, 2014: 1040-1045.
[3]Zhou C 空间七自由度冗余机械臂动力学建模与控制研究.
THE END
0
二维码
海报
组合体惯量法A: matlab程序—机械臂动力学建模
机械臂为典型的多体系统，针对机械臂的动力学建模可以采用传统的多刚体系统建模原理，但是针对机械臂，由于其一般为串行链结构，针对其特殊性，可以采用有别于传统动力学建...
