- Generated by
|
maliput_malidrive
|
RoadCurve defines an interface for a path in a Segment object surface. The path is defined by an elevation and superelevation Function objects and a GroundCurve reference curve. The latter is a C¹ function in the \( z=0 \) plane. Its domain is constrained in \( [GroundCurve::p0(); GroundCurve::p1()] \) interval and it should map a ℝ² curve.
As per notation, \( p \) is the parameter of the reference curve, not necessarily arc length \( s \), and function interpolations and function derivatives as well as headings and heading derivatives are expressed in INERTIAL Frame coordinates.
The geometry here revolves around an abstract "world function":
\( W: (p,r,h) \mapsto (x,y,z) ∈ ℝ³ \)
which maps a Lane-frame position to its corresponding representation in world coordinates (with the caveat that instead of the lane's native longitudinal coordinate s, the reference curve parameter p is used).
\( W \) is derived from three functions which define the lane:
GroundCurve implementation).Function implementation).Function implementation).as:
\( (x,y,z) = W(p,r,h) = (G(p), Z(p)) + R_{αβγ} x (0, r, h) \)
where:
( \( R_{αβγ} \) is essentially the orientation of the \( (s,r,h) \) Lane-frame at a location \( (s,0,0) \) on the reference-line of the lane. However, it is not necessarily the correct orientation at \( r \ne 0 \) or \( h \ne 0 \).)
The \( W(p,r,h) \) "world function" is defined by the RoadCurve referenced by a Lane's Segment. A Lane is also defined by a \( r0 \) lateral offset with respect to the reference curve of the RoadCurve. Thus, a mapping from the local \( (s,r,h) \) lane-frame of the Lane becomes:
\( (x,y,z) = L(s,r,h) = W(P(s, r0), r + r0, h) \)
Where
Let \( p_L = (p, r, h) \) be a point in the RoadCurve surface. The image of the world function \( W(p_L) \) can be expressed as:
\( W(p_L) = W(p, r, h) = (G(p), Z(p)) + R_{αβγ} x (0, r, h) \)
Let \( p_L = (p, r, h) \) be a point in the RoadCurve surface. The tangent vector expressed as the derivative of the world function with respect to \( p \) can be expressed as:
\( \frac{\partial W(p_L)}{\partial p} = \frac{\partial W(p, r, h)}{\partial p} \)
\( \frac{\partial W(p_L)}{\partial p} = (\frac{dG(p)}{dp}, \frac{dZ(p)}{dp}) + \frac{dR_{αβγ}}{dp} x (0, r, h) \)
and
\( \frac{dR_{αβγ}}{dp} = (\frac{dR_{αβγ}}{dα} \frac{dR_{αβγ}}{dβ} \frac{dR_{αβγ}}{dγ}) x (\frac{dα}{dp}, \frac{dβ}{dp}, \frac{dγ}{dp}) \)
There are analytic definitions for each derivative expressed above via the API of GroundCurve, Function, and RollPitchYaw classes.
Specifically, the orientation at the centerline is the orientation of the surface in the INERTIAL frame at any point mapped by the world function whose domain is \( (p, 0, 0) \) (which yields a curve on the surface).
Then, \( R_{αβγ} = R(α, β, γ) \)
Lanes in maliput define a volume as a orthonormal extrusion of the surface in the elevation bounds. The orientation \( p_L = (p, r, h) \) is computed as:
\( \hat{s} = ||\frac{\partial W(p, r, h)}{\partial p}|| \)
\( \hat{r} = R_{αβγ} * (0., 1., 0.) \)
Note that in certain cases \( \hat{s}, \hat{r} \) are not orthogonal, as documented in https://github.com/ToyotaResearchInstitute/malidrive/issues/458#issuecomment-663767176
And,
Then, we can compute the rotation matrix out of \( (α, β, γ) \).
Given a point \( p_W = (x, y, z) \) there might exist a point \( p_L = (p, r, h)\) that satisfies \( W(p_L) = p_W \).
The inverse function is procedurally computed by:
Consider a parameterized curve in 3D space called \( C_{0} \) whose coordinates in an inertial frame with orthonormal basis vectors \( \hat{x}, \hat{y}, \hat{z} \) are defined by continuous functions of \( p ∈ [p_0, p_1] \): \( x(p), y(p), z(p) \) with the requirement that \( x(p), y(p) \) do not jointly stagnate with respect to \( p \) (i.e. that there exists a value of \( k \) such that \( (\frac{dx}{dp})^2 + (\frac{dy}{dp})^2 >= k > 0 \)).
The unit vector tangent to \( C_{0} \) is given by \( \hat{t}(p) = \frac{1}{\sqrt{(\frac{dx}{dp})^2 + (\frac{dy}{dp})^2 + (\frac{dz}{dp})^2}} [ \frac{dx}{dp}, \frac{dy}{dp}, \frac{dz}{dp} ]^T = \frac{C_{0}'}{||C_{0}'||}\)
A vector \( n_0(p) \) is defined as the component of \( \hat{z} \) that is orthogonal to \( \hat{t} \) as \( n_0(p) = \hat{z} - <\hat{t}, \hat{z}> \hat{t} \). The vector \( n_0(p) \) has non-zero length if \( \hat{z} \) and \( \hat{t} \) are not identical, and the condition restricting joint stagnation of \( x(p), y(p) \) is sufficient to guarantee this. A unit vector \( \hat{n}_0 \) is defined by normalizing \( n_0 \): \( \hat{n}_0(p) = \frac{n_0}{||n_0||} \)
A third unit vector orthogonal to both \( \hat{n}_0(p), \hat{t}(p) \) is defined as \( \hat{b}_0(p) = \hat{n}_0(p) \times \hat{t}(p) \). The unit vectors \( \hat{t}(p), \hat{b}_0(p), \hat{n}_0(p) \) form an orthonormal basis.
Another orthonormal basis is defined by rotating \( \hat{t}(p), \hat{b}_0(p), \hat{n}_0(p) \) about \( \hat{t}(p) \) by an angle specified by a continuous function \( Θ(p) \). Using the Rodrigues rotation formula: with components of \( \hat{t} \) in the inertial frame given as \( \hat{t}(p) = t_x \hat{x} + t_y \hat{y} + t_z \hat{z} \), identity matrix \( I \), and \( W = \begin{pmatrix} 0 & -t_z & t_y \\ t_z & 0 & -t_x \\ -t_y & t_x & 0 \end{pmatrix} \), a Rotation matrix is defined as \( R_Θ(p) = I + \sin Θ(p) W + 2 \sin^2 \frac{Θ(p)}{2} W^2 \). By construction, rotation by \( R_Θ(p) \) does not affect \( \hat{t}(p) \), so the new basis vectors are defined as \( \hat{t}, \hat{b} = R_Θ(p) \hat{b}_0, \hat{n} = R_Θ(p) \hat{n}_0 \). A rotation matrix \( R_{\hat{t},\hat{b},\hat{n}} \) is defined with these unit vectors as its columns: \( R_{\hat{t},\hat{b},\hat{n}} = [\hat{t},\hat{b},\hat{n}] \).
Note that these basis vectors \( \hat{t}, \hat{b}, \hat{n} \) bear some resemblance to the basis vectors of a Frenet-Serret frame, since the tangent vector \( \hat{t} \) is identical in both formulations, but the other unit vectors have significant differences. Whereas the Frenet-Serret normal unit vector is defined based on the curvature of the 3D curve, in this formulation it is constrained to be a projection of the inertial \( \hat{z} \) axis subject to a specified rotation. Additionally, the order of the unit vectors differs, with Frenet-Serret defining a right-hand basis with tangent, normal, and binormal ( \( \hat{T} \times \hat{N} = \hat{B} \)), while this formulation defines the right-hand basis with tangent, binormal, normal ( \( \hat{t} \times \hat{b} = \hat{n} \)).
To compute the derivative of the rotation matrix \( R_{\hat{t},\hat{b},\hat{n}} \) with respect to \( p \), the following relationship is used that involves \( ω_{\hat{t},\hat{b},\hat{n}} \), which is like an angular velocity of the basis vectors expressed in the inertial frame with respect to changes in \( p \) rather than changes in time: \( \frac{dR_{\hat{t},\hat{b},\hat{n}}}{dp} = ω_{\hat{t},\hat{b},\hat{n}} \times R_{\hat{t},\hat{b},\hat{n}} \). The term \( ω_{\hat{t},\hat{b},\hat{n}} \) is a a sum of two components: a component \( ω_{⟂ \hat{t}} \) that is orthogonal to \( \hat{t} \) and a component parallel to \( \hat{t} \), which is derived from the derivative of the rotation function \( Θ'(p) = \frac{dΘ}{dp} \).
\( ω_{\hat{t},\hat{b},\hat{n}} = ω_{⟂ \hat{t}} + Θ'(p) \hat{t} \)
Since \( \hat{t} \) has unit length, its derivative \( \frac{d\hat{t}}{dp} \) can be expressed as a cross product with the angular velocity component \( ω_{⟂ \hat{t}} \) that is orthogonal to \( \hat{t} \):
\( \frac{d\hat{t}}{dp} = ω_{⟂ \hat{t}} \times \hat{t} \).
Recalling that \( \hat{t} = \frac{C_{0}'}{||C_{0}'||}\), its derivative is computed as:
\( \frac{d\hat{t}}{dp} = \frac{||C_{0}'|| C_{0}'' - \frac{d||C_{0}'||}{dp} C_{0}'}{||C_{0}'||^2} \)
Recalling that \( ||C_{0}'|| = \sqrt{x'^2 + y'^2 + z'^2} \),
it can be differentiated as:
\( \frac{d||C_{0}'||}{dp} = \frac{1}{2 ||C_{0}'||} (2 x' x'' + 2 y' y'' + 2 z' z'') \).
\( \frac{d||C_{0}'||}{dp} = \frac{<C_{0}', C_{0}''>}{||C_{0}'||} \)
\( \frac{d||C_{0}'||}{dp} = <\hat{t}, C_{0}''> \)
and substituted back as
\( \frac{d\hat{t}}{dp} = \frac{||C_{0}'|| C_{0}'' - <\hat{t}, C_{0}''> C_{0}'}{||C_{0}'||^2} \)
\( \frac{d\hat{t}}{dp} = \frac{C_{0}'' - <\hat{t}, C_{0}''> \hat{t}}{||C_{0}'||} \)
\( \frac{d\hat{t}}{dp} = \frac{C_{0 ⟂ \hat{t}}''}{||C_{0}'||} \)
where \( C_{0 ⟂ \hat{t}}'' \) is the component of \( C_{0}'' \) that is orthogonal to \( \hat{t} \).
Then
\( \frac{d\hat{t}}{dp} = \frac{C_{0 ⟂ \hat{t}}''}{||C_{0}'||} = ω_{⟂ \hat{t}} \times \hat{t} \).
Since both \( C_{0 ⟂ \hat{t}}'' \) and \( ω_{⟂ \hat{t}} \) are orthogonal to \( \hat{t} \), the following can be shown to be equivalent:
\( ω_{⟂ \hat{t}} \times \hat{t} = \frac{C_{0 ⟂ \hat{t}}''}{||C_{0}'||} \)
and
\( ω_{⟂ \hat{t}} = \hat{t} \times \frac{C_{0 ⟂ \hat{t}}''}{||C_{0}'||} \).
The derivative of the rotation matrix is thus
\( \frac{dR_{\hat{t},\hat{b},\hat{n}}}{dp} = (\hat{t} \times \frac{C_{0 ⟂ \hat{t}}''}{||C_{0}'||} + Θ'(p) \hat{t}) \times R_{\hat{t},\hat{b},\hat{n}} \).
Consider a surface that contains \( C_{0} \) and whose surface normal at each point on \( C_{0} \) is given by \( \hat{n} \).
Let the previous parametrized curve in 3D be called \( C_{0} \) and let \( C_{offset} \) be an offset curve at a distance \( r_{(p)} \) along \( \hat{b}_{(p, 0)} \) on the surface where \( C_{0} \) is defined. \( r_{(p)} \) is a C¹ function of \( p \). Then, the curve \( C_{offset_{(p)}} \) can be expressed as:
\( C_{offset_{(p)}} = C_{0_{(p)}} + r_{(p)} \hat{b}_{(p, 0)} \)
We would like to compute the rotation matrix out of the frame basis for any \( p ∈ [p_0, p_1] \) in \( C_{offset_{(p)}} \). To that end, we compute \( C_{offset_{(p)}} \) derivative with respect to \( p \):
\( \frac{dC_{offset_{(p)}}}{dp} = \frac{dC_{0_{(p)}}}{dp} + \frac{dr_{(p)}}{dp} \hat{b}_{(p, 0)} + r_{(p)} \frac{d\hat{b}_{(p, 0)}}{dp} \)
And to compute \( \hat{t}_{(p,r)}, \hat{b}_{(p,r)}, \hat{n}_{(p,r)} \) (i.e. \( \hat{t}_{offset}, \hat{b}_{offset}, \hat{n}_{offset} \) ) we use the same procedure as above.
Let the previous parametrized curve in 3D be called \( C_{0} \) and let \( C_{offset} \) be an offset curve at a distance \( r_{(p)} \) along \( \hat{b}_{(p, 0)} \) on the surface where \( C_{0} \) is defined. And let \( r_{k} \) be a scalar measured in the \( \hat{b}_{(p, r_{(p)})} \) axis from a point in the \( C_{offset} \) curve at a certain \( p \) value. In addition, let \( v \) be a linear speed vector expressed in the orthonormal basis \( \hat{t}_{offset}, \hat{b}_{offset}, \hat{n}_{offset} \) at \( C_{offset_{(p)}} \).
We are interested in measuring the derivative at the \( r_{k} \) distance from \( C_{offset(p)} \) along \( \hat{b}_{(p, 0)} \). That point in the INERTIAL Frame can be expressed as:
\( C_{offset_{(p, r_{k})}} = C_{offset_{(p)}} + r_{k} \hat{b}_{p, r_{(p)}} \)
\( C'_{offset_{(p, r_{k})}} = C'_{offset_{(p)}} + r_{k} \hat{b}'_{p, r_{(p)}} \)
TODO(agalbachicar / scpeters): I think the next step should be something like the following: \( C'_{offset_{(p, r_{k})}} = C'_{offset_{(p)}} + r_{k} ω_{⟂ \hat{b}_{p, r_{(p)}}} \times \hat{b}_{p, r_{(p)}} </blockquote> \)
\( C'_{offset_{(p, r_{k})}} = (1 - r_{k} κ_{offset_{(p)}}) C'_{offset_{(p)}} \)
Where \( κ_{offset_{(p)}} = || \hat{t}'_{(p, r_{(p)})} || \). Consequently, it is expected a change \( v \) by a factor of \( (1 - r_{k} κ_{offset_{(p)}}) \) in the \( \hat{t}_{(p, r_{(p)})} \) direction.
> TODO(agalbachicar / scpeters): Given an offset in the > \( \hat{n}_{p, r_{(p)}}\) direction at \( C_{offset_{(p)}} \), is there any > vertical curvature like quantity that we can introduce to scale > \( v \) ?
Another route to derive the same, i.e. the scaling of \( v \) at a certain position in the curve offset ( \( C_{offset} \)) frame at \( p \), would imply the following procedure:
\( C_{offset_{(p)}} = C_{0(p)} + R_{\hat{t}, \hat{b}, \hat{n}} \times [0, r_{(p)}, 0]^T \)
And the position of a point with a displacement \( [0, r_k, h_k] \) from the curve offset at \( p \) is:
\( C_{offset_{(p, r_k, h_k)}} = C_{0(p)} + R_{\hat{t}, \hat{b}, \hat{n}} \times [0, r_{(p)}, 0]^T + R_{offset_{\hat{t}, \hat{b}, \hat{n}}} \times [0, r_k, h_k]^T \)
\( C_{offset_{(p, r_k, h_k)}} = C_{offset_{(p)}} + R_{offset_{\hat{t}, \hat{b}, \hat{n}}} \times [0, r_k, h_k]^T \)
Where \( R_{offset_{\hat{t}, \hat{b}, \hat{n}}} \) is the rotation matrix at \( C_{offset_{(p)}} \) which is computed from \( \hat{t}_{offset}, \hat{b}_{offset}, \hat{n}_{offset}\). The procedure to compute the basis is exactly the same as it was computed before to derive \( R_{\hat{t}, \hat{b}, \hat{n}} \) but, this time, \( C'_{offset_{(p)}} \) is required to obtain \( \hat{t}_{offset} \) and the other vectors.
To compute \( C'_{offset_{(p, r_k, h_k)}} \), we make use of \( C'_{offset_{(p)}} \):
\( C'_{offset_{(p, r_k, h_k)}} = C'_{offset_{(p)}} + R'_{offset_{\hat{t}, \hat{b}, \hat{n}}} \times [0, r_k, h_k]^T \)
We can apply an analogous procedure to compute \( R'_{offset_{\hat{t}, \hat{b}, \hat{n}}} \) and replace the subscript \( _{0} \) by \( _{offset} \). However, the expansion of \( C''_{offset} \) requires \( R''_{\hat{t}, \hat{b}, \hat{n}} \) to exist, leading to the need of \( C'''_{0} \) to exist too.
> TODO(agalbachicar / scpeters): Determine whether \( C'''_{0} \) is > really needed or not and if for the geometries we are working with there > is a real need of it to be other than a zero, or a well-known constant.
For planar curves, it is expected a change \( v \) by a factor of \( \frac{|| C'_{offset_{(p, r_k, h_k)}} ||}{|| C'_{offset_{(p)}} ||} \) which is the scale ratio of tangent vectors.
This document contains two separate approaches for defining a Frenet-like coordinate frame for each point on a 3D curve \( C_0 \): using RollPitchYaw and by computing unit vectors directly.
Q: Are these approaches equivalent? Are they correct?
Consider a point fixed to \( C_0 \)'s Frenet-like frame. By computing the velocity of that point, an additional (recursive) Frenet-like frame can be computed at that point, starting with a tangent vector computed from the velocity of that point.
Q: For the formulation based on RollPitchYaw, the tangent and binormal unit vectors of that recursive Frenet-like frame are not orthogonal. Where is the problem? Should the approach based on unit vectors be preferred?
In the road context, our documentation says in several places that C¹ continuity is required for the ground curve function, but the formulation defined using unit vectors includes references to the second derivative of \( C_0 \) when computing the velocity of points fixed to the Frenet-like frame. Furthermore, when defining an additional curve at a specified offset from \( C_0 \) and computing a Frenet-like frame for the offset curve, then computing the velocity of points fixed to the offset Frenet-like frame have references to the third derivative of \( C_0 \).
Q: What continuity constraints are required for the curve \( C_0 \) ? And to the surface where the curve \( C_0 \) and its offsets are embedded?