系统建模与动力学分析坐标转换及机器人建模举例.ppt

空间物体的位姿描述与坐标变换,坐标转换的必要性在很多应用环境中，需要进行多个平台之间的数据传递和交互.由于每个平台的数据是在自身独立坐标系下产生的，因此，为了保证数据的统一和有效性，必须进行坐标之间的转换。,空间物体的位姿描述与坐标变换,众所周知，任何一种物体的运动都是相对的，确切地说是相对于一定的参考系而言的。为了研究空间物体的位姿、运动及动态性能等情况，必须把描述其运动的各种量，放在相应的坐标系及各种坐标系的相互关系中去考察。例如，在研究导弹的运动时，常用到的坐标系有1、地面坐标系Oxyz2、弹体坐标系Ox1y1z13、弹道坐标系Ox2y2z24、速度坐标系Ox3y3z3等。,空间物体的位姿描述与坐标变换,为了研究机器人的运动与操作，往往需要表示物体的方位。物体的方位可由某个固接于此物体的坐标系描述。为了规定空间某刚体B的方位，设置一直角坐标系B与此物体固接。用坐标系B的三个单位主矢量xB，yB，zB相对于参考坐标系A的方向余弦组成的33矩阵来表示刚体B相对于坐标系A的方位。称为旋转矩阵。上式中，上标A代表参考坐标系A，下标B代表被描述的坐标系B。它共有9个元素，但只有3个是独立的。由于它的三个列矢量都是单位矢量，且两两相互垂直。,空间物体的位姿描述与坐标变换,因而共有6个约束条件。即可见，旋转矩阵 是正交的，并且满足条件对应于轴x，y，z作转交为的旋转变换，其旋转矩阵分别为,空间物体的位姿描述与坐标变换,位姿描述 通常将物体B与某一坐标系固接。相对参考系A，坐标系B的原点位置和坐标轴的方位，分别由位置矢量和旋转矩阵描述。这样，刚体B的位姿可由坐标系B来描述，即平移和旋转坐标系映射1、平移坐标变换（称为坐标平移方程）2、旋转坐标变换（称为坐标旋转方程）3、坐标旋转和坐标平移的复合变换,空间物体的位姿描述与坐标变换,平移和旋转齐次坐标变换齐次变换：变换式（1）可表示成等价的齐次变换形式。其中，41的列向量表示三维空间的点，称为点的齐次坐标。,空间物体的位姿描述与坐标变换,可把上式写成矩阵形式：式中：其次变换阵为它综合地表示了平移和旋转变换。空间点p的直角坐标描述和齐次坐标描述分别为用1 0 0 0，0 1 0 0，0 0 1 0分别表示x，y，z三个轴的方向。,空间物体的位姿描述与坐标变换,变换矩阵的左乘和右乘的运动解释是不同的，变换顺序“从右向左”，指明运动是相对固定坐标系（参考系）而言的；变换顺序“从左向右”，指明运动是相对运动坐标系而言的。用欧拉变换表示运动姿态机械手的运动姿态由一个绕轴x，y，z的旋转序列来规定。这种转角的序列称为欧拉（Euler）角。,空间物体的位姿描述与坐标变换,用RPY组合变换表示运动姿态另一种常用的旋转集合是横滚（roll）、俯仰（pitch）和偏转（yaw）。,坐标变换,常用坐标系有大地坐标系 坐标表示形式为（L,B,H）。三个参数分别表示大地坐标系下的经度、纬度、海拔高度，用以描述物体在地球上的位置。它是大地测量时以参考椭圆球面为基准来建立的坐标系。地心地固坐标系 坐标表示形式为（X，Y，Z）。它是以地球中心为坐标原点的绝对直角坐标系，其Z轴与地轴平行指向北极点，X轴指向本初子午线与赤道的交点，Y轴垂直于XOZ平面，形成右手直角坐标系。地面坐标系 坐标表示形式为（Xd，Yd，Zd）。它是一种空间直角坐标系，它假设大地是平整的。Yd 轴指向地球北极，Zd轴与Yd轴垂直指向背离地心的正方向，Xd 轴指向东，构成右手系。（也称为东北天直角坐标系）,坐标转换,1.由机体极坐标系到直角坐标系的转换假设传感器安装在量测平台上，传感器对目标的量测在极坐标系完成，获得目标的距离、方位角和高低角，如图所示，注意角度的取向。其中机体（平台）直角坐标系是，构成右手系。其转换关系是注意yp指向机头正方向，zp和yp垂直指向飞机背部正方向，而(xp yp zp)构成右手系。,坐标转换,又假定量测平台的原点在大地坐标系的(xs ys zs)，与东北天（ENU）坐标系的原点重合。东北天坐标系(xl yl zl)的yl指向地球北极，zl与yl垂直指向背离地心的正方向，(xl yl zl)构成右手系。2.由机体直角坐标系到东北天坐标系的转换设()表示平台的姿态角（俯仰、偏航、滚动），则目标在ENU坐标系的表示为其中Rl是由姿态角引起的旋转变换。下面推导Rl的表示。,坐标转换,假定坐标轴采用符号与点坐标符号不加区分，而且机体初始在东北天坐标系姿态角，定义姿态角如下：（a）三维表示（b）二维表示 俯仰角旋转图在单纯俯仰角变化的前提下，此时,坐标转换,同样地，可进行滚动角旋转和偏航角旋转（a）滚动角旋转（b）偏航角旋转 滚动角旋转和偏航角旋转图,坐标转换,那么,坐标转换,3.东北天坐标系到ECEF坐标系的转换假定机载平台中心在大地坐标的位置是（s ls hs）（经、纬、高），则该中心在ECEF坐标系中的位置是其中而e和Eq分别是地球偏心率和地球半径。东北天坐标系与ECEF坐标系如右图所示,坐标转换,又假定目标在东北天坐标系的位置是(xp yp zp)，此时(xp yp zp)坐标系与(x y z)坐标系即ECEF坐标系的关系如上图所示，其旋转变换角分别为s（经度）和ls（纬度）。坐标变换关系为其中Rt表示由东北天坐标系到ECEF坐标系的旋转变换，即（a）纬度角旋转（b）经度角旋转,坐标转换,此时则有再考虑由于机载平台中心并不在ECEF坐标系中的原点，实际的变换公式是此处(xs ys zs)表示机载平台中心在ECEF坐标系中的位置，而(s ls hs)则表示机载平台中心在大地坐标系的位置。,综合应用举例机器人控制概述及建模,机器人控制概述,1.1 机器人发展现状及趋势1.2 机器人控制技术,1.1 机器人现状及趋势,发展史：第一代：示教再现型机器人 第二代：具有感觉的机器人 第三代：智能机器人发展趋势：标准化、模块化、开放化、网络化存在问题：1.驱动系统笨重 2.机械臂过重 3.移动机器人能源携带 4.计算机信息传输、处理能力不够快,1.2 机器人控制技术,机器人模型的建立,2.1 机器人数学基础2.2 机器人运动学模型2.3 机器人动力学模型,2.1 机器人数学基础,（1）位姿描述 1.位置的描述 刚体的位置可用它在某个坐标系中的向量来描述。2.方位的描述 刚体的方位也称刚体的姿态。称为旋转矩阵,（2）坐标变换 坐标变换包括平移变换和旋转变换。1.平移变换2.旋转变换3.复合变换：平移与旋转的结合,（3）齐次坐标变换齐次坐标定义：用四维向量表示三维空间一点的位置P，即上式称为齐次坐标，其中为非零常数。齐次变换：k表示旋转轴线方向的单位矢量，q为转角，ABT为齐次变换矩阵，Trans(ApBO)为平移变换矩阵，Rot(k,q)为旋转变换矩阵。,2.2 机器人运动学模型,最简单的机器人操作手运动学上的模型化方法是运动学链概念的方法。运动链是刚体（或杆件）的集合，通过运动副连接。两种基本类型的关节是1、旋转（或转动）副，记为R。如下图左。2、滑动（或梭柱）副，记为P。如下图右。,2.2 机器人运动学模型,机器人运动学模型是基于坐标变换求得的。D-H坐标变换法：严格定义了每个坐标系的坐标轴，并对连杆和关节定义了4个参数。用两个参数来描述一个连杆，即公共发现距离di：沿zi轴从坐标轴xi到xi+1的距离，规定与zi轴正向一直为正。所在平面内两轴的夹角qi：绕zi轴从坐标轴xi-1到xi的转角，规定逆时针方向为正。,2.2 机器人运动学模型,另外两个参数来表示相邻连杆的关系，即两连杆的相对位置ai:沿xi轴从坐标轴zi到zi+1的距离，规定与zi轴正向一直为正。两连杆法线的夹角i:绕xi+1轴从坐标轴zi到zi+1的转角，规定逆时针方向为正。缺点：很难正确地建立坐标系。因为D-H方法是建立在按严格的规则建立正确地坐标系的基础上的，特别是对多个移动副，很难确定其各个参数。,D-H坐标建立规则,A和B两坐标的坐标原点重合时，后一坐标分别绕前一坐标的x、y、z轴旋转的坐标变换矩阵为,当后一坐标与前一坐标的原点不重合时需要先进行平移变换,机器人运动学方程建立 对于具有n个连杆的机械手，运动学方程是要确定与末端坐标系n固联的手爪相对于基坐标系0的变换。根据齐次变换的乘法规则可得：式中，0nT表示末端坐标系n相对于基座0的位姿。,机器人运动学方程求解1.代数法 代数法求解过程中，通过逐次在运动学方程式的两边同时乘上一个齐次变换的逆，达到分离变量的目的。2.几何法 通过几何图形求解角度值，求解过程中利用正弦定理、余弦定理、反正切公式等求解角度。,2.3 机器人动力学模型,机器人动力学问题,机器人动力学正向问题：已知机器人各关节所需的驱动力或力矩，求解机器人各关节的位移、速度和加速度。从控制角度讲，正向问题用于运动的动态仿真。,机器人动力学逆向问题：已知各关节的位移、速度和加速度（即已知关节空间的轨迹或末端执行器在笛卡尔空间的轨迹已确定），求解机器人各关节所需的驱动力或力矩。,说明：1.不考虑机电控制装置的惯性、摩擦、间隙、饱和等因素时，n自由度机器人动力学方程为n个二阶耦合非线性方程。2.该方程中包括惯性力/力矩，哥氏力/力矩，离心力/力矩以及重力/力矩，是一个耦合的非线性多输入多输出系统。3.对机器人动力学的研究，所采用的方法很多，有拉格朗日法、牛顿-欧拉法、高斯法、凯恩法、旋转对数法、罗伯逊-魏登堡法等等。,研究机器人动力学的目的研究机器人动力学的目的是多方面的。动力学正问题与机器人的仿真有关。逆问题是为了实时控制的需要，利用动力学模型，实现最优控制。在设计中需根据连杆质量、运动学和动力学参数、传动机构特征和负载大小进行动态仿真，从而决定机器人的结构参数和传动方案，验算设计方案的合理性和可行性，以及结构优化程度。在离线编程时，为了估计机器人高速运动引起的动载荷和路径偏差，要进行路径控制仿真和动态模型仿真。这些都需要以机器人动力学模型为基础。,机器人动力学正问题拉格郎日方法,（1）机器人的拉格朗日函数被定义为系统的总动能 EK 和总位能 EP 之差，即 其中，表示动能与势能的广义坐标 表示相应的广义速度,机器人动力学正问题拉格郎日方法,（2）机器人系统的动能在机器人中，连杆是运动部件，连杆i的动能Eki为连杆质心线速度引起的动能和连杆角速度产生的动能之和，即：系统的动能为n个连杆的动能之和。由于vci和i是关节变量q和关节速度dq/dt的函数，因此从上式可知机器人的动能是关节变量和关键速度的标量函数，D(q)nn阶机器人惯性矩阵,机器人动力学正问题拉格郎日方法,（3）机器人系统的势能设连杆i的势能为Epi，连杆i的质心在0坐标系中的位置矢量为pci，则：机器人系统的势能为n个连杆的势能之和。它是q的标量函数，,机器人动力学正问题拉格郎日方法,（4）拉格朗日方程系统的拉格朗日方程为：上式又称为拉格朗日-欧拉方程，简称L-E方程，式中，是n个关节的驱动力和力矩矢量，上式可变为：,机器人动力学正问题拉格郎日方法,例：平面RP机器人如图所示，连杆1和连杆2的质量分别为m1和m2，质心的位置由l1和d2所规定，惯量矩阵为：,机器人动力学正问题拉格郎日方法,（1）取坐标，确定关节变量和驱动力和力矩。建立连杆D-H坐标系如上图所示，关节变量为1+/2，为求解方便，此处取关节变量为1和d2，关节驱动力矩1和力f2.（2）系统动能由机器人动能公式可得系统总动能为,机器人动力学正问题拉格郎日方法,（3）系统势能总势能为（4）偏导数,机器人动力学正问题拉格郎日方法,（5）拉格朗日运动学方程将偏导数代入拉格朗日方程，最终得到平面RP机器人的运动学方程的封闭形式：,机器人动力学正问题拉格郎日方法,关节空间与操作空间动力学模型（1）关节空间动力学方程首先，将上式写成如下的矩阵形式该式为机器人在关节空间中的动力学方程封闭形式的一般结构式，其中,机器人动力学正问题拉格郎日方法,说明：1、关节空间中的动力学方程反应了关节力或力矩与关节变量、速度和加速度之间的函数关系。2、对于n个关节的机器人，D(q)是nn正定对称矩阵，是q的函数，称为机器人惯性矩阵；H(q,dq/dt)是n1的离心力和哥氏力向量；G(q)是n1的重力矢量，与机器人的位形q有关。,机器人动力学正问题拉格郎日方法,（2）操作空间动力学方程与关节空间动力学方程相对应，在笛卡尔操作空间中，操作力F与末端加速度d2x/dt2之间的关系可表示为：其中，Mx(q)操作空间中的惯性矩阵Ux(q,dq/dt)离心力和哥氏力矢量Gx(q)重力矢量F广义操作力矢量x机器人末端位姿矢量,机器人动力学正问题拉格郎日方法,广义操作力与关节力之间的关系为：操作空间与关节空间之间的速度与加速度的关系：比较操作空间与关节空间动力学方程，可得,机器人动力学正问题拉格郎日方法,（3）关节力矩操作运动方程机器人动力学最终是研究其关节输入力矩与其输出的操作运动之间的关系，由前述公式可得：该式反映了输入关节与机器人运动之间的关系。,小结：拉格郎日方法的一般描述,拉格朗日函数被定义为系统的动能 K 和位能 E 之差，即 机器人的动能可以表示为：又因为,拉格朗日方程表示如下：整理得到：定义 则有：,简化得：其中：称为哥氏力和离心力矩阵，其元素可由下式表出：G(q)Rn为重力矢量，它的表达式为：E(q)表示机器人操作臂的重力势能,机器人操作臂的速度雅可比阵Jvi推导由机器人动力学可知，第i个操作臂在基坐标下的速度矢量 可由下式表示：,机器人操作臂角速度的雅可比阵Jwi的推导 由于所建立的坐标系的Z轴的方向均相同，故 所以（2.61）式可化为 故可得：,机器人操作臂的哥氏力矩阵H(q,dq/dt)的推导,将M(q)带入得,机器人操作臂的重力矢量G(q)的推导,