目录
1. 三维刚体运动
对于三维空间的物体,可以通过三个坐标描述其位置,但对于机器人 or 飞行器 航天器来讲,还需要另外三个量描述其姿态——俯仰角、滚转角 和 偏航角,这三个欧拉角可以通过陀螺仪测得。不同的位姿则对应不同的载体坐标系。
这一节内容主要解决的问题是:已知世界(惯性)坐标系中一点 a a a,以及刚体的位姿,求得 a a a在载体坐标系中的坐标。
1.1. 问题阐述
已知:北东地世界坐标系 O w X w Y w Z w O_w X_w Y_w Z_w OwXwYwZw,以及其一组标准正交基 e w = [ e w x , e w y , e w z ] T e_w = [e_{wx},e_{wy},e_{wz}]^T ew=[ewx,ewy,ewz]T。
一点
a
a
a且点
a
a
a在世界坐标系的表示为
a
=
[
e
w
x
,
e
w
y
,
e
w
z
]
[
a
w
x
a
w
y
a
w
z
]
a=[e_{wx},e_{wy},e_{wz}]\left[\begin{array}{lcr} a_{wx} \\ a_{wy} \\ a_{wz} \end{array} \right]
a=[ewx,ewy,ewz]⎣⎡awxawyawz⎦⎤
刚体
c
c
c的世界坐标为
c
w
=
(
c
w
x
,
c
w
y
,
c
w
z
)
c_w=(c_{wx},c_{wy},c_{wz})
cw=(cwx,cwy,cwz),若刚体坐标系为
O
c
X
c
Y
c
Z
c
O_c X_c Y_c Z_c
OcXcYcZc。
并且刚体的偏航角、俯仰角 和 滚转角 分别为 θ Y w θ X c θ Z c \theta_{Y_w} \theta_{X_c} \theta_{Z_c} θYwθXcθZc(此处与课本中坐标系定义不同,是为了与2.1.的相机坐标系一致),单位 r a d rad rad。
求: 点
a
a
a在刚体坐标系
O
c
X
c
Y
c
Z
c
O_c X_c Y_c Z_c
OcXcYcZc下的坐标
[
a
c
x
,
a
c
y
,
a
c
z
]
[a_{cx},a_{cy},a_{cz}]
[acx,acy,acz]
1.2. 变换矩阵
根据矩阵理论的描述,不同坐标系下同一个点的坐标 是通过向量的旋转 和 平移得到的,即应满足
a
c
=
R
c
w
a
w
+
t
c
w
(1)
a_c=R_{cw}a_w + t_{cw} \tag{1}
ac=Rcwaw+tcw(1)
其中 R c w R_{cw} Rcw为从世界坐标系 到 载体坐标系 的 旋转矩阵, t c w t_{cw} tcw表示载体坐标系中从 载体坐标系原点 到 世界坐标系原点 的向量。
注意 R R R与 t t t的下标记法: R 12 R_{12} R12表示从坐标系2旋转到坐标系1的旋转矩阵, t 12 t_{12} t12表示坐标系1中坐标系1原点到坐标系2原点的向量。
或者写成齐次形式 以方便多次旋转 和 平移
[ a c 1 ] = ( R c w R c w t c w 0 T 1 ) [ a w 1 ] = T [ a w 1 ] (2,重要) \left[\begin{array}{lcr} a_{c} \\ 1 \end{array} \right]= \left(\begin{array}{lcr} R_{cw} & R_{cw}t_{cw}\\ 0^T & 1 \\ \end{array}\right) \left[\begin{array}{lcr} a_{w} \\1 \end{array} \right]= T\left[\begin{array}{lcr} a_{w} \\1 \end{array} \right] \tag{2,重要} [ac1]=(Rcw0TRcwtcw1)[aw1]=T[aw1](2,重要)
其中 T T T就叫作变换矩阵,由于 c w c_w cw已知,因此对旋转矩阵 R R R的求解则需要解决。
标准正交基法
思路是根据矩阵理论中两个向量空间基的变换来实现实现旋转关系的,所以需要先确定载体坐标系 在世界坐标系 中的三个基。
标准正交基则是通过 基本旋转得到的,按照偏航-俯仰-滚转的顺序确定标准基。
1. 假设 载体坐标系原点 就是 世界坐标系原点,先绕
Y
w
Y_w
Yw偏航
θ
Y
w
\theta_{Y_w}
θYw
e
c
=
e
w
(
c
o
s
θ
Y
w
0
s
i
n
θ
Y
w
−
s
i
n
θ
Y
w
0
c
o
s
θ
Y
w
0
−
1
0
)
e_c=e_w \left(\begin{array}{lcr} cos\theta_{Y_w} & 0& sin\theta_{Y_w} \\ -sin\theta_{Y_w} & 0& cos\theta_{Y_w} \\ 0& -1& 0 \end{array}\right)
ec=ew⎝⎛cosθYw−sinθYw000−1sinθYwcosθYw0⎠⎞
2. 之后绕
X
c
X_c
Xc俯仰
θ
X
c
\theta_{X_c}
θXc
e
c
=
e
c
(
1
0
0
0
c
o
s
θ
X
c
−
s
i
n
θ
Y
c
0
s
i
n
θ
X
c
c
o
s
θ
X
c
)
e_c=e_c \left(\begin{array}{lcr} 1& 0 & 0\\ 0& cos\theta_{X_c}& -sin\theta_{Y_c} \\ 0& sin\theta_{X_c}& cos\theta_{X_c} \end{array}\right)
ec=ec⎝⎛1000cosθXcsinθXc0−sinθYccosθXc⎠⎞
3. 最后绕 Z c {Z_c} Zc滚转 θ Z c \theta_{Z_c} θZc
e c = e c ( c o s θ Z c s i n θ Z c 0 − s i n θ Z c c o s θ Z c 0 0 0 1 ) e_c=e_c \left(\begin{array}{lcr} cos\theta_{Z_c} & sin\theta_{Z_c} &0 \\ -sin\theta_{Z_c} & cos\theta_{Z_c} & 0 \\ 0 & 0 & 1 \end{array}\right) ec=ec⎝⎛cosθZc−sinθZc0sinθZccosθZc0001⎠⎞
最终同时进行了偏航俯仰滚转的载体坐标系的 标准正交基 就为上述三个系数矩阵不断右乘,即
e c = e w ( c θ Y w c θ Z c − s θ X c s θ Y w s θ Z c c θ Y w s θ Z c + s θ X c s θ Y w s θ Z c c θ X c c θ Y w s θ Y w c θ Z c − s θ X c s θ Y w s θ Z c − s θ Y w s θ Z c + s θ X c c θ Y w c θ Z c c θ X c c θ Y w c θ X c s θ Z c − c θ X c s θ Z c s θ X c ) e_c= e_w \left(\begin{array}{lcr} c\theta_{Y_w}c\theta_{Z_c}-s\theta_{X_c}s\theta_{Y_w}s\theta_{Z_c} & c\theta_{Y_w}s\theta_{Z_c}+s\theta_{X_c}s\theta_{Y_w}s\theta_{Z_c} & c\theta_{X_c}c\theta_{Y_w} \\ s\theta_{Y_w}c\theta_{Z_c}-s\theta_{X_c}s\theta_{Y_w}s\theta_{Z_c} & -s\theta_{Y_w}s\theta_{Z_c}+s\theta_{X_c}c\theta_{Y_w}c\theta_{Z_c}& c\theta_{X_c}c\theta_{Y_w} \\ c\theta_{X_c}s\theta_{Z_c} & -c\theta_{X_c}s\theta_{Z_c} & s\theta_{X_c} \end{array}\right) ec=ew⎝⎛cθYwcθZc−sθXcsθYwsθZcsθYwcθZc−sθXcsθYwsθZccθXcsθZccθYwsθZc+sθXcsθYwsθZc−sθYwsθZc+sθXccθYwcθZc−cθXcsθZccθXccθYwcθXccθYwsθXc⎠⎞
(3) \quad\tag{3} (3)
有了载体坐标系 的三个 标准正交基 在世界坐标系的表达式之后,就可以表示旋转矩阵了,旋转矩阵则为
R c w = e c e w T (4,重要) R_{cw}=e_ce_w^T \tag{4,重要} Rcw=ecewT(4,重要)
旋转向量法
用一个单位长度向量
n
=
[
n
1
,
n
2
,
n
3
]
T
n=[n_1,n_2,n_3]^T
n=[n1,n2,n3]T表示旋转的轴,用角度
θ
\theta
θ表示旋转角(右手定则,大拇指转向轴方向时其余四指的方向为正方向),旋转向量则是
θ
n
\theta n
θn。这样就可以表示旋转矩阵了。即
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
⋅
n
−
R=cos\theta I+(1-cos\theta)nn^T+sin\theta · n^-
R=cosθI+(1−cosθ)nnT+sinθ⋅n−
其中 n − n^- n−为 n n n的反对称矩阵,并且可把矩阵外积转化为矩阵内积,即
∀
a
∈
R
3
×
1
,
n
−
⋅
a
=
n
×
a
\forall a \in \mathbb{R^{3×1}},n^-·a = n×a
∀a∈R3×1,n−⋅a=n×a
n
−
=
(
0
−
n
3
n
2
n
3
0
−
n
1
−
n
2
n
1
0
)
n^- = \left(\begin{array}{lcr} 0 & -n_3 & n_2 \\ n_3 & 0 & -n_1 \\ -n_2 & n_1 & 0 \end{array}\right)
n−=⎝⎛0n3−n2−n30n1n2−n10⎠⎞
综上所述,通过GNSS和IMU测得某时刻机器人的 位置 和 角姿态信息,就可以计算出旋转矩阵,进而得到变换矩阵,就可以测得空间中某一点 在机器人眼中的位置。
2. 相机模型
下面介绍相机模型,对于智能体来讲,就需要视觉相机实现定位。本节主要讲解相机的成像过程。
2.1. 针孔模型
针孔相机模型描述了从 现实世界坐标点 到 像素平面坐标点的一种映射。根据小孔成像原理,现实世界的物体经过小孔成像之后,在透镜后方的平面会形成 倒立缩小的实像。
但实际相机我们看到的并不是倒立的,这是通过软件处理将像素平面上的所有点全部关于中心对称之后的结果,这样人眼看到的就是正像了。
如图所示:
如图中所示, P P P为现实世界中的点, O w X w Y w Z w O_w X_w Y_w Z_w OwXwYwZw为静止的世界坐标系, O c X c Y c Z c O_c X_c Y_c Z_c OcXcYcZc为相机坐标系, P ′ P' P′为 P P P经过小孔成像映射到的像素平面 或是 物理成像平面上的点,而 P ′ ′ P'' P′′则是经过软件处理之后,或我们眼睛看到的点, o u v ouv ouv为像素坐标系。
像素坐标系与一般的坐标系不同,像素坐标系的单位为像素,即有多少个像素点,因此像素坐标系需要另外两个参数 d u du du表示 u u u轴上单个像素的长度,单位为米/像素,同理 d v dv dv对应 v v v轴。
需要说明的是,若按上图所示则 P P P会映射到像素坐标系的 P ′ P' P′,这样产生的像为倒像,而实际我们看到的正像是经过对称之后的。有时候为了方便理解,可将像素平面 u o v uov uov直接绘制在相机中心 O c O_c Oc前面 f f f处得到等效图,如下图所示。
2.2. 畸变模型
实际相机由于相机透镜形状可能造成坐标的畸变,即径向畸变(例如实际是直线而像素平面上显示的是曲线),以及像素平面 和 透镜不平行造成的切向畸变。因此对于点
P
P
P的成像,则主要考虑这两种畸变。
而畸变模型的构建则可以看作是 从相机坐标系 向 像素坐标系投影时,不再是简单的相似三角形,而是遵循另一个非线性规律,这个规律将从2.3.给出。
2.3. 根据GNSS与IMU求像素坐标
由于需要从相机的角度对世界坐标系中的点 P P P进行分析,所以需要根据GNSS和IMU的数据计算出像素坐标系(相机显示器)上 P ′ ′ P'' P′′的像素坐标。该问题与1.1.的问题类似。
已知:北东地世界坐标系 O w X w Y w Z w O_w X_w Y_w Z_w OwXwYwZw,以及其一组标准正交基 e w = [ e w x , e w y , e w z ] T e_w = [e_{wx},e_{wy},e_{wz}]^T ew=[ewx,ewy,ewz]T。
点 P P P的世界坐标系为 P w = ( P w x , P w y , P w z ) T P_w=(P_{wx},P_{wy},P_{wz})^T Pw=(Pwx,Pwy,Pwz)T
相机坐标系为 O c X c Y c Z c O_c X_c Y_c Z_c OcXcYcZc且光心 O c O_c Oc的世界坐标为 O c w = ( O c w x , O c w y , O c w z ) T O_{c_w}=(O_{c_{wx}},O_{c_{wy}},O_{c_{wz}})^T Ocw=(Ocwx,Ocwy,Ocwz)T。
机器人的偏航角、俯仰角 和 滚转角 分别为 θ Z w θ Y c θ X c \theta_{Z_w} \theta_{Y_c} \theta_{X_c} θZwθYcθXc。
求: 点 P P P在像素坐标系中的映射点 P ′ ′ P'' P′′的像素坐标 ( P p u ′ ′ , P p v ′ ′ ) T (P''_{pu},P''_{pv})^T (Ppu′′,Ppv′′)T
从 世界坐标系 到 相机坐标系
由于已知 e w e_w ew和光心世界坐标 O c w O_{c_w} Ocw,直接根据式 ( 3 ) (3) (3)求得 e c e_c ec,再根据式 ( 4 ) (4) (4)求得旋转矩阵 R c w R_{cw} Rcw,进而得出点 P P P在相机坐标系中的坐标 P c = ( P c x , P c y , P c z ) T P_c=(P_{cx},P_{cy},P_{cz})^T Pc=(Pcx,Pcy,Pcz)T
P c = R c w P w + t c w P_c=R_{cw}P_w+t_{cw} Pc=RcwPw+tcw
从 相机坐标系 到 像素坐标系
之后就可以将点 P P P从三维的相机坐标系 映射 到二维的像素坐标系了,可直接参考相机等效图计算。
● 若不考虑畸变
点
P
′
′
(
P
p
x
′
′
,
P
p
y
′
′
)
P''(P''_{px},P''_{py})
P′′(Ppx′′,Ppy′′)求得为
P p u ′ ′ = 1 d u ⋅ f ⋅ P c x P c z + c x , P p v ′ ′ = 1 d v ⋅ f ⋅ P c y P c z + c y P''_{pu}=\frac{1}{du} ·f·\frac{P_{cx}}{P_{cz}}+c_x,\quad P''_{pv}=\frac{1}{dv} ·f·\frac{P_{cy}}{P_{cz}}+c_y Ppu′′=du1⋅f⋅PczPcx+cx,Ppv′′=dv1⋅f⋅PczPcy+cy
其中 c x c_x cx和 c y c_y cy分别为 向量 o O c oO_c oOc 在轴 o u ou ou 和 轴 o v ov ov 上的映射长度, f f f为焦距。
再将上述映射通过齐次坐标的形式表示出来则是
[ P p u ′ ′ P p v ′ ′ 1 ] = ( f d u 0 c x 0 0 f d v c y 0 0 0 1 0 ) ⋅ 1 P c z ⋅ [ P c x P c y P c z 1 ] \left[\begin{array}{lcr} P''_{pu} \\ P''_{pv} \\ 1 \end{array} \right] =\left(\begin{array}{lcr} \frac{f}{du} & 0& c_x & 0\\ 0 & \frac{f}{dv}& c_y& 0\\ 0& 0& 1& 0 \end{array}\right)·\frac{1}{P_{cz}}· \left[\begin{array}{lcr} P_{cx} \\ P_{cy} \\ P_{cz} \\ 1 \end{array} \right] ⎣⎡Ppu′′Ppv′′1⎦⎤=⎝⎛duf000dvf0cxcy1000⎠⎞⋅Pcz1⋅⎣⎢⎢⎡PcxPcyPcz1⎦⎥⎥⎤
其中矩阵 K = ( f d u 0 c x 0 f d v c y 0 0 1 ) K = \left(\begin{array}{lcr} \frac{f}{du} & 0& c_x \\ 0 & \frac{f}{dv}& c_y\\ 0& 0& 1 \end{array}\right) K=⎝⎛duf000dvf0cxcy1⎠⎞称为该相机的内参数,即该矩阵只与摄像机有关。旋转矩阵 R R R 和 光心位置 O c w O_{c_w} Ocw则成为相机的外参数。
此时从世界坐标到像素坐标的转换公式就为
P ′ ′ = ( 1 0 0 0 1 0 ) ⋅ K ⋅ R ⋅ ( P w − O c w ) ( 0 0 1 ) ⋅ R ⋅ ( P w − O c w ) (重要) P''=\left(\begin{array}{lcr} 1 & 0& 0\\ 0 &1& 0\\ \end{array}\right) · K·\frac{R·(P_w-O_{c_w})}{(0 \quad 0 \quad 1)·R·(P_w-O_{c_w})} \tag{重要} P′′=(100100)⋅K⋅(001)⋅R⋅(Pw−Ocw)R⋅(Pw−Ocw)(重要)
● 若考虑畸变
点
P
′
′
(
P
p
u
′
′
,
P
p
v
′
′
)
P''(P''_{pu},P''_{pv})
P′′(Ppu′′,Ppv′′)求得为
P p u ′ ′ = 1 d u ⋅ f ⋅ ( l P c x P c z + 2 p 1 P c x P c y P c z 2 + 2 p 2 P c x 2 P c z 2 + p 2 r 2 ) + c x P''_{pu}=\frac{1}{du} ·f·(l \frac{P_{cx}}{P_{cz}}+2p_1\frac{P_{cx}P_{cy}}{P_{cz}^2}+2p_2\frac{P_{cx}^2}{P_{cz}^2}+p_2r^2)+c_x Ppu′′=du1⋅f⋅(lPczPcx+2p1Pcz2PcxPcy+2p2Pcz2Pcx2+p2r2)+cx
P p v ′ ′ = 1 d v ⋅ f ⋅ ( l P c y P c z + 2 p 2 P c x P c y P c z 2 + 2 p 1 P c x 2 P c z 2 + p 1 r 2 ) + c y \quad P''_{pv}=\frac{1}{dv} ·f·(l \frac{P_{cy}}{P_{cz}}+2p_2\frac{P_{cx}P_{cy}}{P_{cz}^2}+2p_1\frac{P_{cx}^2}{P_{cz}^2}+p_1r^2)+c_y Ppv′′=dv1⋅f⋅(lPczPcy+2p2Pcz2PcxPcy+2p1Pcz2Pcx2+p1r2)+cy
其中 l = 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 l=1+k_1r^2+k_2r^4+k_3r^6 l=1+k1r2+k2r4+k3r6,且参数 k 1 , k 2 , k 3 , p 1 , p 2 k_1,k_2,k_3,p_1,p_2 k1,k2,k3,p1,p2为与相机畸变相关参数。
由该过程可见, P P P在像素平面上的投影 P ′ ′ P'' P′′坐标是二维的,因此已知 P w P_w Pw可以求 P ′ ′ P'' P′′,但从 P ′ ′ P'' P′′反推 P w P_w Pw则是不可能的。其实就是单目相机的劣势,少了深度信息。
2.4. 像素数据结构
在知道点 P P P的成像点 P ′ ′ P'' P′′之后,相机还会采取点 P P P的其他信息,包括颜色、距离等信息,有了这些信息才能完整的描述像素 P ′ ′ P'' P′′。
一个像素的数据结构可以用 结构体/类来描述,这里先列出一些属性。
● 像素坐标的位置
u
,
v
u,v
u,v
这个比较好理解,但是u和v是有最大值的,即屏幕的 分辨率 通常所说的1920x1080 或 800x600描述的就是分辨率,表示
u
u
u轴像素个数(列)×
v
v
v轴像素个数(行)。而用数组描述时则为先行后列,例如1920x1080分辨率,构建的数组则为
vector<vector<pixel> image{1080,vector<pixel>{1920}};
● 彩色图像RBG(RedBlueGreen, RGB)
使用红色、蓝色、绿色三通道构成的彩色像素,每个颜色数值可用1Byte(8bit,0~255)表示颜色的深浅,因此RGB一共24Byte。
● 灰度图与灰度值
灰度图没有颜色,表示R=G=B=灰度值时,可用1Byte表示(8bit,0~255,即灰度级别为256)。灰度值为0表示纯黑色,255表示纯白色。
彩色图像可转化为灰度图,RGB可按照一定算法转化为灰度值,例如 灰度值= R×0.3 + G×0.59 + B×0.11
● 深度值
记录该 像素点对应实际物体点 到相机的距离,可通过双目相机模型 或 RGB-D相机测算深度值,单位毫米mm。位数决定最大距离,例如2Byte(16bit,0-65536,0m-65m)。
下面给出一个像素类的例子,注意由于有些数据类型为char在获得实际值得时候需要做类型转换。
extern const horizontalPixelsNum = 1920;
extern const verticalPixelsNum = 1080;
class pixel{
public:
unsigned int u,v; //位置,4B
unsigned char R,G,B; //三通道彩色 1B,0~255
unsigned char gray; //灰度值 1B,0~255
unsigned short depth; //深度值 2B,0~65535
public:
pixel(){u = 0;v = 0;R = 255;G = 255;B = 255;gray = 0.3*R+0.59*G+0.11*B;depth = 0;}
pixel(int row,int column,unsigned char grayScale = 255,unsigned short depthValue=0)
{
if(row > verticalPixelsNum || column > horizontalPixelsNum)
{v = 0; u = 0;}
else
{v = row;u = column;}
gray = grayScale;
R=gray; G=gray; B=gray;
depth = depthValue ;
}
pixel(int r,int c,unsigned char red,unsigned char green,unsigned char blue,unsigned short depthValue = 0)
{
if(row > verticalPixelsNum || column > horizontalPixelsNum)
{v = 0; u = 0;}
else
{v = row;u = column;}
R = red; B = blue; G = green;
gray = 0.3*R + 0.59*G + 0.11*B;
depth = depthValue;
}
~pixel(){};
};
vector<vector<pixel> image{verticalPixelsNum,vector<pixel>{horizontalPixelsNum}};
for(int r = 0;r <= verticalPixelsNum;r++)
for(int c = 0;c <= horizontalPixelsNum;c++)
image[r][c] = pixel(r,c);
cout<<int(image[1079][1919]);
3. 双目相机模型
顾名思义就是两个相机两个成像平面,一般为左右两个相机,两个相机就有利于我们估计点
P
P
P的深度信息,甚至通过图像3D还原物体在现实世界的样子。
如图所示有左右两个相机坐标系
O
R
X
R
Y
R
Z
R
O_RX_RY_RZ_R
ORXRYRZR和
O
L
X
L
Y
L
Z
L
O_LX_LY_LZ_L
OLXLYLZL,点
P
P
P为现实世界的一点。左右双目相机是平行的,光心距离
O
L
O
R
=
b
O_LO_R=b
OLOR=b称作基线。
因此 P P P的两个相机坐标中Z轴的量相同,记作 P c z P_{cz} Pcz。两个成像点的v轴量相同, P P P的成像点分别为 P L ( P L u L , P v ) P_L(P_{L_{uL}},P_v) PL(PLuL,Pv)和 P R ( P R u R , P v ) P_R(P_{R_{uR}},P_v) PR(PRuR,Pv)。因此可以计算 P c z P_{cz} Pcz。
根据相似三角形有
P c z + f P c z = b + P L u L − P R u R b \frac{P_{cz}+f}{P_{cz}}=\frac{b+P_{L_{uL}}-P_{R_{uR}}}{b} PczPcz+f=bb+PLuL−PRuR
从而得到 P P P的相机Z轴坐标
P c z = f b P L u L − P R u R (重要) P_{cz}=\frac{fb}{P_{L_{uL}}-P_{R_{uR}}} \tag{重要} Pcz=PLuL−PRuRfb(重要)
注意 f f f和 b b b均为相机自身属性。因此 P c z P_{cz} Pcz与视差 P L u L − P R u R P_{L_{uL}}-P_{R_{uR}} PLuL−PRuR成反比,即视差越大, P P P距离相机越近。
综上所述,本篇文章主要讲述的是 现实世界到像素平面 的成像过程,而从像素平面 向现实世界的映射则是另外一个问题了。
譬如对双目相机来说,给出两幅成像,如何确定两幅图像的哪个像素 和 哪个像素是 现实世界同一个点?计算量有多大?如何削弱?这都将是未来研究的方向。