Apollo项目坐标系研究

转自：https://blog.csdn.net/davidhopper/article/details/79162385

声明：本文系作者davidhopper原创，未经允许，不得转载！ 
百度Apollo项目（https://github.com/apolloauto）用到了多种坐标系，其中帮助文档提及的坐标系包括：全球地理坐标系（The Global Geographic coordinate system ）、局部坐标系—东-北-天坐标（The Local Frame – East-North-Up，ENU）、车辆坐标系—右-前-天坐标（The Vehicle Frame —Right-Forward-Up，RFU）。还有一种Frenet坐标系（又称Frenet–Serret公式），Apollo项目文档未提及，但在Apollo项目的规划模块中得到广泛使用。本文首先简介Apollo项目提及的三种坐标系，之后重点介绍Frenet坐标系及其与车辆坐标系之间的转换公式，最后对Apollo项目中Frenet坐标系与车辆坐标系的转换代码进行详细解释。

一、Apollo文档提及的坐标系
（一）全球地理坐标系
Apollo项目使用全球地理坐标系表示高精地图（ the high-definition map，HD Map）中诸元素的几何位置，通常包括：纬度（latitude）、经度（longitude）和海拔（elevation）。全球地理坐标系普遍采用地理信息系统（Geographic Information System，GIS）中用到的WGS-84坐标系（the World Geodetic System dating from 1984），如下图所示，注意海拔定义为椭球体高程（the ellipsoidal height）： 

（二）局部坐标系—东-北-天坐标（ENU）
局部坐标系定义为： 
- X轴：指向东边 
- Y轴：指向北边 
- Z轴：指向天顶 
如下图所示： 
 
ENU局部坐标系采用三维直角坐标系来描述地球表面，实际应用较为困难，因此一般使用简化后的二维投影坐标系来描述。在众多二维投影坐标系中，统一横轴墨卡托（The Universal Transverse Mercator ，UTM）坐标系是一种应用较为广泛的一种。UTM 坐标系统使用基于网格的方法表示坐标，它将地球分为 60 个经度区，每个区包含6度的经度范围，每个区内的坐标均基于横轴墨卡托投影，如下图所示： 

（三）车辆坐标系—右-前-天坐标（RFU）
车辆坐标系定义如下： 
- X轴：面向车辆前方，右手所指方向 
- Y轴：车辆前进方向 
- Z轴：与地面垂直，指向车顶方向 
注意：车辆参考点为后轴中心。 
如下图所示： 

二、Frenet坐标系
Frenet坐标系又称Frenet–Serret公式，Apollo项目文档未提及，但在规划模块中广泛使用。Frenet–Serret公式用于描述粒子在三维欧氏空间 R3ℝ3内沿一条连续可微曲线的运动学特征，如下图所示： 
 
其中，T⃗ T→称为切向量（tangent），表示沿曲线运动方向的单位向量；N⃗ N→称为法向量（normal），表示当前曲线运动平面内垂直于T⃗ T→的单位向量；B⃗ B→称为副法向量（binormal），表示同时垂直于T⃗ T→和N⃗ N→的单位向量。 
令r⃗ (t)r→(t)为欧氏空间内随tt改变的一条非退化曲线。所谓非退化曲线就是一条不会退化为直线的曲线，亦即曲率不为0的曲线。令s(t)s(t)是tt时刻时曲线的累计弧长，其定义如下： 
s(t)=∫t0||r′(σ)||dσ
s(t)=∫0t||r′(σ)||dσ

假定r'≠0r′≠0，则意味着s(t)s(t)是严格单调递增函数。因此可将tt表示为ss的函数，从而有：r⃗ (s)=r⃗ (t(s))r→(s)=r→(t(s))，这样我们就把曲线表示为弧长ss的函数。 
对于采用弧长参数ss表示的非退化曲线r⃗ (s)r→(s)，我们定义其切向量T⃗ T→、法向量N⃗ N→、副法向量B⃗ B→如下： 
T⃗ =dr⃗ ds||dr⃗ ds||
T→=dr→ds||dr→ds||

N⃗ =dT⃗ ds||dT⃗ ds||
N→=dT→ds||dT→ds||

B⃗ =T⃗ ×N⃗ 
B→=T→×N→

基于上述定义的Frenet–Serret公式表示为： 
dT⃗ ds=κN⃗ 
dT→ds=κN→

dN⃗ ds=−κT⃗ +τB⃗ 
dN→ds=−κT→+τB→

dB⃗ ds=−τN⃗ 
dB→ds=−τN→

其中κκ、ττ分别表示曲线r⃗ (s)r→(s)的曲率（ curvature）和挠率（ torsion）。直观地讲，曲率是曲线不能形成一条直线的度量值，曲率越趋于0，则曲线越趋近于直线；挠率是曲线不能形成在同一平面内运动曲线的度量值，挠率越趋于0，则曲线越趋近于在同一平面内运动。 
令T⃗ ′=dT⃗ dsT→′=dT→ds，N⃗ ′=dN⃗ dsN→′=dN→ds，B⃗ ′=dB⃗ dsB→′=dB→ds，则Frenet–Serret公式的矩阵表示形式为： 
⎡⎣⎢⎢⎢T⃗ ′N⃗ ′B⃗ ′⎤⎦⎥⎥⎥=⎡⎣⎢0−κ0κ0−τ0τ0⎤⎦⎥⎡⎣⎢⎢T⃗ N⃗ B⃗ ⎤⎦⎥⎥(1)
[T→′N→′B→′]=[0κ0−κ0τ0−τ0][T→N→B→](1)

易见参数矩阵是反对称（ skew-symmetric）的。 
对于无人驾驶车辆而言，一般对高度信息不感兴趣，因此可以将车辆运动曲线投影到同一平面内，亦即τ=0τ=0，这样Frenet–Serret公式就可以简化为： 
[T⃗ ′N⃗ ′]=[0−κκ0][T⃗ N⃗ ](2)
[T→′N→′]=[0κ−κ0][T→N→](2)
三、Frenet坐标系与笛卡尔坐标系的转换公式
为什么要将笛卡尔坐标系转换为Frenet坐标系？因为可以这样可以将车辆的二维运动问题解耦合为两个一维运动问题。显然，一维问题比二维问题容易求解，这就是笛卡尔坐标系转换为Frenet坐标系的必要性。 
前已述及，在不考虑高度信息的前提下，Frenet坐标系可简化为由曲线切向量T⃗ T→与法向量N⃗ N→组成的二维直角坐标系。如下图所示，假定r⃗ (s)r→(s)是参考线（reference line）在弧长ss处的位置，x⃗ x→是当前车辆轨迹（trajectory）点，该向量一般采用笛卡尔坐标系（常用ENU坐标）表示（x⃗ =[x,y,z]Tx→=[x,y,z]T，zz坐标一般忽略），但这里我们采用弧长ss和横向偏移ll（即沿当前参考线位置r⃗ (s)r→(s)法线方向N⃗ rN→r的偏移量）对其描述，即x⃗ =x⃗ (s,l)x→=x→(s,l)。 

令θrθr、T⃗ rT→r、N⃗ rN→r分别为当前参考线r⃗ (s)r→(s)的方位角、单位切向量和单位法向量，θxθx、T⃗ xT→x、N⃗ xN→x分别为当前轨迹点x⃗ (s,l)x→(s,l)的方位角、单位切向量和单位法向量，根据正交基的定义有： 
T⃗ r=[cosθrsinθr]T
T→r=[cosθrsinθr]T

N⃗ r=[−sinθrcosθr]T
N→r=[−sinθrcosθr]T

T⃗ x=[cosθxsinθx]T
T→x=[cosθxsinθx]T

N⃗ x=[−sinθxcosθx]T
N→x=[−sinθxcosθx]T

根据平面几何知识易知： 
x⃗ (s,l)=r⃗ (s)+l(s)N⃗ r(s)(3)
x→(s,l)=r→(s)+l(s)N→r(s)(3)

从而有（为简洁起见，下面的推导过程均省略参数）： 
lN⃗ TrN⃗ r=N⃗ Tr[r⃗ −x⃗ ]
lN→rTN→r=N→rT[r→−x→]

l=N⃗ Tr[x⃗ −r⃗ ]=[x⃗ −r⃗ ]TN⃗ r(4)
l=N→rT[x→−r→]=[x→−r→]TN→r(4)

(4)式在数学上美观，但不好编程实现，因此换一个表示方法。设x⃗ x→、r⃗ r→的笛卡尔坐标分别为：(x,y)(x,y)、(xr,yr)(xr,yr)，根据两点间距离公式及N⃗ rN→r的定义，可得： 
l=±(x−xr)2+(y−yr)2−−−−−−−−−−−−−−−−−√,if[(y−yr)cosθr−(x−xr)sinθr]>0,positive;otherwisenegetive(4.a)
l=±(x−xr)2+(y−yr)2,if[(y−yr)cosθr−(x−xr)sinθr]>0,positive;otherwisenegetive(4.a)

设a为任意一个变量（或向量），记a˙=d(a)dta˙=d(a)dt，a¨=d(a˙)dta¨=d(a˙)dt，a′=d(a)dsa′=d(a)ds，a′′=d(a′)dsa′′=d(a′)ds，根据该约定，有： 
l˙=[x⃗ ˙−r⃗ ˙]TN⃗ r+[x⃗ −r⃗ ]TN⃗ ˙r
l˙=[x→˙−r→˙]TN→r+[x→−r→]TN→˙r

根据单位切向量和单位法向量的定义（参见第二部分内容），有： 
x⃗ ˙=d||x⃗ ||dtT⃗ x=vxT⃗ x(5)
x→˙=d||x→||dtT→x=vxT→x(5)

r⃗ ˙=d||r⃗ ||dtT⃗ r=s˙T⃗ r(6)
r→˙=d||r→||dtT→r=s˙T→r(6)

根据平面几何知识可知： 
x⃗ −r⃗ =lN⃗ r(7)
x→−r→=lN→r(7)

根据链式求导法则，N⃗ ˙r=dN⃗ rdsdsdtN→˙r=dN→rdsdsdt，又由二维Frenet–Serret公式可知：N⃗ ′r=−κrT⃗ rN→r′=−κrT→r，于是有： 
N⃗ ˙r=−κrs˙T⃗ r(8)
N→˙r=−κrs˙T→r(8)

将(5)-(8)式代入，得： 
l˙=[vxT⃗ x−s˙T⃗ r]TN⃗ r+lN⃗ Tr(−κrs˙T⃗ )r
l˙=[vxT→x−s˙T→r]TN→r+lN→rT(−κrs˙T→)r

因为单位切向量和法向量正交，于是有：T⃗ TrN⃗ r=0T→rTN→r=0，N⃗ TrT⃗ r=0N→rTT→r=0，故： 
l˙=vxT⃗ TxN⃗ r
l˙=vxT→xTN→r

将T⃗ x=[cosθxsinθx)]TT→x=[cosθxsinθx)]T和 N⃗ r=[−sinθrcosθr]TN→r=[−sinθrcosθr]T代入，得到： 
l˙=vx[−cosθxsinθr+sinθxcosθr]=vxsinΔθ(9)
l˙=vx[−cosθxsinθr+sinθxcosθr]=vxsinΔθ(9)

上式中: 
Δθ=θx−θr(10)
Δθ=θx−θr(10)

现在来计算vxvx，根据定义：vx=d||x⃗ ||dt=||dx⃗ dt||=||x⃗ ˙||vx=d||x→||dt=||dx→dt||=||x→˙||，下面先计算x⃗ ˙x→˙。由（3）式，有： 
x⃗ ˙=d(r⃗ +lN⃗ r)dt=r⃗ ˙+l˙N⃗ r+lN⃗ ˙r
x→˙=d(r→+lN→r)dt=r→˙+l˙N→r+lN→˙r

将(6)式和(8)式代入，得到： 
x⃗ ˙=s˙(1−κr)lT⃗ r+l˙N⃗ r(11)
x→˙=s˙(1−κr)lT→r+l˙N→r(11)

于是有： 
vx=||x⃗ ˙||=x⃗ ˙Tx⃗ ˙−−−√=[s˙(1−κr)l]2+l˙2−−−−−−−−−−−−−−√(12)
vx=||x→˙||=x→˙Tx→˙=[s˙(1−κr)l]2+l˙2(12)

下面计算l′l′： 
l′=dds=ddtdtds=l˙s˙
l′=dds=ddtdtds=l˙s˙

将(10)式代入，得到： 
l′=vxs˙sinΔθ(13)
l′=vxs˙sinΔθ(13)

再将(12)式代入，得到： 
l′=(1−κrl)2+l′2−−−−−−−−−−−−√sinΔθ
l′=(1−κrl)2+l′2sinΔθ

l′2=[(1−κrl)2+l′2]sin2Δθ
l′2=[(1−κrl)2+l′2]sin2Δθ

l′2(1−sin2Δθ)=(1−κrl)2sin2Δθ
l′2(1−sin2Δθ)=(1−κrl)2sin2Δθ

假定车辆实际轨迹一直沿参考线附近运动（即不做与参考线反向的运动），使得： 
|Δθ|<π/2
|Δθ|<π/2

1−κrl>0
1−κrl>0

则求解上述关于l′l′的方程得到： 
l′=(1−κrl)tanΔθ(14)
l′=(1−κrl)tanΔθ(14)

将(14)式代入(13)式，可得到速度vxvx的表达式： 
(1−κrl)tanΔθ=vxs˙sinΔθ
(1−κrl)tanΔθ=vxs˙sinΔθ

vx=s˙1−κrlcosΔθ(15)
vx=s˙1−κrlcosΔθ(15)

令sxsx为车辆当前轨迹x⃗ x→的弧长，则有： 
dds=dsxdsddsx=dsxdtdtdsddsx=vxs˙ddsx
dds=dsxdsddsx=dsxdtdtdsddsx=vxs˙ddsx

将(15)式代入，得到： 
dds=1−κrlcosΔθddsx(16)
dds=1−κrlcosΔθddsx(16)

对(14)式求关于参考线弧长ss的偏导： 
l′′=(1−κrl)′tanΔθ+(1−κrl)(Δθ)′cos2Δθ(17)
l′′=(1−κrl)′tanΔθ+(1−κrl)(Δθ)′cos2Δθ(17)

注意到：Δθ=θx−θrΔθ=θx−θr，于是有： 
(Δθ)′=ddsθx−θ′r
(Δθ)′=ddsθx−θr′

根据曲率的定义：κr=θ′r=dθrdsκr=θr′=dθrds，κx=dθxdsxκx=dθxdsx，并将(16)式代入，得到： 
(Δθ)′=1−κrlcosΔθddsxθx−θ′r
(Δθ)′=1−κrlcosΔθddsxθx−θr′

(Δθ)′=κx1−κrlcosΔθ−κr(18)
(Δθ)′=κx1−κrlcosΔθ−κr(18)

将(18)式代入(17)式，得： 
l′′=−(κ′rl+κrl′)tanΔθ+(1−κrl)cos2Δθ[κx1−κrlcosΔθ−κr](19)
l′′=−(κr′l+κrl′)tanΔθ+(1−κrl)cos2Δθ[κx1−κrlcosΔθ−κr](19)

最后求解ax=vx˙=dvxdtax=vx˙=dvxdt。对(15)式求关于时间tt的导数，得： 
ax=s¨1−κrlcosΔθ+s˙dds1−κrlcosΔθs˙
ax=s¨1−κrlcosΔθ+s˙dds1−κrlcosΔθs˙

ax=s¨1−κrlcosΔθ+s˙2cosΔθ[(1−κrl)tanΔθ(Δθ)′−(k′rl+krl′)](20)
ax=s¨1−κrlcosΔθ+s˙2cosΔθ[(1−κrl)tanΔθ(Δθ)′−(kr′l+krl′)](20)

将(18)式代入(20)式，得： 
ax=s¨1−κrlcosΔθ+s˙2cosΔθ[(1−κrl)tanΔθ[κx1−κrlcosΔθ−κr]−(k′rl+krl′)](21)
ax=s¨1−κrlcosΔθ+s˙2cosΔθ[(1−κrl)tanΔθ[κx1−κrlcosΔθ−κr]−(kr′l+krl′)](21)

(4.a)、(14)、(15)、(19)、(21)式便是我们要求的坐标转换公式。
四、Apollo项目中Frenet坐标系与笛卡尔坐标系转换代码
函数声明文件planning/math/frame_conversion/cartesian_frenet_conversion.h：
--------------------- 
作者：知行合一2018 
来源：CSDN 
原文：https://blog.csdn.net/davidhopper/article/details/79162385 
版权声明：本文为博主原创文章，转载请附上博文链接！