maliput_multilane

Sonamed because it admits multiple Lanes
per Segment
, an advance over its predecessor (monolane
) which only admitted a single Lane
per Segment
.
multilane
is an implementation of the maliput
geometry API which synthesizes a road network from a small set of primitive building blocks, mimicking techniques used in the geometric design of real roads. The basic geometry of a Segment
is derived from the combination of a plane curve, an elevation function, and a superelevation function, combined together to define a ruled surface. A Segment
has a longitudinal reference curve (similar to a Lane
's centerline) and each of the Lanes
of a Segment
is defined via a constant lateral offset, along the segment surface, from that reference curve.
Three coordinate frames are involved in the following discussion:
World
frame.Lane
frame (discussed in section Inertialframe versus Laneframe ) of the Lane
with index \(i\).Segment
, analogous to \((s,r,h)_{LANE,i}\) for a Lane
. The parameter \(p_{SEG} \in [0, 1]\) spans the Segment
longitudinally. \(r_{SEG}\) is a lateral offset from the Segment
's reference curve, along the Segment
surface. \(h_SEG\) is height above the surface.TODO Reconsider the use of the word "geometry" below. The geometry of a
Segment
is completely derived from a map\[ W: (p,r,h)_{SEG} \mapsto (x,y,z) \]
which we will construct in stages, starting with the
Segment
reference curve\[ W(p_{SEG}) \equiv W(p_{SEG},0,0), \]
followed by the
Segment
surface\[ W(p_{SEG},r_{SEG}) \equiv W(p_{SEG},r_{SEG},0). \]
The construction of \(W(p_{SEG},r_{SEG},h_{SEG})\) will involve three fundamental functions, \(G_\text{xy}\), \(G_z\), and \(\Theta\).
The first fundamental function \(G_\text{xy}\) defines a two dimensional planar primitive curve in the \(xy\) plane:
\[ G_{xy}: p_{SEG} \mapsto (x,y). \]
This curve establishes the basic geometric primitive of the Segment
(e.g., "constantradius arc"). We define \(l\) as a pathlength along this plane curve, in the range \([0, l_\text{max}]\), where \(l_\text{max}\) is the total pathlength of the curve. \(G_{xy}\) is specifically parameterized such that
\[ p_{SEG} \equiv \frac{l}{l_\text{max}}; \]
in other words, \(p_{SEG}\) is linear in pathlength along the planar primitive curve and \(p_{SEG} \in [0,1]\).
The second fundamental function \(G_z\) specifies elevation above the \((xy)\)plane (albeit with a peculiar scale factor):
\[ G_z: p_{SEG} \mapsto \frac{1}{l_\text{max}}z \]
Taking \(G_{xy} = (G_x, G_y)\) and \(G_z\) together,
\[ \left(\begin{array}{c} G_{xy}\\ l_\text{max}G_z \end{array}\right): p_{SEG} \mapsto \left(\begin{array}{c}x\\y\\z\end{array}\right) \]
\[ \left(\begin{array}{c}x\\y\\z\end{array}\right) = W(p_{SEG}) = \left(\begin{array}{c} G_x(p_{SEG})\\ G_y(p_{SEG})\\ l_\text{max}G_z(p_{SEG}) \end{array}\right) \]
defines the three dimensional reference curve \(W(p_{SEG})\) for the Segment
. \(G_z\) is constructed with the scale factor of \(1/l_\text{max}\) specifically so that:
\[ \begin{eqnarray*} z & = & l_\text{max} G_z(p_{SEG})\\ & = & l_\text{max} G_z\left(\frac{l}{l_\text{max}}\right)\\ \dot{z} & = & \frac{dz}{dl} = \frac{d}{dp_{SEG}}G_z(p_{SEG}) \end{eqnarray*} \]
This allows us to derive the first derivative of \(G_z\) directly from the World
frame slope \(\dot{z} = \frac{dz}{dl}\) of the segment surface along its reference curve. This is convenient because \(\dot{z}\) is what a road designer would nominally specify as the "slope of the road" or the "grade of the road".
The third fundamental function \(\Theta\) specifies the superelevation of the Segment
surface:
\[ \Theta: p_{SEG} \mapsto \frac{1}{l_\text{max}}\theta \]
Superelevation \(\theta\) is the "twist" in a road, given as a righthanded angle of rotation around the tangent of the reference curve \(W(p_{SEG})\). Zero superelevation leaves the surface parallel with the \(xy\) plane. Note that superelevation becomes ambiguous when the tangent of the reference curve points in the \(\hat{z}\) direction.
As with \(G_z\), \(\Theta\) is scaled so that:
\[ \begin{eqnarray*} \theta & = & l_\text{max} \Theta\left(\frac{l}{l_\text{max}}\right)\\ \dot{\theta} & = & \frac{d\theta}{dl} = \frac{d}{dp_{SEG}}\Theta(p_{SEG}) \end{eqnarray*} \]
With the three fundamental functions in hand, we can express the orientation of the \((\hat{p},\hat{r},\hat{h})_{SEG}\) frame along the reference curve, with respect to the
World
frame, as a roll/pitch/yaw rotation:
We use all three fundamental functions to define a rotation
\[ \begin{align*} \mathbf{R}(p_{SEG}) &= \mathbf{R}_{\gamma(p_{SEG})} \mathbf{R}_{\beta(p_{SEG})} \mathbf{R}_{\alpha(p_{SEG})} \end{align*} \]
where
\[ \begin{align*} \mathbf{R}_{\gamma(p_{SEG})} &= \left(\begin{array}{rrr} \cos\gamma & \sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\ 0 & 0 & 1 \end{array}\right) & \text{(yaw)}\\ \end{align*} \]
\[ \begin{align*} \mathbf{R}_{\beta(p_{SEG})} &= \left(\begin{array}{rrr} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ \sin\beta & 0 & \cos\beta \end{array}\right) & \text{(pitch)} \\ \end{align*} \]
\[ \begin{align*} \mathbf{R}_{\alpha(p_{SEG})} &= \left(\begin{array}{rrr} 1 & 0 & 0 \\ 0 & \cos\alpha & \sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{array}\right) & \text{(roll)} \end{align*} \]
and
\[ \begin{align*} \gamma(p_{SEG}) &= \mathrm{atan2}\negthickspace\left(\frac{dG_y}{dp_{SEG}}, \frac{dG_x}{dp_{SEG}}\right) & \text{(yaw)}\\ \beta(p_{SEG}) &= \arctan\negthickspace\left(\frac{dG_z} {dp_{SEG}}\right) & \text{(pitch)} \\ \alpha(p_{SEG}) &= l_\text{max}\Theta(p_{SEG}) & \text{(roll)} \end{align*} \]
Note that \(\hat{p}_{SEG}\) is solely determined by \(W(p_{SEG})\), and as expected, \(\hat{p}_{SEG} = \frac{W'(p_{SEG})}{\lVert W'(p_{SEG})\rVert}\).
With \(\mathbf{R}(p_{SEG})\) , we can extend the Segment
reference curve \(W(p_{SEG})\) to construct the Segment
surface \(W(p_{SEG}, r_{SEG})\) as:
\[ \begin{align*} \left(\begin{array}{c}x\\y\\z\end{array}\right) = W(p_{SEG},r_{SEG}) = \left( \begin{array}{c} G_{xy}(p_{SEG})\\ l_\text{max} G_z(p_{SEG}) \end{array} \right) + \mathbf{R}(p_{SEG})\negthickspace \begin{pmatrix} 0\\ r_{SEG} \\ 0 \end{pmatrix}. \end{align*} \]
This function defines a ruled surface. For any \(p_{SEG}\), \(W(p_{SEG},r_{SEG})\) is linear in \(r_{SEG}\) and motion along \(r_{SEG}\) is in a straight line.
Now that we have the surface embedding \(W(p_{SEG},r_{SEG})\), we can derive the basis vectors \((\hat{p}, \hat{r}, \hat{h})_{SEG}\) along the surface and the corresponding orientation \(\mathbf{R}(p_{SEG},r_{SEG})\):
\[ \begin{align*} \mathbf{R}(p_{SEG},r_{SEG}) &= \begin{pmatrix}\hat{p} & \hat{r} & \hat{h}\end{pmatrix}\\ \hat{p}_{SEG} &= \frac{\partial_{p_{SEG}} W(p_{SEG},r_{SEG})}{\lVert\partial_{p_{SEG}} W(p_{SEG},r_{SEG})\rVert}\\ \hat{r}_{SEG} &= \frac{\partial_{r_{SEG}} W(p_{SEG},r_{SEG})}{\lVert\partial_{r_{SEG}} W(p_{SEG},r_{SEG})\rVert}\\ \hat{h}_{SEG} &= \hat{p}_{SEG} \times \hat{r}_{SEG} \end{align*} \]
A few things are worth noting at this point:
Segment
's reference curve is consistent in both expressions.Segments
endtoend preserving \(G^1\) continuity, discussed in section Ensuring G¹ Continuity .Finally, with \(\mathbf{R}(p_{SEG},r_{SEG})\) in hand (and points 1 and 2 above), we can define the complete volumetric world map \(W(p_{SEG},r_{SEG},h_{SEG})\) for a Segment
's geometry:
\[ \begin{align*} \begin{pmatrix}x\\y\\z\end{pmatrix} = W(p_{SEG},r_{SEG},h_{SEG}) = \left( \begin{array}{c} G_x(p_{SEG})\\ G_y(p_{SEG})\\ l_\text{max} G_z(p_{SEG}) \end{array} \right) + \mathbf{R}(p_{SEG},r_{SEG})\negthickspace \begin{pmatrix} 0\\ r_{SEG} \\ h_{SEG} \end{pmatrix}. \end{align*} \]
This is simply \(W(p_{SEG},r_{SEG})\) displaced by \(h_{SEG}\) along the surface normal \(\hat{h}_{SEG}\).
A Lane
derives its geometry from its Segment
. In multilane
, the centerline of the Lane
with index \(i\) is a parallel curve with a constant lateral offset \(r_i\) from the reference curve (at \(r_{SEG} = 0\)) of the Segment
. We can express this relationship as a transform between \((s,r,h)_{LANE,i}\) (Lane
frame) and \((p,r,h)_{SEG}\) (Segment
frame):
\[ \begin{align*} \begin{pmatrix} p_{SEG}\\ r_{SEG}\\ h_{SEG} \end{pmatrix} &= \begin{pmatrix} P(s_{LANE,i})\\ r_{LANE,i} + r_i\\ h_{LANE,i} \end{pmatrix} \end{align*} \]
The tricky part here is \(P:s_{LANE,i} \mapsto p_{SEG}\), which relates \(s_{LANE,i}\) to \(p_{SEG}\), and involves the pathlength integral over \(W(p_{SEG},r_{SEG})\).
maliput
defines \(s_{LANE,i}\) as the pathlength along a Lane
's centerline, and in multilane
that centerline is a curve with constant \(r_{SEG} = r_i\). Thus:
\[ \begin{align*} s_{LANE,i} = S(p_{SEG}) &= \left. \int \left\lVert \partial_{p_{SEG}}W(p_{SEG}, r_{SEG}) \right\rVert dp_{SEG} \right\rvert_{r_{SEG} = r_i}. \end{align*} \]
The function \(P\) that we need is the inverse of the pathintegral \(S\).
Unfortunately, there is generally no closedform solution for either \(S\) or \(P\), particularly if the surface is not flat. multilane
will compute \(P(s_{LANE,i})\) and \(S(p_{SEG})\) analytically if possible (e.g., for some flat surfaces) and otherwise will use more costly numerical methods to ensure accurate results. Which makes us wonder, perhaps the Lane
frame of maliput
would be better off using an arbitrary longitudinal parameter \(p_{LANE,i}\) which could be converted to a distance \(s_{LANE,i}\) on demand, instead of the other way around.
TODO: Derivation of orientation at arbitrary \((s,r,h)_{LANE,i}\) point.
TODO: Derivation of motionderivatives.
TODO: Derivation of surface/path curvatures.
multilane
currently implements one form for each of \(G_{xy}\), \(G_z\), and \(\Theta\). \(G_{xy}\) is implemented for a constant curvature arc (which includes zero curvature, i.e., straight line segments). Elevation \(G_z\) and superelevation \(\Theta\) are implemented for cubic polynomials. These forms were chosen because they provide the smallest, simplest set of primitives that allow for the assembly of fully threedimensional road networks that maintain \(G^1\) continuity across segment boundaries.
The exact form that \(G_{xy}\) takes is:
\[ \begin{align*} \begin{pmatrix} x\\ y \end{pmatrix} = G_\text{xy}(p_{SEG}) &= \begin{pmatrix}x_0\\ y_0\end{pmatrix} + \left\lbrace \begin{array} \frac{1}{\kappa}\begin{pmatrix} \cos(\kappa l_\text{max} p_{SEG} + \gamma_0  \frac{\pi}{2})  \cos(\gamma_0  \frac{\pi}{2})\\ \sin(\kappa l_\text{max} p_{SEG} + \gamma_0  \frac{\pi}{2})  \sin(\gamma_0  \frac{\pi}{2}) \end{pmatrix} & \text{for }\kappa > 0\\ l_\text{max} p_{SEG} \begin{pmatrix}\cos{\gamma_0}\\ \sin{\gamma_0}\end{pmatrix} & \text{for }\kappa = 0\\ \frac{1}{\kappa}\begin{pmatrix} \cos(\kappa l_\text{max} p_{SEG} + \gamma_0 + \frac{\pi}{2})  \cos(\gamma_0 + \frac{\pi}{2})\\ \sin(\kappa l_\text{max} p_{SEG} + \gamma_0 + \frac{\pi}{2})  \sin(\gamma_0 + \frac{\pi}{2}) \end{pmatrix} & \text{for }\kappa < 0\\ \end{array} \right\rbrace \end{align*} \]
where \(\kappa\) is the signed curvature (positive is counterclockwise/leftward), \(l_\text{max}\) is the arc length, \(\begin{pmatrix}x_0\\y_0\end{pmatrix}\) is the starting point of the arc, and \(\gamma_0\) is the initial yaw of the (tangent) of the arc (with \(\gamma_0 = 0\) in the \(+\hat{x}\) direction). Note that the \(\kappa = 0\) expression is simply a line segment of length \(l_\text{max}\), and it is the limit of the \(\kappa \neq 0\) expressions as \(\kappa \to 0\).
With regards to geometric road design, a constant curvature \(G_\text{xy}\) does not provide a complete toolkit. Most road designs involve clothoid spirals, which are plane curves with curvature that is /linear/ in path length.This is so that vehicles can navigate roads using continuous changes in steering angle, and, likewise, so that their occupants will experience continuous changes in radial acceleration. multilane
is expected to extend support for clothoid \(G_\text{xy}\) in the future.
For \(G_z\) and \(\Theta\), a cubic polynomial is the lowestdegree polynomial which allows for independently specifying the value and the first derivative at both endpoints. Thus, \(G_z\) takes the form:
\[ \begin{align*} \begin{split} \frac{1}{l_\text{max}}z = G_z(p_{SEG}) &= \frac{z_0}{l_\text{max}} + \dot{z_0} p_{SEG} + \left(\frac{3(z_1  z_0)}{l_\text{max}}  2\dot{z_0}  \dot{z_1}\right) p_{SEG}^2 \\ &\quad + \left(\dot{z_0} + \dot{z_1}  \frac{2(z_1  z_0)}{l_\text{max}}\right) p_{SEG}^3 \end{split} \end{align*} \]
where \(z_0\) and \(z_1\) are the initial and final elevation respectively, and \(\dot{z_0}\) and \(\dot{z_1}\) are the initial and final \(\frac{dz}{dl}\), which is simply the slope of the road as measured by the intuitive "rise over run". \(\Theta\) has an identical expression, with every \(z\) replaced by \(\theta\). Note that \(\dot{\theta} = \frac{d\theta}{dl}\), the rate of twisting of the road, is not particularly intuitive, but that's ok because in general \(\dot{\theta_0}\) and \(\dot{\theta_1}\) will be set by multilane
and not by the road designer, as we will see in section Ensuring G¹ Continuity .
TODO: Tell me more!
Users are not expected to assemble a multilane::RoadGeometry
by constructing individual instances of multilane::Lane
, etc, by hand. Instead, multilane
provides a Builder
interface which handles many of the constraints involved in constructing a valid RoadGeometry
.
TODO: Tell me more!
Multilane provides two loader methods ( Load() and LoadFile() ) that will parse a YAML file or string by calling appropriate Builder methods to create a RoadGeometry
.
The serialization is a fairly straightforward mapping of the Builder interface onto YAML.
The basic idea is, however:
Parsing will fail if there is no way to concretely resolve all of the Endpoint references, e.g., if a document specifies that ConnectionA is an arc starting at the end of ConnectionB and that ConnectionB is an arc starting at the end of ConnectionA. All referential chains must bottom out in explicitlynamed Endpoints.
All the road geometry information must be under a root node called maliput_multilane_builder
, otherwise it will not be parsed.
The following list shows the expected units for floatingpoint quantities:
All positions, distances, lengths, angles and derivatives are floating point numbers. Other type of quantities will be integers.
Clarifications to better understand the nomenclature used within this description:
noncapitalized
strings will be used.maliput
, Capitalized
strings will be used.For points in space, a right handed, orthonormal and inertial ℝ³ frame is used. Its basis is (x̂, ŷ, ẑ), where x̂, ŷ are coplanar with the ground and ẑ points upwards, and positions are expressed as (x, y, z)
triples. Also, the Θ angle rotating around the ẑ axis is used to define headings. These rotations are right handed and an angle of 0° points in the x̂ direction. Angles with respect to a plane parallel to z = 0
can be defined. A heading vector pointing the direction of the lane at that point is used as rotation axis and the angle is clockwise. Those will express superelevation.
Below you can see a snippet with the general YAML structure.
maliput_multilane_builder
maliput_multilane_builder
holds all the common and default configurations to build a RoadGeometry
. All of them, except groups
, must be defined though some of them may be empty.
It will be represented as a mapping:
Where:
RoadGeometry
.connections
' reference curve.preferaccuracy
or preferspeed
The former guides the computations to be as accurate as precision states. The latter will be accurate whenever possible, but it's not guaranteed in favor of faster computations.endpoint
s to build connection
s. At least one point is required to anchor the connections to worldframe.connection
definitions. It may be empty if no Connection
is going to be defined.groups
where connections
can be put together. It may be empty or not defined if no group is going to be made.points
A collection of points in 3D space. Each one will be under a tag (used to reference it within connection
description) and defined by an endpoint_xy
and a endpoint_z
. Both sequences must be provided.
It will be represented as a mapping like:
Where:
xypoint
is the endpoint_xy
sequence.zpoint
is the endpoint_z
sequence.endpoint_xy
A point in the plane z = 0
expressed as (x, y, Θ)
, where (x, y)
defines the position and Θ defines the heading angle of the endpoint. All coordinates must be provided.
It will be represented as a sequence:
Where:
x
coordinate.y
coordinate.endpoint_z
Specifies elevation, slope, superelevation and its speed of change at a point over the plane z = 0
as ‘(z, z’, Θ, Θ'), where
z‘ and Θ’ are the elevation and superelevation of the road at the endpoint and ‘z’‘ and Θ’ are their respective derivatives with respect to an arclength coordinate t
that travels along curve’s projection over the z = 0
plane. All coordinates must be provided.
It will be represented as a sequence:
Where:
z
coordinate.z′
coordinate.connections
A connection
defines a Segment
and must provide the number of lanes, start endpoint
and end endpoint_z
information. Either line length
or arc
must be provided to define the planar geometry of that connection
. Optional extra information can also be provided and it will modify the way the connection
will be created. connections
is a collection of connection
s and those will be identified by their tag. Each tag will name a connection
, can be referenced by other connection
s and to create group
s, and will be used as Segment
's ID as well.
start
endpoint
and end
endpoint_z
can either refer to a reference road curve or to the lane start and end Endpoint
s. When only start
and z_end
or explicit_end
are provided, those will refer to the reference road curve of the Connection
. r_ref
can be provided to state the offset distance from the reference road curve to ref_lane
connection
’s lane. Lanes are 0indexed. In addition, left and right shoulder distances can be provided and those will override default values. left_shoulder
and right_shoulder
must be bigger or equal to zero if provided.
Sample lineconnections mapping are shown below:
Within z_end
, THETA_DOT is optional, and typically should be omitted. When omitted, this value will be automatically calculated such that G1 continuity of the road surface is preserved. Otherwise, provided THETA_DOT will be used and the Builder will check whether or not G1 is preserved.
When explicit_end
is used, THETA_DOT will be set by Builder to preserve G1 road surface continuity.
Within z_end
, THETA_DOT is optional, and typically should be omitted. When omitted, this value will be automatically calculated such that G1 continuity of the road surface is preserved. Otherwise, provided THETA_DOT will be used and the Builder will check whether or not G1 is preserved.
When explicit_end
is used, THETA_DOT will be set by Builder to preserve G1 continuity of the road surface.
None of the lanebased flavors allow to have THETA_DOT at either start
or z_end
. Builder will adjust them to preserve G1 continuity of the road surface.
None of the lanebased flavors allow to have THETA_DOT at either start
or z_end
. Builder will adjust them to preserve G1 continuity of the road surface.
From examples above:
lanes
holds number of lanes, reference lane and distance from the reference lane to the reference curve.left_shoulder
is the extra space at the right side of the last lane of the connection. It will override default values.right_shoulder
is the extra space at the left side of the first lane of the connection
. It will override default values.start
is used to define the start endpoint
of one the connection
’s curves. It may have multiple options. Those can be split into two elements:ref
to point the reference curve.lane.LN
to point a specific lane.endpoint
in the points collection. Either forward
or reverse
should be used to indicate the direction of the endpoint
.endpoint
of a connection
’s reference curve or lane. Either forward or reverse should be used to indicate the direction of the endpoint
. When using the forward the endpoint
will be used as is. Otherwise (using reverse
) the Endpoint
will be reversed.length
is the connection
’s reference road curve planar line length.arc
is the connection
’s reference curve planar piece of arc.z_end
is the endpoint_z
information to end one of the connection
’s curves. It is composed of two elements too. The first one points to the reference curve when ref
is present. Otherwise, lane.LN
must be specified.explicit_end
is a node similar to start
. It is composed of two parts. The first one points to the reference curve when ref
is present. Otherwise, lane.LN
must be specified. The second part is used to point the endpoint
information which could be provided by a connection
or the points
collection. When using a connection
, two options are available: the reference curve or a lane.Possible combinations to define a connection
node are:
connection
must have either length
or arc
.connection
must have either z_end
or explicit_end
.start
and explicit_end
possible combinations:At least one connection must start with "LANE from POINT" or "REF from POINT" in order to anchor the road geometry in the world frame.
arc
Constant radius arcs are defined in terms of a radius and an angle span. arc
s are used to define planar curves on the the z = 0
plane, starting from an endpoint_xy
. Radius must be positive, and arc's center would be to the left (i.e. rotating +90° start endpoint_xy
’s heading vector) when theta is positive, otherwise it would be to the right (i.e. rotating 90° start endpoint_xy
’s heading).
It will be represented as a sequence:
Where:
arc
.arc
.groups
A group specifies a set of connections whose Segments will be placed together in the same Junction
. A connection
may only belong to a single group
. If a connection
is not in any group
, its Segment
will receive its own Junction
.
It will be represented as a mapping:
Where:
C_1
, C_2
, C_3
are connections
’ IDs.