位置和姿态的表示

位置描述

在指定坐标系下，空间中任意一点可有 3 × 1 3 \times 1 3×1位置矢量来确定
A p = [ p x p y p z ] ^Ap = \begin{bmatrix} p_x\\ p_y\\ p_z \end{bmatrix} Ap=⎣ ⎡​px​py​pz​​⎦ ⎤​
其中 p x , p y , p z p_x, p_y, p_z px​,py​,pz​是点 P 在坐标系 { A } \{A\} {A}中的三个坐标分量，上标 A 表示参考坐标系 { A } \{A\} {A}。

我们称 A p ^Ap Ap为位置向量。

方位描述

旋转矩阵

坐标系 B 相对于坐标系 A 的方位，可以由坐标系 B 的三个主矢量相对于坐标系 A 的三个方向余弦组成的 3 × 3 3\times3 3×3矩阵来表示：

B A R = [ A x B A y B A z B ] ^A_BR= \begin{bmatrix} ^Ax_B & ^Ay_B & ^Az_B \end{bmatrix} BA​R=[AxB​​AyB​​AzB​​]
B A R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] ^A_BR= \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{bmatrix} BA​R=⎣ ⎡​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​⎦ ⎤​

B A R ^A_BR BA​R称为旋转矩阵

对应三个坐标轴 x , y , z x,y,z x,y,z作转角位 θ \theta θ的旋转变换，其旋转矩阵分别为
R ( x , θ ) = [ 1 0 0 0 c θ − s θ 0 s θ c θ ] R(x,\theta)= \begin{bmatrix} 1 & 0 & 0\\ 0 & c\theta & -s\theta\\ 0 & s\theta & c\theta \end{bmatrix} R(x,θ)=⎣ ⎡​100​0cθsθ​0−sθcθ​⎦ ⎤​

R ( y , θ ) = [ c θ 0 s θ 0 1 0 − s θ 0 c θ ] R(y,\theta)= \begin{bmatrix} c\theta & 0 & s\theta\\ 0 & 1 & 0\\ -s\theta & 0 & c\theta \end{bmatrix} R(y,θ)=⎣ ⎡​cθ0−sθ​010​sθ0cθ​⎦ ⎤​

R ( z , θ ) = [ c θ − s θ 0 s θ c θ 0 0 0 1 ] R(z,\theta)= \begin{bmatrix} c\theta & -s\theta & 0\\ s\theta & c\theta & 0\\ 0 & 0 & 1 \end{bmatrix} R(z,θ)=⎣ ⎡​cθsθ0​−sθcθ0​001​⎦ ⎤​

位姿描述

刚体 B 的位姿可以由坐标系 { B } \{B\} {B}来描述，即
KaTeX parse error: Got group of unknown type: 'internal'

坐标变换

平移坐标变换

平移方程
A p = B p + A p B 0 ^Ap= ^Bp + ^Ap_{B0} Ap=Bp+ApB0​
其中

• A p ^Ap Ap表示任意点 p 在坐标系 A 中的位置矢量
• B p ^Bp Bp表示点 p 在坐标系 B 中的位置矢量
• A p B 0 ^Ap_{B0} ApB0​表示坐标系 B 的原点在坐标系 A 中的位置矢量

旋转坐标变换

坐标旋转方程
A p = B A R B p ^Ap=^A_BR ^Bp Ap=BA​RBp
其中

• A p ^Ap Ap表示任意点 p 在坐标系 A 中的位置矢量
• B p ^Bp Bp表示点 p 在坐标系 B 中的位置矢量
• B A R ^A_BR BA​R表示描述坐标系 B 相对于坐标系 A 方位的旋转矩阵

齐次坐标变换

齐次变换

关于齐次坐标的意义：

齐次坐标的意义一

齐次坐标的意义二

这样，对于上述讨论的旋转和平移变换公式就可以进行统一，即 [ A p 1 ] \begin{bmatrix} ^Ap \\ 1 \end{bmatrix} [Ap1​] = = = [ B A R A p B 0 0 1 ] \begin{bmatrix} ^A_BR & ^Ap_{B0} \\ 0 & 1 \end{bmatrix} [BA​R0​ApB0​1​] [ B p 1 ] \begin{bmatrix} ^Bp\\ 1 \end{bmatrix} [Bp1​]

其中， 4 × 1 4\times1 4×1的列向量表示三维空间的点，上式的矩阵形式为
A p = B A T B p ^Ap= ^A_BT^Bp Ap=BA​TBp
其中 4 × 4 4\times4 4×4的矩阵 B A T ^A_BT BA​T称为齐次变换矩阵，具有如下形式
B A T = [ B A R A p B 0 0 1 ] ^A_BT= \begin{bmatrix} ^A_BR & ^Ap_{B0}\\ 0 & 1 \end{bmatrix} BA​T=[BA​R0​ApB0​1​]

平移齐次坐标变换

平移齐次变换
T r a n s ( a , b , c ) = [ 1 0 0 a 0 1 0 b 0 0 1 c 0 0 0 1 ] Trans(a, b, c)= \begin{bmatrix} 1 & 0 & 0 & a\\ 0 & 1 & 0 & b\\ 0 & 0 & 1 & c\\ 0 & 0 & 0 & 1 \end{bmatrix} Trans(a,b,c)=⎣ ⎡​1000​0100​0010​abc1​⎦ ⎤​
对已知矢量
u = [ x y z w ] T \displaystyle u=[x~y~ z ~w]^\mathrm{T} u=[x y z w]T
进行平移变换得到的矢量为： v = [ 1 0 0 a 0 1 0 b 0 0 1 c 0 0 0 1 ] v = \begin{bmatrix} 1 & 0 & 0 & a \\ 0 & 1 & 0 & b \\ 0 & 0 & 1 & c \\ 0 & 0 & 0 & 1 \end{bmatrix} v=⎣ ⎡​1000​0100​0010​abc1​⎦ ⎤​ [ x y z w ] \begin{bmatrix} x \\ y \\ z \\ w \end{bmatrix} ⎣ ⎡​xyzw​⎦ ⎤​ = = = [ x + a w y + b w z + c w w ] \begin{bmatrix} x+aw \\ y+bw \\ z+cw \\ w \end{bmatrix} ⎣ ⎡​x+awy+bwz+cww​⎦ ⎤​ = = = [ x / w + a y / w + b z / w + c 1 ] \begin{bmatrix} x/w+a\\ y/w+b\\ z/w+c\\ 1 \end{bmatrix} ⎣ ⎡​x/w+ay/w+bz/w+c1​⎦ ⎤​
可以看成是矢量 [ x / w y / w z / w ] T [x/w~~y/w~~z/w]^\mathrm{T} [x/w  y/w  z/w]T与矢量 [ a b c ] T [a ~b~ c]^\mathrm{T} [a b c]T之和

旋转齐次坐标变换

R o t ( x , θ ) = [ 1 0 0 0 0 c θ − s θ 0 0 s θ c θ 0 0 0 0 1 ] Rot(x, \theta)= \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & c\theta & -s\theta & 0\\ 0 & s\theta & c\theta & 0\\ 0 & 0 & 0 &1 \end{bmatrix} Rot(x,θ)=⎣ ⎡​1000​0cθsθ0​0−sθcθ0​0001​⎦ ⎤​
R o t ( y , θ ) = [ c θ 0 s θ 0 0 1 0 0 − s θ 0 c θ 0 0 0 0 1 ] Rot(y, \theta)= \begin{bmatrix} c\theta & 0 & s\theta & 0\\ 0 & 1 & 0 & 0\\ -s\theta & 0 & c\theta & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} Rot(y,θ)=⎣ ⎡​cθ0−sθ0​0100​sθ0cθ0​0001​⎦ ⎤​
R o t ( z , θ ) = [ c θ − s θ 0 0 s θ c θ 0 0 0 0 1 0 0 0 0 1 ] Rot(z,\theta)= \begin{bmatrix} c\theta & -s\theta & 0 & 0\\ s\theta & c\theta & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} Rot(z,θ)=⎣ ⎡​cθsθ00​−sθcθ00​0010​0001​⎦ ⎤​

物体的变换及逆变换

齐次变换的逆变换

复合变换：
C A T = B A T C B T ^A_CT = ^A_BT ^B_CT CA​T=BA​TCB​T

齐次变换的求逆问题

从坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的描述 B A T ^A_BT BA​T，
求得坐标系 { A } \{A\} {A}相对于 { B } \{B\} {B}的描述 A B T ^B_AT AB​T 。

• 直接对 4 × 4 4\times4 4×4齐次变换矩阵求逆

此问题可以等价于给定 B A R ^A_BR BA​R和 A p B 0 ^Ap_{B0} ApB0​，计算 A B R ^B_AR AB​R和 B p A 0 ^Bp_{A0} BpA0​

坐标系 { B } \{B\} {B}的原点，可以表示成 A p B 0 ^Ap_{B0} ApB0​ 的形式，该点在坐标系 { B } \{B\} {B}中的描述是
B ( A p B 0 ) = ( A B R ) ( A p B 0 ) + B p A 0 ^B(^Ap_{B0}) = (^B_AR)(^Ap_{B0}) + ^Bp_{A0} B(ApB0​)=(AB​R)(ApB0​)+BpA0​
而上式中的第一项我们知道，坐标系 B 的原点在坐标系 B 中的描述为 0，故
B p A 0 = ( − A B R ) ( A p B 0 ) ^Bp_{A0} = (-^B_AR)(^Ap_{B0}) BpA0​=(−AB​R)(ApB0​)
A B T = [ B A R T ( − B A R T ) ( A p B 0 ) 0 1 ] ^B_AT = \begin{bmatrix} ^A_BR^\mathrm{T} & (-^A_BR^\mathrm{T})(^Ap_{B0}) \\ 0 & 1 \end{bmatrix} AB​T=[BA​RT0​(−BA​RT)(ApB0​)1​]

• 直接对 4 × 4 4 \times 4 4×4齐次变换矩阵的求逆方法

一般情况下，已知变换 T T T 的各元，
T = [ n x o x a x p x n y o y a y p y n z o z a z p z 0 0 0 1 ] T = \begin{bmatrix} n_x & o_x & a_x & p_x\\ n_y & o_y & a_y & p_y\\ n_z & o_z & a_z & p_z\\ 0 & 0 & 0 & 1 \end{bmatrix} T=⎣ ⎡​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​px​py​pz​1​⎦ ⎤​
其逆变换为
T − 1 = [ n x n y n z − p ⋅ n o x o y o z − p ⋅ o a x a y a z − p ⋅ a 0 0 0 1 ] T^{-1} = \begin{bmatrix} n_x & n_y & n_z & -\pmb{p} \cdot \pmb{n} \\ o_x & o_y & o_z & -\pmb{p} \cdot \pmb{o} \\ a_x & a_y & a_z & -\pmb{p} \cdot \pmb{a} \\ 0 & 0 & 0 & 1 \end{bmatrix} T−1=⎣ ⎡​nx​ox​ax​0​ny​oy​ay​0​nz​oz​az​0​−pp⋅nn−pp⋅oo−pp⋅aa1​⎦ ⎤​

通用旋转变换

我们在前面已经给出了，绕 x , y , z x, y, z x,y,z轴旋转的变换矩阵，现在讨论更一般的情况，即研究绕着从原点出发的任一矢量 f f f旋转 θ \theta θ 角时的旋转矩阵。

通用旋转变换公式

我们假设任意矢量 f f f是坐标系 { C } \{C\} {C}的 z z z轴上的单位矢量。

那么坐标系C就可以表示为：
C = [ n x o x a x 0 n y o y a y 0 n z o z a z 0 0 0 0 1 ] C = \begin{bmatrix} n_x & o_x & a_x & 0\\ n_y & o_y & a_y & 0\\ n_z & o_z & a_z & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} C=⎣ ⎡​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​0001​⎦ ⎤​
f = a x x + a y y + a z z \pmb{f} = a_x \pmb{x} + a_y\pmb{y} + a_z\pmb{z} ff=ax​xx+ay​yy+az​zz
那么，绕矢量 f f f旋转，就等价于绕坐标系 { C } \{C\} {C}的 z z z轴旋转。
即
R o t ( f , θ ) = R o t ( C z , θ ) Rot(\pmb{f}, \theta) = Rot(C_z, \theta) Rot(ff,θ)=Rot(Cz​,θ)

接下来，如果已知一个以参考坐标系描述的坐标系 { T } \{T\} {T}，这个坐标系在坐标系 { C } \{C\} {C}下的描述为坐标系 { S } \{S\} {S}，那么有如下关系：

T = C S T = CS T=CS

那么：
S = C − 1 T S=C^{-1}T S=C−1T

接下来，我们知道，坐标系 { T } \{T\} {T}和任意矢量 f \pmb{f} ff都是在参考坐标系下描述的，而坐标系 { S } \{S\} {S}是坐标系 { T } \{T\} {T}在坐标系 { C } \{C\} {C}中的描述，矢量 C z C_z Cz​是矢量 f \pmb{f} ff在坐标系 { C } \{C\} {C}中的描述。

那么就存在等价关系：

T T T绕 f \pmb{f} ff旋转等价于 S S S绕坐标系 { C } \{C\} {C}的 z z z轴旋转。

即

R o t ( f , θ ) T = R o t ( C z , θ ) S Rot(\pmb{f}, \theta)T = Rot(C_z, \theta)S Rot(ff,θ)T=Rot(Cz​,θ)S
有：
R o t ( f , θ ) T = C R o t ( z , θ ) S Rot(\pmb{f}, \theta)T = CRot(z, \theta)S Rot(ff,θ)T=CRot(z,θ)S
进一步：
R o t ( f , θ ) T = C R o t ( z , θ ) C − 1 T Rot(\pmb{f}, \theta)T = CRot(z, \theta) C^{-1}T Rot(ff,θ)T=CRot(z,θ)C−1T

进而：
R o t ( f , θ ) = C R o t ( z , θ ) C − 1 Rot(\pmb{f}, \theta) = CRot(z, \theta) C^{-1} Rot(ff,θ)=CRot(z,θ)C−1
最终：
R o t ( f , θ ) = [ f x f x v e r s θ + c θ f y f x v e r s θ − f z s θ f z f x v e r s θ + f y s θ 0 f x f y v e r s θ + f z s θ f y f y v e r s θ + c θ f z f y v e r s θ − f x s θ 0 f x f z v e r s θ − f y s θ f y f z v e r s θ + f x s θ f z f z v e r s θ + c θ 0 0 0 0 1 ] Rot(\pmb{f}, \theta) = \begin{bmatrix} f_x f_xvers\theta + c\theta & f_y f_xvers\theta - f_z s\theta & f_z f_xvers\theta + f_y s\theta & 0\\ f_x f_y vers\theta + f_z s \theta & f_y f_y vers \theta + c \theta & f_z f_y vers\theta -f_xs\theta & 0\\ f_x f_z vers\theta -f_ys\theta & f_y f_z vers\theta + f_x s\theta & f_z f_z vers\theta + c\theta & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} Rot(ff,θ)=⎣ ⎡​fx​fx​versθ+cθfx​fy​versθ+fz​sθfx​fz​versθ−fy​sθ0​fy​fx​versθ−fz​sθfy​fy​versθ+cθfy​fz​versθ+fx​sθ0​fz​fx​versθ+fy​sθfz​fy​versθ−fx​sθfz​fz​versθ+cθ0​0001​⎦ ⎤​

等效转角与转轴

由上述我们推得的公式，可以知道给出任意一个旋转变换矩阵，都可以找到一个等效的旋转 θ \theta θ角的转轴。

已知旋转变换：
R = [ n x o x a x 0 n y o y a y 0 n z o z a z 0 0 0 0 1 ] R= \begin{bmatrix} n_x & o_x & a_x & 0\\ n_y & o_y & a_y & 0\\ n_z & o_z & a_z & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} R=⎣ ⎡​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​0001​⎦ ⎤​

令 R = R o t ( f , θ ) R = Rot(\pmb{f}, \theta) R=Rot(ff,θ)

可以确定：
t a n θ = ( o z − a y ) 2 + ( a x − n z ) 2 + ( n y − o x ) 2 n x + o y + a z − 1 tan\theta = \frac{\sqrt{(o_z-a_y)^2+(a_x-n_z)^2+(n_y-o_x)^2}}{n_x+o_y+a_z-1} tanθ=nx​+oy​+az​−1(oz​−ay​)2+(ax​−nz​)2+(ny​−ox​)2 ​​

f x = ( o z − a y ) / 2 s θ f_x = (o_z - a_y)/2s\theta fx​=(oz​−ay​)/2sθ

f y = ( a x − n z ) / 2 s θ f_y = (a_x - n_z)/2s\theta fy​=(ax​−nz​)/2sθ

f z = ( n y − o x ) / 2 s θ f_z = (n_y - o_x)/2s\theta fz​=(ny​−ox​)/2sθ

机器人学的数理基础知识相关推荐

1. 计算机常用数理基础知识

   1.大数定律和中心极限定理 中心极限定理:大量随机变量近似服从正态分布的条件. 大数定律:大数定律讨论的是在什么条件下,随机变量序列的算术平均依概率收敛到其均值的算术平均. 大数定理:切比雪夫.辛钦定 ...

2. 数理基础（高等代数）------sympy三角、指数、对数、幂函数、极限、求导、微分、积分等基础知识

   sympy基础知识 1.sympy import sympy print(sympy.pi.evalf())#圆周率π print(sympy.E)#e print(sympy.I)#虚数i prin ...

3. 论坛报名 | 数理基础：人工智能的重大理论挑战和最新成果

   与6位图灵奖得主和100多位专家 共同探讨人工智能的下一个十年 长按图片或点击阅读原文,内行盛会,首次免费注册 北京智源大会倒计时:10天 2020年6月21-24日,第二届北京智源大会(官网:htt ...

4. “人工智能的数理基础”主题论坛，五位学者从数学角度解决AI问题

   导语:来自北京大学的五位学者参加了"人工智能的数理基础"主题论坛,他们从数学.统计和计算的角度做了学术报告. 智东西5月9日消息,围绕当前人工智能面临的可计算性.可解释性.泛化性. ...

5. c语言 二进制输出_程序员入门C语言，需要掌握的4个基础知识

   C语言是当前所有开发技术中使用较为广泛的一门语言,从它诞生之日起就深受程序员的喜爱.随着C语言的普及,后来的开发语言都或多或少地借鉴或遵循了它的一些模式.另外,C语言是计算机编程领域中使用最早的高级语 ...

6. JavaSE基础知识（附上代码实现）1

   ###01.01_计算机基础知识(计算机概述)(了解) * A:什么是计算机?计算机在生活中的应用举例     * 计算机(Computer)全称:电子计算机,俗称电脑.是一种能够按照程序运行,自动. ...

7. Python基础笔记_Day01_计算机基础知识和Python开发环境搭建

   Day01_计算机基础知识和Python开发环境搭建 目录 01.01_计算机基础知识(计算机概述)(了解) 01.02_计算机基础知识(软件开发和计算机语言概述)(了解) 01.03_计算机基础知识 ...

8. 数学基础（2）~ 数理统计基础知识

   出处:http://www.cnblogs.com/fanling999/p/6708458.html 参考:盛骤, 谢式千, 潘承毅. 概率论与数理统计, 第四版[M]. 高等教育出版社, 2008 ...

9. 浅谈人工智能(`AI`)基础知识

   人工智能(AI)-基础知识 文档导航 人工智能(`AI`)-基础知识 1. 什么是人工智能 1.1 人工智能基础定义 1.2 人工智能基本概述 1.3 人工智能基本组成 1.4 人工智能研究课题 1. ...

最新文章

1. 在？三缺一，来斗个地主——肝个斗地主案例（java）
2. php跟html增删改查,EasyUIDataGrid结合ThinkPHP实现增删改查操作初学者_html/css_WEB-ITnose...
3. 超易懂的MapReduce思想讲解
4. 怎样判断RadioButtonList控件是否有选择
5. Fatal Python error: Cannot recover from stack overflow.（嵌套层数过多超出限制）
6. 用rvest包来抓取Google学术搜索数据
7. 打印modal框中在线生成的二维码
8. 对于Ping的过程，你真的了解吗？
9. F-Stack：ff_run函数详解
10. 献给那些正在“奋起”的90后
11. Linux中如何使用帮助
12. TCPIP header
13. Java序列化技术即将被废除！！！
14. 寻找圣杯 In Search of the Holy Grail
15. ARM通用寄存器和特殊寄存器
16. cmd 打开资源监视器
17. [ERROR] melodic运行比较新的包时报错，我这里是racecar的包出错
18. 愿你一直能够撑下去！
19. ffmpeg实例，fade淡入淡出效果
20. 试题 历届真题 大胖子走迷宫【第十届】【决赛】【C组】

热门文章

1. 解决springboot使用多线程无法注入Bean问题，不能注入Service或者Mapper
2. 五金模具设计，端子模具设计要点
3. 网站百度统计被恶意刷广告的处理方法
4. 手把手教学51单片机第三课 | 复位电路、数码管静态显示
5. PL2303在win10无法使用的解决办法
6. [译]Kotlin中是应该使用序列(Sequences)还是集合(Lists)?
7. 高速光耦（PS8101，TLP112A，TLP109）基本工作原理应用实例
8. 基于过程的软件测试全景图 (2)
9. CHIL-SQL-JOIN
10. 大学生创新创业大赛案例_创新创业大赛获奖名单_大学生创新创业大赛获奖案例...