（八）锥桶赛道路径规划

shiyueyueya

已于 2023-03-08 16:21:39 修改

阅读量1.3k

点赞数 2

分类专栏： jetson agx Xavier 文章标签：自动驾驶目标检测 Powered by 金山文档

于 2023-03-08 12:41:18 首次发布

本文链接：https://blog.csdn.net/qq_53086461/article/details/129388104

版权

jetson agx Xavier 专栏收录该内容

9 篇文章 5 订阅

订阅专栏

1、参考资料

三角剖分：https://blogs.mathworks.com/student-lounge/2022/10/03/path-planning-for-formula-student-driverless-cars-using-delaunay-triangulation/

贝塞尔曲线https://muzing.top/posts/6610880/

2、任务需求

在赛道进行路径规划，赛道两侧分别是红蓝锥桶，这里采用的参考资料2中的算法。

PanoSim仿真中采用「目标级雷达」，直接返回目标相对于自己的相对位置，但是相机下的位置不是真实世界下的位置

同样的我们也缺乏真实的路宽，在一侧没有的时候不能移动d/2

因此如果建立在拥有真实的坐标基础上，同时车身自带定位与建图的情况下，可以直接套用算法，目前采用的方案仅仅针对两侧都有锥桶，通过分别计算两侧贝塞尔曲线然后相加。

3、测试思路

原作者文档中解释非常清楚，这里仅仅做一些小小改动

3.1 贝塞尔曲线

    red = get_traffic_cone_matrix(np.array(test_data[i]), 3)  # 左侧锥桶位置矩阵
    blue = get_traffic_cone_matrix(np.array(test_data[i]), 2)  # 右侧锥桶位置矩阵
    red_bezier = get_bezier_curve(red,25)
    blue_bezier = get_bezier_curve(blue, 25)
    #local_path = local_path_fitting(red, blue, translation_dis=1.5)  # 规划出的局部路径矩阵


    # 左上角为原点
    plt.xlim(0, 1920)
    plt.ylim(0, 1080)
    plt.gca().invert_yaxis()
    plt.plot(red[:, 0], red[:, 1], "o-", color="r")
    plt.plot(blue[:, 0], blue[:, 1], "o-", color="b")
    #plt.plot(local_path[:, 1] * -1, local_path[:, 0], color="k", linestyle="--")
    x = (red_bezier + blue_bezier[::-1])/2

这里采用了，左右两侧分别计算25个点的平均值，可以理解为中线。由于锥桶的顺序至关重要，原作者做了x方向排序，我们相加的取平均的的时候需要对一侧取反 x = (red_bezier + blue_bezier[::-1])/2，这样才能保证结果正确。

为了和原始图片保持一致，这里更改了画图原点，xy的方向。

3.2过滤点

没有采用原作者的fiter，因为相机存在畸变，实际测试中效果表现不好。因此自己尝试了两种方案。

3.2.1过滤置信度低的点

首先通过置信度进行筛选锥桶，可以看到原图的左边有很多蓝色锥桶，但是置信度低直接使用会影响结果。

将上图的坐标点抽出来单独画图，可以看出这样是无法进行规划的。

这里对yolov5 detect.py做一点小改动，在write results的时候，原来是保存归一化后的结果符合yolo数据格式。我们虚妄同时保存锥桶框最下沿中点的位置，x1,y1,x2,y2是框的两个对角点。

xx=(x1+x2)/2

yy=max(y1,y2);

因此又新开了一个txt_path2的txt，保存我们需要的最下沿中点的x,y坐标。最后将保存的点放到作者的test_data.py下

#               #Write results
                for *xyxy, conf, cls in reversed(det):
                    if save_txt:  # Write to file
                        xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist()  # normalized xywh
                        line = (cls, *xywh, conf) if save_conf else (cls, *xywh)  # label format
                        with open(f'{txt_path}.txt', 'a') as f:
                            f.write(('%g ' * len(line)).rstrip() % line + '\n')
                        x1,y1,x2,y2 = xyxy
                        xx=(x1+x2)/2
                        yy=max(y1,y2);
                        line2 = (cls,xx,yy)
                        txt_path2 = txt_path + "_"
                        with open(f'{txt_path2}.txt', 'a') as f2:
                            f2.write(('%g ' * len(line2)).rstrip() % line2 + '\n')

3.2.2过滤远的的点

由于左边第一列红色实际上不是当前赛道，是U型弯回来的时候左侧锥桶，因此采用距离的方法，筛掉，当然这个距离目前是自己定的，因为看起来想个圆形，所以采用（960，1300）为圆心1400000为距离剔除远的点。

traffic_cone = traffic_cone[np.lexsort(traffic_cone[:, ::-1].T)]  # 按x坐标排序
    cone= []
    for i in range(n):
        x=traffic_cone[i]
        xx=(x[0] - 960) ** 2 + (x[1] - 1600) ** 2
        if (x[0]-960)**2 + (x[1]-1300)**2 <= 1400000:
            cone.append(x)
    #clean_traffic_cone = filter_traffic_cone(traffic_cone)  # 剔除异常的锥桶

    return np.array(cone)

效果如下