具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是,此处所描述的具体实施例仅仅用于解释本发明,而非对本发明的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与本发明相关的部分而非全部结构。
在更加详细地讨论示例性实施例之前应当提到的是,一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各步骤描述成顺序的处理,但是其中的许多步骤可以被并行地、并发地或者同时实施。此外,各步骤的顺序可以被重新安排。当其操作完成时处理可以被终止,但是还可以具有未包括在附图中的附加步骤。处理可以对应于方法、函数、规程、子例程、子程序等等。
此外,术语“第一”、“第二”等可在本文中用于描述各种方向、动作、步骤或元件等,但这些方向、动作、步骤或元件不受这些术语限制。这些术语仅用于将第一个方向、动作、步骤或元件与另一个方向、动作、步骤或元件区分。术语“第一”、“第二”等而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中,“多个”、“批量”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
实施例一
图1为本发明实施例一的车辆轨迹跟踪方法流程图,本实施例可以适用于无人驾驶车辆的轨迹跟踪过程。
具体的,如图1所示,实施例一步骤如下:
S110、获取车辆的行驶信息,所述行驶信息包括当前位姿和控制延时。
无人驾驶车辆在轨迹跟踪过程中,其用于控制车辆行驶的相应命令从生成到由执行机构执行具有一定延时,即控制程序发出控制命令到执行机构开始响应相关控制命令时,车辆已经移动一段距离,当车辆高速行驶时,其延时问题对车辆平顺性和控制精度的影响尤为明显。因而本实施例中,在对车辆进行轨迹跟踪时,需要考虑到控制延时这一参数造成的实际影响,在车辆的轨迹跟踪过程中,不断获取车辆的行驶信息,行驶信息中包括车辆的当前位姿和控制延时,当然也包括车辆的行驶速度、加速度、航向角等参数,上述参数举例并非限制。
S120、根据所述当前位姿和所述控制延时确定车辆的虚拟位姿。
本实施例中在解决控制延时对车辆轨迹跟踪的影响时,建立了用于分析车辆运动情况的第一预设模型,第一预设模型为车辆运动学简化模型,其假设无人驾驶车辆在运动过程中前后轴左右轮侧偏刚度保持一致,且在短时间内可以将车辆看作一恒定加速度在运动。
具体的,根据所述当前位姿和所述控制延时确定车辆的虚拟位姿包括(图未示):
S121、通过第一预设模型基于车辆的行驶速度和航向角以及所述控制延时确定所述控制延时时段内的位姿改变量。
S122、基于所述当前位姿和所述位姿改变量确定所述虚拟位姿。
具体的,本实施例提供了如图2所示车辆运动学简化模型,车辆运动学简化模型将四轮车辆简化为两轮车辆,δf、δr分别为前后轮转角,lf、lr分别为车辆重心到前后轴之间的距离,υt为车辆t时刻速度,ψt为车辆t时刻航向角,β为车辆速度方向与车辆纵轴方向间的夹角,at为车辆t时刻加速度,dt为时间改变量即控制延时,Xt(xt,yt,ψt)为t时刻车辆的姿态信息即当前位姿,X't(x't,y't,ψ't)为执行机构开始执行t时刻控制指令时车辆的姿态信息即虚拟位姿,相应的位姿改变量为:
xt+1=xt+υtcos(ψt+β)dt (1)
yt+1=yt+υtsin(ψt+β)dt (2)
ψt+1=ψt+υtsin(β)/lrdt (3)
υt+1=υt+atdt (4)
β=tan-1(lr/lf+lrtan(δf)) (5)
S130、基于车辆的预设规划路线和所述虚拟位姿确定车辆的期望速度和第一方向盘转角。
在确定了车辆的虚拟位姿后,便可以知道当前时刻下发的控制指令其生效时刻车辆的位姿信息,即当前时刻计算得到的用于轨迹跟踪的控制量,对应的是车辆在虚拟位姿处轨迹跟踪需要的控制量,控制量具体包括期望速度和第一方向盘转角。在计算用于轨迹跟踪的控制量时,最优的控制量是能够使得虚拟位姿和预设规划路线间无横向误差,并基于预设规划路线上和虚拟位姿间横向距离最小参考点确定参考线,能够使得参考线在参考点的方向与虚拟位姿对应的航向角之间的误差尽可能小。
S140、根据所述期望速度和所述第一方向盘转角控制车辆位移。
在确定了车辆对应虚拟位姿的期望速度和第一方向盘转角后,将对应的控制量通过控制指令下达给执行机构使得车辆在到达虚拟位姿时按照期望速度和第一方向盘转角进行位移实现轨迹跟踪。
本实施例提供的车辆轨迹跟踪方法,先通过当前位姿和控制延时确定车辆的虚拟位姿,再基于虚拟位姿和车辆的预设规划路线确定车辆的期望速度和第一方向盘转角,根据期望速度和第一方向盘转角控制车辆位移实现轨迹跟踪,该方法能够使得车辆行驶轨迹与预设规划路线更接近,通过设置虚拟位姿降低了执行机构延时导致的误差,既能够满足轨迹跟踪的实时性与准确性要求,又具有控制精度高的特点。
实施例二
本发明实施例二在实施例一的基础上对部分内容作了进一步补充,具体如下:
本实施例中是基于两点预瞄控制模型确定用于轨迹跟踪的控制量,如图3所示,基于车辆的预设规划路线和所述虚拟位姿确定车辆的期望速度和第一方向盘转角,包括:
S131、基于所述虚拟位姿和所述预设规划路线确定第一预瞄点和第二预瞄点。
本实施例中,第一预瞄点为自虚拟位姿点经过预瞄时间之后车辆运动到达的位置,第二预瞄点为所述预设规划路线的最大曲率控制点,示例性的,如图4所示滚动规划控制模型,基于虚拟位姿X't和预设规划路线S,给出两个预瞄点Q1、Q2,预瞄点Q1到X't的直线距离dt1为:
dt1=υ'tT0 (6)
其中T0为预瞄时间,υ't为t时刻位姿补偿后(即虚拟位姿)的车辆速度。
具体的,第二预瞄点的确定过程为(图未示):
S1311、确定根据所述虚拟位姿和所述预设规划路线确定第一参考点和第二参考点,所述第一参考点为所述预设规划路线上到所述虚拟位姿距离最小的点,所述第二参考点为实时参考轨迹约束点。
S1312、根据所述第一参考点和所述第二参考点确定第一直线方程。
S1313、确定所述预设规划路线上所述第一参考点和所述第二参考点之间偏离所述第一直线方程最大的轨迹点,将所述轨迹点作为所述第二预瞄点。
如图5所示的轨迹跟踪控制模型,P'
t为X'
t到预设规划路线距离最小的点,P”
t为实时参考轨迹约束点,将P'
t点至P”
t点间预设规划路线作为t时刻研究对象,P'
t点至P”
t点间距离
为:
其中T
1为参考轨迹选择时间长度。
将P't点至P”t点连接成直线,构成直线方程,得到P't点至P”t点间预设规划路线偏离直线方程最大的轨迹点Q2作为第二预瞄点。
S132、基于所述第二预瞄点在所述预设规划路线上截取第一轨迹。
第一轨迹为预设规划路线上的一小段路线,其中包括第二预瞄点。
S133、基于所述第一轨迹确定车辆在所述虚拟位姿处的所述期望速度。
具体的,步骤S133包括:根据所述第一轨迹拟合得到第一拟合圆;基于所述第一拟合圆确定车辆位于所述第二预瞄点的最大车速;根据所述虚拟位姿到所述第二预瞄点的距离、所述最大车速和车辆的加速度确定所述期望速度。
示例性的,在如图5所示的轨迹跟踪控制模型中,在第二预瞄点Q2附近截取一段距离(第一轨迹),构成点集M{Pt-begin,...,Pt-limit},使用最小二乘法拟合成圆,得到圆心Ot(x,y)。
使用点集M{Pt-begin,...,Pt-limit}拟合得到第一拟合圆:
令:
上式中,x
i、y
i为点集M{P
t-begin,...,P
t-limit}中点的横纵坐标,
R分别为拟合圆的圆心坐标和半径。根据上式可得到第一拟合圆的中心的半径。M
11、M
12、M
21、M
22、H
1、H
2、A、B、C为中间变量,无实际含义,结合车辆的预设减速度a
0整理可得车辆在X
t点处的期望速度υ
t-hope:
其中,
为根据Q
2点处根据拟合圆半径R得到的最大车速,
为半径调节系数,
为点X'
t到Q
2点之间的距离。
S134、基于所述第一预瞄点确定车辆在所述虚拟位姿处的所述第一方向盘转角。
具体的,步骤S134、基于所述第一预瞄点确定车辆在所述虚拟位姿处的所述第一方向盘转角,包括(图未示):
S1341、根据所述虚拟位姿和所述第一预瞄点确定车辆在所述虚拟位姿的最优方向盘转角。
根据“Aeklman”几何关系,车辆轨迹曲率与方向盘转角成正比:
上式中,θ为方向盘转角,i为方向盘传动比,L为车辆轴距,R'
t为t时刻位姿补偿后(虚拟位姿)的车辆转弯半径,
为车辆侧向加速度,再结合第一预瞄点的选取位置可知,经过预瞄时间T
0,车辆自X'
t运动到Q
1点,分析车辆的y轴方向的位移:
根据公式18-20,“Aeklman”几何关系的最优方向盘转角θhope-l为:
S1432、获取航向误差和航向角,根据所述航向误差和航向角调整所述最优方向盘转角得到第一方向盘转角。
根据最优方向盘转角、航向角以及航向角误差确定的第一方向盘转角θhope为:
上式中,k2为最优方向盘误差影响因子,k3为航向误差影响因子,二者由实际经验和车辆实验数据进行综合确定。
更具体的,在一些实施例中,在得到第一方向盘转角后,考虑到执行机构的执行精度,需要进行误差补偿得到用于确定实际控制量的第二方向盘转角,具体如图6所示,步骤S140包括步骤S141-142:
S141、基于所述第一方向盘转角通过PID控制进行误差补偿得到第二方向盘转角。
e(t)=θhope-θ (24)
上式中,kp、ki、kd分别为比例、积分、微分环节的系数,e(t)为t时刻第一方向盘转角与实际方向盘转角之间的误差,μ(t)为t时刻发送给方向盘的实际控制量,T为考虑的历史数据时长。
S142、以所述期望速度和所述第二方向盘转角为实际控制量控制车辆位移。
本实施例中,实际控制量包括用于控制车辆在虚拟位姿点实现第二方向盘转角的方向盘控制量,以及用于控制车辆在虚拟位姿点速度接近期望速度的速度控制量,其中控制车辆在虚拟位姿点速度接近期望速度包括:当判断车辆在虚拟位姿点的速度大于期望速度时,根据二者的差值调节制动减速度,制动减速度的取值为一个在a0(预设减速度)附近的区间,当判断车辆在虚拟位姿点的速度小于期望速度时,控制车辆速度等于预设速度和期望速度间最小的一个。
更具体的,为了进一步明确体现本申请实施例的有益效果,如图7-9所示为基于仿真实验得到多种车辆轨迹跟踪方法效果图,该仿真实验使用MATALB和Carsim进行联合仿真,图7为纯跟踪算法仿真效果图,图8为MPC算法仿真效果,图9为实施例二车辆轨迹跟踪方法仿真效果图,图7-9中每张图分为上下两部分,上部分图中曲线为预设规划路线S和车辆的实际轨迹S1(图7)、实际轨迹S2(图8)和实际轨迹S3(图9),用于表示车辆的实际轨迹与预设规划路线偏离情况,下部分图中曲线为实际轨迹与预设规划路线的横向误差LE1(图7)、横向误差LE2(图8)和横向误差LE3(图9)。以图7为例,横向误差LE1的横向坐标为时间,纵向坐标为误差值,LE1表示随着时间的增加,车辆在实际轨迹S1上的位置与预设规划路线S上对应的位置间的横向误差变化情况。图10为仿真过程中图7-9对应的上述三种方法的仿真时长对比图,根据图7-10可以明确实施例二中的车辆轨迹跟踪方法实时性较高,控制精度好。
示例性的,在本发明一实施例中,以某品牌电动车作为实车实验平台,搭载一套GNSS定位系统根,一个64线激光雷达,两个16线激光雷达,一个4线激光雷达一个毫米波雷达和2个不同焦距的摄像头,该实施例中,预设速度为30km/h。采用本发明车辆轨迹跟踪方法其预设规划路线S0如图11所示,预设规划路线S0的横坐标为经度,纵坐标为纬度;基于S0使用不同轨迹跟踪方法时的横向误差如图12所示:水平方向坐标表示经度和纬度,竖直方向坐标表示横向误差值,LE4为未采用位姿补偿的车辆轨迹跟踪误差曲线,LE5为本发明的车辆轨迹跟踪误差曲线。
本实施例提供的车辆轨迹跟踪方法,进一步给出了基于两点预瞄的车辆轨迹跟踪方法,在虚拟位姿的基础上,基于第一预瞄点和预设规划路线确定第一方向盘转角,基于第二预瞄点和预设规划路线确定期望速度,从而确定轨迹跟踪过程中车辆虚拟位姿对应的控制量,该方法基于虚拟位姿确定当前时刻发出的控制量,实现了位姿补偿,使得横向误差进一步缩小,满足轨迹跟踪的要求,并且基于两点预瞄的计算方法运算速度较快,满足车辆实时性要求,能够在进入弯道前提前减速,驶出弯道都能够立即加速,速度响应快,控制精度高。
实施例三
图13为本发明实施例三提供的一种车辆轨迹跟踪装置300,具体包括如下模块:
行驶信息获取模块310,用于获取车辆的行驶信息,所述行驶信息包括当前位姿和控制延时。
位姿补偿模块320,用于根据所述当前位姿和所述控制延时确定车辆的虚拟位姿。
更具体的,位姿补偿模块320包括:位姿改变量计算单元,用于通过第一预设模型基于车辆的行驶速度和航向角以及所述控制延时确定所述控制延时时段内的位姿改变量;虚拟位姿确定单元,用于基于所述当前位姿和所述位姿改变量确定所述虚拟位姿。
处理模块330,用于基于车辆的预设规划路线和所述虚拟位姿确定车辆的期望速度和第一方向盘转角。
更具体的,处理模块330包括:预瞄点选择单元,用于基于所述虚拟位姿和所述预设规划路线确定第一预瞄点和第二预瞄点;轨迹截取单元,用于基于所述第二预瞄点在所述预设规划路线上截取第一轨迹;期望速度确定单元,用于基于所述第一轨迹确定车辆在所述虚拟位姿处的所述期望速度;方向盘转角确定单元,用于基于所述第一预瞄点确定车辆在所述虚拟位姿处的所述第一方向盘转角。
更具体的,所述预瞄点选择单元包括:参考点确定子单元,用于确定根据所述虚拟位姿和所述预设规划路线确定第一参考点和第二参考点,所述第一参考点为所述预设规划路线上到所述虚拟位姿距离最小的点,所述第二参考点为实时参考轨迹约束点;直线方程确定单元,用于根据所述第一参考点和所述第二参考点确定第一直线方程。
第二预瞄点确定单元,用于确定所述预设规划路线上所述第一参考点和所述第二参考点之间偏离所述第一直线方程最大的轨迹点,将所述轨迹点作为所述第二预瞄点。
更具体的,期望速度确定单元,包括:
拟合子单元,用于根据所述第一轨迹拟合得到第一拟合圆。
最大车速确定单元,用于基于所述第一拟合圆确定车辆位于所述第二预瞄点的最大车速。
期望速度确定子单元,用于根据所述虚拟位姿到所述第二预瞄点的距离、所述最大车速和车辆的加速度确定所述期望速度。
更具体的,方向盘转角确定单元包括:
最优方向盘转角确定子单元,用于根据所述虚拟位姿和所述第一预瞄点确定车辆在所述虚拟位姿的最优方向盘转角。
方向盘转角误差调整子单元,用于获取航向误差和航向角,根据所述航向误差和航向角调整所述最优方向盘转角得到第一方向盘转角。
控制模块340,用于根据所述期望速度和所述第一方向盘转角控制车辆位移。
更具体的,控制模块340包括:
误差补偿单元,用于基于所述第一方向盘转角通过PID控制进行误差补偿得到第二方向盘转角。
控制量确定单元,用于以所述期望速度和所述第二方向盘转角为实际控制量控制车辆位移。
本实施例提供的车辆轨迹跟踪装置,在虚拟位姿的基础上,基于第一预瞄点和预设规划路线确定第一方向盘转角,基于第二预瞄点和预设规划路线确定期望速度,从而确定轨迹跟踪过程中车辆虚拟位姿对应的控制量,该方法基于虚拟位姿确定当前时刻发出的控制量,实现了位姿补偿,使得横向误差进一步缩小,满足轨迹跟踪的要求,并且基于两点预瞄的计算方法运算速度较快,满足车辆实时性要求,能够在进入弯道前提前减速,驶出弯道都能够立即加速,速度响应快,控制精度高。
实施例四
图14为本发明实施例四提供的一种车辆设备的结构示意图,如图14所示,该设备包括处理器40、存储器41、输入装置42和输出装置43;设备中处理器40的数量可以是一个或多个,图中以一个处理器40为例;设备中的处理器40、存储器41、输入装置42和输出装置43可以通过总线或其他方式连接,图14中以通过总线连接为例。
存储器41作为一种计算机可读存储介质,可用于存储软件程序、计算机可执行程序以及模块,如本发明实施例中的车辆轨迹跟踪方法对应的程序指令/模块(例如,车辆轨迹跟踪装置中的行驶信息获取模块310、位姿补偿模块320、处理模块330和控制模块340等)。处理器40通过运行存储在存储器41中的软件程序、指令以及模块,从而执行设备的各种功能应用以及数据处理,即实现上述任一实施例的车辆轨迹跟踪方法。
存储器41可主要包括存储程序区和存储数据区,其中,存储程序区可存储操作系统、至少一个功能所需的应用程序;存储数据区可存储根据设备的使用所创建的数据等。此外,存储器41可以包括高速随机存取存储器,还可以包括非易失性存储器,例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实例中,存储器41可进一步包括相对于处理器40远程设置的存储器,这些远程存储器可以通过网络连接至设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
输入装置42可用于接收输入的数字或字符信息,以及产生与设备的学员设置以及功能控制有关的键信号输入。输出装置43可包括显示屏等显示设备。
上述设备可执行本发明任意实施例所提供的车辆轨迹跟踪方法,具备执行方法对应的功能能模块和有益效果。
实施例五
本发明实施例五还提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如本发明任意实施例所提供的一种车辆轨迹跟踪方法,该方法可以包括:
获取车辆的行驶信息,所述行驶信息包括当前位姿和控制延时;
根据所述当前位姿和所述控制延时确定车辆的虚拟位姿;
基于车辆的预设规划路线和所述虚拟位姿确定车辆的期望速度和第一方向盘转角;
根据所述期望速度和所述第一方向盘转角控制车辆位移。
本发明实施例的计算机可读存储介质,可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中,计算机可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质,该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。
存储介质上包含的程序代码可以用任何适当的介质传输,包括但不限于无线、电线、光缆、RF等等,或者上述的任意合适的组合。
可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码,程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在学员计算机上执行、部分地在学员计算机上执行、作为一个独立的软件包执行、部分在学员计算机上部分在远程计算机上执行、或者完全在远程计算机或设备上执行。在涉及远程计算机的情形中,远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到学员计算机,或者,可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
本实施例基于雷达进行监测完成车辆轨迹跟踪,受光线和天气影响小,也无需对路面进行破坏,由于雷达采集的数据量小其卷积神经网络无需设计的很复杂,对于运算能力要求低,运算也更快速,算法方法复杂度不高,易于在嵌入式设备上实现,可制作成便携式的车流量监测仪。
注意,上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解,本发明不限于这里的特定实施例,对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此,虽然通过以上实施例对本发明进行了较为详细的说明,但是本发明不仅仅限于以上实施例,在不脱离本发明构思的情况下,还可以包括更多其他等效实施例,而本发明的范围由所附的权利要求范围决定。