CN115871658A - 一种面向稠密人流的智能驾驶速度决策方法和系统 - Google Patents

Abstract

一种面向稠密人流的自动驾驶速度规划方法，包括：首先，将汽车与感知到的行人从笛卡尔坐标系转换到基于参考轨迹的Frenet坐标系；再根据车辆长宽、位置以及姿态等信息构造粗过滤区域，对感知到的行人进行粗过滤；然后，根据期望行驶的路线与行人的状态等信息对粗过滤后留下来的行人进行精过滤；最后，基于保守制动能力的速度决策方法得到自车关于每个通过精过滤的行人的最佳期望速度，并选择最小值作为自车当前最佳期望速度。本发明还包括一种面向稠密人流的自动驾驶速度规划方法。本发明能够使自动驾驶系统在人车混流的场景中产生安全且高效的驾驶行为，解决了自动驾驶汽车面对行人时频繁停车或卡死等问题。

Description

一种面向稠密人流的智能驾驶速度决策方法和系统

技术领域

本发明属于智能驾驶行为决策与规划技术领域，特别涉及了一种稠密人流的智能驾驶速度规划方法和系统。

背景技术

随着智能驾驶技术的进一步发展与应用，人车混流下的智能驾驶是智能驾驶技术商业化进程中亟待解决的问题。典型的人车混流的驾驶场景包括城市交通路口、半封闭的工业园区、社区以及商场。相比道路上的其它环境对象，行人具有脆弱与碰撞代价高的特点。行车速度对行人的安全与心理舒适程度起关键影响，如何同时保证行人的安全与行车的效率是速度决策系统要解决的问题。

速度决策系统基于交通参与者的状态、运动趋势以及未来轨迹等信息计算自动驾驶汽车的期望速度。例如，中国专利CN201811061563.6《一种车辆纵向速度规划方法和纵向速度规划装置》根据车辆与前方目标之间的相对速度与距离进行速度规划；中国专利CN202210679403.8《一种基于社会力模型的交互式自动驾驶速度规划方法》尝试将行人意图以及人与人之间的交互模型加入到速度决策系统中，期望得到更加合理的速度。

现有的速度决策方法的有效性依赖对行人状态与意图的准确估计。真实场景中的行人的行为具有机动性强、不确定性高、预测难度大等特点，导致在实际部署这些速度决策方法时需要限定较低的行驶速度与增加较大的安全距离来确保行人的安全。因此，基于这类方法的智能驾驶系统行驶效率低，甚至会频繁出现被人流长时间锁死停车的状况。

发明内容

本发明的目的在于针对现有方法的不足，提供一种面向稠密人流的智能驾驶速度决策方法和系统。本发明为了同时保证行人的安全与行车的效率，在速度决策时主动引入保守制动与加速能力模型替代现有方法中直接限制距离与速度的方式，在保证行人绝对安全的前提下，同时保证自车在人流中高效率的通行。此外，本发明通过对行人进行粗过滤与精过滤，进一步提升方法的决策效率。本发明的目的是通过以下技术方案来实现：一种面向稠密人流的智能驾驶速度决策方法，包括以下步骤：

步骤S1：基于参考轨迹构建Frenet坐标系，将汽车与感知到的行人从笛卡尔坐标系转换到Frenet坐标系；

步骤S2：根据汽车几何形状、大小、位置以及姿态等信息构造粗过滤区域，对区域内的行人进行保留，得到粗过滤后的行人集合；

步骤S3：根据参考轨迹与行人状态等信息对粗过滤后的行人集合进行精过滤，得到精过滤后的行人集合；

步骤S4：基于保守制动与加速能力的速度决策方法得到汽车关于每个精过滤后的行人的期望速度，并选择其中最小值作为自车当前最佳期望速度。

进一步的，所述步骤S1具体为：

首先，基于汽车V的感知模块收集行人状态得到行人集合，标记为P＝{p₁,p₂,...,p_N}，其中N为当行感知到的行人数量；然后，基于汽车V的轨迹规划模块得到参考轨迹T_V，并基于该曲线T_V构建Frenet坐标系；最后，根据行人p在笛卡尔坐标系下的位置坐标，得到其在Frenet坐标系下的横纵坐标值，标记为(s_p,l_p)，并将(s_p,l_p)加入行人状态，根据汽车V在笛卡尔坐标系下的位置坐标，得到Frenet坐标系下的横纵坐标值，标记为(s_v,l_v)，并将(s_v,l_v)加入汽车V的状态。

进一步的，所述的行人状态包括但不限定于以下信息：

1)笛卡尔坐标系下的位置坐标，标记为(x,y)；

2)笛卡尔坐标系下的运动方向角度，标记为yaw；

3)笛卡尔坐标系下的速度，标记为v；

4)笛卡尔坐标系下行人p的未来轨迹T_p，由一系列带时间戳的(x_t,y_t,yaw_t)点表示。

进一步的，所述的根据笛卡尔坐标系下的位置坐标得到Frenet坐标系下的位置坐标具体为：

笛卡尔坐标系下的位置坐标点为(x,y)，找到曲线T_V上距离(x_p,y_p)最近的点，记为(x_r,y_r)；设定曲线T_V起点处的s＝0，则位置点(x_r,y_r)处的Frenet坐标系纵轴坐标等于曲线从起点到该点的曲线长度s_r，则(x,y)在Frenet坐标系下的横轴坐标值s＝s_r；(x,y)在Frenet坐标系下的纵轴坐标值l＝sign((y_p-y_r)cos(theta_r)-(x_p-x_r)sin(theta_r))((x_p-x_r)²+(y_p-y_r)²)^0.5；其中，sign为符号函数，当输入为正值返回1,当输入为负值返回-1,当输入为0则返回0，theta_r为曲线在位置点(x_r,y_r)处的方向角。

进一步的，所述的步骤S2具体为：

首先，获取V在Frenet坐标系横轴的坐标值s_v，在Frenet坐标系下，作以点(s_v+b,0)为圆心，r为半径，圆心角度范围在[-c,c]的扇形S_F；然后，将代表本车的多边形的各边往远离V几何中心的位置平行移动距离d，各边所在直线的交点构成新的多边形R_D，并将R_D由笛卡尔坐标系转换到Frenet坐标系，记为R_F；最后，取S_F与R_F的并集作为粗过滤区域，记为C_F＝S_F∪R_F，并对P中的行人进行遍历，将Frenet坐标位置在C_F区域中的行人加入粗过滤后的行人集合(P_C)；其中，b，r，d为影响粗过滤区域范围的参数。

进一步的，所述的步骤S3具体为：

遍历P_C中的行人，根据行人p在笛卡尔坐标系下的位置坐标(x_p,y_p)、航运动方向角yaw_p、速度v_p，计算p在Frenet坐标系下的纵轴坐标值对时间t的一阶导数dl/dt＝v_p sin(yaw_p-theta_r)，其中theta_r为曲线T_V上距离位置坐标(x_p,y_p)最近点处的方向角度；若((l-l_safe)>0且dl/dt>0)或((l+l_safe)<0且dl/dt<0)则将p从P_C中删除，其中l_safe是与安全相关的参数；遍历完P_C中所有的行人后，将新的P_C重新记为P_F，表示精过滤后的行人集合。

进一步的，所述的步骤S4具体为：

假定汽车V的实测最大加速度为a_MAX，最大减速度为a_MIN；速度决策过程中采用的最大加速度为a_max＝c₁a_MAX，最大减速度为a_min＝c₂a_MIN，其中，c₁,c₂在区间(0,1)上；初始化最佳期望速度为v_d＝min(v_V+a_maxd_t,v_max)，其中，v_V为汽车V此刻的真实速度，v_max为最大允许速度，d_t为规划周期；

遍历P_F中的行人，进行如下操作：

步骤S41，按时间先后顺序依次取行人p的未来运动轨迹T_p中的点在笛卡尔坐标系下的坐标值(x_ti,y_ti)，得到Frenet坐标系下横纵轴坐标值(s_ti,l_ti)，若|l_ti|<l_safe，则记录t_p＝t_i-t₀，s_p＝s_ti，并退出对T_p后续轨迹点的检查；

步骤S42，基于V的保守加速度a_max，计算V从s_v到达s_p需要的时间，记为t_v；

步骤S43，若t_p<＝t_v，说说明行人p比汽车V更早或同时到达V前方的交汇点，行人具有更高的通行权利，则汽车需要在确保行驶效率的同时，又能确保汽车几何中心点能在距离交汇点s_p前的ds_safe处停车，因此根据V离交汇点的距离ds、期望停车点处汽车V的几何中心与交汇点的曲线长度ds_safe、V当前速度v_v、保守制动能力a_min、保守加速能力a_max以及实测制动能力a_MIN计算与行人p相关的期望速度，记为v_{d_temp}；若t_p>t_v，说明汽车V比行人更早到达V前方的交汇点，具有更高的通行权利，则基于V的保守加速度a_max计算汽车V关于行人p的期望速度v_{d_temp}＝min(v_V+a_max d_t,v_max)；

步骤S44，若v_d>v_{d_temp},则更新汽车V的期望速度v_d＝v_{d_temp}；

若遍历完P_F中的所有行人，返回最佳期望速度v_d。

进一步的，所述的步骤S42具体为：

V在Frenet坐标系的横轴坐标值为s_v，交汇点在Frenet坐标系的横轴坐标值为s_p；

首先，计算汽车V到交汇点的曲线长度为ds＝s_p-s_v；然后，计算汽车V利用保守的最大加速度a_max从当前速度v_V加速到v_max行驶的距离为ds1＝(v_max ²-v_V ²)/2a_max；最后，根据ds与ds1的大小关系，计算V从当前位置到达交汇点的时间t_v；有以下两种情况：

1)当ds>ds1，则t_v＝(v_max-v_V)/a_max+(ds-ds1)/v_max；

2)当ds<＝ds1，则t_v＝((v_V ²+2a_maxds1)^0.5-v_V)/a_max；

进一步的，所述的步骤S43所述的若t_p<＝t_v，根据V离交汇点的距离ds、期望停车点处汽车V的几何中心与交汇点的曲线长度ds_safe、V当前速度v_v、保守制动能力a_min、保守加速能力a_max以及实测制动能力a_MIN计算与行人p相关的期望速度v_{d_temp}具体为：

首先，计算汽车V以保守制动a_min与当前速度v_v减速到0时的移动距离ds_stop＝-v_v ²/(2a_min)；

当ds_stop>＝(ds-ds_safe)时，说明按保守制动能力进行速度规划无法使汽车V在设定的安全位置处停车，根据停车距离(ds-ds_safe)计算实际需要的减速度a_true＝-v_v ²/(2(ds-ds_safe)),再根据a_true计算v_{d_temp}＝v_v+a_trued_t；当ds_stop＝(ds-ds_safe)时，a_true＝a_min；

当ds_stop>(ds-ds_safe)时，说明按保守制动能力进行速度规划能够使车在设定的安全位置前停车，则允许汽车V先加速到v_mid然S后再减速到0,其中，v_mid＝min(((v_v ²a_max-v_v ²a_min-2a_maxa_min(ds_stop-(ds-ds_safe)))/(a_max-a_min))^0.5,v_max)，则v_{d_temp}＝min(v_v+a_maxd_t,v_mid)。

一种面向稠密人流的自动驾驶速度规划系统，包括：

坐标系转换模块，用于基于参考轨迹构建Frenet坐标系，将汽车与感知到的行人从笛卡尔坐标系转换到Frenet坐标系；

粗过滤模块，用于根据汽车几何形状、大小、位置以及姿态等信息构造粗过滤区域，对区域内的行人进行保留，得到粗过滤后的行人集合；

精过滤模块，用于根据参考轨迹与行人状态等信息对粗过滤后的行人集合进行精过滤，得到精过滤后的行人集合；

当前最佳期望速度模块，用于基于保守制动与加速能力的速度决策方法得到汽车关于每个精过滤后的行人的期望速度，并选择其中最小值作为自车当前最佳期望速度。

与现有技术相比，本发明的方法在速度决策时主动引入保守制动与加速能力模型，在保证行人绝对安全的前提下，同时保证自车在人流中高效率的通行。相比现有方法中直接限制距离与速度的方式，主动引入保守制与加速能力模型允许更高的行驶速度与更小的安全距离，从而减小了智能汽车停车的概率。此外，本发明的方法为了提升速度决策的稳定性与效率，设计了精过滤器与精过滤器过滤掉场景中对智能汽车行驶无关的行人。

附图说明

图1是本发明实施例中的面向稠密人流的智能驾驶速度决策方法的流程示意图；

图2是本发明实施例中面向稠密人流的智能驾驶速度决策系统的数据流图；

图3是本发明实施例中粗过滤与精过滤示意图；

图4是本发明实施例中基于单个行人进行速度决策的示意图。

图5是本发明系统的结构示意图。

具体实施方式

为了使本发明的目的、技术方案和技术效果更加清楚明白，以下结合说明书附图和实施例，对本发明作进一步详细说明。应当理解，此处所提供的实施例仅仅以解释发明，并不用于限定本发明。

如图1所示，本发明的一种面向稠密人流的智能驾驶速度决策方法，首先，基于参考轨迹构建Frenet坐标系，将汽车与感知到的行人从笛卡尔坐标系转换到Frenet坐标系；再根据汽车几何形状、大小、位置以及姿态等信息构造粗过滤区域；然后，根据参考轨迹与行人状态等信息对粗过滤后的行人集合进行精过滤；最后，基于保守制动与加速能力的速度决策方法得到汽车关于每个过滤后的行人的期望速度，并选择其中最小值作为自车当前最佳期望速度。

更加具体的，包括以下步骤：

步骤S1：基于参考轨迹构建Frenet坐标系，将汽车与感知到的行人从笛卡尔坐标系转换到Frenet坐标系；

首先，基于汽车V的感知模块收集行人状态得到行人集合，标记为P＝{p₁,p₂,...,p_N}，其中N为当行感知到的行人数量；然后，基于汽车V的轨迹规划模块得到参考轨迹T_V，并基于该曲线T_V构建Frenet坐标系；最后，根据行人p在笛卡尔坐标系下的位置坐标，得到其在Frenet坐标系下的横纵坐标值，标记为(s_p,l_p)，并将(s_p,l_p)加入行人状态，根据汽车V在笛卡尔坐标系下的位置坐标，得到Frenet坐标系下的横纵坐标值，标记为(s_v,l_v)，并将(s_v,l_v)加入汽车V的状态。行人p的状态包括但不限定于笛卡尔坐标系下的位置(x_p,y_p)、运动方向角度yaw_p、速度v_p以及未来轨迹T_p(由一系列带时间戳的(x_t,y_t,yaw_t)点表示)。

根据笛卡尔坐标系下的位置坐标得到Frenet坐标系下的位置坐标具体为：笛卡尔坐标系下的位置坐标点为(x,y)，找到曲线T_V上距离(x_p,y_p)最近的点，记为(x_r,y_r)；设定曲线T_V起点处的s＝0，则位置点(x_r,y_r)处的Frenet坐标系纵轴坐标等于曲线从起点到该点的曲线长度s_r，则(x,y)在Frenet坐标系下的横轴坐标值s＝s_r；(x,y)在Frenet坐标系下的纵轴坐标值l＝sign((y_p-y_r)cos(theta_r)-(x_p-x_r)sin(theta_r))((x_p-x_r)²+(y_p-y_r)²)^0.5；其中，sign为符号函数，当输入为正值返回1,当输入为负值返回-1,当输入为0则返回0，theta_r为曲线在位置点(x_r,y_r)处的方向角。

步骤S2：根据汽车几何形状、大小、位置以及姿态等信息构造粗过滤区域，对区域内的行人进行保留，得到粗过滤后的行人集合；

首先，获取V在Frenet坐标系横轴的坐标值s_v，在Frenet坐标系下，作以点(s_v+b,0)为圆心，r为半径，圆心角度范围在[-c,c]的扇形S_F；然后，将代表本车的多边形的各边往远离V几何中心的位置平行移动距离d，各边所在直线的交点构成新的多边形R_D，并将R_D由笛卡尔坐标系转换到Frenet坐标系，记为R_F；最后，取S_F与R_F的并集作为粗过滤区域，记为C_F＝S_F∪R_F，并对P中的行人进行遍历，将Frenet坐标位置在C_F区域中的行人加入粗过滤后的行人集合(P_C)；其中，b，r，d为影响粗过滤区域范围的参数。

图3左图给出粗过滤步骤的示意图，已知感知到的行人集合P＝{p₁,p₂,...,p₅}，假定参考轨迹以汽车V的几何中心为起点，可得V在Frenet坐标系横(S)轴的坐标值s_v＝0，以A点(b,0)作为圆心，r为半径，圆心角度范围在[-c,c]的扇形S_F如图3左边的灰色扇形区域所示；对汽车V的矩形进行平移距离d得到的矩形R_F如图3左边的灰色矩形区域所示；最终得到的粗过滤区域C_F＝S_F∪R_F如图3左边灰色区域所示。我们将Frenet坐标位置在灰色区域中的行人加入到P_C，得到粗过滤后的行人集合P_C＝{p₂,p₃,p₅}。

步骤S3：根据参考轨迹与行人状态等信息对粗过滤后的行人集合进行精过滤，

得到精过滤后的行人集合；

遍历P_C中的行人，根据行人p在笛卡尔坐标系下的位置坐标(x_p,y_p)、航运动方向角yaw_p、速度v_p，计算p在Frenet坐标系下的纵轴坐标值对时间t的一阶导数dl/dt＝v_p sin(yaw_p-theta_r)，其中theta_r为曲线T_V上距离位置坐标(x_p,y_p)最近点处的方向角度；若((l-l_safe)>0且dl/dt>0)或((l+l_safe)<0且dl/dt<0)则将p从P_C中删除，其中l_safe是与安全相关的参数；遍历完P_C中所有的行人后，将新的P_C重新记为P_F，表示精过滤后的行人集合。

图3右图给出精过滤步骤的示意图，对P_C中的行人p₂、p₃、p₅依次判断其是否满足((l-l_safe)>0且dl/dt>0)或((l+l_safe)<0且dl/dt<0)，也即行人位置不在图3右图所示的灰色区域且行人的运动趋势是远离该灰色区域时，则将行人从P_C中删除。p₂、p₃、p₅中只有p₃满足该条件，最终得到精过滤后的行人集合P_F＝{p₂,p₅}。

步骤S4：基于保守制动与加速能力的速度决策方法得到汽车关于每个精过滤后的行人的期望速度，并选择其中最小值作为自车当前最佳期望速度。

假定汽车V的实测最大加速度为a_MAX，最大减速度为a_MIN；速度决策过程中采用的最大加速度为a_max＝c₁a_MAX，最大减速度为a_min＝c₂a_MIN，其中，c₁,c₂在区间(0,1)上；初始化最佳期望速度为v_d＝min(v_V+a_maxd_t,v_max)，其中，v_V为汽车V此刻的真实速度，v_max为最大允许速度，d_t为规划周期；

遍历P_F中的行人，进行如下操作：

步骤S41，按时间先后顺序依次取行人p的未来运动轨迹T_p中的点在笛卡尔坐标系下的坐标值(x_ti,y_ti)，得到Frenet坐标系下横纵轴坐标值(s_ti,l_ti)，若|l_ti|<l_safe，则记录t_p＝t_i-t₀，s_p＝s_ti，并退出对T_p后续轨迹点的检查；

步骤S42，基于V的保守加速度a_max，计算V从s_v到达s_p需要的时间，记为t_v；假定V在Frenet坐标系的横轴坐标值为s_v，交汇点在Frenet坐标系的横轴坐标值为s_p；首先，计算汽车V到交汇点的曲线长度为ds＝s_p-s_v；然后，计算汽车V利用保守的最大加速度a_max从当前速度v_V加速到v_max行驶的距离为ds1＝(v_max ²-v_V ²)/2a_max；最后，根据ds与ds1的大小关系，计算V从当前位置到达交汇点的时间t_v；有两种情况：1)当ds>ds1，则t_v＝(v_max-v_V)/a_max+(ds-ds1)/v_max；2)当ds<＝ds1，则t_v＝((v_V ²+2a_maxds1)^0.5-v_V)/a_max；

步骤S43，t_p为行人p到达交汇点的时间，t_v为汽车V到达交汇点的时间；若t_p<＝t_v，说说明行人p比汽车V更早或同时到达V前方的交汇点，行人具有更高的通行权利，则汽车需要在确保行驶效率的同时，又能确保汽车几何中心点能在距离交汇点s_p前的ds_safe处停车，因此根据V离交汇点的距离ds、期望停车点处汽车V的几何中心与交汇点的曲线长度ds_safe、V当前速度v_v、保守制动能力a_min、保守加速能力a_max以及实测制动能力a_MIN计算与行人p相关的期望速度，首先，计算汽车V以保守制动a_min与当前速度v_v减速到0时的移动距离ds_stop＝-v_v ²/(2a_min)；当ds_stop>＝(ds-ds_safe)时，说明按保守制动能力进行速度规划无法使汽车V在设定的安全位置处停车，根据停车距离(ds-ds_safe)计算实际需要的减速度a_true＝-v_v ²/(2(ds-ds_safe)),再根据a_true计算v_{d_temp}＝v_v+a_trued_t；当ds_stop＝(ds-ds_safe)时，a_true＝a_min；当ds_stop>(ds-ds_safe)时，说明按保守制动能力进行速度规划能够使车在设定的安全位置前停车，则允许汽车V先加速到v_mid然S后再减速到0,其中，v_mid＝min(((v_v ²a_max-v_v ²a_min-2a_maxa_min(ds_stop-(ds-ds_safe)))/(a_max-a_min))^0.5,v_max)，则v_{d_temp}＝min(v_v+a_maxd_t,v_mid)。；若t_p>t_v，说明汽车V比行人更早到达V前方的交汇点，具有更高的通行权利，则基于V的保守加速度a_max计算汽车V关于行人p的期望速度v_{d_temp}＝min(v_V+a_max d_t,v_max)；

步骤S44，若v_d>v_{d_temp},则更新汽车V的期望速度v_d＝v_{d_temp}；

若遍历完P_F中的所有行人，返回最佳期望速度v_d。

图4以行人p5为例说明了交汇点、汽车V到交汇点的曲线长度为ds、期望停车点处汽车V的几何中心与交汇点的曲线长度ds_safe以及安全距离d_safe的示意图。

本发明能够使自动驾驶系统在人车混流的场景中产生安全且高效的驾驶行为，解决了自动驾驶汽车面对行人时频繁停车或卡死等问题，对于提升自动驾驶在人车混流场景下的安全性和通行效率等方面有显著效果。

本发明还提供了图5所示的一种对应于图1的一种面向稠密人流的自动驾驶速度规划系统的示意结构图。如图5所述，在硬件层面，该一种面向稠密人流的自动驾驶速度规划系统包括处理器、内部总线、网络接口、内存以及非易失性存储器，当然还可能包括其他业务所需要的硬件。处理器从非易失性存储器中读取对应的计算机程序到内存中然后运行，以实现上述图1所述的数据采集的方法。当然，除了软件实现方式之外，本发明并不排除其他实现方式，比如逻辑器件抑或软硬件结合的方式等等，也就是说以下处理流程的执行主体并不限定于各个逻辑单元，也可以是硬件或逻辑器件。

对于一个技术的改进可以很明显地区分是硬件上的改进(例如，对二极管、晶体管、开关等电路结构的改进)还是软件上的改进(对于方法流程的改进)。然而，随着技术的发展，当今的很多方法流程的改进已经可以视为硬件电路结构的直接改进。设计人员几乎都通过将改进的方法流程编程到硬件电路中来得到相应的硬件电路结构。因此，不能说一个方法流程的改进就不能用硬件实体模块来实现。例如，可编程逻辑器件(ProgrammableLogic Device,PLD)(例如现场可编程门阵列(Field Programmable Gate Array，FPGA))就是这样一种集成电路，其逻辑功能由用户对器件编程来确定。由设计人员自行编程来把一个数字系统“集成”在一片PLD上，而不需要请芯片制造厂商来设计和制作专用的集成电路芯片。而且，如今，取代手工地制作集成电路芯片，这种编程也多半改用“逻辑编译器(logiccompiler)”软件来实现，它与程序开发撰写时所用的软件编译器相类似，而要编译之前的原始代码也得用特定的编程语言来撰写，此称之为硬件描述语言(Hardware DescriptionLanguage，HDL)，而HDL也并非仅有一种，而是有许多种，如ABEL(Advanced BooleanExpression Language)、AHDL(Altera Hardware Description Language)、Confluence、CUPL(Cornell University Programming Language)、HDCal、JHDL(Java HardwareDescription Language)、Lava、Lola、MyHDL、PALASM、RHDL(Ruby Hardware DescriptionLanguage)等，目前最普遍使用的是VHDL(Very-High-Speed Integrated CircuitHardware Description Language)与Verilog。本领域技术人员也应该清楚，只需要将方法流程用上述几种硬件描述语言稍作逻辑编程并编程到集成电路中，就可以很容易得到实现该逻辑方法流程的硬件电路。

控制器可以按任何适当的方式实现，例如，控制器可以采取例如微处理器或处理器以及存储可由该(微)处理器执行的计算机可读程序代码(例如软件或固件)的计算机可读介质、逻辑门、开关、专用集成电路(Application Specific Integrated Circuit，ASIC)、可编程逻辑控制器和嵌入微控制器的形式，控制器的例子包括但不限于以下微控制器：ARC 625D、Atmel AT91SAM、Microchip PIC18F26K20以及Silicone Labs C8051F320，存储器控制器还可以被实现为存储器的控制逻辑的一部分。本领域技术人员也知道，除了以纯计算机可读程序代码方式实现控制器以外，完全可以通过将方法步骤进行逻辑编程来使得控制器以逻辑门、开关、专用集成电路、可编程逻辑控制器和嵌入微控制器等的形式来实现相同功能。因此这种控制器可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构。或者甚至，可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。

为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本发明时可以把各单元的功能在同一个或多个软件和/或硬件中实现。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

本领域技术人员应明白，本发明的实施例可提供为方法、系统或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本发明，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。

本发明中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

以上所述仅为本发明的实施例而已，并不用于限制本发明。对于本领域技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims (10)

1.一种面向稠密人流的自动驾驶速度规划方法，其特征在于，包括以下步骤：

步骤S1：基于参考轨迹构建Frenet坐标系，将汽车与感知到的行人从笛卡尔坐标系转换到Frenet坐标系；

步骤S2：根据汽车几何形状、大小、位置以及姿态等信息构造粗过滤区域，对区域内的行人进行保留，得到粗过滤后的行人集合；

步骤S3：根据参考轨迹与行人状态等信息对粗过滤后的行人集合进行精过滤，得到精过滤后的行人集合；

步骤S4：基于保守制动与加速能力的速度决策方法得到汽车关于每个精过滤后的行人的期望速度，并选择其中最小值作为自车当前最佳期望速度。

2.如权利要求1所述的一种面向稠密人流的自动驾驶速度规划方法，其特征在于，所述的步骤S1具体为：

首先，基于汽车V的感知模块收集行人状态得到行人集合，标记为P＝{p₁,p₂,...,p_N}，其中N为当行感知到的行人数量；

然后，基于汽车V的轨迹规划模块得到参考轨迹T_V；并基于该曲线T_V构建Frenet坐标系，

最后，根据行人p在笛卡尔坐标系下的位置坐标，得到其在Frenet坐标系下的横纵坐标值，标记为(s_p,l_p)，并将(s_p,l_p)加入行人状态，根据汽车V在笛卡尔坐标系下的位置坐标，得到Frenet坐标系下的横纵坐标值，标记为(s_v,l_v)，并将(s_v,l_v)加入汽车V的状态。

3.如权利要求2所述的一种面向稠密人流的自动驾驶决策规划方法，其特征在于，所述的行人状态包括但不限定于以下信息：

1)笛卡尔坐标系下的位置坐标，标记为(x,y)；

2)笛卡尔坐标系下的运动方向角度，标记为yaw；

3)笛卡尔坐标系下的速度，标记为v；

4)笛卡尔坐标系下行人p的未来轨迹T_p，由一系列带时间戳的(x_t,y_t,yaw_t)点表示。

4.如权利要求2所述的一种面向稠密人流的自动驾驶速度规划方法，其特征在于，根据笛卡尔坐标系下的位置坐标得到Frenet坐标系下的位置坐标具体为：

笛卡尔坐标系下的位置坐标点为(x,y)，找到曲线T_V上距离(x_p,y_p)最近的点，记为(x_r,y_r)；设定曲线T_V起点处的s＝0，则位置点(x_r,y_r)处的Frenet坐标系纵轴坐标等于曲线从起点到该点的曲线长度s_r，则(x,y)在Frenet坐标系下的横轴坐标值s＝s_r；(x,y)在Frenet坐标系下的纵轴坐标值l＝sign((y_p-y_r)cos(theta_r)-(x_p-x_r)sin(theta_r))((x_p-x_r)²+(y_p-y_r)²)^0.5；其中，sign为符号函数，当输入为正值返回1,当输入为负值返回-1,当输入为0则返回0，theta_r为曲线在位置点(x_r,y_r)处的方向角。

5.如权利要求1所述的一种面向稠密人流的自动驾驶速度规划方法，其特征在于，所述的步骤S2具体为：

首先，获取V在Frenet坐标系横轴的坐标值s_v，在Frenet坐标系下，作以点(s_v+b,0)为圆心，r为半径，圆心角度范围在[-c,c]的扇形S_F；

然后，将代表本车的多边形的各边往远离V几何中心的位置平行移动距离d，各边所在直线的交点构成新的多边形R_D，并将R_D由笛卡尔坐标系转换到Frenet坐标系，记为R_F；

最后，取S_F与R_F的并集作为粗过滤区域，记为C_F＝S_F∪R_F，并对P中的行人进行遍历，将Frenet坐标位置在C_F区域中的行人加入粗过滤后的行人集合(P_C)；

其中，b，r，d为影响粗过滤区域范围的参数。

6.如权利要求1所述的一种面向稠密人流的自动驾驶速度规划方法，其特征在于，所述的步骤S3具体为：

遍历P_C中的行人，进行如下操作：

根据行人p在笛卡尔坐标系下的位置坐标(x_p,y_p)、航运动方向角yaw_p、速度v_p，计算p在Frenet坐标系下的纵轴坐标值对时间t的一阶导数dl/dt＝v_psin(yaw_p-theta_r)，其中theta_r为曲线T_V上距离位置坐标(x_p,y_p)最近点处的方向角度；若((l-l_safe)>0且dl/dt>0)或((l+l_safe)<0且dl/dt<0)则将p从P_C中删除，其中l_safe是与安全相关的参数；

遍历完P_C中所有的行人后，将新的P_C重新记为P_F，表示精过滤后的行人集合。

7.如权利要求2所述的一种面向稠密人流的自动驾驶速度规划方法，其特征在于，所述的步骤S4具体为：

假定汽车V的实测最大加速度为a_MAX，最大减速度为a_MIN；速度规划过程中采用的最大加速度为a_max＝c₁a_MAX，最大减速度为a_min＝c₂a_MIN，其中，c₁,c₂在区间(0,1)上；初始化最佳期望速度为v_d＝min(v_V+a_maxd_t,v_max)，其中，v_V为汽车V此刻的真实速度，v_max为最大允许速度，d_t为规划周期；

遍历P_F中的行人，进行如下操作：

步骤S41，按时间先后顺序依次取行人p的未来运动轨迹T_p中的点在笛卡尔坐标系下的坐标值(x_ti,y_ti)，得到Frenet坐标系下横纵轴坐标值(s_ti,l_ti)，若|l_ti|<l_safe，则记录t_p＝t_i-t₀，s_p＝s_ti，并退出对T_p后续轨迹点的检查；

步骤S42，基于V的保守加速度a_max，计算V从s_v到达s_p需要的时间，记为t_v；

步骤S43，若t_p<＝t_v，说说明行人p比汽车V更早或同时到达V前方的交汇点，行人具有更高的通行权利，则汽车需要在确保行驶效率的同时，又能确保汽车几何中心点能在距离交汇点s_p前的ds_safe处停车，因此根据V离交汇点的距离ds、期望停车点处汽车V的几何中心与交汇点的曲线长度ds_safe、V当前速度v_v、保守制动能力a_min、保守加速能力a_max以及实测制动能力a_MIN计算与行人p相关的期望速度，记为v_{d_temp}；若t_p>t_v，说明汽车V比行人更早到达V前方的交汇点，具有更高的通行权利，则基于V的保守加速度a_max计算汽车V关于行人p的期望速度v_{d_temp}＝min(v_V+a_max d_t,v_max)；

步骤S44，若v_d>v_{d_temp},则更新汽车V的期望速度v_d＝v_{d_temp}；

若遍历完P_F中的所有行人，返回最佳期望速度v_d。

8.如权利要求7所述的一种面向稠密人流的自动驾驶速度规划方法，其特征在于，所述的步骤S42具体为：

V在Frenet坐标系的横轴坐标值为s_v，交汇点在Frenet坐标系的横轴坐标值为s_p；

首先，计算汽车V到交汇点的曲线长度为ds＝s_p-s_v；

然后，计算汽车V利用保守的最大加速度a_max从当前速度v_V加速到v_max行驶的距离为ds2＝(v_max ²-v_V ²)/2a_max；

最后，根据ds与ds1的大小关系，计算V从当前位置到达交汇点的时间t_v；有以下两种情况：

1)当ds>ds1，则t_v＝(v_max-v_V)/a_max+(ds-ds1)/v_max；

2)当ds<＝ds1，则t_v＝((v_V ²+2a_maxds1)^0.5-v_V)/a_max。

9.如权利要求7所述的一种面向稠密人流的自动驾驶速度规划方法，其特征在于，所述的步骤S43所述的若t_p<＝t_v，根据V离交汇点的距离ds、期望停车点处汽车V的几何中心与交汇点的曲线长度ds_safe、V当前速度v_v、保守制动能力a_min、保守加速能力a_max以及实测制动能力a_MIN计算与行人p相关的期望速度v_{d_temp}具体为：

首先，计算汽车V以保守制动a_min与当前速度v_v减速到0时的移动距离ds_stop＝-v_v ²/(2a_min)；

当ds_stop>＝(ds-ds_safe)时，说明按保守制动能力进行速度规划无法使汽车V在设定的安全位置处停车，根据停车距离(ds-ds_safe)计算实际需要的减速度a_true＝-v_v ²/(2(ds-ds_safe)),再根据a_true计算v_{d_temp}＝v_v+a_trued_t；当ds_stop＝(ds-ds_safe)时，a_true＝a_min；

当ds_stop>(ds-ds_safe)时，说明按保守制动能力进行速度规划能够使车在设定的安全位置前停车，则允许汽车V先加速到v_mid然S后再减速到0,其中，v_mid＝min(((v_v ²a_max-v_v ²a_min-2a_maxa_min(ds_stop-(ds-ds_safe)))/(a_max-a_min))^0.5,v_max)，则v_{d_temp}＝min(v_v+a_maxd_t,v_mid)。

10.一种面向稠密人流的自动驾驶速度规划系统，其特征在于，包括：

坐标系转换模块，用于基于参考轨迹构建Frenet坐标系，将汽车与感知到的行人从笛卡尔坐标系转换到Frenet坐标系；

粗过滤模块，用于根据汽车几何形状、大小、位置以及姿态等信息构造粗过滤区域，对区域内的行人进行保留，得到粗过滤后的行人集合；

精过滤模块，用于根据参考轨迹与行人状态等信息对粗过滤后的行人集合进行精过滤，得到精过滤后的行人集合；

当前最佳期望速度模块，用于基于保守制动与加速能力的速度决策方法得到汽车关于每个精过滤后的行人的期望速度，并选择其中最小值作为自车当前最佳期望速度。

Images