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

1、基于旋量理论的机器人建模方法介绍旋量理论旋量理论2. 为什么要学习旋量理论？为什么要学习旋量理论？机器人学研究的有两种主要工具：D-H参数法和旋量理论。相对于D-H参数法，基于旋量理论的方法有两大优点：1. 整体描述刚体运动无需在每个关节处都建立坐标系，只需建立全局坐标系与工具坐标系；旋量坐标模型蕴含各个刚体的空间绝对几何信息，从而可直接得到系统整体的模型。2. 几何描述直观旋量可直观描述刚体运动的几何特点，从而简化分析过程。1. 什么是旋量理论什么是旋量理论 (screw theory)？旋量运动：刚体系统从一个位姿到另一个位姿的运动都可以用绕某直线的转动和沿该直线的移动复合表示，通常称这种

2、复合运动为旋量运动（screw motion）。运动旋量（twist）：旋量运动的无穷小量即为运动旋量。力旋量（wrench）：作用在刚体上的任何力系都可以合成一个沿某直线的合力和绕该直线的合力矩。有关运动旋量的规律同样适用于力旋量！*互易旋量（reciprocal screw）：若力旋量F和运动旋量V具有互易关系，则 F V=0。基本概念基本概念如图，A 表示固定的全局坐标系，B 表示与刚体固定的物体坐标系，则刚体的姿态可描述成一个如下形式的旋转矩阵： R的求解方式一：其中 为物体坐标系主轴方向向量。1. 旋转运动旋转运动3,abababxyzR R3 3;abababxyzRR R旋转矩阵

3、 R 具有如下性质：SO(3)是包含旋转矩阵 R 的一种特殊正交群，我们称之为三维旋转群。 3,det1TSOI RR R RR基本概念基本概念任意的三维空间旋转运动都可以表示为绕某一单位轴 的转动，设转动角度为 ，则旋转矩阵可描述为矩阵指数的形式： R的求解方式二：其中， 是 对应的反对称变换矩阵。将所有的3X3反对称矩阵的矢量空间定义为so(3)。即：33 3eRR R3 3R R 323 331210030soR R2) so(3)和和SO(3)的关系？的关系？ 3 333soeSORR RR R指数映射指数映射关系！关系！1）欧拉定理（欧拉角表示法同样可以描述）欧拉定理（欧拉角表示法同

4、样可以描述 R）：）：基本概念基本概念如图，A 表示固定的全局坐标系，B 表示与刚体固定的物体坐标系，则刚体的位姿矩阵 g 可由刚体的位置矢量 p 和姿态矩阵 R 共同表示，即：注意：g 既能表示刚体的位姿状态，又能表示刚体位姿由一个坐标系到另一个坐标系的坐标变换关系。2. 刚体运动刚体运动4 401pgRR R位姿矩阵 g 具有如下性质：SE(3)被称之为刚体变换群。 3 13,3gSEppSORR基本概念基本概念任意刚体运动都可用绕某一轴的转动加上平行于该轴的移动来实现。假设运动旋量坐标为 ，运动量为 ，则刚体运动变换矩阵可表示为：其中， 是 的运算关系为 (wedge) ，即：所有 构成

5、的空间定义为se(3)。即：6 1; v 3geSE 4 43seR R2) se(3)和和SE(3)的关系？的关系？ 3 333segeSER RR R指数映射指数映射关系！关系！1）Chasles 定理定理: 刚体运动的指数矩阵表示法刚体运动的指数矩阵表示法00vv 如何确定运动旋量坐标是完成基于如何确定运动旋量坐标是完成基于POE公式的机器人建模公式的机器人建模的关键一步！的关键一步！1) 转动关节：转动关节：其中q为转轴上任意一点的坐标，则转动关节对应的运动旋量坐标：2) 移动关节移动关节：移动关节的运动旋量中 w 对应分量为0，即移动关节旋量坐标为： 6 13geSEv，R RR R

6、转动关节转动关节移动关节移动关节6 1, v0 0R R6 1, q vvR R大多数机器人都是由一组通过运动副（关节）联接而成的刚性连杆构成。由关节空间运动量到末端任务空间的转化就是机器人的正向运动学建模过程，即在给定组成运动副的相邻连杆的相对位置的情况下，确定机器人的末端位姿。正向运动学正向运动学 正向运动学定义正向运动学定义运动链描述：指数积公式运动链描述：指数积公式(POE)描述机器人的运动就是要描述由关节的运动而带来的刚体(也就是连杆)之间的位置变化。在如图所示的二自由度机器人，有四个连杆L0, L1, L2及L3，两个关节1和2，其中0号坐标系称为基坐标系，3号坐标系称为工具坐标系

7、。两关节转角分别为 和 ，关节运动旋量坐标为 和 ，则工具坐标系相对于基坐标系的正向运动学关系可表示为：2112212 2031203( ,)( )geeg 0其中的刚体变换矩阵 可看做关节运动对末端位姿的影响尺度。iiiiq11q2q2i ie正向运动学正向运动学 串联开链机器人的正向运动学公式串联开链机器人的正向运动学公式12( )( )n nststgeeeg120对于n个转动/移动关节的串联机器人来说，设 base0 为基坐标系，Tool n+1 为工具坐标系，则应用POE公式计算机器人的正向运动学模型仅需三步完成：1. 计算机器人末端初始位姿 ，2. 计算关节对应的运动旋量坐标 ，3

8、. 代入POE公式：( )stg0iiiiiq( )( )( )( )( )( )( )1( )( )( )( );stststststststststtsstxyzgpR0000R0p0000000 xyz正向运动学正向运动学 SCARA机器人共4个DOF，由三个转动关节和一个移动关节组成，如图为初始位姿下的机器人状态，建立工具坐标系 T 和基坐标系 S ：1. 计算SCARA机器人初始位姿 ：2. 计算关节对应的运动旋量坐标 ：( )stg0i331200( )1XstIllgl00iiiiq11123241234000 ,01000000 ,00001lqqqqll 正向运动学正向运动学

9、 3 3124 4(0)( )( )?( )?( )( )(1)i istststgeeeegpgep12RR02. 计算关节对应的运动旋量坐标 ，i13124120000000010,000000001001iiiilllq 3. 代入POE公式得其正向运动学模型方程：End与正向运动学模型的输入、输出正好相反，逆向运动学由给定的机器人末端位姿解算对应的关节运动量。注意注意：正向运动学解唯一，而逆向运动学可能有多解、唯一解或者无解。逆向运动学逆向运动学 逆向运动学定义逆向运动学定义逆运动学解法介绍逆运动学解法介绍给定机器人运动学正解映射 ，一个期望位姿 ，逆运动学即是解算如下方程：运动学逆解

10、可以分为两种思路：1. 解析解一般是位置级解法1）几何法直接解算：对于简单的平面运动模型，一般直接根据几何关系求解。2）P-K子问题：将运动学逆问题转化成三类P-K子问题，而后通过求解子问题得到逆解。2. 数值解一般是速度级解法即是合理选择数值算法对 进行求解，例如牛顿迭代法( )stdggdgstg( )stdgg逆向运动学逆向运动学 1. 解析解解析解P-K子问题子问题P-K子问题法的基本技巧是：将POE运动学模型应用于某些特殊点，比如两个或多个轴的交点，这样可以将消去这些轴关节的耦合，从而消掉其对应的变化矩阵 ：Subproblem 1: 绕一个轴的旋转 ，求 Subproblem 2:

11、 绕两个有序轴的旋转，求 和 Subproblem 3: 旋转至给定距离，求epq12eepq qep12ewwepp1). 基于POE公式的机器人正向运动学模型：2). 定义机器人末端的空间速度：3). 由上式展开，得到末端空间速度与关节速度的线性关系：其中，可定义雅克比矩阵：逆向运动学逆向运动学 2. 数值解法数值解法速度级解法速度级解法12( )( )n nststgeeeg120速度级算法是逆运动学常用的方法，首先求解运动学微分模型，即机器人各关节和末端执行器的速度关系（雅克比矩阵）。在速度级上求取机器人逆运动学模型比较快，适合于实时控制，但由于它只得到速度解，所以位置精度难以保证。1

12、1( )( )( )( )sstststststggggV速度级模型速度级模型雅克比矩阵雅克比矩阵1111nnsstststiststiiiiiggggV111( )sstststststnggJqgg逆向运动学逆向运动学 速度级模型速度级模型雅克比矩阵雅克比矩阵4). 将式(3)展开，得雅克比矩阵：其中，Ad 为伴随变换运算符， 表示经刚体运动后运动旋量。5). 机器人速度级逆解可表示成：若为冗余机器人，则运动学解不唯一，可优化：其中，雅克比矩阵的M-P逆矩阵 ，雅克比矩阵的零空间矩阵 。若为欠约束机器人，则运动学解不一定存在。 运动学奇异性是否存在可根据雅克比矩阵雅克比矩阵的秩来判断，当雅

13、克比矩阵降秩则机器人产生了运动学奇异。111111111iiiiqqqqstistiiiiggeeeeAd g 11 sssstststJN JfV sstN J sstJ 1ssststJV1( )sstnJq i 逆向运动学逆向运动学 逆运动学逆运动学牛顿迭代法牛顿迭代法1). 由雅克比矩阵得机器人速度级解：2). 选择速度模型的最小二乘逆解，两边等式同乘 dt ：3). 定义机器人末端位姿的微分为：4). 由(2)得角度微分量，应用关节角度逆解更新方式：5). 关节角度微分满足迭代收敛条件时，得到机器人关节角度值。 sststdgdJdtdt 1sststdJdg1iiidlogstst

14、stddststggdgIggiid算例算例Stanford 机器人是6-DOF机器人（如图），由基座的两个转动关节，一个移动关节和末端的一个球腕关节构成。分析机器人的正逆向运动学解法。正向运动学1. 机器人初始位姿：2.计算关节对应的运动旋量坐标：3. 将运动旋量带入POE公式：3100( )1stIlgl 00,iiiiiiqq6 612( )( )ststgeeeg120算例算例逆向运动学逆向运动学可通过求解雅克比矩阵得到，而雅克比矩阵的列即是机器人关节实时的运动旋量坐标值，在此，给出基于雅克比矩阵的运动学建模方法：1. 旋量坐标变换求解：1) 第一二、三个关节的运动旋量：,iiiiiiqq1231211201120100211112300sc0s0 ,0 ,cc0cs00,0c0c00s0 ,s,011110lqqvlll 121223scccs,000算例算例逆向运动学2) 球腕关节，即第四、五、六个关节的运动旋量：4561214124512414125121412451241412522424525sssssssscs,ss,sscsssc ccccccscc cc