复合变换练习题

复合变换：已知坐标系{B}在{A}下描述求{B}下向量，在{A}下描述。定义中间坐标系{C}与{A}姿态相同，原点与{B}原点重合。则 图2-5复合变换平移{A}→{C}得先将{A}转到{C]，则成果相同

先转再移，平移量在{C}下表达第1页例2.1{B}出示位姿与{A}重合，{B}相对{A}轴转，平移设求解：第2页第3页例：坐标系{B}最初与坐标系{A}重合，将坐标系{B}绕轴转，按着将上一步旋转得到坐标系绕转，求从到矢量变换旋转矩阵。解：绕相对坐标轴旋转

验证，正交性第4页求：解：①{A}绕转{B}②{B}转转再绕新轴转第5页③{A}绕转90°，再绕新x轴转-30°{C}第6页作业：P26，2基坐标系用{A}表达第7页5.(1)(2)第8页(3)因此(4)第9页由几何关系能够直接写出变换矩阵第10页1.欧拉角旋转变换第11页(2,3)对应相等

或 或差π(180°)(1,3)(3,3)：(2,1)(2,2)：第12页2.滚、仰、偏变换解第13页(2,1)相等得(1,1)(3,1)：(2,2)(2,3)：第14页例：ZYX欧拉变换及求解：第15页逆运动学可解性与求解办法：1.逆运动学问题由给定机械手末端（工具坐标系）姿态和位置确定机械手各关节角度。即映射，正运动学，2.可解性逆运动学需要解非线性方程组，常用机械手具有6个自由度，位置和姿态有6个独立分量，从数学上看，六个方程六个未知量。①无解：机械手不能达成盼望位姿。（图1）②多解：有多种能够达成盼望位姿。（图2）第16页3.解法①数值解，非线性方程组数值解（慢）②封闭解（解析解），对于6自由度机械手一般情况难以得到封闭解。但在特殊情况下能够求得封闭解。如Puma560机械手。由于机械手是人设计，一般设计成能够解析求解机械手。第17页4.平面机械手求解如图两自由度机械手，已知末端位置。△OAB中α能够确定β能够计算，也可计算解不唯一，与OB轴对称位置也是问题解。图3平面机械手第18页一、时变位姿符号表达1.位置矢量微分以B为参照系，矢量微分定义为速度矢量在其他坐标系可表达{A}，C坐标原点相对于固定参照坐标系速度能够简化表达表达速度在{A}上表达，但相对{U}速度。第19页2.角速度矢量角速度矢量用表达，描述刚体旋转运动表达{B}相对于{A}旋转，方向代表转轴，大小表达转动速度。相对于固定参照系角速度可省略参照系符号，{C}角速度。是角速度矢量在{A}下表达，但角速度是相对{U}。第20页二、刚体线速度和角速度1.线速度坐标系{B}固连在刚体上，要求描述相对{A}运动设不随时间变化则时间求导得(5)，(5)只适合{B}相对{A}位姿不变情况。2.角速度不为零时线速度。当时比较复杂即第21页例：机器人质心沿轴以1m/s速度移动，同步绕质心以1rad/s，半径r=0.5m角速度转动，求点Q速度解：第22页机器人连杆运动一般用{O}作为参照系，表达{i}原点线速度（都是相对参照系{O}），表达{i}角速度。一、连杆间速度传递机械手各连杆速度能够从基坐标系{O}开始依次计算。如图1，连杆速度用线速度和角速度描述。矢量用{i}描述比较方便。同步将相邻连杆速度矢量用同一坐标系表达，则速度能够相加。连杆i+1角速度等于i角速度加上连杆i+1关节旋转引发角速度（相对{i}）。第23页在{i}坐标系下，角速度其中同乘在(1)两端，得线速度：由前面公式表达在{i+1}下第24页连杆线速度和角速度能够用(2)和(4)以此计算。需要注意是，和是客观量，在不一样坐标系下表达是同一种矢量。都是相对于固定坐标系{O}速度。第25页例：如图因此两连杆机械手，{0}~{3}给定，计算各连杆速度。第26页在基坐标系下表达第27页二、雅可比矩阵1.多元函数（x是t函数）写成矢量形式称为雅可比矩阵，是时变。第28页2.机器人是笛卡尔速度矢量，是关节角。对于6关节机械手即关节角速度和线速度第29页例：两关节机械手雅可比计算与直接计算相同。由(4)，当可逆时可得即可由笛卡尔空间速度求出关节角速度。第30页3.奇异性当J奇异或接近奇异时，给定笛卡尔速度不能实现。如图机械臂，末端延x轴以1m/s速度运动，求关节速度当时，关节速度→∞4.力与雅可比，关节力矩与末端环境力关系。第31页一、刚体线加速度与角加速度线加速度角加速度表达坐标系{B}相对{A}转动角速度，是{B}描述一点。相对运动公式比较复杂：当不变时线加速度：第32页角加速度传递，{B}以相对{A}转动，{C}以相对{B}转动。计算和第33页二、刚体质量分布1.定轴转动①匀质圆盘绕子轴转动，绕z轴转动惯量，质量为M定义为r是微元到子轴距离，面密度则②身质杆长为，质量为M，计算线杆中点转动惯量③绕端点转动惯量第34页④平行移轴定理d是转轴到质心距离对于匀质杆：端点验证是正确。第35页对于空间问题定义惯性矩阵是单元密度，当选择坐标轴使为零时，坐标轴称为主轴。和定轴转动同样，也有平行移轴定理。第36页三、牛顿——欧拉方程刚体质心加速度，作用在刚体上协力，质量M牛顿方程刚体角速度和角加速度，绕质心坐标系{C}转动惯量矩阵欧拉方程是作用在刚体上协力矩。每个杆能够建立6个方程，还要利用连杆间传递（速度、加速度），能够求解。方程中具有约束力，计算效率相对较高。第37页四、拉格朗日方程采取解析方程得到系统动力学方程，抱负约束下，不出现约束力。拉格朗日函数L=K-P(4.1)K是系统动能，P是系统势能，系统动力学方程为qi是描述位置坐标，称为广义坐标，是作用在qi上广义力。第38页例1：无质量杆末端带一集中质量m，杆长，建立动力学方程。解：①定轴转动即②拉格朗日方程第39页拉格朗日方程：与直接计算相同，广义力是不能用势能表达力。第40页例2：两自由度平面机械手，连杆长为L1，L2，杆质量为m1，m2，质心到杆端点距离为Lc1，Lc2，绕质心转动惯量为 ，力矩求动力学方程。解：刚体动能表达为是质心速度大小，是刚体角速度。第41页第42页拉格朗日方程能够直接计算有关和两个方程。下面分析方程构造第43页拉格朗日方程能够表达为其中第44页以上成果代入到(*)式即得到机械手动力学方程。第45页例3：双足机器人，杆长，腿质量集中在中点，，集中质量，求动力学方程。解：第46页第47页第48页动力学方程：第49页动力学仿真：令则一阶微分方程组：机械手参数和力矩可用全局变量表达。能够用四阶R—K办法求解。只需定义函数，给定初值，采取ODE45仿真，若考虑控制作用最佳自己编RK4，也比较简单。第50页轮式移动机器人运动学与途径规划一、轮式移动机器人运动学两轮机器人，位姿，前进速度v，转动速度，则运动学约束条件： ，非完整约束，不能沿轴方向横移。设轮距为D，轮半径为r，则两轮独立驱动时轮子转速，与机器人运动速度间关系为（设轮子与地面不滑动）：第51页当给定机器人速度时，两轮速度由下式计算：可由通过控制轮速达成控制机器人速度目标。第52页二、机器人位置估计轮上装增量编码器，已知初始位姿，两轮转角则轮移动距离 ，机器人移动距离 方位角变化 。n步机器人位姿按下面公式更新：

能够确定任意时刻机器人位姿，比较简单，但积累误差大长时间不可靠。第53页三、轮式机器人途径规划任务：已知机器人初始位姿，给定机器人目标位姿，在存在障碍环境中规划一条无碰撞、时间（能量）最优途径。（已知地图）1.人工势场法人工势场基本思想是构造目标位置引力场和障碍物周围斥力场共同作用人工势场。搜索势函数下降方向来寻找无碰撞途径。①目标引力场其中是机器人位置，是目标位置，K是引力常数。第54页②障碍物斥力场其中是障碍物位置，表达障碍物影响范围，是斥力常数。引力表达为斥力表达机器人所受协力为：第55页算法长处：①简单方便，能够实时控制，考虑多种障碍，连续移动。②规划途径比较平滑安全。算法缺陷：①局部最优算法②复杂多障碍环境也许出现局部极值点，停滞。第56页四、栅格法思想：将机器人工作空间划分为多种简单区域，称为栅格。若栅格内没有障碍物称为自由栅格，不然称障碍栅格。将栅格编号，划分就是搜索由起点到目标点自由栅格组成连通域。能够用栅格序号表达，再将栅格序号转换成机器人空间实际坐标，令机器人按此途径运动。第57页栅格法途径规划步骤：1.建立栅格。将机器人和目标点间区域划分栅格，大小与机器人有关。2.障碍地图生成。标注障碍栅格和自由栅格。3.搜索元障碍最优途径，算法，遗传算法，人工势场，蚁群算法等。长处：①若存在最优途径，算法得当一定能够得到问题最优解。②有成熟途径搜索算法使用。缺陷：①栅格粒度影响较大。划分细时，存贮大和搜索时间长。②得到是折线，需要光滑处理。第58页五、全向移动机器人运动学分析三轮全向移动机器人构造如图所示，xoy是机器人坐标系，机器人速度 三个全向轮角速度 轮半径为R，距中心距离为L，则速度间关系为：即第59页场地坐标系与机器人坐标系变换为由(1)、(3)得第60页公式(4)表达给定机器人速度矢量，三个全向轮速度即可确定。电机控制一般采取PID控制 参数优化需要数次试验和调整以达成最佳运行状态。第61页六、双轮机器人非完整运动规划任务：确定使机器人从，同步控制作用最小。

，T是运行时间。常用办法：将参数化

第62页是正交基函数，则，不考虑约束，最优解，采取罚函数法：能够采取最优化办法求(2)最优解。在优化时，给初值，逐渐增大至，能够得到最优化解。解一系列最优化问题。另一种思绪是对机器人状态插值.，满足边界条件，再由(1)计算出，进而计算，不需要罚函数。第63页轨迹跟踪控制，单关节建模和控制一、动力学方程如图二阶系统方程为： (1)令则 (2)选择则系统方程变为 (3)第64页二、轨迹跟踪控制设盼望轨迹是时间光滑函数，定义误差 ，控制律 (4)代入到(3)得 (5)特性方程 (6)设(6)解为

， ，则(5)解为 (7)若

， 都有负实部，则系统是稳定，即 (8)第65页图2给出了轨迹跟踪方程构造图，假定模型是始终（m,b,k已知）。当系统存在外部干扰时，（干扰力）误差方程(6)变为 (9)若

有界，则误差也是有界。1.稳态误差若干扰是常数，稳态误差方程 (10)即百分比系数越大，稳态误差越小。2.附加积分享在(4)中加积分项 (11)第66页则误差方程为 (12)求导(12)式得 (13)当干扰力

是常数时，稳态误差为 (14)即 ，积分作用能够消除稳态误差。第67页三、单关节建模与控制1.电动机模型直流电动机等效电枢电路如图1所示，电源电压，电枢绕组感抗，电阻，反电动势

。电动机输出转矩 (1)是转矩常数。反电动势常数，反电动势为 (2)电路方程 (3)第68页2.动力学模型设传动比为

，负载力矩为

，转角

与电机对应量之间关系为 (4) (5)电机转子动力学方程为

(6)其中 和

分别为电机转子和负载惯量，

和

分别为电机转子和负载轴承粘性摩擦系数，将(5)代入到(6)得电机变量动力学方程：

(7)系统由(3)和(7)组成，通过控制电压 实现转动控制。第69页用负载变量表达动力学方程为 (8)

称为等效惯量，

称为等效阻尼。当大传动比时，电动转子惯量占有效惯量主要部分。例连杆惯量工变化范围 ，转子惯量 ，传动比 ，求有效惯量最大值和最小值。解：能够看出减速器使惯量变化减小了。大传动比时有效惯量能够当作常数。第70页3.单关节控制假设：(1)电动电感能够忽视(2)大传动比，有效惯量是常数(3)忽视构造柔性影响单关节控制控制率为 (9) (10) (11) (12)第71页则闭环系统动力学方程为 (12)忽视电感，电路方程为 (13)而转矩与电流成正比，

，

，由(11)得到

进而得到电流 ，可由(13)调整电压来实现单关节控制。第72页四、工业机器人控制构造

UnimationPUMA560工业机器人控制系统系统构造如图1所示。上位机DECLSI-11是主控计算机，通过接口给6个Rockwell6503微处理器发送指令。每一种6503处理器采取PID控制律控制一种独立关节。

每个关节安装光电编码器，测量关节角度，再通过差分运算得到关节速度。微处理器通过调整电枢两端电压控制电机电流。图2是关节控制框图。

上位机执行逆运动学计算得到关节盼望轨迹

，发送到6503微处理器实现单关节控制。

目前机械手控制能够采取PC机为上位机进行高层规划计算，而关节控制作为独立伺服模块，使用比较方便，采取C++语言编程即可实现机械手控制。（Matlab也能够使用）第73页第74页操作臂控制问题，工业机器人控制系统一、非线性控制前面介绍单关节控制属于线性控制，忽视了各关节耦合。机器臂动力学方程为 (1) 离心和柯氏力重力摩擦力若模型已知，能够采取分解控制办法： (2) (3)伺服控制律 (4) (5)第75页由(1)~(5)能够得到系统闭环方程 (6)取

和

为对角矩阵，则矢量方程(6)是解耦，能够写成独立关节形式 (7)前面基于模型控制办法如图1所示。由于实际问题模型是不精确实际应用能够采取模型估计值实现对机械手控制。系统稳定性分析比较复杂，会引发伺服误差。第76页二、目前工业机器人控制系统模型和参数不确定性、模型计算复杂性限制了基于模型控制实际应用。从经济角度出发，工业机器人采取简单控制率，加上误差赔偿。1.独立关节PID控制当今大部分工业机器人控制方式为 (8)是单位矩阵。伺服控制部分为 (9)

，和为常数对角矩阵，多数情况下

简单设为零，即简单控制器在控制律中主线不使用基于模型控制部分，每个关节采取独立控制系统来控制。由于没有解耦，关节间存在干扰，一般采取高增益使干扰很快得到抑制。第77页2.附加重力赔偿重力项容易引发静态定位误差，有点工业机器人在控制器中加入重力赔偿项 ，即 ，控制率为 (10)重力计算相对简单，但(10)中各关节控制不再是独立。第78页李雅普诺夫稳定性分析对于非线性系统，当模型存在误差不能解耦和线性化时，不能用特性方程直接判定系统稳定性。以简单弹簧阻尼-质量系统为例介绍非线性系统稳定性判定办法： (1)系统总能量 (2)能量变化率 (3)(3)辨明系统能量是耗散，除非

，因此，系统受到初始干扰，能量将不停耗散懂得静止。即 ，静止时由(1)得 。第79页因此，系统(1)在任意初始条件下，最后将稳定在平衡点 (4)这种基于能量分析稳定性证明办法称为李雅普诺夫第二办法（直接法）。李雅普诺夫办法用于确定下列微分方程稳定性 (5)构造广义能量函数 ，满足：1.具有一阶连续偏导数，

，

2.

，

是指

在系统所有轨迹上变化率。若 ，则系统是渐进稳定，即系统状态收敛于零矢量，不然，系统是稳定（有界）。第80页例1.对于线性系统 ，A是对称正定矩阵，定义李雅普诺夫函数

满足条件1，求导（对时间）得

，（只有

时，导数等于0）满足条件2，系统是稳定。第81页例2.机械手控制稳定性动力学方程 (6)控制律为： (7)系统