CN112731925B - 用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法 - Google Patents
用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法 Download PDFInfo
- Publication number
- CN112731925B CN112731925B CN202011517192.5A CN202011517192A CN112731925B CN 112731925 B CN112731925 B CN 112731925B CN 202011517192 A CN202011517192 A CN 202011517192A CN 112731925 B CN112731925 B CN 112731925B
- Authority
- CN
- China
- Prior art keywords
- speed
- cone
- prediction
- control
- constraint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 40
- 230000000007 visual effect Effects 0.000 claims abstract description 11
- 238000005070 sampling Methods 0.000 claims description 35
- 230000001133 acceleration Effects 0.000 claims description 28
- 238000001514 detection method Methods 0.000 claims description 23
- 230000008859 change Effects 0.000 claims description 19
- 238000004891 communication Methods 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 11
- 238000000605 extraction Methods 0.000 claims description 10
- 230000009466 transformation Effects 0.000 claims description 7
- 208000037170 Delayed Emergence from Anesthesia Diseases 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000003287 optical effect Effects 0.000 claims description 6
- 238000012549 training Methods 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 4
- 238000005452 bending Methods 0.000 claims description 3
- 230000005540 biological transmission Effects 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000011161 development Methods 0.000 abstract description 8
- 230000006870 function Effects 0.000 description 35
- 238000010586 diagram Methods 0.000 description 12
- 238000003384 imaging method Methods 0.000 description 5
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000002372 labelling Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000010606 normalization Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000004913 activation Effects 0.000 description 2
- 239000003086 colorant Substances 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000002441 reversible effect Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 101100326853 Pectobacterium carotovorum subsp. carotovorum carI gene Proteins 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000005352 clarification Methods 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 230000036461 convulsion Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000006866 deterioration Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011176 pooling Methods 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 238000013139 quantization Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000000087 stabilizing effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 238000004804 winding Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
Abstract
本发明公开了一种用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，利用双目摄像头获取图像并传输给工控机，工控机利用视觉识别算法对图像中的锥桶进行识别；再根据锥桶在图像上的位置进行双目视差测距，获得锥桶在真实坐标系中的坐标位置，选取已知颜色和坐标位置的锥桶作为关键节点，建立拓扑地图，生成一个行驶的参考轨迹；然后根据参考轨迹采用纯跟踪算法获取赛车期望航向角，根据参考轨迹采用二次规划的速度规划算法获得期望车速；最后横向控制器控制前轮转向；纵向控制器控制驱动轮驱动或制动。本发明可以有效应用于本项赛事的锥桶识别、路径规划和赛车控制，并可拓展应用至其他相似场景，提高了汽车无人驾驶的开发效率。
Description
技术领域
本发明涉及汽车驾驶技术领域，特别是用于无人驾驶方程式赛车 锥桶识别和路径规划及控制方法。
背景技术
中国大学生无人驾驶方程式大赛(英文检测：FSAC)是一项由高 等院校汽车工程或汽车相关专业在校学生组队参加的无人驾驶赛车 设计与制造比赛。此赛事被誉为“汽车工程师的摇篮”。在这项赛事 中，各个无人驾驶赛车队普遍采用了多线激光雷达作为无人驾驶环境 感知系统的重要传感器。
在该项赛事中，参赛车队的无人驾驶方程式赛车需完成直线加速 项目、8字绕环项目、高速循迹项目等动态赛项目。如图1所示，不 同赛道均由固定尺寸(20*20*30cm)的锥桶按照不同的赛道形状进行 标记。按照赛事规则要求，在赛车进行动态赛之前，不允许对赛道进 行勘测建图，即无人驾驶方程式赛车无法事先获取所要完成的赛道地 图。因此，锥桶是无人驾驶系统对赛道进行有效识别的重要标识，需 要充分利用车载传感器对赛道边界、可行驶区域进行实时检测。为了 使赛车能尽可能快的完成相应比赛项目，其必须要规划出车辆当前位 置后2s的路径，然而当车速在30-50km/h时，仅仅依靠单个传感器 (如激光雷达、相机等)是无法实现这一要求的。
发明内容
本发明的目的在于，提供一种用于无人驾驶方程式赛车锥桶识别 和路径规划及控制方法。本发明可以有效应用于本项赛事的锥桶识别、 路径规划和赛车控制，并可拓展应用至其他相似场景，提高了汽车无 人驾驶技术的开发效率。
本发明的技术方案：用于无人驾驶方程式赛车锥桶识别和路径规 划及控制方法，包括赛车，赛车中设有工控机，横向控制器和纵向控 制器；所述工控机连接有双目摄像头，按以下步骤进行：
S1、双目摄像头拍取赛车行驶前方的图像并传输给工控机，工控 机利用视觉识别算法对图像中的锥桶进行识别；
S2、根据锥桶在图像上的位置进行双目视差测距，获得锥桶在真 实坐标系中的坐标位置，选取已知颜色和坐标位置的左右侧锥桶作为 关键节点，建立拓扑地图，生成一个行驶的参考轨迹；
S3、根据参考轨迹采用纯跟踪算法获取车辆期望航向角；根据参 考轨迹采用二次规划的速度规划算法获得期望车速；
S4、横向控制器采用航向预估算法，根据赛车期望航向角和赛车 的航向反馈量计算横向控制量，即转向电机转角信号，通过CAN通讯 将电机转角信号传递给转向电机驱动器，由转向电机驱动器驱动转向 电机作用于转向轴，控制赛车前轮转向；
纵向控制器根据期望车速和实际车速，计算驱动控制量和制动控 制量，即驱动电机转矩信号和制动舵机转角信号；驱动控制量通过 CAN通讯传递转矩信号给电机驱动器，由电机驱动器来驱动电机作用 于赛车的驱动轮进行驱动；制动控制量通过CAN通讯传递转角信号给 制动舵机驱动器，由制动舵机驱动器来驱动制动舵机作用于制动踏板 操纵液压制动系统进行制动。
上述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方 法，步骤S1中，所述的视觉识别算法的结构如下：输入图像的尺寸 大小为608像素×608像素，经过CSPDarknet53主干特征提取网络 进行特征提取，输出大小分别为76×76、38×38、19×19的特征层， 分别负责检测小中大三种目标；其中19×19的特征层经过SSP模块 后再与76×76和38×38的特征层采用PANet模块的网络进行张量拼 接，然后再经过Prediction模块输出三种尺度目标的检测结果；
在CSPDarknet53主干特征提取网络的CSP×2与CSP×8两个残 差块之间增加一个CSP×4的残差块，利用这一个4倍降采样的特征 融合目标检测层对小目标进行训练，提高对小目标检测的准确率。
前述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方 法，所述的Prediciton模块输出3个不同尺度的预测边界框，每个 预测边界框包含x，y，w，h，confidence五个基本参数，其中x，y为预测边界 框边界的中心点坐标；w，h分别为预测边界框的宽和高， confidence为预测目标的置信度；所述置信度根据loss损失函数计 算所得，损失函数L由四个部分组成：
L＝Lxy+Lwh+Lconf+Lcls；
式中：Lxy为预测边界框定位损失函数，Lwh为预测边界框尺寸 损失函数，Lconf为预测边界框置信度损失函数，Lcls为预测边界框 类别损失函数；
所述预测边界框定位损失函数Lxy的表达式为：
式中：s2为输入的图像被分成S×S个网格图像；B为单个网格 预测边界框个数，取值为3；表示当第1个网格预测的第j个预测 边界框检测到某个目标时取值为1，否则为0；xi为预测边界框中心 点横坐标；yi为预测边界框中心点纵坐标；/>为真实边界框中心点横坐标；/>为真实边界框中心点纵坐标；
所述预测边界框尺寸损失函数Lwh的表达式为：
式中：wi为预测边界框宽度；hi为预测边界框高度；为真 实边界框宽度；/>为真实边界框高度；
所述预测边界框置信度损失函数Lconf的表达式为：
式中：λnbj为权重系数，取值为1；λnobj为权重系数，取值为 100；表示当第i个网格预测的第j个预测边界框检测到某个目 标时取值为0，否则为1；Ci为预测目标的置信度；为真实目标的 置信度；
所述预测边界框类别损失函数Lcls的表达式为：
式中：c为检测到的目标所属类别；为第i个网格检测到 某个目标时，该目标属于类别c的真实概率；pi(c)为第i个网格检 测到某个目标时，该目标属于类别c的预测概率；
经过上述步骤后若置信度大于90％，即识别出锥桶。
前述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方 法，在识别锥桶时，相近图像中的锥桶具有相似性，利用锥桶的预测 边界框进行判断，如果该张图像的预测边界框与上一张图像的预测边 界框的标签一样，且重叠区域超过一定的阈值，便寻找两个预测边界 框的最大外接矩形，让最大外接矩形作为下一张图像的检测候选区域， 下一张图像只需在候选区域附近搜索，而不需要全图搜索，极大地提 高检测速度。
前述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方 法，步骤S2中获得锥桶在真实坐标系中的坐标位置计算过程为：
令锥桶在双目摄像头的左视图中的二维坐标为(x，y)，在双目 摄像头的左视图与右视图形成的视差为d，锥桶在以双目摄像头的 左摄像头光心为原点的空间坐标系中的坐标为(X，Y，Z)，通过变换 矩阵Q：
求解得到：
其中：f为焦距；B为双目摄像头中心距；cx为左视图中坐标 系原点与空间坐标系中原点的横向偏移值；cy为左视图中坐标系原 点与空间坐标系中原点的纵向偏移值；c′x为右视图中坐标系与原点 与空间坐标系中原点的横向偏移值。
前述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方 法，步骤S3中纯跟踪算法为从车辆二自由度模型开始，以车后轴为 切点，车辆纵向车身为切线，通过控制前轮转角，使车辆沿着一条经 过参考轨迹上目标点的圆弧行驶，
车辆期望航向角为：
其中，δ(t)为当前时刻的前轮转角，a(t)为当前车身姿态和目 标点连线的夹角，v1(t)为当前时刻的纵向车速；k1为变换比例系 数。
前述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方 法，基于二次规划的速度规划算法获得期望车速过程如下：
基于无人驾驶方程式赛车的当前位置和行驶轨迹建立路径-时间 坐标系；采用五次样条曲线作为速度生成方法,单次规划总时长为T， 时间步长为Δt,采样点个数m＝T/Δt；将速度曲线按照市场均 分为K条子曲线，K＝m/n，n＝1，2...，即：
式中，ui，k为曲线系数；
令：
Mk＝[u0，k，u1，k，u2，k，u3，k，u4，k，u5，k]T，k∈[1，K]；
解析式1中的各未知参数求得规划时长内的速度曲线，具体包括 构建目标函数、构建约束条件和求解最优速度：
构建目标函数：
C1(t)为速度、加速度和加速度变化率的目标函数，使速度平滑，加 速度和加速度变化率适当；
C2(t)为速度与赛车 最大行驶速度差值的目标函数，使赛车尽可能的提高车速；
式中：ε1为速度权重；ε2为加速度权重；ε3为加速度变化率权 重；Vt为最大行驶速度；为第k条曲线的速度；/>为第k条 曲线的加速度；/>为第k条曲线的加速度变化率；
总目标函数表达式为：
式中：Nk＝[σ0，k，σ1，k，σ2，k，σ3，k，σ3，k，σ4，k，σ5，k]为随子曲 线最大行驶速度变化而变化的行向量；令te＝T/(ΔtK)，则H1， H2，H3分别为：
构建约束条件，包括起点约束、单调约束、平滑约束、障碍约束 和速度约束：
所述起点约束中，令车辆的当前速度、加速度分别为v0、a0, 当前状态满足规划结果，即：
所述单调约束中，车辆单向向前行驶，路径-时间坐标系中的时 间值单调递增，时间步长Δt的两个采样点(s0，t0)和(s1，t1)满足约 束：
a.若两采样点位于同一子曲线k上，则：
b.若两采样点位于两条不同的子曲线k和l上，则：
所述平滑约束中，同时满足两条子曲线的采样点，令两条子曲线 为k和k+1，则：
所述障碍约束中，基于障碍与道路宽度获取采样点的S之上、 下边界分别为和/>则：
所述速度约束中，根据行驶轨迹的弯曲程度自主限速为Vka， 最大行驶速度Vt≤Vka；采样点的最高和最低速度分别为vuj和vdj，则：
vuj＜Tk，j·[u0，k，u1，k，u2，k，u3，k，u4，k，u5，k]T＜vdj；
求解最优速度：采用pqOASES库求解满足约束条件，即获得规划 总时长为T内的期望车速。
前述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方 法，步骤S4中，赛车期望航向角在一个控制周期内的变化量Δθ近 似为：
式中：Δθ(k)为当前采样时间的航向角预估变化量； v1(k-1)为上一采样时间的纵向车速，T为控制周期，即单 次规划总时长；δ1(k-1)为上一个采样时间的实际航向角；
当前航向角与航向角预估变化量之和作为航向反馈量，期望航向 角与航向反馈量的差值作为控制器的输入偏差。
前述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方 法，步骤S4中，纵向控制器包括驱动控制和制动控制；
所述驱动控制采用增量式PID控制算法输出控制量：
Δu＝ut(k)-ut(k-1)
＝kp[e(k)-e(k-1)]+kie(k)+kd[e(k)-2e(k-1)+e(k-2)] ；
其中，kp、ki、kd分别为比例、积分和微分系数；ut(k)表 示第k个采样时间的控制量；e(k)表示第k个采样时间的速度输 入偏差；
根据传动比、轮边电机每转一圈所需的驱动脉冲数确定比例系数 kdrive，将Δu乘上系数作为驱动控制量以电压信号通过CAN通讯 传递给电机驱动器，由电机驱动器来驱动电机作用于赛车的驱动轮进 行驱动；
所述制动控制采用模糊分挡式制动控制方法，用于模仿人工驾驶， 减速时使制动踏板转动平稳，提供无人驾驶汽车行驶平稳性。
前述的所述的用于无人驾驶方程式赛车锥桶识别和路径规划及 控制方法，当期望车速与实际车速之间差值大于阈值A时，选择驱动 控制，纵向控制器输出驱动控制量，制动控制量为0；当期望车速与 实际车速之间差值小于阈值B且制动控制器输出的控制量fufilte(k) 小于0时，选择制动控制，纵向控制器输出制动控制量，驱动控制量 为0；若期望车速与实际车速之间插值在阈值B和阈值A区间内时， 既不进行驱动控制也不进行制动控制，即纵向控制器既不输出驱动控 制量也不输出制动控制量。
与现有技术相比，本发明利用双目摄像头拍取赛车行驶前方的图 像并传输给工控机，工控机利用视觉识别算法对图像中的锥桶进行识 别；再根据锥桶在图像上的位置进行双目视差测距，获得锥桶在真实 坐标系中的坐标位置，选取已知颜色和坐标位置的锥桶作为关键节点， 建立拓扑地图，生成一个可行驶的参考轨迹；然后根据参考轨迹采用 纯跟踪算法获取赛车期望航向角，根据参考轨迹采用二次规划的速度 规划算法获得期望车速；最后横向控制器采用航向预估算法，根据赛 车期望航向角和赛车的航向反馈量计算横向控制量，即转向电机转角 信号，通过CAN通讯将转角信号传递给转向电机驱动器，由转向电机 驱动器驱动转向电机作用于转向轴，控制赛车前轮转向；纵向控制器 根据期望车速和实际车速，计算驱动控制量和制动控制量，即驱动电 机转矩信号和制动舵机转角信号；驱动控制量通过CAN通讯传递转矩 信号给电机驱动器，由电机驱动器来驱动电机作用于赛车的驱动轮进 行驱动；制动控制量通过CAN通讯传递转角信号给制动舵机驱动器， 由制动舵机驱动器来驱动制动舵机作用于制动踏板操纵液压制动系 统进行制动。本发明可以有效应用于本项赛事的锥桶识别、路径规划 和赛车控制，并可拓展应用至其他相似场景，提高了汽车无人驾驶的 开发效率。本发明可以有效提高无人驾驶的仿真精度和开发效率，能够快速及时发现无人驾驶系统在软、硬件上的不足之处，为加快无人 驾驶的ECU产业化进程奠定了基础，适合小型乘用车、小型货车的无 人系统的开发需求，具有广阔的应用前景。
附图说明
图1是本发明的软硬件框架示意图；
图2是基于改进的YOLOv4算法网络结构图；
图3是CSPDarknet53模块结构示意图；
图4是图3中的基本组件结构示意图；
图5是SSP模块结构示意图；
图6是PANet模块结构示意图；
图7是Prediciton模块示意图；
图8是改进的YOLOv4算法训练流程框图；
图9是轨迹规划及决策流程图；
图10是双目测距原理示意图；
图11是纯跟踪算法示意图；
图12是路径-时间图；
图13是步骤S4的跟踪控制流程图。
具体实施方式
下面结合附图和实施例对本发明作进一步说明，但并不作为对本 发明限制的依据。
实施例：用于无人驾驶方程式赛车锥桶识别和路径规划及控制方 法，包括赛车，赛车中设有工控机，横向控制器和纵向控制器；所述 工控机连接有双目摄像头，如图1所示，按以下步骤进行：
S1、双目摄像头拍取赛车行驶前方的图像以JPEG的格式通过USB 传输给工控机，工控机利用视觉识别算法对图像中的锥桶进行识别；
步骤S1中，所述的视觉识别算法是基于改进的YOLOv4算法，结 构如图2所示，输入图像的尺寸大小为608像素×608像素，经过 CSPDarknet53主干特征提取网络进行特征提取，输出大小分别为76 ×76、38×38、19×19的特征层，分别负责检测小中大三种目标； 其中19×19的特征层经过SSP模块后再与76×76和38×38的特征 层采用PANet模块的网络进行张量拼接，加强网络对不同尺寸物体的 检测性能，然后再经过Prediction模块输出三种尺寸目标的检测结 果；
如图3所示，在CSPDarknet53主干特征提取网络的CSP×2与 CSP×8两个残差块之间增加一个CSP×4的残差块，利用这一个4倍 降采样的特征融合目标检测层对小目标进行训练，提高对小目标检测 的准确率。同时，该网络借鉴了CSPNet(Cross Stage PartialNetwork) 的结构，增强了卷积神经网络的学习能力，使得网络结构在保持检测 精度的同时减少计算量，降低计算瓶颈和内存成本。
图3中的基本组件如图4所示。视觉识别算法中最小的两个组件 为CBM和CBL，CBM组件由Conv(卷积)、BN(批量归一化)和Mish (激活函数)三部分组成，CBL组件由Conv(卷积)、BN(批量归一 化)和Leaky_Relu(激活函数)三部分组成。Res Unit组件，借鉴 Resnet网络中的残差结构，将输入特征经过两个CBM组件卷积后再 与输入特征张量相加输出，使特征网络构建的更深。CSP×n借鉴 CSPNet网络结构，将浅层的特征映射划分为两部分，一部分经过两 次CBM卷积和n次残差组件计算，另一部分在一次CBM卷积后直接与 残差模块的输出进行张量拼接后，再进行一次CBM卷积后输出。
SSP模块结构如图5所示，使用k＝{1×1，5×5，9×9，13×13} 的最大池化方式，再将不同尺度的特征图进行融合，更加有效地增加 了主干特征的接受范围(增加了网络的感受视野)，显著的分离了最 重要的上下文特征。
PANet模块结构如图6所示，采用19×19，38×38，76×76这三 个不同尺度的特征图分别进行大、中、小目标的检测。将低分辨率的 特征图上采样后与高分辨率的特征图进行拼接后，再进行下采样与原 低分辨率的特征图拼接，加强了网络对不同尺寸物体的检测性能。
如图7所示，所述的Prediciton模块输出3个不同尺度的预测 边界框，每个预测边界框包含x，y，w，h，confidence五个基本参数，其中 x，y分别为预测边界框的中心点坐标；w，h分别为预测边界框的宽和 高，confidence为预测目标的置信度，置信度越高，目标检测的准 确率越高；所述置信度根据loss损失函数计算所得，损失函数L由 四个部分组成：
L＝Lxy+Lwh+Lconf+Lcls；
式中：Lxy为预测边界框定位损失函数，Lwh为预测边界框尺寸 损失函数，Lconf为预测边界框置信度损失函数，Lcls为预测边界框 类别损失函数；
所述预测边界框定位损失函数Lxy的表达式为：
式中：S2为输入的图像被分成S×S个网格图像；B为单个网格 预测边界框个数，取值为3；表示当第i个网格预测的第j个预测 边界框检测到某个目标时取值为1，否则为0；Xi为预测边界框中心 点横坐标；yi为预测边界框中心点纵坐标；/>为真实边界框中心点 横坐标；/>为真实边界框中心点纵坐标；
所述预测边界框尺寸损失函数Lwh的表达式为：
式中：wi为预测边界框宽度；hi为预测边界框高度；为真 实边界框宽度；/>为真实边界框高度；
所述预测边界框置信度损失函数Lconf的表达式为：
式中：λobj为权重系数，取值为1；λnobj为权重系数，取值为 100；表示当第i个网格预测的第j个预测边界框检测到某个目 标时取值为0，否则为1；Ci为预测目标的置信度；为真实目标的 置信度；
所述预测边界框类别损失函数Lcls的表达式为：
式中：c为检测到的目标所属类别；为第i个网格检测到 某个目标时，该目标属于类别c的实际概率；pi(c)为第i个网格检测 到某个目标时，该目标属于类别c的预测概率；
经过上述步骤后若置信度大于90％，即识别出锥桶。
在得出视觉识别算法，如图8步骤所示，进行数据集制作以及对 算法进行训练。
数据集制作：在实际赛道行驶状况中，由于天气条件，光强和光 源位置投射角度的变化，会导致导摄像头不能准确识别锥桶颜色和位 置，存在错检、漏检的可能。
因此在不同天气条件下，利用MER-231-41U3C数字摄像头获取锥 桶图像数据，其二维RGB图像分辨率为1920像素×1200像素。采集 图像数据时，模拟车辆行驶的角度以及距离锥桶的距离。经过人工筛 选，最终获得1340幅图像，其中包含红色、蓝色和黄色等锥桶，包 含顺光，逆光等不同光照情况，还包含晴天、雨天和阴天等不同天气 环境。然后使用标注工具labelImg对锥桶图像进行标注，标注时将 锥桶的最小外接矩形框作为真实框。
在顺光或逆光条件下采集到的锥桶图像颜色差异很大。此外，远 距离的锥桶目标国小，且重叠遮挡严重，导致锥桶特征提取困难。因 此，在锥桶图像训练时，对图像数据采用Mosaic数据增强技术和自 对抗训练的数据扩充技术，增加了数据集中的数据量，提高了模型的 泛化能力，提升模型的鲁棒性。同时，对标注数据进行交叉小批量归 一化处理，提升了模型的收敛速度和精度，防止梯度爆炸。
数据集训练：
用于图像处理及模型训练的工业控制计算机处理器为 CoreTM i7-6700T，主频为2.8GHz，显卡为NVIDIA GeForce GTX 2080Ti， 16GB内存，512G固态硬盘。所有程序均在Ubuntu18.04系统下运行， 图像显示处调用OpenCV3.4.9库、数据计算调用CUDA10.2、CuDNN7.6.5等。
输入图像尺寸为608×608，最大迭代次数20000，权值衰减速率 0.0005，初始学习率0.001，动量设置0.9。
在识别锥桶时，相近图像中的锥桶具有相似性，利用锥桶的预测 边界框进行判断，如果当前图像预测边界框与上一帧图像预测边界框 的类别标签一样，且重叠区域超过一定的阈值，便寻找两个预测边界 框的最大外接矩形，让最大外接矩形作为下一张图像的检测候选区域， 下一张图像只需要在候选区域附近搜索，而不需要全图搜索，可以极 大地提高检测速度。
判断两张图像的相似性，采用感知哈希算法。该算法给每张图像 生成一个“指纹”，两张“指纹”字符串汉明距离越小，说明两张图 像越相似。通过指纹的相似度，可对图像进行分类。当判断两张图像 属于同一类时，将第一张图向定位区域作为后一张图像的检测候选区 域，之在候选区域扫描，避免了全图大范围扫描，从而实现对检测算 法的加速。
S2、如图9所示，根据锥桶在图像上的位置进行双目视差测距， 获得锥桶在真实坐标系中的坐标位置，选取已知颜色和坐标位置的锥 桶作为关键节点，建立拓扑地图，生成一个行驶的参考轨迹；具体地：
双目视差是模拟人类视觉原理，使用计算机被动感知距离的方法。 从两点观察一个物体，在不同视角下的图像，根据图像之间像素的匹 配关系，通过三角测量原理计算出像素之间的便宜来获取物体的三维 信息。
双目摄像头由于光学透镜的特性使得成像存在着径向畸变，可由 三个参数k1,k2,k3确定；由于装配方面的误差，传感器与光学镜头 之间并非完全平行，因此成像存在切向畸变，可由两个参数p1,p2 确定。单个摄像头的定标主要是计算出摄像头的内参(焦距f和成像 原点cx,cy、五个畸变参数(一般只需要计算出k1,k2,p1,p2，对于 鱼眼镜头等径向畸变特别大的才需要计算k3))以及外参(标定物 的世界坐标)。双目摄像头定标不经要得出每个摄像头的内部参数， 还要测定两个摄像头之间的相对位置(即右摄像头相对于左摄像头的 旋转矩阵R，平移向量t)。
双目摄像头标定不仅要得出每个摄像头的内部参数，还需要通过 标定来测量两个摄像头之间的相对位置。要计算目标点在左右两个视 图上形成的视差，首先要把该点在左右视图上两个对应的像点匹配起 来。利用极线约束使得对应点的匹配由二维搜索将为一维搜索，减少 匹配搜索范围。双目校正的作用就是要把消除畸变后的两幅图像严格 地进行对应，使得两幅图像的对极线正好在同一水平线，这样一幅图 像上的任意一点与其在另一幅图像上的对应点必然由有相同的行号， 只需在该行进行一维搜索即可匹配到对应点。
摄像头标定：在摄像头前的标定参照物(棋盘纸)，摄像头获取 该物体的图像，并由此计算摄像头的内外参数。标定参照物上的每一 个特征点相对于世界坐标系的位置在制作时应精确测定，世界坐标系 可选为参照物的物体坐标系。在得到这些已知点在图像上的投影位置， 可计算出摄像头的内外参数。
双目视差测距的原理如图10所示，P是待测物体上的某一点， OR与OT分别是两个相机的光心，点P在两个相机传感器上的成像 点分别为P′和P″(相机的成像平面经过旋转后放在了镜头前方)， f为相机焦距，B为两个相机的中心距，乙为所求的深度信息，设 点P′到P″的距离为Δx，则：Δx＝B-(XR-XT)；
根据相似三角形原理：
可得：
公式中，焦距f和中心距B可通过标定得到，因此，获得了 XR-XT(即视差d)的值即可求得深度信息，即：
因此，步骤S2中获得锥桶在真实世界中的坐标位置计算过程为：
令锥桶在双目摄像头的左视图中的二维坐标为(X，y)，在双目 摄像头的左视图与右视图形成的视差为d，锥桶在以双目摄像头的 左摄像头光心为原点的空间坐标系中的坐标为(X，Y，Z)，通过变换 矩阵Q：
求解得到：
其中：f为焦距；B为双目摄像头中心距；cx为左视图中坐标 系原点与空间坐标系中原点的横向偏移值；cy为左视图中坐标系原 点与空间坐标系中原点的纵向偏移值；c′x为右视图中坐标系与原点 与空间坐标系中原点的横向偏移值。本实施例中摄像头分辨率1920 (H)×1200(V)，帧率41FPS，焦距9mm。
根据识别到的锥桶位置，选择左右颜色不同的两个锥桶的中点作 为关键节点，建立拓扑地图(环境地图)，生成一个可行驶的参考轨 迹。
S3、根据参考轨迹采用纯跟踪算法获取车辆期望航向角；
如图11所示，纯跟踪算法为从车辆二自由度模型开始，以车后 轴为切点，车辆纵向车身为切线，通过控制前轮转角，使车辆沿着一 条经过参考轨迹上目标点(goal point)的圆弧行驶，
车辆期望航向角为：
其中，δ(t)为当前时刻的前轮转角，a(t)为当前车身姿态和目 标点连线的夹角，v1(t)为当前时刻的纵向车速；k1为变换比例系数。
根据参考轨迹采用二次规划的速度规划算法获得期望车速，过程 如下：
基于无人驾驶方程式赛车的当前位置和行驶轨迹建立如图12所 示的路径-时间坐标系；采用五次样条曲线作为速度生成方法，单次规 划总时长为T，时间步长为Δt，采样点个数m＝T/Δt；将速度 曲线按照市场均分为K条子曲线，K＝m/n，n＝1，2...，即：
式中，ui，k为曲线系数；
令：
Mk＝[u0，k，u1，k，u2，k，u3，k，u4，k，u5，k]T，k∈[1，K]；
解析式1中的各未知参数求得规划时长内的速度曲线，具体包括 构建目标函数、构建约束条件和求解最优速度：
构建目标函数：
C1(t)为速度、加速度和加速度变化率的目标函数，使速度平滑，加 速度和加速度变化率适当；
C2(t)为速度与赛车最大行驶速度差值的目标函数，使赛车尽可能的 提高车速；
式中：ε1为速度权重；ε2为加速度权重；ε3为加速度变化率权 重；Vt为最大行驶速度；为第k条曲线的速度；/>为第k条 曲线的加速度；/>为第k条曲线的加速度变化率。
总目标函数表达式为：
式中：为随子曲 线最大行驶速度变化而变化的行向量；令te＝T/(ΔtK)，则H1， H2，H3分别为：/>
构建约束条件，包括起点约束、单调约束、平滑约束、障碍约束 和速度约束：
所述起点约束中，令车辆的当前速度、加速度分别为v0、a0, 当前状态满足规划结果，即：
所述单调约束中，车辆单向向前行驶，路径-时间坐标系中的时 间值单调递增，时间步长Δt的两个采样点(s0，t0)和(s1，t1)满足约 束：
a.若两采样点位于同一子曲线k上，则：
b.若两采样点位于两条不同的子曲线k和l上，则：
所述平滑约束中，同时满足两条子曲线的采样点，令两条子曲线 为k和k+1，则：
所述障碍约束中，基于障碍与道路宽度获取采样点的S之上、 下边界分别为和/>则：
所述速度约束中，根据行驶轨迹的弯曲程度自主限速为Vka， 最大行驶速度Vt≤Vka；采样点的最高和最低速度分别为vuj和vdj，则：
vuj＜Tk，j·[u0，k，u1，k，u2，k，u3，k，u4，k，u5，k]T＜vdj；
求解最优速度:采用pqOASES库求解满足约束条件，即获得规划 总时长为T内的期望车速。函数库qpOASES是一个开源的C++函数库， 用于处理二次规划最优解。
S4、如图13所示，横向控制器采用航向预估算法，根据赛车期 望航向角和赛车的航向反馈量计算横向控制量，即转向电机转角信号， 通过CAN通讯将转角信号传递给转向电机驱动器，由转向电机驱动器 驱动转向电机作用于转向轴，控制赛车前轮转向；
纵向控制器根据期望车速和实际车速，计算驱动控制量和制动控 制量，即驱动电机转矩信号和制动舵机转角信号；驱动控制量通过 CAN通讯传递转矩信号给电机驱动器，由电机驱动器来驱动电机作用 于赛车的驱动轮进行驱动；制动控制量通过CAN通讯传递转角信号给 制动舵机驱动器，由制动舵机驱动器来驱动制动舵机作用于制动踏板 操纵液压制动系统进行制动。
具体的，横向控制器控制前轮转向过程中，车辆期望航向角在一 个控制周期内的变化量Δθ近似为：
式中：Δθ(k)为当前采样时间的航向角预估变化量； v1(k-1)为上一采样时间的纵向车速，T为控制周期； δ1(k-1)为上一个采样时间的实际航向角；
当前航向角与航向角预估变化量之和作为航向反馈量，期望航向 角与航向反馈量的差值作为控制器的输入偏差。
纵向控制器包括驱动控制和制动控制；
所述驱动控制采用增量PID控制算法输出控制量：
Δu＝ut(k)-ut(k-1)
＝kp[e(k)-e(k-1)]+kie(k)+kd[e(k)-2e(k-1)+e(k-2)] ；
其中，kp、ki、kd分别为比例、积分和微分系数；ut(k)表 示第k个采样时间的控制量；e(k)表示第k个采样时间的速度输 入偏差；
根据传动比、轮边电机每转一圈所需的驱动脉冲数确定比例系数 kdrive，将Δu乘上系数作为驱动控制量以电压信号通过CAN通讯 传递给电机驱动器，由电机驱动器来驱动电机作用于赛车的驱动轮进 行驱动；
所述制动控制采用模糊分挡式制动控制方法，用于模仿人工驾驶， 减速时使制动踏板踏板平稳，提供无人驾驶汽车行驶平稳性。
模糊分档，模糊控制器的查表输出量不直接用于驱动制动踏板， 而是将控制量按一定规则分为有限的u1，u2，…，uN(N≥1)行 程挡。为了减少制动踏板的频繁动作，分挡应尽量少。当制动踏板达 到某一行程后，稳定时间就可适当延长，从而保证了驾驶的平稳性。
本案例分为13个级别，模糊制动框图如图13所示，速度偏差e(k) 和一个控制周期内的速度偏差变化量ec(k)(即e(k)-ec(k)， 可代表无人驾驶汽车加速度)分别乘以速度偏差变换比例系数kfe和 速度偏差变换量比例系数kfec后再进行均匀模糊化，得到fe(k)和fec(k)。其量化过程见表，论域为[-6，6]。
表1
然后利用fe(k)和fec(k)查模糊控制规则表得到模糊控制表输出 量fu(k)。fu(k)经过滤波后得到fufilter(k)(fufilter(k)同时也是进 行油门控制与制动控制切换的判据)，通过分挡规则调整得到分挡控 制输出fsu(k)，最后乘以实际控制量比例因子kfu，转换成实际控制 量ub(k)。
模糊控制规则表可以通过离线模糊计算获得。设速度偏差e(k)和 速度偏差变化量ec(k)经过模糊量化后，输入变量fe(k)和fec(k)及 模糊控制表输出量fu(k)的论域分别为为FE,FEC和FU，模糊语言变 量值集合分别为T(fe),T(fec)和T(fu),模糊语言变量值有NB(负大)， NM(负中)，NS(负小)，ZE(零)，PS(正小)，PM(负中)和 PB(负大)，且有：
FE，FEC，FU∈{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；
T(fe)，T(fec)＝{NB，NM，NS，ZE，PS，PM，PB}；
T(fu)＝{NM，NS，ZE，PS，PM}；
各模糊语言变量值的隶属度函数见表2。
| -6 | -5 | -4 | -3 | -2 | -1 | 0 | 1 | 2 | 3 | 4 | 5 | 6 | |
| PB | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0.1 | 0.4 | 0.8 | 1 |
| PM | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0.2 | 0.7 | 1 | 0.7 | 0.2 |
| PS | 0 | 0 | 0 | 0 | 0 | 0.1 | 0.4 | 0.8 | 1 | 0.8 | 0.4 | 0.1 | 0 |
| ZE | 0 | 0 | 0 | 0.1 | 0.4 | 0.8 | 1 | 0.8 | 0.4 | 0.1 | 0 | 0 | 0 |
| NS | 0 | 0.1 | 0.4 | 0.8 | 1 | 0.8 | 0.4 | 0.1 | 0 | 0 | 0 | 0 | 0 |
| NM | 0.2 | 0.7 | 1 | 0.7 | 0.2 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
| NB | 1 | 0.8 | 0.4 | 0.1 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
表2
根据人工驾驶制动经验和多次制动控制实验结果，制定了如表3 所示的基于模糊控制的制动控制规则。表3中NM和NS表示制动，ZE 表示不动作，而PS和PM(表中用括号表示)则没有没有实际意义。 制定PS和PM控制输出量只是为了作为油门控制和制动控制的切换依 据，在制动控制规则时，考虑到油门踏板完全松开时，无人驾驶方程 式赛车也可以依靠行驶阻力减速，故这时不需要进行制动控制。
表3
下面采用PRODUCT-SUM-GRAVITY方法进行模糊推理和清晰化，也 称中位数法，或者LARSEN积运算。表4是利用PRODUCT-SUM-GRAVITY 方法进行模糊推理及清晰化计算得到的模糊控制表。
表4
经过滤波后的控制量fuflilter(k)，仍存在抖动现象。为保证 制动时的平稳性，可以将滤波后的控制量fuflilter(k)按一定规则 分挡，形成台阶式输出，从而使制动踏板在一定时间内位置固定。分 挡间隔大于模糊控制输出变量论域元素之间的最大间隔，模糊论域为 [-6,6]，元素间隔为1，小于0的元素为6个，因此分为3挡，规则 如下：
1)、若0＞fuflilter(k)≥pctrl1，则fsu(k)＝0；
2)、若pctrl1＞fuflilter(k)≥pctrl2，则fsu(k)＝Step1；
3)、若pctrl2＞fuflilter(k)≥pctrl3，则fsu(k)＝step2；
4)、若pctrl3＞fuflilter(k)，则fsu(k)＝step3。
由于速度偏差变换比例系数kfe过小将引起较大的稳态误差，而 过大则会导致超调量变大；速度偏差变换量比例系数kfec过小将是系 统响应性能变差，收敛速度减慢，而过大将导致上升时间增加，稳态 误差变大，同时超调量会减小；实际控制量比例因子kfu过小将导致 上升时间增加，收敛速度加快，而过大则相反。
对无人驾驶汽车速度控制而言，设速度偏差最小值，最大值分别 分别为emin和emax，模糊控制器模糊变量论域为[elmin，elmax]， 采用线性变换，则有：
其中，
设速度偏差变换量最小值，最大值分别分别为ecmin和ecmax， 模糊控制器模糊变量论域为[eclmin，eclmax，采用线性变换，则有：
其中，
设实际控制量最小值，最大值分别分别为ubmin和ubmax， 模糊控制器模糊变量论域为[ublmin，ublmax]，采用线性变换，则 有：
其中，
驱动控制与制动控制的及时切换是减小速度跟踪误差并保证无 人驾驶汽车行驶平稳性的关键。切换规则利用驱动控制器输出控制量 ut和制动控制器输出滤波后的控制量fufilter(k)进行判断；同时 引入速度偏差e作为判据。切换规则如下(u表示选择的控制量)：
当速度偏差e大于阈值A时，选择驱动控制，纵向控制器输出驱 动控制量，制动控制量为0；当速度偏差e小于阈值B且制动控制器 输出的控制量fufilter(k)小于0时，选择制动控制，纵向控制器输出 制动控制量，驱动控制量为0；若速度偏差e在(阈值B，阈值A)区间内时，既不进行驱动控制也不进行制动控制，即纵向控制器既不输出 驱动控制量也不输出制动控制量。阈值A、阈值B根据实际情况调试， 阈值A初始值暂定0.5m/s，阈值B初始值暂定-0.5m/s。
综上，本发明可以有效应用于本项赛事的锥桶识别、路径规划和 赛车控制，并可拓展应用至其他相似场景，提高了汽车无人驾驶的开 发效率。本发明可以有效提高无人驾驶的仿真精度和开发效率，能够 快速及时发现无人驾驶系统在软、硬件上的不足之处，为加快无人驾 驶的ECU产业化进程奠定了基础，适合小型乘用车、小型货车的无人 系统的开发需求，具有广阔的应用前景。
Claims (10)
1.用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，包括赛车，赛车中设有工控机，横向控制器和纵向控制器；所述工控机连接有双目摄像头，其特征在于：按以下步骤进行：
S1、双目摄像头拍取赛车行驶前方的图像并传输给工控机，工控机利用视觉识别算法对图像中的锥桶进行识别；
S2、根据锥桶在图像上的位置进行双目视差测距，获得锥桶在真实坐标系中的坐标位置，选取已知颜色和坐标位置的左右侧锥桶作为关键节点，建立拓扑地图，生成一个行驶的参考轨迹；
S3、根据参考轨迹采用纯跟踪算法获取车辆期望航向角；根据参考轨迹采用二次规划的速度规划算法获得期望车速；
S4、横向控制器采用航向预估算法，根据赛车期望航向角和赛车的航向反馈量计算横向控制量，即转向电机转角信号，通过CAN通讯将电机转角信号传递给转向电机驱动器，由转向电机驱动器驱动转向电机作用于转向轴，控制赛车前轮转向；
纵向控制器根据期望车速和实际车速，计算驱动控制量和制动控制量，即驱动电机转矩信号和制动舵机转角信号；驱动控制量通过CAN通讯传递转矩信号给电机驱动器，由电机驱动器来驱动电机作用于赛车的驱动轮进行驱动；制动控制量通过CAN通讯传递转角信号给制动舵机驱动器，由制动舵机驱动器来驱动制动舵机作用于制动踏板操纵液压制动系统进行制动。
2.根据权利要求1所述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，其特征在于：步骤S1中，所述的视觉识别算法的结构如下：输入图像的尺寸大小为608像素×608像素，经过CSPDarknet53主干特征提取网络进行特征提取，输出大小分别为76×76、38×38、19×19的特征层，分别负责检测小中大三种目标；其中19×19的特征层经过SSP模块后再与76×76和38×38的特征层采用PANet模块的网络进行张量拼接，然后再经过Prediction模块输出三种尺度目标的检测结果；
在CSPDarknet53主干特征提取网络的CSP×2与CSP×8两个残差块之间增加一个CSP×4的残差块，利用这一个4倍降采样的特征融合目标检测层对小目标进行训练，提高对小目标检测的准确率。
3.根据权利要求2所述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，其特征在于：所述的Prediciton模块输出3个不同尺度的预测边界框，每个预测边界框包含x，y，w，h，confidence五个基本参数，其中x，y为预测边界框边界的中心点坐标；w，h分别为预测边界框的宽和高，confidence为预测目标的置信度；所述置信度根据loss损失函数计算所得，损失函数L由四个部分组成：
L＝Lxy+Lwh+Lconf+Lcls；
式中：Lxy为预测边界框定位损失函数，Lwh为预测边界框尺寸损失函数，Lconf为预测边界框置信度损失函数，Lcls为预测边界框类别损失函数；
所述预测边界框定位损失函数Lxy的表达式为：
式中：s2为输入的图像被分成S×S个网格图像；B为单个网格预测边界框个数，取值为3；表示当第i个网格预测的第j个预测边界框检测到某个目标时取值为1，否则为0；xi为预测边界框中心点横坐标；yi为预测边界框中心点纵坐标；/>为真实边界框中心点横坐标；/>为真实边界框中心点纵坐标；
所述预测边界框尺寸损失函数Lwh的表达式为：
式中：wi为预测边界框宽度；hi为预测边界框高度；为真实边界框宽度；/>为真实边界框高度；
所述预测边界框置信度损失函数Lconf的表达式为：
式中：λobj为权重系数，取值为1；λnobj为权重系数，取值为100；表示当第i个网格预测的第j个预测边界框检测到某个目标时取值为0，否则为1；Ci为预测目标的置信度；/>为真实目标的置信度；
所述预测边界框类别损失函数Lcls的表达式为：
式中：c为检测到的目标所属类别；为第i个网格检测到某个目标时，该目标属于类别c的真实概率；pi(c)为第1个网格检测到某个目标时，该目标属于类别c的预测概率；
经过上述步骤后若置信度大于90％，即识别出锥桶。
4.根据权利要求1所述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，其特征在于：在识别锥桶时，相近图像中的锥桶具有相似性，利用锥桶的预测边界框进行判断，如果该张图像的预测边界框与上一张图像的预测边界框的标签一样，且重叠区域超过一定的阈值，便寻找两个预测边界框的最大外接矩形，让最大外接矩形作为下一张图像的检测候选区域，下一张图像只需在候选区域附近搜索，而不需要全图搜索，极大地提高检测速度。
5.根据权利要求1所述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，其特征在于：步骤S2中获得锥桶在真实坐标系中的坐标位置计算过程为：
令锥桶在双目摄像头的左视图中的二维坐标为(x，y)，在双目摄像头的左视图与右视图形成的视差为d，锥桶在以双目摄像头的左摄像头光心为原点的空间坐标系中的坐标为(X，Y，Z)，通过变换矩阵Q：
求解得到：
其中：f为焦距；B为双目摄像头中心距；cx为左视图中坐标系原点与空间坐标系中原点的横向偏移值；cy为左视图中坐标系原点与空间坐标系中原点的纵向偏移值；c′x为右视图中坐标系与原点与空间坐标系中原点的横向偏移值。
6.根据权利要求1所述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，其特征在于：步骤S3中纯跟踪算法为从车辆二自由度模型开始，以车后轴为切点，车辆纵向车身为切线，通过控制前轮转角，使车辆沿着一条经过参考轨迹上目标点的圆弧行驶，
车辆期望航向角为：
其中，δ(t)为当前时刻的前轮转角，a(t)为当前车身姿态和目标点连线的夹角，v1(t)为当前时刻的纵向车速；k1为变换比例系数。
7.根据权利要求1所述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，其特征在于：基于二次规划的速度规划算法获得期望车速过程如下：
基于无人驾驶方程式赛车的当前位置和行驶轨迹建立路径-时间坐标系；采用五次样条曲线作为速度生成方法，单次规划总时长为T，时间步长为Δt，采样点个数m＝T/Δt；将速度曲线按照市场均分为K条子曲线，K＝m/n，n＝1，2...，即：
式中，ui，k为曲线系数；
令：
Mk＝[u0，k，u1，k，u2，k，u3，k，u4，k，u5，k]T，k∈[1，K]；
解析式1中的各未知参数求得规划时长内的速度曲线，具体包括构建目标函数、构建约束条件和求解最优速度：
构建目标函数：
C1(t)为速度、加速度和加速度变化率的目标函数，使速度平滑，加速度和加速度变化率适当；
C2(t)为速度与赛车最大行驶速度差值的目标函数，使赛车尽可能的提高车速；
式中：ε1为速度权重；ε2为加速度权重；ε3为加速度变化率权重；Vt为最大行驶速度；为第k条曲线的速度；/>为第k条曲线的加速度；/>为第k条曲线的加速度变化率；
总目标函数表达式为：
式中：Nk＝[σ0，k，σ1，k，σ2，k，σ3，k，σ4，k，σ5，k]为随子曲线最大行驶速度变化而变化的行向量；令te＝T/(ΔtK)，则H1，H2，H3分别为：
构建约束条件，包括起点约束、单调约束、平滑约束、障碍约束和速度约束：
所述起点约束中，令车辆的当前速度、加速度分别为v0、a0，当前状态满足规划结果，即：
所述单调约束中，车辆单向向前行驶，路径-时间坐标系中的时间值单调递增，时间步长Δt的两个采样点(s0，t0)和(s1，t1)满足约束：
a.若两采样点位于同一子曲线k上，则：
b.若两采样点位于两条不同的子曲线k和1上，则：
所述平滑约束中，同时满足两条子曲线的采样点，令两条子曲线为k和k+1，则：
所述障碍约束中，基于障碍与道路宽度获取采样点的S之上、下边界分别为和/>则：
所述速度约束中，根据行驶轨迹的弯曲程度自主限速为Vka，最大行驶速度Vt≤Vka；采样点的最高和最低速度分别为vuj和vdj，则：
vuj＜Tk，j·[u0，k，u1，k，u2，k，u3，k，u4，k，u5，k]T＜vdj；
求解最优速度：采用pqOASES库求解满足约束条件，即获得规划总时长为T内的期望车速。
8.根据权利要求1所述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，其特征在于：步骤S4中，赛车期望航向角在一个控制周期内的变化量Δθ近似为：
式中：Δθ(k)为当前采样时间的航向角预估变化量；v1(k-1)为上一采样时间的纵向车速，T为控制周期，即单次规划总时长；δ1(k-1)为上一个采样时间的实际航向角；
当前航向角与航向角预估变化量之和作为航向反馈量，期望航向角与航向反馈量的差值作为控制器的输入偏差。
9.根据权利要求1所述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，其特征在于：步骤S4中，纵向控制器包括驱动控制和制动控制；
所述驱动控制采用增量式PID控制算法输出控制量：
Δu＝ut(k)-ut(k-1)
＝kp[e(k)-e(k-1)]+kie(k)+kd[e(k)-2e(k-1)+e(k-2)]；
其中，kp、ki、kd分别为比例、积分和微分系数；ut(k)表示第k个采样时间的控制量；e(k)表示第k个采样时间的速度输入偏差；
根据传动比、轮边电机每转一圈所需的驱动脉冲数确定比例系数kdrive，将Δu乘上系数作为驱动控制量以电压信号通过CAN通讯传递给电机驱动器，由电机驱动器来驱动电机作用于赛车的驱动轮进行驱动；
所述制动控制采用模糊分挡式制动控制方法，用于模仿人工驾驶，减速时使制动踏板转动平稳，提供无人驾驶汽车行驶平稳性。
10.根据权利要求9所述的所述的用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法，其特征在于：当期望车速与实际车速之间差值大于阈值A时，选择驱动控制，纵向控制器输出驱动控制量，制动控制量为0；当期望车速与实际车速之间差值小于阈值B且制动控制器输出的控制量fufilter(k)小于0时，选择制动控制，纵向控制器输出制动控制量，驱动控制量为0；若期望车速与实际车速之间插值在阈值B和阈值A区间内时，既不进行驱动控制也不进行制动控制，即纵向控制器既不输出驱动控制量也不输出制动控制量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011517192.5A CN112731925B (zh) | 2020-12-21 | 2020-12-21 | 用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011517192.5A CN112731925B (zh) | 2020-12-21 | 2020-12-21 | 用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112731925A CN112731925A (zh) | 2021-04-30 |
| CN112731925B true CN112731925B (zh) | 2024-03-15 |
Family
ID=75603800
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011517192.5A Active CN112731925B (zh) | 2020-12-21 | 2020-12-21 | 用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112731925B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113467480B (zh) * | 2021-08-09 | 2024-02-13 | 广东工业大学 | 一种用于无人驾驶方程式的全局路径规划算法 |
| CN113587950B (zh) * | 2021-08-26 | 2024-04-09 | 清华大学 | 自动驾驶汽车静态路径规划方法、装置及存储介质 |
| CN113762137A (zh) * | 2021-09-02 | 2021-12-07 | 甘肃同兴智能科技发展有限责任公司 | 一种基于深度学习的遥感图像森林区域碳汇的计算方法 |
| CN113561189B (zh) * | 2021-09-27 | 2021-12-31 | 深圳市优必选科技股份有限公司 | 冗余度机器人的关节加速度规划方法、装置、设备及介质 |
| CN114399882A (zh) * | 2022-01-20 | 2022-04-26 | 红骐科技(杭州)有限公司 | 一种用于消防机器人的火源探测识别并预警方法 |
| CN114859796A (zh) * | 2022-06-09 | 2022-08-05 | 中国第一汽车股份有限公司 | 一种车辆控制方法、电子设备及存储介质 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102819263A (zh) * | 2012-07-30 | 2012-12-12 | 中国航天科工集团第三研究院第八三五七研究所 | 多摄像头无人车视觉感知系统 |
| CN103852265A (zh) * | 2014-03-27 | 2014-06-11 | 北京联合大学 | 一种无人驾驶车辆环境分项性能测试系统及测试方法 |
| WO2015024407A1 (zh) * | 2013-08-19 | 2015-02-26 | 国家电网公司 | 基于电力机器人的双目视觉导航系统及方法 |
| CN106950964A (zh) * | 2017-04-26 | 2017-07-14 | 北京理工大学 | 无人电动大学生方程式赛车及其控制方法 |
| CN109747434A (zh) * | 2019-01-16 | 2019-05-14 | 浙江科技学院 | 分布式驱动电动汽车转矩矢量分配控制方法 |
| CN110018689A (zh) * | 2019-05-15 | 2019-07-16 | 福州大学 | 一种基于动态窗口的多虚拟目标点全局动态路径规划算法 |
| CN110780305A (zh) * | 2019-10-18 | 2020-02-11 | 华南理工大学 | 一种基于多线激光雷达的赛道锥桶检测及目标点追踪方法 |
| CN111258323A (zh) * | 2020-03-30 | 2020-06-09 | 华南理工大学 | 一种智能车辆轨迹规划与跟踪的联合控制方法 |
| CN111665850A (zh) * | 2020-06-29 | 2020-09-15 | 合肥工业大学 | 一种无人驾驶方程式赛车轨迹跟踪横向控制方法及装置 |
-
2020
- 2020-12-21 CN CN202011517192.5A patent/CN112731925B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102819263A (zh) * | 2012-07-30 | 2012-12-12 | 中国航天科工集团第三研究院第八三五七研究所 | 多摄像头无人车视觉感知系统 |
| WO2015024407A1 (zh) * | 2013-08-19 | 2015-02-26 | 国家电网公司 | 基于电力机器人的双目视觉导航系统及方法 |
| CN103852265A (zh) * | 2014-03-27 | 2014-06-11 | 北京联合大学 | 一种无人驾驶车辆环境分项性能测试系统及测试方法 |
| CN106950964A (zh) * | 2017-04-26 | 2017-07-14 | 北京理工大学 | 无人电动大学生方程式赛车及其控制方法 |
| CN109747434A (zh) * | 2019-01-16 | 2019-05-14 | 浙江科技学院 | 分布式驱动电动汽车转矩矢量分配控制方法 |
| CN110018689A (zh) * | 2019-05-15 | 2019-07-16 | 福州大学 | 一种基于动态窗口的多虚拟目标点全局动态路径规划算法 |
| CN110780305A (zh) * | 2019-10-18 | 2020-02-11 | 华南理工大学 | 一种基于多线激光雷达的赛道锥桶检测及目标点追踪方法 |
| CN111258323A (zh) * | 2020-03-30 | 2020-06-09 | 华南理工大学 | 一种智能车辆轨迹规划与跟踪的联合控制方法 |
| CN111665850A (zh) * | 2020-06-29 | 2020-09-15 | 合肥工业大学 | 一种无人驾驶方程式赛车轨迹跟踪横向控制方法及装置 |
Non-Patent Citations (2)
| Title |
|---|
| 基于MPC自适应巡航系统控制策略联合仿真研究;刘文祥;李强;;浙江科技学院学报;20200731(第04期);全文 * |
| 基于运动微分约束的无人车辆纵横向协同规划算法的研究;姜岩;龚建伟;熊光明;陈慧岩;;自动化学报;20131215(第12期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112731925A (zh) | 2021-04-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112731925B (zh) | 用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法 | |
| CN109597087B (zh) | 一种基于点云数据的3d目标检测方法 | |
| CN114384920B (zh) | 一种基于局部栅格地图实时构建的动态避障方法 | |
| CN112232490B (zh) | 一种基于视觉的深度模仿强化学习驾驶策略训练方法 | |
| CN110009648B (zh) | 基于深浅特征融合卷积神经网络的路侧图像车辆分割方法 | |
| CN111292366B (zh) | 一种基于深度学习和边缘计算的视觉行车测距算法 | |
| CN110378210B (zh) | 一种基于轻量化YOLOv3的车辆和车牌检测及长短焦融合测距方法 | |
| CN113126115B (zh) | 基于点云的语义slam方法、装置、电子设备和存储介质 | |
| US11586865B2 (en) | Apparatus, system and method for fusing sensor data to do sensor translation | |
| CN109299656B (zh) | 一种车载视觉系统场景视深确定方法 | |
| CN113689483B (zh) | 一种基于单目摄像头和激光雷达的测距方法 | |
| CN114913493A (zh) | 一种基于深度学习的车道线检测方法 | |
| CN111814667A (zh) | 一种智能路况识别方法 | |
| CN115285143A (zh) | 一种基于场景分类的自动驾驶车辆导航方法 | |
| CN113076988B (zh) | 基于神经网络的移动机器人视觉slam关键帧自适应筛选方法 | |
| CN117115690A (zh) | 一种基于深度学习和浅层特征增强的无人机交通目标检测方法及系统 | |
| CN114708568B (zh) | 基于改进RTFNet的纯视觉自动驾驶控制系统、方法、介质 | |
| Holder et al. | Learning to drive: End-to-end off-road path prediction | |
| CN116311136A (zh) | 用于辅助驾驶的车道线参数计算方法 | |
| CN116403186A (zh) | 基于FPN Swin Transformer与Pointnet++ 的自动驾驶三维目标检测方法 | |
| Wang et al. | An end-to-end auto-driving method based on 3D LiDAR | |
| CN113625718A (zh) | 车辆的行驶路径规划方法 | |
| CN113569663B (zh) | 车辆车道偏移的测量方法 | |
| US20220261658A1 (en) | Apparatus, system and method for translating sensor label data between sensor domains | |
| Abrinia et al. | Advanced driving assistance system using distributed computation on single-board computers |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |