2、开链机器人动力学
2.1 开链机器人的拉格朗日函数 计算n个关节的开链机器人的动能，可将其中每一连杆动能求和， 定义一固连于第i杆质心的坐标系Li，则可得Li位形:
第i杆质心的物体速度为：
式中
ξ Ad1ˆ j j j
(e
e
ˆ i i
gsl i (0))
j
j i
为相对于第i连杆坐标系的第j个瞬时关节运动螺旋。
机器人的动力学及控制
1.拉格朗日方程
2.开链机器人动力学方程
1、拉格朗日方程
1.1 刚体的惯性 设V R3表示刚体的体积，ρ(r), r∈V是刚体的密度。如果物 体是均匀的，那么ρ(r)= ρ为常量。 刚体的质量可以表示为：
m (r )dV
V
刚体的质心是密度的加权平均：
r 1 (r )rdV mV
如图所示刚体，在质心建立 物体坐标系，g=(p,R)∈SE(3)为 物体相对于惯性坐标系的运动轨 迹，r∈R3为刚体上一点相对于 物体坐标系的坐标，现求刚体的 动能。
1、拉格朗日方程
1.1刚体的惯性 点在惯性坐标系的速度为：
物体的动能可用如下求得：
展开计算可得：
=
其中w为在物体坐标系中表示的刚体角速度，矩阵З ∈R3x3为物体坐标 系中的物体惯性张量
T=(1/2)VT
V=(1/2)(AdgV)T (Adg)-1
(AdgV)
=(AdgT)-1
选取三个坐标轴，使刚体的广义惯性矩阵为对角阵，则这三个轴 为刚体的惯性主轴。
1、拉格朗日方程
1.2 拉格朗日方程 定义拉格朗日函数示为：
式中T和V分别表示系统的动能和势能。 对于广义坐标为q∈Rm、拉格朗日函数为L的机械系统，其运动方 程为： 作用于第i个广 义坐标的外力 上式即为拉格朗日方程，将其写成矢量形式为：
2、开链机器人动力学
2.1 开链机器人的拉格朗日函数 第i杆的动能为
总动能为：
矩阵M(θ)∈Rnxn为机器人惯性矩阵，可将其定义如下：
第i杆势能为：
Hi(θ)为第i杆质心高度。
2、开链机器人动力学
2.1 开链机器人的拉格朗日函数 总势能为
将其与动能加以组合，得拉格朗日函数
2、开链机器人动力学
2.2 开链机器人的运动方程 拉格朗日函数
对机器人，可将关节角作为其广义坐标，广义力就是作用于关节轴 线上的力矩。
1、拉格朗日方程
1.2 刚体的牛顿—欧拉方程 对于位行为g∈SE(3)的刚体，利用运动螺旋和力螺旋建立动力 学方程。 设g=(p,R)∈SE(3)表示刚体质心坐标系相对于惯性坐标系的位形， f为在惯性坐标系中表示的作用于质心的力。由牛顿定律可得移动方 程：
1、拉格朗日方程
1.1 刚体的惯性 物体坐标系中表示的物体惯性张量З 如下：
其中：
物体总动能可写为移动动能和转动动呢之和，如下式所示：
1、拉格朗日方程
1.1 刚体的惯性 上式中 矩阵 ：称为在物体坐标系中表示的物体广义惯性矩 阵。该矩阵是对称且正定的。 可以看出动能是个标量，与刚体的位置和姿态无关，由此可以 通过运动旋量的坐标变换来求得新坐标系下的刚体广义惯性矩阵。
包括重力和作用于关节的
2、开链机器人动力学
2.2 开链机器人的运动方程 反映机器人惯性特性的矩阵M和C具有如下重要性质。 1）M（θ）是对称且正定的；
2） M
2C R nn 是反对称矩阵。
C的定义不是唯一的。 下面为一个求三杆机器人的动力学 如右图，在每一连杆 的质心处建立一坐标系Li, 坐标轴方向取为与连杆惯 性主轴方向一致。 机器人动力学方程如下：
2、开链机器人动力学
由于坐标轴均建立在惯性 主轴上，故连杆惯性矩阵有如 下的一般形式。
式中mi为连杆质量，Ixi，Iyi，Izi为第i连杆分别关于x,y和z轴的惯性矩。 为计算机器人的惯性矩M（θ），需先算出相对于每一连杆的物体雅 可比矩阵。
2、开链机器人动力学
雅可比矩阵的第j列为相对
于第i连杆坐标系的第j个 瞬时关节运动螺旋。
同理，可推导刚体的转动方程，相对于惯性坐标系的角动量为 为相对于惯性坐标系的瞬时惯性张量，ws是空间角速度。故 转动方程为
化简得：
该式称为欧拉方程
1、拉格朗日方程
1.2 刚体的牛顿—欧拉方程 （1） （2） 上述两式描述了刚体的动力学，力和力矩是相对于惯性坐标系表 示的。 下面推导用运动螺旋和力螺旋来表示刚体动力学，将物体速度 和物体力 代的惯性矩阵为：
其中的元素为
2、开链机器人动力学
矩阵元素：
哥氏力和离心力的计算
经复杂的计算后，不为零的项为：
2、开链机器人动力学
不为零的项：
最后计算重力对机器人 的影响，这些力可表示为
2、开链机器人动力学
势能为：
各杆高度hi可根据机器人正运动学计算
求得
代入势能公式
至此完成了动力学方程的推导。
2.2 开链机器人的运动方程 外力可分为两部分：设 并定义 为作用于第i个广义坐标系的任意其它力，包 括有势力和摩擦力产生的保守力，β 为阻尼系数。 作用于机器人上的其它力，可以通过雅克比矩阵的转置将其折 算到关节上。 T
J F
称矩阵C为机器人的哥氏矩阵，于是（1）式可改写为
式中τ 为驱动力矩矢量， 其它力。
l1c 2 r2 c 23 0 0 1 p1 0 1 s 23 c 23
2、开链机器人动力学
同样，可得雅可比矩阵的 其它两列：
为计算机器人的惯性矩M（θ），需先算出相对于每一连杆的物体雅 可比矩阵。
等式两边左乘RT，得：
1、拉格朗日方程
1.2 刚体的牛顿—欧拉方程 化简得：
在物体坐标系 中为常量
将物体坐标系表示的牛顿定律与上式结合，可得质心处作用有外 力螺旋F、相对于物体坐标系的物体运动方程
上式称为在物体坐标系中表示的牛顿-欧拉方程，由于相对于 物体坐标系的线速度和当前姿态有关，故移动和转动存在耦合联 系。
将动能表示成和的形式，
将上式代入拉格朗日方程：
Ri表示第i关节的驱动力矩和其它非保守力。
2、开链机器人动力学
2.2 开链机器人的运动方程 展开并整理得：
(1)
其中
(2)
从(1)式看出方程由4部分组成：与关节加速度有关的惯性力和，与 关节速度平方成正比的离心力和哥氏力，有势力和外力。 其中
2、开链机器人动力学
判断θ的正负时需注意，以θ3为例，选择一根较容 易看出来的轴（例如本例可选择与z轴平行方向的轴） 当θ3转过一个角度后的到一个位形，判断该选定的轴相 对于坐标系L3是如何转的，本例是逆时针，故θ3相对于 L3为正。
θ3
L3
1
0 0 0 ˆ p1 r2 e x 3 l1 r2 l1c3 0 0 l1 s 3
容易得第1、2连杆对应的 物体雅可比矩阵如下：
2、开链机器人动力学
对连杆三的物体雅可比可 按如下来求： 雅可比矩阵的第一列为关 节1的运动螺在L3下的表示
0 0 ˆ ˆ x 3 x 2 1 e e 0 s 23 1 c 23