第2章数学基础.pdf

第二章第二章 数学基础数学基础 Ch 2 Mathematical Basis 参考 参考 Saeed B Niku 机器人学导论机器人学导论 分析 系统及分析 系统及 应用应用 第第2章章 2 3 2 6节节 电子工业出版社电子工业出版社 2004年年 哈尔滨工业大学哈尔滨工业大学 威海威海 2 本章内容本章内容 2 1 位置和姿态的表示位置和姿态的表示 2 2 坐标变换坐标变换 2 3 齐次坐标变换齐次坐标变换 2 4 物体的变换及逆变换物体的变换及逆变换 2 5 通用旋转变换通用旋转变换 2 6 小结小结 哈尔滨工业大学哈尔滨工业大学 威海威海 3 2 1 位置和姿态的表示位置和姿态的表示 2 1 1 位置描述位置描述 2 1 2 方位描述方位描述 2 1 3 位姿描述位姿描述 2 1 位置和姿态的表示位置和姿态的表示 哈尔滨工业大学哈尔滨工业大学 威海威海 4 2 1 位置和姿态的表示位置和姿态的表示 p x A y z p p p 2 1 图图2 1 位置表示位置表示 对于直角坐标系对于直角坐标系 A 空间空间 任一点任一点 p 的位置可用的位置可用3 1的的 列矢量列矢量Ap表示表示 xyz ppp 是该向量在参考坐标系是该向量在参考坐标系 A 中的三个坐标分中的三个坐标分 量量 Ap的上标的上标A代表代表参考坐标系参考坐标系 A 我们称我们称Ap为位为位 置矢量置矢量 2 1 1 位置位置 position 描述描述 哈尔滨工业大学哈尔滨工业大学 威海威海 5 2 1 2 方位方位 orientation 描述描述 2 1 位置和姿态的表示位置和姿态的表示 图图2 2 方位表示方位表示 物体的方位可由某个物体的方位可由某个固接于固接于 此物体的坐标系此物体的坐标系描述描述 为了为了 规定空间某刚体规定空间某刚体B的方位的方位 设置一直角坐标系设置一直角坐标系 B 与此与此 刚体固接刚体固接 用坐标系用坐标系 B 的的 三个单位矢量三个单位矢量相对于参考坐相对于参考坐 标系标系 A 的方向余弦组成的方向余弦组成 3 3矩阵矩阵来表示刚体来表示刚体B相对相对 于坐标系于坐标系 A 的方位的方位 哈尔滨工业大学哈尔滨工业大学 威海威海 6 111213 212223 313233 AAAA BBBB rrr Rxyzrrr rrr 2 2 称为称为旋转矩阵旋转矩阵 式中 上标 式中 上标A代表代表参考坐标系参考坐标系 A 下标 下标B代表代表被描述的坐标系被描述的坐标系 B A B R 1 AAAAAA BBBBBB xxyyzz 正交条件 正交条件 0 AAAAAA BBBBBB xyyzzx 哈尔滨工业大学哈尔滨工业大学 威海威海 7 旋转矩阵是旋转矩阵是正交矩阵正交矩阵 并且满足条件 并且满足条件 A B R 1AAT BB RR 复习 正交矩阵复习 正交矩阵 定义 定义 一个一个n阶实矩阵阶实矩阵A满足 满足 AA A A I 则 则 这个这个n阶实矩阵阶实矩阵A叫做一个正交矩阵 叫做一个正交矩阵 结论 结论 则下列诸条件是等价的 则下列诸条件是等价的 1 A 是正交矩阵是正交矩阵 2 A A I 为单位矩阵为单位矩阵 A 是转置矩阵 是转置矩阵 3 A 是正交矩阵是正交矩阵 4 A的各行是单位向量且两两正交的各行是单位向量且两两正交 5 A的各列是单位向量且两两正交的各列是单位向量且两两正交 哈尔滨工业大学哈尔滨工业大学 威海威海 8 2 1 位置和姿态的表示位置和姿态的表示 方位描述方位描述 2 8 0 0 001 cs R zsc 100 0 0 R xcs sc 0 010 0 cs R y sc 2 7 2 6 推导过程见推导过程见 Niku P36 38 标量标量 rij 可用每个矢量可用每个矢量 在其参考坐标系中单在其参考坐标系中单 位方向上投影的分量位方向上投影的分量 来表示来表示 111213 212223 313233 A B rrr Rrrr rrr 哈尔滨工业大学哈尔滨工业大学 威海威海 9 2 1 位置和姿态的表示位置和姿态的表示 相对参考系相对参考系 A 坐标系坐标系 B 的原点位置和坐标轴的原点位置和坐标轴 的方位的方位 分别由分别由位置矢量位置矢量 和和旋转矩阵旋转矩阵 描描 述述 这样这样 刚体的位姿刚体的位姿 位置和姿态位置和姿态 可由坐标系可由坐标系 B 来描述来描述 即有即有 2 9 AA BBo BRp A B R A Bo p 2 1 3 位姿描述位姿描述 要完全描述刚体要完全描述刚体B在空间的位姿 位置和姿态 在空间的位姿 位置和姿态 通常将物体通常将物体B与某一坐标系与某一坐标系 B 相固接 相固接 B 的坐的坐 标原点一般选在物体标原点一般选在物体B的特征点上 如质心等 的特征点上 如质心等 哈尔滨工业大学哈尔滨工业大学 威海威海 10 A B RI 当当表示位置表示位置时 式 时 式 2 9 中的旋转矩阵 中的旋转矩阵 表示坐标系表示坐标系 B 与参考系与参考系 A 各坐标轴方位一致 各坐标轴方位一致 所以所以 描述坐标系描述坐标系 B 的位姿矩阵的位姿矩阵既可以表示位既可以表示位 置置 也可以表示方位也可以表示方位 当当表示方位表示方位时 式 时 式 2 9 中的位置矢量 中的位置矢量 表示坐标系表示坐标系 B 与参考系与参考系 A 的原点重合的原点重合 Bo表示表示 坐标系坐标系 B 的原点的原点 0 A Bo p 返回主目录返回主目录 哈尔滨工业大学哈尔滨工业大学 威海威海 11 2 2 坐标变换坐标变换 2 2 坐标坐标变换变换 2 2 1 平移坐标变换平移坐标变换 2 2 2 旋转坐标变换旋转坐标变换 2 2 3 复合变换复合变换 哈尔滨工业大学哈尔滨工业大学 威海威海 12 2 2 坐标变换坐标变换 2 2 1 平移坐标变换平移坐标变换 2 10 Bo ABA ppp 图图2 3 平移变换平移变换 A Bo p B 相对于相对于 A 的平移矢量 的平移矢量 B p 点 点p在坐标系在坐标系 B 中的位置 中的位置 A p 点 点p相对于坐标系相对于坐标系 A 的位置矢量 的位置矢量 哈尔滨工业大学哈尔滨工业大学 威海威海 13 2 2 坐标变换坐标变换 2 2 2 旋转坐标变换旋转坐标变换 2 11 AAB B pR p 图图2 4 旋转变换旋转变换 1BAAT ABB R RR 2 12 设坐标系设坐标系 B 与与 A 有共同的坐标原点有共同的坐标原点 但两者的方但两者的方 位不同位不同 如图如图2 4所示所示 用旋转矩阵用旋转矩阵 描述描述 B 相对相对 于于 A 的方位的方位 同一点同一点p在两个坐标系在两个坐标系 B 和和 A 中的中的 描述描述 和和 具有如下的变换关系 具有如下的变换关系 R A B p A p B 用用 描述坐标系描述坐标系 A 相对于相对于 B 的方位 的方位 R B A 哈尔滨工业大学哈尔滨工业大学 威海威海 14 在坐标系旋转前后的点的坐标在坐标系旋转前后的点的坐标 x y z n o a p x p y p z p 旋转前旋转前 a 旋转后旋转后 b x y z n o a p x p y p z p 哈尔滨工业大学哈尔滨工业大学 威海威海 15 相对于参考坐标系的点的坐标和从相对于参考坐标系的点的坐标和从x轴上观察旋转坐标系轴上观察旋转坐标系 12 34 cossin sincos xn yoa zoa pp pllpp pllpp 100 0cossin 0sincos xn yo a z pp pp p p xyznoa pRot xp 100 0 0 xcs sc R y z o a p y p z p 1 l 2 l 3 l 4 l a p a p o p 哈尔滨工业大学哈尔滨工业大学 威海威海 16 绕轴的旋转绕轴的旋转 0 0 001 cs zsc R 100 0 0 xcs sc R 0 010 0 cs y sc R 2 8 2 7 2 6 哈尔滨工业大学哈尔滨工业大学 威海威海 17 2 2 坐标变换坐标变换 2 2 3 复合变换复合变换 2 13 AABA BBo pR pp 图图2 5 复合变换复合变换 哈尔滨工业大学哈尔滨工业大学 威海威海 18 哈尔滨工业大学哈尔滨工业大学 威海威海 19 返回主目录返回主目录 哈尔滨工业大学哈尔滨工业大学 威海威海 20 2 3 齐次坐标变换齐次坐标变换 2 3 齐次坐标变换齐次坐标变换 2 3 1 齐次变换齐次变换 2 3 2 平移齐次坐标变换平移齐次坐标变换 2 3 3 旋转齐次坐标变换旋转齐次坐标变换 哈尔滨工业大学哈尔滨工业大学 威海威海 21 2 3 齐次坐标变换齐次坐标变换 已知一直角坐标系中的某点坐标已知一直角坐标系中的某点坐标 则该点在另一直则该点在另一直 角坐标系中的坐标可通过角坐标系中的坐标可通过齐次坐标变换齐次坐标变换求得求得 所谓所谓齐次坐标就是将一个原本是齐次坐标就是将一个原本是n维的向量用一个维的向量用一个 n 1维向量来表示维向量来表示 一个向量的齐次表示是不唯一的一个向量的齐次表示是不唯一的 比如齐次坐标比如齐次坐标 8 4 2 4 2 1 表示的都是二维点表示的都是二维点 2 1 齐次坐标提供了用齐次坐标提供了用矩阵运算矩阵运算把二维把二维 三维甚至高维三维甚至高维 空间中的一个点集从一个坐标系变换到另一个坐标空间中的一个点集从一个坐标系变换到另一个坐标 系的有效方法系的有效方法 1 pp xwx x ywy y zwz z w 2 3 1 齐次变换齐次变换 齐次坐标齐次坐标 哈尔滨工业大学哈尔滨工业大学 威海威海 22 kcj bi av T V x y x y zw z w 式中式中i j k为为x y z 轴上的单位矢量轴上的单位矢量 a b c w为比例系数为比例系数 w x w y w z 显然显然 齐次坐标表达并不是齐次坐标表达并不是 唯一的唯一的 随随w值的不同而不值的不同而不 同同 在计算机图学中在计算机图学中 w 作作 为通用比例因子为通用比例因子 它可取任它可取任 意正值意正值 但但在机器人的运动在机器人的运动 分析中分析中 总是取总是取w 1 列列 矩矩 阵阵 齐次坐标齐次坐标 哈尔滨工业大学哈尔滨工业大学 威海威海 23 例 例 kjiV 543 可以表示为 可以表示为 V 3 4 5 1 T 或或 V 6 8 10 2 T 或或 V 12 16 20 4 T 齐次坐标齐次坐标 哈尔滨工业大学哈尔滨工业大学 威海威海 24 齐次坐标与三维直角坐标的区别齐次坐标与三维直角坐标的区别 V点在点在 OXYZ坐标系坐标系 直直 角坐标角坐标 中表示是中表示是唯一唯一 的的 x y z 而在而在齐次坐标齐次坐标中表示可中表示可 以是以是多值多值的的 不同的表不同的表 示方法代表的示方法代表的V点在空点在空 间位置上不变间位置上不变 x y z z y x V o 齐次坐标齐次坐标 哈尔滨工业大学哈尔滨工业大学 威海威海 25 几个特定意义的齐次坐标 几个特定意义的齐次坐标 0 0 0 1 T 坐标原点矢量坐标原点矢量的齐次坐标的齐次坐标 n为任为任 意非零比例系数意非零比例系数 a b c 0 T 表示无限远矢量表示无限远矢量 用来用来表示方向表示方向 1 0 0 0 T 指向无穷远处的指向无穷远处的OX 轴轴 0 1 0 0 T 指向无穷远处的指向无穷远处的OY 轴轴 0 0 1 0 T 指向无穷远处的指向无穷远处的OZ 轴轴 齐次坐标齐次坐标 哈尔滨工业大学哈尔滨工业大学 威海威海 26 两个常用的公式两个常用的公式 规定两矢量的点积为一标量规定两矢量的点积为一标量 而两矢量的交积为另一个与此两相乘矢量所而两矢量的交积为另一个与此两相乘矢量所 决定的平面垂直的矢量决定的平面垂直的矢量 2 17 xxyyzz a ba ba ba b 2 18 yzzyzxxzxyyx a ba ba b ia ba bja ba b k 齐次坐标齐次坐标 哈尔滨工业大学哈尔滨工业大学 威海威海 27 两矢量的交积 行列式形式 两矢量的交积 行列式形式 2 19 xyz xyz ijk abaaa bbb 齐次坐标齐次坐标 哈尔滨工业大学哈尔滨工业大学 威海威海 28 矩阵形式 矩阵形式 2 14 1011 AAAB BBo pRpp AAB B pTp 2 15 AABA BBo pR pp 2 13 01 AA ABBo B Rp T 2 16 齐次变换齐次变换 哈尔滨工业大学哈尔滨工业大学 威海威海 29 哈尔滨工业大学哈尔滨工业大学 威海威海 30 哈尔滨工业大学哈尔滨工业大学 威海威海 31 2 3 齐次坐标变换齐次坐标变换 2 3 2 平移齐次坐标变换平移齐次坐标变换 2 20 100 010 001 0001 a b Trans a b c c 空间中的某点用矢量空间中的某点用矢量 ai bj ck 描述 该点也可表示为 描述 该点也可表示为 对已知矢量对已知矢量u x y z w T进行平移变换所得的矢量进行平移变换所得的矢量v为 为 100 010 001 00011 axxawx wa byybwy wb vTrans a b cu czzcwz wc ww 哈尔滨工业大学哈尔滨工业大学 威海威海 32 2 3 齐次坐标变换齐次坐标变换 2 3 3 旋转齐次坐标变换旋转齐次坐标变换 2 22 1000 00 00 0001 cs Rot x sc 00 0100 00 0001 cs Rot y sc 2 23 00 00 0010 0001 cs sc Rot z 2 24 哈尔滨工业大学哈尔滨工业大学 威海威海 33 哈尔滨工业大学哈尔滨工业大学 威海威海 34 解 解 如果把上述两变换组合在一起如果把上述两变换组合在一起 哈尔滨工业大学哈尔滨工业大学 威海威海 35 2 3 齐次坐标变换齐次坐标变换 图图2 6 旋转次序对变换结果的影响旋转次序对变换结果的影响 a Rot y 90 Rot z 90 b Rot z 90 Rot y 90 哈尔滨工业大学哈尔滨工业大学 威海威海 36 解 解 把上述三变换组合在一起把上述三变换组合在一起 哈尔滨工业大学哈尔滨工业大学 威海威海 37 2 3 齐次坐标变换齐次坐标变换 把旋转变换与平移变换结合起来 变换结果把旋转变换与平移变换结合起来 变换结果 如图如图2 7所示所示 返回主目录返回主目录 图图2 7 平移变换和旋转变换的组合平移变换和旋转变换的组合 哈尔滨工业大学哈尔滨工业大学 威海威海 38 2 4 物体的变换及逆变换物体的变换及逆变换 2 4 1 物体位置描述物体位置描述 2 4 2 齐次变换的逆变换齐次变换的逆变换 2 4 3 变换方程初步变换方程初步 2 4 物体的变换及逆变换物体的变换及逆变换 哈尔滨工业大学哈尔滨工业大学 威海威海 39 2 4 物体的变换及逆变换物体的变换及逆变换 我们可以用描述空间一点的变换方法来描述物体在我们可以用描述空间一点的变换方法来描述物体在 空间的位置和方向空间的位置和方向 例如例如 下图所示物体可由固定下图所示物体可由固定 该物体的坐标系内的六个点来表示该物体的坐标系内的六个点来表示 2 4 1 物体位置描述物体位置描述 哈尔滨工业大学哈尔滨工业大学 威海威海 40 如果首先让物体绕如果首先让物体绕z轴旋转轴旋转90 接着绕接着绕y轴旋转轴旋转90 再沿再沿x轴方向平移轴方向平移4个单位个单位 则该变换可描述为 则该变换可描述为 0014 1000 4 0 0 90 90 0100 0001 TTransRot yRot z 哈尔滨工业大学哈尔滨工业大学 威海威海 41 0014111111446644 1000000044111111 0100002200000044 0001111111111111 上述楔形物体的六个点变换如下 上述楔形物体的六个点变换如下 哈尔滨工业大学哈尔滨工业大学 威海威海 42 2 4 物体的变换及逆变换物体的变换及逆变换 图图2 8 对楔形物体的变换对楔形物体的变换 a 变换前的坐标系变换前的坐标系 b 变换后的坐标系变换后的坐标系 哈尔滨工业大学哈尔滨工业大学 威海威海 43 2 4 物体的变换及逆变换物体的变换及逆变换 2 4 2 齐次变换的逆变换齐次变换的逆变换 2 28 BBC C pTp AABABC BBC pTpT Tp AAB CBC TT T 2 29 2 27 定义复合变换 定义复合变换 哈尔滨工业大学哈尔滨工业大学 威海威海 44 2 4 物体的变换及逆变换物体的变换及逆变换 2 30 0101 01 AABB AABBBoCCo CBC ABABA BCBCoBo RpRp TT T R RR pp 哈尔滨工业大学哈尔滨工业大学 威海威海 45 齐次变换的逆变换齐次变换的逆变换 2 4 物体的变换及逆变换物体的变换及逆变换 0101 AABB ABBBoAAo BA RpRp TT 哈尔滨工业大学哈尔滨工业大学 威海威海 46 2 4 物体的变换及逆变换物体的变换及逆变换 BABAB BoABoAo pR pp 01 ATAT A BBBBo A RRp T BBAAT A AoABoBBo pR pRp 1BAAT ABB RRR AABA BBo pR pp 由 由 哈尔滨工业大学哈尔滨工业大学 威海威海 47 给定一个变换矩阵给定一个变换矩阵 求 求 1001 02 03 0001 A B cs T sc B AT 解 解 1001 023 02301 0001 ATAT A BBBBo A cscsRRp T scsc 2 4 物体的变换及逆变换物体的变换及逆变换 哈尔滨工业大学哈尔滨工业大学 威海威海 48 2 4 物体的变换及逆变换物体的变换及逆变换 必须建立机器人各连杆之间必须建立机器人各连杆之间 机器人与周围环境之机器人与周围环境之 间的运动关系间的运动关系 用于描述机器人的操作用于描述机器人的操作 要要规定各规定各 种坐标系来描述机器人与环境的相对位姿关系种坐标系来描述机器人与环境的相对位姿关系 在在 图图2 9 a 中中 位姿关系可用相应的齐次变换来描述位姿关系可用相应的齐次变换来描述 机器人控制和规划的目标与其他变换之间的关系机器人控制和规划的目标与其他变换之间的关系可可 用空间尺寸链用空间尺寸链 有向变换图有向变换图 来表示来表示 如图如图2 9 b 所示所示 BBSG TSGT TT T T 2 37 2 4 3 变换方程初步变换方程初步 哈尔滨工业大学哈尔滨工业大学 威海威海 49 2 4 物体的变换及逆变换物体的变换及逆变换 变换方程初步变换方程初步 a 机械手与环境间的运动关系机械手与环境间的运动关系 b 对应的有向变换图对应的有向变换图 图图2 9 变换方程及其有向变换图变换方程及其有向变换图 返回主目录返回主目录 BBSG TSGT TT T T 哈尔滨工业大学哈尔滨工业大学 威海威海 50 2 5 通用旋转变换通用旋转变换 2 5 通用旋转变换通用旋转变换 2 5 1 通用旋转变换公式通用旋转变换公式 2 5 2 等效转角与转轴等效转角与转轴 哈尔滨工业大学哈尔滨工业大学 威海威海 51 2 5 通用旋转变换通用旋转变换 2 5 1 通用旋转变换公式通用旋转变换公式 0 0 0 0001 xxyxzzxy xyzyyzyx xzyyzxzz Rot f f f