文章目录
OpenDRIVE Notes
Github: https://github.com/minhanghuang/opendrive-engine
#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
- start:
<!-- 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点的坐标, 所以:
- 计算左边的车道:
- 左边第一车道:
- 左边界与中心车道重合(中心车道的左右边界重合)
- 中线与中心车道的距离为车道宽度的一半
- 右边界与中心车道的距离为车道的宽度
- 左边第二车道:
- 左边界与第一车道右边界重合
- 中线与第一车道右边界的距离为车道宽度的一半
- 右边界与第一车道右边界的距离为车道宽度
- 以此类推…即可计算出左边所有车道的坐标点
- 左边第一车道:
- 计算右边车道:
- 原理同上
至此, 所有车道的点坐标都可以计算出来了。
290
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
