CN117647254B - 自动驾驶车辆融合定位方法、装置、设备、存储介质 - Google Patents
自动驾驶车辆融合定位方法、装置、设备、存储介质 Download PDFInfo
- Publication number
- CN117647254B CN117647254B CN202410123304.0A CN202410123304A CN117647254B CN 117647254 B CN117647254 B CN 117647254B CN 202410123304 A CN202410123304 A CN 202410123304A CN 117647254 B CN117647254 B CN 117647254B
- Authority
- CN
- China
- Prior art keywords
- positioning
- determining
- value
- current
- count value
- 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.)
- Active
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 75
- 238000000034 method Methods 0.000 title claims abstract description 58
- 238000006243 chemical reaction Methods 0.000 claims abstract description 57
- 238000006073 displacement reaction Methods 0.000 claims description 47
- 230000008859 change Effects 0.000 claims description 42
- 230000007704 transition Effects 0.000 claims description 39
- 230000000007 visual effect Effects 0.000 claims description 29
- 238000013507 mapping Methods 0.000 claims description 26
- 230000001360 synchronised effect Effects 0.000 claims description 25
- 238000004590 computer program Methods 0.000 claims description 21
- 102000027426 receptor tyrosine kinases Human genes 0.000 description 33
- 108091008598 receptor tyrosine kinases Proteins 0.000 description 33
- 238000010586 diagram Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 8
- 230000004807 localization Effects 0.000 description 6
- 238000012986 modification Methods 0.000 description 5
- 230000004048 modification Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 238000005259 measurement Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Classifications
-
- 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/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请提供一种自动驾驶车辆融合定位方法、装置、设备、存储介质，该方法包括：当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式；其中，定位方式包括实时差分定位RTK，激光SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个；确定切换后的定位方式的预测值；根据当前的转换计数值和预测值确定虚拟观测点；将虚拟观测点输入滤波器中，进行融合定位。本申请的方法在融合定位时，会根据切换后的定位方式的预测值确定虚拟观测点，将虚拟观测点输入滤波器中，进行融合定位，通过虚拟观测点，避免了直接退出后因为输出频率不同导致定位轨迹出现跳变，进而导致的车辆晃动或者画龙。
Description
技术领域
本申请涉及自动驾驶技术领域，尤其涉及一种自动驾驶车辆融合定位方法、装置、设备、存储介质。
背景技术
在车辆进行自动驾驶时，要保证在各种复杂场景下行驶的稳定性，需要有高精定位作为支撑。要实现在各复杂场景下的高精定位，依靠单一传感器很难实现，需要通过多传感器信息进行融合定位。例如，基于IMU（Inertial Measurement Unit，惯性测量单元）和RTK（Real-time kinematic，实时差分定位）实现的融合定位方案。
RTK会在车辆遇到城市、峡谷、隧道等场景时受到干扰，无法得到高精度的定位信息。常用的解决方案是加入激光SLAM（Simultaneous Localization and Mapping，同步定位与建图）的定位结果进行补偿。
然而由于激光SLAM的输出频率一般低于RTK/IMU的输出频率，例如激光SLAM的输出频率通常为5Hz，RTK/IMU的输出频率为100Hz，因此会出现RTK/IMU输出的过程中没有激光SLAM输出的情况，或者激光SLAM也可能会由于遮挡、干扰等情况没有输出定位结果，此时直接退出激光SLAM定位可能会导致定位轨迹出现跳变，导致车辆晃动或者画龙。
发明内容
为了解决上述技术缺陷之一，本申请提供了一种自动驾驶车辆融合定位方法、装置、设备、存储介质。
本申请第一个方面，提供了一种自动驾驶车辆融合定位方法，方法包括：
当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式；其中，定位方式包括实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个；
确定切换后的定位方式的预测值；
根据当前的转换计数值和预测值确定虚拟观测点；
将虚拟观测点输入滤波器中，进行融合定位。
可选地，定位方式优先级由高到低依次为：实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，视觉SLAM。
可选地，根据当前的转换计数值和预测值确定虚拟观测点，包括：
若当前的转换计数值为0，则将预测值确定为虚拟观测点。
可选地，根据当前的转换计数值和预测值确定虚拟观测点，包括：
若当前的转换计数值大于0，则将当前的转换计数值减1；确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商；根据单帧变化位移量、当前的转换计数值和预测值计算当前的观测值；将当前的观测值作为虚拟观测点。
可选地，确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商之前，还包括：
将激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM进行时间同步以及频率扩展；其中，频率扩展后激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM的输出频率均与实时差分定位RTK的输出频率相同；
基于频率扩展后的信息，预测各定位方式在通用横墨卡托格网***UTM坐标系下的当前位置与GPS在UTM坐标系下的当前位置的差。
可选地，状态转换时间初始值为300；
在单帧变化位移量大于0.2米并且小于或等于0.4米，或者，车辆速度大于40千米/小时并且小于或等于50千米/小时，状态转换时间更新为600；
在单帧变化位移量大于0.4米，或者，车辆速度大于40千米/小时，状态转换时间更新为900。
可选地，根据单帧变化位移量、当前的转换计数值和预测值计算当前的观测值，包括：
计算当前的转换计数值与单帧变化位移量的积；
将预测值与积的差确定为当前的观测值。
本申请第二个方面，提供了一种自动驾驶车辆融合定位装置，装置包括：
第一确定模块，用于当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式；其中，定位方式包括实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个；
第二确定模块，用于确定切换后的定位方式的预测值；
第三确定模块，用于根据当前的转换计数值和预测值确定虚拟观测点；
融合定位模块，用于将虚拟观测点输入滤波器中，进行融合定位。
本申请第三个方面，提供了一种电子设备，包括：
存储器；
处理器；以及
计算机程序；
其中，所述计算机程序存储在所述存储器中，并被配置为由所述处理器执行以实现如上述第一个方面所述的方法。
本申请第四个方面，提供了一种计算机可读存储介质，其上存储有计算机程序；所述计算机程序被处理器执行以实现如上述第一个方面所述的方法。
本申请提供一种自动驾驶车辆融合定位方法、装置、设备、存储介质，该方法包括：当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式；其中，定位方式包括实时差分定位RTK，激光SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个；确定切换后的定位方式的预测值；根据当前的转换计数值和预测值确定虚拟观测点；将虚拟观测点输入滤波器中，进行融合定位。
本申请的方法在融合定位时，会根据切换后的定位方式的预测值确定虚拟观测点，将虚拟观测点输入滤波器中，进行融合定位，通过虚拟观测点，避免了直接退出后因为输出频率不同导致定位轨迹出现跳变，进而导致的车辆晃动或者画龙。
附图说明
此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1为本申请实施例提供的一种自动驾驶车辆融合定位方法的流程示意图；
图2为本申请实施例提供的一种自动驾驶车辆融合定位装置的结构示意图；
图3为本申请实施例提供的一种电子设备的结构示意图。
具体实施方式
为了使本申请实施例中的技术方案及优点更加清楚明白，以下结合附图对本申请的示例性实施例进行进一步详细的说明，显然，所描述的实施例仅是本申请的一部分实施例，而不是所有实施例的穷举。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
在实现本申请的过程中，发明人发现，目前在融合定位时由于激光SLAM的输出频率一般低于RTK/IMU的输出频率，会出现RTK/IMU输出的过程中没有激光SLAM输出的情况，或者激光SLAM也可能会由于遮挡、干扰等情况没有输出定位结果，此时直接退出激光SLAM定位可能会导致定位轨迹出现跳变，导致车辆晃动或者画龙。
针对上述问题，本申请实施例中提供了一种自动驾驶车辆融合定位方法、装置、设备、存储介质，该方法包括：当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式；其中，定位方式包括实时差分定位RTK，激光SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个；确定切换后的定位方式的预测值；根据当前的转换计数值和预测值确定虚拟观测点；将虚拟观测点输入滤波器中，进行融合定位。本申请的方法在融合定位时，会根据切换后的定位方式的预测值确定虚拟观测点，将虚拟观测点输入滤波器中，进行融合定位，通过虚拟观测点，避免了直接退出后因为输出频率不同导致定位轨迹出现跳变，进而导致的车辆晃动或者画龙。
参见图1，本实施例提供的自动驾驶车辆融合定位方法实现过程如下：
101，当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式。
其中，定位方式包括RTK（Real-time kinematic，实时差分定位），激光SLAM（Simultaneous Localization and Mapping，同步定位与建图），VAL（视觉辅助定位），或者，视觉SLAM中的一个。
其中，VAL通过车辆的运动关系与高精地图做匹配实现定位，视觉SLAM通过车辆的运动关系与自建视觉语义地图做匹配实现定位。
另外，每种定位方式均有对应的优先级，定位方式优先级由高到低依次为：RTK，激光SLAM，VAL，视觉SLAM。若用数字既表示定位方式，又表示优先级，则可以数字的值越小，优先级越高，如可以将RTK定位方式表示为0，将激光SLAM定位方式表示为1，将VAL定位方式表示为2，将视觉SLAM定位方式表示为3。
本步骤在执行时，可以先将上一帧的定位方式确定为切换前的定位方式，再根据优先级，选择优先级仅低于切换前的定位方式的定位方式作为切换后的定位方式。
具体实现时，可以读取参数slam_flag_last的值，该值即上一帧的定位方式。例如，slam_flag_last=0，则表示上一帧的定位方式为RTK，根据优先级的设置，确定0的下一个数值（即1），那么1所对应的激光SLAM即为切换后的定位方式。
方便描述，本实施例用before_switching代表切换前的定位方式，用after_switching代表切换后的定位方式。
即，如果before_switching=0（即RTK），那么after_switching=1（即激光SLAM）。如果before_switching=1（即激光SLAM）），那么after_switching=2（即VAL）。如果before_switching=2（即VAL），那么after_switching=3（即视觉SLAM）。
需要说明的是，本步骤的执行条件是进入融合定位，在判断是否进入融合定位时，采用的现有方案，例如，监测RTK定位信号的不可用状态，如果出现浮点解52的RTK定位信号，则说明定位误差小于0.5米，此时认为需要进入融合定位。或者，如果出现单点解12的RTK定位信号，则说明定位误差在1-5米，此时认为需要进入融合定位。再或者，如果出现32，则说明定位误差在百米级，此时认为需要进入融合定位。本实施例不对判断是否进入融合定位的具体方案进行限定。
另外，在具体实现时，在进行定位方式切换的第一个动作为初始化一个转换计数值，该值的初始值为300，对应3s（秒）。转换计数值表示需要进行虚拟观测的次数，变为0表示虚拟观测结束。方便描述，本实施例用sensor_change_cnt代表转换计数值，即sensor_change_cnt的初始值为300。
102，确定切换后的定位方式的预测值。
本步骤会采用现有的方法确定after_switching的预测值，此处不进行赘述。为了方便描述，本实施例用after_switching_new_pos代表切换后的定位方式的预测值。
103，根据当前的转换计数值和预测值确定虚拟观测点。
本步骤在具体实现时，若当前的转换计数值为0，则将预测值确定为虚拟观测点。若当前的转换计数值大于0，则将当前的转换计数值减1。确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商。根据单帧变化位移量、当前的转换计数值和预测值计算当前的观测值。将当前的观测值作为虚拟观测点。
为了方便描述，本实施例用sensors_utm_predict代表虚拟观测点，也就是说，如果当前的sensor_change_cnt为0，则sensors_utm_predict=after_switching_new_pos。如果当前的sensor_change_cnt大于0（即为正数），则执行步骤201至203进而得到sensors_utm_predict。
201，将当前的转换计数值减1。
即sensor_change_cnt=sensor_change_cnt-1。
202，确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商。
在步骤202实现时会用到切换后的定位方式的当前位置差进行单帧变化位移量的计算，因此在执行步骤202之前，还会确定各定位方式的当前位置差，具体实现过程如步骤301至302所示。
301，将激光SLAM、VAL和视觉SLAM进行时间同步以及频率扩展。
其中，频率扩展后激光SLAM、VAL和视觉SLAM的输出频率均与RTK的输出频率相同。
RTK的输出频率为100Hz（赫兹），激光SLAM的输出频率为5Hz，VAL的输出频率为10Hz，视觉SLAM的输出频率为10Hz。在执行步骤301时，将激光SLAM，VAL，视觉SLAM进行时间同步并利用IMU（Inertial Measurement Unit，惯性测量单元）和论速计进行预测，将频率扩展成100Hz。
302，基于频率扩展后的信息，预测各定位方式在UTM（Universal TransverseMercator Grid System，通用横墨卡托格网***）坐标系下的当前位置差。
在步骤302中，会将各定位方式的信息预测到当前时刻，并基于当前的GPS（GlobalPositioning System，全球定位***）预测各定位方式在UTM坐标系下的当前位置差。
该当前位置差为各定位方式在通用横墨卡托格网***UTM坐标系下的当前位置与GPS（Global Positioning System，全球定位***）在UTM坐标系下的当前位置的差。
例如，若激光SLAM在UTM坐标系下的当前位置为lidar_pos，GPS在UTM坐标系下的当前位置为gps_pos，则激光SLAM在UTM坐标系下的当前位置差为lidar_pos-gps_pos。若VAL在UTM坐标系下的当前位置为val_pos，GPS在UTM坐标系下的当前位置为gps_pos，则VAL在UTM坐标系下的当前位置差为val_pos-gps_pos。若视觉SLAM在UTM坐标系下的当前位置为vslam_pos，GPS在UTM坐标系下的当前位置为gps_pos，则视觉SLAM在UTM坐标系下的当前位置差为vslam_pos-gps_pos。
为了方便描述，本实施例用sensor_change_dp表示单帧变化位移量，即切换过程中每一帧需要变化的位移量，用after_switching_pos-gps_pos表示切换后的定位方式的当前位置差，用SENSORS_CHANGE_TIME表示状态转换时间。那么在执行步骤202时，确定sensor_change_dp=after_switching_pos-gps_pos/SENSORS_CHANGE_TIME。
另外，状态转换时间SENSORS_CHANGE_TIME初始值为300，即SENSORS_CHANGE_TIME=300，对应3s。后续随着单帧变化位移量sensor_change_dp的变化或者车辆速度的变化而变化（此处的变化为SENSORS_CHANGE_TIME当前值的变化，也就说说，SENSORS_CHANGE_TIME初始值为300后，随着sensor_change_dp变化或者车辆速度变化，SENSORS_CHANGE_TIME当前值进行变化）。
在单帧变化位移量sensor_change_dp>0.2m（米）并且sensor_change_dp≤0.4m，或者，车辆速度>40Km（千米）/h（小时）并且车辆速度≤50Km/h，状态转换时间更新为600，即SENSORS_CHANGE_TIME=600，对应6s。
在单帧变化位移量量sensor_change_dp>0.4m，或者，车辆速度>40Km/h，状态转换时间更新为900, 即SENSORS_CHANGE_TIME=900，对应9s。
203，根据单帧变化位移量、当前的转换计数值和预测值计算当前的观测值，将当前的观测值作为虚拟观测点。
本步骤的实现方式为：计算当前的转换计数值与单帧变化位移量的积，将预测值与积的差确定为当前的观测值，将当前的观测值作为虚拟观测点。
即sensors_utm_predict=
after_switching_new_pos-sensor_change_cntsensor_change_dp。
104，将虚拟观测点输入滤波器中，进行融合定位。
由于融合定位是一个过程，因此会持续的重复执行本实施例提供的自动驾驶车辆融合定位方法。在每次执行时，并不会均执行步骤101以及初始化sensor_change_cnt，也就是说，仅在进行定位方式切换的时候才执行步骤101及初始化sensor_change_cnt，在切换到一个定位方式后，在该方式下进行持续融合定位时，仅重复执行步骤102至104即可。
本实施例提供的自动驾驶车辆融合定位方法在进入融合定位时不仅考虑切换前后的定位方式的输出，而且加入了虚拟观测点，避免了由于直接退出后因为输出频率不同导致定位轨迹出现跳变，进而导致的车辆晃动或者画龙。
本实施例提供一种自动驾驶车辆融合定位方法，当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式；其中，定位方式包括实时差分定位RTK，激光SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个；确定切换后的定位方式的预测值；根据当前的转换计数值和预测值确定虚拟观测点；将虚拟观测点输入滤波器中，进行融合定位。本实施例提供的方法在融合定位时，会根据切换后的定位方式的预测值确定虚拟观测点，将虚拟观测点输入滤波器中，进行融合定位，通过虚拟观测点，避免了直接退出后因为输出频率不同导致定位轨迹出现跳变，进而导致的车辆晃动或者画龙。
基于自动驾驶车辆融合定位方法的同一发明构思，本实施例提供一种自动驾驶车辆融合定位装置，如图2所示，该装置包括：
第一确定模块201，用于当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式。
其中，定位方式包括实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个。
第二确定模块202，用于确定第一确定模块201确定的切换后的定位方式的预测值。
第三确定模块203，用于根据当前的转换计数值和第二确定模块202确定的预测值确定虚拟观测点。
融合定位模块204，用于将第三确定模块203确定的虚拟观测点输入滤波器中，进行融合定位。
可选地，定位方式优先级由高到低依次为：实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，视觉SLAM。
可选地，第三确定模块203，还用于若当前的转换计数值为0，则将预测值确定为虚拟观测点。
可选地，第三确定模块203，还用于若当前的转换计数值大于0，则将当前的转换计数值减1。确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商。根据单帧变化位移量、当前的转换计数值和预测值计算当前的观测值。将当前的观测值作为虚拟观测点。
可选地，该装置还包括：处理模块，用于将激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM进行时间同步以及频率扩展；其中，频率扩展后激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM的输出频率均与实时差分定位RTK的输出频率相同。基于频率扩展后的信息，预测各定位方式在通用横墨卡托格网***UTM坐标系下的当前位置与GPS在UTM坐标系下的当前位置的差。
可选地，状态转换时间初始值为300。在单帧变化位移量大于0.2米并且小于或等于0.4米，或者，车辆速度大于40千米/小时并且小于或等于50千米/小时，状态转换时间更新为600。在单帧变化位移量大于0.4米，或者，车辆速度大于40千米/小时，状态转换时间更新为900。
可选地，第三确定模块203，还用于计算当前的转换计数值与单帧变化位移量的积。将预测值与积的差确定为当前的观测值。
本实施例提供的装置在融合定位时，会根据切换后的定位方式的预测值确定虚拟观测点，将虚拟观测点输入滤波器中，进行融合定位，通过虚拟观测点，避免了直接退出后因为输出频率不同导致定位轨迹出现跳变，进而导致的车辆晃动或者画龙。
基于自动驾驶车辆融合定位方法的同一发明构思，本实施例提供一种电子设备，该电子设备如图3所示，包括：存储器301，处理器302，以及计算机程序。
其中，计算机程序存储在存储器301中，并被配置为由处理器302执行以实现上述自动驾驶车辆融合定位方法。
具体的，
当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式。其中，定位方式包括实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个。
确定切换后的定位方式的预测值。
根据当前的转换计数值和预测值确定虚拟观测点。
将虚拟观测点输入滤波器中，进行融合定位。
可选地，定位方式优先级由高到低依次为：实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，视觉SLAM。
可选地，根据当前的转换计数值和预测值确定虚拟观测点，包括：
若当前的转换计数值为0，则将预测值确定为虚拟观测点。
可选地，根据当前的转换计数值和预测值确定虚拟观测点，包括：
若当前的转换计数值大于0，则将当前的转换计数值减1。确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商。根据单帧变化位移量、当前的转换计数值和预测值计算当前的观测值。将当前的观测值作为虚拟观测点。
可选地，确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商之前，还包括：
将激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM进行时间同步以及频率扩展；其中，频率扩展后激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM的输出频率均与实时差分定位RTK的输出频率相同。
基于频率扩展后的信息，预测各定位方式在通用横墨卡托格网***UTM坐标系下的当前位置与GPS在UTM坐标系下的当前位置的差。
可选地，状态转换时间初始值为300。
在单帧变化位移量大于0.2米并且小于或等于0.4米，或者，车辆速度大于40千米/小时并且小于或等于50千米/小时，状态转换时间更新为600。
在单帧变化位移量大于0.4米，或者，车辆速度大于40千米/小时，状态转换时间更新为900。
可选地，根据单帧变化位移量、当前的转换计数值和预测值计算当前的观测值，包括：
计算当前的转换计数值与单帧变化位移量的积。
将预测值与积的差确定为当前的观测值。
本实施例提供的电子设备，其上计算机程序被处理器执行以在融合定位时，会根据切换后的定位方式的预测值确定虚拟观测点，将虚拟观测点输入滤波器中，进行融合定位，通过虚拟观测点，避免了直接退出后因为输出频率不同导致定位轨迹出现跳变，进而导致的车辆晃动或者画龙。
基于自动驾驶车辆融合定位方法的同一发明构思，本实施例提供一种计算机可读存储介质，且其上存储有计算机程序。计算机程序被处理器执行以实现上述自动驾驶车辆融合定位方法。
具体的，
当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式。其中，定位方式包括实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个。
确定切换后的定位方式的预测值。
根据当前的转换计数值和预测值确定虚拟观测点。
将虚拟观测点输入滤波器中，进行融合定位。
可选地，定位方式优先级由高到低依次为：实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，视觉SLAM。
可选地，根据当前的转换计数值和预测值确定虚拟观测点，包括：
若当前的转换计数值为0，则将预测值确定为虚拟观测点。
可选地，根据当前的转换计数值和预测值确定虚拟观测点，包括：
若当前的转换计数值大于0，则将当前的转换计数值减1。确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商。根据单帧变化位移量、当前的转换计数值和预测值计算当前的观测值。将当前的观测值作为虚拟观测点。
可选地，根确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商之前，还包括：
将激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM进行时间同步以及频率扩展；其中，频率扩展后激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM的输出频率均与实时差分定位RTK的输出频率相同。
基于频率扩展后的信息，预测各定位方式在通用横墨卡托格网***UTM坐标系下的当前位置与GPS在UTM坐标系下的当前位置的差。
可选地，状态转换时间初始值为300。
在单帧变化位移量大于0.2米并且小于或等于0.4米，或者，车辆速度大于40千米/小时并且小于或等于50千米/小时，状态转换时间更新为600。
在单帧变化位移量大于0.4米，或者，车辆速度大于40千米/小时，状态转换时间更新为900。
可选地，根据单帧变化位移量、当前的转换计数值和预测值计算当前的观测值，包括：
计算当前的转换计数值与单帧变化位移量的积。
将预测值与积的差确定为当前的观测值。
本实施例提供的计算机可读存储介质，其上的计算机程序被处理器执行以在融合定位时，会根据切换后的定位方式的预测值确定虚拟观测点，将虚拟观测点输入滤波器中，进行融合定位，通过虚拟观测点，避免了直接退出后因为输出频率不同导致定位轨迹出现跳变，进而导致的车辆晃动或者画龙。
本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。本申请实施例中的方案可以采用各种计算机语言实现，例如，面向对象的程序设计语言Java和直译式脚本语言JavaScript等。
本申请是参照根据本申请实施例的方法、设备（***）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本申请的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本申请范围的所有变更和修改。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。
Claims (8)
1.一种自动驾驶车辆融合定位方法，其特征在于，所述方法包括：
当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式；其中，定位方式包括实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个；
确定切换后的定位方式的预测值；
根据当前的转换计数值和所述预测值确定虚拟观测点；
将虚拟观测点输入滤波器中，进行融合定位；
所述根据当前的转换计数值和所述预测值确定虚拟观测点，包括：
若当前的转换计数值大于0，则将当前的转换计数值减1；确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商；根据所述单帧变化位移量、当前的转换计数值和所述预测值计算当前的观测值；将当前的观测值作为虚拟观测点；
所述根据所述单帧变化位移量、当前的转换计数值和所述预测值计算当前的观测值，包括：
计算当前的转换计数值与所述单帧变化位移量的积；
将所述预测值与所述积的差确定为当前的观测值。
2.根据权利要求1所述的方法，其特征在于，所述定位方式优先级由高到低依次为：实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，视觉SLAM。
3.根据权利要求1所述的方法，其特征在于，所述根据当前的转换计数值和所述预测值确定虚拟观测点，包括：
若当前的转换计数值为0，则将所述预测值确定为虚拟观测点。
4.根据权利要求1所述的方法，其特征在于，所述确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商之前，还包括：
将激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM进行时间同步以及频率扩展；其中，频率扩展后激光同步定位与建图SLAM、视觉辅助定位VAL和视觉SLAM的输出频率均与实时差分定位RTK的输出频率相同；
基于频率扩展后的信息，预测各定位方式在通用横墨卡托格网***UTM坐标系下的当前位置与GPS在UTM坐标系下的当前位置的差。
5.根据权利要求1所述的方法，其特征在于，所述状态转换时间初始值为300；
在单帧变化位移量大于0.2米并且小于或等于0.4米，或者，车辆速度大于40千米/小时并且小于或等于50千米/小时，所述状态转换时间更新为600；
在单帧变化位移量大于0.4米，或者，车辆速度大于40千米/小时，所述状态转换时间更新为900。
6.一种自动驾驶车辆融合定位装置，其特征在于，所述装置包括：
第一确定模块，用于当进入融合定位时，根据上一帧的定位方式以及预先设置的定位方式优先级，确定切换后的定位方式；其中，定位方式包括实时差分定位RTK，激光同步定位与建图SLAM，视觉辅助定位VAL，或者，视觉SLAM中的一个；
第二确定模块，用于确定切换后的定位方式的预测值；
第三确定模块，用于根据当前的转换计数值和所述预测值确定虚拟观测点；
融合定位模块，用于将虚拟观测点输入滤波器中，进行融合定位；
所述根据当前的转换计数值和所述预测值确定虚拟观测点，包括：
若当前的转换计数值大于0，则将当前的转换计数值减1；确定单帧变化位移量为切换后的定位方式的当前位置差和状态转换时间的商；根据所述单帧变化位移量、当前的转换计数值和所述预测值计算当前的观测值；将当前的观测值作为虚拟观测点；
所述根据所述单帧变化位移量、当前的转换计数值和所述预测值计算当前的观测值，包括：
计算当前的转换计数值与所述单帧变化位移量的积；
将所述预测值与所述积的差确定为当前的观测值。
7.一种电子设备，其特征在于，包括：
存储器；
处理器；以及
计算机程序；
其中，所述计算机程序存储在所述存储器中，并被配置为由所述处理器执行以实现如权利要求1-5任一项所述的方法。
8.一种计算机可读存储介质，其特征在于，其上存储有计算机程序；所述计算机程序被处理器执行以实现如权利要求1-5任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410123304.0A CN117647254B (zh) | 2024-01-30 | 2024-01-30 | 自动驾驶车辆融合定位方法、装置、设备、存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410123304.0A CN117647254B (zh) | 2024-01-30 | 2024-01-30 | 自动驾驶车辆融合定位方法、装置、设备、存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117647254A CN117647254A (zh) | 2024-03-05 |
| CN117647254B true CN117647254B (zh) | 2024-04-09 |
Family
ID=90043771
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410123304.0A Active CN117647254B (zh) | 2024-01-30 | 2024-01-30 | 自动驾驶车辆融合定位方法、装置、设备、存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117647254B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
| CN108535749A (zh) * | 2018-03-19 | 2018-09-14 | 千寻位置网络有限公司 | 基于cors的定位增强方法及***、定位*** |
| CN109900265A (zh) * | 2019-03-15 | 2019-06-18 | 武汉大学 | 一种camera/mems辅助北斗的机器人定位算法 |
| CN111998849A (zh) * | 2020-08-27 | 2020-11-27 | 湘潭大学 | 一种基于惯性导航***的差分动动定位的方法 |
| CN115143952A (zh) * | 2022-07-12 | 2022-10-04 | 智道网联科技(北京)有限公司 | 基于视觉辅助的自动驾驶车辆定位方法、装置 |
| CN116429090A (zh) * | 2023-04-27 | 2023-07-14 | 北京石头创新科技有限公司 | 基于线激光的同步定位和建图方法、装置、移动机器人 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102019203202A1 (de) * | 2019-03-08 | 2020-09-10 | Zf Friedrichshafen Ag | Lokalisierungssystem für ein fahrerloses Fahrzeug |
-
2024
- 2024-01-30 CN CN202410123304.0A patent/CN117647254B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
| CN108535749A (zh) * | 2018-03-19 | 2018-09-14 | 千寻位置网络有限公司 | 基于cors的定位增强方法及***、定位*** |
| CN109900265A (zh) * | 2019-03-15 | 2019-06-18 | 武汉大学 | 一种camera/mems辅助北斗的机器人定位算法 |
| CN111998849A (zh) * | 2020-08-27 | 2020-11-27 | 湘潭大学 | 一种基于惯性导航***的差分动动定位的方法 |
| CN115143952A (zh) * | 2022-07-12 | 2022-10-04 | 智道网联科技(北京)有限公司 | 基于视觉辅助的自动驾驶车辆定位方法、装置 |
| CN116429090A (zh) * | 2023-04-27 | 2023-07-14 | 北京石头创新科技有限公司 | 基于线激光的同步定位和建图方法、装置、移动机器人 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117647254A (zh) | 2024-03-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP2498058B1 (en) | Navigation apparatus | |
| US7460952B2 (en) | Navigation apparatus, and data processing method and computer program used therewith | |
| EP2482039B1 (en) | Navigation apparatus | |
| JP5298904B2 (ja) | 車両用走行情報提供装置 | |
| JP2009058332A (ja) | ナビゲーション装置 | |
| JP2008014973A (ja) | ナビゲーション装置 | |
| CN103033190A (zh) | 显示方向看板实景图的方法、装置及导航仪 | |
| US8825386B2 (en) | Navigation apparatus | |
| CN113340318A (zh) | 车辆导航方法、装置、电子设备及存储介质 | |
| JP3736382B2 (ja) | ナビゲーション装置及び画像表示方法のプログラム | |
| CN117490705B (zh) | 车辆导航定位方法、***、装置和计算机可读介质 | |
| JP4950624B2 (ja) | 自動制動制御装置 | |
| CN117647254B (zh) | 自动驾驶车辆融合定位方法、装置、设备、存储介质 | |
| EP3470790A1 (en) | Information processing device and travel control system | |
| JP2008190906A (ja) | 車両用地図表示装置 | |
| JP2000283772A (ja) | 走行位置表示装置 | |
| JP2006138798A (ja) | 車載ナビゲーション装置及び経路案内方法 | |
| JP2013205224A (ja) | 車載機、車載システム、情報処理方法、及び、プログラム | |
| JP5881308B2 (ja) | ナビゲーション装置および交差点案内方法 | |
| EP3845860A1 (en) | Image control program, image control device, and image control method | |
| JP2011058960A (ja) | Ｐｏｉ位置決定方法およびｐｏｉ情報の加工方法ならびにナビゲーション装置 | |
| JP2006242890A (ja) | ナビゲーション装置およびプログラム | |
| JP2020140602A (ja) | 地図データ更新システム、走行プローブ情報収集装置、走行プローブ情報提供装置および走行プローブ情報収集方法 | |
| JP3773015B2 (ja) | 地図描画用データ作成方法及びナビゲーション装置 | |
| JP5826368B2 (ja) | 地図情報処理装置 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |