机器人技术 二、齐次坐标变换

齐 次 坐 标 变 换主 讲 ： 吴 海 彬福州大学机械工程及自动化学院第二讲 主 要 内 容引言点的向量表示单位向量点和向量的齐次表示坐标系的位姿刚体的位姿平移变换旋转变换一般变换 相对参考坐标系的变换相对自身坐标系的变换 引 言 (Introduction) 机 器 人 运 动 学 解 决 的 基 本 问 题 ： 正 向 运 动 学 逆 向 运 动 学 机 器 人 机 构 一 个 自 由 度 情 况 多 个 自 由 度 情 况 误 差 的 反 馈 点 、 向 量 和 坐 标 系 的 传 统 表 示坐标轴的定义kcjbiaP zyx zyxcbaP或kcjbiaP zyx zyxcbaP或 Pzaon Paon PaonT zzz yyyy xxxx非方阵相乘结果的维数发生变化 点 、 向 量 和 坐 标 系 的 齐 次 表 示 在 三 维 向 量 中 加 入 一 比 例 因 子 w； 其 物 理 意 义 是 ， 随 着 W的 改 变 ， 向 量 的 大 小 会 发 生 变 化 ， 而 方 向 不 变 ； W大 于 1， 向 量 的 分 量 变 大 ； W小 于 1， 向 量 的 分 量 变 小 ； 若 W 1， 各 分 量 大 小 不 变 ； 若 W 0， 则 表 示 一 个 无 穷 小 的 向 量 ， 其 方 向 不 变 。 第二章 机器人运动学 zyxcbaP wzyxP wxax wyby wzcz 其中齐次坐标与传统坐标的关系 点 、 向 量 和 坐 标 系 的 齐 次 表 示第二章 机器人运动学 因此，习惯上用W1表示向量的长度，用W0表示向量的方向，而且方向向量一般表示成单位向量的形式。形式如下： 1 zyxcbaP 0 222 222 222 zyx z zyx y zyx x cba c cba b cba aP例：有一向量P（3，5，2），请按如下要求表示成矩阵形式：1、比例因子为2；2、表示为方向的单位向量。 点 、 向 量 和 坐 标 系 的 齐 次 表 示原点重合情况坐标系的齐次表示是由坐标系的三个方向向量和原点位置齐次坐标组成： 1000 zzzz yyyy xxxx Paon Paon PaonF例：如图所示为F坐标系位于参考坐标系中（3，5，7）的位置，它的n轴与x 轴平行，o轴相对于y轴的角度为45度，a轴相对于z的角度为45度。请写出该坐标的齐次表达形式。 点 、 向 量 和 坐 标 系 的 齐 次 表 示第二章 机器人运动学刚体的表示 一个刚体在空间的表示可以这样实现：通过在它上面固连一个坐标系，再将该固连的坐标系在空间表示出来。由于这个坐标系一直固连在该刚体上，所以该刚体相对于坐标系的位姿是已知的。因此，只要这个坐标系可以在空间表示出来，那么这个刚体相对于固定坐标系的位姿也就已知了。由此可知，刚体在参考坐标系的表示与坐标系是完全一样的。 1000 zzzz yyyy xxxxobject Paon Paon PaonF图 约 束 变 量点 、 向 量 和 坐 标 系 的 齐 次 表 示第二章 机器人运动学由刚体（坐标系）在参考坐标系的齐次矩阵表达可知，该矩阵有12个变量，但描述刚体位姿只需要6个变量（自由度）就足够了，因此，齐次矩阵中12个变量之间并不是相互独立的，而是有约束的，约束条件为：1、三个方向向量相互垂直；2、每个单位向量的长度均为1。即：0on 0an 0oa1n 1o 1a 已 知 两 个 向 量 a = ax i + ay j + az k b = bx i + by j + bz k 向 量 的 点 积 是 标 量 。 用 “ ”来 定 义 向 量 点 积 ， 即 a b = ax bx + ay by + az bz 向 量 的 叉 积 是 一 个 垂 直 于 由 叉 积 的 两 个 向 量 构 成 的 平 面 的 向 量 。用 “ ”表 示 叉 积 ， 即 a b = ( a y bz az by ) i + ( az bx ax bz ) j + ( ax by ay bx ) k 可 用 行 列 式 表 示 为 i j k a b = ax ay az bx by bz 例 题 点 、 向 量 和 坐 标 系 的 齐 次 表 示第二章 机器人运动学对于下列坐标系，求解所缺元素的值，并用矩阵来表示这个坐标系。 1000 20? 3?707.0 5?0?F kajaiaooo nnn kji zyxzyx zyx aon 注：三个点积约束条件可以用叉积代替，即：进一步有 齐 次 变 换 矩 阵 变 换 定 义 为 空 间 的 一 个 运 动 ； 当 空 间 的 一 个 坐 标 系 （ 向 量 、 刚 体 、 运 动 坐标 系 ） 相 对 于 固 定 的 参 考 坐 标 系 运 动 时 ， 这一 运 动 可 以 用 类 似 于 表 示 坐 标 系 的 方 式 来 表示 ； 变 换 有 如 下 几 种 形 式 ： 纯 平 移 ， 纯 旋 转 ， 平 移 和 旋 转 的 结 合 。第二章 机器人运动学 纯 平 移 齐 次 变 换 矩 阵第二章 机器人运动学特点：运动过程中姿态不变，坐标方向单位向量保持同一方向不变。),(1000 100 010 001 zyxzyx dddTransdddT 变换矩阵可表示为 100010001000 100 010 001 zzzzz yyyyy xxxxxzzzz yyyy xxxxzyxnew dPaon dPaon dPaonPaon Paon PaondddF变换过程为：注：相对固定坐标系的平移，变换矩阵左 乘 ， 公 式 为 oldzyxnew FdddTransF ),( 例 纯 旋 转 (相 对 坐 标 绕 参 考 坐 标 X轴 ）齐 次 变 换 矩 阵第二章 机器人运动学nx PP sincos21 aoy PPllP cossin43 aoz PPllP aonzyx PPPPPP cossin0 sincos0 001例 必须从原点开始变换！ 纯 旋 转 齐 次 变 换 矩 阵第二章 机器人运动学noaxyz PxRotP ),( cossin0 sincos0 001),(xRot cos0sin 010 sin0cos),(yRot 100 0cossin 0sincos),( zRot也就相当于旋转变换前在固定参考坐标系的初始位置。式中noaP PTP RRUU 图、例注：相对固定坐标系的旋转，变换矩阵左 乘 ， 公 式 为绕x轴旋转可简写成其中同理 纯 旋 转 例 题齐 次 变 换 矩 阵第二章 机器人运动学旋转坐标系中有一点P（2，3，4），此坐标系绕参考坐标系x轴旋转90度。求旋转后该点相对于参考坐标系的坐标。 复 合 变 换 齐 次 变 换 矩 阵第二章 机器人运动学特点：既有平移，又有旋转，而且可以多次。假设坐标系（n，o，a）相对于参考坐标系（x，y，z）依次进行如下变换：1、绕x轴旋转 角；2、平移 ；3、再绕y轴旋转 角。 321 lll noaxyz PxRotlllTransyRotP ),(),(),( 321 注：矩阵的顺序不能变； 相对固定坐标系的平移和旋转，变换矩阵左 乘 。 例 复 合 变 换 例 题 齐 次 变 换 矩 阵相对坐标系的齐次矩阵固连在坐标系（n，o，a）上的点P（7，3，2）经历如下变换，求出变换后该点相对于参考坐标系的坐标。1、绕z轴旋转90度；2、接着绕y轴旋转90度；3、接着再平移（4，-3，7）。 复 合 变 换 例 题 齐 次 变 换 矩 阵第二章 机器人运动学假设（n，o，a）坐标系上的点P（7，3，2）也经历相同变换，但变换顺序按如下进行，求出变换后该点相对于参考坐标系的坐标。1、绕z轴旋转90度；2、接着平移（4，-3，7）；3、接着再绕y轴旋转90度。 相 对 动 坐 标 系 的 变 换齐 次 变 换 矩 阵第二章 机器人运动学相对运动坐标系的变换与相对固定参考坐标系不同，这时需要 右 乘 变 换 矩 阵 而 不 是 左 乘。相对自身的运动即是相对动坐标。相对动坐标是指动坐标系本身相对自身的运动，而不是动坐标系中的点相对动坐标系的运动。如果在一个变换过程中，既有相对固定坐标系的变换，也有相对于动坐标系的变换，则应先写出第一个变换因子，在根据变换的具体过程，依次左乘或右乘变换因子，最后乘以被变换的对象（点或坐标）。 相 对 动 坐 标 系 的 变 换 例 题齐 次 变 换 矩 阵第二章 机器人运动学假设与上例相同的点现在进行相同的变换，但所有变换都是相对当前运动坐标系的，具体变换如下，求变换完成后该点相对于参考坐标系的坐标。1、绕a轴旋转90度；2、然后沿n、o、a轴平移（4，-3，7）；3、接着绕o轴旋转90度。 相 对 动 坐 标 系 的 变 换 例 题齐 次 变 换 矩 阵第二章 机器人运动学坐标系B绕x轴旋转90度，然后沿当前坐标系a轴做了3英寸的平移，然后再绕z轴旋转90度，最后沿当前坐标系o轴做5英寸的平移。1、写出描述该运动的方程；2、求坐标系中的点P（1，5，4）相对于参考坐标系的最终位置。提示：先求 ，再求 BUT PTP BBUU 变 换 矩 阵 的 逆第二章 机器人运动学钻孔点位置的描述： EPPUEHHRRUEU TTTTTT 式中：只有 是未知的，其它都可以通过传感器获得，或本身就是已知的。因此，通过求逆阵就可以求得 。HRT HRT 求 矩 阵 逆 例 题变 换 矩 阵 的 逆第二章 机器人运动学在一个具有六自由度的机器人的第五个连杆上装有照相机，照相机观察物体并测定它相对于照相机坐标系的位置，然后根据以下数据来确定末端执行器要到达物体所必须完成的运动。 1000 5001 0010 31005 camT 1000 4100 0001 00105 HT 1000 4010 2001 2100objcamT 1000 3100 0010 0001EHT objcamcamRobjEEHHR TTTTTTT 5555 objET提示：根据求 ，这可以用于测距 变 换 矩 阵 的 逆求 逆 阵 的 步 骤 ：第二章 机器人运动学1、计算矩阵的行列式；2、将矩阵转置；3、将转置矩阵的每个元素用它的子行列式（伴随矩阵）代替；4、用转换后的矩阵除以行列式AAA *1 即 cossin0 sincos0 001),(xRot例：求的逆阵。满足TAA 1的矩阵称为酉矩阵。 齐 次 矩 阵 的 逆变 换 矩 阵 的 逆第二章 机器人运动学 对于4X4齐次变换矩阵，可以将矩阵分成两部分求逆。其旋转部分仍是酉矩阵，只需要简单的转置；矩阵的位置部分是向量P分别与n、o、a向量点积的取反。 10001 aPaaa oPooo nPnnnT zyx zyx zyx 1000 zzzz yyyy xxxx Paon Paon PaonT即的逆阵为 1000 5010 25.00866.0 3866.005.0T例：求的逆阵。 图 2.12所 示 为 点 A绕 任 意 过 原 点 的 单 位 矢 量 此 旋 转 角 的 情况 。 kx， ky， kz分 别 为 此 矢 量 在 固 定 参 考 系 坐 标 轴 X、 Y、Z上 的 三 个 分 量 ， 可 以 证 得 ， 绕 任 意 过 原 点 的 单 位 矢 量 k转 角 的 旋 转 齐 次 变换 公 式 为 式 (2-18)称 为 一 般 旋 转 齐 次 变 换 通 式 ， 它 概 括了 绕 X轴 、 Y轴 、 Z轴 进 行 旋 转 齐 次 变 换 的 各 种特 殊 情 况 ， 例 如 ： 当 kx=1， 即 ky=kz=0时 ， 则 由 式 (2-18)可 得 到式 (2-16)； 当 ky=1， 即 kx=kz=0时 ， 则 由 式 (2-18)可 得 到式 (2-17)； 当 kz=1， 即 kx=ky=0时 ， 则 由 式 (2-18)可 得 到式 (2-15)。 反 之 ， 若 给 出 某 个 旋 转 齐 次 矩 阵则可根据式 (2-18)求出其等效矢量k及等效转角 式中：当取0到180。之间的值时，式中的符号取+号；当转角时很小时，公式很难确定转轴；当接近0。或180。时，转轴完全不确定。 与 平 移 变 换 一 样 ， 旋 转 变 换 算 子 公 式 (2-15)、 (2-16)、 (2-17)以 及 一 般 旋 转 变 换 算 子 公 式 (2-18)， 不仅 仅 适 用 于 点 的 旋 转 变 换 ,而 且 也 适 用 于 矢 量 、 坐 标 系、 物 体 等 旋 转 变 换 计 算 。 若 相 对 固 定 坐 标 系 进 行 变 换 ,则 算 子 左 乘 ； 若 相 对 动 坐 标 系 进 行 变 换 ， 则 算 子 右 乘。 例 2-5 已 知 坐 标 系 中 点 U的 位 置 矢 量 u=7 3 2 1T 将 此 点 绕 Z轴 旋 转 90， 再 绕 Y轴 旋 转 90， 如 图 2-13所 示， 求 旋 转 变 换 后 所 得 的 点 W。 2-6 如 图 2-14所 示 单 臂 操 作 手 ， 手 腕 也 具 有 一个 自 由 度 。 已 知 手 部 起 始 位 姿 矩 阵 为若手臂绕Z0轴旋转+90，则手部到达G2若手臂不动，仅手部绕手腕Zl轴旋转+90,则手部到达G 3。写出手部坐标系G2及G3的矩阵表达式。