第 3 章：位姿描述和齐次变换

主讲：李文龙、陈文斌

单位：数字制造装备与技术国重

邮箱： wlli@mail.hust.edu.cn

办公：先进制造大楼东楼 D213

版权所有：华中科技大机器人学教研组
位姿描述—机器人铣削修边应用场景

飞机蒙皮加工系统：机器人、扫描仪、跟踪仪、工具、工件

跟踪定 结构光扫描仪 机器人

位单元
跟踪误差 {L }
手眼误差

关节误差

机身空位

航空蒙皮
位姿描述—机器人操作（磨抛）应用场景

大叶片加工工艺：模锻——铣削——变形矫正——磨抛

手工磨抛 机器人磨抛
位姿描述—机器人操作（磨抛）应用场景

机器人磨抛涉及的坐标系：
磨削
装置

待磨
削叶
片

机器人
基 (世
界 )坐
标系

视觉测量坐标系 末端执行器坐标系
本章提纲

3.1 刚体位姿描述

3.2 齐次坐标和齐次变换

3.3 运动算子

3.4 变换矩阵的运算

3.5 欧拉角与 RPY 角

3.6 旋转变换通式

3.7 位姿的综合

3.8 计算的复杂性
3.1 刚体位姿描述

 研究机器人操作臂的运动：涉及各连杆位姿关系、连杆与周围
环境（操作对象和障碍物）的关系。我们把操作臂的各个连杆、
操作对象、工具、工件和障碍物都当成刚体。
 刚体位姿描述：齐次变换（矩阵）、矢量法、四元数

 位姿（ Location ）＝位置（ Position ）＋姿态（ Orientation ）

 齐次变换法
– 将运动、变换和映射与矩阵运算联系起来；
– 在操作臂运动 / 动力学、机器人控制算法、计算机图学、视觉信
息处理、手 - 眼建模标定都有广泛应用。
 描述刚体在坐标系中的相对位姿（描述）； Description
 表示刚体运动前后位姿描述的变换（算子）； Operator
 把点从一个坐标系映射到另一个坐标系（映射）； Mapping
3.1 位置的描述（位置矢量）

 在坐标系｛ A ｝中，空间任意一点可表示为列矢量 Ap

 px 
A 
p   py  
 pz 

直角坐标系

 符号表示：矢量 / 向量等（小写粗体）、矩阵 / 坐标系 / 群等

（大写粗体）、变量 / 角度等（小写非斜体）
3.1 旋转矩阵与旋转群

 坐标系｛ B ｝相对于坐标系｛ A ｝的旋转矩阵

 r11 r12 r13 
A
B R   A xB A
yB A
z B    r21 r22 r23 
 r31 r32 r33 
A {A}
B R 是正交矩阵，满足如下关系：
A
xB  A xB  A yB  A yB  A z B  A z B  1 {B}

A A A A A A
xB  yB  y B  z B  z B  x B  0

 所以旋转矩阵中 9 个元素只有 3 个独立变量

 还满足： R 1
 A T
B R A
;
B det  B R   1
A

上标 T 表示转置， det 表示行列式符

号
3.1 旋转矩阵与旋转群

 绕 x 轴旋转的旋转矩阵  思考？
1 0 0 
 这三个旋转矩阵的结
R( x, )  0 cos   sin  
0 sin  cos   构有什么关系，如果
 绕 y 轴旋转的旋转矩阵 通过记住一个旋转矩
 cos  0 sin   阵而得到其它两个？
R( y, )   0 1 0   如果绕某一个轴多次
  sin  0 cos   旋转，我们会看到什
 绕 z 轴旋转的旋转矩阵 么结果？
cos   sin  0  如果依次绕
R( z , )   sin  cos  0 
x 、 y 、 z 轴旋转，
 0 0 1 
旋转矩阵如何计算？
旋转矩阵描述姿态
3.1 旋转矩阵与旋转群

 满足正交条件和特殊条件的旋转矩阵 R  33 为旋转群SO (3)

SO 3  R  33
: RR  I , det R = +1
T

 旋转群也称为特殊正交群，推广到 n 维空间：

SO  n   R  nn
: RR  I , det R = +1
T

 旋转矩阵对于矩阵乘法满足“群公理条件”：封闭性、
可逆性、结合律，具有逆元和单位元 R  I ，不可交
换
 旋转矩阵集合构成的流形是光滑流形，且矩阵乘法运
算和求逆运算都是光滑映射，因此旋转群是李群
p 3 R  SO 3
 采用位置矢量 描述位置， 描述姿态
3.1 坐标系的描述

 将刚体 B 与坐标系｛ B ｝固接，｛ B ｝的原点选择

刚体质心，相对参考坐标系 {A} ，坐标系｛ B ｝的位
姿： A A
{B}  { B R pBo }

{A}

 如果只表示位置时，坐标系 {B} 是什么形式？

 思考？ {B}
A
RI {B}  {I A
pBo }
答： B
（单位矩阵），

 如果只表示方位时，坐标系 {B} 是什么形式？

A
pBo  0 {B}  { BA R 0}
答： （单位矩阵），
3.1 坐标系的描述

 机器人手爪位姿描述与坐标系相同：
 手爪坐标系——与手爪固接一起的坐标系
– z 轴——手指接近物体的方向，接近矢量 a (approach)
– y 轴——两手指的连线方向，方位矢量 o (orientation)
– x 轴——右手法则规定，法向矢量 n=o × a (normal)
 手爪的方位——旋转矩阵 R ，描述手爪的姿态
R  n o a 
 手爪位姿的描述

T   n o a p
坐标原点位置
3.1 坐标变换

 平移变换方程—｛ A ， B ｝方位相同
zB
zA A
p B
p
A
pBo
A B A
p  p  pB0 A
B yB

xB
yA

xA

zB
 旋转变换方程—｛ A ， B ｝原点相同
zA
A
p  BA R B p B
p
A yA
xB
B A 1 A T B
 正交矩阵： A R R  R
B B xA
yB
3.1 坐标变换

 一般变换方程—｛ A ， B ｝方位和原点均不同
A A B A 坐标系 B 的坐标原点相对
p  R p  pB0
B 于相对于 A 的位置
坐标系 B 相对于
z B C
相对于 A 方位矩
阵 B
zA p
A yB
p
 过渡矩阵—公式 3-13
B oB
C C B A B
p R p R p
B B A A
pBo
o

oA yA
xB
 再由 3-11 得到复合变换 xA
A C A A B A
p  p  pC0  R p  pB0
B
3.1 一般变换实例
 ｛ A ， B ｝初始位姿重合， ｛ B ｝相对于｛ A ｝的
zA 轴转 30 度，再沿 xA 轴移动 10 个单位、沿 yA 轴移
动 5 个单位，求位置矢量和旋转矩阵
 假设 p  3 7 0
B T
求它在坐标系｛ A ｝中的描述。
A
p  BA R B p  A pBo
zA A
z B1
3 10  
  A   zB2 z B 30 y B1
B
p  7  , pBo   5  5
    10 oA yA
 0   0  yB2
xA yB
A
B 
R  R z , 30  x B1
B
 cos 30  sin 30 0
 
  sin 30

cos 30 0  A p
 xB 2 xB
 0 0 1

3.2 齐次坐标和齐次变换

 笛卡尔坐标—齐次坐标
 A p   BA R A
pBo   B p  矩阵形式
A
    p  ABT B p
1   0 1  1 
A
 齐次变换矩阵 T 是 4×4 的方阵，具有如下形式：
B

A A
A  B R pBo 
BT   
 0 1 
 齐次变换：
齐次坐标 齐次坐标

 A p   BA R A
pBo   B p 
 
A
p = BA R B p  A pBo  
1   0 1  1 

旋转矩阵 平移矢量
3.2 齐次坐标和齐次变换

 规定：如下列向量表示空间的无穷远点
a b c 0 , a 2  b 2  c 2  0
T
空间某点直角坐标和齐次坐标

[1 0 0 0]T —— x 轴  x   wx 
 x  y   wy 
[0 1 0 0]T —— y 轴无穷远点 p   y  , p  
 z   wz 
[0 0 1 0]T —— z 轴无穷远点  z     
1   w 
[0 0 0 0]T —— 无意义
[0 0 0 1]T —— 代表 0 点（坐标原点）
 利用齐次坐标不仅可以规定点的位置，还可用来规定
矢量的方向。当第 4 个元素非零时，代表点的位置；
第 4 个元素为零时，代表方向
3.2 齐次坐标和齐次变换

 齐次变换矩阵：
0 0 1 1
1 0 0 3
A
BT 
 
0 1 0 4
 
0 0 0 1

1 ）｛ B ｝坐标原点相对于｛ A ｝的位置是：
 
T
1  3 4 1

2 ）｛ B ｝的三个坐标轴相对于｛ A ｝的方向：
 ｛ B ｝的 x 轴与｛ A ｝的 y 轴同向 ；
 ｛ B ｝的 y 轴与｛ A ｝的 z 轴同向 ；
 ｛ B ｝的 z 轴与｛ A ｝的 x 轴同向 。
3.2 齐次坐标和齐次变换

 齐次变换矩阵：代表坐标平移与旋转的复合，可分解：

 BA R A
pBo   I 33 A
pBo   BA R 0
   
 0 1   0 1   0 1

 可表示为：
A
BT  Trans  A pBo   Rot  k , 

由矢量
A
pBo 决定 由过原点转轴 k 和转角  决定
3.3 运动算子

 平移算子 Trans(AP): 左上角为 3*3 的单位矩阵

A
p2 = A p1 + A p

A
p2 = Trans  A p  A p1

A
 旋转算子 R: p2 = R A p1

A
p2 = R  k ,  A p1
3.3 运动算子

 运动算子的一般形式： A p2 = T A p1
 练习：在坐标系 ｛ A ｝中，点p 的运动轨迹如下：首先
绕 z 轴旋转 30 度，再沿 x 轴平移 10 单位，最后沿 y 轴
p A
平移 5 单位。点 原来位置是
p1 = 3 7 0 
T
，求运动
后位置 。
 0.866 0.5 0 10 
 0.5 0.866 0 5 
T  
 0 0 1 0
 实现上述旋转和平移的运动算子：  
 0 0 0 1

0.866 0.5 0 10   3   9.098 

 0.5 0.866 0 5  7  12.562
A
p2  T A p1       
 0 0 1 0  0   0 
 所以： 
0 0 0 1
  
1 1

    
3.3 变换矩阵的运算

 齐次变换矩阵的物理含义
 坐标系的描述：BT 描述坐标系B 相对于参考系 A 的位姿。其
A

A
中 B R 的各列分别描述B 的 3 个坐标主轴的方向； B
A
pBo 描述
的坐标原点的位置。齐次变换矩阵 A
BT 的前三列表示坐标系 B
相对参考系 A 的三个坐标轴的方向；最后一列表示 B 的原
点。 A
T
B
p  A, B
A B A
 p
BT 坐标映射：
A
p代表同一点
A
BR
pBo
在两个坐标系 描述的映射关
系。 将 T 映射为 。其中 为旋转映射，p 为平移映射。
 运动算子： p1 p2
T 表示在同一坐标系中，点 运动前、后的算子关
系。算子 作用于 得出 。任一算子也可分解为平移算子
与旋转算子的复合。
3.4 变换矩阵相乘

 对于给定的坐标系 {A} 、 {B} 和 {C} ，已知 {B} 相对 {A} 的描

A B
述为
BT ，已知 {C} 相对 {B} CT
的描述为
B
p  CBT C p
A
p  ABT B p  ABT CBT C p

 从而定义复合变换 CAT
A A
A A B  BR pBo   CB R B
pCo 
CT  BT CT    
 031 1  031 1 
 BA R CB R A
B R B pCo  A pBo 
 
 031 1 
3.4 变换矩阵相乘

A A B
CT T T
B C

 变换矩阵 CAT 可解释为坐标系的映射变换；因为CAT 和CBT 分别

代表同一坐标系 C  相对于 A 和B 的描述。上式表示映
A
BT
射变换 C  CBT从
将坐标系 A
映射为
CT
。
 变换矩阵相乘还可作另一种解释：坐标系 C  相对 A 的描
C  A
述 是这样实现的：开始坐标系 与 重合，首先相对于 A
作运动A
BT B
到达 
，然后相对B B
CT
做运动 C 
到达位姿
。
3.4 变换矩阵相乘

 矩阵相乘一般不满足交换律：

A B B A
BT T T T
C C B

 变换矩阵的左乘和右乘的运动解释是不同的：变换顺序“从右
向左”，指明运动是相对固定坐标系而言的；变换顺序“从左
向右”，指明运动是相对运动坐标系而言的。
 变换顺序可调换的两个特例：两次变换都是平移变换、两次变
换为绕同一轴的旋转变换
3.4 变换矩阵求逆

 A
 给定变换： AT  B R
A
pBo 
B  
031 1 

 如何确定： BAT = ABT -1 ？

 计算方法：

 直接计算逆矩阵
B  B
AR
B
p Ao  A
 利用变换关系： AT    T
B
0 0 0 1 
3.4 变换矩阵求逆

 旋转变换的正交性：

B
A R  BA R 1  BA R T

 可以得到：

 pBo   AB R A pBo  B pAo  0

B A

｛ A ｝中某点
B
pAo   AB R A pBo   BA R T A pBo

A T A T A
B  B R  BR pBo 
AT   
0 0 0 1 
3.4 变换矩阵求逆

 练习：坐标系 B 相对于坐标系 A 绕其 z 轴转 30° ，再沿 x

B
T
轴移动 4 个单位，沿 y 轴移动 3 个单位，求 A

解答：因为 A
B T  Trans  4,3,0  Rot  z ,30 
A
B R  0.866 0.5 0 4  A pBo
 0.5 0.866 0 3 
A
BT 
 
 0 0 1 0
 0 0 0 1 

所以  0.866 0.5 0 4.964 
 A T A T A
  0.5 0.866 0 0.598 
R  R p
 
A 1 B B Bo
BT  
0 0 0 1   0 0 1 0 
 
 0 0 0 1 

另一种解释：T  A 1
BT  B
Rot
A  z , 30 
Trans  4, 3,0 
3.4 变换方程

 B 代表基坐标系 ( 基座框)W、

 T 
代表腕框、
S  是工具框、G 是工作站框、 是目标框
{W }

 BST 描述工作站框相对于基座框的位姿；
{T }
{G}
 GST 描述目标框相对于工作站框的位姿； {B}

 WBT 描述腕框相对于基座框的位姿； {S}

G
如何得到 T ？ T 变换方程
{W }
1 ）建立变换方程
B
T  WBT WTT ,
T
B
TT  BST GST GTT {T }
{B}
2 ）通过方程计算
{G}
G
T  GST 1 BST 1 WBT WTT
T {S}
空间尺寸链
3.4 刚体变换群

 任一刚体的位姿由  p, R : p  , R  SO 3
3
决定，定义刚体变
换群
SE 3   p, R : p 3 , R  SO 3  3  SO 3（乘积空间）

SE 3
 称为三维空间的特殊欧式群：满足封闭性和结合律，具
有单位元和可逆性，但不具有交换律
SE 3 SO 3
 刚体变换群 和旋转群 都是光滑流形，且矩阵乘法
运算和求逆运算都是光滑映射，且构成李群（不可交换）
SE  n    p, R : p  n , R  SO  n   n  SO  n 

 推广到 nSE维空间：
3 SE  2 

 n=3 时， SE 3
表示空间运动，单位元为 I4 ； n=2 时， 表
示平面运动，单位元为 I3 。 子群包括圆柱运动群，该群
绕一直线旋转同时还沿该直线平移（螺旋运动）。
3.4 刚体变换群

1 0 0 px  1 0 0 p 2 
0 cos 0 0 cos

 sin 
  sin  0 
 
0 sin  cos  0 0 sin  cos  0 
   
0 0 0 1
0 0 0 1 

圆柱运动群（ x/y/z 轴） 螺旋运动子群 H p （ p 称为

螺旋运动的节距轴）
3.5 欧拉角与 RPY 角

引入其它参数法表示的必要性
 旋转矩阵 R 用 9 个元素表示 3 个独立变量（不方便）；
 R 作为变换或算子使用比较方便，作为方位描述并不方便，需
要输入较多信息。如机器人手爪方位的描述需输入： [n o a ]

 欧拉角 /RPY 角广泛的应用于航海、航天和天文学。

3.5 绕固定轴 x-y-z 旋转（ RPY 角）

 船行驶方向为 z 轴，绕
z 轴旋转 α 角，滚动
(Roll); 绕 y 轴旋转 β 角，
俯仰 (Pitch) ；铅直方
向为 x 轴，绕 x 轴旋转
γ 角偏转 (Yaw) 。

 RPY 描述坐标系 {B} 的

方法如下： {B} 的初始
方位与参考系 { Ａ } 重
合。首先将 {B} 绕 xA
转 γ 角，再绕 yA 转 β 角，
最后绕 zA 转 α 角。
3.5 绕固定轴 x-y-z 旋转（ RPY 角）

 三次旋转都是相对于固定坐标系｛Ａ｝而言，称为“绕固
定轴 x-y-z 旋转”的 RPY 角法。按照“从右向左”的原则
B Rxyz  ,  ,    R  z A ,   R  y A ,   R  x A ,  
A

c  s 0   c 0 s   1 0 0 
=  s c 0   0 1 0  0 c  s 
   
 0 0 1    s 0 c   0 s c 
 c c c s  s  s c c s  c  s s 
  s c s s s  c c s s  c  c s 
 
  s c s c c 
 r11 r12 r13 
r r23 
 如果给定 A
R
B xyz   ,  ,   
 21
r22

 r31 r32 r33 

 如何计算  ,  , ？
3.5 绕固定轴 x-y-z 旋转（ RPY 角）

 超越方程： 3 个独立变量、 9 个约束方程（ 6 个不独立）

cos   r112  r212 （取 -90º≤β≤90º ）
 如 cos   0 ，得到 RPY 角的反正切表示：


  A tan 2  r31 , r112  r212  t an  
r21
r11
  A tan 2  r21 , r11  r31
t an  
  A tan 2  r32 , r33  r112  r212
r32
其中 A tan 2  y, x  是 t an  
r33
双变量反正切函数
3.5 绕相对轴 z-y-x 旋转（欧拉角）

 RPY 描述坐标系 {B} 的方

法如下： {B} 的初始方位与
参考系 { Ａ } 重合。首先将
{B} 绕 zB 转 α 角，再绕 yB
转 β 角，最后绕 xB 转 γ 角。
 各次转动相对运动坐标系的某轴进行的，转动顺序是绕 z 轴， y
轴和 x 轴，故称为 z-y-x （欧拉角）。按照“从左向右”原则
B Rzyx  ,  ,   = R  z ,   R  y ,   R  x,  
A

c  s 0   c 0 s   1 0 0 
=  s c 0   0 1 0  0 c  s 
   
 0 0 1    s 0 c  0 s c 
 c c c s s  s c c s c  s s 
  s c s s s  c c s s c  c s 
 
  s c s c c 
3.5 绕相对轴 z-y-x 旋转（欧拉角）

 可以看出，与绕固定轴 x-y-z 的旋转结果完全相同。这是

因为绕固定轴旋转的顺序若与绕运动旋转的顺序相反，且
旋转的角度也对应相等，所得到变换矩阵是相同的。
 因此， z-y-x 欧拉角与固定轴 x-y-z 转角描述坐标系｛ B ｝
是完全等价的。公式 3-49 和 3-56 是一致的。
 绕运动坐标系转动的 z-y-z 欧拉角，遵循“从左向右”原则：
A
B Rzyz  ,  ,    R  z ,   R  y,   R  z ,  
 c c c  s s c c s  s c c s 
  s c c  c s  s c s  c c s s 
 
  s c s s c 

 反解与绕固定轴类似（练习）？
3.6 旋转变换通式

 令 k  k xi  k y j  k z k 是过原点的单位矢量
 求绕任意轴 k 旋转 角的变换矩阵R  k , 
 为求 R  k ,  ，定义两个辅助坐标系｛ A′ ， B′ ｝
– {A′} 和 {B′} 分别与 {A} 和 {B} 固接；
– {A′} 和 {B′} 的 z 轴与 k 重合， x ， y 轴任意；
– 旋转之前， {A′} 和 {B′} 重合， {A} 和 {B} 重合。

 nx ox kx 
A
R  B
R  n oy ky 
A B  y 
 nz oz k z 
3.6 旋转变换通式

 坐标系 {B} 绕 k 轴相对于 {A} 旋转 θ 角相当于：坐标系 {B

′} 相对于 {A′} 的 z 轴旋转 θ 角，由下图得：
A
B R  R  k ,   AA R BA R BB R

 得到相似变换：
R  k ,   AA RR  z ,  BB R 1 ,
R  k ,   AA RR  z ,  BB R T

 将上式展开并化简，得出 R  k , 
的表达式。它只与矢量 k 有关，
即只与 {A′} 的 z 轴有关。
3.6 旋转变换通式

 实际上：
 nx ox k x   cos  sin  0   nx ny nz 
 
R ( k , )   n y oy k y   sin  cos  0   ox oy oz 
  
 nz oz k z   0 0 1   k x ky k z 

 运用旋转矩阵的正交性：
nn  oo  a a 1 包括了各种特殊情况：
no  oa  a n  0 当 kx=1 ， ky=kz=0 ，则可得式 3-5
a  no 当 ky=1 ， kx=kz=0 ，则可得式 3-6

 得到旋转变换通式： 当 kz=1 ， ky=kx=0 ，则可得式 3-7

 k x k x Vers  c k y k x Vers  k z s k z k x Vers  k y s 

 
R  k ,    k x k y Vers  k z s k y k y Vers  c k z k y Vers  k x s 
 k x k z Vers  k y s k y k z Vers  k x s k z k z Vers  c 

3.6 等效转角和等效转轴

 旋转变换通式：根据转轴和转角建立相应旋转变换矩阵；反向
问题：根据旋转矩阵求其等效转轴与等效转角 (k,θ) 。
 nx ox ax 
R   ny oy ay 
 给定旋转矩阵：   求出它的 (k,θ)
 nz oz az 

 令 R  R  k ,  ，根据方程相等：
 nx ox ax   k x k x Vers  c k y k x Vers  k z s k z k x Vers  k y s 
n  
oy a y    k x k y Vers  k z s k y k y Vers  c k z k y Vers  k x s 
 y 
 nz oz az   k x k z Vers  k y s k y k z Vers  k x s k z k z Vers  c 
1
 所以对角线相加：nx  oy  az  1  2cos cos    nx  o y  az  1
2
oz  a y  2k x sin 
oz  a y a x  nz n y  ox
 非对角线相减：ax  nz  2k y sin  kx  ,ky  , kz 
2sin  2sin  2sin 
n y  ox  2k z sin 
3.6 等效转角和等效转轴

 练习：求复合矩阵 BA R  R  y,90  R  z ,90  的等效转轴和转角 (k,θ) 。

 0 0 1   0 1 0   0 0 1 
解答： 1 ）计算旋转矩阵A 
BR  0 1 0  1 0 0    1 0 0 
   
 1 0 0 0 0 1   0 1 0 

1 1
cos   0  0  0  1  
 2 2

2 ）确定  120
1 3
1  0   1  0   1  0  
2 2 2
sin  
2 2
3  1
tan   
2  2

1 0 1 1 0 1 1 0 1
kx   , ky   , kz  
3 ）确定转轴： 3 3 3 3 3 3

 可以证明（欧拉定理）：任何一组绕过原点的轴线的复合转动总是等
价于绕某一过原点的轴线的转动 。
3.6 齐次变换通式

 推广：旋转轴线 k 为不过原点的齐次变换通式
 假设单位矢量 k 通过点 p ，存在：
T T
k   k x ky k z  , p   px py pz 

 为求 ABT ，定义两个辅助坐标系｛ A′ ， B′ ｝
– {A′} 和 {B′} 分别与 {A} 和 {B} 固接；
– {A′, B′} 和 {A, B } 平行，原点过 p 点；
– 旋转之前， {A′} 和 {B′} 重合， {A} 和 {B} 重合。
3.6 齐次变换通式

 变换方程（相似变换）： A
BT  AA'T AB''T BB'T

 其中：
 I 33 p
A
A' T    Trans  p 
 0 1
 I 33  p
B'
B T T B
B'
1
   Trans   p 
 0 1 

 R  k ,  0 
A'
B' T  Rot  k ,    
 0 1 

 所以齐次变换通式：
 R  k ,   R  k ,  p  p 
A
BT  Trans  p  Rot  k , Trans   p    
 0 1 
3.7 位姿综合 / 计算复杂性（学习）

 楔块 发生如下位姿的变换，如何保证变换 T 的唯一性？
条件的相容性、独立性和完备性
 条件不相容时，没有解；条件不完备时，解不唯一，条件相容
且完备时，解是存在的且是唯一的（旋转 / 任意变换综合）。

 计算复杂性：前者 9 次乘、 9 次加；后者 16 次乘、 12 次加

A
p  BA R B p  A pB 0 ,
齐次变换会使计算量增加
 A p   RA R A
pBo   B p 
    浪费在 0 和 1 的无效计算
1   0 1  1 