CN115027482A - 智能驾驶中的融合定位方法 - Google Patents
智能驾驶中的融合定位方法 Download PDFInfo
- Publication number
- CN115027482A CN115027482A CN202210761697.9A CN202210761697A CN115027482A CN 115027482 A CN115027482 A CN 115027482A CN 202210761697 A CN202210761697 A CN 202210761697A CN 115027482 A CN115027482 A CN 115027482A
- Authority
- CN
- China
- Prior art keywords
- curve
- curvature
- point
- vehicle
- lane line
- 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
- 230000004927 fusion Effects 0.000 title claims abstract description 33
- 238000000034 method Methods 0.000 title claims abstract description 31
- 238000012937 correction Methods 0.000 claims abstract description 22
- 238000004364 calculation method Methods 0.000 claims abstract description 11
- 238000005070 sampling Methods 0.000 claims description 18
- 230000000007 visual effect Effects 0.000 claims description 17
- 230000003137 locomotive effect Effects 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 9
- 238000004806 packaging method and process Methods 0.000 claims description 7
- 238000009795 derivation Methods 0.000 claims description 5
- 238000005516 engineering process Methods 0.000 description 16
- 238000013507 mapping Methods 0.000 description 5
- 238000012986 modification Methods 0.000 description 4
- 230000004048 modification Effects 0.000 description 4
- 230000007547 defect Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 241000283070 Equus zebra Species 0.000 description 1
- 241000282414 Homo sapiens Species 0.000 description 1
- 238000005299 abrasion Methods 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005611 electricity Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000008570 general process Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000001931 thermography Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
- B60W40/06—Road conditions
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
- B60W40/06—Road conditions
- B60W40/072—Curvature of the road
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2556/00—Input parameters relating to data
- B60W2556/35—Data fusion
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2556/00—Input parameters relating to data
- B60W2556/40—High definition maps
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Mathematical Physics (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明涉及智能驾驶中的融合定位方法,包含步骤:输出边界拟合曲线;计算得车道线拟合曲线;得到全局坐标系下的航向角计算曲线;得到对应的车道中线形点信息;得到车道线曲线;得到第一离散型点集合;得到第二离散型点集合;将第一离散型点对应的曲率第一曲率值;得到第一曲率集合;将第二离散型点对应的曲率第二曲率值;得到第二曲率集合;输出基准点;对车辆纵向位置校正;对车辆横向位置校正;对车辆航向角校正。本发明得到教现有技术更准确的拟合模型,且获得的校正点可以更准确的修正车辆当前位置的横坐标;推算出现有技术所不具备的车辆的纵向位置修正信息;进一步修正车辆当前的航向角。
Description
技术领域
本发明涉及智能驾驶技术领域,具体地涉及智能驾驶中的融合定位方法。
背景技术
高精度定位是发展智能自动驾驶不可或缺的底层技术之一;当前对于智能自动驾驶技术领域中,如何才能克服现阶段的技术局限,确保高精度定位的连续性、完好性和高可用性,尚无定论。目前的主流观点集中在视觉定位和雷达传感器这两大类别中。
首先要明确,现阶段人类对于智能自动驾驶的共识在以下四大性能指标:精度:测量值和真实值之间的重合度;完好性:服务不可用时提出告警的能力;连续性:告知客户***正常工作的持续能力;可用性:提供符合指标定位服务的百分比;
现有技术中,视觉定位是指用车载摄像头拍摄环境图像,跟已知的地图元素做对比或以递推方式计算车辆位置的定位方式,可分为绝对定位和相对定位;其中:
绝对定位的素材来源主要有三类:地面印刷物,包括路政部门在道路地面上印刷的车道线、斑马线、导流带、地面文字、地面图标等,只要没有施工涂改或使用磨损,这种语义特征是非常稳定的;空中语义对象,包括道路上空的道路标牌、交通标识、红绿灯等,这些装置的位置基本固定而且语义信息明确,也非常适合做定位;街景,相对来说没有前两种方法主流。
相对定位,也就是现在比较流行的vSLAM(视觉同步定位和建图)和VO(视觉里程计)。这两个词常常一起出现,前者包含后者,一般讨论中都以vSLAM代替,其主要特点是提供后端的回环和优化,但车辆正常行驶时,开出去以后短时间内几乎不会再回到之前去过的地方,回环的用途并不是很大,所以视觉定位中主要使用的还是VO。
vSLAM和VO的理论基础是多视图几何,从上图中可以看出,相机从不同位置拍摄同一个物体得到的图像一定是相似而略有区别的。通过图像处理方法,可以找到两张图像中一一对应的特征点。当匹配的特征点数量足够时,通过求解单应矩阵或本质矩阵即可求得两个相机之间的旋转和平移关系,旋转和平移合称变换。当相机连续采集的数据构成一个视频序列时,求解两两帧之间的变换并组合起来即可得到一条从起始位置出发到当前位置的轨迹。由于得到的是相对轨迹,SLAM本身并不能直接完成定位任务,还需要和绝对定位做融合。可以将其他传感器的数据放到SLAM框架中作为额外约束,也可以将视觉观测或里程计的局部关系作为约束输出给其他定位框架。
无论采用哪种方式,视觉定位的一般流程可分为四步:摄像头采集图像、图像预处理、提取图像特征或提取语义、用多视图几何和优化方法求解位姿。视觉定位任务中的摄像头需要考虑多种硬件因素。比如,为了让视觉算法在夜间也能运行,可选用红外相机、星光相机,甚至热成像相机;为了覆盖不同的视场范围,可选用广角镜头、鱼眼镜头、全视相机等;车载摄像头有多种安装位置和多种数量配置,用于定位任务的主要是前视单目或前视双目。
现有技术视觉定位的缺陷在于:
1.由于视场角小时取像范围小,从而并不利于定位;
2.而由于视场角大时取像范围大,虽然有利于看到更多的东西,但对同样尺寸的CCD靶面,如果装载了视场角较大的镜头,那么图像中每个物体的尺寸都要小得多,从而大视场角不利于完成某些视觉任务;
3.如果采用单目摄像头,则还有一个缺陷,就是无法分辨物体的尺度。
现有技术中,雷达传感器技术有很多种,目前主流用于车辆定位的是激光雷达。AGV或机器人定位导航中常用的是二维激光雷达,可将其定位原理简单理解为上方有一束激光向下照射,下方是一个镜子不断旋转将激光转为横向扫描,激光打在不同距离物体上返回时间不同,据此就可以在扫描平面上获得***环境的轮廓。不过,在自动驾驶领域,用的最多的还是三维激光雷达。
三维激光雷达的原理是:多束激光的发射管和接收管按不同角度排列,中间装有隔离板,按照一定的顺序交错发射避免相互干扰,光源和接收器组件旋转起来之后,即可获得对周围环境的多线扫描结果,形成一个三维坐标系中点的集合,称之为点云。
现有技术中,激光雷达定位可分为有图定位和无图定位两类;其中:
有图定位分为建图和用图两个步骤:建图时将点云逐帧叠加在车辆的运行轨迹上即可获得点云地图。这里所说的轨迹,可以是高精度组合惯导***输出的轨迹,也可以是点云SLAM输出的轨迹。
无图定位则和视觉里程计类似,将点云两两帧之间匹配并组合后可以构造一个点云里程计实现相对定位,例如开源软件Autoware里的点云定位模块。也可以提取点云的平面特征和角点特征做匹配,构建点云特征里程计实现定位,例如开源算法LOAM。
雷达定位技术的缺陷在于:
1.由于点云直接建图很可能生成体积特别巨大的点云文件,从而使得原始点云地图并不适合大范围使用;
2.价格昂贵、寿命短,且机械式激光雷达不能过车规,无法真实在工业领域上使用。
为此,现有技术也有考虑采用融合的方式进行定位;
融合定位融合了目前市面上的所有定位方式,包括GPS、基站定位、Wifi定位、蓝牙定位以及传感器定位。
当前在大陆,这种融合定位技术的实现,主要是通过第三方的位置服务厂商,例如高德,与芯片厂商进行合作,从而在自动驾驶汽车的硬件和***层次集成融合定位技术。
融合定位技术具有如下优点:
1.定位效果不受周边环境影响。GPS定位无法在室内、隧道等区域使用,而基站、WiFi定位也受网络信号制约,融合定位能够在上述环境下自动以传感器辅助定位,因此不受上述环境因素影响;
2.由于融合定位技术将定位逻辑以及关键定位技术融入了芯片中,从而可以将定位能力随芯片而植入车辆的运算控制***之中,而***能够自动根据硬件状况而指定定位策略;
3.由于GPS定位对电量消耗很大,而融合定位能够在条件允许的情况下自动以低功耗的方式,如网络定位、传感器,进行定位,从而大幅度地节省电力消耗;
4.地理围栏功能得以真正意义地实现;
5.由于融合定位技术包含通过加速度传感器获得的车辆运动状态信息,从而可以判断出自动驾驶车辆当前的运动状态;
6.由于能够融合定位技术以低功耗方式记录车辆的轨迹,并且可以不依赖网络和GPS信号,因此可以更加持续地追踪车辆的位置,在汽车用户轨迹回访时,会比以往的技术更加平滑、省电。
但现有技术的融合定位技术还尚不完善;典型的融合定位现有技术如申请名称为“一种车辆融合定位***及方法”,申请号为CN202111055356.1的中国发明申请,公开了以下技术方案:
首先通过安装在智能驾驶车辆上的惯性测量模块、卫星导航信息接收模块和轮速信息采集模块采集车辆的运动信息,同时通过前向安装的车道线采集模块,动态的获取到车辆前方车道线信息,对采集的车道线信息进行处理,识别出车辆前方的车道线情况,计算车辆与车道线的相对距离,并根据采集到的车道线的具体情况以及车道线地图中的车道线的具***置,计算出车辆的绝对位置信息或部分绝对位置信息,然后进行第二次融合定位计算,得到修正后的融合定位结果以及***误差的修正量,完成一个周期融合定位过程。
该发明的技术思路在于:利用视觉传感器输出的车辆距离两侧车道线距离信息,高精度地图输出的世界坐标系下当前车道的车道线信息以及车辆在世界坐标系下的位置信息进行融合滤波,得到一个较为准确的位置信息。
现有技术的融合定位技术的缺陷在于:
1.由于激光雷达技术如前所述,实际无法真实工业应用,因此现有的融合定位技术多是利用视觉传感器,从而不可避免的先天继承了视觉定位技术的上述缺点;
2.申请号为CN202111055356.1的中国发明是只能对车辆在车道内的横向距离进行校正,对纵向位置无法进行约束。在弯道行驶的场景中,无法提供准确的位置信息,从而影响智能驾驶车辆控制精度。
发明内容
本发明针对上述问题,提供智能驾驶中的融合定位方法,其目的在于得到教现有技术更准确的拟合模型,且获得的校正点可以更准确的修正车辆当前位置的横坐标;推算出现有技术所不具备的车辆的纵向位置修正信息;进一步修正车辆当前的航向角,使融合定位的精确度再上一个台阶。
为解决上述问题,本发明提供的技术方案为:
一种智能驾驶中的融合定位方法,包含以下步骤:
S100.通过视觉传感器,输出所需要定位的车辆的当前所在车道的两侧的边界拟合曲线;所述边界拟合曲线位于车辆坐标系中;
S200.根据所述边界拟合曲线,计算得到关于车道中心线的车道线拟合曲线;
S300.将记载于高精度地图中的车道中线形点进行曲线拟合,得到全局坐标系下的航向角计算曲线;再将记载于高精度地图中的车道中线转换到所述车辆坐标系中,得到对应的车道中线形点信息;所述车道中线形点信息包含所述车道中线形点;
S400.将由所述高精度地图输出并转换得到的所述车道中线形点进行曲线拟合,得到车道线曲线;
S500.对来自所述视觉传感器的所述车道线拟合曲线按人工预设的采样间隔进行采样,得到第一离散型点集合;所述第一离散型点集合包含多个采样得到第一离散型点;
对来自所述高精度地图的所述车道线曲线按同样数值的所述采样间隔进行采样,得到第二离散型点集合;所述第二离散型点集合多个包含采样得到第二离散型点;
S600.截取距离车头距离为第一可信距离范围内的所有所述第一离散型点;所述第一可信距离范围由人工预设;然后逐一提取每个所述第一离散型点所在位置在所述车道线拟合曲线上的曲率信息;然后将每一个所述第一离散型点所对应的曲率信息作为一个第一曲率值;然后将所有所述第一曲率值打包,得到第一曲率集合;
S700.截取距离车头距离为第二可信距离范围内的所有所述第二离散型点;所述第二可信距离范围由人工预设;然后逐一提取每个所述第二离散型点所在位置在所述车道线曲线上的曲率信息;然后将每一个所述第二离散型点所对应的曲率信息作为一个第二曲率值;然后将所有所述第二曲率值打包,得到第二曲率集合;
S800.根据所述第一曲率集合和所述第二曲率集合,在所述车道线曲线上找到一个点,使满足以此点为基准,所述第一曲率集合相对于所述第二曲率集合的曲率误差值最小;然后将这个点作为基准点输出;
S900.用S800中提取的所述基准点的纵坐标减去所述第一可信距离范围中距离车头最近的点的纵坐标,得到当前车辆位置的纵坐标,实现对车辆的纵向位置进行校正;
S1000.分别在所述车道线拟合曲线和所述车道线曲线上截取纵坐标都为S900中得到的所述当前车辆位置的纵坐标的点;输出截取所述车道线拟合曲线得到的点作为第一校正点;输出截取所述车道线曲线得到的点作为第二校正点;
S1100.在所述第一离散型点集合中找出一个到所述当前车辆位置的距离最小的点,作为横向校准点;然后用所述横向校准点代替所述第一校正点,得到所述当前车辆位置的横坐标,实现对车辆的横向位置进行校正;
S1200.在所述车道线拟合曲线上计算所述车道线拟合曲线与y轴的夹角值,作为第一夹角;然后在所述航向角计算曲线中计算得到车道中线的绝对航向角信息,作为第二夹角;然后根据所述第一夹角与所述第二夹角,计算得到所述当前车辆位置的航向角,实现对车辆的航向角进行校正;
S1300.将经过校正的所述当前车辆位置的纵坐标、所述当前车辆位置的横坐标和所述当前车辆位置的航向角打包输出,即为本定位方法的最终输出结果。
优选地,所述车道线拟合曲线按下式表达:
x=c3y3+c2y2+c1y+c0
其中:x为横坐标,在所述车辆坐标系下;y为纵坐标,在所述车辆坐标系下;所述车辆坐标系为:以车辆前方为x轴,且车头指向方向为正方向,且所述车辆坐标系满足右手法则;c3为所述车道线拟合曲线与所述车辆坐标系上的y轴交点处的曲率变化率的6倍;c2为所述车道线拟合曲线与所述车辆坐标系上的y轴交点处的曲率的2倍;c1为所述车道线拟合曲线与所述车辆坐标系上的y轴交点处的曲率;c0为所述车道线拟合曲线与所述车辆坐标系上的y轴的截距。
优选地,所述车道中心线按下式表达:
x=c3y3+cy2+c1y+c'0
其中:c'0为所述车道中心线与所述车辆坐标系上的y轴的截距。
优选地,所述车道线曲线按下式表达:
x=b3y3+b2y2+b1y+b0
其中:b3为所述车道线曲线与所述车辆坐标系上的y轴交点处的曲率变化率的6倍;b2为所述车道线曲线与所述车辆坐标系上的y轴交点处的曲率的2倍;b1为所述车道线曲线与所述车辆坐标系上的y轴交点处的曲率;b0为所述车道线曲线与所述车辆坐标系上的y轴的截距。
优选地,所述采样间隔为20cm。
优选地,所述曲率误差值按下式表达:
M=(r'p-r1)2+(r'p+1-r1)2+...+(r'p+n-1-r1)2(1≤p≤m-n+1)
其中:M为所述曲率误差值;r1∈[r1,r2,r3,r4,r5…rn],其中[r1,r2,r3,r4,r5…rn]为所述第一曲率集合,n为所述第一曲率集合的计数器;r'p∈[r'1,r'2,r'3,r'4,r'5...r'm],r'p+1∈[r'1,r'2,r'3,r'4,r'5…r'm],...r'p+n-1∈[r'1,r'2,r'3,r'4,r'5...r'm],其中[r'1,r'2,r'3,r'4,r'5…r'm]为所述第二曲率集合,m为所述第二曲率集合的计数器,且n<m。
优选地,所述第一夹角按下式表达:
其中:y1为横纵向位置校正后位置的纵向坐标值。
优选地,所述第二夹角按下式表达:
其中:y2为全局坐标系下横纵向位置校正后位置的纵向坐标值;e3为所述航向角计算曲线与所述车辆坐标系上的y轴交点处的曲率变化率的6倍;e2为所述航向角计算曲线与所述车辆坐标系上的y轴交点处的曲率的2倍;e1为所述航向角计算曲线与所述车辆坐标系上的y轴交点处的曲率。
优选地,所述当前车辆位置的航向角按下式表达:
θ=α+β
其中:θ为所述当前车辆位置的航向角。
优选地,S所述第一离散型点中的最后一个点距离车头的距离不超过所述第一可信距离范围的最大值;
所述第一可信距离范围距车头不超过20m;
所述第二可信距离范围距车头不超过60m。
本发明与现有技术对比,具有以下优点:
1.由于本发明在现有技术的基础上,采用了由视觉传感器输出当前所在车道的车道中心线的多项式,并由高精度地图输出之策坐标系下对应的车道中线形点信息,将地图输出的车道中线形点进行曲线拟合得到对应的曲线方程,再对视觉传感器输出的车道线拟合曲线和高精度地图输出的车道线曲线进行等间距采样,得到两组离散形点的技术方案,从而得到了教现有技术更准确的拟合模型,且获得的校正点可以更准确的修正车辆当前位置的横坐标;
2.由于本发明在现有技术的基础上,采用相机输出的三次多项式仅在距离车辆较近处置信度较高,因此,取距离车头位置为点s0距离开始的一组点并提取每一个点处的曲率信息,从高精度地图也可以得到一系列车道中线形点的曲率,将相机输出的一组曲率在地图输出的曲率上进行滑动匹配,计算最小的误差,最后根据最小误差得到高精度地图中的形点的起点的,从而推算出车辆的纵向位置修正信息,而这一修正是现有技术所不具备的;
3.由于本发明能够获得更准确的车辆当前位置的横坐标,且可以推算出现有技术无法获得的纵坐标修正信息,从而可以在此基础上,进一步修正车辆当前的航向角,使融合定位的精确度再上一个台阶,而这也是现有技术所不具备的。
附图说明
图1为本发明具体实施例的融合定位方法流程示意图;
图2为本发明具体实施例的车辆坐标系下的车道线拟合曲线、车道线曲线及可信距离选择的示意图;
图3为本发明具体实施例的经过纵向位置校正后,横向距离校正的过程示意图。
具体实施方式
下面结合具体实施例,进一步阐明本发明,应理解这些实施例仅用于说明本发明而不用于限制本发明的范围,在阅读了本发明之后,本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
如图1所示,一种智能驾驶中的融合定位方法,包含以下步骤:
S100.通过视觉传感器,输出所需要定位的车辆的当前所在车道的两侧的边界拟合曲线;边界拟合曲线位于车辆坐标系中。
如图2所示,S200.根据边界拟合曲线,计算得到关于车道中心线的车道线拟合曲线。
本具体实施例中,车道线拟合曲线按式(1)表达:
x=c3y3+c2y2+c1y+c0 (1)
其中:x为横坐标,在车辆坐标系下;y为纵坐标,在车辆坐标系下;车辆坐标系为:以车辆前方为x轴,且车头指向方向为正方向,且车辆坐标系满足右手法则;c3为车道线拟合曲线与车辆坐标系上的y轴交点处的曲率变化率的6倍;c2为车道线拟合曲线与车辆坐标系上的y轴交点处的曲率的2倍;c1为车道线拟合曲线与车辆坐标系上的y轴交点处的曲率;c0为车道线拟合曲线与车辆坐标系上的y轴的截距。
本具体实施例中,车道中心线通过将车道线拟合曲线在车辆坐标系中平移得到,按式(2)表达:
x=c3y3+cy2+c1y+c'0 (2)
其中:c'0为车道中心线与车辆坐标系上的y轴的截距。
S300.将记载于高精度地图中的车道中线形点进行曲线拟合,得到全局坐标系下的航向角计算曲线;再将记载于高精度地图中的车道中线转换到车辆坐标系中,得到对应的车道中线形点信息;车道中线形点信息包含车道中线形点;
S400.将由高精度地图输出并转换得到的车道中线形点进行曲线拟合,得到车道线曲线。
本具体实施例中,车道线曲线按式(3)表达:
x=b3y3+b2y2+b1y+b0 (3)
其中:b3为车道线曲线与车辆坐标系上的y轴交点处的曲率变化率的6倍;b2为车道线曲线与车辆坐标系上的y轴交点处的曲率的2倍;b1为车道线曲线与车辆坐标系上的y轴交点处的曲率;b0为车道线曲线与车辆坐标系上的y轴的截距。
S500.对来自视觉传感器的车道线拟合曲线按人工预设的采样间隔进行采样,得到第一离散型点集合;第一离散型点集合包含多个采样得到第一离散型点。
对来自高精度地图的车道线曲线按同样数值的采样间隔进行采样,得到第二离散型点集合;第二离散型点集合多个包含采样得到第二离散型点。
本具体实施例中,采样间隔为20cm。
需要说明的是,到此步骤位置,就得到了第一离散型点集合和第二离散型点集合这两组离散形点;后面的步骤,都是基于在这离散形点中选出合适的点对车辆位置、航向角进行校正,以期得到一个相对符合真实值的结果。
S600.截取距离车头距离为第一可信距离范围内的所有第一离散型点;第一可信距离范围由人工预设;然后逐一提取每个第一离散型点所在位置在车道线拟合曲线上的曲率信息;然后将每一个第一离散型点所对应的曲率信息作为一个第一曲率值;然后将所有第一曲率值打包,得到第一曲率集合。
本具体实施例中,第一离散型点中的最后一个点距离车头的距离不超过第一可信距离范围的最大值。
本具体实施例中,第一可信距离范围距车头不超过20m。
本具体实施例中,第二可信距离范围距车头不超过60m。
需要说明的是,视觉传感器输出的三次多项式仅在距离车辆较近处置信度较高,因此,取距离车头位置为点s0,距离点s0开始的一组点[x1,x2,x3,x4,x5…,xn]。
需要进一步说明的是,最后一个点xn距离车头的位置sn需在相机输出的车道线置信度较高范围内,亦即第一可信距离范围内。
S700.截取距离车头距离为第二可信距离范围内的所有第二离散型点;第二可信距离范围由人工预设;然后逐一提取每个第二离散型点所在位置在车道线曲线上的曲率信息;然后将每一个第二离散型点所对应的曲率信息作为一个第二曲率值;然后将所有第二曲率值打包,得到第二曲率集合。
S800.根据第一曲率集合和第二曲率集合,在车道线曲线上找到一个点,使满足以此点为基准,第一曲率集合相对于第二曲率集合的曲率误差值最小;然后将这个点作为基准点输出。
本具体实施例中,曲率误差值按式(4)表达:
M=(r'p-r1)2+(r'p+1-r1)2+...+(r'p+n-1-r1)2 (1≤p≤m-n+1) (4)
其中:M为曲率误差值;r1∈[r1,r2,r3,r4,r5…rn],其中[r1,r2,r3,r4,r5…rn]为第一曲率集合,n为第一曲率集合的计数器;r'p∈[r'1,r'2,r'3,r'4,r'5...r'm],r'p+1∈[r'1,r'2,r'3,r'4,r'5…r'm],...r'p+n-1∈[r'1,r'2,r'3,r'4,r'5…r'm],其中[r'1,r'2,r'3,r'4,r'5...r'm]为第二曲率集合,m为第二曲率集合的计数器,且n<m。
需要说明的是,S800的逻辑是这样的:
首先,定义一个曲率误差值M。
然后,根据式(4)逐一去试,必然就能找到一个值p,使得在值p处的M最小。
S900.用S800中提取的基准点的纵坐标减去第一可信距离范围中距离车头最近的点的纵坐标,得到当前车辆位置的纵坐标,实现对车辆的纵向位置进行校正。
需要说明的是,S900的原理是这样的:
在高精度地图中,值p对应的是由视觉传感器输出的车道线拟合曲线的第一个点;因此,用距离车头位置的为p的点sp,减去距离车头位置为点s0的纵坐标距离,两者的差就是当前车辆位置的纵坐标。
如图3所示,S1000.分别在车道线拟合曲线和车道线曲线上截取纵坐标都为S900中得到的当前车辆位置的纵坐标的点;输出截取车道线拟合曲线得到的点作为第一校正点;输出截取车道线曲线得到的点作为第二校正点。
需要说明的是,第一校正点的横坐标就是式(1)中的c0,而第二校正点的横坐标就是式(3)中的b0。
S1100.在第一离散型点集合中找出一个到当前车辆位置的距离最小的点,作为横向校准点;然后用横向校准点代替第一校正点,得到当前车辆位置的横坐标,实现对车辆的横向位置进行校正。
需要说明的是,S1100的原理是这样的:
在点sp附近可以找到距离车辆最近的点D;然后分别可以在车道线拟合曲线上找到一个距离车辆最近的点、在车道线曲线上找到一个距离车辆最近的点;对这两个点,分别计算到车辆的横向距离,此处命名为d1和d2;然后由于距离车辆较近,置信度较高,而横向偏移上d1比d2少了车道线拟合曲线与车道线曲线的横向偏移差,正好可以用来进行横向距离校正,因此就可以在d2所在的线段上,将线段的距离缩减为d1,就得到了横向校正点。
S1200.在车道线拟合曲线上计算车道线拟合曲线与y轴的夹角值,作为第一夹角;然后在航向角计算曲线中计算得到车道中线的绝对航向角信息,作为第二夹角;然后根据第一夹角与第二夹角,计算得到当前车辆位置的航向角,实现对车辆的航向角进行校正。
本具体实施例中,通过对视觉传感器输出的车道线拟合曲线多项式求导,得到第一夹角按式(5)表达:
其中:y1为横纵向位置校正后位置的纵向坐标值。
本具体实施例中,通过对高精度地图输出的车道线拟合曲线多项式求导,第二夹角按式(7)表达:
其中:y2为全局坐标系下横纵向位置校正后位置的纵向坐标值;e3为所述航向角计算曲线与所述车辆坐标系上的y轴交点处的曲率变化率的6倍;e2为所述航向角计算曲线与所述车辆坐标系上的y轴交点处的曲率的2倍;e1为所述航向角计算曲线与所述车辆坐标系上的y轴交点处的曲率。
本具体实施例中,当前车辆位置的航向角按式(9)表达:
θ=α+β (9)
其中:θ为当前车辆位置的航向角。
S1300.将经过校正的当前车辆位置的纵坐标、当前车辆位置的横坐标和当前车辆位置的航向角打包输出,即为本定位方法的最终输出结果。
在上述的详细描述中,各种特征一起组合在单个的实施方案中,以简化本公开。不应该将这种公开方法解释为反映了这样的意图,即,所要求保护的主题的实施方案需要比清楚地在每个权利要求中所陈述的特征更多的特征。相反,如所附的权利要求书所反映的那样,本发明处于比所公开的单个实施方案的全部特征少的状态。因此,所附的权利要求书特此清楚地被并入详细描述中,其中每项权利要求独自作为本发明单独的优选实施方案。
为使本领域内的任何技术人员能够实现或者使用本发明,上面对所公开实施例进行了描述。对于本领域技术人员来说;这些实施例的各种修改方式都是显而易见的,并且本文定义的一般原理也可以在不脱离本公开的精神和保护范围的基础上适用于其它实施例。因此,本公开并不限于本文给出的实施例,而是与本申请公开的原理和新颖性特征的最广范围相一致。
上文的描述包括一个或多个实施例的举例。当然,为了描述上述实施例而描述部件或方法的所有可能的结合是不可能的,但是本领域普通技术人员应该认识到,各个实施例可以做进一步的组合和排列。因此,本文中描述的实施例旨在涵盖落入所附权利要求书的保护范围内的所有这样的改变、修改和变型。此外,就说明书或权利要求书中使用的术语“包含”,该词的涵盖方式类似于术语“包括”,就如同“包括,”在权利要求中用作衔接词所解释的那样。此外,使用在权利要求书的说明书中的任何一个术语“或”是要表示“非排它性的或者”。
以上所述的具体实施方式,对本发明的目的、技术方案和有益效果进行了进一步详细说明,所应理解的是,以上所述仅为本发明的具体实施方式而已,并不用于限定本发明的保护范围,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (10)
1.一种智能驾驶中的融合定位方法,其特征在于:包含以下步骤:
S100.通过视觉传感器,输出所需要定位的车辆的当前所在车道的两侧的边界拟合曲线;所述边界拟合曲线位于车辆坐标系中;
S200.根据所述边界拟合曲线,计算得到关于车道中心线的车道线拟合曲线;
S300.将记载于高精度地图中的车道中线形点进行曲线拟合,得到全局坐标系下的航向角计算曲线;再将记载于高精度地图中的车道中线转换到所述车辆坐标系中,得到对应的车道中线形点信息;所述车道中线形点信息包含所述车道中线形点;
S400.将由所述高精度地图输出并转换得到的所述车道中线形点进行曲线拟合,得到车道线曲线;
S500.对来自所述视觉传感器的所述车道线拟合曲线按人工预设的采样间隔进行采样,得到第一离散型点集合;所述第一离散型点集合包含多个采样得到第一离散型点;
对来自所述高精度地图的所述车道线曲线按同样数值的所述采样间隔进行采样,得到第二离散型点集合;所述第二离散型点集合多个包含采样得到第二离散型点;
S600.截取距离车头距离为第一可信距离范围内的所有所述第一离散型点;所述第一可信距离范围由人工预设;然后逐一提取每个所述第一离散型点所在位置在所述车道线拟合曲线上的曲率信息;然后将每一个所述第一离散型点所对应的曲率信息作为一个第一曲率值;然后将所有所述第一曲率值打包,得到第一曲率集合;
S700.截取距离车头距离为第二可信距离范围内的所有所述第二离散型点;所述第二可信距离范围由人工预设;然后逐一提取每个所述第二离散型点所在位置在所述车道线曲线上的曲率信息;然后将每一个所述第二离散型点所对应的曲率信息作为一个第二曲率值;然后将所有所述第二曲率值打包,得到第二曲率集合;
S800.根据所述第一曲率集合和所述第二曲率集合,在所述车道线曲线上找到一个点,使满足以此点为基准,所述第一曲率集合相对’于所述第二曲率集合的曲率误差值最小;然后将这个点作为基准点输出;
S900.用S800中提取的所述基准点的纵坐标减去所述第一可信距离范围中距离车头最近的点的纵坐标,得到当前车辆位置的纵坐标,实现对车辆的纵向位置进行校正;
S1000.分别在所述车道线拟合曲线和所述车道线曲线上截取纵坐标都为S900中得到的所述当前车辆位置的纵坐标的点;输出截取所述车道线拟合曲线得到的点作为第一校正点;输出截取所述车道线曲线得到的点作为第二校正点;
S1100.在所述第一离散型点集合中找出一个到所述当前车辆位置的距离最小的点,作为横向校准点;然后用所述横向校准点代替所述第一校正点,得到所述当前车辆位置的横坐标,实现对车辆的横向位置进行校正;
S1200.在所述车道线拟合曲线上计算所述车道线拟合曲线与y轴的夹角值,作为第一夹角;然后在所述航向角计算曲线中计算得到车道中线的绝对航向角信息,作为第二夹角;然后根据所述第一夹角与所述第二夹角,计算得到所述当前车辆位置的航向角,实现对车辆的航向角进行校正;
S1300.将经过校正的所述当前车辆位置的纵坐标、所述当前车辆位置的横坐标和所述当前车辆位置的航向角打包输出,即为本定位方法的最终输出结果。
2.根据权利要求1所述的智能驾驶中的融合定位方法,其特征在于:所述车道线拟合曲线按下式表达:
x=c3y3+c2y2+c1y+c0
其中:x为横坐标,在所述车辆坐标系下;y为纵坐标,在所述车辆坐标系下;所述车辆坐标系为:以车辆前方为x轴,且车头指向方向为正方向,且所述车辆坐标系满足右手法则;c3为所述车道线拟合曲线与所述车辆坐标系上的y轴交点处的曲率变化率的6倍;c2为所述车道线拟合曲线与所述车辆坐标系上的y轴交点处的曲率的2倍;c1为所述车道线拟合曲线与所述车辆坐标系上的y轴交点处的曲率;c0为所述车道线拟合曲线与所述车辆坐标系上的y轴的截距。
3.根据权利要求2所述的智能驾驶中的融合定位方法,其特征在于:所述车道中心线按下式表达:
x=c3y3+cy2+c1y+c'0
其中:c′0为所述车道中心线与所述车辆坐标系上的y轴的截距。
4.根据权利要求3所述的智能驾驶中的融合定位方法,其特征在于:所述车道线曲线按下式表达:
x=b3y3+b2y2+b1y+b0
其中:b3为所述车道线曲线与所述车辆坐标系上的y轴交点处的曲率变化率的6倍;b2为所述车道线曲线与所述车辆坐标系上的y轴交点处的曲率的2倍;b1为所述车道线曲线与所述车辆坐标系上的y轴交点处的曲率;b0为所述车道线曲线与所述车辆坐标系上的y轴的截距。
5.根据权利要求4所述的智能驾驶中的融合定位方法,其特征在于:所述采样间隔为20cm。
6.根据权利要求5所述的智能驾驶中的融合定位方法,其特征在于:所述曲率误差值按下式表达:
M=(r'p-r1)2+(r'p+1-r1)2+...+(r'p+n-1-r1)2(1≤p≤m-n+1)
其中:M为所述曲率误差值;r1∈[r1,r2,r3,r4,r5...rn],其中[r1,r2,r3,r4,r5...rn]为所述第一曲率集合,n为所述第一曲率集合的计数器;r'p∈[r1',r'2,r'3,r'4,r'5...r'm],r'p+1∈[r1',r'2,r'3,r'4,r'5...r'm],...r'p+n-1∈[r1',r'2,r'3,r'4,r'5...r'm],其中[r1',r'2,r'3,r'4,r'5…r'm]为所述第二曲率集合,m为所述第二曲率集合的计数器,且n<m。
9.根据权利要求8所述的智能驾驶中的融合定位方法,其特征在于:所述当前车辆位置的航向角按下式表达:
θ=α+β
其中:θ为所述当前车辆位置的航向角。
10.根据权利要求9所述的智能驾驶中的融合定位方法,其特征在于:所述第一离散型点中的最后一个点距离车头的距离不超过所述第一可信距离范围的最大值;
所述第一可信距离范围距车头不超过20m;
所述第二可信距离范围距车头不超过60m。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210761697.9A CN115027482A (zh) | 2022-06-29 | 2022-06-29 | 智能驾驶中的融合定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210761697.9A CN115027482A (zh) | 2022-06-29 | 2022-06-29 | 智能驾驶中的融合定位方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115027482A true CN115027482A (zh) | 2022-09-09 |
Family
ID=83128623
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210761697.9A Pending CN115027482A (zh) | 2022-06-29 | 2022-06-29 | 智能驾驶中的融合定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115027482A (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115950441A (zh) * | 2023-03-08 | 2023-04-11 | 智道网联科技(北京)有限公司 | 自动驾驶车辆的融合定位方法、装置及电子设备 |
CN115993137A (zh) * | 2023-02-22 | 2023-04-21 | 禾多科技(北京)有限公司 | 车辆定位评估方法、装置、电子设备和计算机可读介质 |
CN116630928A (zh) * | 2023-07-25 | 2023-08-22 | 广汽埃安新能源汽车股份有限公司 | 一种车道线优化方法、装置及电子设备 |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103940434A (zh) * | 2014-04-01 | 2014-07-23 | 西安交通大学 | 基于单目视觉和惯性导航单元的实时车道线检测*** |
US20160018229A1 (en) * | 2014-07-16 | 2016-01-21 | GM Global Technology Operations LLC | Accurate curvature estimation algorithm for path planning of autonomous driving vehicle |
JP2016045144A (ja) * | 2014-08-26 | 2016-04-04 | アルパイン株式会社 | 走行中レーン検出装置及び運転支援システム |
CN109017780A (zh) * | 2018-04-12 | 2018-12-18 | 深圳市布谷鸟科技有限公司 | 一种车辆智能驾驶控制方法 |
JP2019059319A (ja) * | 2017-09-26 | 2019-04-18 | 株式会社Subaru | 車両の走行制御装置 |
CN110969837A (zh) * | 2018-09-30 | 2020-04-07 | 长城汽车股份有限公司 | 自动驾驶车辆的道路信息融合***及方法 |
CN111516673A (zh) * | 2020-04-30 | 2020-08-11 | 重庆长安汽车股份有限公司 | 基于智能摄像头和高精地图定位的车道线融合***及方法 |
CN113602267A (zh) * | 2021-08-26 | 2021-11-05 | 东风汽车有限公司东风日产乘用车公司 | 车道保持控制方法、存储介质及电子设备 |
CN113682313A (zh) * | 2021-08-11 | 2021-11-23 | 中汽创智科技有限公司 | 一种车道线确定方法、确定装置及存储介质 |
US20210362741A1 (en) * | 2018-09-30 | 2021-11-25 | Great Wall Motor Company Limited | Method for constructing driving coordinate system, and application thereof |
CN114002725A (zh) * | 2021-11-01 | 2022-02-01 | 武汉中海庭数据技术有限公司 | 一种车道线辅助定位方法、装置、电子设备及存储介质 |
-
2022
- 2022-06-29 CN CN202210761697.9A patent/CN115027482A/zh active Pending
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103940434A (zh) * | 2014-04-01 | 2014-07-23 | 西安交通大学 | 基于单目视觉和惯性导航单元的实时车道线检测*** |
US20160018229A1 (en) * | 2014-07-16 | 2016-01-21 | GM Global Technology Operations LLC | Accurate curvature estimation algorithm for path planning of autonomous driving vehicle |
JP2016045144A (ja) * | 2014-08-26 | 2016-04-04 | アルパイン株式会社 | 走行中レーン検出装置及び運転支援システム |
JP2019059319A (ja) * | 2017-09-26 | 2019-04-18 | 株式会社Subaru | 車両の走行制御装置 |
CN109017780A (zh) * | 2018-04-12 | 2018-12-18 | 深圳市布谷鸟科技有限公司 | 一种车辆智能驾驶控制方法 |
CN110969837A (zh) * | 2018-09-30 | 2020-04-07 | 长城汽车股份有限公司 | 自动驾驶车辆的道路信息融合***及方法 |
US20210362741A1 (en) * | 2018-09-30 | 2021-11-25 | Great Wall Motor Company Limited | Method for constructing driving coordinate system, and application thereof |
CN111516673A (zh) * | 2020-04-30 | 2020-08-11 | 重庆长安汽车股份有限公司 | 基于智能摄像头和高精地图定位的车道线融合***及方法 |
CN113682313A (zh) * | 2021-08-11 | 2021-11-23 | 中汽创智科技有限公司 | 一种车道线确定方法、确定装置及存储介质 |
CN113602267A (zh) * | 2021-08-26 | 2021-11-05 | 东风汽车有限公司东风日产乘用车公司 | 车道保持控制方法、存储介质及电子设备 |
CN114002725A (zh) * | 2021-11-01 | 2022-02-01 | 武汉中海庭数据技术有限公司 | 一种车道线辅助定位方法、装置、电子设备及存储介质 |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115993137A (zh) * | 2023-02-22 | 2023-04-21 | 禾多科技(北京)有限公司 | 车辆定位评估方法、装置、电子设备和计算机可读介质 |
CN115950441A (zh) * | 2023-03-08 | 2023-04-11 | 智道网联科技(北京)有限公司 | 自动驾驶车辆的融合定位方法、装置及电子设备 |
CN116630928A (zh) * | 2023-07-25 | 2023-08-22 | 广汽埃安新能源汽车股份有限公司 | 一种车道线优化方法、装置及电子设备 |
CN116630928B (zh) * | 2023-07-25 | 2023-11-17 | 广汽埃安新能源汽车股份有限公司 | 一种车道线优化方法、装置及电子设备 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110057373B (zh) | 用于生成高精细语义地图的方法、装置和计算机存储介质 | |
CN108802785B (zh) | 基于高精度矢量地图和单目视觉传感器的车辆自定位方法 | |
CN109696663B (zh) | 一种车载三维激光雷达标定方法和*** | |
US11024055B2 (en) | Vehicle, vehicle positioning system, and vehicle positioning method | |
CN115027482A (zh) | 智能驾驶中的融合定位方法 | |
EP3742200B1 (en) | Detection apparatus and parameter adjustment method thereof | |
WO2018196391A1 (zh) | 一种车载摄像机的外部参数标定方法及装置 | |
CN108983248A (zh) | 一种基于3d激光雷达及v2x的网联车定位方法 | |
CN111856491B (zh) | 用于确定车辆的地理位置和朝向的方法和设备 | |
JP5157067B2 (ja) | 自動走行用マップ作成装置、及び自動走行装置。 | |
CN110945379A (zh) | 从地图数据、激光和相机确定偏航误差 | |
JP2018533721A (ja) | ローカライゼーション基準データを生成及び使用する方法及びシステム | |
CN116685873A (zh) | 一种面向车路协同的感知信息融合表征及目标检测方法 | |
CN112577517A (zh) | 一种多元定位传感器联合标定方法和*** | |
CN103424112A (zh) | 一种基于激光平面辅助的运动载体视觉导航方法 | |
CN111077907A (zh) | 一种室外无人机的自主定位方法 | |
CN106127787A (zh) | 一种基于逆投影变换的相机标定方法 | |
CN112683260A (zh) | 一种基于高精度地图和v2x的组合导航定位精度提升***及方法 | |
JP2018077162A (ja) | 車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラム | |
CN114485654A (zh) | 一种基于高精地图的多传感器融合定位方法及装置 | |
CN113312403B (zh) | 地图获取方法、装置、电子设备及存储介质 | |
CN115166721A (zh) | 路侧感知设备中雷达与gnss信息标定融合方法及装置 | |
CN106292660B (zh) | 基于里程计和灰度传感器的平衡车路径校正装置及方法 | |
CN114705199A (zh) | 一种车道级融合定位方法及*** | |
CN114035167A (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 |