发明内容
本发明的目的是针对现有技术存在的技术缺陷,提供一种复杂环境下的反馈预测控制方法及其装置、车辆。
为此,本发明提供了一种复杂环境下的反馈预测控制方法,包括以下步骤:
步骤S1,车辆启动自动驾驶:根据上层规划的速度和给定的行车路径,根据预设的控制算法,控制车辆完成速度的控制操作和路径的跟随操作,启动车辆的自动驾驶;
步骤S2,预测车辆未来运动轨迹:根据所述步骤S1确定的控制算法,预测获得车辆将要行驶的预测路径;
步骤S3,环境信息预处理:当车辆沿着预测路径行驶时,实时获取车辆四周的障碍物边界信息或道路边界信息,然后根据所述障碍物边界信息或道路边界信息,在障碍物边界A1或道路边界A2靠近车辆一侧,对应设定平行于障碍物边界A1或道路边界A2且间隔预设安全距离L的安全带B1或者B2;
步骤S4,车辆擦碰检测:检测预测路径上的车辆是否进入安全带B1或者B2与障碍物边界A1或道路边界A2之间的危险区域内,如果进入,判断车辆存在擦碰风险。
优选地,在步骤S2中,通过运用纯跟踪pure pursuit算法进行预测,获得车辆将要行驶的预测路径;
在步骤S2中,纯跟踪pure pursuit算法模型基于车辆运动学,具体如下:
式中,X和Y为车辆的后车轴轴心位置;
式中,l为轴距,
为车辆航向角,v为车辆的后轴轴心速度,w为车辆横摆角速度,δ
f为前轮偏角。
优选地,在步骤S2中,还包括以下步骤:
在预测路径的基础上,当车辆沿着预测路径行驶时,以车辆的后轴落于预测路径上,并且车辆的航向对齐于预测路径的切线方向的方式,向前移动车辆。
优选地,对于步骤S4,具体包括以下步骤:
预先将车辆的车框模型,逐点放在预测路径上进行擦碰风险校验,当车辆的车框模型放在预测路径上时,如果车辆的车框模型进入安全带B1或B2与道路边界或者障碍物边界之间的危险区域内时,判断该车辆具有擦碰风险。
优选地,在步骤S4之后,还包括以下步骤:
步骤S5:当步骤S1所采用的预设的控制算法为纯跟踪pure pursuit算法时,采用第一种控制算法反馈控制方式,对车辆的行驶路径进行矫正,具体包括以下步骤:
第一步,设定步长θh;
第二步,在纯跟踪pure pursuit算法计算的车辆当前角度下,进行第一步采样
具有i个采样角度,i为奇数,顺时针分布当前角度两边,两边各(i-1)/2个采样角度,算法计算角度为第(i+1)/2个角度,其中相邻角度差为采样步长;
第三步,以上一步的采样角度为当前角度,进行第二步采样
具有i
2个采样角度;
第五步,每次i个,n次采样,得到多条分别具有不同采样角度的触须状运动轨迹;
在第五步中,通过三个评价条件对多条运动轨迹进行评价,其中符合三个条件的运动轨迹为最优运动轨迹;这三个评价条件具体为:a、采样得到的运动轨迹无碰撞,b、角度变化最小,c、运动轨迹最短;
第六步,执行最优运动轨迹的第一个角度;
第七步,angle_out=anglepure_pursuit+angle最优采样值,即将最优运动轨迹的第一个角度加在pure pursuit算法算出的角度上,完成反馈矫正过程。
优选地,在步骤S4之后,还包括以下步骤:
步骤S5:当步骤S1所采用的预设的控制算法为现有的PID算法时,采用第二种控制算法反馈控制方式,对车辆的行驶路径进行矫正,具体包括以下步骤:
第一步,Err_dis=max(L-d),选出预测时域中最大偏差值,即计算pid算法的输入;
第三步,angle_out=anglepure_pursuit+angleincrease_pid(t),即最终输出角度计算;
其中,在第一步中,Err_dis是偏差;
在第二步中,increase_pid(t)是pid算法的输出值,依次对应比例环节、积分环节和微分环节;
在第三步中,angle_out为最终输出角度,anglepure_pursuit为步骤S1中采用的原控制算法计算的角度,angleincrease_pid(t)为pid矫正后的角度。
优选地,在步骤S4或S5之后,还包括以下步骤:
步骤S6,控制车辆的行驶姿态:对车辆进行纵向的速度控制,当车辆靠近安全带B1或者B2时,控制车辆减速行驶。
另外,本发明还提供了一种复杂环境下的反馈预测控制装置,其包括以下模块:
车辆启动自动驾驶模块,用于根据上层规划的速度和给定的行车路径,根据预设的控制算法,控制车辆完成速度的控制操作和路径的跟随操作,启动车辆的自动驾驶;
预测车辆未来运动轨迹模块,与车辆启动自动驾驶模块相连接,用于根据所述车辆启动自动驾驶模块确定的控制算法,预测获得车辆将要行驶的预测路径;
环境信息预处理模块,与预测车辆未来运动轨迹模块相连接,用于当车辆沿着预测路径行驶时,实时获取车辆四周的障碍物边界信息或道路边界信息,然后根据所述障碍物边界信息或道路边界信息,在障碍物边界A1或道路边界A2靠近车辆一侧,对应设定平行于障碍物边界A1或道路边界A2且间隔预设安全距离L的安全带B1或者B2;
车辆擦碰检测模块,与环境信息预处理模块相连接,用于检测预测路径上的车辆是否进入所述环境信息预处理模块确定的安全带B1或者B2与障碍物边界A1或道路边界A2之间的危险区域内,如果进入,判断车辆存在擦碰风险。
优选地,还包括以下模块:
控制车辆的行驶姿态模块:对车辆进行纵向的速度控制,当车辆靠近安全带B1或者B2时,控制车辆减速行驶。
另外,本发明还提供了一种车辆,包括前面所述的复杂环境下的反馈预测控制装置。
由以上本发明提供的技术方案可见,与现有技术相比较,本发明提供了一种复杂环境下的反馈预测控制方法及其装置、车辆,其设计科学,能够让车辆发现预测路径上有擦碰风险。
此外,本发明能够在得到环境信息的反馈情况下,通过调整车辆的运动轨迹,避免擦碰,继续进行无人驾驶任务,提高车辆控制能力,具有重大的实践意义。
具体实施方式
为使本发明实现的技术手段更容易理解,下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是,此处所描述的具体实施例仅仅用于解释相关申请,而非对该申请的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与本申请相关的部分。
需要说明的是,在不冲突的情况下,本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
需要说明的是,在本申请的描述中,术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方向或位置关系的术语是基于附图所示的方向或位置关系,这仅仅是为了便于描述,而不是指示或暗示所述装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本申请的限制。
参见图1至图5,本发明提供了一种复杂环境下的反馈预测控制方法,包括以下步骤:
步骤S1,车辆启动自动驾驶:根据上层(例如,汽车行车电脑ECU,即电子控制器单元)规划的速度和给定的行车路径,根据预设的控制算法,控制车辆完成速度的控制操作和路径的跟随操作,启动车辆的自动驾驶;
步骤S2,预测车辆未来运动轨迹:根据所述步骤S1确定的控制算法,预测获得车辆将要行驶的预测路径(即通过使用与步骤S1相同的控制算法来预测获得车辆将要行驶的预测路径,即未来运动轨迹);
步骤S3,环境信息预处理:当车辆沿着预测路径行驶时(具体根据所述步骤S1相同的控制算法),实时获取车辆四周的障碍物边界信息或道路边界信息(属于车辆四周的环境信息),然后根据所述障碍物边界信息或道路边界信息,在障碍物边界A1或道路边界A2靠近车辆一侧,对应设定平行于障碍物边界A1或道路边界A2且间隔预设安全距离L(例如0.5m或者1m)的安全带B1或者B2(参见图3、图4所示);步骤S4,车辆擦碰检测:检测预测路径上的车辆是否进入安全带B1或者B2与障碍物边界A1或道路边界A2之间的危险区域内,如果进入,判断车辆存在擦碰风险。
在本发明中,具体实现上,在步骤S1中,车辆启动自动驾驶,可以通过现有的控制算法(或控制算法模块),来完成速度和路径的跟随。
在本发明中,具体实现上,在步骤S1中,控制算法包括两个控制算法,具体为:横向(即车辆的左右方向)控制算法和纵向(即车辆的前后方向)控制算法。其中,关于纵向控制算法,具体通过使用PID方法,控制车辆来跟随上层规划的速度;关于横向控制算法,横向控制算法的主体,通过使用现有的纯跟踪pure pursuit算法(作为路径跟踪算法),控制车辆完成上层给定的路径跟随。
在本发明中,具体实现上,在步骤S2中,通过运用纯跟踪pure pursuit算法进行预测,获得车辆将要行驶的预测路径。
需要说明的是,在步骤S2中,使用与步骤S1相同的控制算法,来预测车辆的未来运动轨迹。
在步骤S2中,预测的控制算法(即纯跟踪pure pursuit算法)模型基于车辆运动学,具体如下:
式中,X和Y为车辆的后车轴轴心位置;
式中,l为轴距,
为车辆航向角,v为车辆的后轴轴心速度,w为车辆横摆角速度,δ
f为前轮偏角。
在步骤S2中,需要说明的是,对于本发明,可以在基于车辆运动学模型基础上,通过运用pure pursuit算法进行多步预测,可以得到图2虚线所示的预测路径。
在本发明中,具体实现上,在步骤S2中,还包括以下步骤:
在预测路径的基础上,当车辆沿着预测路径行驶时,以车辆的后轴落于预测路径上,并且车辆的航向(即向前行驶方向)对齐于预测路径的切线方向的方式,向前移动车辆(具体为车辆的车框模型)。
需要说明的是,对于本发明,在步骤S3中,能够在采集外部反馈的障碍物边界A1信息或者道路边界A2信息的基础上,对应设定平行于边界且间隔一定距离L的安全带B1或B2,如图3、图4所示。
对于本发明,可以根据障碍物边界信息或者道路边界信息,计算预测路径上车辆与道路边界d1,或者车辆与障碍物边界的距离d2。
在本发明中,障碍物边界,是通过现有的感知、预测等设备(如激光雷达等设备),感知检测到的障碍物激光点云。
关于道路边界,其中,凸起且能被现有的感知模块发现的道路边界,可当障碍物边界同等对待,而边界是坑或者感知识别不到的道路边界,需要由现有的高精度地图提供。
对于本发明,障碍物边界和道路边界这两个边界,都可以由上层模块提供,是车辆坐标系下一串点的信息。
需要说明的是,障碍物边界信息和道路边界信息,是作为环境信息预处理模块的控制模块的上层模块提供的,作为环境信息预处理模块的控制模块接收过来的是基于车辆坐标下的一连串点的信息,这些信息得到后,作为环境信息预处理模块的控制模块可以划定危险区域(安全带B1或B2),这样才能得到d1和d2距离值。
需要说明的是,通过现有的感知的激光算法和高精度地图,都可以提供障碍物边界信息和道路边界信息。也就是说,通过现有的感知的激光算法和高精度地图,获得障碍物或者道路的激光点云信息,即可以获得车辆四周的障碍物边界信息或道路边界信息(属于车辆四周的环境信息)。
在本发明中,具体实现上,对于步骤S3中,还包括以下步骤:
根据车辆的预测路径和障碍物边界信息或道路边界信息,获得预设路径上的车辆与障碍物边界之间的危险间隔距离d1,或者获得预设路径上的车辆与道路边界之间的危险间隔距离d2。
具体操作如下:对于预测路径上的车辆,当车辆在预设路径上的多个位置点进入安全带B1或B2与道路边界或者障碍物边界之间的危险区域时,定义这些位置点为危险点,测量获得危险点与道路边界或者障碍物边界之间的垂直距离,作为危险间隔距离d1或危险间隔距离d2;
其中,垂直距离最短的位置点,为最危险点。
需要说明的是,对于本发明,可以预测一段路,把车辆模型往上一摆,然后进行擦碰风险校验,就可以得到深入危险区域的车辆上最危险的那个位置点,做位置点到直线的运算,就得到了d1,d2的值。位置点是车辆的危险点,危险点附近一小段边界或者膨胀的边界近似作为直线。步骤S3进行环境信息处理得到的障碍物轮廓,可以给下文的第一种方式进行采样矫正使用,算出来的d1和d2,可以用来给下文的第二种方式(PID调节方式)反馈矫正使用。
在本发明中,具体实现上,对于步骤S4,具体包括以下步骤:
车辆模型校验:预先将车辆的车框模型,逐点放在预测路径上进行擦碰风险校验,当车辆的车框模型放在预测路径上时,如果车辆的车框模型(如车辆的车框)进入安全带B1或B2与道路边界或者障碍物边界之间的危险区域内时,判断该车辆具有擦碰风险。
需要说明的是,对于本发明,具体实现上,可以在障碍物或者道路周边划定危险区域(即安全带),危险区域(即安全带)不同于障碍物轮廓,可以认为对障碍物做了膨胀处理,膨胀出来的区域为危险区域,在预测路径上校验车模型时,车模型进入划定的危险区域(即安全带)内,则认为有擦碰风险。危险区域就是安全带,车辆进入危险区域(即安全带B1或B2,与道路边界或者障碍物边界之间的区域),就需要开始矫正角度,否则会撞路边界或者障碍物。
在本发明中,具体实现上,在步骤S4之后,还包括以下步骤:
步骤S5:当步骤S1所采用的预设的控制算法为纯跟踪pure pursuit算法时,采用第一种控制算法反馈控制方式,对车辆的行驶路径进行矫正,基于车辆运动学模型,具体包括以下步骤:
第一步,设定步长θh(步长即车辆在单位时间行驶的距离,例如1秒行驶的距离);
第二步,在纯跟踪pure pursuit算法计算的车辆当前角度下,进行第一步采样
具有i个采样角度,i为奇数,顺时针分布当前角度两边,两边各(i-1)/2个采样角度,算法计算角度为第(i+1)/2个角度,其中相邻角度差为采样步长,例如:
第三步,以上一步的采样角度为当前角度,进行第二步采样
具有i
2个采样角度;
第五步,每次i个,n次采样,参见图5所示,得到多条分别具有不同采样角度的触须状运动轨迹,即车辆的多条未来运动轨迹。需要说明的是,采样矫正是避开危险的一种方式,触须状的路线是采样角度可能出现的行驶轨迹,图5中画曲线便于理解,如果触须状运动轨迹,存在碰撞问题,则直接删除掉。
在本发明中,在第五步中,通过三个评价条件对多条运动轨迹进行评价,其中符合三个条件的运动轨迹为最优运动轨迹;这三个评价条件具体为:a、采样得到的运动轨迹无碰撞,b、角度变化最小(运动轨迹与车辆当前路径之间的夹角),c、运动轨迹最短。
在本发明中,触须状的轨迹是采样角度时得出来的,为的是最优角度选择,第一个条件说的是碰撞检测,是用来得到最优的当前采样角度。
具体实现上,可以设定一个评价函数,评价函数包括这三个条件,计算评价函数最小。评价函数通过设定不同的权值,来决定更倾向哪个条件。
需要说明的是,对于本发明,在之前的步骤S2中,只是预测了一条路径,后续用两种方式做反馈矫正。一种方式是,pid矫正,一种是采样矫正,pid和采样的输入都是上层输出的危险障碍物边界和道路边界做反馈。
第六步,执行最优运动轨迹的第一个角度。
第七步,angle_out=anglepure_pursuit+angle最优采样值,即:采样矫正角度,得到最优第一个角度,将最优运动轨迹的第一个角度加在pure pursuit算法算出的角度上,完成反馈矫正过程。
在本发明中,具体实现上,在步骤S4之后,还包括以下步骤:
步骤S5:当步骤S1所采用的预设的控制算法为现有的PID算法时,采用第二种控制算法反馈控制方式,对车辆的行驶路径进行矫正,具体为:基于上层PID控制器的输入指令,运用现有的PID方法,采用PID调节方式,实现反馈矫正。
第二种控制算法反馈控制方式,具体包括以下步骤:
第一步,Err_dis=max(L-d),选出预测时域中最大偏差值,即计算pid算法的输入;
在本发明中,L-d得到的偏差值是pid算法的输入,L为预设安全距离,d为d1或者d2(如前文所述)。
第三步,angle_out=anglepure_pursuit+angleincrease_pid(t),即最终输出角度计算;
通过调整kp、ki和kd三个参数,就能得到较好的避让效果。
需要说明的是,对于本发明,和前面的第一种控制算法反馈控制方式一样,本发明的第二种控制算法反馈控制方式,也是在控制算法计算的角度基础上加上反馈矫正角度,只是第二种控制算法反馈控制方式反馈矫正的方式是pid方法。
在第一步中,Err_dis是偏差,L为与障碍物边界A1或道路边界A2所间隔的预设安全距离L,是安全线的距离(远离障碍物的一侧是安全区域,靠近障碍物一侧是危险区域);
在第二步中,increase_pid(t)是pid算法的输出值,在公式中依次对应比例环节、积分环节和微分环节。
在第三步中,angle_out为最终输出角度,anglepure_pursuit为原控制算法(即步骤S1中所采用的预设的控制算法)计算的角度,angleincrease_pid(t)为pid矫正后的角度。
在本发明中,kp、ki和kd三个参数这三个参数是算法可调参数,其中Kd是比例参数,ki是积分参数,kd是微分参数。
需要说明的是,对于本发明,Pid矫正是根据车辆未来可能深入危险区域的程度,反向给个修正角度,深入危险区域越深(L-d越大),pid修正角度值越大。
在本发明中,具体实现上,在步骤S4或步骤S5之后,还包括以下步骤:
步骤S6,控制车辆的行驶姿态:对车辆进行纵向(即车辆前后方向)的速度控制,当车辆靠近安全带B1或者B2时(例如车辆与安全带B1或者B2的距离为预设距离,如1米),控制车辆减速行驶。
需要说明的是,对于本发明,通过对车辆进行纵向的速度控制,在靠近安全带附近减速处理,以便于横向避让擦碰无效时,让驾驶员紧急停车。
对于本发明,对车辆进行横向(即车辆的左右方向)的行驶路径控制,可以基于现有的PID方法或者现有的采样方法(例如现有的纯跟踪pure pursuit算法)。
其中,对车辆横向控制输出,采取的采样算法(方法)可以为:angle_out=anglepure_pursuit+angle最优采样值,为现有的算法;
对车辆横向控制输出,采取的PID算法(方法)可以为:angle_out=anglepure_pursuit+increase_pid(t),为现有的算法。
此外,基于以上本发明提供的一种复杂环境下的反馈预测控制方法,为了执行上述复杂环境下的反馈预测控制方法,本发明还提供了一种复杂环境下的反馈预测控制装置,该装置包括以下模块:
车辆启动自动驾驶模块,用于根据上层(例如,汽车行车电脑ECU,即电子控制器单元)规划的速度和给定的行车路径,根据预设的控制算法,控制车辆完成速度的控制操作和路径的跟随操作,启动车辆的自动驾驶;
预测车辆未来运动轨迹模块,与车辆启动自动驾驶模块相连接,用于根据所述车辆启动自动驾驶模块确定的控制算法,预测获得车辆将要行驶的预测路径(即通过使用与车辆启动自动驾驶模块所使用的相同控制算法,来预测获得车辆将要行驶的预测路径,即未来运动轨迹);
环境信息预处理模块,与预测车辆未来运动轨迹模块相连接,用于当车辆沿着预测路径行驶时(根据所述车辆启动自动驾驶模块确定的控制算法),实时获取车辆四周的障碍物边界信息或道路边界信息(属于车辆四周的环境信息),然后根据所述障碍物边界信息或道路边界信息,在障碍物边界A1或道路边界A2靠近车辆一侧,对应设定平行于障碍物边界A1或道路边界A2且间隔预设安全距离L(例如0.5m或者1m)的安全带B1或者B2(参见图3、图4所示);
车辆擦碰检测模块,与环境信息预处理模块相连接,用于检测预测路径上的车辆是否进入所述环境信息预处理模块确定的安全带B1或者B2与障碍物边界A1或道路边界A2之间的危险区域内,如果进入,判断车辆存在擦碰风险。
在本发明中,具体实现上,本发明提供的一种复杂环境下的反馈预测控制装置,还包括以下模块:
控制车辆的行驶姿态模块:对车辆进行纵向(即车辆前后方向)的速度控制,当车辆靠近安全带B1或者B2时(例如车辆与安全带B1或者B2的距离为预设距离,如1米),控制车辆减速行驶。
需要说明的是,对于本发明,通过对车辆进行纵向的速度控制,在靠近安全带附近减速处理,以便于横向避让擦碰无效时,让驾驶员紧急停车。
另外,本发明还提供了一种车辆,所述车辆包括前面所述的复杂环境下的反馈预测控制装置。
基于以上技术方案可知,与现有技术相比较,对于本发明,不仅丰富了控制模块的预测功能,还能够提升了车辆无人驾驶的控制能力,进而提高无人车辆的安全性。
对于本发明,其提出了基于环境信息反馈和控制预测,有效提升车辆控制能力的方式。
综上所述,与现有技术相比较,本发明提供的一种复杂环境下的反馈预测控制方法及其装置、车辆,其设计科学,能够让车辆发现预测路径上有擦碰风险。
此外,本发明能够在得到环境信息的反馈情况下,通过调整车辆的运动轨迹,避免擦碰,继续进行无人驾驶任务,提高车辆控制能力,具有重大的实践意义。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。