皮皮网

1.Apollo EM中path_assesment_task相关细节的讨论

Apollo EM中path_assesment_task相关细节的讨论

       网上已有关于本task流程的详细注释，本文主要探讨Apollo EM中path_assesment task的内部算法细节。

       首先，我们来看SetPathPointType函数。函数中，ego_center_shift_distance代表车体几何中心距离后轴中心的php免费源码大全距离。初始化过程中，以车体后轴中心点为基准获取整个车体Box。ego_box所调用的Shift函数将后轴中心点前移至车体几何中心点并计算box四个角的位置。这样做的原因是，Control使用后轴中心点跟踪每一个pathpoint，而本车SLboundary依据其几何中心点在参考线上进行投影，因此这一步转化是短域名转换源码必不可少的。

       Shift函数内所调用的InitCorners()如上图所示，其中cos_heading,sin_heading_,half_length_,half_width_,center_,是在Box2d初始化时进行赋值。简单用图示介绍一下含义：X-Y用于表示地图坐标系，x-y用于表示将地图坐标系平移至车体几何中心点之后的坐标系，[公式] 用于表示heading。特别解释一下：此处的使用 [公式] 来表示长度half_width x[公式]。

       获取到本车box的四个角的坐标之后，再将依靠参考线将box转化到SL坐标系下的boundary。之后遍历SL坐标系下每个pathpoint，计算自车位于每个pathpoint的时候所对应的车体的边界。将边界与车道线比较，判断每个pathpoint的92源码域名授权类型（在lane_borrow/lane_change决策下是在旁车道还是本车道）。此处不粘贴源码，直接用图示进行说明各种情况。

       需要注意：由于要在统一的坐标系下进行比较，所以，采用-right_width。所以对应上图的情况，即处于车道边界之外还没换道的情况：start_l>lane_left_width || end_1<-lane_right_width此时type为IN_LANE。对应上图情况，即严格处于本车边界之内的车辆，需要考虑将车道边界添加buffer：所以本车实际处于虚线之内，此时的type为IN_LANE，表示已经换道完成。源码吧 狮子鱼其它状态下为lane_change的过渡状态，type为OUT_ON_FORWARD_LANE。

       需要注意的是：由于借道其实是拐入别的车道一点点之后再拐回来，所以需要添加动态的迟滞边界in_and_out_lane_hysteresis_buffer，来对pathpoint进行严格地区分。类似的做法在量产ADAS功能中也用到，主要用于对前方动态Target的识别，目的是为了防止障碍车辆沿车道线来回画龙或压线行驶的情况。扯远了~

       如上述，因为借道只是占据旁车道一点点，所以判断条件与lane_change略有不同，此处的米哈游源码判断条件使用end_l与lane_left_width相比较，start_l与-lane_right_width相比较。

       如上图所示，当前一个路径点在车道内（is_prev_point_out_lane = false），即绿点所在位置，则判断lane_borrow的条件：此时，in_and_out_lane_hysteresis_buffer = 0.2；当前一个路径点在车道内（is_prev_point_out_lane = true），即红点所在位置，此时，in_and_out_lane_hysteresis_buffer = 0；

       接下来讨论path与静态障碍物collision的问题：具体流程图参见下图：IsCollidingWithStaticObstacles中流程概述：过滤掉虚拟障碍物，高速运动障碍物等，按照固定距离的pathpoint建立VehBox，所有点转化为Frenet坐标系，调用函数IsPointIn检验所有点是否处于障碍物的内部。

       下面开始详细讲解IsPointIn函数：首先判断本车的Corner点是否在障碍物边界上，如果不在，则进行下一步取Corner点在与轴方向异侧的两个点，分别于Corner点构成向量，之后做叉乘，叉乘结果若大于0，则比较两个点的y值，若y值结果小于0，则看向量正旋转到目标向量的实际角度是否小于度，若小于，则c由零变为1；若y值结果为大于，则看向量正旋转到目标的实际角度是否大于度，若大于，则c由零变为1。同理再构建另外两个向量，若与上述情况相同，则c由1变为2。最后判断C为奇数还是偶数，如果为奇数，则返回true，表示Corner点处于Obstacle内部。如果为偶数，则表示Corner点在Obstacle外部。

       下面举个栗子：对于左图A点的这种情况，0点与3点在Y轴方向上位于A点异侧，0.y<3.y，side>0（即比较向量A-0正旋转到目标向量A-3是否小于度），实际大于度（蓝色箭头），结果为否，则c值仍为0；2点与1点在Y轴方向上位于A点异侧，side<0（即比较向量A-2正旋转到目标向量A-1是否大于度），实际大于度（红色箭头），结果为是，则c值变为1。最终c&1为奇数，表示A点在obstacle之内。

       对于右图A点情况，0点与3点在Y轴方向上位于A点异侧，0.y<3.y，side>0（即比较向量A-0正旋转到目标向量A-3是否小于度），实际小于度（红色箭头），结果为是，c值由0变为1；2点与1点在Y轴方向上位于A点异侧，side<0（即比较向量A-2正旋转到目标向量A-1是否大于度），实际大于度（蓝色箭头），结果为是，则c值变为2。最终c&1为偶数，表示A点在obstacle之外。

       正旋转相关的知识：叉乘几何含义：须注意：本算法所有的坐标系为上图所示，所以正旋转方向为X轴沿绿三色箭头转到Y轴。

       最后依旧借用M星云男神女神镇楼。。