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

R SO 3 R RT R I , det R 1


SO(3)是包含旋转矩阵 R 的一种特殊正交群，我们 称之为三维旋转群。
基本概念
旋转运动的矩阵指数表示法 1）欧拉定理（欧拉角表示法同样可以描述 R）： 任意的三维空间旋转运动都可以表示为绕某一单位 3 轴 的转动，设转动角度为 ，则旋转矩阵 可描述为矩阵指数的形式：
n g st 1 n g st 1 s ˆ Vst i g st g st i i 1 i 1 i i
g g s 1 1 st st g st g st 其中，可定义雅克比矩阵：J st (q) 1 n
1
2
i qi ξi i
q1
q2
g03 (1 ,2 ) e
ˆ
ˆ ξ 1 2
e
ˆ ξ 2 2
g03 (0)
其中的刚体变换矩阵 eξii 可看做关节运动对末端位姿的影响尺度。
正向运动学
串联开链机器人的正向运动学公式 对于n个转动/移动关节的串联机器人来说，设 base0 为基坐标系，Tool n+1 为工具 坐标系，则应用POE公式计算机器人的正向运动学模型仅需三步完成： 1. 计算机器人末端初始位姿 gst (0) ，
ˆ v v ω ω 0 0
ξ ˆ ξ

ˆ se 3 R 44 ˆ 构成的空间定义为se(3)。即： ξ 所有 ξ
2) se(3)和SE(3)的关系？ 指数映射 关系！
ˆ se 3 R 33 ξ ˆ ξ g e SE 3 R
ˆ
e1 e2 p q
Subproblem 3: 旋转至给定距离，求
ˆ
ˆ
q e p
ˆ
逆向运动学
2. 数值解法——速度级解法 速度级算法是逆运动学常用的方法，首先求解运动学微分模型，即机器人各关节和 末端执行器的速度关系（雅克比矩阵）。在速度级上求取机器人逆运动学模型比较 快，适合于实时控制，但由于它只得到速度解，所以位置精度难以保证。 速度级模型——雅克比矩阵 1). 基于POE公式的机器人正向运动学模型：



5). 机器人速度级逆解可表示成： J s θ 1 V ˆs θ st st
刚体运动的矩阵指数表示法 1）Chasles 定理: 刚体运动的指数矩阵表示法 任意刚体运动都可用绕某一轴的转动加上平行于该轴的移动来实现。假设运动旋量 坐标为 ξ v; ω 61，运动量为 ，则刚体运动变换矩阵可表示为：
g eξ SE 3
ˆ
ˆ 是 ξ 的运算关系为∧ (wedge) ，即： ξ 其中，
gst (θ) eξ11 eξ22 eξnn gst (0)
ˆ ˆ ˆ
2). 定义机器人末端的空间速度：
g st (θ) 1 1 ˆs g V ( θ ) g ( θ ) θ g st (θ) st st st θ
3). 由上式展开，得到末端空间速度与关节速度的线性关系：
基本概念
1. 旋转运动
旋转运动的定义：
如图，A 表示固定的全局坐标系，B 表示与刚体固 定的物体坐标系，则刚体的姿态可描述成一个如下 形式的旋转矩阵：
R的求解方式一： R xab ; yab ; zab R
3
33
其中 xab , yab , zab R 为物体坐标系主轴方向向量。 旋转矩阵 R 具有如下性质：
R st (0) p st (0) R st (0) x st (0); y st (0); z st (0) g st (0) 0 1 p ( 0 ) x ( 0 ) ; y ( 0 ) ; z ( 0 ) s t st st s t
3. 代入POE公式得其正向运动学模型方程：
g st (θ) eξ11 eξ22 eξ33 eξ44 g st (0) eξii R (θ) g st (θ) 0 p(θ) 1
End
ˆ
ˆ
ˆ
ˆ
ˆ

R (θ) ? p(θ) ?
逆向运动学
逆向运动学定义 与正向运动学模型的输入、输出正好相反，逆向运动学由给定的机器人末端位姿解 算对应的关节运动量。 注意：正向运动学解唯一，而逆向运动学可能有多解、唯一解或者无解。 逆运动学解法介绍 给定机器人运动学正解映射 g st ，一个期望位姿 gd ，逆运动学即是解算如下方程：
逆向运动学
速度级模型——雅克比矩阵 4). 将式(3)展开，得雅克比矩阵：
s ξ J st (q) ξ1 n
1 g st 1 ˆ q ˆ q ˆ q ˆ q ξ ξ ξ ξ i 1 i 1 i 1 i 1 1 1 1 1 g st e e ξi e e Ad gi 1 ξ i 其中， ξ i i Ad 为伴随变换运算符，ξ i 表示经刚体运动后运动旋量。
R g 0
p 44 R 1
注意：g 既能表示刚体的位姿状态，又能表示 刚体位姿由一个坐标系到另一个坐标系的坐 标变换关系。
位姿矩阵 g 具有如下性质：
g SE 3
p, R p
31
, R SO 3

SE(3)被称之为刚体变换群。
基本概念
gst (θ) gd
运动学逆解可以分为两种思路： 1. 解析解——一般是位置级解法 1）几何法直接解算： 对于简单的平面运动模型，一般直接根据几何关系求解。 2）P-K子问题： 将运动学逆问题转化成三类P-K子问题，而后通过求解子问题得到逆解。 2. 数值解——一般是速度级解法 即是合理选择数值算法对 gst (θ) gd 进行求解，例如牛顿迭代法…
运动学分析准备工作——运动旋量坐标
如何确定运动旋量坐标是完成基于POE公式的机器人建模 的关键一步！！
ξ
1) 转动关节： 其中q为转轴上任意一点的坐标，则 转动关节对应的运动旋量坐标：
v v ω q, ξ R61 ω
2) 移动关节： 移动关节的运动旋量中 w 对应分量 为0，即移动关节旋量坐标为：
逆向运动学
1. 解析解——P-K子问题
e pw pw
ˆ
P-K子问题法的基本技巧是：将POE运动学模型应用于某些特殊点，比如两个或多 ˆ 个轴的交点，这样可以将消去这些轴关节的耦合，从而消掉其对应的变化矩阵 e ： Subproblem 1: 绕一个轴的旋转 ，求
e p q
Subproblem 2: 绕两个有序轴的旋转，求 1 和 2
转动关节 移动关节
v ω 0, ξ R61 0
v ˆ R，ξ R61 g eξ SE 3 ω
正向运动学
正向运动学定义 大多数机器人都是由一组通过运动副（关节）联接而成的刚性连杆构成。由关节空 间运动量到末端任务空间的转化就是机器人的正向运动学建模过程，即在给定组成 运动副的相邻连杆的相对位置的情况下，确定机器人的末端位姿。 运动链描述：指数积公式(POE) 描述机器人的运动就是要描述由关节的运动 而带来的刚体(也就是连杆)之间的位置变化。 在如图所示的二自由度机器人，有四个连杆 L0, L1, L2及L3，两个关节1和2，其中0号 坐标系称为基坐标系，3号坐标系称为工具 坐标系。两关节转角分别为 1 和 2 ，关节 运动旋量坐标为 ξ 1 和 ξ2 ，则工具坐标系相 对于基坐标系的正向运动学关系可表示为：
基于旋量理论的机器人建模方法介绍
机器人运动学、动力学及其控制，本质上就是基 于对单刚体或者多刚体系统的运动问题研究!
旋量理论
1. 什么是旋量理论 (screw theory)？ 旋量运动：刚体系统从一个位姿到另一个位姿的运动都可以用绕某直线的转动和沿 该直线的移动复合表示，通常称这种复合运动为旋量运动（screw motion）。 • 运动旋量（twist）： 旋量运动的无穷小量即为运动旋量。 • 力旋量（wrench）： 作用在刚体上的任何力系都可以合成一个沿某直线的合力和绕该直线的合力矩。 ——有关运动旋量的规律同样适用于力旋量！ • *互易旋量（reciprocal screw）： 若力旋量F和运动旋量V具有互易关系，则 F V=0。 2. 为什么要学习旋量理论？ 机器人学研究的有两种主要工具：D-H参数法和旋量理论。相对于D-H参数法，基 于旋量理论的方法有两大优点： 1. 整体描述刚体运动 无需在每个关节处都建立坐标系，只需建立全局坐标系与工具坐标系；旋量坐标模 型蕴含各个刚体的空间绝对几何信息，从而可直接得到系统整体的模型。 2. 几何描述直观 旋量可直观描述刚体运动的几何特点，从而简化分析过程。
ˆ R的求解方式二： R eω R33
ω

ˆ R 是 ω 对应的反对称变换矩阵。 ω 其中， 将所有的3X3反对称矩阵的矢量空间 3 ω 0 1 0 2 1
正向运动学
SCARA机器人算例：
ˆ ， 2. 计算关节对应的运动旋量坐标 ξ i
l1 0 l1 l2 0 0 0 0 0 0 0 0 1 i qi ξi ξ1 , ξ 2 , ξ 3 , ξ4 i 0 0 0 0 0 0 0 0 1 0 0 1
ˆ ， 2. 计算关节对应的运动旋量坐标 ξ i
q ξi i i i
3. 代入POE公式：
gst (θ) eξ11 eξ22 eξnn gst (0)
ˆ ˆ ˆ
正向运动学
SCARA机器人算例： SCARA机器人共4个DOF，由三个转动关节和一个移动关节组成，如图为初始位姿 下的机器人状态，建立工具坐标系 T 和基坐标系 S ： 1. 计算SCARA机器人初始位姿 gst (0) ：
2) so(3)和SO(3)的关系？ 指数映射 关系！
33
ˆ so 3 R33 ω ˆ ω R e SO 3 R
基本概念
2. 刚体运动 刚体运动定义： 如图，A 表示固定的全局坐标系，B 表示与刚 体固定的物体坐标系，则刚体的位姿矩阵 g 可由刚体的位置矢量 p 和姿态矩阵 R 共同表 示，即：
I g st (0) 3 X 3 0 0 l l 1 2 l0 1
ˆ ： 2. 计算关节对应的运动旋量坐标 ξ i
0 0 , 0 1 2 3 0 4 i q 1 i0 ξi i 0 0 0 0 , q l , q l l , q 0 q1 0 2 1 3 1 2 4 0 0 0 1