Coursera Self Driving Car Part4 运动规划FinalProject原理及关键代码解析

本博客针对Coursera上无人驾驶课程的第四章运动规划的课后大作业-设计一个运动规划器的源码进行解析。(2021.11.20更新未完待续，最近工作较忙更新较慢，请见谅~)

代码解析主要通过功能简述和代码详细注释来说明，对于coursera官网给出的软件的架构，各个模块的构建，能学到很多东西。

相关链接：

课程作业运行效果视频链接

Project题目介绍

Course4FinalProject源码链接

Coursera课程视频链接

1.代码架构简述

在Coursera官网下载Part4运动规划的课后最终项目题目文件"Coursera4FinalProject"可以看到如下文件：

其中“ behavioural_planner.py ”，“ collision_checker.py ”，“ local_planner.py ”，“ path_optimizer.py ”和“ velocity_planner.py ”类文件需要我们填充关键内容来实现运动规划器。

整个项目的软件架构很复杂，这里只放项目整体架构简图：

2. 代码解析

2.1 behavioural_planner.py

大纲

该py程序主要是实现了BehaviouralPlanner类，该类主要是实现一个行为规划的状态机，其主要函数是transition_state()。其他函数都是为了实现状态机的一些辅助函数。_init_是类成员的初始化函数，get_goal_index()，get_closet_index()是获取目标点和最近点在待跟踪路径waypoints中的索引。check_for_stop_signs()，check_for_lead_vehicle()是检查待跟踪轨迹上是否有stopsign标志以及检查前车的状态来判断自车是否需要进入跟车模式。

这里的要求是在stopsign前要停3秒再出发，通过状态机里进入停车状态经历的周期数是否达到STOP_COUNTS来判断。

# 行为规划器状态机有3个状态
FOLLOW_LANE = 0
DECELERATE_TO_STOP = 1
STAY_STOPPED = 2
# 速度小于0.02m/s时认为停住了
STOP_THRESHOLD = 0.02
# 在stopsign标志前需要停得周期数
STOP_COUNTS = 10

该程序定义了一个BehaviouralPlanner类来实现行为规划状态机：

2.1.1 transition_state 状态转移函数

def transition_state(self, waypoints, ego_state, closed_loop_speed):
        """处理状态转移并计算目标状态  
        
        输入参数:
            waypoints: 当前要跟踪得航点集 (大地坐标). 
                位置和速度单位为m,m/s
                (包含在每个位置时的参考速度)
                数据格式: [[x0, y0, v0],
                         [x1, y1, v1],
                         ...
                         [xn, yn, vn]]
                例如:
                    waypoints[2][1]: 
                    返回第3个点的y坐标

                    waypoints[5]:
                    返回第6个点的坐标及速度[x5,y5,v5]
            ego_state: 本车状态 (大地坐标)
                数据格式: [ego_x, ego_y, ego_yaw, ego_open_loop_speed]
                    ego_x and ego_y     : 位置 (m)
                    ego_yaw             : 与x轴成的角度，航向角 [-pi to pi]
                    ego_open_loop_speed : 开环速度，即参考速度 (m/s)
            closed_loop_speed: 闭环速度，当前车辆的实际速度 (m/s)
        要设定的变量:
            self._goal_index: 车辆目标点在waypoints中的索引
            self._goal_state: 车辆目标状态 (大地坐标)，就是目标索引在waypoints中对应的状态
                数据格式: [x_goal, y_goal, v_goal]
            self._state: 当前车辆的状态，就是前面状态机里的状态
                车辆状态有以下三种: 
                    FOLLOW_LANE         : 车道跟随模式.
                    DECELERATE_TO_STOP  : 减速到停模式.
                    STAY_STOPPED        : 保持停止模式.
            self._stop_count: 计数器，记录车辆进入保持停止模式后经历的周期数
        有用的常数:
            STOP_THRESHOLD  : 速度阈值，当车速小于此阈值，认为车辆已经静止
            STOP_COUNTS     : 国外通常要求在stopsign前停留一小会儿，观察情况后通过，这个计数值为车辆需要在stopsign前停留的周期数，停满后退出保持停止模式。
        """
        # 在车道跟随状态时，持续跟踪waypoints里前视距离lookahead范围内的目标点。
        # 然后检查到目标点的路径是否与任意一个停止线相交，如果是，确保目标状态会强制车辆停在
        # 停止线前。
        # 用get_closest_index(), get_goal_index()，check_for_stop_signs() 三个辅助函数
        # 分别为获取waypoints中离车辆当前位置最近点，目标点，以及检查是否有stopsign
        if self._state == FOLLOW_LANE:
            # 首先，找到待跟踪waypoints中离车当前位置最近点索引及最近的距离
            closest_len, closest_index = get_closest_index(waypoints, ego_state)

            # 接着，在待跟踪的waypoints中找距当前车距离刚好不超过前视距离lookahead的目标点索引
            goal_index = self.get_goal_index(waypoints, ego_state, closest_len, closest_index)
            
            # 最后检查waypoints中当前车最近点和目标点之间是否有stopsigns标志
            # 并据此更新目标状态。
            goal_index, stop_sign_found = self.check_for_stop_signs(waypoints, closest_index, goal_index)
            self._goal_index = goal_index
            self._goal_state = waypoints[self._goal_index]
            
            # 如果最近点和目标点间发现了stopsign标志，将目标状态速度设置为0
            # 并将车辆状态转换为减速到停模式
            if stop_sign_found:
                self._state = DECELERATE_TO_STOP
                waypoints[self._goal_index][2] = 0
                self._goal_state = waypoints[self._goal_index]
        

        # 在减速到停模式，检查是否车到达了一个完全的停止，和设定的速度阈值进行比较，
        # 用车辆的闭环速度即实际速度来检查，如果车速比这个很小的速度阈值要小，则
        # 状态机转换为保持停止模式
        elif self._state == DECELERATE_TO_STOP:
            if closed_loop_speed < STOP_THRESHOLD:
                self._state = STAY_STOPPED


        
        # 在保持停止模式，检查是否车辆已经停够了设定的停车周期数，如果是，车辆可以
        # 离开stopsign并且车辆状态机转换为车道跟随状态。
        elif self._state == STAY_STOPPED:
            # 车辆在stopsign前已经停够了设定的周期数，则允许车辆离开
            # 一旦车辆离开stopsign,就进入车道跟随模式，需要更新目标点
            if self._stop_count == STOP_COUNTS:
                closest_len, closest_index = get_closest_index(waypoints, ego_state)
                goal_index = self.get_goal_index(waypoints, ego_state, closest_len, closest_index)
                
                # 车辆已经停够了周期数，所以新的目标点索引就又与停止线无关了，继续用
                # 前视距离在waypoints中找目标点
                goal_index, stop_sign_found = self.check_for_stop_signs(waypoints, closest_index, goal_index)
                self._goal_index = goal_index
                self._goal_state = waypoints[self._goal_index]
                
                # 如果stop sign不再在车辆短期的路径上(最近点和目标点之间)，可以将车辆状态机转换
                # 为车道跟随模式
                if not stop_sign_found:
                    self._state = FOLLOW_LANE



            # 如果在保持停止模式下，停车的周期数不够，继续保持停止，但计数器需要累加
            else:
                self._stop_count += 1

        # 若车辆状态机模式不在上面三种模式里，则返回错误信息
        else:
            raise ValueError('Invalid state value.')

2.1.2 _init_类初始化函数 

# BehaviouralPlanner类的初始化函数
# 类实例化时需输入前视距离，stopsign位置信息，探测前车的距离范围
def __init__(self, lookahead, stopsign_fences, lead_vehicle_lookahead):
        # 前视距离
        self._lookahead                     = lookahead
        # stopsign在大地坐标系下的坐标
        self._stopsign_fences               = stopsign_fences
        # 探测是否存在前车的距离范围，超过此距离的前车不纳入考虑
        self._follow_lead_vehicle_lookahead = lead_vehicle_lookahead
        # 初始化车辆的状态为车道跟随
        self._state                         = FOLLOW_LANE
        # 车辆不在跟车模式
        self._follow_lead_vehicle           = False
        # 车辆的目标点[0,0,0]
        self._goal_state                    = [0.0, 0.0, 0.0]
        # 车辆的目标点索引初始化为0
        self._goal_index                    = 0
        # 车辆进入保持停止模式所经历的周期数初始化为0
        self._stop_count                    = 0

2.1.3 设定前视距离函数set_lookahead 

# 类的成员函数设置前视距离为类实例化时输入的参数lookahead
def set_lookahead(self, lookahead):
        self._lookahead = lookahead

2.1.4 获取目标点函数get_goal_index

    def get_goal_index(self, waypoints, ego_state, closest_len, closest_index):
        """获取车辆的目标点索引
        
        设定待跟踪路径点waypoints中，距当前车累计距离(车到路径上最近点距离+最近点到下一点+下一点到下下一点+...直到目标点累加距离)最早超过前视距离的点作为目标点。

        函数的输入参数:
            waypoints: 变量含义及数据结构参见2.1.1
            ego_state: 自车的状态(全局坐标系)
                数据格式: [ego_x, ego_y, ego_yaw, ego_open_loop_speed]
                    ego_x and ego_y     : 位置(m)
                    ego_yaw             : 航向角[-pi to pi]
                    ego_open_loop_speed : 开环速度(m/s)
            closest_len: 当前车到待跟踪路径waypoints中最近点的距离
            closest_index: 最近点在waypoints中的索引
        函数的返回值:
            wp_index: 车辆目标点在待跟踪路径waypoints中的路径
        """
        # 找到路径上前视距离范围内最远的点
        # 将当前车到路径上最近点距离纳入考虑
        arc_length = closest_len    # 初始化到目标点累计距离为车到最近点距离
        wp_index = closest_index    # 初始化目标点索引为最近点索引
        
        # 如果到最近距离已经大于前视距离，则最近点索引即为目标点索引，返回之
        if arc_length > self._lookahead:
            return wp_index

        # 如果到了waypoints中的最后一个点，累计距离仍没有超过前视距离，则最后一个点索引即为
        # 目标点索引
        if wp_index == len(waypoints) - 1:
            return wp_index

        # 不属于上述两种情况，到waypoints中wp_index点累计距离既小于前视距离，又没到waypoints最后一个点
        # 累计距离就加上wp_index到下一点的距离，同时wp_index+1
        while wp_index < len(waypoints) - 1:
            x_diff = waypoints[wp_index][0] - waypoints[wp_index + 1][0]
            y_diff = waypoints[wp_index][1] - waypoints[wp_index + 1][1]
            dist = np.linalg.norm(np.array([x_diff, y_diff]))
            arc_length += dist
            wp_index += 1
            if arc_length >= self._lookahead:
                break
        # 到wp_index的累计距离为最后一个点或者超过前视距离了，则跳出while循环，返回当前的wp_index即为目标点索引

        return wp_index

2.1.5 检查路径上是否有停车标志check_for_stop_signs函数

    # 检查待跟踪轨迹waypoints是否与stopsign的停止线相交，若是则更新目标状态为该标志处
    def check_for_stop_signs(self, waypoints, closest_index, goal_index):
        """
        返回一个新的目标索引以及是否与stopsign停止线相交的布尔类型flag标志位。
        函数输入:
            waypoints: 参数含义参见2.1.1
                format: [[x0, y0, v0],
                         [x1, y1, v1],
                         ...
                         [xn, yn, vn]]

                closest_index: 最近点索引
                goal_index (current): 目标点索引
        函数输出:
            [goal_index (updated), stop_sign_found]: 
                goal_index (updated): 目标点在waypoints中的索引
                stop_sign_found: stopsign标志是否被发现的布尔标志位，True or False
        """
        for i in range(closest_index, goal_index):
            # 遍历最近点到目标点之间的每一个点，这个点到下一个待跟踪点是否与停止线的两个端        
            # 点相交
            intersect_flag = False
            # 对于每一个点，又去遍历所有的stopsign，本项目中只有1个
            for stopsign_fence in self._stopsign_fences: 
                # wp_1,wp_2为waypoints中第i个点和第i+1个点
                wp_1   = np.array(waypoints[i][0:2])  #python里数组a[i:j]表示a[i]到a[j-1]
                wp_2   = np.array(waypoints[i+1][0:2])
                # s_1,s_2为stopsign的停止线的两个端点坐标x0,y0 x1,y1
                s_1    = np.array(stopsign_fence[0:2])
                s_2    = np.array(stopsign_fence[2:4])
                # 调用判断两线段是否相交函数，输入是两线段的两端点，返回布尔标志位    
                intersect_flag = judge_segments_intersect(wp_1,wp_2,s_1,s_2);

                # 如果待跟踪轨迹与停止线相交，更新目标状态为停在停止线前，这个相交flag
                # 会在transition_state函数里状态机里一旦检测到会把目标点速度置0
                if intersect_flag:
                    goal_index = i
                    return goal_index, True

        return goal_index, False

2.1.6 检查两线段是否相交函数 judge_segments_intersect()

        实现原理参见博客 判断两线段相交，代码如下：

# judge two linesegments ab,cd intersect?
def judge_segments_intersect(a,b,c,d):
    # 快速排斥
    if max(c[0],d[0])<min(a[0],b[0]) or max(a[0],b[0])<min(c[0],d[0]) or \
        max(c[1],d[1])<min(a[1],b[1]) or max(a[1],b[1])<min(c[1],d[1]):
        return False
    # 跨立实验
    if np.cross(a-d,c-d)*np.cross(b-d,c-d)>0 or np.cross(d-b,a-b)*np.cross(c-b,a-b)>0:
        return False
    return True

2.1.7 判断自车是否需进入跟车模式函数 check_for_lead_vehicle()

    def check_for_lead_vehicle(self, ego_state, lead_car_position):
        """检查前车是否足够接近自车，是的话自车应当进入跟车模式。
        函数输入:
            ego_state: 自车状态. (大地坐标)
                数据格式: [ego_x, ego_y, ego_yaw, ego_open_loop_speed]
                    ego_x and ego_y     : 位置坐标 (m)
                    ego_yaw             : 航向角 [-pi to pi]
                    ego_open_loop_speed : 开环速度即参考速度 (m/s)
            lead_car_position: 前车在大地坐标系下的xy坐标
        
        函数输出:
            self._follow_lead_vehicle: 布尔标志位，自车是否应该进入跟随前车的模式？即跟前        
                                     车保持一定距离而放弃自车原先的速度规划，True or False
        """
        # 检查前车相对自车的位置和角度，来判断是否进入跟车模式
        # 检查前车是否在自车前方需要注意的范围内
        if not self._follow_lead_vehicle: #如果自车不在跟随前车模式时

            # 计算前车与自车xy坐标的delta_x, delta_y以及距离
            lead_car_delta_vector = [lead_car_position[0] - ego_state[0], 
                                     lead_car_position[1] - ego_state[1]]
            lead_car_distance = np.linalg.norm(lead_car_delta_vector)
            
            # 若前车在自车设定视距范围内并且前车与自车的连线与自车航向所称角度小于45度
            # 则进入跟车模式，这个45度角度的约束判断是否在同一车道，不在同一车道就不用考虑
            if (lead_car_distance < self._follow_lead_vehicle_lookahead + 15) and \
                    (ego_state[2] - math.atan2(lead_car_delta_vector[1],lead_car_delta_vector[0])) <= 3.1415926/4:
                self._follow_lead_vehicle = True
                return
            return

        else:  # 若自车处于跟车模式

            # 计算前车与自车xy坐标的delta_x, delta_y以及距离
            lead_car_delta_vector = [lead_car_position[0] - ego_state[0], 
                                     lead_car_position[1] - ego_state[1]]
            lead_car_distance = np.linalg.norm(lead_car_delta_vector)
            
            # 自车处于跟车模式时，若到前车距离大于设定视距范围，或自车航向与前车所成角
            # 大于45度，就从跟车模式跳转到非跟车模式。
            if (lead_car_distance > self._follow_lead_vehicle_lookahead + 15) or \
                    (ego_state[2] - math.atan2(lead_car_delta_vector[1],lead_car_delta_vector[0])) > 3.1415926/4:
                self._follow_lead_vehicle = False
                return
            return

2.2 collision_checker.py

py程序大纲

该py程序主要是实现了一个CollisionChecker类，该类主要通过函数collision_check()来进行碰撞检测，通过函数select_best_path_index()来选择一条无碰撞最优路径。_init_函数实现该类成员的初始化。

2.2.1 _init_类初始化函数

def __init__(self, circle_offsets, circle_radii, weight):
        self._circle_offsets = circle_offsets
        self._circle_radii   = circle_radii
        self._weight         = weight

 参考mathwork官网inflationCollisionChecker，进行CollisionChecker类成员说明：

这里碰撞检测的原理就是将车辆所占面积等效为几个圆心在车辆中轴线上具有一定半径的圆来近似，如下图，将车辆等效为三个圆构成，以车辆后轴中心为0的话，三个圆的圆心分别为0，1.5，3(中轴线上圆心到车辆后轴中心的距离，车辆后轴中心即为车体系的原点)，3个圆的半径分别为1.3，1.3，1.3

则称circle_offsets 为圆心位置[0,1.5,3]

则circle_radii为各圆半径[1.3,1.3,1.3]

下图是Mathwork的示意图和这个项目的圆心位置定义略有不同（按圆心位置在中轴线上的比例定义），不过方法是一样的，放在这里便于理解。

weight则是到碰撞路径距离的权重，在路径选择时应用。

self指代该类对象。表明是该类的成员函数，调用时无需加上此参数，仅函数定义时加上。

将车辆等效成圆进行碰撞检测的好处显而易见，只需判断障碍物点到圆心的距离是否大于半径即可，大于则无碰撞。

2.2.2 collision_check 碰撞检测函数

    # 考虑一系列的路径和障碍物，并且返回一个布尔数组，布尔数组里存放了每条
    # 路径是否无碰撞的标志位。
    def collision_check(self, paths, obstacles):
        """返回一个布尔数组，存放每条路径是否无碰撞

        函数输入:
            paths: 全局坐标系下的一系列路径列表list.
                单个路径的数据格式如下：
                    [x_points, y_points, t_points]:
                        x_points: 路径上所有点x坐标构成的list，单位m
                        y_points: 路径上所有点y坐标构成的list，单位m
                        t_points: 路径上所有点航向角yaw构成的list，单位rad
                    数据访问示例，例如访问第i+1条路径第j个点的信息：
                        paths[i][2][j]
            obstacles: 障碍物所有边界点坐标[x,y]构成的list，坐标都是在全局坐标系下
                
                数据格式: [[x0, y0],
                         [x1, y1],
                         ...,
                         [xn, yn]]
                         n是障碍物边界点的数量，单位是[m,m]
        函数输出:
            collision_check_array: 一个布尔数组，表明输入的一系列路径是否是无碰撞(true)，            
                                    有碰撞(false).该布尔数组第i个索引对应着第i条路径。
        """
        # 初始化待输出的布尔数组，长度和路径数相同，数据类型为布尔，初始都为0 false
        collision_check_array = np.zeros(len(paths), dtype=bool)
        # 遍历输入的每条路径
        for i in range(len(paths)):
            # 初始定义第i+1条路径是免碰撞的，是paths路径集合里的第i+1条(i从0开始)
            collision_free = True
            path = paths[i]

            # 遍历第i+1条路径上的所有点，j从0开始到len-1, 代表第j+1个点
            for j in range(len(path[0])):
                # 计算沿着这些路径点圆的位置
                # 这些圆代表对该车辆碰撞边界近似估计
                # 用来对路径上的障碍物进行车辆的潜在的碰撞检查

                # 输入参数车辆边界近似圆心位置circle offsets 由
                # collision_checker类成员self._circle_offsets给出
                # The circle offsets need to placed at each point along the path,
                # with the offset rotated by the yaw of the vehicle.
                # Each path is of the form [[x_values], [y_values],
                # [theta_values]], where each of x_values, y_values, and
                # theta_values are in sequential order.

                # Thus, we need to compute:
                # circle_x = point_x + circle_offset*cos(yaw)
                # circle_y = point_y circle_offset*sin(yaw)
                # for each point along the path.
                # point_x is given by path[0][j], and point _y is given by
                # path[1][j]. 
                circle_locations = np.zeros((len(self._circle_offsets), 2))

                # TODO: INSERT YOUR CODE BETWEEN THE DASHED LINES
                # --------------------------------------------------------------
                for c in range(len(self._circle_offsets)):
                    circle_locations[c, 0] = path[0][j] + self._circle_offsets[c] * cos(path[2][j])
                    circle_locations[c, 1] = path[1][j] + self._circle_offsets[c] * sin(path[2][j])

                # --------------------------------------------------------------

                # Assumes each obstacle is approximated by a collection of
                # points of the form [x, y].
                # Here, we will iterate through the obstacle points, and check
                # if any of the obstacle points lies within any of our circles.
                # If so, then the path will collide with an obstacle and
                # the collision_free flag should be set to false for this flag
                for k in range(len(obstacles)):
                    collision_dists = \
                        scipy.spatial.distance.cdist(obstacles[k], 
                                                     circle_locations)
                    collision_dists = np.subtract(collision_dists, 
                                                  self._circle_radii)
                    collision_free = collision_free and \
                                     not np.any(collision_dists < 0)

                    if not collision_free:
                        break
                if not collision_free:
                    break

            collision_check_array[i] = collision_free

        return collision_check_array