CN114877904A - 一种道路曲率的估计方法、测量装置和计算机设备 - Google Patents
一种道路曲率的估计方法、测量装置和计算机设备 Download PDFInfo
- Publication number
- CN114877904A CN114877904A CN202210494713.2A CN202210494713A CN114877904A CN 114877904 A CN114877904 A CN 114877904A CN 202210494713 A CN202210494713 A CN 202210494713A CN 114877904 A CN114877904 A CN 114877904A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- road
- curvature
- coordinate system
- curve
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3807—Creation or updating of map data characterised by the type of data
- G01C21/3815—Road data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/66—Radar-tracking systems; Analogous systems
- G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/88—Radar or analogous systems specially adapted for specific applications
- G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
- G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/80—Technologies aiming to reduce greenhouse gasses emissions common to all road transportation technologies
- Y02T10/84—Data processing systems or methods, management, administration
Abstract
本发明公开了一种道路曲率的估计方法、测量装置和计算机设备,涉及智能汽车领域,针对常见的横向控制算法有预瞄‑跟随理论,经典PID控制方法,驾驶员预瞄跟随理论等,这些控制算法的实现需要获取足够的道路信息和车辆信息。而道路曲率对于控制算法的设计就有一定的影响等问题,现提出如下方案,包括以下步骤:S1:在车辆运动过程中实时获取道路信息以及车辆横摆角信息,所述道路信息包括前方车道线的变化,所述车辆横摆角信息包括横摆角速度的稳定性,获取完成后进入步骤S2。本发明有助于车辆在弯道下更好的实现自动紧急制动功能、自适应巡航控制功能以及车道保持功能,对实现智能辅助驾驶系统的诸多功能。
Description
技术领域
本发明涉及智能汽车领域,尤其涉及一种道路曲率的估计方法、测量装置和计算机设备。
背景技术
自从1885年卡尔本茨发明出第一辆现代汽车以来,汽车行业迅猛发展逐渐成为诸多技术革新的交汇点,电动化、智能化、网联化和共享化开启了汽车的新四化浪潮。智能驾驶技术为代表的汽车智能化是当今飞速发展信息技术的研究热点。智能汽车有环境感知、规划决策和运动控制三大核心技术,每一部分都是智能车辆发挥功能的关键环节。
路径跟踪是实现车辆完成驾驶任务的最后一步,也是整个智能车辆技术的关键一步。弯道行驶工况是车辆行驶过程中常见的路况,大功率道路行驶工况是智能车辆路径跟踪过程中要面对的典型工况。弯道工况对车辆运动控制精度、行驶稳定性和控制实时性要求较高,必要时甚至需要考虑横向和纵向运动控制。本文提出一种估计道路曲率的方法,使得智能车辆在弯道下的自动紧急制动功能、自适应巡航控制功能等均能实现良好的运动效果。
车辆本身是一个复杂、非线性和时变的动力学系统,运动控制更是智能车辆的研究关键问题。运动控制包括纵向控制和横向控制,常见的横向控制算法有预瞄-跟随理论,经典PID控制方法,驾驶员预瞄跟随理论等。这些控制算法的实现需要获取足够的道路信息和车辆信息。而道路曲率对于控制算法的设计就有一定的影响。
发明内容
(一)发明目的
为解决背景技术中存在的技术问题,本发明提出一种道路曲率的估计方法、测量装置和计算机设备,有助于车辆在弯道下更好的实现自动紧急制动功能、自适应巡航控制功能以及车道保持功能,对实现智能辅助驾驶系统的诸多功能有巨大的帮助。
(二)技术方案
本发明提供了一种道路曲率的估计方法,包括以下步骤:
S1:在车辆运动过程中实时获取道路信息以及车辆横摆角信息,所述道路信息包括前方车道线的变化,所述车辆横摆角信息包括横摆角速度的稳定性,获取完成后进入步骤S2;
S2:根据道路信息和车辆横摆角信息判断车辆距离弯路的距离,所述距离分为近距离弯道、中距离弯道以及远距离弯道,判断完成后进入步骤S3或S4或S5;
S3:获取车辆前方远距离静止物体,以车辆为原点计算静止物体与车辆之间的相对位置变化,根据车辆前方远距离静止物体的位置变化估计弯道曲率,所述静止物体包括树木、护栏、路灯以及标识,完成后进入步骤S4;
S4:按照一定的频率采集前方车辆的位置,并建立车辆坐标系,同时选取特定时刻本车辆的位置作为基础坐标系,并将该时刻之后记录的所有车辆坐标系转换到基础坐标系上,并对前方车辆的运动状态进行估计,获取前方车辆运动轨迹的道路曲率,从而估计本车辆前方中距离弯道的道路曲率,完成后进入步骤S5;
S5:根据特定的频率采集本车辆的位置信息,并判断本车辆是都进入弯道区域,当本车辆进入弯道区域内时,根据本车辆横摆角以及道路数据点对道路曲率进行估计。
作为本发明的进一步优化:步骤S3中,设定速度阈值△v和位移阈值△s,并计算前方远距离静止物体与本车辆之间的速度v和位移s,若满足:
则判定前方远距离静止物体与本车辆之间为相对静止,从而得到道路轨迹,基于道路轨迹估计道路曲率。
作为本发明的进一步优化:步骤S4中,车辆坐标系转换基础坐标系的方法为:
假设基础坐标系原点坐标(X0,Y0),θ表示本车辆的第n-1个轨迹点和第n个轨迹点连线与基础坐标系的X轴正方向的夹角,(Xn,Yn)表示本车辆第n个轨迹点在基础坐标系下的坐标。则可以近似表示为:
其中t表示采样周期,wn为本车在第n个轨迹点处的横摆角速度;
本车辆在基础坐标系内的位置坐标为:
其中vn表示本车辆在第n个轨迹点处的速度。
通过坐标转换将本车辆在不同时刻下的坐标投影到基础坐标系上,在此假设车辆坐标系和基础坐标系坐标原点相同,通过检测并生成感兴趣区域在基础坐标系上与雷达检测点融合生成前方车辆坐标点,在基础坐标系下前方车辆坐标系使用牛顿插值法获取前方车辆运动轨迹的曲率,估计车辆前方中距离的道路曲率。
作为本发明的进一步优化:步骤S4中,所述前方车辆运动轨迹道路曲率的获取方法为:
采取类似递推平均滤波的方法,基于当前时刻的采样值和之前的两个时刻采样值建立牛顿插值多项式,获得3个周期内的车辆轨迹曲线并计算处当前时刻曲率,把连续的3个周期的采样值视为一个队列,队列长度固定,将每次采样得到的新数据放入队尾,并舍弃原来位于队首的数据,对新队列的3个数据建立牛顿插值多项式,得到新时刻的道路曲率,采样值为前方车辆的位置。
作为本发明的进一步优化:步骤S5中,所述本车辆位置信息的采集方法为:
按照设定的频率采集本车辆位置信息,以连续3个时刻本车辆位置信息为原始数据计算弯道曲率;
设最早时刻为t0,本车辆位置(x0,y0),则下一时刻本车辆位置为(x1,y1),当前时刻本车辆位置为(x2,y2),采用牛顿向前差分法可以得到预测轨迹:
y=a0+a1(x-x0)+a2(x-x0)(x-x1))
则y″=2a2,y′=a1+a2*x-a2(x0+x1)
作为本发明的进一步优化:所述步骤S4中,还包括基础坐标系的更新,当本车辆与前方车辆重新恢复直线行驶后,对基础坐标系进行更新。
作为本发明的进一步优化:所述恢复直线行驶的判断方法为:
由于存在道路颠簸和车身悬架的影响,设立曲率阈值|Kap|;
当Kap>|Kap|时,认为车辆在进行曲线行驶,否则认为车辆给在进行直线行驶。
作为本发明的进一步优化:步骤S5中,根据车辆横摆角以及道路数据点对道路曲率进行估计的方法为:
X(k+1|k)=AX(k|k)+Bu(k|k)
状态变量分别为道路弧长、车道夹角、道路曲率,输入变量为车辆前轮转角α。
X(k+1|k+1)为下一时刻的观测值,建立转换矩阵H,得到观测值与预测值的残差:ε(k+1)=X(k+1|k+1)-HX(k+1|k)。K时刻后验估计协方差为P(k|k),则下一时刻先验估计协方差P(k+1|k)=AP(k|k)AT+Q,其中Q为过程激励协方差。
增益矩阵K(k+1)=P(k+1|k)HT[H P(k+1|k)HT+R]-1,其中R为测量噪声协方差,测量过程中可知。
K+1时刻后验观测值X(k+1|k+1)=X(k+1|k)+K(k+1)·ε(k+1)
K+1时刻后验估计协方差为P(k+1|k+1)=(I-K(k+1)H)P(k+1|k)。
通过每一时刻预测与更新,最终效果使预测值越来越接近观测值,达到预测道路曲率的目的。
一种道路曲率的测量装置,该测量装置应用于如权利1-8的任意一项所述的一种道路曲率的估计方法,包括:
车辆横摆角传感器,所述车辆横摆角传感器用于获取本车辆横摆角以及道路数据点;
毫米波雷达,所述毫米波雷达用于探测本车辆前方的静置物体;
摄像头,所述摄像头和毫米波雷达均设置在本车辆中轴线上的特定位置,用于获取前方车辆位置坐标和运动学参数。
一种道路曲率的计算机设备,包括:
存储器和处理器,所述储存器用于存储计算机可执行指令,所述处理器用于执行所述计算机可执行指令,该计算机可执行指令被处理器执行时实现权利要求1至8任意一项所述一种道路曲率的估计方法。
与现有技术相比,本发明的上述技术方案具有如下有益的技术效果:
本发明提出一种道路曲率的估计方法,通过将弯路划分为三个区域,不同的区域采用不同的算法来估计道路曲率,包括近距离、中距离和远距离位置弯道,近距离采用卡尔曼滤波方法(KF)预测车辆运动轨迹进而估计道路曲率,中距离弯道以本车前方车辆轨迹作为基础,通过牛顿多项式插值法估计道路曲率,远距离弯道以本车前方远距离静止物体轨迹为基础,进而估计道路曲率。本发明更好的实现自动紧急制动功能、自适应巡航控制功能以及车道保持功能,对实现智能辅助驾驶系统的诸多功能有巨大的帮助。
附图说明
图1为弯道曲率估计算法流程图;
图2为本车辆轨迹曲线及各采样点的曲率半径效果图;
图3为本车辆基础坐标系转换示意图;
图4为卡尔曼滤波算法实现流程图;
图5为牛顿多项式插值法示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明了,下面结合具体实施方式并参照附图,对本发明进一步详细说明。应该理解,这些描述只是示例性的,而并非要限制本发明的范围。此外,在以下说明中,省略了对公知结构和技术的描述,以避免不必要地混淆本发明的概念。
通过图1-5所示,一种道路曲率的估计方法,包括以下步骤:
S1:在车辆运动过程中实时获取道路信息以及车辆横摆角信息,道路信息包括前方车道线的变化,车辆横摆角信息包括横摆角速度的稳定性,获取完成后进入步骤S2;
S2:根据道路信息和车辆横摆角信息判断车辆距离弯路的距离,距离分为近距离弯道、中距离弯道以及远距离弯道,判断完成后进入步骤S3或S4或S5;
S3:获取车辆前方远距离静止物体,以车辆为原点计算静止物体与车辆之间的相对位置变化,根据车辆前方远距离静止物体的位置变化估计弯道曲率,静止物体包括树木、护栏、路灯以及标识,完成后进入步骤S4;
S4:按照一定的频率采集前方车辆的位置,并建立车辆坐标系,同时选取特定时刻本车辆的位置作为基础坐标系,并将该时刻之后记录的所有车辆坐标系转换到基础坐标系上,并对前方车辆的运动状态进行估计,获取前方车辆运动轨迹的道路曲率,从而估计本车辆前方中距离弯道的道路曲率,完成后进入步骤S5;
S5:根据特定的频率采集本车辆的位置信息,并判断本车辆是都进入弯道区域,当本车辆进入弯道区域内时,根据本车辆横摆角以及道路数据点对道路曲率进行估计。
步骤S3中,设定速度阈值△v和位移阈值△s,并计算前方远距离静止物体与本车辆之间的速度v和位移s,若满足:
则判定前方远距离静止物体与本车辆之间为相对静止,从而得到道路轨迹,基于道路轨迹估计道路曲率。
步骤S4中,车辆坐标系转换基础坐标系的方法为:
假设基础坐标系原点坐标(X0,Y0),θ表示本车辆的第n-1个轨迹点和第n个轨迹点连线与基础坐标系的X轴正方向的夹角,(Xn,Yn)表示本车辆第n个轨迹点在基础坐标系下的坐标。则可以近似表示为:
其中t表示采样周期,wn为本车在第n个轨迹点处的横摆角速度;
本车辆在基础坐标系内的位置坐标为:
其中vn表示本车辆在第n个轨迹点处的速度。
通过坐标转换将本车辆在不同时刻下的坐标投影到基础坐标系上,在此假设车辆坐标系和基础坐标系坐标原点相同,通过检测并生成感兴趣区域在基础坐标系上与雷达检测点融合生成前方车辆坐标点,在基础坐标系下前方车辆坐标系使用牛顿插值法获取前方车辆运动轨迹的曲率,估计车辆前方中距离的道路曲率。
步骤S4中,前方车辆运动轨迹道路曲率的获取方法为:
采取类似递推平均滤波的方法,基于当前时刻的采样值和之前的两个时刻采样值建立牛顿插值多项式,获得3个周期内的车辆轨迹曲线并计算处当前时刻曲率,把连续的3个周期的采样值视为一个队列,队列长度固定,将每次采样得到的新数据放入队尾,并舍弃原来位于队首的数据,对新队列的3个数据建立牛顿插值多项式,得到新时刻的道路曲率,采样值为前方车辆的位置。
步骤S5中,本车辆位置信息的采集方法为:
按照设定的频率采集本车辆位置信息,以连续3个时刻本车辆位置信息为原始数据计算弯道曲率;
设最早时刻为t0,本车辆位置(x0,y0),则下一时刻本车辆位置为(x1,y1),当前时刻本车辆位置为(x2,y2),采用牛顿向前差分法可以得到预测轨迹:
y=a0+a1(x-x0)+a2(x-x0)(x-x1))
则y″=2a2,y′=a1+a2*x-a2(x0+x1)
步骤S4中,还包括基础坐标系的更新,当本车辆与前方车辆重新恢复直线行驶后,对基础坐标系进行更新,恢复直线行驶的判断方法为:
由于存在道路颠簸和车身悬架的影响,设立曲率阈值|Kap|;
当Kap>|Kap|时,认为车辆在进行曲线行驶,否则认为车辆给在进行直线行驶。
步骤S5中,根据车辆横摆角以及道路数据点对道路曲率进行估计的方法为:
X(k+1|k)=AX(k|k)+Bu(k|k)
状态变量分别为道路弧长、车道夹角、道路曲率,输入变量为车辆前轮转角α。
X(k+1|k+1)为下一时刻的观测值,建立转换矩阵H,得到观测值与预测值的残差:ε(k+1)=X(k+1|k+1)-HX(k+1|k)。K时刻后验估计协方差为P(k|k),则下一时刻先验估计协方差P(k+1|k)=AP(k|k)AT+Q,其中Q为过程激励协方差。
增益矩阵K(k+1)=P(k+1|k)HT[H P(k+1|k)HT+R]-1,其中R为测量噪声协方差,测量过程中可知。
K+1时刻后验观测值X(k+1|k+1)=X(k+1|k)+K(k+1)·ε(k+1)
K+1时刻后验估计协方差为P(k+1|k+1)=(I-K(k+1)H)P(k+1|k)。
通过每一时刻预测与更新,最终效果使预测值越来越接近观测值,达到预测道路曲率的目的。
一种道路曲率的测量装置,该测量装置应用于如权利1-8的任意一项的一种道路曲率的估计方法,包括:
车辆横摆角传感器,车辆横摆角传感器用于获取本车辆横摆角以及道路数据点;
毫米波雷达,毫米波雷达用于探测本车辆前方的静置物体;
摄像头,摄像头和毫米波雷达均设置在本车辆中轴线上的特定位置,用于获取前方车辆位置坐标和运动学参数。
一种道路曲率的计算机设备,包括:
存储器和处理器,储存器用于存储计算机可执行指令,处理器用于执行计算机可执行指令,该计算机可执行指令被处理器执行时实现权利要求1至8任意一项一种道路曲率的估计方法。
在本发明的描述中,需要理解的是,术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的设备或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中,“多个”的含义是两个或两个以上,除非另有明确具体的限定。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,根据本发明的技术方案及其发明构思加以等同替换或改变,都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种道路曲率的估计方法,其特征在于,包括以下步骤:
S1:在车辆运动过程中实时获取道路信息以及车辆横摆角信息,所述道路信息包括前方车道线的变化,所述车辆横摆角信息包括横摆角速度的稳定性,获取完成后进入步骤S2;
S2:根据道路信息和车辆横摆角信息判断车辆距离弯路的距离,所述距离分为近距离弯道、中距离弯道以及远距离弯道,判断完成后进入步骤S3或S4或S5;
S3:获取车辆前方远距离静止物体,以车辆为原点计算静止物体与车辆之间的相对位置变化,根据车辆前方远距离静止物体的位置变化估计弯道曲率,所述静止物体包括树木、护栏、路灯以及标识,完成后进入步骤S4;
S4:按照一定的频率采集前方车辆的位置,并建立车辆坐标系,同时选取特定时刻本车辆的位置作为基础坐标系,并将该时刻之后记录的所有车辆坐标系转换到基础坐标系上,并对前方车辆的运动状态进行估计,获取前方车辆运动轨迹的道路曲率,从而估计本车辆前方中距离弯道的道路曲率,完成后进入步骤S5;
S5:根据特定的频率采集本车辆的位置信息,并判断本车辆是都进入弯道区域,当本车辆进入弯道区域内时,根据本车辆横摆角以及道路数据点对道路曲率进行估计。
3.根据权利要求1所述的一种道路曲率的估计方法,其特征在于:步骤S4中,车辆坐标系转换基础坐标系的方法为:
假设基础坐标系原点坐标(X0,Y0),θ表示本车辆的第n-1个轨迹点和第n个轨迹点连线与基础坐标系的X轴正方向的夹角,(Xn,Yn)表示本车辆第n个轨迹点在基础坐标系下的坐标,则可以近似表示为:
其中t表示采样周期,wn为本车在第n个轨迹点处的横摆角速度;
本车辆在基础坐标系内的位置坐标为:
其中vn表示本车辆在第n个轨迹点处的速度;
通过坐标转换将本车辆在不同时刻下的坐标投影到基础坐标系上,在此假设车辆坐标系和基础坐标系坐标原点相同,通过检测并生成感兴趣区域在基础坐标系上与雷达检测点融合生成前方车辆坐标点,在基础坐标系下前方车辆坐标系使用牛顿插值法获取前方车辆运动轨迹的曲率,估计车辆前方中距离的道路曲率。
4.根据权利要求1所述的一种道路曲率的估计方法,其特征在于:步骤S4中,所述前方车辆运动轨迹道路曲率的获取方法为:
采取类似递推平均滤波的方法,基于当前时刻的采样值和之前的两个时刻采样值建立牛顿插值多项式,获得3个周期内的车辆轨迹曲线并计算处当前时刻曲率,把连续的3个周期的采样值视为一个队列,队列长度固定,将每次采样得到的新数据放入队尾,并舍弃原来位于队首的数据,对新队列的3个数据建立牛顿插值多项式,得到新时刻的道路曲率,采样值为前方车辆的位置。
5.根据权利要求1所述的一种道路曲率的估计方法,其特征在于:步骤S5中,所述本车辆位置信息的采集方法为:
按照设定的频率采集本车辆位置信息,以连续3个时刻本车辆位置信息为原始数据计算弯道曲率;
设最早时刻为t0,本车辆位置(x0,y0),则下一时刻本车辆位置为(x1,y1),当前时刻本车辆位置为(x2,y2),采用牛顿向前差分法可以得到预测轨迹:
y=a0+a1(x-x0)+a2(x-x0)(x-x1));
则y″=2a2,y′=a1+a2*x-a2(x0+x1);
6.根据权利要求4所述的一种道路曲率的估计方法,其特征在于:所述步骤S4中,还包括基础坐标系的更新,当本车辆与前方车辆重新恢复直线行驶后,对基础坐标系进行更新。
7.根据权利要求5所述的一种道路曲率的估计方法,其特征在于:所述恢复直线行驶的判断方法为:
由于存在道路颠簸和车身悬架的影响,设立曲率阈值|Kap|;
当Kap>|Kap|时,认为车辆在进行曲线行驶,否则认为车辆给在进行直线行驶。
8.根据权利要求1所述的一种道路曲率的估计方法,其特征在于:步骤S5中,根据车辆横摆角以及道路数据点对道路曲率进行估计的方法为:
X(k+1|k)=AX(k|k)+Bu(k|k);
状态变量分别为道路弧长、车道夹角、道路曲率,输入变量为车辆前轮转角α;
X(k+1|k+1)为下一时刻的观测值,建立转换矩阵H,得到观测值与预测值的残差:ε(k+1)=X(k+1|k+1)-HX(k+1|k);K时刻后验估计协方差为P(k|k),则下一时刻先验估计协方差P(k+1|k)=AP(k|k)AT+Q,其中Q为过程激励协方差;
增益矩阵K(k+1)=P(k+1|k)HT[H P(k+1|k)HT+R]-1,其中R为测量噪声协方差,测量过程中可知:
K+1时刻后验观测值X(k+1|k+1)=X(k+1|k)+K(k+1)·ε(k+1);
K+1时刻后验估计协方差为P(k+1|k+1)=(I-K(k+1)H)P(k+1|k);
通过每一时刻预测与更新,最终效果使预测值越来越接近观测值,达到预测道路曲率的目的。
9.一种道路曲率的测量装置,其特征在于,该测量装置应用于如权利1-8的任意一项所述的一种道路曲率的估计方法,包括:
车辆横摆角传感器,所述车辆横摆角传感器用于获取本车辆横摆角以及道路数据点;
毫米波雷达,所述毫米波雷达用于探测本车辆前方的静置物体;
摄像头,所述摄像头和毫米波雷达均设置在本车辆中轴线上的特定位置,用于获取前方车辆位置坐标和运动学参数。
10.一种道路曲率的计算机设备,其特征在于,包括:
存储器和处理器,所述储存器用于存储计算机可执行指令,所述处理器用于执行所述计算机可执行指令,该计算机可执行指令被处理器执行时实现权利要求1至8任意一项所述一种道路曲率的估计方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210494713.2A CN114877904A (zh) | 2022-05-07 | 2022-05-07 | 一种道路曲率的估计方法、测量装置和计算机设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210494713.2A CN114877904A (zh) | 2022-05-07 | 2022-05-07 | 一种道路曲率的估计方法、测量装置和计算机设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114877904A true CN114877904A (zh) | 2022-08-09 |
Family
ID=82672977
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210494713.2A Pending CN114877904A (zh) | 2022-05-07 | 2022-05-07 | 一种道路曲率的估计方法、测量装置和计算机设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114877904A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116142180A (zh) * | 2023-04-14 | 2023-05-23 | 北京裕峻汽车技术研究院有限公司 | 基于毫米波雷达的碰撞风险确定方法 |
-
2022
- 2022-05-07 CN CN202210494713.2A patent/CN114877904A/zh active Pending
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116142180A (zh) * | 2023-04-14 | 2023-05-23 | 北京裕峻汽车技术研究院有限公司 | 基于毫米波雷达的碰撞风险确定方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110471408B (zh) | 基于决策过程的无人驾驶车辆路径规划方法 | |
CN109855639B (zh) | 基于障碍物预测与mpc算法的无人驾驶轨迹规划方法 | |
US10814876B2 (en) | Vehicle lane-changing control method, vehicle lane-changing control device and related equipment | |
CN111688663B (zh) | 机动交通工具及控制其自动驾驶操作的方法 | |
WO2021175313A1 (zh) | 自动驾驶控制方法、装置、车辆及存储介质 | |
US20220080961A1 (en) | Control system and control method for sampling based planning of possible trajectories for motor vehicles | |
CN110361013B (zh) | 一种用于车辆模型的路径规划系统及方法 | |
US8428843B2 (en) | Method to adaptively control vehicle operation using an autonomic vehicle control system | |
WO2019042295A1 (zh) | 一种无人驾驶路径规划方法、系统和装置 | |
US8170739B2 (en) | Path generation algorithm for automated lane centering and lane changing control system | |
CN109270933A (zh) | 基于圆锥曲线的无人驾驶避障方法、装置、设备及介质 | |
WO2017077794A1 (ja) | 車両の走行支援装置 | |
CN112965476B (zh) | 一种基于多窗口模型的高速无人车轨迹规划系统及方法 | |
CN113721637B (zh) | 智能车动态避障路径连续规划方法、系统及存储介质 | |
CN112327830B (zh) | 车辆自动驾驶变道轨迹的规划方法及电子设备 | |
EP3738849A1 (en) | Vehicle control device | |
JP2022515083A (ja) | 自動車車両のための可能な走行経路を決定するハイブリッドアプローチのための制御システムおよび制御方法 | |
CN112947469A (zh) | 汽车换道轨迹规划与动态轨迹跟踪控制方法 | |
CN115520218B (zh) | 自动驾驶车辆四点转弯轨迹规划方法 | |
WO2022217209A1 (en) | Lane changing based only on local information | |
CN114942642A (zh) | 一种无人驾驶汽车轨迹规划方法 | |
CN114877904A (zh) | 一种道路曲率的估计方法、测量装置和计算机设备 | |
CN114852085A (zh) | 基于路权侵入度的车辆自动驾驶轨迹规划方法 | |
JPWO2019003302A1 (ja) | 車両制御装置 | |
CN114228707A (zh) | 一种无人驾驶车辆的防撞方法及系统 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |