- Generated by
|
maliput_multilane
|
So-named 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 inertial_frame_versus_lane_frame ) 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
Segmentis completely derived from a map\[ W: (p,r,h)_{SEG} \mapsto (x,y,z) \]
which we will construct in stages, starting with the
Segmentreference curve\[ W(p_{SEG}) \equiv W(p_{SEG},0,0), \]
followed by the
Segmentsurface\[ 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., "constant-radius arc"). We define \(l\) as a path-length along this plane curve, in the range \([0, l_\text{max}]\), where \(l_\text{max}\) is the total path-length 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 path-length 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 right-handed 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 end-to-end 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 path-length integral over \(W(p_{SEG},r_{SEG})\).
maliput defines \(s_{LANE,i}\) as the path-length 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 path-integral \(S\).
Unfortunately, there is generally no closed-form 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 motion-derivatives.
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 three-dimensional 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 lowest-degree 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 Connection-A is an arc starting at the end of Connection-B and that Connection-B is an arc starting at the end of Connection-A. All referential chains must bottom out in explicitly-named 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 floating-point 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:
non-capitalized 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_buildermaliput_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.prefer-accuracy or prefer-speed 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.endpoints to build connections. At least one point is required to anchor the connections to world-frame.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.pointsA 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_xyA 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_zSpecifies elevation, slope, superelevation and its speed of change at a point over the plane z = 0 as ‘(z, z’, Θ, Θ'), wherez‘ and Θ’ are the elevation and superelevation of the road at the endpoint and ‘z’‘ and Θ’ are their respective derivatives with respect to an arc-length 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.connectionsA 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 connections and those will be identified by their tag. Each tag will name a connection, can be referenced by other connections and to create groups, 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 Endpoints. 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 0-indexed. 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 line-connections 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 lane-based 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 lane-based 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.
arcConstant radius arcs are defined in terms of a radius and an angle span. arcs 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.groupsA 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.