CN114889642B - 一种规划控制系统 - Google Patents

Abstract

本发明实施例涉及一种规划控制系统，所述系统包括：预测模块、规划模块和控制模块；预测模块分别与感知系统和规划模块连接，用于接收感知系统发送的第一障碍物位姿集合，并进行障碍物轨迹预测处理生成第一预测轨迹组集合向规划模块发送；规划模块与控制模块连接，用于根据第一预测轨迹组集合进行障碍物预测轨迹融合处理，并进行车辆多轨迹预规划处理；控制模块与感知系统连接，用于从感知系统获取第二障碍物位姿集合向规划模块发送；规划模块还用于根据第二障碍物位姿集合进行规划轨迹查询处理生成第二规划轨迹τ^,向控制模块回发；控制模块还用于根据第二规划轨迹τ^,进行车辆轨迹跟踪与行驶控制。通过本发明可以提高对车辆行驶控制的安全保障。

Description

一种规划控制系统

技术领域

本发明涉及数据处理技术领域，特别涉及一种规划控制系统。

背景技术

车辆的自动驾驶系统中包括感知系统和规划控制系统。感知系统常规由多种传感器(诸如摄像头、雷达等)构成，感知系统主要用于对车辆周围的障碍物进行目标识别与跟踪，并根据跟踪结果输出障碍物的位姿序列。规划控制系统常规由预测模块、规划模块和控制模块构成；其中，预测模块基于感知系统采集的障碍物位姿信息对障碍物的未来轨迹进行预测，并将预测结果发送至规划模块；规划模块则基于接收到的障碍物预测轨迹，在当前行驶空间下对自车未来的多种行驶轨迹进行规划，并从得到的多个规划轨迹中选择概率最大的一个作为规划结果发送至控制模块；控制模块则以接收到的最大概率规划轨迹作为参考轨迹并对其进行轨迹跟踪，在轨迹跟踪过程中通过行驶控制也就是对自车的实时运动状态进行控制(诸如方向盘控制、油门/制动控制等)来使得实际运动轨迹最大限度地贴近参考轨迹。

通过上述描述不难看出常规的规划控制系统中，在每个规划-控制模块联动周期内由规划模块到控制模块的输出是一次性的单条规划轨迹输出关系。这种单向输出关系有个问题，那就是若在当次规划-控制模块联动周期的控制模块轨迹跟踪过程中周围障碍物的运动轨迹发生突变(例如，自车前方障碍物即前方车辆1的预测轨迹为一条匀速行驶轨迹，但前方车辆1突然刹车从而导致其实际运动轨迹产生突变)，则控制模块当前使用的参考轨迹会瞬间失效且无法立即获得新的规划轨迹进行补充。通过上述描述我还知道控制模块的行驶控制是基于当前有效参考轨迹完成的，若当前参考轨迹失效又不能得到及时补充则极有可能对车辆的行驶控制带来安全隐患。

发明内容

本发明的目的，就是针对现有技术的缺陷，提供一种规划控制系统，该系统包括：预测模块、规划模块和控制模块。本发明系统，对常规预测模块进行改进使之可以对各个障碍物的所有运动可能(包括突发的可能事件)的对应轨迹进行预测；并对常规规划模块进行改进，使之可通过创建融合轨迹组集合将未来时段所有障碍物的所有动作可能的预测轨迹都包括在内，使之可通过创建规划轨迹集合将未来时段所有可能场景下的自车规划轨迹都包括在内；并对常规规划模块与控制模块的互动关系进行改进，取消常规由规划模块到控制模块一次性设定轨迹的单向输出方式，转以控制模块基于实时障碍物轨迹向规划模块进行实时规划轨迹反查的处理方式来替代。通过本发明系统的技术改进，就可以解决在周围障碍物运动轨迹发生突变时控制模块丢失参考轨迹无法补充的问题，不但可以降低车辆行驶控制的安全隐患，还可以提高控制模块参考轨迹与实际场景的适配性，从而进一步提高车乘人员的舒适度。

为实现上述目的，本发明实施例提供了一种规划控制系统，所述系统包括：预测模块、规划模块和控制模块；所述系统与感知系统连接；

所述预测模块分别与所述感知系统和所述规划模块连接；所述预测模块用于接收所述感知系统发送的第一障碍物位姿集合；并根据所述第一障碍物位姿集合进行障碍物轨迹预测处理生成对应的第一预测轨迹组集合；并将所述第一预测轨迹组集合向所述规划模块发送；

所述规划模块与所述控制模块连接；所述规划模块用于接收所述第一预测轨迹组集合；并根据所述第一预测轨迹组集合进行障碍物预测轨迹融合处理生成对应的第一融合轨迹组集合并保存；并根据所述第一融合轨迹组集合进行车辆多轨迹预规划处理生成对应的第一规划轨迹集合并保存；

所述控制模块与所述感知系统连接；所述控制模块用于从所述感知系统获取第二障碍物位姿集合；并将所述第二障碍物位姿集合向所述规划模块发送；

所述规划模块还用于接收所述第二障碍物位姿集合；并根据所述第二障碍物位姿集合与保存的所述第一融合轨迹组集合和所述第一规划轨迹集合进行规划轨迹查询处理生成对应的第二规划轨迹τ^,；并将所述第二规划轨迹τ^,向所述控制模块回发；

所述控制模块还用于接收所述第二规划轨迹τ^,；并根据所述第二规划轨迹τ^,进行车辆轨迹跟踪与行驶控制。

优选的，所述感知系统用于对车辆周围所有障碍物的位姿信息进行采集；并将指定时长内采集到的各个障碍物的位姿序列组成所述第一障碍物位姿集合；并将所述第一障碍物位姿集合向所述预测模块发送。

优选的，所述第一障碍物位姿集合包括多个第一位姿序列M_i；各个所述第一位姿序列M_i的序列时长为指定时长，且各序列的起始时间、结束时间均相同，且序列中相邻位姿的间隔时间也相同；i为障碍物索引，1≤i≤n,n为障碍物数量；

所述第一预测轨迹组集合包括多个第一预测轨迹组P_i；所述第一预测轨迹组P_i包括多个第一预测轨迹p_i,j，每个所述第一预测轨迹p_i,j为对应障碍物的一条可能发生的预测轨迹；j为可能轨迹索引，1≤j；

所述第一融合轨迹组集合包括多个第一融合轨迹组F_k；所述第一融合轨迹组F_k包括多个所述第一预测轨迹p_i,j；k为融合轨迹组索引，1≤k；

所述第一规划轨迹集合包括多个第一规划轨迹τ_k；所述第一规划轨迹τ_k与所述第一融合轨迹组F_k对应；所述第一规划轨迹τ_k的轨迹时长为T_th；所述轨迹时长T_th远大于(T_p+T_c)，T_p为所述规划模块的工作周期，T_c为所述控制模块的工作周期；

所述第二障碍物位姿集合包括多个第二位姿序列M^’ _i；各个所述第二位姿序列M^’ _i的起始时间、结束时间均相同，且序列中相邻位姿的间隔时间也相同；各个所述第二位姿序列M^’ _i的序列时长相同且可调，最小可调至一个时刻。

优选的，所述预测模块具体用于在所述障碍物轨迹预测处理时，根据所述第一障碍物位姿集合的各个所述第一位姿序列M_i和当前环境的道路交通信息，对各个障碍物在未来时段的一种或多种运动可能进行轨迹预测生成对应的一个或多个所述第一预测轨迹p_i,j；并由同一障碍物的所有所述第一预测轨迹p_i,j组成与该障碍物对应的所述第一预测轨迹组P_i；并由得到的所述障碍物数量n的所述第一预测轨迹组P_i组成所述第一预测轨迹组集合。

进一步的，所述道路交通信息包括地图信息、路网信息、道路信息、车道信息、交通标识标线信息和红绿灯信息。

优选的，所述规划模块具体用于在所述障碍物预测轨迹融合处理时，从各个所述第一预测轨迹组P_i中任选一个所述第一预测轨迹p_i,j组成对应的第一轨迹组合，并确定任两个所述第一轨迹组合中所述第一预测轨迹p_i,j的数量相同均为所述障碍物数量n，并确定任两个所述第一轨迹组合中所述第一预测轨迹p_i,j的组合关系不同；并将各个所述第一轨迹组合作为对应的所述第一融合轨迹组F_k，再由得到的所有所述第一融合轨迹组F_k组成所述第一融合轨迹组集合。

优选的，所述规划模块具体用于在所述车辆多轨迹预规划处理时，根据所述第一融合轨迹组集合的各个所述第一融合轨迹组F_k进行自车轨迹规划处理生成对应的所述第一规划轨迹τ_k；并由得到的所有所述第一规划轨迹τ_k组成所述第一规划轨迹集合。

进一步的，所述规划模块具体用于在所述自车轨迹规划处理时，以当前第一融合轨迹组F_k的所有所述第一预测轨迹p_i,j为障碍物规避轨迹，基于自车当前行驶状态对自车在未来时段的行驶轨迹进行规划生成对应的所述第一规划轨迹τ_k。

优选的，所述规划模块具体用于在所述规划轨迹查询处理时，将所述第二障碍物位姿集合的所述第二位姿序列M^’ _i的起始、结束时间提取出来构成第一时段；并在所述第一融合轨迹组集合中，对各个所述第一预测轨迹p_i,j在所述第一时段的预测轨迹信息进行提取生成对应的第一时段预测轨迹；并对各个所述当前第二位姿序列M^’ _i对应障碍物的所有所述第一时段预测轨迹进行查询，将与当前第二位姿序列M^’ _i匹配的所述第一时段预测轨迹作为对应的第一时段匹配轨迹；并在所述第一融合轨迹组集合中，将各个所述第一时段匹配轨迹所在的所述第一预测轨迹p_i,j作为对应的第一匹配预测轨迹，并将所有所述第一匹配预测轨迹所在的所述第一融合轨迹组F_k作为第一匹配轨迹组；并在所述第一规划轨迹集合中，将所述第一匹配轨迹组对应的所述第一规划轨迹τ_k作为第一匹配规划轨迹，并从所述第一匹配规划轨迹中提取所述第一时段之后的规划轨迹信息作为所述第二规划轨迹τ^,。

优选的，所述控制模块可在所述规划模块的同一个工作周期内，多次从所述感知系统获取最新的所述第二障碍物位姿集合向所述规划模块发送，并根据所述规划模块回发的最新的所述第二规划轨迹τ^,进行车辆轨迹跟踪与行驶控制。

本发明实施例提供了一种规划控制系统，该系统包括：预测模块、规划模块和控制模块。本发明系统的预测模块对接感知系统，该预测模块基于感知系统的障碍物位姿序列集合并结合当前环境的道路交通信息，对各个障碍物的一种或多种运动可能(包括突发的可能事件)的对应轨迹进行预测得到预测轨迹组，并将由所有障碍物的预测轨迹组构成的预测轨迹组集合发送至规划模块。本发明系统的规划模块根据预测轨迹组集合对所有障碍物的各种运动可能进行组合，从而使得每个组合关系可以代表一种全障碍物运动组合场景；并基于各个组合关系对所有障碍物的单个运动可能预测轨迹进行融合得到对应的融合轨迹组，并由得到的多个融合轨迹组构成融合轨迹组集合，从而使得融合轨迹组集合可将未来时段所有障碍物的所有动作可能的预测轨迹都包括在内；在融合轨迹组集合创建完成后，规划模块会基于每个场景也就是每个融合轨迹组进行自车轨迹预测从而得到对应场景的规划轨迹，并由得到的所有规划轨迹构成规划轨迹集合，从而使得规划轨迹集合可将未来时段所有可能场景下的自车规划轨迹都包括在内；在规划轨迹集合创建完成后，规划模块并不从中进行最大概率规划轨迹的筛选与发送操作，而是等待控制模块发送的最新障碍物位姿序列集合；规划模块在接收到控制模块从感知系统获取的最新障碍物位姿序列集合之后，将之视为实时场景下的全障碍物历史轨迹信息，并通过查询融合轨迹组集合得到与此全障碍物历史轨迹信息匹配的融合轨迹组，并将与该匹配融合轨迹组对应的规划轨迹作为最适合当前场景的规划轨迹进行有效轨迹提取并将提取结果向控制模块回发。本发明系统的控制模块则可根据查询得到的最适合当前场景的规划轨迹进行车辆轨迹跟踪与行驶控制。通过本发明系统的技术改进，不但解决了在周围障碍物运动轨迹发生突变时控制模块丢失参考轨迹无法补充的问题，降低了车辆行驶控制的安全隐患；还提高了控制模块参考轨迹与实际场景的适配性，从而进一步提高了车乘人员的舒适度。

附图说明

图1为本发明实施例提供的一种规划控制系统的结构示意图；

图2为本发明实施例提供的第一行驶场景示意图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

图1为本发明实施例提供的一种规划控制系统的结构示意图，如图1所示，本规划控制系统1包括：预测模块11、规划模块12和控制模块13；规划控制系统1与感知系统2连接。

感知系统2用于对车辆周围所有障碍物的位姿信息进行采集；并将指定时长内采集到的各个障碍物的位姿序列组成第一障碍物位姿集合；并将第一障碍物位姿集合向规划控制系统1的预测模块11发送；

其中，第一障碍物位姿集合包括多个第一位姿序列M_i；各个第一位姿序列M_i的序列时长为指定时长，且各序列的起始时间、结束时间均相同，且序列中相邻位姿的间隔时间也相同；i为障碍物索引，1≤i≤n,n为障碍物数量。

这里，感知系统2由一个或多个感知传感器构成，感知传感器包括摄像头、激光雷达、毫米波雷达等；感知传感器按设定时长对车辆周围环境中的障碍物进行目标识别与轨迹跟踪，从而得到一个或多个障碍物的轨迹跟踪结果也就是带有时序关系的位姿序列即第一位姿序列M_i；每个第一位姿序列M_i对应一个障碍物目标，第一位姿序列M_i中的位姿数据可包括对应目标的位置、朝向、速度、加速度等信息，也可为上述信息的其中一个或多个的组合；为保证数据同步性，本发明实施例系统要求各个第一位姿序列M_i的序列长度一致即序列时长相同且与预先设定的指定时长一致，要求各序列的采样时间段一致即起始时间、结束时间均相同，要求各序列的采样频率一致即序列中相邻位姿的间隔时间相同。需要说明的是，感知系统2与规划控制系统1之间的数据传输方式可以有主动传输与查询传输两种。感知系统2基于主动传输方式向预测模块11发送第一障碍物位姿集合，即每隔指定时间间隔向预测模块11主动发送最新的第一障碍物位姿集合；该指定时间间隔应小于或等于第一障碍物位姿集合对应的指定时长。

预测模块11分别与感知系统2和规划模块12连接；预测模块11用于接收感知系统2发送的第一障碍物位姿集合；并根据第一障碍物位姿集合进行障碍物轨迹预测处理生成对应的第一预测轨迹组集合；并将第一预测轨迹组集合向规划模块12发送；

其中，第一预测轨迹组集合包括多个第一预测轨迹组P_i；第一预测轨迹组P_i包括多个第一预测轨迹p_i,j，每个第一预测轨迹p_i,j为对应障碍物的一条可能发生的预测轨迹；j为可能轨迹索引，1≤j。此处，每个第一预测轨迹组P_i对应一个障碍物。

在本发明实施例系统的一个具体实现方式中，预测模块11具体用于在障碍物轨迹预测处理时，根据第一障碍物位姿集合的各个第一位姿序列M_i和当前环境的道路交通信息，对各个障碍物在未来时段的一种或多种运动可能进行轨迹预测生成对应的一个或多个第一预测轨迹p_i,j；并由同一障碍物的所有第一预测轨迹p_i,j组成与该障碍物对应的第一预测轨迹组P_i；并由得到的障碍物数量n的第一预测轨迹组P_i组成第一预测轨迹组集合；

其中，道路交通信息包括地图信息、路网信息、道路信息、车道信息、交通标识标线信息和红绿灯信息。

这里，在对各个障碍物未来时段的一种或多种运动可能进行轨迹预测时，采用的轨迹预测算法与传统轨迹预测方式类似在此不做进一步赘述。为能更好的理解上述内容，下文以图2为本发明实施例提供的第一行驶场景示意图为例进行说明。

图2的直行道路R₁由可车道线L₁分为两条直行车道分别为左车道L_left和右车道L_right，车道线L₁虚线说明其为可超车车道线；在右车道L_right上有2个直行车辆分别为车辆C₁、C₂，车辆C₂为车辆C₁的前车，在左车道L_left上有1个直行车辆为车辆C₃，车辆C₃为车辆C₁的平行行驶车辆。

若以车辆C₁为自车，则车辆C₂、C₃为车辆C₁周围的2个障碍物，对应的第一障碍物位姿集合应包括2个第一位姿序列M₁、M₂分别对应车辆C₂、C₃。另外，设通过图2中的道路交通信息推导得到在未来时段车辆C₂存在4种运动可能：刹车急停、直行加速、直行减速、向左车道L_left并线，同理推导得到车辆C₃也存在4种运动可能：刹车急停、直行加速、直行减速、向右车道L_right并线。

那么，基于第一位姿序列M₁和由道路交通信息得到的4种运动可能，对车辆C₂在未来时段进行轨迹预测就会得到4个第一预测轨迹分别为p_1,1、p_1,2、p_1,3、p_1,4；其中，p_1,1为刹车急停预测轨迹、p_1,2为直行加速预测轨迹、p_1,3为直行减速预测轨迹、p_1,4为向左车道L_left并线预测轨迹。同理，基于第一位姿序列M₂和由道路交通信息得到的4种运动可能，对车辆C₃在未来时段进行轨迹预测就会得到4个第一预测轨迹分别为p_2,1、p_2,2、p_2,3、p_2,4；其中，p_2,1为刹车急停预测轨迹、p_2,2为直行加速预测轨迹、p_2,3为直行减速预测轨迹、p_2,4为向右车道L_right并线预测轨迹。

那么最终，可以得到对应2个障碍物的2组第一预测轨迹组P_i，即对应车辆C₂的第一预测轨迹组P₁＝(p_1,1、p_1,2、p_1,3、p_1,4)，以及对应车辆C₃的第一预测轨迹组P₂＝(p_2,1、p_2,2、p_2,3、p_2,4)。第一预测轨迹组集合也就是由第一预测轨迹组P₁、P₂构成的集合[P₁,P₂]＝[(p_1,1、p_1,2、p_1,3、p_1,4),(p_2,1、p_2,2、p_2,3、p_2,4)]。

规划模块12与控制模块13连接；规划模块12用于接收第一预测轨迹组集合；并根据第一预测轨迹组集合进行障碍物预测轨迹融合处理生成对应的第一融合轨迹组集合并保存；并根据第一融合轨迹组集合进行车辆多轨迹预规划处理生成对应的第一规划轨迹集合并保存；

其中，第一融合轨迹组集合包括多个第一融合轨迹组F_k；第一融合轨迹组F_k包括多个第一预测轨迹p_i,j；k为融合轨迹组索引，1≤k；

第一规划轨迹集合包括多个第一规划轨迹τ_k；第一规划轨迹τ_k与第一融合轨迹组F_k对应；第一规划轨迹τ_k的轨迹时长为T_th；轨迹时长T_th远大于(T_p+T_c)，T_p为规划模块12的工作周期，T_c为控制模块13的工作周期。这里，轨迹时长T_th可设为10秒，规划模块12、控制模块13的工作周期T_p、T_c都是毫秒级。

在本发明实施例系统的又一个具体实现方式中，规划模块12具体用于在障碍物预测轨迹融合处理时，从各个第一预测轨迹组P_i中任选一个第一预测轨迹p_i,j组成对应的第一轨迹组合，并确定任两个第一轨迹组合中第一预测轨迹p_i,j的数量相同均为障碍物数量n，并确定任两个第一轨迹组合中第一预测轨迹p_i,j的组合关系不同；并将各个第一轨迹组合作为对应的第一融合轨迹组F_k，再由得到的所有第一融合轨迹组F_k组成第一融合轨迹组集合。

这里，从各个第一预测轨迹组P_i中任选一个第一预测轨迹p_i,j组成对应的第一轨迹组合，并确定任两个第一轨迹组合中第一预测轨迹p_i,j的数量相同均为障碍物数量n，并确定任两个第一轨迹组合中第一预测轨迹p_i,j的组合关系不同；实际就是根据预测轨迹组集合对所有障碍物的各种运动可能进行组合，从而使得每个组合关系可以代表一种全障碍物运动组合场景。每个第一融合轨迹组F_k对应一个具体的组合关系，也就是对应一种具体的全障碍物运动组合场景。自然，由所有第一融合轨迹组F_k组成的第一融合轨迹组集合则对应了所有全障碍物运动组合场景，也就是将未来时段所有障碍物的所有动作可能的预测轨迹都包括在内。

需要说明的是组合数量k_max＝max(j_i＝1)×max(j_i＝2)…×max(j_i＝n)；其中，max(j_i＝1)表示当i＝1时对应的第一预测轨迹p_i＝1,j中脚标j的最大值也就是障碍物1的运动可能种类总数，max(j_i＝2)表示当i＝2时对应的第一预测轨迹p_i＝2,j中脚标j的最大值，依次类推，max(j_i＝n)表示当i＝障碍物数量n时对应的第一预测轨迹p_i＝n,j中脚标j的最大值。

例如，第一预测轨迹组集合为[P₁,P₂]＝[(p_1,1、p_1,2、p_1,3、p_1,4),(p_2,1、p_2,2、p_2,3、p_2,4)]，说明图2中车辆C₂、C₃各自都有4种运动可能，则有max(j_i＝1)为4，max(j_i＝2)为4，组合数量k_max＝max(j_i＝1)×max(j_i＝2)＝4×4＝16。规划模块12分别从第一预测轨迹组P₁、P₂中任选一个第一预测轨迹p_i,j组成对应的第一轨迹组合并确保组合关系不重复，那么就可以得到以下16种组合关系也就是16个第一轨迹组合：

(p_1,1,p_2,1)、(p_1,1,p_2,2)、(p_1,1,p_2,3)、(p_1,1,p_2,4)、

(p_1,2,p_2,1)、(p_1,2,p_2,2)、(p_1,2,p_2,3)、(p_1,2,p_2,4)、

(p_1,3,p_2,1)、(p_1,3,p_2,2)、(p_1,3,p_2,3)、(p_1,3,p_2,4)、

(p_1,4,p_2,1)、(p_1,4,p_2,2)、(p_1,4,p_2,3)、(p_1,4,p_2,4)；

从而也就得到了16个第一融合轨迹组F₁、F₂…F₁₆：

F₁＝(p_1,1,p_2,1)、F₂＝(p_1,1,p_2,2)、F₃＝(p_1,1,p_2,3)、F₄＝(p_1,1,p_2,4)、

F₅＝(p_1,2,p_2,1)、F₆＝(p_1,2,p_2,2)、F₇＝(p_1,2,p_2,3)、F₈＝(p_1,2,p_2,4)、

F₉＝(p_1,3,p_2,1)、F₁₀＝(p_1,3,p_2,2)、F₁₁＝(p_1,3,p_2,3)、F₁₂＝(p_1,3,p_2,4)、

F₁₃＝(p_1,4,p_2,1)、F₁₄＝(p_1,4,p_2,2)、F₁₅＝(p_1,4,p_2,3)、F₁₆＝(p_1,4,p_2,4)；

从而也就得到了第一融合轨迹组集合[F₁,F₂…F₁₆]。

在本发明实施例系统的又一个具体实现方式中，规划模块12具体用于在车辆多轨迹预规划处理时，根据第一融合轨迹组集合的各个第一融合轨迹组F_k进行自车轨迹规划处理生成对应的第一规划轨迹τ_k；并由得到的所有第一规划轨迹τ_k组成第一规划轨迹集合。

进一步的，在本发明实施例系统的又一个具体实现方式中规划模块12具体用于在自车轨迹规划处理时，以当前第一融合轨迹组F_k的所有第一预测轨迹p_i,j为障碍物规避轨迹，基于自车当前行驶状态对自车在未来时段的行驶轨迹进行规划生成对应的第一规划轨迹τ_k。

这里，基于障碍物轨迹对自车未来时段行驶轨迹进行规划的规划算法可通过常规轨迹规划算法实现，都是在获取了当次规划的起始、结束位置之后，以障碍物规避轨迹的各个轨迹点为避让目标，结合自车行驶状态进行最短路径和运动状态规划从而得到对应的规划轨迹，具体实现可参考传统规划算法技术实现，在此不做进一步赘述。因为第一融合轨迹组集合将未来时段所有障碍物的所有动作可能的预测轨迹都包括在内，每个第一融合轨迹组F_k对应未来的一种全障碍物运动组合场景，那么基于每个第一融合轨迹组F_k规划得到的第一规划轨迹τ_k也就是对应场景下的未来轨迹规划结果，从而由所有第一规划轨迹τ_k组成的第一规划轨迹集合也就可将未来时段所有可能场景下的自车规划轨迹都包括在内。并且，在第一规划轨迹集合创建成功后，本发明实施例系统的规划模块12并不会在第一规划轨迹集合中进行预测轨迹概率评估，也不会做最大概率预测轨迹筛选，更不会主动向控制模块13发送任何预测轨迹。由后续模块说明可知，此时规划模块12会等待控制模块13发送实时的障碍物位姿信息来激活对应的规划轨迹查询处理流程。

例如，第一融合轨迹组集合[F₁,F₂…F₁₆]为

F₁＝(p_1,1,p_2,1)、F₂＝(p_1,1,p_2,2)、F₃＝(p_1,1,p_2,3)、F₄＝(p_1,1,p_2,4)、

F₅＝(p_1,2,p_2,1)、F₆＝(p_1,2,p_2,2)、F₇＝(p_1,2,p_2,3)、F₈＝(p_1,2,p_2,4)、

F₉＝(p_1,3,p_2,1)、F₁₀＝(p_1,3,p_2,2)、F₁₁＝(p_1,3,p_2,3)、F₁₂＝(p_1,3,p_2,4)、

F₁₃＝(p_1,4,p_2,1)、F₁₄＝(p_1,4,p_2,2)、F₁₅＝(p_1,4,p_2,3)、F₁₆＝(p_1,4,p_2,4)；

其中，p_1,1为车辆C₂刹车急停预测轨迹、p_1,2为车辆C₂直行加速预测轨迹、p_1,3为车辆C₂直行减速预测轨迹、p_1,4为车辆C₂向左车道L_left并线预测轨迹；p_2,1为车辆C₃刹车急停预测轨迹、p_2,2为车辆C₃直行加速预测轨迹、p_2,3为车辆C₃直行减速预测轨迹、p_2,4为车辆C₃向右车道L_right并线预测轨迹；

那么，对应就可得到第一规划轨迹集合[τ₁,τ₂…τ₁₆]；其中，

τ₁到τ₄实际就是在车辆C₂发生刹车急停，而车辆C₃发生刹车急停、直行加速、直行减速或向右车道L_right的场景下对自车未来时段的4种轨迹规划结果；

τ₅到τ₈实际就是在车辆C₂发生直行加速，而车辆C₃发生刹车急停、直行加速、直行减速或向右车道L_right的场景下对自车未来时段的4种轨迹规划结果；

τ₉到τ₁₂实际就是在车辆C₂发生直行减速，而车辆C₃发生刹车急停、直行加速、直行减速或向右车道L_right的场景下对自车未来时段的4种轨迹规划结果；

τ₁₃到τ₁₆实际就是在车辆C₂发生向左车道L_left并线，而车辆C₃发生刹车急停、直行加速、直行减速或向右车道L_right的场景下对自车未来时段的4种轨迹规划结果。

控制模块13与感知系统2连接；控制模块13用于从感知系统2获取第二障碍物位姿集合；并将第二障碍物位姿集合向规划模块12发送；

其中，第二障碍物位姿集合包括多个第二位姿序列M^’ _i；各个第二位姿序列M^’ _i的起始时间、结束时间均相同，且序列中相邻位姿的间隔时间也相同；各个第二位姿序列M^’ _i的序列时长相同且可调，最小可调至一个时刻。

这里，控制模块13从感知系统2获取到最新障碍物位姿序列集合也即是第二障碍物位姿集合之后，将之视为实时场景下的全障碍物历史轨迹信息集合；控制模块13将其向规划模块12发送，是为了激活规划模块12中的规划轨迹查询处理流程。

在本发明实施例系统的又一个具体实现方式中，控制模块13具体用于处理从感知系统2获取第二障碍物位姿集合时，向感知系统2发送障碍物位姿信息获取指令，并接收感知系统2回发的第二障碍物位姿集合。

需要说明的是，感知系统2此时是基于被动传输方式向控制模块13发送第二障碍物位姿集合。在本发明实施例系统的又一个具体实现方式中，感知系统2还用于接收控制模块13发送的障碍物位姿信息获取指令；并将各个障碍物最新的长度为约定时间长度的位姿序列组成第二障碍物位姿集合向控制模块13回发；该约定时间长度可调，最小可调至一个时刻也即是约定时间长度＝1。这里，在约定时间长度为1时，各个第二位姿序列M^’ _i实际只包括一个轨迹点的位姿信息，也是各个障碍物的实时轨迹点信息；在约定时间长度大于1时，各个第二位姿序列M^’ _i实际是各个障碍物的最新历史轨迹信息。

规划模块12还用于接收第二障碍物位姿集合；并根据第二障碍物位姿集合与保存的第一融合轨迹组集合和第一规划轨迹集合进行规划轨迹查询处理生成对应的第二规划轨迹τ^,；并将第二规划轨迹τ^,向控制模块13回发。

在本发明实施例系统的又一个具体实现方式中，规划模块12具体用于在规划轨迹查询处理时，将第二障碍物位姿集合的第二位姿序列M^’ _i的起始、结束时间提取出来构成第一时段；并在第一融合轨迹组集合中，对各个第一预测轨迹p_i,j在第一时段的预测轨迹信息进行提取生成对应的第一时段预测轨迹；并对各个当前第二位姿序列M^’ _i对应障碍物的所有第一时段预测轨迹进行查询，将与当前第二位姿序列M^’ _i匹配的第一时段预测轨迹作为对应的第一时段匹配轨迹；并在第一融合轨迹组集合中，将各个第一时段匹配轨迹所在的第一预测轨迹p_i,j作为对应的第一匹配预测轨迹，并将所有第一匹配预测轨迹所在的第一融合轨迹组F_k作为第一匹配轨迹组；并在第一规划轨迹集合中，将第一匹配轨迹组对应的第一规划轨迹τ_k作为第一匹配规划轨迹，并从第一匹配规划轨迹中提取第一时段之后的规划轨迹信息作为第二规划轨迹τ^,。

这里，规划模块12在接收到控制模块13发送的最新障碍物位姿序列集合也就是第二障碍物位姿集合之后，将之视为实时场景下的全障碍物历史轨迹信息，并通过查询第一融合轨迹组集合得到与此全障碍物历史轨迹信息匹配的第一融合轨迹组F_k，并将与该匹配融合轨迹组对应的第一规划轨迹τ_k作为最适合当前场景的规划轨迹，并对其进行有效轨迹提取再将提取结果作为第二规划轨迹τ^,向控制模块回发。此处，本发明实施例系统将第一规划轨迹τ_k中第一时段的结束时间之前的所有轨迹都视为已经发生的历史轨迹，将第一时段之后的轨迹视为尚未发生的未来轨迹，自然在进行有效轨迹提取时选择第一时段之后的未来轨迹进行提取作为第二规划轨迹τ^,。

在本发明实施例系统的又一个具体实现方式中，规划模块12在对各个当前第二位姿序列M^’ _i对应障碍物的所有第一时段预测轨迹进行查询时，将当前第二位姿序列M^’ _i的对应障碍物记为第一障碍物；并将与第一障碍物对应的所有第一时段预测轨迹组成对应的第一轨迹集合；并对第一轨迹集合中的各个第一时段预测轨迹与当前第二位姿序列M^’ _i的轨迹相似度进行估计生成对应的第一相似度；并将第一轨迹集合中与最大第一相似度对应的第一时段预测轨迹，作为与当前第二位姿序列M^’ _i匹配的第一时段预测轨迹。

为了对轨迹匹配有进一步的了解，下文继续基于前文示例进行举例说明。例如，已知第一融合轨迹组集合[F₁,F₂…F₁₆]为

F₁＝(p_1,1,p_2,1)、F₂＝(p_1,1,p_2,2)、F₃＝(p_1,1,p_2,3)、F₄＝(p_1,1,p_2,4)、

F₅＝(p_1,2,p_2,1)、F₆＝(p_1,2,p_2,2)、F₇＝(p_1,2,p_2,3)、F₈＝(p_1,2,p_2,4)、

F₉＝(p_1,3,p_2,1)、F₁₀＝(p_1,3,p_2,2)、F₁₁＝(p_1,3,p_2,3)、F₁₂＝(p_1,3,p_2,4)、

F₁₃＝(p_1,4,p_2,1)、F₁₄＝(p_1,4,p_2,2)、F₁₅＝(p_1,4,p_2,3)、F₁₆＝(p_1,4,p_2,4)；

各个第一预测轨迹p_i,j的长度均为10秒，起始时间为t，结束时间为t+10秒；另外，第一规划轨迹集合为[τ₁,τ₂…τ₁₆]，各个第一规划轨迹τ_k的长度、起始时间和结束时间都与各个第一预测轨迹p_i,j相同；

设第二障碍物位姿集合的2个第二位姿序列M₁ ^’、M^’ ₂的起始时间为t+2秒、结束时间为t+4秒，那么第一时段就为t+2秒到t+4秒；此处，第二位姿序列M₁ ^’、M^’ ₂实际可被视为车辆C₂、C₃在t+2秒到t+4秒的一段真实轨迹；

规划模块12在接收到第二位姿序列M₁ ^’、M^’ ₂之后，首先在第一融合轨迹组集合中，对8个第一预测轨迹p_i,j在第一时段的预测轨迹信息进行提取从而得到8个第一预测轨迹p_i,j在第一时段的子轨迹也就是第一时段预测轨迹1、2…8：

第一时段预测轨迹1＝p_1,1(t_x)，第一时段预测轨迹2＝p_1,2(t_x)，

第一时段预测轨迹3＝p_1,3(t_x)，第一时段预测轨迹4＝p_1,4(t_x)，

第一时段预测轨迹5＝p_2,1(t_x)，第一时段预测轨迹6＝p_2,2(t_x)，

第一时段预测轨迹7＝p_2,3(t_x)，第一时段预测轨迹8＝p_2,4(t_x)，

t_x∈(t+2秒,t+4秒)；

然后，对第二位姿序列M₁ ^’对应障碍物的所有第一时段预测轨迹进行查询：将第二位姿序列M₁ ^’的对应障碍物记为第一障碍物也就是车辆C₂,将与车辆C₂对应的所有第一时段预测轨迹也就是第一时段预测轨迹1、2、3、4组成第一轨迹集合，分别计算第一时段预测轨迹1、2、3、4与第二位姿序列M₁ ^’的轨迹相似度得到4个第一相似度1、2、3、4，设其中最大值为第一相似度1，那么与第二位姿序列M₁ ^’匹配的第一时段匹配轨迹1实际就是第一时段预测轨迹1；

同理，对第二位姿序列M₂ ^’对应障碍物的所有第一时段预测轨迹进行查询：将第二位姿序列M^’ ₂的对应障碍物记为第一障碍物也就是车辆C₃,然后将与车辆C₃对应的所有第一时段预测轨迹也就是第一时段预测轨迹5、6、7、8组成第一轨迹集合，然后分别计算第一时段预测轨迹5、6、7、8与第二位姿序列M₁ ^’的轨迹相似度得到4个第一相似度5、6、7、8，设其中最大值为第一相似度5，那么与第二位姿序列M^’ ₂匹配的第一时段匹配轨迹2实际就是第一时段预测轨迹5；

在得到为第一时段匹配轨迹1、2之后，自然就得到了对应的2个第一匹配预测轨迹1、2，即第一预测轨迹p_1,1、p_2,1；因为第一匹配预测轨迹1、2所在的第一融合轨迹组F_k为F₁＝(p_1,1,p_2,1)，那么第一匹配轨迹组就是第一融合轨迹组F₁；因为第一融合轨迹组F₁对应的第一规划轨迹τ₁，那么第一匹配规划轨迹也就是第一规划轨迹τ₁；在确定第一匹配规划轨迹是第一规划轨迹τ₁之后，规划模块12会从中提取第一时段也就是(t+2秒,t+4秒)之后的规划轨迹信息作为第二规划轨迹τ^,；也就是说，第二规划轨迹τ^,实际就是第一规划轨迹τ₁从t+4秒到t+10秒这一时段的轨迹。

需要说明的是，传统的规划模块在一个工作周期内只会输出一次规划轨迹，而本发明实施例系统中的规划模块12可在一个工作周期中不限次输出，只要接收到控制模块13发送的第二障碍物位姿集合规划模块12就会做一次规划轨迹查询并对应实施一次规划轨迹输出。这样设计的目的就是为了在突发事件发生时对控制模块13的规划轨迹进行及时更换。

控制模块13还用于接收第二规划轨迹τ^,；并根据第二规划轨迹τ^,进行车辆轨迹跟踪与行驶控制。

这里，控制模块13以第二规划轨迹τ^,作为参考轨迹并对其进行轨迹跟踪，在轨迹跟踪过程中通过行驶控制也就是对自车的实时运动状态进行控制(诸如方向盘控制、油门/制动控制等)来使得实际运动轨迹最大限度地贴近参考轨迹。

与规划模块12可在一个工作周期中不限次输出规划轨迹对应的，控制模块13也可在规划模块12的同一个工作周期内，多次从感知系统2获取最新的第二障碍物位姿集合向规划模块12发送，并根据规划模块12回发的最新的第二规划轨迹τ^,进行车辆轨迹跟踪与行驶控制。这样设计的目的就是为了在突发事件已经发生或即将发生时及时将障碍物实时轨迹发送到规划模块12获得最新的规划轨迹，继而以最新的规划轨迹作为参考轨迹并对其进行轨迹跟踪。

本发明实施例提供了一种规划控制系统，该系统包括：预测模块、规划模块和控制模块。本发明实施例系统的预测模块对接感知系统，该预测模块基于感知系统的障碍物位姿序列集合并结合当前环境的道路交通信息，对各个障碍物的一种或多种运动可能(包括突发的可能事件)的对应轨迹进行预测得到预测轨迹组，并将由所有障碍物的预测轨迹组构成的预测轨迹组集合发送至规划模块。本发明实施例系统的规划模块根据预测轨迹组集合对所有障碍物的各种运动可能进行组合，从而使得每个组合关系可以代表一种全障碍物运动组合场景；并基于各个组合关系对所有障碍物的单个运动可能预测轨迹进行融合得到对应的融合轨迹组，并由得到的多个融合轨迹组构成融合轨迹组集合，从而使得融合轨迹组集合可将未来时段所有障碍物的所有动作可能的预测轨迹都包括在内；在融合轨迹组集合创建完成后，规划模块会基于每个场景也就是每个融合轨迹组进行自车轨迹预测从而得到对应场景的规划轨迹，并由得到的所有规划轨迹构成规划轨迹集合，从而使得规划轨迹集合可将未来时段所有可能场景下的自车规划轨迹都包括在内；在规划轨迹集合创建完成后，规划模块并不从中进行最大概率规划轨迹的筛选与发送操作，而是等待控制模块发送的最新障碍物位姿序列集合；规划模块在接收到控制模块从感知系统获取的最新障碍物位姿序列集合之后，将之视为实时场景下的全障碍物历史轨迹信息，并通过查询融合轨迹组集合得到与此全障碍物历史轨迹信息匹配的融合轨迹组，并将与该匹配融合轨迹组对应的规划轨迹作为最适合当前场景的规划轨迹进行有效轨迹提取并将提取结果向控制模块回发。本发明实施例系统的控制模块则可根据查询得到的最适合当前场景的规划轨迹进行车辆轨迹跟踪与行驶控制。由此可见，本发明实施例系统与常规的规划控制系统相比，分别对预测、规划和控制模块都进行了改进：1)对预测模块进行改进，使之可以对各个障碍物的所有运动可能(包括突发的可能事件)的对应轨迹进行预测；2)对常规规划模块进行改进，使之可通过创建融合轨迹组集合将未来时段所有障碍物的所有动作可能的预测轨迹都包括在内，使之可通过创建规划轨迹集合将未来时段所有可能场景下的自车规划轨迹都包括在内；3)对常规规划模块与控制模块的互动关系进行改进，取消常规由规划模块到控制模块一次性设定轨迹的单向输出方式，转以控制模块基于实时障碍物轨迹向规划模块进行实时规划轨迹反查的处理方式来替代。通过本发明实施例系统的上述技术改进，不但解决了在周围障碍物运动轨迹发生突变时传统控制模块会丢失参考轨迹并无法获得补充的问题，从而进一步提高了对车辆行驶控制的安全保障；还提高了控制模块参考轨迹与实际场景的适配性，从而进一步提高了车乘人员的舒适度。

专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的系统、模块、单元及算法的步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的系统、模块、单元或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种规划控制系统，其特征在于，所述系统包括：预测模块、规划模块和控制模块；所述系统与感知系统连接；

所述预测模块分别与所述感知系统和所述规划模块连接；所述预测模块用于接收所述感知系统发送的第一障碍物位姿集合；并根据所述第一障碍物位姿集合进行障碍物轨迹预测处理生成对应的第一预测轨迹组集合；并将所述第一预测轨迹组集合向所述规划模块发送；

所述规划模块与所述控制模块连接；所述规划模块用于接收所述第一预测轨迹组集合；并根据所述第一预测轨迹组集合进行障碍物预测轨迹融合处理生成对应的第一融合轨迹组集合并保存；并根据所述第一融合轨迹组集合进行车辆多轨迹预规划处理生成对应的第一规划轨迹集合并保存；

所述控制模块与所述感知系统连接；所述控制模块用于从所述感知系统获取第二障碍物位姿集合；并将所述第二障碍物位姿集合向所述规划模块发送；

所述规划模块还用于接收所述第二障碍物位姿集合；并根据所述第二障碍物位姿集合与保存的所述第一融合轨迹组集合和所述第一规划轨迹集合进行规划轨迹查询处理生成对应的第二规划轨迹τ’；并将所述第二规划轨迹τ’向所述控制模块回发；

所述控制模块还用于接收所述第二规划轨迹τ’；并根据所述第二规划轨迹τ’进行车辆轨迹跟踪与行驶控制。

2.根据权利要求1所述的规划控制系统，其特征在于，

所述感知系统用于对车辆周围所有障碍物的位姿信息进行采集；并将指定时长内采集到的各个障碍物的位姿序列组成所述第一障碍物位姿集合；并将所述第一障碍物位姿集合向所述预测模块发送。

3.根据权利要求1所述的规划控制系统，其特征在于，

所述第一障碍物位姿集合包括多个第一位姿序列M_i；各个所述第一位姿序列M_i的序列时长为指定时长，且各序列的起始时间、结束时间均相同，且序列中相邻位姿的间隔时间也相同；i为障碍物索引，1≤i≤n,n为障碍物数量；

所述第一预测轨迹组集合包括多个第一预测轨迹组P_i；所述第一预测轨迹组P_i包括多个第一预测轨迹p_i,j，每个所述第一预测轨迹p_i,j为对应障碍物的一条可能发生的预测轨迹；j为可能轨迹索引，1≤j；

所述第一融合轨迹组集合包括多个第一融合轨迹组F_k；所述第一融合轨迹组F_k包括多个所述第一预测轨迹p_i,j；k为融合轨迹组索引，1≤k；

所述第一规划轨迹集合包括多个第一规划轨迹τ_k；所述第一规划轨迹τ_k与所述第一融合轨迹组F_k对应；所述第一规划轨迹τ_k的轨迹时长为T_th；所述轨迹时长T_th远大于(T_p+T_c)，T_p为所述规划模块的工作周期，T_c为所述控制模块的工作周期；

所述第二障碍物位姿集合包括多个第二位姿序列M’_i；各个所述第二位姿序列M’_i的起始时间、结束时间均相同，且序列中相邻位姿的间隔时间也相同；各个所述第二位姿序列M’_i的序列时长相同且可调，最小可调至一个时刻。

4.根据权利要求3所述的规划控制系统，其特征在于，

所述预测模块具体用于在所述障碍物轨迹预测处理时，根据所述第一障碍物位姿集合的各个所述第一位姿序列M_i和当前环境的道路交通信息，对各个障碍物在未来时段的一种或多种运动可能进行轨迹预测生成对应的一个或多个所述第一预测轨迹p_i,j；并由同一障碍物的所有所述第一预测轨迹p_i,j组成与该障碍物对应的所述第一预测轨迹组P_i；并由得到的所述障碍物数量n的所述第一预测轨迹组P_i组成所述第一预测轨迹组集合。

5.根据权利要求4所述的规划控制系统，其特征在于，

所述道路交通信息包括地图信息、路网信息、道路信息、车道信息、交通标识标线信息和红绿灯信息。

6.根据权利要求3所述的规划控制系统，其特征在于，

所述规划模块具体用于在所述障碍物预测轨迹融合处理时，从各个所述第一预测轨迹组P_i中任选一个所述第一预测轨迹p_i,j组成对应的第一轨迹组合，并确定任两个所述第一轨迹组合中所述第一预测轨迹p_i,j的数量相同均为所述障碍物数量n，并确定任两个所述第一轨迹组合中所述第一预测轨迹p_i,j的组合关系不同；并将各个所述第一轨迹组合作为对应的所述第一融合轨迹组F_k，再由得到的所有所述第一融合轨迹组F_k组成所述第一融合轨迹组集合。

7.根据权利要求3所述的规划控制系统，其特征在于，

所述规划模块具体用于在所述车辆多轨迹预规划处理时，根据所述第一融合轨迹组集合的各个所述第一融合轨迹组F_k进行自车轨迹规划处理生成对应的所述第一规划轨迹τ_k；并由得到的所有所述第一规划轨迹τ_k组成所述第一规划轨迹集合。

8.根据权利要求7所述的规划控制系统，其特征在于，

所述规划模块具体用于在所述自车轨迹规划处理时，以当前第一融合轨迹组F_k的所有所述第一预测轨迹p_i,j为障碍物规避轨迹，基于自车当前行驶状态对自车在未来时段的行驶轨迹进行规划生成对应的所述第一规划轨迹τ_k。

9.根据权利要求3所述的规划控制系统，其特征在于，

所述规划模块具体用于在所述规划轨迹查询处理时，将所述第二障碍物位姿集合的所述第二位姿序列M’_i的起始、结束时间提取出来构成第一时段；并在所述第一融合轨迹组集合中，对各个所述第一预测轨迹p_i,j在所述第一时段的预测轨迹信息进行提取生成对应的第一时段预测轨迹；并对各个所述当前第二位姿序列M’_i对应障碍物的所有所述第一时段预测轨迹进行查询，将与当前第二位姿序列M’_i匹配的所述第一时段预测轨迹作为对应的第一时段匹配轨迹；并在所述第一融合轨迹组集合中，将各个所述第一时段匹配轨迹所在的所述第一预测轨迹p_i,j作为对应的第一匹配预测轨迹，并将所有所述第一匹配预测轨迹所在的所述第一融合轨迹组F_k作为第一匹配轨迹组；并在所述第一规划轨迹集合中，将所述第一匹配轨迹组对应的所述第一规划轨迹τ_k作为第一匹配规划轨迹，并从所述第一匹配规划轨迹中提取所述第一时段之后的规划轨迹信息作为所述第二规划轨迹τ’。

10.根据权利要求1所述的规划控制系统，其特征在于，

所述控制模块可在所述规划模块的同一个工作周期内，多次从所述感知系统获取最新的所述第二障碍物位姿集合向所述规划模块发送，并根据所述规划模块回发的最新的所述第二规划轨迹τ’进行车辆轨迹跟踪与行驶控制。