自由漂浮机器人运动学与动力学建模：space robot工具箱

原创

ZC_Robot机器人技术

修改于 2021-05-15 21:30:38

4.8K110

代码可运行

文章被收录于专栏：机器人技术与系统Robot机器人技术与系统Robot

运行总次数：0

代码可运行

根据冗余空间机器人的拓扑形式，建立其运动学方程，进而可以得到各个部分之间的位置关系、速度关系以及加速度关系。基座的运动将会引起机械臂末端的位置和姿态的变化，由于空间机器人在自由漂浮状态下的动量守恒，任意时刻基座的动量和机械臂的动量可以表示成一阶微分形式，进而，基座的运动关系可以表示为机械臂的各个关节角度的表达式。

1 自由漂浮空间机器人一般运动方程

单臂空间机器人的模型如下，由n个自由度DOF(Degree of Freedom)机械臂和作为其基座的平台组成。

从图可知自由漂浮机器人的各个杆件之间的位置关系，且由位置关系可以进一步得到空间机器人各个杆件之间的速度以及加速度关系。空间机器人的运动关系是研究空间机器人路径规划，正/逆向运动学以及动力学的基础。

空间机器人一般可以看作是基座和若干个杆件的组合体。由上图的空间机器人拓扑关系，可以得到各个组成空间机器人的刚体质心处的速度（线速度、角速度）为

进一步得到空间机器人末端的速度级正向运动学

其中

2 基座姿态受控下的运动学方程

空间机器人基座姿态控制系统会主动控制空间机器人的基座姿态，此时空间机器人处于自由飞行状态，基座质心处只受到外部作用力矩，空间机器人系统的角动量不守恒，系统线动量守恒，可以得到线动量的表达式：

其中

3 基座漂浮下的运动学方程

空间机器人基座处于自由漂浮状态时，即自由漂浮空间机器人，系统的线动量和角动量守恒，假定初始线动量和角动量为0，则整个系统除了满足完整约束外，还满足如下的非完整约束：

考虑整个系统相对于卫星本体质心的角动量L_0,则有L=L_0+r_0\times P ,.基座漂浮下的空间机器人系统动量守恒方程：

根据上述公式反解出基座线速度的表达式 v_0,再将其代入到前三行，可以得到

通过上式求解基座角速度与关节角速度可以得到

由此可以得到

上式中，J_g空间机器人的广义雅可比矩阵.对于空间机器人笛卡尔连续路径规划，即空间机器人末端连续路径的跟踪或者空间机器人基座姿态调整下的末端连续位置或者姿态跟踪任务，此时,\dot X_e将退化为末端线速度或者角速度。

4 The Spacedyn : a MATLAB Toolbox for Space and Mobile Robots

主页链接：Here. Spacedyn是一个MATLAB工具箱，可用于运动基础上的铰接多体系统的运动学和动态分析以及仿真。这样的系统的示例是具有机械附件的卫星，自由飞行的空中机器人，轮式移动机器人和步行机器人，所有这些机器人都可以在具有或不具有重力的环境中运动。

该工具箱可以处理具有拓扑树配置的开放链系统。例如，然后就不能直接支持并行操纵器。步行机器人一次在地面上接触两个以上的腿或四肢似乎形成了一个包括地面的闭合链，但是，我们可以在每个接触点使用适当的地面接触模型来处理这样的系统。并联机械手可以通过运动链的虚拟切割和相应的虚拟力模型进行处理。

Spacedyn包含了空间机器人相关的所有计算函数，通过简单的参数给定，可以很快的计算出来机器人的广义雅克比矩阵，空间机器人惯性矩阵等。

主要函数包含了

aw.m:	    function Aw =  aw( w0 )
calc_aa.m:	function AA = calc_aa( A0 , q )
calc_acc.m:	function [ vd , wd ] = calc_acc( A0, AA, w0, ww, vd0, wd0, q, qd, qdd )
calc_gj.m:	function GJ = calc_gj( R0, RR, A0, AA, q, num_e ) 
calc_hh.m:	function HH = calc_hh( R0, RR, A0, AA ) 
calc_je.m:	function Jacobian = calc_je( RR, AA, q, joints )
calc_jr.m:	function JJ_r = calc_jr( AA )
calc_jre.m:	function JJ_re = calc_jre( AA, joints )
calc_jt.m:	function JJ_t = calc_jt( RR, AA )
calc_jte.m:	function JJ_te = calc_jte( RR, AA, q, joints )
calc_pos.m:	function RR = calc_pos( R0, A0, AA, q )
calc_vel.m:	function [ vv,ww ] = calc_vel( A0, AA, v0, w0, q, qd ) 
cross.m:	function n = cross(u, v)
cx.m:	    function Cx = cx(theta)
cy.m:	    function Cy = cy(theta)
cz.m:	    function Cz = cz(theta)
dc2eul.m:	function euler = dc2eul( dc )
dc2qtn.m:   function qtn = dc2qtn( dc )
dc2rpy.m:	function rpy = dc2rpy( dc )
eul2dc.m:	function dc = eul2dc( euler ), dc = eul2dc( phi, theta, psi )
f_dyn.m:	function [ vd0, wd0, qdd ] = f_dyn(R0, A0, v0, w0, q, qd, F0, T0, Fe, Te, tau)
f_dyn_nb.m:	function [ R0, A0, v0, w0, q, qd ] = f_dyn_nb( R0, A0, v0, w0, q, qd, F0, T0, Fe, Te, tau )
f_dyn_nb2.m:function [ R0, A0, v0, w0, q, qd ] = f_dyn_nb2( R0, A0, v0, w0, q, qd, F0, T0, Fe, Te, tau ) 
f_dyn_rk.m:	function [ R0, A0, v0, w0, q, qd ] = f_dyn_rk( R0, A0, v0, w0, q, qd, F0, T0, Fe, Te, tau ) 
f_dyn_rk2.m:function [ R0, A0, v0, w0, q, qd ] = f_dyn_rk2( R0, A0, v0, w0, q, qd, F0, T0, Fe, Te, tau ) 
f_kin_e.m:	function [ POS_e, ORI_e ] = f_kin_e( RR, AA, joints ) 
f_kin_j.m:	function [ POS_j, ORI_j ] = f_kin_j( RR, AA, q, joints ) 
i_kine.m:	function q_sol = i_kine( R0, A0, POS_e, ORI_e, q_init, num_e ) 
int_eu.m:	function [ R0, A0, v0, w0, q, qd ] = int_eu( R0, A0, v0, w0, vd0, wd0, q, qd, qdd )
int_eu2.m:	function [ R0, A0, v0, w0, q, qd ] = int_eu2( R0, A0, v0, w0, vd0, wd0, q, qd, qdd )
j_num.m:	function joint = j_num(num_e)
r_ne.m:		function Force = r_ne( R0, RR, A0, AA, v0, w0, vd0, wd0, q, qd, qdd, Fe, Te ) 
rpy2dc.m:	function dc = rpy2dc( rpy )
tilde.m:	function B = tilde( vector )
tr2diff.m:	function diff = tr2diff( TR1, TR2 )

典型函数的具体使用方法具体表示如下：

积分	说明
采用龙格库塔积分	1 F_DYN_RK：with Integration by Runge-Kutta method [ R0, A0, v0, w0, q, qd ] = f_dyn_rk( R0, A0, v0, w0, q, qd, F0, T0, Fe, Te, tau ) 2 F_DYN_RK2：with Integration by Runge-Kutta method [ R0, A0, v0, w0, q, qd] = f_dyn_rk2( R0, A0, v0, w0, q, qd, F0, T0, Fe, Te, tau ) F_DYN_RK2采用有限转动理论更新姿态信息，而F_DYN_RK采用反对称阵求解姿态更新。对于F_DYN_RK2 k1_A0 = aw( w0 ) * A0-A0; 对于F_DYN_RK k1_C0 = d_time * tilde(w0)'*C0;
采用Newmark beta积分	F_DYN_NB with Integration by Newmark beta method F_DYN_NB with Integration by Newmark beta method
采用Trapezoidal Rule (2nd order Adams-Moulton)积分	F_DYN_TR

针对多体系统的仿真参数给定主要如下所示

global Qi J_type BB SS SE S0
global cc c0 ce
global m0 inertia0 m inertia mass
global d_time
global Qe
global Gravity
global num_q Ez

参数具体说明如下所示：

Ez代表Z轴的向量
BB参见多体动力学中的“关联数组”。
SS为关联矩阵
S0中元素为0和1。其中与基座相连的刚体对应的元素为1，其余刚体对应为0。
SE中元素为0和1。其中与外力相关联的刚体对应的元素为1，其余刚体对应的为0。
J_type为关节类型
c0为杆件0 的质心到各个关节的i 的距离 ;在计算杆件i的质心位置时用到了c0，该向量仅仅用于与基座项链的关节。其余不相连的杆件c0一致设置为0。这样是为了表示基座质心的位置。
m0,inertia0,m,inertia为基座和各个刚体杆件的质量和转动惯量
ce 杆件i的质心到末端点的向量。该量是为了表述机械臂末端的位置。并配合Qe给出末端的位姿。对于不与末端点项链的杆件其均为0
Qe为表示末端点的方向
cc表示从连杆i的质心到关节i的向量，在连体系下
Qi暂时未给出。但是在内部已经给出其变换阵的求法。如果需要编写通用计算程序，需要对程序进行进一步改进。
R0为基座的位置向量
Q0为基座的RPY 变换矩阵
A0为基座的方向余弦

5 实例

%  给定各个关节的驱动力矩，验证正向动力学的正确性
% clc;clear;close all
global Qi J_type BB SS SE S0
global cc c0 ce
global m0 inertia0 m inertia mass
global d_time
global Qe
global Gravity
global num_q Ez
% Unit vector in z-axis
     Ez = [0;0;1];               %代表Z轴的向量
% Link Parameters
%%%   需要下面这些量   R0, A0, v0, w0, q, qd, F0, T0, Fe, Te, tau 
    BB = [ 0 1 2 3 4 5 6 ];
    SS = [ -1  1  0  0  0  0  0;       %%  关联矩阵
                 0 -1  1  0  0  0  0;
                 0  0 -1  1 0  0   0;
                 0  0  0 -1  1  0  0;
                 0  0  0  0 -1  1  0;
                 0  0  0  0  0 -1  1;
                 0  0  0  0  0  0 -1 ];
    S0 = [ 1 0 0 0 0 0 0 ];%  与基座相邻的为1，其余为0。
    SE = [ 0 0 0 0 0 0 1 ]; %  SE 为与外力矩相关的一个矩阵   如果为末端杆件 则SE(i)=0;  对于主程序中给出的为双臂机械臂，因此本文为【0 0 1 0 0 1】
    J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' ];   %  关节类型
    c0 = [ 0   0 0 0 0 0 0;         %  杆件0 的质心到各个关节的i 的距离 ;在计算杆件i的质心位置时用到了c0
              0    0 0 0 0 0 0;
              3.5 0 0 0 0 0 0];
     m0 =15;  % 基座质量
     inertia0 =0.001*[ 2400  0  0;   % 基座转动惯量
              0 2400  0;
              0  0 2400 ];
     m = [ 20.76  20.76 40.45 31.15 20.76 20.76 27.96];  %  杆件质量
     mass = sum(m) + m0;    % 系统总质量
     inertia = [  0.645  0   0         0.645  0   0        0.284  0   0          0.137  0  0        0.645  0   0             0.645  0   0        1.311 0 0;   %  6个连杆的转动惯量
                   0 0.152   0          0 0.152   0        0 1.312 0.54             0 1.011 0           0  0.645   0            0  0.645   0        0 1.311 0;
                   0 0     0.645        0 0     0.645      0   0.54 1.308           0   0 0.013           0 0 0.152              0 0 0.152          0   0 0.220 ];
    ce = [ 0   0 0 0 0 0 0;         %  杆件i的质心到末端点的向量
              0    0 0 0 0 0 0;
              0    0 0 0 0 0 0.3525];
    Qe = [ 0   0 0 0 0 0 0;       %  末端点的方向
              0    0 0 0 0 0 0;
              0    0 0 0 0 0 0];
cc = zeros(3,7,7);
cc(:,1,1) = 0.001*[ 0 168.41 0 ]';       % 从连杆i的质心到关节i的向量 ,不知道是不是在连体系下的坐标？？？？
cc(:,2,2) = 0.001*[  0 -168.41 0]';       
cc(:,3,3) = 0.001*[ -968.5 0 -23.7]';
cc(:,4,4) = 0.001*[ -1162.32 0 0 ]';
cc(:,5,5) = 0.001*[ 0 0 -261.6 ]';
cc(:,6,6) =0.001* [ 0 0 -261.6 ]';
cc(:,7,7) =0.001* [ 0 0 -347.5 ]';

cc(:,1,2) =0.001* [ 0 -261.59 0]';
cc(:,2,3) =0.001* [ 0 261.59 0 ]';
cc(:,3,4) = 0.001*[ 1111.5 0 273.3]';
cc(:,4,5) =0.001* [ 917.68 0 0 ]';
cc(:,5,6) =0.001* [ 0 0 168.4]';
cc(:,6,7) =0.001* [ 0 0 168.4]';
% Initialize variables
q   = zeros(7,1);          % 机械臂各个关节角度
% % q=[0;90;0;0;0;-90;0]*pi/180;
qd  = zeros(7,1);
qdd = zeros(7,1);
v0  = [ 0 0 0 ]';   % 基座质心速度
w0  = [ 0 0 0 ]';   
vd0 = [ 0 0 0 ]';  %基座质心加速度 
wd0 = [ 0 0 0 ]';
vv = zeros(3,7);  % 各个杆件质心的速度
ww = zeros(3,7);
vd = zeros(3,7);
wd = zeros(3,7);
R0 = [ 0 0 0 ]';       %  基座的位置向量
Q0 = [ 0 0 0 ]';        % 基座RPY变换矩阵？？？？？？？？？
A0 = eye(3,3);       % 基座方向余弦
Fe = zeros(3,7);  %  作用在机械臂末端点上的力Fe和力矩Te
Te = zeros(3,7);
F0 = [ 0 0 0 ]';  %  作用在基座质心上的力F0和力矩T0
T0 = [ 0 0 0 ]';
tau = zeros(7,1);
d_time = 0.01;
Gravity = [ 0 0 0 ]';
num_q = length( q );
%%% Simulation Loop start
numj=1;
for time = 0:d_time:10,
        tau = zeros(7,1);
        tau(1:7,1)=[sin(time); 0.5*cos(time); 0.5*sin(time);0.1*cos(time); 0.1*sin(time); 0.05*sin(time); 0.01*sin(time)];
 AA = calc_aa( A0, q );  %%  各个杆件的方向  转换矩阵 
          RR = calc_pos( R0, A0, AA, q );   %%  杆件i的质心位置    
    [ R0, A0, v0, w0, q, qd ] = f_dyn_rk2( R0, A0, v0, w0, q, qd, F0, T0, Fe, Te, tau );
    v0j(numj,1:3)=v0;
    w0j(numj,1:3)=w0;
    numq(numj,1:7)=q;
     numj=numj+1;
end
 time = 0:d_time:10
 numqq=numq*180/pi;
    figure(111)
subplot(421)
    plot(time, numqq(:,1) ,'r-','LineWidth',1.8);grid on;legend('q1')
subplot(422)
    plot(time,numqq(:,2),'b-','LineWidth',1.8);grid on;legend('q2')
subplot(423)
    plot(time,numqq(:,3),'y-','LineWidth',1.8);grid on;legend('q3')
subplot(424)
    plot(time,numqq(:,4),'m-','LineWidth',1.8);grid on;legend('q4')
subplot(425)
    plot(time,numqq(:,5),'c-','LineWidth',1.8);grid on;legend('q5')
subplot(426)
    plot(time,numqq(:,6),'g-','LineWidth',1.8);grid on;legend('q6')
subplot(427)
    plot(time,numqq(:,7),'g-','LineWidth',1.8);grid on;legend('q7')

仿真结果如下所示：

空间机器人matlab工具箱移动机器人matlab工具箱机器人工具箱机器人运动学和动力学建模

原创声明：本文系作者授权腾讯云开发者社区发表，未经许可，不得转载。

如有侵权，请联系 cloudcommunity@tencent.com 删除。

机器人

原创声明：本文系作者授权腾讯云开发者社区发表，未经许可，不得转载。

如有侵权，请联系 cloudcommunity@tencent.com 删除。

机器人

登录后参与评论

0 条评论

热度