基于旋量理论的机器人建模方法介绍

基于旋量理论的机器人建模方法介绍机器人运动学、动力学及其控制，本质上就是基于对单刚体或者多刚体系统的运动问题研究!旋量理论2.为什么要学习旋量理论？机器人学研究的有两种主要工具：D-H参数法和旋量理论。相对于D-H参数法，基于旋量理论的方法有两大优点：1.整体描述刚体运动无需在每个关节处都建立坐标系，只需建立全局坐标系与工具坐标系；旋量坐标模型蕴含各个刚体的空间绝对几何信息，从而可直接得到系统整体的模型。2.几何描述直观旋量可直观描述刚体运动的几何特点，从而简化分析过程。1.什么是旋量理论(screwtheory)？旋量运动：刚体系统从一个位姿到另一个位姿的运动都可以用绕某直线的转动和沿该直线的移动复合表示，通常称这种复合运动为旋量运动（screwmotion）。•运动旋量（twist）：旋量运动的无穷小量即为运动旋量。•力旋量（wrench）：作用在刚体上的任何力系都可以合成一个沿某直线的合力和绕该直线的合力矩。——有关运动旋量的规律同样适用于力旋量！•*互易旋量（reciprocalscrew）：若力旋量F和运动旋量V具有互易关系，则FV=0。基本概念旋转运动的定义：如图，A表示固定的全局坐标系，B表示与刚体固定的物体坐标系，则刚体的姿态可描述成一个如下形式的旋转矩阵：R的求解方式一：其中为物体坐标系主轴方向向量。1.旋转运动3,,abababxyzR33;;abababxyzRR旋转矩阵R具有如下性质：SO(3)是包含旋转矩阵R的一种特殊正交群，我们称之为三维旋转群。3,det1TSOIRRRRR基本概念旋转运动的矩阵指数表示法ω任意的三维空间旋转运动都可以表示为绕某一单位轴的转动，设转动角度为，则旋转矩阵可描述为矩阵指数的形式：R的求解方式二：其中，是对应的反对称变换矩阵。将所有的3X3反对称矩阵的矢量空间定义为so(3)。即：3ˆ33eωRR33ˆωRω323331210ˆ030soωR2)so(3)和SO(3)的关系？33ˆˆ33soeSOωωRRR指数映射关系！1）欧拉定理（欧拉角表示法同样可以描述R）：基本概念刚体运动定义：如图，A表示固定的全局坐标系，B表示与刚体固定的物体坐标系，则刚体的位姿矩阵g可由刚体的位置矢量p和姿态矩阵R共同表示，即：注意：g既能表示刚体的位姿状态，又能表示刚体位姿由一个坐标系到另一个坐标系的坐标变换关系。2.刚体运动4401pgRR位姿矩阵g具有如下性质：SE(3)被称之为刚体变换群。313,,3gSEppSORR基本概念刚体运动的矩阵指数表示法任意刚体运动都可用绕某一轴的转动加上平行于该轴的移动来实现。假设运动旋量坐标为，运动量为，则刚体运动变换矩阵可表示为：其中，是的运算关系为∧(wedge)，即：所有构成的空间定义为se(3)。即：61;vξωˆ3geSEξξ44ˆ3seξR2)se(3)和SE(3)的关系？33ˆˆ33segeSEξξRR指数映射关系！1）Chasles定理:刚体运动的指数矩阵表示法ˆξˆˆ00vvξξωωˆξ运动学分析准备工作——运动旋量坐标如何确定运动旋量坐标是完成基于POE公式的机器人建模的关键一步！！1)转动关节：其中q为转轴上任意一点的坐标，则转动关节对应的运动旋量坐标：2)移动关节：移动关节的运动旋量中w对应分量为0，即移动关节旋量坐标为：ˆ613geSEξvξω，RR转动关节移动关节61,vω0ξ0R61,qvvωξωRξ大多数机器人都是由一组通过运动副（关节）联接而成的刚性连杆构成。由关节空间运动量到末端任务空间的转化就是机器人的正向运动学建模过程，即在给定组成运动副的相邻连杆的相对位置的情况下，确定机器人的末端位姿。正向运动学正向运动学定义运动链描述：指数积公式(POE)描述机器人的运动就是要描述由关节的运动而带来的刚体(也就是连杆)之间的位置变化。在如图所示的二自由度机器人，有四个连杆L0,L1,L2及L3，两个关节1和2，其中0号坐标系称为基坐标系，3号坐标系称为工具坐标系。两关节转角分别为和，关节运动旋量坐标为和，则工具坐标系相对于基坐标系的正向运动学关系可表示为：211ξ2ξ2122031203ˆˆ(,)()geegξξ0其中的刚体变换矩阵可看做关节运动对末端位姿的影响尺度。iiiiqξ11q2q2ˆiieξ正向运动学串联开链机器人的正向运动学公式12ˆˆˆ()()nnststgeeeg12ξξξθ0对于n个转动/移动关节的串联机器人来说，设base0为基坐标系，Tooln+1为工具坐标系，则应用POE公式计算机器人的正向运动学模型仅需三步完成：1.计算机器人末端初始位姿，2.计算关节对应的运动旋量坐标，3.代入POE公式：()stg0ˆiξiiiiqξ()()()()()()()1()()()();;;;stststststststststtsstxyzgpR0000R0p0000000xyz正向运动学SCARA机器人算例：SCARA机器人共4个DOF，由三个转动关节和一个移动关节组成，如图为初始位姿下的机器人状态，建立工具坐标系T和基坐标系S：1.计算SCARA机器人初始位姿：2.计算关节对应的运动旋量坐标：()stg0ˆiξ331200()1XstIllgl00iiiiqξ11123241234000,01000000,,,00001lqqqqll正向运动学SCARA机器人算例：331244ˆˆˆˆˆ(0)()()?()?()()(1)iistststgeeeegpgep12ξξξξξRRθ0θθθθθ2.计算关节对应的运动旋量坐标，ˆiξ13124120000000010,,,000000001001iiiilllqξξξξξ3.代入POE公式得其正向运动学模型方程：End与正向运动学模型的输入、输出正好相反，逆向运动学由给定的机器人末端位姿解算对应的关节运动量。注意：正向运动学解唯一，而逆向运动学可能有多解、唯一解或者无解。逆向运动学逆向运动学定义逆运动学解法介绍给定机器人运动学正解映射，一个期望位姿，逆运动学即是解算如下方程：运动学逆解可以分为两种思路：1.解析解——一般是位置级解法1）几何法直接解算：对于简单的平面运动模型，一般直接根据几何关系求解。2）P-K子问题：将运动学逆问题转化成三类P-K子问题，而后通过求解子问题得到逆解。2.数值解——一般是速度级解法即是合理选择数值算法对进行求解，例如牛顿迭代法…()stdggθdgstg()stdggθ逆向运动学1.解析解——P-K子问题P-K子问题法的基本技巧是：将POE运动学模型应用于某些特殊点，比如两个或多个轴的交点，这样可以将消去这些轴关节的耦合，从而消掉其对应的变化矩阵：Subproblem1:绕一个轴的旋转，求Subproblem2:绕两个有序轴的旋转，求和Subproblem3:旋转至给定距离，求ˆepq12ˆˆeepqˆqep12ˆeˆwwepp1).基于POE公式的机器人正向运动学模型：2).定义机器人末端的空间速度：3).由上式展开，得到末端空间速度与关节速度的线性关系：其中，可定义雅克比矩阵：逆向运动学2.数值解法——速度级解法12ˆˆˆ()()nnststgeeeg12ξξξθ0速度级算法是逆运动学常用的方法，首先求解运动学微分模型，即机器人各关节和末端执行器的速度关系（雅克比矩阵）。在速度级上求取机器人逆运动学模型比较快，适合于实时控制，但由于它只得到速度解，所以位置精度难以保证。11()ˆ()()()sstststststggggθVθθθθθ速度级模型——雅克比矩阵1111ˆnnsstststiststiiiiiggggV111()sstststststnggJqgg逆向运动学速度级模型——雅克比矩阵4).将式(3)展开，得雅克比矩阵：其中，Ad为伴随变换运算符，表示经刚体运动后运动旋量。5).机器人速度级逆解可表示成：若为冗余机器人，则运动学解不唯一，可优化：其中，雅克比矩阵的M-P逆矩阵，雅克比矩阵的零空间矩阵。若为欠约束机器人，则运动学解不一定存在。运动学奇异性是否存在可根据雅克比矩阵的秩来判断，当雅克比矩阵降秩则机器人产生了运动学奇异。1111111ˆˆˆˆ11iiiiqqqqstistiiiiggeeeeAdg11ξξξξξξξˆsssstststJNJfθθVθsstNJθsstJθ1ˆssststJθθV1()sstnJqξξiξ逆向运动学逆运动学——牛顿迭代法1).由雅克比矩阵得机器人速度级解：2).选择速度模型的最小二乘逆解，两边等式同乘dt：3).定义机器人末端位姿的微分为：4).由(2)得角度微分量，应用关节角度逆解更新方式：5).关节角度微分满足迭代收敛条件时，得到机器人关节角度值。sststdgdJdtdtθθ1sststdJdgθθ1iiidθθθlogstststddststggdgIggiidθθθ算例算例——Stanford机械臂Stanford机器人是6-DOF机器人（如图），由基座的两个转动关节，一个移动关节和末端的一个球腕关节构成。分析机器人的正逆向运动学解法。正向运动学1.机器人初始位姿：2.计算关节对应的运动旋量坐标：3.将运动旋量带入POE公式：3100()1stIlgl00,iiiiiiqqξ6612ˆˆˆ()()ststgeeeg12ξξξθ0算例算例——Stanford机械臂逆向运动学逆向运动学可通过求解雅克比矩阵得到，而雅克比矩阵的列即是机器人关节实时的运动旋量坐标值，在此，给出基于雅克比矩阵的运动学建模方法：1.旋量坐标变换求解：1)第一二、三个关节的运动旋量：,iiiiiiqqξ1231211201120100211112300sc0s0,0,cc0cs00,0c0c00s0,s,011110lqqvlllξξ121223scccs,000ξ算例算例——Stanford机械臂逆向运动学2)球腕关节，即第四、五、六个关节的运动旋量：456121412451