全局坐标系与车身坐标系转换
Posted 穿越临界点
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了全局坐标系与车身坐标系转换相关的知识，希望对你有一定的参考价值。
在学习资料满天飞的大环境下，知识变得非常零散，体系化的知识并不多，这就导致很多人每天都努力学习到感动自己，最终却收效甚微，甚至放弃学习。我的使命就是过滤掉大量的无效信息，将知识体系化，以短平快的方式直达问题本质，把大家从大海捞针的痛苦中解脱出来。
文章目录
0 前言
做规划控制必然离不开坐标和坐标变换。一般涉及到的就是全局坐标系和车身坐标系互转，还有笛卡尔坐标系和自然坐标系之间的转换。坐标转换一般会涉及到三角函数、向量、复数、矩阵等知识。
本篇先介绍全局坐标系与车身坐标系之间的转换，为后续的内容热一下身。
1 全局坐标系和车身坐标系
1.1 全局坐标系
一般全局坐标系是将地球作为参考系进行定义的，也就是大地坐标系。
对于行车来说，车辆在大地上的位置信息通常来自于卫星导航系统，位置坐标在WGS-84坐标系（地理坐标系，使用经纬度表示任意位置）中表示。为了方便计算距离和面积，还需要通过通用横轴墨卡托（Universal Transverse Mercator, UTM）网络系统将其转换至笛卡尔坐标系。
但对于局部泊车系统来说，不需要全局定位，也就无需考虑地理坐标系因素，只需要在地图上选取一点作为原点构建全局坐标系即可。
1.2 车身坐标系
车身坐标系是将车辆自身作为参考系建立的坐标系。原点一般选取车辆后轴中心点或者车辆的质心。
车辆上安装的各种传感器（比如摄像头、雷达）的位置，以及车辆的轮廓（如果是用矩形逼近车辆轮廓，则通常使用四个角点表示车辆轮廓）都需要使用车身坐标系下的坐标。
2 坐标系转换
2.1 旋转矩阵推导
2.1.1 坐标变换和向量旋转的关系
一个向量 O P → \\overrightarrowOP OP旋转 θ \\theta θ 角度，相当于先把坐标系A旋转 θ \\theta θ 角度得到坐标系B，然后在坐标系B中取与 O P → \\overrightarrowOP OP相同坐标值的向量 O Q → \\overrightarrowOQ OQ ，最后再把 O Q → \\overrightarrowOQ OQ 的坐标映射回坐标系A。
相应地，将坐标系B（相对于坐标系A的夹角为 θ \\theta θ）中的一个坐标值映射到坐标系A的操作就等同于——在坐标系A中先取和坐标系B中相同坐标值的向量，然后将其旋转 θ \\theta θ 角度。
为什么会有这种等价关系呢？
因为坐标和向量是等价的，坐标只是向量的一种表示方法。坐标系和向量空间也是等价的，坐标轴就是相互正交的单位向量，对坐标轴的操作等价于/可以代替对整个向量空间中向量的操作。
Tips：对概念的深入理解很重要，尤其是等价变换。因为这往往是公式推导和证明的思路。
2.1.2 基于复数和欧拉公式推导
在复平面上使用复数乘法等价向量旋转运算。
O
Q
→
=
O
P
→
⋅
e
i
θ
=
(
x
p
+
i
y
p
)
⋅
(
c
o
s
θ
+
i
s
i
n
θ
)
=
(
c
o
s
θ
⋅
x
p
−
s
i
n
θ
⋅
y
p
)
+
i
(
s
i
n
θ
⋅
x
p
+
c
o
s
θ
⋅
y
p
)
(1)
\\beginalign \\overrightarrowOQ &= \\overrightarrowOP \\cdot e^i\\theta \\\\[2ex] &=(x_p+iy_p) \\cdot (cos\\theta + isin\\theta)\\\\[2ex] &=(cos\\theta \\cdot x_p - sin\\theta \\cdot y_p) + i(sin\\theta \\cdot x_p + cos\\theta \\cdot y_p) \\endalign \\tag 1
OQ=OP⋅eiθ=(xp+iyp)⋅(cosθ+isinθ)=(cosθ⋅xp−sinθ⋅yp)+i(sinθ⋅xp+cosθ⋅yp)(1)
易得：
[
x
q
y
q
]
=
[
c
o
s
θ
,
−
s
i
n
θ
s
i
n
θ
,
c
o
s
θ
]
⋅
[
x
p
y
p
]
=
R
⋅
[
x
p
y
p
]
(2)
\\beginaligned \\left[ \\beginarraycc x_q \\\\ y_q \\endarray \\right ] &= \\left[ \\beginarraycc cos\\theta,& -sin\\theta \\\\ sin\\theta,& cos\\theta \\\\ \\endarray \\right ] \\cdot \\left[ \\beginarraycc x_p \\\\ y_p \\\\ \\endarray \\right ]\\\\[3ex] &=R \\cdot \\left[ \\beginarraycc x_p \\\\ y_p \\\\ \\endarray \\right ] \\endaligned \\tag2
[xqyq]=[cosθ,sinθ,−sinθcosθ]⋅[xpyp]=R⋅[xpyp](2)
其中，
R
R
R即为旋转矩阵。
2.1.3 基于纯向量运算推导
如图所示，坐标系B( x 1 o 1 y 1 x_1o_1y_1 x1o1y1) 相对于坐标系A( x 0 o 0 y 0 x_0o_0y_0 x0o0y0) 旋转了 θ \\theta θ 角度。两个坐标系内的任意两个坐标值相同（指在各自坐标系中的坐标值）的两个向量均相差 θ \\theta θ角度。
所以，求坐标系B中的某一坐标值（向量）在坐标系A中的坐标值（向量投影），就等价于求坐标系B的坐标轴（基底）在坐标系A上的坐标值（投影）。
则有
R
=
[
X
1
0
,
Y
1
0
]
=
[
x
1
⋅
x
0
,
y
1
⋅
x
0
x
1
⋅
y
0
,
y
1
⋅
y
0
]
=
[
c
o
s
θ
,
c
o
s
(
π
2
+
θ
)
s
i
n
θ
,
s
i
n
(
π
2
+
θ
)
]
=
[
c
o
s
θ
,
−
s
i
n
θ
s
i
n
θ
,
c
o
s
θ
]
(3)
\\beginalign R &= [X_1^0, Y_1^0] \\\\[3ex] &= \\left[ \\beginarraycc x_1 \\cdot x_0,& y_1 \\cdot x_0 \\\\ x_1 \\cdot y_0,& y_1 \\cdot y_0 \\\\ \\endarray \\right ] \\\\[3ex] &= \\left[ \\beginarraycc cos\\theta,& cos(\\frac\\pi2+\\theta) \\\\ sin\\theta,& sin(\\frac\\pi2+\\theta) \\\\ \\endarray \\right ]\\\\[3ex] &= \\left[ \\beginarraycc cos\\theta,& -sin\\theta \\\\ sin\\theta,& cos\\theta \\\\ \\endarray \\right ] \\endalign\\tag3
R=[X10,Y10]=全局坐标系与车身坐标系转换