问题描述

在2.3的规划中，我们将相邻两点之间的路段拟合为一个五次多项式，并且计算出该路段的代价，通过动态规划的方法求解出了从规划起点到达最后一层节点的最短路径，本质上是得到了粗略的路径解，即完成了遇到障碍物是左绕还是右绕的决策。

我们在动态规划中得到了图中红色的路径，路径由一系列的稠密的离散点构成

我们要依据路径坐标信息、障碍物形状信息、道路边界信息生成图示红色的凸空间。

生成凸空间

我们需要计算的凸空间$ConvexS$定义如下：

即我们需要确定所有的路径点在横向空间上的最大$l$值和最小$l$值。

初始化道路边界

简单考虑单车道，考虑第$i$个轨迹点处的上下道路边界的$l$值为$l^{i}_{t}、l^{i}_{b}$

剥离障碍物占据的空间

伪代码逻辑：

简单考虑障碍物为矩形

初始化哈希表：
l_upper_bound={},l_lower_bound={}
For i in [1,n]:
    l_upper_bound[i] = l_t(i) # l_t(i)表示第i个轨迹点的道路上界l值
    l_lower_bound[i] = l_b(i) # l_b(i)表示第i个轨迹点的道路下界l值

遍历障碍物：
    对于当前第i个障碍物：
        计算障碍物占据的的s坐标范围obs_s_range
        搜索在obs_s_range范围内的轨迹点坐标
        判断轨迹是左绕还是右绕：
        若：左绕：
            l_lower_bound[i] = max(l_lower_bound[i], 障碍物空间的最大l坐标)
        若：右绕： 
            l_upper_bound[i] = min(l_upper_bound[i], 障碍物空间的最小l坐标)

生成凸空间代码实现

import numpy as np

UPPER_BOUND = 6.0
LOWER_BOUND = -6.0


def generate_convex_space(dp_path_s_set=None,
                          dp_path_l_set=None,
                          obs_cen_s_set=None,
                          obs_cen_l_set=None,
                          obs_length_set=None,
                          obs_width_set=None):
    """
    基于动态规划的粗解以及障碍物信息得到sl空间下的凸空间
    :param dp_path_s_set: 动态规划路径s坐标序列
    :param dp_path_l_set: 动态规划路径l坐标序列
    :param obs_cen_s_set: 障碍物中心s坐标序列
    :param obs_cen_l_set: 障碍物中心l坐标序列
    :param obs_length_set: 障碍物长度序列
    :param obs_width_set: 障碍物宽度序列
    :return:
    """
    # 初始化边界
    l_lower_bound = np.array([LOWER_BOUND] * len(dp_path_s_set))  # 凸空间下界
    l_upper_bound = np.array([UPPER_BOUND] * len(dp_path_s_set))   # 凸空间上界
 
    # 遍历每一个障碍物
    for obs_index in range(0, len(obs_cen_s_set)):

        # 找到在s轴上离障碍起点\中心点\终点最近的dp轨迹序列
        start_index, cen_index, end_index = find_obs_nearest_s(obs_cen_s=obs_cen_s_set[obs_index],
                                                               obs_length=obs_length_set[obs_index],
                                                               dp_path_s_set=dp_path_s_set)
        print(f'障碍物起点、中心、终点：'
              f'{(dp_path_s_set[start_index])}, {dp_path_s_set[cen_index]},{dp_path_s_set[end_index]}')

        # 只要返回了None的索引值, 即认为该障碍物不在规划的范围内
        if start_index is None or cen_index is None or end_index is None:
            continue
        else:
            # 判断是左绕还是右绕, 依据终点s对应的dp轨迹l值和障碍物中心l值作比较
            if dp_path_l_set[cen_index] < obs_cen_l_set[obs_index]:
                print('右绕')
                # 右绕, 凸空间的上界值取障碍物l边界值的最小值
                for j in range(start_index, end_index + 1):
                    l_upper_bound[j] = min(l_upper_bound[j], obs_cen_l_set[obs_index] - obs_width_set[obs_index] / 2)
            else:
                # 左绕, 凸空间的下界值取障碍物l边界值的最大值
                print('左绕')
                for j in range(start_index, end_index + 1):
                    print(obs_cen_l_set[obs_index], obs_width_set[obs_index] / 2)
                    print(l_lower_bound[j], obs_cen_l_set[obs_index] + obs_width_set[obs_index] / 2)
                    l_lower_bound[j] = max(l_lower_bound[j], obs_cen_l_set[obs_index] + obs_width_set[obs_index] / 2)
                    print(l_lower_bound[j])
    return l_upper_bound, l_lower_bound


# 应该传入障碍物的形心, 长度和宽度, 返回start_index, cen_index,  end_index
def find_obs_nearest_s(obs_cen_s=None, obs_length=None,
                       dp_path_s_set=None):
    """
    依据障碍物的几何信息搜寻障碍物起点、中心点、终点在dp_path中的匹配索引
    :param obs_cen_s:
    :param obs_length:
    :param dp_path_s_set:
    :return:
    """
    # 如果障碍物在规划起点的后面, 则不识别该障碍物
    if dp_path_s_set[0] > obs_cen_s + obs_length / 2:
        return None, None, None

    # 如果障碍物在规划路径末端的前方较远位置, 则不识别该障碍物
    elif dp_path_s_set[-1] < obs_cen_s - obs_length / 2:
        return None, None, None
    else:
        # 找障碍物起点s坐标的匹配的dp索引
        start_index = find_nearest_s(search_s=obs_cen_s - obs_length / 2, dp_path_s_set=dp_path_s_set)

        # 找障碍物中心点s坐标的匹配的dp索引
        cen_index = find_nearest_s(search_s=obs_cen_s, dp_path_s_set=dp_path_s_set)

        # 找障碍物终点s坐标的匹配的dp索引
        end_index = find_nearest_s(search_s=obs_cen_s + obs_length / 2, dp_path_s_set=dp_path_s_set)

        # 偏保守的估计, 起点匹配索引 - 1, 终点匹配索引 + 1
        if start_index > 0:
            start_index -= 1
        if end_index < len(dp_path_s_set) - 1:
            end_index += 1
        return start_index, cen_index, end_index


def find_nearest_s(search_s=None, dp_path_s_set=None):
    """
    依据单个s坐标在dp_path中搜寻匹配点
    :param search_s:
    :param dp_path_s_set:
    :return:
    """
    index_res = None

    if dp_path_s_set[0] >= search_s:
        # 如果dp_path的第一个s就比search_s大, 则返回0
        return 0
    elif dp_path_s_set[-1] <= search_s:
        # 如果dp_path的最后一个s都比search_s小, 则返回len(dp_path_s_set) - 1
        return len(dp_path_s_set) - 1
    else:
        # search_s在dp的s范围内
        for i in range(0, len(dp_path_s_set)):
            if dp_path_s_set[i] < search_s:
                pass
            else:
                index_res = i
                break
        # 需要比较index_res对应的s和index_res - 1对应的s, 谁离search_s更近一点
        if np.abs(dp_path_s_set[index_res] - search_s) < np.abs(dp_path_s_set[index_res - 1] - search_s):
            return index_res
        else:
            return index_res - 1

参考来源

space.bilibili.com/287989852