第三章机器人系统的动力学模型

3.1 独立关节模型

\quad

3.2 基于欧拉–拉格朗日方程的机器人动力学模型

机器人动力学方程：描述机器人的力和运动之间的关系的方程，运动学方程不考虑产生运动的力和扭矩。

欧拉–拉格朗日方程：处于完整约束下并且约束力满足虚功原理的机械系统的力和运动随时间的变化

\quad

3.2.1 欧拉–拉格朗日方程

ddt∂∂qi−∂L∂qi=τii=1,...,n\frac{d}{dt} \frac{\partial}{\partial q_i} - \frac{\partial L}{\partial q_i} = \tau_i \quad i=1,...,n \\ dtd∂qi∂−∂qi∂L=τii=1,...,n

拉格朗日算子LLL: 系统动能与势能之差
系统广义坐标算子：qiq_iqi
系统广义力 τi\tau_iτi

建模步骤

求K：动能广义速度（坐标）的表达
求P，势能关于广义坐标的表达
求LLL，运动方程
求ddt∂L∂q˙\frac{d}{dt} \frac{\partial L}{\partial \dot{q}}dtd∂q˙∂L: LLL方程对广义速度q˙\dot{q}q˙ 的偏导数
求∂L∂q\frac{\partial L}{\partial q}∂q∂L: LLL 方程对于广义位置q的偏导数

\quad

3.2.2 动能和势能的一般表达

基于欧拉–拉格朗日方程推导机器人动力学的前提：存在一组广义坐标表示的系统动能和势能

动能

刚体的动能表示：平移动能 + 质心旋转动能
κ=12mvTv+12wTIw\kappa = \frac{1}{2}mv^Tv + \frac{1}{2}w^TIw \\ κ=21mvTv+21wTIw
物体的惯性张量：仅取决于物体的形状与质量分布情况、与物体的运动无关

连杆上任何一点的线速度和角速度可通过雅可比矩阵和关节速度来表示
vi=Jvi(q)q˙wi=Jwi(q)q˙v_i = J_{v_i}(q)\dot{q} \\ w_i = J_{w_i}(q)\dot{q} \\ vi=Jvi(q)q˙wi=Jwi(q)q˙
连杆i质量为mim_imi、惯性矩阵为IiI_iIi，机器人总动能为：
K=12q˙T∑i=1n[miJvi(q)TJvi(q)+Jwi(q)TRi(q)IiRi(q)TJwi(q)]q˙K = \frac{1}{2} \dot{q}^T \sum_{i=1}^n [m_iJ_{v_i}(q)^TJ_{v_i}(q) + J_{w_i}(q)^TR_i(q)I_iR_i(q)^TJ_{w_i}(q)] \dot{q} \\ K=21q˙Ti=1∑n[miJvi(q)TJvi(q)+Jwi(q)TRi(q)IiRi(q)TJwi(q)]q˙
求和部分表示为机器人惯性矩阵D(q)
K=12q˙TD(q)q˙K = \frac{1}{2} \dot{q}^T D(q) \dot{q} \\ K=21q˙TD(q)q˙
机器人惯性矩阵特点

只与机器人构型有关
对称且正定
动能总是非负的

势能

刚体动力学情形下，势能的唯一来源是重力
假设物体的质量都集中在质心处来计算第i个连杆的势能

Pi=gTrcimiP_i = g^Tr_{ci}m_i \\ Pi=gTrcimi

其中g是惯性坐标系中的重力向量、rcir_{ci}rci 是连杆i的质心坐标。

n连杆的机器人总势能(机器人的势能仅仅是广义坐标的函数)
P=∑i=1nPi=∑i=1ngTrcimiP = \sum_{i=1}^n P_i = \sum_{i=1}^n g^Tr_{ci}m_i \\ P=i=1∑nPi=i=1∑ngTrcimi
\quad

3.2.3 运动方程表示

采用欧拉–拉格朗日法的前提

系统的动能是关于广义速度（坐标微分）的二次函数
K=12∑i,jndij(q)q˙iq˙j:=12q˙TD(q)q˙K = \frac{1}{2} \sum_{i,j}^n d_{ij}(q) \dot{q}_i \dot{q}_j := \frac{1}{2} \dot{q}^TD(q) \dot{q} \\ K=21i,j∑ndij(q)q˙iq˙j:=21q˙TD(q)q˙
系统的势能与广义速度（坐标微分）无关
P=∑i=1nPi=∑i=1ngTrcimiP = \sum_{i=1}^n P_i = \sum_{i=1}^n g^Tr_{ci}m_i \\ P=i=1∑nPi=i=1∑ngTrcimi

欧拉–拉格朗日算子：
L=K−P=12∑i,jndij(q)q˙iq˙j−P(q)L = K-P = \frac{1}{2} \sum_{i,j}^n d_{ij}(q) \dot{q}_i \dot{q}_j - P(q) \\ L=K−P=21i,j∑ndij(q)q˙iq˙j−P(q)

ddt∂∂qi−∂L∂qi=τii=1,...,n\frac{d}{dt} \frac{\partial}{\partial q_i} - \frac{\partial L}{\partial q_i} = \tau_i \quad i=1,...,n \\ dtd∂qi∂−∂qi∂L=τii=1,...,n

L算子相对于第k个关节速度的偏导数为：
∂L∂q˙k=∑jdkjq˙j\frac{\partial L }{\partial \dot{q}_k} = \sum_j d_{kj} \dot{q}_j \\ ∂q˙k∂L=j∑dkjq˙j
L算子相对于第k个关节速度的偏导数对时间的导数：
ddt∂L∂q˙k=∑idkjq¨j+∑jdkjq˙j=∑jdkjq¨j+∑i,j∂dkj∂qiq˙iq˙j\frac{d}{dt} \frac{\partial L }{\partial \dot{q}_k} = \sum_i d_{kj} \ddot{q}_j + \sum_j d_{kj} \dot{q}_j = \sum_j d_{kj} \ddot{q}_j + \sum_{i,j} \frac{\partial d_{kj}}{\partial q_i} \dot{q}_i \dot{q}_j \\ dtd∂q˙k∂L=i∑dkjq¨j+j∑dkjq˙j=j∑dkjq¨j+i,j∑∂qi∂dkjq˙iq˙j
L算子相对于第K的关节位置的偏导数：
∂L∂qk=12∑i,j∂dij∂qkq˙iq˙j−∂P∂qk\frac{\partial L}{\partial q_k} = \frac{1}{2} \sum_{i,j} \frac{\partial d_{ij}}{ \partial q_k} \dot{q}_i \dot{q}_j - \frac{\partial P}{\partial q_k} \\ ∂qk∂L=21i,j∑∂qk∂dijq˙iq˙j−∂qk∂P
因此对于每个k = 1，2，… ，n, 欧拉–拉格朗日方程可以写成：
∑jdkjq¨j+∑i,j{∂dkj∂qi−12∂dij∂qk}q˙iq˙j−∂P∂qk=τk\sum_j d_{kj} \ddot{q}_j + \sum_{i,j}\{\frac{\partial d_{kj}}{\partial q_i} - \frac{1}{2} \frac{\partial d_{ij}}{\partial q_k} \} \dot{q}_i \dot{q}_j - \frac{\partial P}{\partial q_k} = \tau_k \\ j∑dkjq¨j+i,j∑{∂qi∂dkj−21∂qk∂dij}q˙iq˙j−∂qk∂P=τk
通过改变求和顺序和使用对称性质，存在
∑i,j{∂dkj∂qi}q˙iq˙j=12∑i,j{∂dkj∂qi+∂dki∂qj}q˙iq˙j\sum_{i,j} \{\frac{\partial d_{kj}}{ \partial q_i} \} \dot{q}_i \dot{q}_j = \frac{1}{2} \sum_{i,j} \{\frac{\partial d_{kj}}{ \partial q_i} + \frac{\partial d_{ki}}{ \partial q_j} \} \dot{q}_i \dot{q}_j \\ i,j∑{∂qi∂dkj}q˙iq˙j=21i,j∑{∂qi∂dkj+∂qj∂dki}q˙iq˙j
从而有：
∑i,j{∂dkj∂qi−12∂dij∂qk}q˙iq˙j=∑i,j12{∂dkj∂qi+∂dki∂qj−∂dij∂qk}q˙iq˙j\sum_{i,j}\{\frac{\partial d_{kj}}{\partial q_i} - \frac{1}{2} \frac{\partial d_{ij}}{\partial q_k} \} \dot{q}_i \dot{q}_j = \sum_{i,j}\frac{1}{2} \{\frac{\partial d_{kj}}{\partial q_i} + \frac{\partial d_{ki}}{\partial q_j} - \frac{\partial d_{ij}}{\partial q_k} \} \dot{q}_i \dot{q}_j \\ i,j∑{∂qi∂dkj−21∂qk∂dij}q˙iq˙j=i,j∑21{∂qi∂dkj+∂qj∂dki−∂qk∂dij}q˙iq˙j
定义如下：
cijk:=12{∂dkj∂qi+∂dki∂qj−∂dij∂qk}c_{ijk} := \frac{1}{2} \{\frac{\partial d_{kj}}{\partial q_i} + \frac{\partial d_{ki}}{\partial q_j} - \frac{\partial d_{ij}}{\partial q_k} \} \\ cijk:=21{∂qi∂dkj+∂qj∂dki−∂qk∂dij}

cijk=cjikc_{ijk} = c_{jik} cijk=cjik

定义广义重力：
ϕk=∂P∂qk\phi _k = \frac{\partial P}{ \partial q_k} \\ ϕk=∂qk∂P
最终得到的欧拉–拉格朗日方程为：
∑idkj(q)q¨j+∑i,jcijk(q)q˙iq˙j+ϕk(q)=τk,k=1,....,n\sum_i d_{kj}(q)\ddot{q}_j + \sum_{i,j} c_{ijk}(q)\dot{q}_i \dot{q}_j + \phi_k(q) = \tau_k, \quad k =1,....,n \\ i∑dkj(q)q¨j+i,j∑cijk(q)q˙iq˙j+ϕk(q)=τk,k=1,....,n
简记为：
D(q)q¨+C(q,q˙)q˙+g(q)=τD(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau \\ D(q)q¨+C(q,q˙)q˙+g(q)=τ

\quad

3.2.4 举例：平面二关节机器人动力学模型

作业！！！！！！！！！！！！

思考：如何得到一个空间6关节机器人的动力学模型

\quad

3.2.5 机器人动力学方程的性质

反对称性：矩阵N是反对称的, 可同于证明控制器的稳定性
N(q,q˙)=D(q)−2C(q,q˙)N(q,\dot{q}) = D(q) -2C(q,\dot{q}) \\ N(q,q˙)=D(q)−2C(q,q˙)

njk=−nkjn_{jk} = -n_{kj} \\ njk=−nkj

无源性

存在常数满足：可用于证明控制器的稳定性
∫0Tq˙T(ζ)τ(ζ)dζ≥−β,∀T>0\int ^T_0 \dot{q}^T(\zeta) \tau(\zeta) d\zeta \geq - \beta, \quad \forall T > 0 \\ ∫0Tq˙T(ζ)τ(ζ)dζ≥−β,∀T>0
参数的线性化性

存在n×ln \times ln×l 函数 Y(q,q˙,q¨)Y(q,\dot{q},\ddot{q})Y(q,q˙,q¨) 和lll 维向量 θ\thetaθ 满足：
D(q)q¨+C(q,q˙)q˙+g(q)=Y(q,q˙,q¨)θ=τD(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = Y(q,\dot{q},\ddot{q})\theta = \tau \\ D(q)q¨+C(q,q˙)q˙+g(q)=Y(q,q˙,q¨)θ=τ
Y(q,q˙,q¨)Y(q,\dot{q},\ddot{q})Y(q,q˙,q¨): 回归方程， θ\thetaθ: 参数向量

机器人动力学特性是其惯性参数的线性函数，可用于简化模型计算、动力学参数辨识。

拉格朗日动力学方程 VS 牛顿欧拉动力学方程

二者得出的结果是一致的！

区别在于

L法是闭式法、N法是迭代法；
L法处理的是整体、N处理的是连杆

\qquad

3.3 机器人动力学模型的参数辨识

3.3.1 机器人动力学参数辨识的必要性

实现基于模型的控制以改善运动控制效果
基于动力学模型比较实现碰撞检测
实现负载辨识以提高工作节拍

机器人动力学参数的获取方法

解析计算：MATLAB、ADAMS中常用的方法
解体测量法：直接简单的方法，但难于获得哥氏力、摩檫力参数
实验辨识法：综合近似结果

当前建模方法的困境

大量的高维偏导数、导数计算难以完成
惯性参数的计算因几何形状的简化而不准确
未考虑关节柔性与传动损耗（摩擦力的影响）

摩擦力影响

粘性摩擦、库伦摩擦
只有通过参数辨识的方法得到摩擦力项

机器人动力学参数辨识

参数辨识：已知输入/输出数据与系统模型，求解系统参数
前提：机器人动力学模型是机器人惯性参数的线性函数

τ=D(q)q¨+C(q,q˙)q˙+ϕ(q)=Y(q,q˙,q¨)X\tau = D(q)\ddot{q} + C(q,\dot{q})\dot{q} + \phi(q) = Y(q,\dot{q},\ddot{q})X \\ τ=D(q)q¨+C(q,q˙)q˙+ϕ(q)=Y(q,q˙,q¨)X

\quad

3.3.2 机器人动力学参数辨识的条件

机器人动力学模型与惯性参数：线性关系

刚体的惯性参数：10个，包括总质量、质心的三个坐标、惯性张量的6个独立元素，一个n连杆最多有10n个惯性参数。

最小惯性参数：X

所有惯性参数均可采用最小惯性参数的线性组合表示
最小惯性参数不唯一、但个数唯一
并不能独立的确定机器人每一个惯性参数、只能辨识出最小惯性参数
可以利用机器人几何参数直接确定最小参数

最小惯性参数：确定方法

采用MDH参数，直接确定机器人的最小惯性参数（见《机器人动力学与控制》P116 霍伟著）

\quad

3.3.3 机器人动力学参数辨识的方法

辨识数据：

关节扭矩 τ\tauτ：依据电流测量值实现估计 I→τ^I \rightarrow \hat{\tau}I→τ^
轨迹数据 : b编码器反馈值结合差分运算

辨识算法：

最小二乘法辨识

\quad

3.3.4 机器人动力学参数辨识的步骤

构建动力学模型
由基本参数集构造最小参数集
设计合理的激励轨迹
执行激励轨迹、记录关节扭矩、位置、速度、加速度
辨识动力学最小参数集
验证已辨识动力学参数的正确性

动力学参数辨识举例

埃斯顿 ER16 机器人SLU三轴动力学模型辨识

构建ER16前三轴动力学模型：不考虑摩擦力
确定最小参数集
设计激励轨迹
结果验证