机器人仿真技术学习笔记(二)

机器人的动力学和多项式路径规划

1.刚体机器人加速度
2.机器人刚体的质量分布
3.牛顿欧拉递推动力学方程
4.状态空间方程
- 1.定义
- 2. P u m a 560 Puma560 Puma560参数的获取
- - (1)运动学和动力学参数
  - (2)惯性矩阵
  - (3)科里奥利矩阵
  - (4)重力矩阵
  - (5)摩擦矩阵
  - (6)逆向动力学的求解
5.正向动力学问题
- 1.正动力学计算方法
- 2. M a t l a b Matlab Matlab代码
6. S e r i a l L i n k SerialLink SerialLink 类的函数说明
7.机器人运动轨迹问题
- 1.运动轨迹五项式插值
- 2. M a t l a b Matlab Matlab代码

机器人工具箱用的是robot-9.10的MATLAB机器人工具箱

MATLAB机器人工具箱网址

用的书籍是《机器人仿真与编程技术》清华大学出版社2018年2月第1版。
电子书密码是love

1.刚体机器人加速度

设有坐标系 { A } \{A\} {A}和 { B } \{B\} {B},坐标系 { B } \{B\} {B}固连与刚体上，该刚体相对 { A } \{A\} {A}有一个运动 Q B Q^B QB。 { B } \{B\} {B}相对于 { A } \{A\} {A}的位置用位置矢量和旋转矩阵 P B O R G A ， R B A P_{BORG}^A，R_B^A PBORGA，RBA来描述， Q Q Q在 { A } \{A\} {A}中的线速度:
V Q A = V B R O G A + R B A V Q B V_Q^A=V^A_{BROG}+R^A_BV^B_Q VQA=VBROGA+RBAVQB
设 { B } \{B\} {B}相对于 { A } \{A\} {A}的旋转角速度为 Ω B A \Omega_B^A ΩBA。而加速度可以描述为:
a Q A = a B R O G A + Ω B A × ( Ω B A × R B A Q B ) + Ω B A × R B A × Q B a^A_Q=a_{BROG}^A+{\Omega}^A_B\times(\Omega^A_B \times R^A_BQ^B)+\Omega_B^A\times R_B^A \times Q^B aQA=aBROGA+ΩBA×(ΩBA×RBAQB)+ΩBA×RBA×QB
角速度关系:
Ω C A = Ω B A + R B A Ω C B \Omega^A_C = \Omega_B^A+R_B^A\Omega_C^B ΩCA=ΩBA+RBAΩCB
求导后得到：
α C A = α B A + R B A α C B + Ω B A × R B A Ω C B \alpha_C^A=\alpha_B^A+R_B^A\alpha_C^B+\Omega_B^A\times R_B^A\Omega_C^B αCA=αBA+RBAαCB+ΩBA×RBAΩCB

2.机器人刚体的质量分布

对于转动关节的机械臂，在一个刚体绕任意轴旋转时候，惯性张量表示机器人的质量分布，在 { A } \{A\} {A}上的惯性张量：
I A = ( I x x − I x y − I x z − I x y I y y − I y z − I x z − I y z I z z ) I_A=\begin{pmatrix} I_{xx}&-I_{xy}&-I_{xz}\\ -I_{xy}&I_{yy}&-I_{yz}\\ -I_{xz}&-I_{yz}&I_{zz} \end{pmatrix} IA=⎝⎛Ixx−Ixy−Ixz−IxyIyy−Iyz−Ixz−IyzIzz⎠⎞
其中：
I x x = ∫ ∫ ∫ V ( y 2 + z 2 ) ρ d v ( 同理 I y y , I z z 也类似 ) I x y = ∫ ∫ ∫ V x y ρ d v ( 同理 I y z , I x z 也类似 ) I_{xx}=\int\int\int_V(y^2+z^2)\rho dv(同理I_{yy},I_{zz}也类似)\\ I_{xy} = \int\int\int_V xy\rho dv(同理I_{yz},I_{xz}也类似) Ixx=∫∫∫V(y2+z2)ρdv(同理Iyy,Izz也类似)Ixy=∫∫∫Vxyρdv(同理Iyz,Ixz也类似)

3.牛顿欧拉递推动力学方程

先对机器人的每个刚体求出每个连杆的质心位置和惯性张量。

由质心运动定律，作用于机器人连杆质心上的作用力为 F F F，与刚体的速度 v c v_c vc有以下关系：
F ⃗ = m v c ˙ ⃗ \vec{F}=m\vec{\dot{v_c}} F =mvc˙
对于一个转动的刚体，引起刚体转动的力矩为 N N N。欧拉方程用来表示作用在刚体上的力矩与角速度和角加速度之间的关系，其中 I C I^C IC为惯性张量，但是刚体的质心位置要在坐标原点：
N = I C ω ˙ + ω × I C ω N=I^C\dot{\omega}+\omega\times I^C\omega N=ICω˙+ω×ICω
以上的方程只是为了得到已知关节位姿，速度和加速度为别为 q , q ˙ , q ¨ q,\dot{q},\ddot{q} q,q˙,q¨时，求出驱动力和驱动力矩。

以下是迭代步骤：

S t e p 1 : Step1: Step1:已知连杆位置 q q q，从连杆 1 1 1递推到 n n n,计算连杆的速度 q ˙ \dot{q} q˙和加速度 q ¨ \ddot{q} q¨,再对所有连杆用上述两个方程，得到连杆质心上的力和力矩。对于转动关节：
ω i + 1 ( i + 1 ) = R i i + 1 ω i ( i ) + θ ˙ i + 1 Z i + 1 \omega_{i+1}^{(i+1)}=R_i^{i+1}\omega _i^{(i)}+\dot{\theta}_{i+1}Z_{i+1}\\ ωi+1(i+1)=Rii+1ωi(i)+θ˙i+1Zi+1
由于太过复杂，故公式如下：

S t e p 2 : Step2: Step2:计算关节力矩(是驱动器施加在机器人上的力矩或作用在机器人上使其运动的外力)

其中，角速度，角加速度和线加速度的初值分别是：
ω 0 0 = ( 0 , 0 , 0 ) T ω ˙ 0 0 = ( 0 , 0 , 0 ) T v 0 ˙ = ( − g , 0 , 0 ) T \omega_0^{0}=(0 ,0 ,0)^T\\ \dot{\omega}_0^{0}=(0 ,0 ,0)^T\\ \dot{v_0} = (-g,0,0)^T ω00=(0,0,0)Tω˙00=(0,0,0)Tv0˙=(−g,0,0)T

拉格朗日方程建立机器人动力学方程的方法省去。

4.状态空间方程

1.定义

不考虑一切摩擦因素的状态空间方程如下：
M ( q ) q ¨ + C ( q , q ˙ ) q ˙ + G ( q ) + F ( q ˙ ) + J ( q ) T f = τ M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)+F(\dot{q})+J(q)^Tf = \tau M(q)q¨+C(q,q˙)q˙+G(q)+F(q˙)+J(q)Tf=τ
其中， q ∈ R n q \in R^n q∈Rn，关节偏移量， M ( q ) ∈ n × n M(q) \in n \times n M(q)∈n×n，机器人惯性矩阵角对称矩阵，里面非零元素的大小取决于 q ( θ 1 , θ 2 , . . . θ n ) q(\theta_1,\theta_2,...\theta_n) q(θ1,θ2,...θn), M ( q ) ( ¨ q ) M(q)\ddot(q) M(q)(¨q)表示惯性力的大小。 C ( q , q ˙ ) q ˙ ∈ R n C(q,\dot{q})\dot{q}\in R^n C(q,q˙)q˙∈Rn为科里奥利矩阵,表示科里奥利力和离心力。 G ( q ) ∈ R n G(q)\in R^n G(q)∈Rn表示重力矩阵， F ( q ˙ ) F(\dot{q}) F(q˙)为摩擦力矩， J ( q ) T f J(q)^Tf J(q)Tf表示关节力， J J J是雅可比矩阵， τ ∈ R n \tau \in R^n τ∈Rn,是与关节位移量 q q q有关的广义驱动力向量。

2. P u m a 560 Puma560 Puma560参数的获取

Matlab工具箱可提取部分机器人的模型的动力参数。以 p u m a 560 puma560 puma560为例子，对函数说明。

(1)运动学和动力学参数

>> mdl_puma560
>> p560.links(6).dyntheta=q, d=          0, a=          0, alpha=          0, offset=          0 (R,stdDH)m    =        0.09r    =           0           0       0.032I    = |     0.00015           0           0 ||           0     0.00015           0 ||           0           0       4e-05 |Jm   =     3.3e-05Bm   =    3.67e-05Tc   =     0.00396(+)     -0.0105(-)G    =       76.69qlim = -4.642576 to 4.642576

可以求得第 6 6 6个连杆的 D H DH DH参数，连杆质量，质心坐标，惯性矩阵，电机转动惯量，电机摩擦力，库仑力和齿轮传动比。

(2)惯性矩阵

当机器人关节为 q q q时，获取惯性矩阵的函数为：
S e r i a l L i n k . i n e r t i a ( q ) SerialLink.inertia(q) SerialLink.inertia(q)

>> p560.inertia([0 0 0 0 0 pi/2])

(3)科里奥利矩阵

机器人关节角为 q q q,关节角速度为 q ˙ \dot{q} q˙,获取科里奥利矩阵的函数：
S e r i a l L i n k . c o r i o l i s ( q , q ˙ ) SerialLink.coriolis(q,\dot{q}) SerialLink.coriolis(q,q˙)

>> q = [0 0 0 0 0 0];
>> qd = [pi/6 pi/6 pi/6 pi/6 pi/6 pi/6];
>> p560.coriolis(q,qd)

(4)重力矩阵

当机器人关节角为 q q q时，获取重力矩阵的函数为：
S e r i a l L i n k . g r a v l o a d ( q ) SerialLink.gravload(q) SerialLink.gravload(q)

>> q = [0 0 0 0 0 pi/2];
>> p560.gravload(q)

(5)摩擦矩阵

在 p 560. l i n k s ( 6 ) . d y n p560.links(6).dyn p560.links(6).dyn这个函数中有说明。

>> p560.links(6).Tc

(6)逆向动力学的求解

τ \tau τ:不同关节的驱动力矩，函数为：
τ = p 560. r n e ( q , q ˙ , q ¨ , g r a v ) \tau = p560.rne(q,\dot{q},\ddot{q},grav) τ=p560.rne(q,q˙,q¨,grav)
例如，用该函数计算机器人沿一条轨迹运动时的每个时刻的驱动力矩：

mdl_puma560;
T1 = transl(0.3,0.1,0.2)*trotx(pi/2);
q_1 = p560.ikine6s(T1);
T2 = transl(0.2,0.4,0)*trotx(pi)/2;
q_2 = p560.ikine6s(T2);
t = [0:0.1:1]';
[q,qd,qdd] = jtraj(q_1,q_2,t);
%tranimate(T1,T2);
tu = p560.rne(q,qd,qdd);
plot(t,tu(:,1),'r');
hold on
plot(t,tu(:,1),'r');
plot(t,tu(:,2),'g');
plot(t,tu(:,3),'b');
plot(t,tu(:,4),'c');
plot(t,tu(:,5),'m');
plot(t,tu(:,6),'y');
xlabel('时间/s');
ylabel('驱动力矩/N.mm^2');
title('求驱动力矩图');

5.正向动力学问题

1.正动力学计算方法

有机器人系统的状态空间方程可知其加速度为：
q ¨ = M ( q ) − 1 ( τ − C ( q , q ˙ ) q ˙ − G ( q ) + F ( q ˙ ) ) \ddot{q} = M(q)^{-1}(\tau-C(q,\dot{q})\dot{q}-G(q)+F(\dot{q})) q¨=M(q)−1(τ−C(q,q˙)q˙−G(q)+F(q˙))
如果我们从 t = 0 t=0 t=0时刻开始计算。设 q ( 0 ) = q 0 , q ˙ ( 0 ) = 0 : q(0)=q_0,\dot{q}(0)=0: q(0)=q0,q˙(0)=0:

当经过 Δ t 时 \Delta t时 Δt时，机械臂的速度为：
q ˙ ( t + Δ t ) = q ˙ ( t ) + q ¨ ( t ) Δ t \dot{q}(t+\Delta t) = \dot{q}(t)+\ddot{q}(t)\Delta t q˙(t+Δt)=q˙(t)+q¨(t)Δt
此时，机械臂的位置是：
q ( t + Δ t ) = q ( t ) + q ˙ ( t ) Δ t + 1 2 q ¨ ( t ) Δ t 2 q(t+\Delta t) = q(t)+\dot{q}(t)\Delta t +\frac{1}{2}\ddot{q}(t)\Delta t^2 q(t+Δt)=q(t)+q˙(t)Δt+21q¨(t)Δt2
若将 q = q ( t + Δ t ) , q ˙ = q ˙ ( t + Δ t ) q = q(t+\Delta t ),\dot{q} = \dot{q}(t+\Delta t) q=q(t+Δt),q˙=q˙(t+Δt)带入上面第一个方程，就可以求得 q ¨ = q ¨ ( t + Δ t ) \ddot{q} = \ddot{q}(t+\Delta t) q¨=q¨(t+Δt)。这样就可以求出其对应的 q , q ˙ , q ¨ q,\dot{q},\ddot{q} q,q˙,q¨。

相应的数值积分方法就不多说了。

2. M a t l a b Matlab Matlab代码

如果我们 T T T为采样时间(时间间隔)， t o r q f u n torqfun torqfun表示给定的力矩函数， q , q d q,qd q,qd分别指代 q , q ˙ q,\dot{q} q,q˙。
[ T , q , q d ] = S e r i a l L i n k . f d y n ( T , t o r q f u n ) [T,q,qd] = SerialLink.fdyn(T,torqfun) [T,q,qd]=SerialLink.fdyn(T,torqfun)
如果要计算角加速度 q ¨ \ddot{q} q¨， τ \tau τ为驱动力矩。可以做如下函数计算：
q d d = S e r i a l L i n k . a c c e l ( q , q d , τ ) qdd = SerialLink.accel(q,qd,\tau) qdd=SerialLink.accel(q,qd,τ)

mdl_puma560;
q = [0 0 0 0 0 0];
qd = [0 0 0 0 0 0];
tor = [1 1 1 1 1 1];
qdd = p560.accel(q,qd,tor);>> qddqdd =0.0471-8.53724.18545.19525.88415.1508

6. S e r i a l L i n k SerialLink SerialLink 类的函数说明

7.机器人运动轨迹问题

1.运动轨迹五项式插值

已知路径段的起始点和终止点的位置，速度和加速度:
θ ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 \theta(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5 θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5
约束条件为：
θ 0 = a 0 θ f = a 0 + a 1 t f + a 2 t f 2 + a 3 t f 3 + a 4 t f 4 + a 5 t f 5 θ 0 ˙ = a 1 θ f ˙ = a 1 + 2 a 2 t f + 3 a 3 t f 2 + 4 a 4 t f 3 + 5 a 5 t f 4 θ 0 ¨ = 2 a 2 θ f ¨ = 2 a 2 + 6 a 3 t f + 12 a 4 t f 2 + 20 a 5 t f 3 \theta_0 = a_0\\ \theta_f = a_0+a_1t_f+a_2t_f^2+a_3t_f^3+a_4t_f^4+a_5t_f^5\\ \dot{\theta_0} = a_1\\ \dot{\theta_f} = a_1+2a_2t_f+3a_3t_f^2+4a_4t_f^3+5a_5t_f^4\\ \ddot{\theta_0} = 2a_2\\ \ddot{\theta_f} = 2a_2+6a_3t_f+12a_4t_f^2+20a_5t_f^3 θ0=a0θf=a0+a1tf+a2tf2+a3tf3+a4tf4+a5tf5θ0˙=a1θf˙=a1+2a2tf+3a3tf2+4a4tf3+5a5tf4θ0¨=2a2θf¨=2a2+6a3tf+12a4tf2+20a5tf3
解得最终的解：
a 0 = θ 0 a 1 = θ 0 ˙ a 2 = θ 0 ¨ 2 a 3 = 20 θ f − 20 θ 0 − ( 8 θ f ˙ + 20 θ 0 ˙ ) t f − ( 3 θ 0 ¨ − θ f ¨ ) t f 2 2 t f 3 a 4 = 30 θ 0 − 30 θ f − ( 14 θ f ˙ + 16 θ 0 ˙ ) t f + ( 3 θ 0 ¨ − 2 θ f ¨ ) t f 2 2 t f 4 a 5 = 12 θ f − 12 θ 0 − ( 6 θ f ˙ + 6 θ 0 ˙ ) t f − ( θ 0 ¨ − θ f ¨ ) t f 2 2 t f 5 a_0 = \theta_0\\ a_1 = \dot{\theta_0}\\ a_2 = \frac{\ddot{\theta_0}}{2}\\ a_3 = \frac{20\theta_f-20\theta_0-(8\dot{\theta_f}+20\dot{\theta_0})t_f-(3\ddot{\theta_0}-\ddot{\theta_f})t_f^2}{2t_f^3}\\ a_4 = \frac{30\theta_0-30\theta_f-(14\dot{\theta_f}+16\dot{\theta_0})t_f+(3\ddot{\theta_0}-2\ddot{\theta_f})t_f^2}{2t_f^4}\\ a_5 = \frac{12\theta_f-12\theta_0-(6\dot{\theta_f}+6\dot{\theta_0})t_f-(\ddot{\theta_0}-\ddot{\theta_f})t_f^2}{2t_f^5} a0=θ0a1=θ0˙a2=2θ0¨a3=2tf320θf−20θ0−(8θf˙+20θ0˙)tf−(3θ0¨−θf¨)tf2a4=2tf430θ0−30θf−(14θf˙+16θ0˙)tf+(3θ0¨−2θf¨)tf2a5=2tf512θf−12θ0−(6θf˙+6θ0˙)tf−(θ0¨−θf¨)tf2

2. M a t l a b Matlab Matlab代码

关节速度为 v v v，加速度为 a a a， q q q为 m × n m \times n m×n的变化过程中的角度， m m m为采样点， n n n为自由度：
[ q , v , a ] = j t r a j ( q 1 , q 2 , t ) [q,v,a] = jtraj(q_1,q_2,t) [q,v,a]=jtraj(q1,q2,t)

mdl_puma560
T1 = transl(0.3,0.1,0)*trotx(pi);
T2 = transl(0.3,0.3,0)*transl(0.3,0.1,0)*trotx(pi);
t = 0:0.1:10;
q = jtraj(p560.ikine6s(T1),p560.ikine6s(T2),t);%移动
p560.plot(q) %显示动画
%绘制笛卡尔空间的轨迹
T3 = p560.fkine(q);
p = transl(T3);
plot(p(:,1),p(:,2));