精华内容
下载资源
问答
  • 2020-08-27 13:04:40

    无人驾驶路径规划论文

    It was a year ago (summer 2019) when I was deciding what topic to choose for my bachelor thesis. At the time I was finishing an internship at the European Space Agency where I worked on mineralogical machine learning research¹. However, I also knew that at my university there is a lot of research with self-driving cars in collaboration with Toyota. While mineralogical research that could be possibly used in future planetary missions is very important, self-driving cars are not only exciting, but also tangible and presentable. So I contacted my former research supervisor from my university whether he had any research ideas in this area. Among many proposed, I was most excited by Autonomous Car Chasing.

    一年前(2019年夏季),我正在决定为我的学士论文选择哪个主题。 当时,我正在欧洲航天局完成实习,在那里我从事矿物学机器学习研究 ¹。 但是,我也知道,在我的大学里,有很多与丰田合作的无人驾驶汽车研究。 虽然这可能在未来行星任务可能被用于矿物学研究是非常重要的,自动驾驶汽车不仅令人兴奋,但也是有形的和像样的 。 因此,我联系了我大学的前研究主管,他是否对此领域有任何研究想法。 在提出的众多建议中,自动驾驶汽车追逐令我最为兴奋。

    为什么要追车? (Why Car Chasing?)

    When you look at autonomous cars in real-life, you will see that they are most often used in situations when the driving is too boring for a driver. They are used on a highway in a traffic jam or in a convoy driving at a near-constant speed. However, regular traffic forces the drivers to react to many unexpected situations and perform dynamic maneuvers. The proposed car chasing scenario, in which an autonomous car chases a non-cooperative vehicle that actively tries to drive away, is so extreme that an autonomous vehicle with car chasing capabilities can surely handle regular traffic.

    在现实生活中观察自动驾驶汽车时,您会发现它们最常用于驾驶员无聊的情况。 它们用在高速公路上的交通拥堵中或以接近恒定的速度行驶的车队中。 但是,常规交通迫使驾驶员对许多意外情况做出React并进行动态操纵。 拟议的汽车追逐场景非常极端,以至于无人驾驶汽车追逐主动试图驶离的非合作车辆,因此具有汽车追逐能力的自动驾驶汽车肯定可以应付正常的交通。

    新的具有挑战性的数据集 (New challenging dataset)

    The first task was to create an autonomous car chasing system and test it in CARLA. To perform the experiments we first collected a new challenging publicly available CARLA Car Chasing Dataset collected by manually driving the chased car. We created a baseline algorithm, which detected the pursued car and then basically drove straight after it. This approach had a huge flaw. It wasn’t considering the surrounding environment while driving. It was only focused on the pursued car. Therefore, this system crashed a lot. We realized our system needed an understanding of the surrounding environment. We considered adding semantic segmentation neural network, but we already had a neural network detector. We needed to analyze as many frames per second as possible and also had a very limited computational power (embedded system on a RC car). We needed a fast and effective solution.

    第一个任务是创建一个自动驾驶汽车追踪系统,并在CARLA中对其进行测试。 为了进行实验,我们首先收集了一个新的具有挑战性的公开可用的CARLA汽车追踪数据集 ,该数据集是通过手动驾驶被追踪的汽车而收集的。 我们创建了一个基准算法,该算法可以检测出所追逐的汽车,然后基本上在行驶后一直行驶。 这种方法有一个巨大的缺陷。 开车时没有考虑周围的环境。 它只专注于追逐的汽车。 因此,该系统崩溃很多。 我们意识到我们的系统需要了解周围的环境。 我们考虑过添加语义分割神经网络,但是我们已经有一个神经网络检测器。 我们需要每秒分析尽可能多的帧,并且计算能力也非常有限(RC汽车上的嵌入式系统)。 我们需要一种快速有效的解决方案。

    双任务神经网络 (Dual-task neural network)

    During inference, an image is passed through the network just once. The network provides an object detection as well as the semantic segmentation outputs. While the training is slightly slower than training a single-task neural network, the extra cost during inference of the proposed architecture is negligible. The segmentation output provides a semantic map of the input image consisting of 10x10 cells of two classes: a drivable surface and a background, see the image below.

    在推理期间,图像仅通过网络传递一次。 网络提供对象检测以及语义分段输出。 虽然训练比训练单任务神经网络要慢一些,但是在推断所提出的体系结构期间的额外费用可以忽略不计。 分割输出提供输入图像的语义图,该输入图像由两类10x10的单元组成:可驱动的表面和背景,请参见下图。

    Image for post
    Segmented image, taken from²
    分割图像,取自²

    The neural network shares the same backbone for both tasks — a 53 layer feature extractor called Darknet-53⁴. Attached to the feature extractor are two sets of layers — one that gives the output for the object detection and the other that gives an output for the image segmentation. The architecture of the neural network is depicted in the image below. The network is trained by alternating optimization — in every second batch, the network is optimized only for detection, while the segmentation is optimized in the remaining batches. The neural network uses different loss functions depending on the batch².

    神经网络在两个任务上共享相同的主干-一个称为Darknet-53的53层特征提取器。 连接到特征提取器的是两组图层-一组为对象检测提供输出,另一组为图像分割提供输出。 下图描述了神经网络的体系结构。 通过交替优化来训练网络-在每第二批中,仅针对检测对网络进行优化,而在其余批次中对分段进行优化。 根据批次²,神经网络使用不同的损失函数。

    Image for post
    Network architecture, taken from²
    网络架构,取自²

    实验结果 (Experiment results)

    First, we tested the system using a difficult subset of the CARLA Car Chasing Dataset. We observed, that the full algorithm performed significantly better than other versions of the system that did not use coarse semantic segmentation. It achieved almost 10 percentage points higher drive completion on average than the next best-evaluated version. We also showed that the dual-task neural network system is more resistant to detector miss rate (when the detector failed to detect the pursued car).

    首先,我们使用CARLA Car Chasing数据集的困难子集测试了系统。 我们观察到,完整算法的性能明显优于不使用粗略语义分割的其他版本的系统。 与下一个最佳评价版本相比,它的驱动器平均完成率提高了近10个百分点。 我们还表明,双任务神经网络系统对检测器未命中率的抵抗力更高(当检测器无法检测到所追捕的汽车时)。

    Then, we performed several live tests under different weather and lighting conditions. The system was tested on an empty roundabout as well as in a residential area as depicted in the image below. The autonomous system followed the other car smoothly without jerky movements. For the most part, it was able to successfully chase the other vehicle. It was maintaining the desired distance when the pursued car was driving in a straight line. If the chased car stopped, so did the autonomous system. A limitation of the system comes from its current reactive nature, which in certain rides affected the ability to make a U-turn on a narrow road.

    然后,我们在不同的天气和光照条件下进行了几次现场测试。 如下图所示,该系统在空的回旋处以及住宅区进行了测试。 自主系统平稳地跟随另一辆汽车,没有晃动。 在大多数情况下,它能够成功追逐另一辆车。 当被追赶的汽车以直线行驶时,它保持了所需的距离。 如果被追赶的汽车停了,那么自动驾驶系统也会停下来。 该系统的局限性在于它目前的React特性,在某些行驶过程中,它会影响在狭窄道路上掉头的能力。

    Image for post
    Real-world test where trajectories of both vehicles are shown: blue of the chasing and dashed black of the pursued car, taken from²
    真实世界的测试,其中显示了两种车辆的轨迹:追逐的蓝色和追逐的黑色的虚线,取自²

    会议 (Conference)

    The results of the novel proposed approach were clear and before the bachelor thesis was even finished, we knew there was a potential for publication. After finishing polishing the thesis, we started its transformation into a research article. A multiple-page section in the thesis easily turned into a couple of lines in the publication. Unlike a thesis that mainly serves as proof of your understanding of the topic, we turned it into something that could be read by other people. We decided to publish to a workshop associated with the third most prestigious conference on computer vision called European Conference on Computer Vision (ECCV). The main advantage of a workshop is its short processing time. In two weeks, we knew our paper was accepted.

    提出的新方法的结果很明确,甚至在学士学位论文尚未完成之前,我们就知道有发表的潜力。 完成论文的完善后,我们开始将其转化为研究论文。 论文中多页的部分很容易变成出版物中的两行。 与主要用来证明您对该主题的理解的论文不同,我们将其转变为其他人可以阅读的东西。 我们决定将与第三届最负盛名的计算机视觉会议 ( 欧洲计算机视觉会议 (ECCV))相关的研讨会发布。 车间的主要优点是加工时间短。 两周后,我们知道我们的论文被接受了

    讨论区 (Discussion)

    This paper isn’t the first (nor second) paper I have published. I am also not flying to the other side of the world to present it like with my first publication (FG 2018 in China) because the conference is only online/virtual due to COVID-19. That said, I am very proud of the paper. The project is very exciting and hopefully, other researchers (or students at FIT CTU) will attempt to improve on the method.

    本文不是我发表的第一篇(也是第二篇)。 我也没有像我的第一本出版物(FG 2018在中国)那样飞往世界的另一端,因为由于COVID-19,这次会议只是在线/虚拟的。 就是说,我为这份文件感到骄傲。 该项目非常令人兴奋,希望其他研究人员(或FIT CTU的学生)将尝试改进该方法。

    I believe that my motivation for the topic was the main reason for the success of the thesis. The thesis topic was exciting and also purposeful and I wish you to find a topic that you can get excited about. I hope there can be a collaboration between Center for Machine Perception (CMP) at FEL CTU and FIT CTU. If you are interested in autonomous driving or computer vision, CMP is a place to be. I can also definitely recommend my supervisor Jan Cech, who discussed the thesis progress with me on a regular weekly basis.

    我相信我对该主题的动机是论文取得成功的主要原因。 论文主题既令人兴奋,又有针对性,我希望您能找到一个令您兴奋的主题。 我希望FEL CTU的机器感知中心(CMP)与FIT CTU之间可以进行合作。 如果您对自动驾驶或计算机视觉感兴趣,可以选择CMP。 我当然也可以推荐我的主管Jan Cech,他每周定期与我讨论论文的进展情况。

    结论 (Conclusion)

    We have developed a system, capable of autonomously chasing another vehicle, using the novel dual-task network that concurrently detects objects and predicts coarse semantic segmentation. The proposed system was extensively tested in CARLA simulator using a new challenging publicly available (at our GitHub) chasing dataset and on a real sub-scale vehicle platform (video available on youtube).

    我们已经开发了一种系统,该系统能够使用新颖的双任务网络同时自动检测对象并预测粗略的语义分割,从而能够自动追逐另一辆车。 拟议的系统已在CARLA模拟器中使用新的具有挑战性的公开可用( 在我们的GitHub上 )的追踪数据集和真实的小规模车辆平台( 在youtube上提供了视频 )进行了广泛测试。

    Despite the simplicity of the proposed system, it shows robust chasing capabilities by using only information from a single RGB camera. One of the system limitations is its reactive nature. We believe that the system could improve by using a more sophisticated trajectory planning algorithm that would include predictive modeling of the chased car.

    尽管建议的系统很简单,但仅使用来自单个RGB摄像机的信息,它仍显示出强大的跟踪能力。 系统限制之一是其React性。 我们认为,可以通过使用更复杂的轨迹规划算法(包括对被追赶的汽车进行预测建模)来改善系统。

    We hope to inspire other researchers or students looking for a thesis topic to try and improve our methods. We believe that testing an autonomous driving system under extreme conditions is an important step to creating public trust in self-driving cars. Ultimately, extreme challenges like the DARPA challenge, or our autonomous car chasing is what will drive innovation forward.

    我们希望启发其他正在寻找论文主题的研究人员或学生尝试并改进我们的方法。 我们认为,在极端条件下测试自动驾驶系统是建立公众对自动驾驶汽车的信任的重要步骤。 最终,诸如DARPA挑战之类的极端挑战或我们的自动驾驶汽车追逐将推动创新向前发展。

    翻译自: https://medium.com/student-success-stories/autonomous-car-chasing-from-thesis-to-a-conference-paper-a750ef46b76e

    无人驾驶路径规划论文

    更多相关内容
  • 基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于...
  • 采用人工势场法进行路径规划和避障,建立新的汽车无人驾驶路径规划环境模型。汽车以自身的圆周势场进行探测,每次移动都会使势力场发生改变,在进行信息收集后判断避开障碍物的所有路径,最终在规划的路径中找出最佳路径...
  • 一、无人驾驶路径规划 众所周知,无人驾驶大致可以分为三个方面的工作:感知,决策及控制。 路径规划是感知和控制之间的决策阶段,主要目的是考虑到车辆动力学、机动能力以及相应规则和道路边界条件下,为车辆提供...

    前言:由于后续可能要做一些无人驾驶相关的项目和实验,所以这段时间学习一些路径规划算法并自己编写了matlab程序进行仿真。开启这个系列是对自己学习内容的一个总结,也希望能够和优秀的前辈们多学习经验。

    一、无人驾驶路径规划

    众所周知,无人驾驶大致可以分为三个方面的工作:感知,决策及控制

    路径规划是感知和控制之间的决策阶段,主要目的是考虑到车辆动力学、机动能力以及相应规则和道路边界条件下,为车辆提供通往目的地的安全和无碰撞的路径。

    路径规划问题可以分为两个方面:

    (一)全局路径规划:全局路径规划算法属于静态规划算法,根据已有的地图信息(SLAM)为基础进行路径规划,寻找一条从起点到目标点的最优路径。通常全局路径规划的实现包括Dijikstra算法,A*算法,RRT算法等经典算法,也包括蚁群算法、遗传算法等智能算法;

    (二)局部路径规划:局部路径规划属于动态规划算法,是无人驾驶汽车根据自身传感器感知周围环境,规划处一条车辆安全行驶所需的路线,常应用于超车,避障等情景。通常局部路径规划的实现包括动态窗口算法(DWA),人工势场算法,贝塞尔曲线算法等,也有学者提出神经网络等智能算法。

    本系列就从无人驾驶路径规划的这两方面进行展开,对一些经典的算法原理进行介绍,并根据个人的一些理解和想法提出了一些改进的意见,通过Matlab2019对算法进行了仿真和验证。过程中如果有错误的地方,欢迎在评论区留言讨论,如有侵权请及时联系。

    那么废话不多说,直接进入第一部分的介绍,全局路径规划算法-RRT算法。

    二、全局路径规划 - RRT算法原理

    RRT算法,即快速随机树算法(Rapid Random Tree),是LaValle在1998年首次提出的一种高效的路径规划算法。RRT算法以初始的一个根节点,通过随机采样的方法在空间搜索,然后添加一个又一个的叶节点来不断扩展随机树。当目标点进入随机树里面后,随机树扩展立即停止,此时能找到一条从起始点到目标点的路径。算法的计算过程如下:

    step1:初始化随机树Tree。将环境中起点作为随机树搜索的起点,此时树中只包含一个节点即根节点qq_{start} 

    stpe2:在环境中随机采样。在环境中随机产生一个点x_{random},若该点不在障碍物范围内则计算随机树Tree中所有节点到x_{random}的欧式距离,并找到距离最近的节点x_{near},若在障碍物范围内则重新生成x_{random}并重复该过程直至找到x_{near};  

    stpe3:生成新节点。在x_{near}x_{random}连线方向,由x_{near}指向x_{random}固定生长距离growdistance生成一个新的节点x_{new},并判断该节点是否在障碍物范围内,若不在障碍物范围内则将x_{new}添加到随机树 Tree中,否则的话返回step2重新对环境进行随机采样;

    step4: 停止搜索。当x_{near}和目标点x_{goal}之间的距离小于设定的阈值时,则代表随机树已经到达了目标点,将x_{goal}作为最后一个路径节点加入到随机树中,算法结束并得到所规划的路径 。

    RRT算法由于其随机采样及概率完备性的特点,使得其具有如下优势:

    (1)不需要对环境具体建模,有很强空间搜索能力;

    (2)路径规划速度快;

    (3)可以很好解决复杂环境下的路径规划问题。

    但同样是因为随机性,RRT算法也存在很多不足的方面:

    (1)随机性强,搜索没有目标性,冗余点多,且每次规划产生的路径都不一样,均不一是最优路径;

    (2)可能出现计算复杂、所需的时间过长、易于陷入死区的问题;

    (3)由于树的扩展是节点之间相连,使得最终生成的路径不平滑;

    (4)不适合动态环境,当环境中出现动态障碍物时,RRT算法无法进行有效的检测;

    (5)对于狭长地形,可能无法规划出路径。

    三、RRT算法Matlab实现

    使用matlab2019来编写RRT算法,下面将贴出部分代码进行解释。

    1、生成障碍物

    在matlab中模拟栅格地图环境,自定义障碍物位置。

    %% 生成障碍物
    ob1 = [0,-10,10,5];             % 三个矩形障碍物
    ob2 = [-5,5,5,10];
    ob3 = [-5,-2,5,4];
    
    ob_limit_1 = [-15,-15,0,31];    % 边界障碍物
    ob_limit_2 = [-15,-15,30,0];
    ob_limit_3 = [15,-15,0,31];
    ob_limit_4 = [-15,16,30,0];
    
    ob = [ob1;ob2;ob3;ob_limit_1;ob_limit_2;ob_limit_3;ob_limit_4];  % 放到一个数组中统一管理
    
    x_left_limit = -16;             % 地图的边界
    x_right_limit = 15;
    y_left_limit = -16;
    y_right_limit = 16;

    我在这随便选择生成三个矩形的障碍物,并统一放在ob数组中管理,同时定义地图的边界。

    2、初始化参数设置

    初始化障碍物膨胀范围、地图分辨率,机器人半径、起始点、目标点、生长距离和目标点搜索阈值。

    %% 初始化参数设置
    extend_area = 0.2;        % 膨胀范围
    resolution = 1;           % 分辨率
    robot_radius = 0.2;       % 机器人半径
    
    goal = [-10, -10];        % 目标点
    x_start = [13, 10];       % 起点
    
    grow_distance = 1;        % 生长距离
    goal_radius = 1.5;        % 在目标点为圆心,1.5m内就停止搜索

    3、初始化随机树

    初始化随机树,定义树结构体tree以保存新节点及其父节点,便于后续从目标点回推规划的路径。

    %% 初始化随机树
    tree.child = [];               % 定义树结构体,保存新节点及其父节点
    tree.parent = [];
    tree.child = x_start;          % 起点作为第一个节点
    
    flag = 1;                      % 标志位
    
    
    new_node_x = x_start(1,1);     % 将起点作为第一个生成点
    new_node_y = x_start(1,2);
    new_node = [new_node_x, new_node_y];

    4、主函数部分

    主函数中首先生成随机点,并判断是否在地图范围内,若超出范围则将标志位置为0

    rd_x = 30 * rand() - 15;    % 生成随机点
    rd_y = 30 * rand() - 15;    
    if (rd_x >= x_right_limit || rd_x <= x_left_limit ||... % 判断随机点是否在地图边界范围内
        rd_y >= y_right_limit || rd_y <= y_left_limit)
        flag = 0;
    end

    调用函数cal_distance计算tree中距离随机点最近的节点的索引,并计算该节点与随机点连线和x正向的夹角。

    [angle, min_idx] = cal_distance(rd_x, rd_y, tree);    % 返回tree中最短距离节点索引及对应的和x正向夹角

    cal_distance函数定义如下:

    function [angle, min_idx] = cal_distance(rd_x, rd_y, tree)
        distance = [];
        i = 1;
        while i<=size(tree.child,1)
            dx = rd_x - tree.child(i,1);
            dy = rd_y - tree.child(i,2);
            d = sqrt(dx^2 + dy^2);
            distance(i) = d;
            i = i+1;
        end
        [~, min_idx] = min(distance);
        angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1));
    end

    随后生成新节点。

    new_node_x = tree.child(min_idx,1)+grow_distance*cos(angle);% 生成新的节点
    new_node_y = tree.child(min_idx,2)+grow_distance*sin(angle);
    new_node = [new_node_x, new_node_y];

    接下来需要对该节点进行判断:

    ① 新节点是否在障碍物范围内;

    ②  新节点和父节点的连线线段是否和障碍物有重合部分。

    若任意一点不满足,则将标志位置为0。实际上可以将两个判断结合,即判断新节点和父节点的连线线段上的点是否在障碍物范围内。

    for k=1:1:size(ob,1) 
        for i=min(tree.child(min_idx,1),new_node_x):0.01:max(tree.child(min_idx,1),new_node_x)    % 判断生长之后路径与障碍物有无交叉部分
            j = (tree.child(min_idx,2) - new_node_y)/(tree.child(min_idx,1) - new_node_x) *(i - new_node_x) + new_node_y;
            if(i >=ob(k,1)-resolution && i <= ob(k,1)+ob(k,3) && j >= ob(k,2)-resolution && j <= ob(k,2)+ob(k,4))
                flag = 0;
                break
            end
        end
    end

    在这我采用的方法是写出新节点和父节点连线的直线方程,然后将x变化范围限制在min(tree.child(min_idx,1),new_node_x)到max(tree.child(min_idx,1),new_node_x)内,0.01即坐标变换的步长,步长越小判断的越精确,但同时会增加计算量;步长越大计算速度快但是很可能出现误判,如下图所式。

                左图:合适的步长                                                          右图:步长过大

    判断标志位若为1,则可以将该新节点加入到tree中,注意保存新节点和它的父节点,同时显示在figure中,之后重置标志位。

    if (flag == true)           % 若标志位为1,则可以将该新节点加入tree中
        tree.child(end+1,:) = new_node;
        tree.parent(end+1,:) = [tree.child(min_idx,1), tree.child(min_idx,2)];
        plot(rd_x, rd_y, '.r');hold on
        plot(new_node_x, new_node_y,'.g');hold on
        plot([tree.child(min_idx,1),new_node_x], [tree.child(min_idx,2),new_node_y],'-b');
    end
        
    flag = 1;                   % 标志位归位

    最后就是把障碍物、起点终点等显示在figure中,并判断新节点到目标点距离。若小于阈值则停止搜索,并将目标点加入到node中,否则重复该过程直至找到目标点。

    %% 显示
    for i=1:1:size(ob,1)        % 绘制障碍物
        fill([ob(i,1)-resolution, ob(i,1)+ob(i,3),ob(i,1)+ob(i,3),ob(i,1)-resolution],...
             [ob(i,2)-resolution,ob(i,2)-resolution,ob(i,2)+ob(i,4),ob(i,2)+ob(i,4)],'k');
    end
    hold on
    
    plot(x_start(1,1)-0.5*resolution, x_start(1,2)-0.5*resolution,'b^','MarkerFaceColor','b','MarkerSize',4*resolution); % 起点
    plot(goal(1,1)-0.5*resolution, goal(1,2)-0.5*resolution,'m^','MarkerFaceColor','m','MarkerSize',4*resolution); % 终点
    set(gca,'XLim',[x_left_limit x_right_limit]); % X轴的数据显示范围
    set(gca,'XTick',[x_left_limit:resolution:x_right_limit]); % 设置要显示坐标刻度
    set(gca,'YLim',[y_left_limit y_right_limit]); % Y轴的数据显示范围
    set(gca,'YTick',[y_left_limit:resolution:y_right_limit]); % 设置要显示坐标刻度
    grid on
    title('D-RRT');
    xlabel('横坐标 x'); 
    ylabel('纵坐标 y');
    pause(0.05);
    if (sqrt((new_node_x - goal(1,1))^2 + (new_node_y- goal(1,2))^2) <= goal_radius) % 若新节点到目标点距离小于阈值,则停止搜索,并将目标点加入到node中
        tree.child(end+1,:) = goal;         % 把终点加入到树中
        tree.parent(end+1,:) = new_node;
        disp('find goal!');
        break
    end

    5、绘制最优路径

    从目标点开始,依次根据节点及父节点回推规划的路径直至起点,要注意tree结构体中parent的长度比child要小1。最后将规划的路径显示在figure中。

    %% 绘制最优路径
    temp = tree.parent(end,:);
    trajectory = [tree.child(end,1)-0.5*resolution, tree.child(end,2)-0.5*resolution];
    for i=size(tree.child,1):-1:2
        if(size(tree.child(i,:),2) ~= 0 & tree.child(i,:) == temp)
            temp = tree.parent(i-1,:);
            trajectory(end+1,:) = tree.child(i,:);
        if(temp == x_start)
            trajectory(end+1,:) = [temp(1,1) - 0.5*resolution, temp(1,2) - 0.5*resolution];
        end
        end
    end
    plot(trajectory(:,1), trajectory(:,2), '-r','LineWidth',2);
    pause(2);

    程序运行最终效果如下:

     红点都是生成点随机点,绿点是tree中节点,红色路径即为RRT算法规划的路径。

    6、路径平滑(B样条曲线)

    由于规划的路径都是线段连接,在节点处路径不平滑,这也是RRT算法的弊端之一。一般来说轨迹平滑的方法有很多种,类似于贝塞尔曲线,B样条曲线等。我在这采用B样条曲线对规划的路径进行平滑处理,具体的方法和原理我后续有时间再进行说明,这里先给出结果:

     黑色曲线即位平滑处理后的路径。

    四、多组结果对比

    ① 相邻两次仿真结果对比:

    可以看出由于随机采样的原因,任意两次规划的路径都是不一样的。 

    ② 复杂环境下的路径规划。选取一个相对复杂的环境,仿真结果如下:

    可以看出RRT算法可以很好解决复杂环境下的路径规划问题。

    ③ 狭窄通道下的路径规划。选取一个狭窄通道环境,仿真结果如下:

     由于环境采样的随机性,在狭长通道内生成随机点的概率相对较低,导致可能无法规划出路径。

    五、结语

    由最终仿真结果可以看出,RRT算法通过对空间的随机采样可以规划出一条从起点到终点的路径,规划速度很快,同时不依赖于环境。但规划过程随机性很强,没有目的性,会产生很多冗余点,且每次规划的路径都不一样,对于狭窄通道可能无法规划出路径。

    下篇文章我将对RRT算法的优化提出一些自己的想法,并在现有的程序上进行修改,最终对比改进前后的RRT算法效果。

    文中如有错误或侵权的地方还欢迎各位指出,我会及时回复并进行修改。

    PS:需要matlab源码的朋友可以在评论区留下邮箱。

    展开全文
  • 前言:对于无人驾驶路径规划系列的第二篇RRT算法的改进部分,由于有些内容属于个人想到的创新点,有想法投一篇小论文所以暂时没有公开,等后续完成后我会再公开介绍。今天第三篇内容开启一个新的算法介绍:Frenet...

    前言:对于无人驾驶路径规划系列的第二篇RRT算法的改进部分,由于有些内容属于个人想到的创新点,有想法投一篇小论文所以暂时没有公开,等后续完成后我会再公开介绍。今天第三篇内容开启一个新的算法介绍:Frenet坐标系下的动态规划。我花了将近半个月的时间来了解、研究算法原理,理解网上python开源的代码,最后根据个人理解在matlab上进行了复现。如果还没有看过我前面文章的读者,可以点击下方的传送门:

    无人驾驶路径规划(一)全局路径规划 - RRT算法原理及实现

    同样,如果文中有错误或侵权的地方还请各位读者指出,我会及时作出修改,笔者在这先行谢过。

    一、轨迹规划方法简介

    在第一篇文章中我对无人驾驶路径规划技术做了个简单的介绍,即可以分为全局路径规划和局部路径规划两部分。今天介绍的Frenet坐标系下动态轨迹规划就属于局部路径规划的内容。在这我先简要介绍一下轨迹规划的方法。参考论文:基于激光雷达的环境识别及局部路径规划技术研究

    无人车的局部路径规划不仅需要考虑到道路交通中的静态障碍物的避障问题,还需要对环境中行人、车辆等动态障碍物未来轨迹做出的预判,对未来时间段内的碰撞可能性进行分析,保证车辆安全、舒适的行驶,因而在求解过程中需要考虑时间维度 t。当路径点增加了时间这一维度,实质上转变为了轨迹点\left ( x,y,t \right ),因而在自动驾驶技术领域局部路径规划也可以称为轨迹规划。车辆在三维空间内的轨迹规划,一般有三种常用的策略:

    • 一将三维空间离散化,通过启发式搜索算法进行求解。
    • 二是将轨迹分解为路径规划和速度规划两个过程。该方法的思想在于通过路径规划完成静态环境下路径求解,然后使用速度规划方法解决运动障碍物的避撞问题。
    • 三是将轨迹分解为两个方向关于时间的优化求解问题。最早由于 Werling 提出将三维的轨迹规划分解为横向的运动规划和纵向的运动规划问题,这里的横向是指车辆相对于道路中心的法线方向,纵向指中心线的切线方向。

    那么Frenet坐标系下动态轨迹规划实际上就属于第三种,将规划问题分解为纵向和横向来分别求解。

    二、Frenet坐标系转换

    1、Frenet坐标系概念

    在轨迹规划过程中,使用笛卡尔直角坐标系并不能很好的描述车辆当前位置与当前所在车道的关系,因此需要引入Frenet坐标系的概念。在Frenet坐标系中,使用道路中心线作为参考线,将车辆的轨迹点投影到参考线上得到参考点,令沿参考线方向为纵轴s,垂直于参考线方向为横轴d。Frenet坐标系可以很容易确定车辆偏离车道中心线的距离及沿着车道线行驶的距离,可以忽略道路曲率的影响,相比于笛卡尔坐标系下的描述更为简洁和直观。 Frenet坐标系和笛卡尔坐标系关系如下:

    2、Frenet-笛卡尔坐标系转换

    假设车辆行驶至t时刻实际轨迹和参考轨迹如下:

    全局坐标系下,t时刻车辆的状态可以描述为:[\vec{x}, \theta_{x}, \kappa _{x}, v_{x}, a_{x}],各参数含义如下:

    \vec{x} -Q(x, y) / Q (s, d),车辆当前位置;

    \theta_{x}- 方位角,速度方向与x正向夹角;

    \kappa _{x}- 曲率;v_{x}- 速度; a_{x}- 加速度。

    \vec{r}(t)- Q投影到参考线上的点,P在全局坐标系下的位置向量。

    Frenet坐标系下,车辆的状态量可以描述为:[s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d}, \frac{\ddot{d}}{​{d}'}, {d}''],各参数含义如下:

    s- 沿参考线纵向位移; \dot{s}- 沿参考线纵向速度; \ddot{s}- 沿参考线纵向加速度;

    d- 参考线法线方向(横向)位移; \dot{d}- 横向速度; \ddot{d}- 横向加速度;

    {​{d}'}- 横向位移对应的弧长的一阶导数;{​{d}''}- 横向位移对应的弧长的二阶导数。

    笛卡尔坐标系与Frenet坐标系状态量转化可以表示为如下关系:

    \\v_x =\frac{\dot{s}(1-\kappa_rd)}{cos\Delta \theta} \\a_x=\dot{v_x}=\ddot{s}\frac{(1-\kappa_rd)}{cos\Delta \theta}+\frac{\dot{s}^2}{cos\Delta\theta}[(1-\kappa_rd)tan\Delta\theta\Delta\dot{\theta}-({\kappa_r}'d+\kappa_r{d}')] \\x = x_r - dsin\Delta\theta_r \\y = y_r + dcos\Delta\theta_r \\\theta_x = arctan\frac{​{d}'}{1-\kappa_rd}+\theta_r \\\kappa_x =\frac{\theta_x^k-\theta_x^{k-1}}{\sqrt{(x_k-x_{k-1})^2}+(y_k-y_{k-1})^2}

    由于具体推导过程比较繁琐和冗长,我在这不进行展开介绍,有兴趣的读者可以参考这篇论文的内容:基于Frenet坐标系的自动驾驶轨迹规划与优化算法

    三、动态轨迹规划过程

    1、基于jerk实时轨迹规划

    轨迹规划过程实际上是一个优化问题。通常来说,乘坐舒适性和安全性是轨迹规划的目标,过大的加速度变化会使得乘客感到不适。因此轨迹规划过程中可以将加速度变化率,也就是“加加速度”-jerk作为优化的目标,来保证乘坐的舒适性。

    根据文献中的介绍,经过Frenet分解后可以构建一个一维积分系统:

    \dot{\overrightarrow{u(t)}}=\begin{bmatrix} 0 &1 &0 \\ 0 &0 &1\\ 0& 0 & 0 \end{bmatrix}\overrightarrow{u(t)}+\begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix}\dddot{f(t)}

    其中\overrightarrow{u(t)}=[f(t), \dot{f(t)}, \ddot{f(t)}]f(t)为横向/纵向运动,d(t)/s(t)\dddot{f(t)}为jerk。

    Takahashi已证明对于上述系统由t_0时刻初始配置S_0=[f(t_0), \dot{f(t_0)}, \ddot{f(t_0)}]^Tt_1时刻目标配置S_1=[f(t_1), \dot{f(t_1)}, \ddot{f(t_1)}]^T,基于jerk的优化轨迹都在一个五次多项式中,且必存在最小化J_{f(t)}的解:

    J_{f(t)}=\int_{t_0}^{t_1}g(\dddot{f(t)})dt+h(\overrightarrow{u(t)},t)_{t1}

    其中g(\dddot{f(t)})=\frac{1}{2}\dddot{f(t)},可以用于评价舒适性,h(\overrightarrow{u(t)},t)_{t1}是目标配置函数,用以评价轨迹。

    2、横向轨迹规划求解

    横向规划主要承担的是避障、换道等任务,已知在t_0时刻的配置D_0=[d(t_0),\dot{d(t_0)},\ddot{d(t_0)}]^Tt_1时刻目标配置D_1=[d(t_1),\dot{d(t_1)}, \ddot{d(t_1)}]^T,则五次多项式可以写为:

    \\d(t)=c_{d0}+c_{d1}t+c_{d2}t^2+c_{d3}t^3+c_{d4}t^4+c_{d5}t^5 \\\dot{d(t)}=c_{d1}+2c_{d2}t+3c_{d3}t^2+4c_{d4}t^3+5c_{d5}t^4 \\\ddot{d(t)}=2c_{d2}+6c_{d3}t+12c_{d4}t^2+20c_{d5}t^3

    则有:

    \begin{bmatrix} d(t) \\\dot{d(t)} \\\ddot{d(t)} \end{bmatrix}=\begin{bmatrix} 1 & t^2 & t^3\\ 0& 1 & 2t\\ 0& 0 & 2 \end{bmatrix}\begin{bmatrix} c_{d0} \\c_{d1} \\c_{d2} \end{bmatrix}+\begin{bmatrix} t^3 & t^4 & t^5\\ 3t^2& 4t^3 & 5t^4\\ 6t& 12t^2 & 20t^3 \end{bmatrix}\begin{bmatrix} c_{d3} \\c_{d4} \\c_{d5} \end{bmatrix}

    为简化运算,令t_0=0t_1=\tau,则可求得:

    c_{d0}=d(0), c_{d1}=\dot{d(0)}, c_{d2}=\frac{1}{2}\ddot{d(0)}

    代入上式,可以解得c_{d3}, c_{d4}, c_{d5}

    \begin{bmatrix} c_{d3} \\c_{d4} \\c_{d5} \end{bmatrix}=\begin{bmatrix} \tau^3 & \tau^4 &\tau^5 \\ 3\tau^2& 4\tau^3 & 5\tau^4\\ 6\tau& 12\tau^2 & 20\tau^3 \end{bmatrix}^{-1} \bigl(\begin{smallmatrix} (\begin{bmatrix} d(\tau) \\ \dot{d(\tau)} \\ \ddot{d(\tau)} \end{bmatrix}-\begin{bmatrix} 1 & \tau^2 & \tau^3\\ 0& 1 & 2\tau\\ 0& 0 & 2 \end{bmatrix}\begin{bmatrix} c_{d0} \\ c_{d1} \\ c_{d2} \end{bmatrix} \end{smallmatrix}\bigr)=\begin{bmatrix} \tau^3 & \tau^4 &\tau^5 \\ 3\tau^2& 4\tau^3 & 5\tau^4\\ 6\tau& 12\tau^2 & 20\tau^3 \end{bmatrix}^{-1} \bigl(\begin{smallmatrix} (\begin{bmatrix} d(\tau) \\ \dot{d(\tau)} \\ \ddot{d(\tau)} \end{bmatrix}-\begin{bmatrix} 1 & \tau^2 & \tau^3\\ 0& 1 & 2\tau\\ 0& 0 & 2 \end{bmatrix}\begin{bmatrix} d(0) \\ \dot{d(0)} \\ \frac{1}{2}\ddot{d(0)} \end{bmatrix} \end{smallmatrix}\bigr)

    由此可以求得d(t)。值得注意的是,通常来说希望车辆沿着参考线行驶,横向速度和横向加速度均为0,那么t_1时刻的目标配置可以写为D_1=[d(t_1),0, 0]^T

    通过设置采样间隔\Delta T和横向距离间隔\Delta d,可以生成一系列的备选曲线:

    3、纵向轨迹规划求解

    对于纵向轨迹规划,需要考虑到车辆的实际行驶场景需求,例如跟车、定速巡航、停车、汇流等等。不同的行驶情景有不同的期望配置。在这我仅针对最简单的定速巡航情景进行介绍。

    由于是定速巡航,那么此时沿着参考线方向的位置配置则无需考虑,仅需要配置纵向速度\dot{s_1}和纵向加速度\ddot{s_1}。同时,由于减少了一个配置量,轨迹多项式可以降为一个4次多项式来考虑。由t_0时刻的配置S_0=[s(t_0),\dot{s(t_0)},\ddot{s(t_0)}]^Tt_1时刻目标配置S_1=[s(t_1),\dot{s(t_1)},\ddot{s(t_1)}]^T,4次多项式可以写为:

    \\s(t)=c_{s0}+c_{s1}t+c_{s2}t^2+c_{s3}t^3+c_{s4}t^4 \\\dot{s(t)}=c_{s1}+2c_{s2}t+3c_{s3}t^2+4c_{s4}t^3\\\ddot{s(t)}=2c_{s2}+6c_{s3}t+12c_{s4}t^2

    则有:

    \begin{bmatrix} \dot{s(t)} \\ \ddot{s(t)} \end{bmatrix}=\begin{bmatrix} 1 & 2t\\ 0 & 2 \end{bmatrix}\begin{bmatrix} c_{s1} \\ c_{s2} \end{bmatrix}+\begin{bmatrix} 3t^2& 4t^3\\ 6t& 12t^2 \end{bmatrix}\begin{bmatrix} c_{s3} \\ c_{s4} \end{bmatrix}

    同样,令t_0=0t_1=\tau,则可求得:

    c_{s0}=s(0), c_{s1}=\dot{s(0)}, c_{s2}=\frac{1}{2}\ddot{s(0)}

    代入上式,可以解得c_{s3}, c_{s4}, c_{s5}

    \\\begin{bmatrix} c_{s3} \\ c_{s4} \end{bmatrix}=\begin{bmatrix} 3\tau^2 &4\tau^3 \\ 6\tau& 12\tau^2 \end{bmatrix}^{-1} (\begin{bmatrix} \dot{s(\tau)} \\ \ddot{s(\tau)} \end{bmatrix}-\begin{bmatrix} 1 & 2\tau\\ 0 & 2 \end{bmatrix}\begin{bmatrix} \dot{s(0)} \\\ddot{s(0)} \end{bmatrix})

    通过设置采样间隔\Delta T和速度变化间隔\Delta \dot{s(t_1)},也可以生成一系列的备选曲线:

    4、最优轨迹选择

    对于横向和纵向轨迹规划,都生成了一系列的备选轨迹,通过设定评价函数来确定每一时刻的最优路径。

    根据参考论文,横向轨迹评价函数如下:

    C_d=w_JJ_t(d(t))+w_dd^2+w_tT

    其中:J_t(d(t))=\int_{t_0}^{t_1}\dddot{d(\tau)d}\tau,即jerk在采样时间内的变化幅度,用来评价舒适性;w_dd^2表示目标状态偏离中心线的指标;w_tT为车辆行驶效率评价指标。

    纵向轨迹评价函数如下:

    C_s=w_JJ_t(d(t))+w_s(\dot{s_1}-\dot{s_{set}})^2+w_tT

    与横向评价函数唯一不同的是w_s(\dot{s_1}-\dot{s_{set}})^2代表目标配置速度与设定期望速度的差距损失。

    最后将二者进行合并,加入权重系数,即可得到最优路径评价函数

    C=K_{lat}C_d+K_{lon}C_s

    通常轨迹评价函数越小,代表该轨迹的代价越小,优先度越高。

    5、碰撞检测

    Atsushi Sakai在Python代码中设置的碰撞检测方法是:

    假设车辆在备选轨迹一个采样时刻内沿参考线方向行驶纵向距离s,若结束后车辆与障碍物的距离小于设定的阈值,则该轨迹不满足碰撞检测需求,进行剔除。对所有备选轨迹进行该项操作。

    我在matlab移植代码的时候,没有对这部分进行修改。

    四、Python-Matlab代码移植

    我在网上找到的算法的Python代码是日本无人驾驶工程师Atsushi Sakai编写的,并已经在github上开源。我在文末也给出了源码的链接,有需要的读者可以自行获取。

    为了加深对算法的理解,同时保持之前所有算法验证平台的一致性,我将Python源码移植到了matlab2019。在这我不对Python源码进行解释,仅对我移植后的matlab代码进行一个介绍。

    1、初始参数定义(宏定义)

    在程序最开始先给出需要用到的参数的定义及数值,例如最大采样横向距离、横向距离采样间隔、最大采样时间、采样时间间隔、最大加速度/速度等等,具体如下:

    SIM_LOOP = 500;
    
    MAX_SPEED = 50.0 / 3.6;  % 最大速度 [m/s]
    MAX_ACCEL = 2.0;  % 最大加速度 [m/ss]
    MAX_CURVATURE = 1.0;  % 最大曲率 [1/m]
    MAX_ROAD_WIDTH = 7.0;  % 最大采样横向距离 [m]
    D_ROAD_W = 1.0;  % 横向距离采样间隔 [m]
    DT = 0.2;  % 采样时间间隔 [s]
    MAX_T = 5.0;  % 最大采样时间 [s]
    MIN_T = 4.0;  % 最小采样时间 [s]
    TARGET_SPEED = 30.0 / 3.6;  % 期望速度 [m/s]
    D_T_S = 5.0 / 3.6;  % 纵向速度采样间隔 [m/s]
    N_S_SAMPLE = 1;
    ROBOT_RADIUS = 2;  % 碰撞检测阈值 [m]
    robot_width = 1;   % 机器人宽 [m]
    robot_length = 2;  % 机器人长 [m]
    
    % 评价函数权重
    K_J = 0.1;
    K_T = 0.1;
    K_D = 1.0;
    K_LAT = 1.0;
    K_LON = 1.0;
    
    %% 参数范围确定
    c_speed = 10.0 / 3.6;   % 当前速度
    c_d = 2.0;              % 当前横向位移
    c_d_d = 0.0;            % 当前横向速度
    c_d_dd = 0.0;           % 当前横向加速度
    s0 = 0.0;               % 当前沿车道线位移
    area = 20.0;

    2、障碍物点及参考路径生成

    原文中是给定了几个参考路径点,采用三次样条的方法生成参考路径。

    %% 参考路径点
    nodes = [0, 0;
            10, -6;
            20.5 5;
            35, 6.5;
            70.5, 0;
            100, 5];
    
    %% 设置障碍物坐标点
    ob = [20, 10
          30, 9;
          30, 6;
          35, 9;
          50, 3;
          75, 0];
    %% 生成期望路径
    csp = Cubic_spline(nodes);

    Cubic_spline是我自定义的三次样条生成函数。

    3、定义FrenetPath结构体

    定义FrenetPath结构体,存储每一个备选路径的横向、纵向规划结果,及笛卡尔坐标系下的状态量:

    %%  Frenet轨迹结构体
    FrenetPath.t = [];
    FrenetPath.d = [];
    FrenetPath.d_d = [];
    FrenetPath.d_dd = [];
    FrenetPath.d_ddd = [];
    FrenetPath.s = [];
    FrenetPath.s_d = [];
    FrenetPath.s_dd = [];
    FrenetPath.s_ddd = [];
    FrenetPath.cd = 0.0;
    FrenetPath.cv = 0.0;
    FrenetPath.cf = 0.0;
    
    FrenetPath.x = [];
    FrenetPath.y = [];
    FrenetPath.yaw = [];
    FrenetPath.ds = [];
    FrenetPath.c = [];

    4、Frenet动态轨迹规划算法

    这一部分是该算法的核心函数,即完成了纵向、横向轨迹的规划过程,备选轨迹的评价过程及碰撞检测过程。最后将当前时刻最优轨迹输出给path,path属于FrenetPath结构体类型。

    path =  calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, ...
                                   s0,MAX_ROAD_WIDTH,D_ROAD_W, MIN_T, MAX_T, DT, ...
                                   FrenetPath, TARGET_SPEED,D_T_S, N_S_SAMPLE, ...
                                   K_D, K_J, K_LAT, K_LON, K_T, ob, MAX_ACCEL, ...
                                   MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);

    calc_frenet_path_fixed_velocity函数具体内容如下:

    function fplist = calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, ...
                                   s0,MAX_ROAD_WIDTH,D_ROAD_W, MIN_T, MAX_T, DT, ...
                                   FrenetPath, TARGET_SPEED,D_T_S, N_S_SAMPLE, ...
                                   K_D, K_J, K_LAT, K_LON, K_T, ob, MAX_ACCEL, ...
                                   MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);
        min_cost = [inf,inf];
        cost = [];
        flag = 1;
        for di=-MAX_ROAD_WIDTH:D_ROAD_W:MAX_ROAD_WIDTH
            for Ti = MIN_T:DT:MAX_T
                fp = FrenetPath;
                lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0, 0, Ti);
                
                for t=0:DT:Ti-DT
                    fp.t(1,end+1) = t;
                end
                
                for t=fp.t(1,1):DT:fp.t(1,end)
                    fp.d(1,end+1)     = lat_qp.a0   + lat_qp.a1*t    + lat_qp.a2*t^2    + lat_qp.a3*t^3   + lat_qp.a4*t^4  + lat_qp.a5*t^5;
                    fp.d_d(1,end+1)   = lat_qp.a1   + 2*lat_qp.a2*t  + 3*lat_qp.a3*t^2  + 4*lat_qp.a4*t^3 + 5*lat_qp.a5*t^4;
                    fp.d_dd(1,end+1)  = 2*lat_qp.a2 + 6*lat_qp.a3*t  + 12*lat_qp.a4*t^2 + 20*lat_qp.a5*t^3;
                    fp.d_ddd(1,end+1) = 6*lat_qp.a3 + 24*lat_qp.a4*t + 60*lat_qp.a5*t^2;
                end
                
                for tv = TARGET_SPEED - D_T_S * N_S_SAMPLE : D_T_S: TARGET_SPEED + D_T_S * N_S_SAMPLE
                    tfp = fp;
                    lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti);
                    
                    for t=fp.t(1,1):DT:fp.t(1,end)
                        tfp.s(1,end+1)     = lon_qp.b0   + lon_qp.b1*t   + lon_qp.b2*t^2   + lon_qp.b3*t^3  + lon_qp.b4*t^4;
                        tfp.s_d(1,end+1)   = lon_qp.b1   + 2*lon_qp.b2*t + 3*lon_qp.b3*t^2 + 4*lon_qp.b4*t^3;
                        tfp.s_dd(1,end+1)  = 2*lon_qp.b2 + 6*lon_qp.b3*t + 12*lon_qp.b4*t^2 ;
                        tfp.s_ddd(1,end+1) = 6*lon_qp.b3 + 24*lon_qp.b4*t;
                    end
                    Jp = sum(tfp.d_ddd .^2);
                    Js = sum(tfp.s_ddd .^2);
                    
                    ds = (TARGET_SPEED - tfp.s_d(1,end))^ 2;
                    tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d(1,end)^2;
                    tfp.cv = K_J * Js + K_T * Ti + K_D * ds;
                    tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv;
                    
                    tfp = calc_global_paths(tfp, csp);
                    flag = check_paths(tfp, ob, MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);  
    
                    if (flag == 1)
                        cost(end+1,:) = [tfp.cf, di];
                        if min_cost(end,1) >= tfp.cf
                            min_cost(end+1,:) = [tfp.cf, di];
                            fplist = tfp;
                        end
                    else
                        flag = 1;
                    end
                    
                end
            end
        end
    end

    其中QuinticPolynomial和QuarticPolynomial函数就是轨迹规划的具体求解公式:

    function lat_qp = QuinticPolynomial(xs, vxs, axs, xe, vxe, axe, time)
        lat_qp.a0 = xs;
        lat_qp.a1 = vxs;
        lat_qp.a2 = axs/2;
        A = [time^3,   time^4,     time^5;
             3*time^2, 4*time^3,   5*time^4;
             6*time,   12*time^2,  20*time^3];
        B = [xe - lat_qp.a0 - lat_qp.a1*time - lat_qp.a2*time^2;
             vxe - lat_qp.a1 - 2*lat_qp.a2*time;
             axe - 2*lat_qp.a2];
        temp = A^(-1)*B;
        lat_qp.a3 = temp(1,1);
        lat_qp.a4 = temp(2,1);
        lat_qp.a5 = temp(3,1);   
    end
    function lon_qp = QuarticPolynomial(xs, vxs, axs, vxe, axe, time)
        lon_qp.b0 = xs;
        lon_qp.b1 = vxs;
        lon_qp.b2 = axs / 2.0;
        A = [3*time^2, 4*time^3
             6*time,   12*time^2];
        B = [vxe - lon_qp.b1 - 2*lon_qp.b2*time;
             axe - 2*lon_qp.b2];
        temp = A^(-1)*B;
        lon_qp.b3 = temp(1,1);
        lon_qp.b4 = temp(2,1);
    end

    calc_global_paths用于求解笛卡尔坐标系下的状态量:

    function fplist = calc_global_paths(fplist, csp)
        for i = 1:1:size(fplist.s,2)
            [idx] = findPoint(fplist.s(1,i), csp);
            i_x = csp(idx,1);
            i_y = csp(idx,2);
            i_yaw = (csp(idx+1,2) - csp(idx,2))/(csp(idx+1,1) - csp(idx,1));
            di = fplist.d(1,i);
            fplist.x(1,end+1) = i_x - di * sin(i_yaw);
            fplist.y(1,end+1) = i_y + di * cos(i_yaw);
        end
        
        for i = 1:1:size(fplist.x,2)-1
            dx = fplist.x(1,i+1) - fplist.x(1,i);
            dy = fplist.y(1,i+1) - fplist.y(1,i);
            fplist.yaw(1,end+1) = atan(dy/dx);
            fplist.ds(1,end+1) = sqrt(dx^1 + dy^2);
        end
        
        fplist.yaw(1,end+1) = fplist.yaw(1,end);
        fplist.ds(1,end+1) = fplist.ds(1,end);
        
        for i=1:1:size(fplist.yaw,2)-1
            fplist.c(1,end+1) = (fplist.yaw(1,i+1) - fplist.yaw(1,i+1))/fplist.ds(1,i);
        end
        fplist.c(1,end+1) = fplist.c(1,end);
    end

    check_paths用于碰撞检测,在这里还加入了最大纵向加速度、最大纵向速度和最大曲率约束。我设置了一个标志位flag,只有当通过碰撞检测,即标志位为1的时候,才会进行后续的判断,否则该轨迹就被舍弃:

    function flag = check_paths(fplist, ob, MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS)
        flag = 1;
        for i=1:1:size(fplist.s_d ,2)
            if (fplist.s_d(1,i) > MAX_SPEED)
                flag = 0;
                break
            end
            
            if (fplist.s_dd(1,i) > MAX_ACCEL)
                flag = 0;
                break
            end
            
            if (fplist.c(1,i) > MAX_CURVATURE)
                flag = 0;
                break
            end
        end
        
        if (flag == 1)
            for i=1:1:size(fplist.x,2)
                for j=1:1:size(ob)
                    d = sqrt((ob(j,1)-fplist.x(1,i))^2 + (ob(j,2)-fplist.y(1,i))^2);
                    if(d <= ROBOT_RADIUS)
                        flag = 0;
                        break
                    end
                end
                if (flag == 0)
                    break
                end
                if (flag == 0)
                    break
                end
            end
        end
    end

    若当前轨迹通过碰撞检测即标志位为1,则与截至目前最小代价的轨迹进行比较,如果新的轨迹的代价小于当前的最小代价,则把当前轨迹选作临时最优轨迹,并把其代价值赋给最小代价;若标志位为0,则将其复位为1,并不进行最小代价的判断过程:

    if (flag == 1)
        cost(end+1,:) = [tfp.cf, di];
        if min_cost(end,1) >= tfp.cf
            min_cost(end+1,:) = [tfp.cf, di];
            fplist = tfp;
        end
    else
        flag = 1;
    end

    5、主循环部分

    当循环次数小于最大循环次数时,重复进行轨迹动态规划的过程,如果车辆到达了参考轨迹的最后一个点,则提前跳出循环过程。每次循环都对车辆当前的位置和最优轨迹进行显示:

    %% 循环过程
    for i=1:1:SIM_LOOP
        path =  calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, s0,MAX_ROAD_WIDTH, ...
                                   D_ROAD_W, MIN_T, MAX_T, DT, FrenetPath, TARGET_SPEED,...
                                   D_T_S, N_S_SAMPLE,K_D, K_J, K_LAT, K_LON, K_T, ob,...
                                   MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);
    
        s0 = path.s(1,2);
        c_d = path.d(1,2);
        c_d_d = path.d_d(1,2);
        c_d_dd = path.d_dd(1,2);
        c_speed = path.s_d(1,2);
        
    
        plot(csp(:,1), csp(:,2),'-.b'); hold on
        plot(csp(:,1), csp(:,2)+8,'-k');
        plot(csp(:,1), csp(:,2)-8,'-k');
        plot(ob(:,1), ob(:,2),'*g');
        plot_robot(robot_length, robot_width, path.yaw(1,1) , path.x(1,1), path.y(1,1));
        plot(path.x(1,1), path.y(1,1),'vc','MarkerFaceColor','c','MarkerSize',6);
        plot(path.x(1,:), path.y(1,:),'-r','LineWidth',2);
        plot(path.x(1,:), path.y(1,:),'ro','MarkerFaceColor','r','MarkerSize',4);    
        set(gca,'XLim',[path.x(1,1) - area, path.x(1,1) + area]);
        set(gca,'YLim',[path.y(1,1) - area, path.y(1,1) + area]);
        grid on
        title('Frenet');
        xlabel('横坐标 x'); 
        ylabel('纵坐标 y');
        pause(0.01);
        hold off
        
        if sqrt((path.x(1,1) - nodes(end,1))^2+ (path.y(1,1) - nodes(end,2))^2) <= 1.5
            disp("Goal");
            break
        end
    end

    6、算法运行效果

    Python源码运行效果如下:

    matlab2019代码移植运行效果如下:

    五、结语

    由仿真结果可以看出,车辆在定速巡航的模式下可以按照期望的速度沿着参考线行驶,当遇到障碍物的时候由于实时规划采样的缘故,即使最优轨迹会与障碍物相交,车辆也可以做出判断从而选择其他备选轨迹,从而避开障碍物,保证行驶的安全性。

    对于其他的车辆行驶情景,均可以根据需求进行目标配置和评价函数的修改,在这我不在做更多的说明。

    PS: Frenet坐标系下动态轨迹规划技术是在2010年的ICRA(机器人领域顶会)上首次提出并得到广泛的关注,会议原文链接如下:Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frames

    Takahashi关于jerk优化过程五次多项式推导原文链接如下:

    Local Path Planning And Motion Control For Agv In Positioning

    Atsushi Sakai的Python源码地址如下:

    https://github.com/AtsushiSakai/

    最后,如有需要我matlab原码的朋友可以在评论区留下邮箱,我看到后会及时回复。

    彩蛋:这篇文章的公式实在是太多了,手都要敲麻了TAT。

    展开全文
  • 蚁群优化算法是解决无人驾驶汽车路径规划问题的有效途径。 首先,建立无人车道规划的环境模型,处理和描述环境信息,最后实现问题空间的划分。 接下来,描述蚁群算法的仿生行为。 蚁群算法已通过添加惩罚策略进行了...
  • 本文介绍了应用于无人驾驶汽车路径规划中全局路径规划的A*算法,从规划结果出发,分析传统A*的缺陷,并提出16邻域改进算法。为提高规划效率,减少路径规划时间提出双向16邻域改进算法。并与24邻域及48邻域算法进行比较,...
  • 运用多点预瞄与滚动优化相结合的模型预测控制算法设计了汽车的跟随...在双移线工况下进行了多组速度的跟随实验,结果表明该控制器跟随路径的误差小,对速度的适应性强。与Carsim控制器的跟随结果相比,其跟随效果更好。
  • 基于Matlab的汽车无人驾驶路径规划
  • 基于Matlab的汽车无人驾驶路径规划.rar
  • 无人路径规划的定义: A点到B点 ,构建一个车辆运动轨迹,结合HDMap,Localization 和Prediction 输出:可行驶轨迹,有一系列点组成 两个层面:导航层面; 运动轨迹层面 Routing routing的目标是导航一条A到B的...

    系列文章目录


    【无人驾驶——路径规划】第一章 基础知识

    【无人驾驶——路径规划】第二章 无人车路径规划


    定义

    无人车路径规划的定义:

    1. A点到B点 ,构建一个车辆运动轨迹,结合HDMap,Localization 和Prediction
    2. 输出:可行驶轨迹,有一系列点组成
    3. 两个层面:导航层面; 运动轨迹层面

    Routing

    routing的目标是导航一条A到B的全局路径,一条cost最小的路径
    输入:地图(网络信息,交通信息等)、当前位置、目的地(乘客决定)
    输出:可行驶道路的连线
    搜索:地图数据转化成图网络
    * 节点表示道路
    * 边表示路口
    什么情况下cost高?:
    权重规则:例如左转的权重相较于直行的权重更高,所以Node1到Node4的边权重大,Node1到Node3权重小。
    拥堵情况:比如说Node1到Node3的道路很拥堵,那么它的cost就高;Node4的道路更堵,那么Node1到Node4的cost更高。
    在这里插入图片描述

    A*算法

    在rounting中目前A*算法的应用还是非常广泛的。
    公式:F(n) = G(n) + H(n)

    F(n)表示道路的routing的总cost
    G(n)表示起始点到候选点的cost
    H(n)表示候选点通过启发函数得到的目标点cost

    Motion Planning

    导航信息相当于给了粗略的路径信息,而Planning相当于一个高精度,低级别的search

    规划的约束条件

    • Collision 碰撞(躲避任何障碍物)
    • Comfortable 舒适 (路径点必须平滑,速度也要平滑)
    • 运动学约束(高速转弯,掉头曲率角度)
    • Illegal合法 (交通法规)

    Cost Function

    • cost function由许多部分组成
      • 道路偏离中心线
      • 碰撞或者靠得太近
      • 速度太大,超速
      • Curvature太大,方向盘太急
        针对不同的场景,我们可能有多个不同得cost

    Frenet坐标系

    一般情况下我们会用笛卡尔坐标系(世界坐标系),但是表征的东西并不全面。因此在道路形式方面,我们采用Frenet坐标系,能够更好地表征偏离道路中心线的距离。

    【注】Frenet坐标系和Track坐标系的区别
    * L方向不同
    * Track是基于Road级别
    * Frenet是基于Lane级别

    Path 和 Speed解耦

    • Path和Speed解耦能够让我们将motion planning问题转化为多个凸优化问题。

    Path Planning

    步骤

    • 先生成道路网格
    • 每个网格单元中随机采样
    • 每个网格选一个点
    • 组成多条候选Path

    Cost Function

    需要cost最低的path,也就是最优path,cost的设计往往是planning的重点

    • 中心线距离 l*a0
    • 障碍物距离 d*a1
    • 速度变化率 acc * a2
    • 曲率 kappa * a3
    • F(x) = la0 + da1 + acc * a2 + acc * a3 + a4
      【思考】为什么线性加权可以在一定程度上解决所有问题呢?

    Speed Planning

    ST图

    S-T图表示在path上的速度规划,S表示Path上的纵向距离,T表示运动时间。
    斜率越大,表示速度越快。
    在这里插入图片描述

    如何规划ST轨迹

    1. 连续空间离散化(grid map)

    2. 单元格内速度不变

    3. 把障碍物投影进来

      • 将挡住我们Path轨迹的部分画进ST图中
      • 因此必须要有良好的轨迹预测
      • 例如下图中,to-t1时刻障碍物会在我们的Path轨迹中挡住s0,s1部分,(如何理解黄色部分? 相当于t0-t1时刻,s0-s1这块区域是不能有车通行的)
        在这里插入图片描述
    4. 速度曲线不能碰到这个区域

    • 有多个车的情况
      在这里插入图片描述

    如何优化

    由于折线并不平滑,我们需要将不平滑的折线优化成平滑的线性曲线。

    • QP (Quadratic Programming) 二次规划
    • 这个方法很大程度上依赖于线性空间离散化
    • 将平滑的非线性曲线与这些线段进行拟合
    • 现成的工具:qpQASES

    轨迹规划

    实例

    • 遇到一辆速度很慢的车我们如何超车?
      在这里插入图片描述

    • 生成很多轨迹(撒点采样)
      在这里插入图片描述

    • 用cost function对其进行评估,选择cost最小的一条
      在这里插入图片描述

    • 生成一个ST图来表述速度规划

    • 生成多条速度曲线
      在这里插入图片描述

    • 使用优化工具对多条速度采样进行最优化求解

      • 让整个线路变得平滑
        在这里插入图片描述
    • 最后将每个轨迹点(跟我们自己定义的轨迹点Resolution)的Path,Speed合并得到最终结果。

    在这里插入图片描述

    展开全文
  • 无人驾驶路径规划

    2019-11-16 12:17:14
    使用迪杰斯特拉算法 OpenCV的边缘检测将物体边缘标记 通过对面积大小进行分类 再通过随机点分出的不同区域进行判定 找到可行的最短路径
  • 无人驾驶汽车路径规划概述

    千次阅读 多人点赞 2019-07-30 17:18:42
    无人驾驶汽车路径规划概述 原地址:http://imgtec.eetrend.com/blog/2019/100018447.html 无人驾驶汽车路径规划是指在一定的环境模型基础上,给定无人驾驶汽车起始点和目标点后,按照性能指标规划出一条无碰撞、能...
  • 无人驾驶路径规划之RRT算法

    千次阅读 2019-03-26 00:32:04
    无人驾驶路径规划之RRT算法 简介 快速搜索随机树(RRT-Rapidly-ExploringRandom Trees)是一种常见的用于机器人路径规划的方法,他的原始算法思想很简单,以一个初始点作为根节点,通过随机采样增加叶子节点的方式,...
  • 文章针对近年来的无人驾驶汽车路径规划算法进行总结和归纳。首先对目前主流的环境建模方法进行阐述;其次对路径规划算法进行介绍,通过分析其优缺点,指出融合轨迹规划算法具有最好的适用性;最后总结当前研究挑战并提出...
  • 无人驾驶路径规划算法分析

    千次阅读 2020-03-14 01:13:35
    无人驾驶路径规划层分为全局路径规划和局部路径规划。而规划首先是建立于准确的决策层面和可靠的地图层面的,这两个及其重要的环节很大程度上影响着无人驾驶成功与否!在全局规划中,常用的算法有A*,PRM,RRT,D* ...
  • 作为无人驾驶汽车顺利运行的重要环节,路径规划是指无人驾驶汽车在具有障碍物的环境中,能够规划出一条从起始位置状态到目标位置状态无碰撞的最优路径或次优路径,并满足所有约束条件,是实现汽车智能化的关键技术之...
  • kalman滤波在状态估计和多传感器融合等方面应用广泛,典型的应用有无人机的飞行姿态估计,汽车的行驶状态估计,无人驾驶中的激光雷达和其他传感器之间的融合等。 2、算法步骤 1)线性系统微分表达 根据现代控制理论...
  • 无人驾驶常用路径规划

    万次阅读 多人点赞 2019-01-05 16:54:15
    无人驾驶系统介绍行为决策运动轨迹规划路径规划的不同之处路径规划的定义路径规划方法全局路径规划Dubins路径方法Dubins路径改良算法局部路径规划模糊逻辑算法基于行为的路径规划算法基于再励学习的路径规划算法基于...
  • Floyd算法是一种动态规划算法,稠密图效果最佳,节点间的连接权值可正可负。此算法简单有效,由于三重循坏结构紧凑,对于稠密图,效果要高于Dijkstra算法。
  • 无人驾驶汽车的三次样条路径规划和拟合,python程序编写,效果很好
  • 无人驾驶汽车全局路径规划

    千次阅读 2020-01-20 15:24:18
    单体智能的无人驾驶系统,根据功能可划分为不同的子模块,包括:高精度地图、定位模块、感知模块、预测模块、全局路径规划模块、运动规划模块、运动控制模块以及人机交互模块等。本文研究的主要内容是无人驾驶汽车...
  • 无人驾驶12:路径规划,生成路径曲线

    千次阅读 多人点赞 2020-02-29 21:14:01
    运动规划问题: 配置空间 配置空间:定义机器人的所有可能配置,一般在二维空间中,定义为[x,y, theat],即二维坐标加方向。 运动规划: 就是根据初始配置(由定位模块传感器获得),目的配置(由行为规划模块获得),...
  • 实现无人驾驶车辆基于车辆动力学模型的轨迹跟踪及路径规划
  • 2、无人驾驶--路径规划算法:Dijkstra

    千次阅读 2021-11-13 16:57:31
    迪杰斯特拉算法是由荷兰计算机科学家狄克斯特拉于1959年提出的,因此又称为狄克斯特拉算法;它是从一个节点遍历其余各个节点的最短路径算法,解决的是有权图中最短路径问题。

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 7,892
精华内容 3,156
关键字:

无人驾驶路径规划

友情链接: OSD.rar