目录 Catalog
- 前言 Foreword
- 坐标系 Coordinate Systems
- 参考线几何描述 Geometry
前段时间在看OpenDRIVE文档，做相关的建模。现整理一下参考线（Reference Line）的采样方式，考虑采样出来的每个点都记录一些辅助信息。
前言 Foreword
本文根据OpenDRIVE 1.6 第6章Coordinate Systems以及第7章Geometry编写。
坐标系 Coordinate Systems
关于坐标系，OpenDRIVE 1.6版本并没有改动，很多讲OpenDRIVE的文章也讲得很详细，这里不细讲。
OpenDRIVE的坐标系分为惯性系（xyz）、参考线系（sth）、局部系（uvz），均为右手系，如Figure 5。
Figure 5. available coordinate systems in OpenDRIVE
坐标系的关系可由Figure 6反映。局部系的原点一般为上下文中参考线系的原点，即局部系 u = 0 , v = 0 u=0,v=0 u=0,v=0处，参考线系 s = 0 , t = 0 s=0,t=0 s=0,t=0。
Figure 6. Coordinate systems in OpenDRIVE interacting with another
坐标系的旋转如Figure 7, Figure 10所示。
Figure 7. Inertial coordinate system with defined rotations
Figure 10. Reference line system with defined rotations
从Figure 9可以看出，道路上每个位置的参考线坐标系的 s s s方向沿着参考线切线方向，所以参考系的坐标基可能处处不相等。
Figure 9. Reference line coordinate system
参考线坐标系在OpenDRIVE至关重要，大部分几何属性都是以参考线坐标系为基础定义的。
参考线几何描述 Geometry
OpenDRIVE使用的参考线类型如Figure 19所示（三次曲线cubic polynomials在1.6版本已经弃用）。
Figure 19. Geometry elements in OpenDRIVE
几何体的数据结构如Figure 21所示。
Figure 21. UML model Road Geometry including the Reference Line elements
采样时，我们希望记录每个点的惯性系xyz坐标、参考线系sth坐标（在进行后续Elevation, Superelevation, Shape等变换之前，总有 t = 0 , h = 0 t=0, h=0 t=0,h=0）、参考线系的坐标基在惯性系中的表示。即： V ( s ) = { p ⃗ = ( x , y , z ) , q ⃗ = ( s , t , h ) , e s ⃗ , e t ⃗ , e h ⃗ , κ } . \mathbf{V}(s)=\{\vec{p}=(x,y,z),\vec{q}=(s,t,h),\vec{e_s},\vec{e_t},\vec{e_h},\kappa\}. V(s)={p =(x,y,z),q =(s,t,h),es ,et ,eh ,κ}.
直线 Line
Figure 22. A straight line
<planView>
<geometry
s="0.0000000000000000e+00"
x="-4.7170752711170401e+01"
y="7.2847983820912710e-01"
hdg="6.5477882613167993e-01"
length="5.7280000000000000e+01">
<line/>
</geometry>
</planView>
参考线坐标系 e s ⃗ , e t ⃗ , e h ⃗ \vec{e_s}, \vec{e_t}, \vec{e_h} es ,et ,eh
参考线坐标基可以通过 h d g hdg hdg属性求出：
e s ⃗ = R h d g ∗ e x ⃗ , \vec{e_s}=R_{hdg}*\vec{e_x}, es =Rhdg∗ex , e t ⃗ = R h d g ∗ e y ⃗ , \vec{e_t}=R_{hdg}*\vec{e_y}, et =Rhdg∗ey , e h ⃗ = e z ⃗ , \vec{e_h}=\vec{e_z}, eh =ez ,其中 R h d g = [ c o s ( h d g ) − s i n ( h d g ) 0 s i n ( h d g ) c o s ( h d g ) 0 0 0 1 ] . R_{hdg}=\begin{bmatrix} cos(hdg) & -sin(hdg) & 0 \\ sin(hdg) & cos(hdg) & 0 \\ 0 & 0 & 1 \end{bmatrix}. Rhdg=⎣⎡cos(hdg)sin(hdg)0−sin(hdg)cos(hdg)0001⎦⎤.
曲率 κ \kappa κ
由于直线上处处有曲率 κ = 0 \kappa=0 κ=0，所以参考线坐标系处处相同。
位置 p ⃗ \vec{p} p
对于每个 s s s对应的采样点，下一个采样点 s + Δ s s+\Delta s s+Δs的位置可以通过下式求得：
p ⃗ ( s + Δ s ) = p ⃗ ( s ) + Δ s ∗ e s ⃗ \vec{p}(s+\Delta s)=\vec{p}(s)+\Delta s*\vec{e_s} p (s+Δs)=p (s)+Δs∗es
欧拉螺旋线 & 弧线 Sipral & Arc
此处两种几何线统一处理。
弧线是曲率为定值的曲线，即圆形的一段。
欧拉螺旋线是曲率随长度线性变化的曲线，的主要作用是消除直线与弧线之间的曲率突变。所以，欧拉螺旋线一般会与其他两种曲线相连。
欧拉螺旋线与弧线的曲率都可以取负值。取正表示曲线以逆时针方向绘制，取负表示曲线以顺时针方向绘制。
Figure 23. Road geometry described by a spiral
<geometry s="100.0" x="38.00" y="-1.81" hdg="0.33" length="30.00">
<spiral curvStart="0.0" curvEnd="0.013"/>
</geometry>
Figure 24. Road geometry described by an arc
<planView>
<geometry
s="3.6612031746270386e+00"
x="-4.6416930098385274e+00"
y="4.3409250448366459e+00"
hdg="5.2962250374496271e+00"
length="9.1954178989066371e+00">
<arc curvature="-1.2698412698412698e-01"/>
</geometry>
</planView>
Figure 25. Creating a reference line from geometry elements
曲率 κ \kappa κ
参考线的初始坐标基通过 h d g hdg hdg属性求出：
e s ⃗ ( s s t a r t ) = R h d g ∗ e x ⃗ , \vec{e_s}(s_{start})=R_{hdg}*\vec{e_x}, es (sstart)=Rhdg∗ex , e t ⃗ ( s s t a r t ) = R h d g ∗ e y ⃗ , \vec{e_t}(s_{start})=R_{hdg}*\vec{e_y}, et (sstart)=Rhdg∗ey , e h ⃗ ( s s t a r t ) = e z ⃗ , \vec{e_h}(s_{start})=\vec{e_z}, eh (sstart)=ez ,其中 R h d g = [ c o s ( h d g ) − s i n ( h d g ) 0 s i n ( h d g ) c o s ( h d g ) 0 0 0 1 ] R_{hdg}=\begin{bmatrix} cos(hdg) & -sin(hdg) & 0 \\ sin(hdg) & cos(hdg) & 0 \\ 0 & 0 & 1 \end{bmatrix} Rhdg=⎣⎡cos(hdg)sin(hdg)0−sin(hdg)cos(hdg)0001⎦⎤
曲线上每个点的曲率 κ \kappa κ与 s s s坐标线性相关，有
κ ( s ) = c u r v S t a r t + c u r v E n d − c u r v S t a r t l e n g t h ∗ ( s − s s t a r t ) , \kappa(s)=curvStart+\frac{curvEnd-curvStart}{length}*(s-s_{start}), κ(s)=curvStart+lengthcurvEnd−curvStart∗(s−sstart),其中 l e n g t h length length为曲线终点与起点的 s s s值之差（在三次曲线中 s s s并非等于长度）。
参考线坐标系 e s ⃗ , e t ⃗ , e h ⃗ \vec{e_s},\vec{e_t},\vec{e_h} es ,et ,eh
又有每个点的曲率中心 c ⃗ ( s ) \vec{c}(s) c (s)与 x x x坐标轴夹角为 θ ( s ) \theta(s) θ(s)，满足关系 d θ ( s ) = κ ( s ) ∗ d s , d\theta(s)=\kappa(s)*ds, dθ(s)=κ(s)∗ds,因为螺旋线中 κ = O ( s ) \kappa=O(s) κ=O(s)为一阶关系，所以积分后得到关系： Δ θ ( s ) = κ ( s ) + κ ( s + Δ s ) 2 ∗ Δ s \Delta \theta(s)=\frac{\kappa(s)+\kappa(s+\Delta s)}{2}*\Delta s Δθ(s)=2κ(s)+κ(s+Δs)∗Δs所以 Δ θ ( s ) \Delta \theta(s) Δθ(s)与 s s s也呈线性关系： Δ θ ( s ) = c u r v S t a r t ∗ Δ s + c u r v E n d − c u r v S t a r t l e n g t h ∗ Δ s ∗ ( s + Δ s 2 − s s t a r t ) . \Delta \theta(s)=curvStart*\Delta s+\frac{curvEnd-curvStart}{length}*\Delta s*(s+\frac{\Delta s}{2}-s_{start}). Δθ(s)=curvStart∗Δs+lengthcurvEnd−curvStart∗Δs∗(s+2Δs−sstart).
现考虑这样一种几何关系：
已知 τ ⃗ ( s ) \vec\tau(s) τ (s)，则 τ ⃗ ( s + Δ s ) = R Δ θ ( s ) ∗ τ ⃗ ( s ) \vec\tau(s+\Delta s)=R_{\Delta \theta(s)}*\vec\tau(s) τ (s+Δs)=RΔθ(s)∗τ (s).
可以看到，当 Δ θ ( s ) = 0 \Delta \theta(s)=0 Δθ(s)=0时， τ ⃗ ( s + Δ s ) = τ ⃗ ( s ) \vec\tau(s+\Delta s)=\vec\tau(s) τ (s+Δs)=τ (s)，与直线的结论相同。
之后可以求整个参考线坐标系的基： e s ⃗ ( s ) = τ ⃗ ( s ) \vec{e_s}(s)=\vec\tau(s) es (s)=τ (s) e t ⃗ = R ⊥ ∗ e s ⃗ \vec{e_t}=R_\perp*\vec{e_s} et =R⊥∗es e h ⃗ = e s ⃗ × e t ⃗ \vec{e_h}=\vec{e_s}\times\vec{e_t} eh =es ×et
其中 R ⊥ = [ 0 − 1 0 1 0 0 0 0 1 ] R_\perp=\begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} R⊥=⎣⎡010−100001⎦⎤
位置 p ⃗ \vec{p} p
如果 Δ θ ( s ) = 0 \Delta \theta(s)=0 Δθ(s)=0，则对于每个 s s s对应的采样点，使用直线的采样方案：
p ⃗ ( s + Δ s ) = p ⃗ ( s ) + Δ s ∗ e s ⃗ \vec{p}(s+\Delta s)=\vec{p}(s)+\Delta s*\vec{e_s} p (s+Δs)=p (s)+Δs∗es
其余情况，考虑 r ⃗ ( s ) = − 1 κ ( s ) e t ⃗ \vec{r}(s)=-\frac{1}{\kappa(s)}\vec{e_t} r (s)=−κ(s)1et ，则 p ⃗ ( s + Δ s ) = p ⃗ ( s ) − r ⃗ ( s ) + r ⃗ ( s + Δ s ) \vec{p}(s+\Delta s)=\vec{p}(s)-\vec{r}(s)+\vec{r}(s+\Delta s) p (s+Δs)=p (s)−r (s)+r (s+Δs)
参数三次曲线 Parametric Cubic Curve
有了以上三种基本曲线，我们已经可以描述任意形状的道路参考线。但是对于使用点云拟合道路的来重建道路方式来说，使用这些曲线并不方便。此时，使用参数三次曲线是一个好的选择。如：使用Herimite曲线等特定的参数三次曲线进行拟合，以保证曲线连接处二阶导数连续。
参数三次曲线使用局部系定义点的位置，而局部系的原点即参考线起点。
u ( p ) = a U + b U ∗ p + c U ∗ p 2 + d U ∗ p 3 u(p) = aU + bU*p + cU*p^2 + dU*p^3 u(p)=aU+bU∗p+cU∗p2+dU∗p3 v ( p ) = a V + b V ∗ p + c V ∗ p 2 + d V ∗ p 3 v(p) = aV + bV*p + cV*p^2 + dV*p^3 v(p)=aV+bV∗p+cV∗p2+dV∗p3该曲线还有一个 p R a n g e pRange pRange参数，定义了 p p p的取值范围是 [ 0 , 1 ] [0,1] [0,1]还是 [ 0 , l e n g t h ] [0,length] [0,length]。此时不难看出，每个点上的 s s s不再表示曲线的长度。
Figure 30. A parametric cubic polynom for interpolation of u coordinate
Figure 31. A parametric cubic polynom for interpolation of v coordinate
Figure 32. A parametric cubic polynom
<planView>
<geometry
s="0.000000000000e+00"
x="6.804539427645e+05"
y="5.422483642942e+06"
hdg="5.287405485081e+00"
length="6.565893957370e+01">
<paramPoly3
aU="0.000000000000e+00"
bU="1.000000000000e+00"
cU="-4.666602734948e-09"
dU="-2.629787927644e-08"
aV="0.000000000000e+00"
bV="1.665334536938e-16"
cV="-1.987729787588e-04"
dV="-1.317158625579e-09"
pRange="arcLength">
</paramPoly3>
</geometry>
</planView>
曲线上每个点的位置比较好求（已知 s s s和 p p p有线性关系）。
位置 p ⃗ \vec{p} p
x ( s ) = x s t a r t ( s ) + u ( s ) ∗ c o s ( h d g ) x(s)=x_{start}(s)+u(s)*cos(hdg) x(s)=xstart(s)+u(s)∗cos(hdg) y ( s ) = y s t a r t ( s ) + v ( s ) ∗ s i n ( h d g ) y(s)=y_{start}(s)+v(s)*sin(hdg) y(s)=ystart(s)+v(s)∗sin(hdg) z ( s ) = 0 z(s)=0 z(s)=0
参考线坐标系 e s ⃗ , e t ⃗ , e h ⃗ \vec{e_s},\vec{e_t},\vec{e_h} es ,et ,eh
切线方向
e s ⃗ = R h d g ∗ τ ⃗ ( s ) \vec{e_s}=R_{hdg}*\vec{\tau}(s) es =Rhdg∗τ (s)其中 τ ⃗ ( s ) = n o r m a l i z e d ( t u , t v , 0 ) \vec{\tau}(s)=normalized(t_u,t_v,0) τ (s)=normalized(tu,tv,0)，以下考虑多种情况：
一般来说， n o r m a l i z e d ( t u , t v , 0 ) = n o r m a l i z e d ( d u d p , d v d p , 0 ) normalized(t_u,t_v,0)=normalized(\frac{du}{dp} ,\frac{dv}{dp} ,0) normalized(tu,tv,0)=normalized(dpdu,dpdv,0)
即 t u ( s ) = d u d p = b U + 2 ∗ c U ∗ p + 3 ∗ d U ∗ p 2 t_u(s)=\frac{du}{dp}=bU+2*cU*p+3*dU*p^2 tu(s)=dpdu=bU+2∗cU∗p+3∗dU∗p2 t v ( s ) = d v d p = b V + 2 ∗ c V ∗ p + 3 ∗ d V ∗ p 2 t_v(s)=\frac{dv}{dp}=bV+2*cV*p+3*dV*p^2 tv(s)=dpdv=bV+2∗cV∗p+3∗dV∗p2但可能存在 d u d p = 0 , d v d p = 0 \frac{du}{dp}=0,\frac{dv}{dp}=0 dpdu=0,dpdv=0的情况，此时（通过洛必达法则可以求出）：
t u ( s ) = d 2 u d p 2 = 2 ∗ c U + 6 ∗ d U ∗ p t_u(s)=\frac{d^2u}{dp^2}=2*cU+6*dU*p tu(s)=dp2d2u=2∗cU+6∗dU∗p t v ( s ) = d 2 v d p 2 = 2 ∗ c V + 6 ∗ d V ∗ p t_v(s)=\frac{d^2v}{dp^2}=2*cV+6*dV*p tv(s)=dp2d2v=2∗cV+6∗dV∗p同样，可能存在 d 2 u d p 2 = 0 , d 2 v d p 2 = 0 \frac{d^2u}{dp^2}=0,\frac{d^2v}{dp^2}=0 dp2d2u=0,dp2d2v=0的情况，此时（通过洛必达法则可以求出）：
t u ( s ) = d 3 u d p 3 = 6 ∗ d U t_u(s)=\frac{d^3u}{dp^3}=6*dU tu(s)=dp3d3u=6∗dU t v ( s ) = d 3 v d p 3 = 6 ∗ d V t_v(s)=\frac{d^3v}{dp^3}=6*dV tv(s)=dp3d3v=6∗dV此时若再有 d 3 u d p 3 = 0 , d 3 v d p 3 = 0 \frac{d^3u}{dp^3}=0,\frac{d^3v}{dp^3}=0 dp3d3u=0,dp3d3v=0的情况，不难证明曲线定义非法。
求出 e s ⃗ \vec{e_s} es 后，不难求出 e t ⃗ = R ⊥ ∗ e s ⃗ \vec{e_t}=R_\perp*\vec{e_s} et =R⊥∗es , e h ⃗ = e s ⃗ × e t ⃗ \vec{e_h}=\vec{e_s}\times\vec{e_t} eh =es ×et
其中 R ⊥ = [ 0 − 1 0 1 0 0 0 0 1 ] R_\perp=\begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} R⊥=⎣⎡010−100001⎦⎤
曲率 κ \kappa κ
使用参数方程曲率公式求出曲率： κ ( s ) = u ′ ( p ) ∗ v ′ ′ ( p ) − v ′ ( p ) ∗ u ′ ′ ( p ) ( u ′ ( p ) 2 + v ′ ( p ) 2 ) 1.5 \kappa(s)=\frac{u'(p)*v''(p)-v'(p)*u''(p)}{(u'(p)^2+v'(p)^2)^{1.5}} κ(s)=(u′(p)2+v′(p)2)1.5u′(p)∗v′′(p)−v′(p)∗u′′(p)而 u ′ ( p ) 2 + v ′ ( p ) 2 = 0 u'(p)^2+v'(p)^2=0 u′(p)2+v′(p)2=0时曲率为0（由洛必达法则可证）.
来源：oschina
链接：https://my.oschina.net/u/4408053/blog/4549981