文章目录

OpenDRIVE Notes
- #1 前言
- #2 OpenDRIVE结构
- - #2.1 Road
  - - #2.1.1 道路属性
    - #2.1.2 道路联接
    - #2.1.3 参考线
  - #2.2 laneSection
  - #2.3 laneOffset
  - #2.4 junction
  - - #2.4.1 路口的联接
  - #2.5 poly3(三次多项式)
- #3 解析
- - #3.1 数据结构
  - - #3.1.1 ID
    - #3.1.2 Point
- #4 构建topo
- #5 邻接点
- #6 路径规划
- #7 UI可视化

OpenDRIVE Notes

在这里插入图片描述

#1 前言

这篇文章很简单, 记录这段时间使用OpenDRIVE的心得, 加深印象, 也便于后期查阅。
先说说使用OpenDRIVE最终要达成的目标: 输入一个OpenDRIVE地图文件, 在UI界面上显示出来, 并且能在UI界面上进行交互(查找最近车道、路径规划等功能)。

大致分为以下几个部分:

OpenDRIVE结构(重点)
解析
构建Topo
查找邻接点
路径规划
UI界面

#2 OpenDRIVE结构

下面介绍到的结构, 只是OpenDRIVE的其中一部分, 有些元素(element)可能并不是很重要, 这里会忽略, 只讲我们需要到的元素结构(目的是: UI展示+路径规划)。

#2.1 Road

道路。一个地图(Map)由若干个Road构成, Road之间的联接关系体现在元素中。

#2.1.1 道路属性

路口: 标记一个道路是路口道路还是普通道路
- 路口道路: road::junction为路口(junction)的id
- 普通道路: road::junction为-1

<!-- 普通车道 -->
<road name="Road 1" length="3.3943470911332746e+1" id="1" junction="-1">/road>
<!-- 路口车道, 属于id=100的路口 -->
<road name="Road 2" length="3.3943470911332746e+1" id="2" junction="100">/road>

限速: 当前道路的限速, 注意: 限速不是一个道路只有一个限速, 可能存在一个道路出现多段不同的限速

<!-- 0~20 限速: 70km/h; 20以后 限速: 80km/h; -->
<type s="0.0000000000000000e+0" type="town">
  <speed max="35" unit="km/h"/>
</type>
<type s="20.0000000000000000e+0" type="town">
  <speed max="40" unit="km/h"/>
</type>

#2.1.2 道路联接

道路联接可以用来表示一条车道的topo关系, 有前驱(predecessor)道路和后继(successor)道路, 前驱和后继是相对于参考线(reference line)的方向来定义(后面会讲到什么会是参考线), 沿着参考线方向的下一个联接道路为后继道路, 反之为前驱道路。

被链接道路的类型(elementType): road_link_predecessorSuccessor::elementType用于标记当前道路联接的是路口还是普通车道, 如果是路口, 可能会联接多条车道, 进通过当前元素不足以得出结果, 还需要对应的路口(<junction>)是如何定义(后面会讲到junction元素)。
接触点类型: road_link_predecessorSuccessor::contactPoint被联接道路的接触点
- start: 被链接道路参考线的起点与其联接。
- end: 被链接道路参考线的终点与其联接。

<road name="Road 4" length="1.7627947484681812e+0" id="4" junction="-1">
  <link>
      <!-- 前驱道路: 联接junction为585的路口, 可能存在多个前驱道路, 具体前驱需要查看对应<junction>的定义  -->
      <predecessor elementType="junction" elementId="585"/>
      <!-- 后继道路: 联接road位5的道路, 且联接到被链接道路的起点位置  -->
      <successor elementType="road" elementId="5" contactPoint="start"/>
  </link>
  ...
</road>

#2.1.3 参考线

道路参考线是每条道路的基本元素, 描述道路形状以及其他属性的几何元素都依照参考线来定义, 参考线沿s方向延伸, 道路信息物体则是沿t方向伸展。参考线的形状用<geometry>元素来表示。每条道路仅有一个参考线。参考线的方向与道路前驱后继并无直接关系, 可能存在两条前驱后继关系的道路, 它们的参考项方向正好相反的情况。参考线不能出现段口不能出现打结的情况。
参考线在实际的道路中并不存在, 但是道路上的车道和特征都是基于参考线横向平移得出。

在这里插入图片描述

参考线有以下几种:

直线(line)
螺旋线(spiral)
弧线(arc)
三次多项式曲线(Poly3)(已弃用)
带参数三次多项式曲线(ParamPoly3)

#2.2 laneSection

车道段。一个道路有多个车道段组成, 每个车道段里包含多条车道。车道段里的车道分为左边车道、右边车道和中心车道, 每个车道段有且只有一个中间车道。

start_position(s): 表示当前车道段起点在s-t坐标系中的位置(单位:m)
车道id:
- 中心车道: id为0
- 左边车道: id沿t方向一次递增
- 右边车道: id沿t方向一次递增(绝对值依次递减)
车道宽度: 除了中心车道没有宽度, 其余车道的宽度由两个元素决定, 一个是<width>, 另一个是<border>。如果有<width>元素, 则车道的宽度由<width>决定,如果没有<width>元素, 则车道的宽度由<border>决定。简单一句话就是<width>优先级高于<border>。
车道联接: 车道的联接顾名思义就是车道的前驱和后继, 类似于道路的联接, 不同的是, 车道的联接只有前驱或者后继车道的id。

#2.3 laneOffset

偏移。车道偏移指的是中心车道相对参考线的偏移, 通常情况下, 中心车道与参考线是重合的, 这时就没有车道偏移(偏移为0)。但是, 如果出现高速匝道, 就会出现车道偏移。

#2.4 junction

路口。有两条以上的车道聚集形成路口, 路口分为三类: 常规路口、虚拟路口和直连路口。

常规路口:
虚拟路口: 该类型路口多用于小区门口、停车场出入口等类型的路口; 最大的特点就是不会破坏主路的结构(没有对主路切割)。
直连路口: 该类型路口多用于高速匝道等路口, 相对于常规路口, 该类型路口减少车道的数量, 方便构建, 使用也相对简单。

#2.4.1 路口的联接

路口的联接描述的是路口内的车道与飞路口车道之间的topo关系, 在构建topo关系时, 以下几个属性必不可少:

来路(incomingRoad): 路口道路不能为来路, 即, incomingRoad表示的是被路口联接的道路。
联接路(connectingRoad): 路口道路
来路车道(from): 来路车道
联接路车道(to): 联接路车道
联接点类型(contactPoint): junction参考线的起点或终点类型, 注意表示的是junction road
- start: 道路(road)连接到路口道路(junction road)的起点, topo关系: from->to
- end: 道路(road)连接到路口道路(junction road)的终点, topo关系: to->from

<!-- junction virtual -->
<road name="ConnectingRoad2" length="20" id="2" junction="555">
    <link>
        <predecessor elementType="road" elementId="1" elementS="50.0" elementDir="+"/>
        <successor elementType="road" elementId="99" contactPoint="end"/>
    </link>
    <laneSection s="0.0000000000000000e+00">
        <left/>
        <center/>
        <right>
            <lane id="-1" type="driving" level="false">
                <link>
                    <predecessor id="-2"/>
                    <successor id="1"/>
                </link>
            </lane>
        </right>
    </laneSection>
</road>
<road name="ConnectingRoad4" length="23" id="4" junction="555">
    <link>
        <predecessor elementType="road" elementId="99" contactPoint="end"/>
        <successor elementType="road" elementId="1" elementS="70.0" elementDir="+"/>
    </link>
    <laneSection s="0.0000000000000000e+00">
        <left/>
        <center/>
        <right>
            <lane id="-1" type="driving" level="false">
                <link>
                    <predecessor id="-1"/>
                    <successor id="-1"/>
                </link>
            </lane>
        </right>
    </laneSection>
</road>
<road name="ConnectingRoad5" length="20" id="5" junction="555">
    <link>
        <predecessor elementType="road" elementId="99" contactPoint="end"/>
        <successor elementType="road" elementId="1" elementS="70.0" elementDir="+"/>
    </link>
    <laneSection s="0.0000000000000000e+00">
        <left/>
        <center/>
        <right>
            <lane id="-1" type="driving" level="false">
                <link>
                    <predecessor id="-1"/>
                    <successor id="-2"/>
                </link>
            </lane>
        </right>
    </laneSection>
</road>
...
<junction name="myJunction" type="virtual" id="555" mainRoad="1" sStart="50" sEnd="70" orientation="+">
    <connection id="0" incomingRoad="1" connectingRoad="2" contactPoint="start">
        <laneLink from="-2" to="-1"/>
    </connection>
    <connection id="1" incomingRoad="99" connectingRoad="4" contactPoint="start">
        <laneLink from="-1" to="-1"/>
    </connection>
    <connection id="2" incomingRoad="99" connectingRoad="5" contactPoint="start">
        <laneLink from="-1" to="-1"/>
    </connection>
</junction>

#2.5 poly3(三次多项式)

三次多项式不是地图元素, 但是在OpenDRIVE中, 多个元素使用了三次多项式来描述。例如: 参考线、车道宽度、车道偏移等。

三次多项式方程:

y = a + b*x + c*x*x + d*x*x*x

在计算车道宽度时, 在s-t坐标系中, 可以计算出在s处的车道宽度, 这样我们就可以通过<width>元素中的已知的abcd求出在s处的车道宽度。

#3 解析

说完OpenDRIVE的基本结构, 接下来就是要对OpenDRIVE文件解析。OpenDRIVE文件采用的是xml格式, 文件后缀通常是.xodr。不同的语言各自有自己的解析库, 这里使用的是C++的tinyxml2解析库。以下列出几个常用的tinyxml2接口:

加载xml文件:

#include <tinyxml2.h>
std::string file_path = "";
tinyxml2::XMLDocument xml_doc;
xml_doc.LoadFile(file_path.c_str());
if (xml_doc.Error()) {
  // parse xml file fault.
}

获取xml根节点:

const tinyxml2::XMLElement* xml_root = xml_doc.RootElement();

获取元素节点:

// 获取road节点
const tinyxml2::XMLElement* xml_road = xml_root->FirstChildElement("road");

获取兄弟节点:

const tinyxml2::XMLElement* xml_road_2 = xml_road->NextSiblingElement("root");

获取节点属性:

// 1. 返回查询结果
const char* road_name = xml_road->Attribute("name");

// 2. 返回查询状态和结果
int road_id;
tinyxml2::XMLError status = xml_road->QueryIntAttribute("id", &road_id);

#3.1 数据结构

这里定义了两套数据结构, 一个用于存储原始的OpenDRIVE数据(element-struct), 另一个是存储处理后的数据(core-struct), 包括生成车道线的点、车道的前驱后继等。这里具体讲处理后的数据(code-struct)。

#3.1.1 ID

ID是每个具体事物的唯一标识: 道路ID、车道段ID、车道ID、路点ID

ID的规则
- 字符串类型std::string
- 通过ID可以确定它的上下关系
  - 道路ID: road_id
  - 车道段ID: section_id = road_id + “_” + section_index
  - 车道ID: lane_id = section_id + “_” + lane_id
  - 路点ID: point_id = lane_id + “_” + point_index

*_index: 表示该物体所在位置; 如: section_index = 2表示当前道路的第3个车道段。

e.g.

point_id:"100_2_-4_10"表示该点是id为100的第3个车道段的右边第4车道的第11个点。以此类推, 通过任何id都可以确定它所在的位置。

#3.1.2 Point

生成每条车道的具体点集([ [x1, y1], [x2, y2], [x3, y3], …])

车道除了可以使用OpenDRIVE中的<geometry>描述, 还可以使用具体的[x, y]集合表示。

Point:

typedef std::string Id;
struct Point {
  Id id; // point id
  double x = 0.;   // 惯性坐标系x
  double y = 0.;   // 惯性坐标系y
  double hdg = 0.; // 航向角(东北天ENU)
};

Lane: 每天车道有左边界、右边界和中线组成

#include <vector>
typedef std::vector<Point> Points;
struct Boundary {
  Points line;
};
struct Lane {
  Id id; // lane id
  Points central_curve;    // 中线
  Boundary left_boundary;  // 左边界
  Boundary right_boundary; // 右边界
};

生成车道线:
- 确定参考线(reference line)
- 确定中心车道(center lane)
- 计算车道宽度(lane width)
- 生成点(point)

参考线
参考线随着s(road s)的变化而变化, 可能同在一个Section内, 会有不一样的参考线。即, 参考线只与s相关, 这一点非常关键。
首先需要确定一个步进step(点距, 每进一步生成一个点)

// line
virtual Point GetPoint(double road_ds) const override {
  const double ref_line_ds = road_ds - s;
  const double xd = x + (cos_hdg * ref_line_ds);
  const double yd = y + (sin_hdg * ref_line_ds);
  return Point{.x = xd, .y = yd, .hdg = hdg};
}

// arc
virtual Point GetPoint(double road_ds) const override {
  const double ref_line_ds = road_ds - s;
  const double angle_at_s = ref_line_ds * curvature - M_PI / 2;
  const double xd = radius * (std::cos(hdg + angle_at_s) - sin_hdg) + x;
  const double yd = radius * (std::sin(hdg + angle_at_s) + cos_hdg) + y;
  const double tangent = hdg + ref_line_ds * curvature;
  return Point{.x = xd, .y = yd, .hdg = tangent};
}

// spiral
virtual Point GetPoint(double road_ds) const override {
  const double ref_line_ds = road_ds - s;
  const double s1 = curve_start / curve_dot + ref_line_ds;
  double x1;
  double y1;
  double t1;
  // odrSpiral: OpenDRIVEv1.7.0文档中提供的计算Spiral代码, 也可在Github中找到该接口仓库
  // https://github.com/DLR-TS/odrSpiral
  odrSpiral(s1, curve_dot, &x1, &y1, &t1);
  const double s0 = curve_start / curve_dot;
  double x0;
  double y0;
  double t0;
  odrSpiral(s1, curve_dot, &x0, &y0, &t0);

  x1 -= x0;
  y1 -= y0;
  t1 -= t0;
  const double angle = hdg - t0;
  const double cos_a = std::cos(angle);
  const double sin_a = std::sin(angle);
  const double xd = x + x1 * cos_a - y1 * sin_a;
  const double yd = y + y1 * cos_a + x1 * sin_a;
  const double tangent = hdg + t1;
  return Point{.x = xd, .y = yd, .hdg = tangent};
}

// poly3
virtual Point GetPoint(double road_ds) const override {
  const double ref_line_ds = road_ds - s;
  const double u = ref_line_ds;
  const double v = a + b * u + c * std::pow(u, 2) + d * std::pow(u, 3);
  const double x1 = u * cos_hdg - v * sin_hdg;
  const double y1 = u * sin_hdg + v * cos_hdg;
  const double tangent_v = b + 2.0 * c * u + 3.0 * d * std::pow(u, 2);
  const double theta = std::atan2(tangent_v, 1.0);
  const double xd = x + x1;
  const double yd = y + y1;
  const double tangent = hdg + theta;
  return Point{.x = xd, .y = yd, .hdg = tangent};
}

// parampoly3
virtual Point GetPoint(double road_ds) const override {
  const double ref_line_ds = road_ds - s;
  double p = ref_line_ds;
  if (PRange::NORMALIZED == p_range) {
    p = std::min(1.0, ref_line_ds / length);
  }
  const double u = au + bu * p + cu * std::pow(p, 2) + du * std::pow(p, 3);
  const double v = av + bv * p + cv * std::pow(p, 2) + dv * std::pow(p, 3);
  const double x1 = u * cos_hdg - v * sin_hdg;
  const double y1 = u * sin_hdg + v * cos_hdg;
  const double tangent_u = bu + 2 * cu * p + 3 * du * std::pow(p, 2);
  const double tangent_v = bv + 2 * cv * p + 3 * dv * std::pow(p, 2);
  const double theta = std::atan2(tangent_v, tangent_u);
  const double xd = x + x1;
  const double yd = y + y1;
  const double tangent = hdg + theta;
  return Point{.x = xd, .y = yd, .hdg = tangent};
}

中心车道
通过步进step和参考线公式我们可以计算出参考线上的具体xy坐标, 有了参考线后, 我们可以进一步计算中心车道, 计算中心车道时, 有一个非常重要的元素将会用到, 那就是<laneOffset>, 上面讲到, laneOffset描述的是参考线与中心车道偏离,即,中心车道偏离了参考线多少米。

首先我们通过<laneOffset>元素可以计算出在s位置中线的偏离值distance, 因为参考线的点是已经计算好了, 所以可以算出在s位置中心车道的具体点坐标

// point: 参考线点坐标
// lateral_offset: 中心车道偏离值
template <typename T>
static T GetOffsetPoint(const T& point, double lateral_offset) {
  const double x = -std::sin(point.hdg);
  const double y = std::cos(point.hdg);
  T offset_point = point;
  offset_point.x += lateral_offset * x;
  offset_point.y += lateral_offset * y;
  return offset_point;
}

车道宽度
确定了中心车道点坐标后, 我们可以通过车道的宽度, 算出左边界、右边界和中线的具体坐标值。已知A点的坐标和A与B的距离, 我们是可以计算出B点的坐标, 所以:

计算左边的车道:
- 左边第一车道:
  - 左边界与中心车道重合(中心车道的左右边界重合)
  - 中线与中心车道的距离为车道宽度的一半
  - 右边界与中心车道的距离为车道的宽度
- 左边第二车道:
  - 左边界与第一车道右边界重合
  - 中线与第一车道右边界的距离为车道宽度的一半
  - 右边界与第一车道右边界的距离为车道宽度
- 以此类推…即可计算出左边所有车道的坐标点
计算右边车道:
- 原理同上