CN114323033A - 基于车道线和特征点的定位方法、设备及自动驾驶车辆 - Google Patents

基于车道线和特征点的定位方法、设备及自动驾驶车辆 Download PDF

Info

Publication number
CN114323033A
CN114323033A CN202111637724.3A CN202111637724A CN114323033A CN 114323033 A CN114323033 A CN 114323033A CN 202111637724 A CN202111637724 A CN 202111637724A CN 114323033 A CN114323033 A CN 114323033A
Authority
CN
China
Prior art keywords
sliding window
determining
lane line
time
coordinate system
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.)
Granted
Application number
CN202111637724.3A
Other languages
English (en)
Other versions
CN114323033B (zh
Inventor
何宇喆
侯深化
周尧
彭亮
万国伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Baidu Netcom Science and Technology Co Ltd
Original Assignee
Beijing Baidu Netcom Science and Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Baidu Netcom Science and Technology Co Ltd filed Critical Beijing Baidu Netcom Science and Technology Co Ltd
Priority to CN202111637724.3A priority Critical patent/CN114323033B/zh
Publication of CN114323033A publication Critical patent/CN114323033A/zh
Priority to JP2022205078A priority patent/JP2023036796A/ja
Priority to EP22216555.7A priority patent/EP4155679A3/en
Priority to US18/088,954 priority patent/US20230134569A1/en
Priority to KR1020220185242A priority patent/KR20230008000A/ko
Application granted granted Critical
Publication of CN114323033B publication Critical patent/CN114323033B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • 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/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3844—Data obtained from position sensors only, e.g. from inertial navigation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G06V20/588—Recognition of the road, e.g. of lane markings; Recognition of the vehicle driving pattern in relation to the road
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • 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/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • 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/36—Input/output arrangements for on-board computers
    • G01C21/3626—Details of the output of route guidance instructions
    • G01C21/3658—Lane guidance
    • 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
    • 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/3859—Differential updating map data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G06V20/58—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads
    • 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
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0002—Automatic control, details of type of controller or control system architecture
    • B60W2050/0004—In digital systems, e.g. discrete-time systems involving sampling
    • B60W2050/0005—Processor details or data handling, e.g. memory registers or chip architecture
    • 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
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/28—Wheel speed
    • 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
    • B60W2552/00—Input parameters relating to infrastructure
    • B60W2552/53—Road markings, e.g. lane marker or crosswalk
    • 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
    • B60W2554/00—Input parameters relating to objects
    • B60W2554/40—Dynamic objects, e.g. animals, windblown objects
    • B60W2554/404—Characteristics
    • B60W2554/4041—Position
    • 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
    • B60W2554/00—Input parameters relating to objects
    • B60W2554/40—Dynamic objects, e.g. animals, windblown objects
    • B60W2554/404—Characteristics
    • B60W2554/4049—Relationship among other objects, e.g. converging dynamic objects
    • 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
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Theoretical Computer Science (AREA)
  • Human Computer Interaction (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Traffic Control Systems (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)
  • Closed-Circuit Television Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本公开提供了基于车道线和特征点的定位方法、设备及自动驾驶车辆，涉及计算机技术领域，尤其涉及自动驾驶、智能交通、计算机视觉等领域。具体实现方案为：根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差，第一传感器信息包括第一惯性测量单元信息、第一车道线特征信息和第一非车道线特征点信息，第一实时测量残差包括第一惯性测量单元测量残差、第一车道线测量残差和第一非车道线测量残差；根据惯性测量单元的运动学模型和多个第一实时测量残差，对可移动对象的状态向量进行更新；以及根据更新后的状态向量中的位姿量，确定可移动对象在与更新后的状态向量相对应时刻的位姿。

Description

基于车道线和特征点的定位方法、设备及自动驾驶车辆
技术领域
本公开涉及计算机技术领域，尤其涉及自动驾驶、智能交通、计算机视觉等领域，具体地，涉及一种基于车道线和特征点的定位方法、设备及自动驾驶车辆。
背景技术
随着计算机、互联网技术的发展，在车辆、机器人等领域衍生出一种用于控制车辆或机器人移动的领航辅助驾驶方案。领航辅助驾驶方案可以在高速、城市快速、城市等道路场景中，以及有高精地图的区域和无高精地图的区域中，根据导航引导路径、限速信息和车流状态等条件，辅助车辆控制巡航车速，汇入主路，进入匝道，并在合适的时机变换车道。
发明内容
本公开提供了一种基于车道线和特征点的定位方法、设备及自动驾驶车辆。
根据本公开的一方面，提供了一种基于车道线和特征点的定位方法，包括：根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差，所述第一传感器信息包括第一惯性测量单元信息、第一车道线特征信息和第一非车道线特征点信息，所述第一实时测量残差包括第一惯性测量单元测量残差、第一车道线测量残差和第一非车道线测量残差；根据惯性测量单元的运动学模型和所述多个第一实时测量残差，对所述可移动对象的状态向量进行更新；以及根据更新后的状态向量中的位姿量，确定所述可移动对象在与所述更新后的状态向量相对应时刻的位姿。
根据本公开的另一方面，提供了一种基于车道线和特征点的定位装置，包括：第一确定模块，用于根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差，所述第一传感器信息包括第一惯性测量单元信息、第一车道线特征信息和第一非车道线特征点信息，所述第一实时测量残差包括第一惯性测量单元测量残差、第一车道线测量残差和第一非车道线测量残差；更新模块，用于根据惯性测量单元的运动学模型和所述多个第一实时测量残差，对所述可移动对象的状态向量进行更新；以及第二确定模块，用于根据更新后的状态向量中的位姿量，确定所述可移动对象在与所述更新后的状态向量相对应时刻的位姿。
根据本公开的另一方面，提供了一种电子设备，包括：至少一个处理器；以及与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如上所述的基于车道线和特征点的定位方法。
根据本公开的另一方面，提供了一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行如上所述的基于车道线和特征点的定位方法。
根据本公开的另一方面，提供了一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现如上所述的基于车道线和特征点的定位方法。
根据本公开的另一方面，提供了一种自动驾驶车辆，包括如本公开所述的电子设备。
应当理解，本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征，也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。
附图说明
附图用于更好地理解本方案，不构成对本公开的限定。其中：
图1示意性示出了根据本公开实施例的可以应用基于车道线和特征点的定位方法及装置的示例性系统架构；
图2示意性示出了根据本公开实施例的基于车道线和特征点的定位方法的流程图；
图3示意性示出了根据本公开实施例的初始化流程的示意图；
图4A示意性示出了根据本公开一实施例的确定车道线点线关联关系的示意图；
图4B示意性示出了根据本公开另一实施例的确定车道线点线关联关系的示意图；
图5示意性示出了根据本公开实施例的基于车道线和特征点的视觉惯性里程计的工作原理图；
图6示意性示出了根据本公开实施例的基于车道线和特征点的定位装置的框图；以及
图7示出了可以用来实施本公开的实施例的示例电子设备的示意性框图。
具体实施方式
以下结合附图对本公开的示范性实施例做出说明，其中包括本公开实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本公开的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
在本公开的技术方案中，所涉及的用户个人信息的收集、存储、使用、加工、传输、提供和公开等处理，均符合相关法律法规的规定，采取了必要保密措施，且不违背公序良俗。
领航辅助驾驶方案需要首先基于视觉惯性里程计定位技术实现对车辆的实时定位，然后根据定位结果对车辆进行领航。视觉惯性里程计定位技术采用纯视觉的方法，结合局部里程计模型，通过融合图像和IMU(惯性测量单元)的信息，实现车辆定位。
发明人在实现本公开构思的过程中发现，在高速路以及一些公路场景中，存在大量的动态车辆并且环境纹理较弱，采用上述视觉惯性里程计定位方案难以提取到足够鲁棒的特征点，室外场景中通常光照变化明显，直接法匹配容易失败，导致仅依赖图像和IMU特征的视觉惯性里程计定位方案精度较弱并且不够鲁棒，定位失败的情况较多。
图1示意性示出了根据本公开实施例的可以应用基于车道线和特征点的定位方法及装置的示例性系统架构。
需要注意的是，图1所示仅为可以应用本公开实施例的系统架构的示例，以帮助本领域技术人员理解本公开的技术内容，但并不意味着本公开实施例不可以用于其他设备、系统、环境或场景。例如，在另一实施例中，可以应用基于车道线和特征点的定位方法及装置的示例性系统架构可以包括终端设备，但终端设备可以无需与服务器进行交互，即可实现本公开实施例提供的基于车道线和特征点的定位方法及装置。
如图1所示，根据该实施例的系统架构100可以包括车辆101、网络102和终端设备103、104、105、106。网络102用以在车辆101和终端设备103、104、105、106之间提供通信链路的介质。网络102可以包括各种连接类型，例如有线和/或无线通信链路等等。
车辆101的车机系统可以通过网络102与终端设备103、104、105、106交互，以接收或发送消息等。终端设备103、104、105、106上可以安装有各种通讯客户端应用，例如知识阅读类应用、网页浏览器应用、搜索类应用、即时通信工具、邮箱客户端和/或社交平台软件等(仅为示例)。
终端设备103、104、105可以是具有显示屏并且支持网页浏览的各种电子设备，包括但不限于智能手机、平板电脑、膝上型便携计算机和台式计算机等等。
终端设备106也可以是提供各种服务的服务器，例如对车机系统101接收到的数据进行处理的后台管理服务器(仅为示例)。后台管理服务器可以对接收到的车机数据进行分析等处理，并将处理结果反馈给车机101或其他终端设备103、104、105等。服务器可以是云服务器，又称为云计算服务器或云主机，是云计算服务体系中的一项主机产品，以解决了传统物理主机与VPS服务(″Virtual Private Server″，或简称″VPS″)中，存在的管理难度大，业务扩展性弱的缺陷。服务器也可以为分布式系统的服务器，或者是结合了区块链的服务器。
需要说明的是，本公开实施例所提供的基于车道线和特征点的定位方法一般可以由车机101中的车机系统执行。相应地，本公开实施例所提供的基于车道线和特征点的定位装置也可以设置于车机101中。
或者，本公开实施例所提供的基于车道线和特征点的定位方法一般也可以由终端设备103、104、105、106执行。相应地，本公开实施例所提供的基于车道线和特征点的定位装置一般可以设置于终端设备103、104、105、106中。本公开实施例所提供的基于车道线和特征点的定位方法也可以由不同于终端设备103、104、105、106且能够与终端设备103、104、105、106通信的服务器或服务器集群执行。相应地，本公开实施例所提供的基于车道线和特征点的定位装置也可以设置于不同于终端设备103、104、105、106且能够与终端设备103、104、105、106通信的服务器或服务器集群中。
例如，在需要确定可移动对象的位姿时，车辆101中的车机系统或终端设备103、104、105、106可以根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差，根据惯性测量单元的运动学模型和多个第一实时测量残差，对可移动对象的状态向量进行更新，并根据更新后的状态向量中的位姿量，确定可移动对象在与更新后的状态向量相对应时刻的位姿。或者由能够与车辆101或终端设备103、104、105、106通信的服务器或服务器集群对多个第一传感器信息进行分析，并实现确定可移动对象的位姿。需要说明的是，第一传感器信息可以包括第一惯性测量单元信息、第一车道线特征信息和第一非车道线特征点信息。
应该理解，图1中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。
图2示意性示出了根据本公开实施例的基于车道线和特征点的定位方法的流程图。
如图2所示，该方法包括操作S210～S230。
在操作S210，根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差，第一传感器信息包括第一惯性测量单元信息、第一车道线特征信息和第一非车道线特征点信息，第一实时测量残差包括第一惯性测量单元测量残差、第一车道线测量残差和第一非车道线测量残差。
在操作S220，根据惯性测量单元的运动学模型和多个第一实时测量残差，对可移动对象的状态向量进行更新。
在操作S230，根据更新后的状态向量中的位姿量，确定可移动对象在与更新后的状态向量相对应时刻的位姿。
根据本公开的实施例，可移动对象可以包括车辆、机器人等。可移动对象中可以设置有多个传感器，传感器可以包括IMU、图像采集装置、轮速计、车速计等其中至少之一。第一传感器信息可以包括第一惯性测量单元信息、第一车道线特征信息、第一非车道线特征点信息、第一轮速计尺度信息和第一车速计信息等其中至少之一。第一实时测量残差可以包括第一惯性测量单元测量残差、第一车道线测量残差、第一非车道线测量残差、第一轮速计尺度测量残差和第一车速测量残差等其中至少之一。
根据本公开的实施例，IMU可以实时采集车辆的角速度(rad/s)、加速度(m/s2)等信息，构成第一惯性测量单元信息。图像采集装置可以实时采集车辆周边的车道线图像、非车道线图像等信息。通过对非车道线图像进行提取特征匹配、稀疏光流法跟踪角点或者直接法匹配灰度梯度明显的像素点等处理，可以得到每个图像的特征点信息，构成第一非车道线特征点信息。通过对车道线图像进行处理，可以得到车道线中的每个点在车体坐标系下的三维坐标信息，构成第一车道线特征点信息。轮速计可以实时采集车辆的左轮、右轮等各个轮子的线速度(m/s)等信息，构成第一轮速计尺度信息。车速计可以实时采集车辆的车速(m/s)等信息，构成第一车速计信息。
需要说明的是，上述实时检测到的多个第一传感器信息，可以以滑动窗口为单位进行采集。每一帧滑动窗口中可以包括一帧图像，多个车道线信息，一个车速，一个轮速。每一帧滑动窗口可以将从与该帧滑动窗口相对应的前一帧滑动窗口所定位的时刻到该帧滑动窗口锁定为的时刻的IMU信息采集到一起。每一帧对应一个时刻，一个时刻对应一个相机坐标系，一个IMU坐标系。
根据本公开的实施例，可移动对象的状态向量可以包括IMU的状态向量、滑动窗口内相机位姿的状态向量以及轮速计尺度的状态向量等其中至少之一。IMU的状态向量可以表示为xI，xI中可以包括IMU坐标系和世界坐标系之间的旋转
Figure BDA0003442552080000061
IMU坐标系和世界坐标系之间的平移
Figure BDA0003442552080000062
IMU坐标系的速度在世界坐标系的表示
Figure BDA0003442552080000063
IMU的角速度计零偏(零偏)bg，以及IMU的加速度计零偏ba等，得到
Figure BDA0003442552080000064
T表示转置。滑动窗口内相机位姿的状态向量可以表示为
Figure BDA0003442552080000065
Figure BDA0003442552080000066
可以表示第i帧滑动窗口所对应的相机位姿的表示，
Figure BDA0003442552080000071
可以表示第i帧滑动窗口所对应的相机坐标系和世界坐标系之间的旋转，
Figure BDA0003442552080000072
可以表示第i帧滑动窗口所对应的相机坐标系和世界坐标系之间的平移。轮速计尺度的状态向量可以包括sl和sr，sl可以表示左轮轮速计尺度，sr可以表示右轮轮速计尺度。基于此，可以得到可移动对象的状态向量可以表示为
Figure BDA0003442552080000073
根据本公开的实施例，可移动对象的状态向量中的每个向量的值可以随着时间的变化，根据实时检测到的可移动对象的第一惯性测量单元信息、第一车道线特征信息、第一非车道线特征点信息、第一轮速计尺度信息和第一车速计信息等信息，对该所有信息进行融合，实现状态向量的实时更新。融合框架可以采用迭代优化方法或者卡尔曼滤波方法。
例如，融合框架可以采用MSCKF(Multi-State Constraint Kalman Filter，多约束状态卡尔曼滤波器)，基于误差状态卡尔曼滤波的方法，实现状态向量的实时更新。滤波器的状态可以包括IMU的误差状态、滑动窗口内相机位姿的误差状态以及轮速计尺度的误差状态。根据IMU的状态向量xI，可以得到IMU的误差状态向量可以表示为
Figure BDA0003442552080000074
Figure BDA0003442552080000075
中的每个向量可以相应的表示xI中的每个向量的误差。根据滑动窗口内相机位姿的状态向量
Figure BDA0003442552080000076
可以得到滑动窗口内相机位姿的误差状态向量可以表示为
Figure BDA0003442552080000077
Figure BDA0003442552080000078
中的每个向量可以相应的表示
Figure BDA0003442552080000079
中的每个向量的误差。根据轮速计尺度的状态向量sl和sr，可以得到轮速计尺度的误差状态向量可以表示为
Figure BDA00034425520800000710
和
Figure BDA00034425520800000711
基于此，可以得到滤波器的状态向量可以表示为
Figure BDA00034425520800000712
通过实时更新滤波器的状态向量，可以实时更新的可移动对象的状态向量。
根据本公开的实施例，在完成可移动对象的状态向量的更新后，可以输出当前时刻可移动对象的IMU位姿，根据IMU位姿可以确定当前时刻可移动对象的位姿。
通过本公开的上述实施例，可以融合IMU、车道线特征和非车道线特征，甚至融合轮速计、车速计等特征实现视觉惯性里程计定位，能够得到横向精度高并且平滑的局部定位结果。
下面结合具体实施例，对图2所示的方法做进一步说明。
根据本公开的实施例，上述第一传感器信息可以是在对可移动对象的所有状态向量均初始化完成之后的时间段内采集到的传感器信息。在可移动对象的状态向量未完成初始化之前，上述基于车道线和特征点的定位方法还可以包括：根据检测到的可移动对象的多个第二传感器信息，确定多个第二实时测量残差。第二传感器信息包括第二惯性测量单元信息、第二车道线特征信息和第二非车道线特征点信息，第二实时测量残差包括第二惯性测量单元测量残差、第二车道线测量残差和第二非车道线测量残差。根据多个第二实时测量残差，确定可移动对象的初始化位姿。根据第二车道线特征信息和第二惯性测量单元信息，确定可移动对象的初始化角速度计零偏。根据初始化角速度计零偏、第二惯性测量单元信息和初始化位姿，确定初始化轮速计尺度和初始化加速度计零偏其中至少之一。根据初始化位姿、初始化角速度计零偏、初始化轮速计尺度和初始化加速度计零偏，确定初始化完成的状态向量。
根据本公开的实施例，第二传感器信息可以是在未完成对可移动对象的状态向量的初始化的时间段内采集都的传感器信息。第二传感器信息可以包括第二惯性测量单元信息、第二车道线特征信息、第二非车道线特征点信息、第二轮速计尺度信息和第二车速计信息等其中至少之一。第二实时测量残差可以包括第二惯性测量单元测量残差、第二车道线测量残差、第二非车道线测量残差、第二轮速计尺度测量残差和第二车速测量残差等其中至少之一。第二传感器信息也可以通过设置于可移动对象中的IMU、图像采集装置、轮速计、车速计等传感器检测得到。
根据本公开的实施例，在对可移动对象的状态向量进行初始化的过程中，可以采用分阶段的方法，逐步估计和调优可移动对象的状态向量。
图3示意性示出了根据本公开实施例的初始化流程的示意图。
如图3所示，该初始化流程可以包括操作S310～S340。
在操作S310，BA优化。在该部分中可以根据多个第二实时测量残差，确定可移动对象的初始化位姿。例如，可以融合帧间轮速计积分残差311、帧间车道线特征点信息312、非车道线图像特征点信息313和IMU相关信息314等多个传感器的信息，基于BA(bundleadjustment，光束平差法)优化的方法，求解滑动窗口中每个图像帧的位姿，实现位姿的初始化。
在操作S320，视觉惯性对齐。该部分中例如还可以分解为操作S321～S324等子步骤，实现逐步估计除位姿外的其他状态变量。
在操作S321，估计角速度零偏。在该部分中例如可以根据第二车道线特征信息和第二惯性测量单元信息，结合最小二乘的方法，求解出可移动对象的初始化角速度计零偏。
在操作S322，估计速度、重力和轮速计尺度。在该部分中例如可以根据相邻两帧滑动窗口所定位的两个时刻的速度量、时间差、重力量、轮速计尺度以及位姿量等，结合最小二乘的方法，求解速度、重力、轮速计尺度的估计值。
在操作S323，优化重力、速度和轮速计尺度。在该部分中例如可以根据预设模长的重力量，结合速度、重力、轮速计尺度的估计值，实现优化重力、速度和轮速计尺度。
在操作S324，优化重力、轮速计尺度，估计加速度零偏。在该部分中例如可以根据初始化角速度计零偏、第二惯性测量单元信息和初始化位姿，确定初始化轮速计尺度和初始化加速度计零偏其中至少之一。需要说明的是，在该部分中还可以确定初始化重力，重力在初始化之后可以保持不变。
在操作S330，初始化是否完成？若是，则执行操作S340；若否，则执行操作S310～S320。
在操作S340，输出初始化状态。
根据本公开的实施例，在初始化为完成的情况下，可以不断重复上述操作S310～S320对应的初始化流程，直到状态初始化完成。得到每一图像帧优化后的状态向量，可以包括角速度计零偏、加速度零偏、位姿(包括旋转量、平移量)和速度量等。
通过本公开的上述实施例，结合IMU、车道线特征和非车道线特征，甚至融合轮速计、车速计等特征实现状态向量的初始化，可以提高初始化的成功率。此外，基于该初始化状态向量，进行视觉惯性里程计定位时，可以提高求解的稳定性，能够得到横向精度高并且平滑的局部定位结果。
根据本公开的实施例，在基于BA优化确定初始化位姿的过程中，例如需要首先获得滑动窗口内帧间特征点以及车道线的关联关系。特征点的提取可以采用基于深度学习的方法，可以滤除位于前景的特征点。特征点的帧间匹配可以采用描述子最近邻的方法，获得帧间的点对匹配。在获取3D车道线关联关系时，可以使用IMU和轮速计积分获得帧间的关联位姿，再将3D车道线上的点重投影到前几帧内，在一定半径范围内搜索车道线点，获得点线间的匹配对。然后，可以根据前述关联关系，采用图优化的方法，优化滑动窗口内图像帧的位姿。
根据本公开的实施例，第二轮速计尺度测量残差可以表征相邻两帧滑动窗口所定位的两个时刻之间的轮速计积分残差。根据检测到的可移动对象的多个第二传感器信息，确定多个第二实时测量残差可以包括：针对两个时刻中的每个时刻，确定惯性测量单元坐标系与世界坐标系之间的第七旋转量和第八平移量。根据惯性测量单元坐标系在两个时刻形成的第一旋转偏移量的第三测量值、轮速计积分在两个时刻形成的平移偏移量的第四测量值、惯性测量单元的角速度计零偏以及对应于两个时刻中的每个时刻的第七旋转量和第八平移量，确定轮速计积分残差。
根据本公开的实施例，例如可以通过IMU和轮速计积分获得不同帧滑动窗口所定位的不同时刻之间的相对位姿变换。在任意两个相邻帧滑动窗口所定位的时刻之间，可以构建帧间的轮速计积分残差rI，如公式(1)所示。
Figure BDA0003442552080000101
在公式(1)中，w可以表示世界坐标系。bi可以表示第i帧滑动窗口所对应的IMU坐标系。
Figure BDA0003442552080000102
可以表示第i帧滑动窗口所对应的IMU坐标系和世界坐标系之间的第七旋转量，以旋转矩阵形式表示。
Figure BDA0003442552080000103
可以表示第i帧滑动窗口所对应的IMU坐标系和世界坐标系之间的第七旋转量，以四元数形式表示。
Figure BDA0003442552080000104
可以表示第i帧滑动窗口所对应的IMU坐标系和世界坐标系之间的第八平移量。
Figure BDA0003442552080000105
可以表示第i帧滑动窗口所定位的时刻和第i+1帧滑动窗口所定位的时刻之间的轮速计积分的平移偏移量的第四测量值。
Figure BDA0003442552080000106
可以表示惯性测量单元坐标系在第i帧滑动窗口所定位的时刻和第i+1帧滑动窗口所定位的时刻之间形成的第一旋转偏移量的第三测量值。bg可以表示IMU的角速度计零偏。
Figure BDA0003442552080000111
可以表示旋转积分对角速度零偏的雅克比矩阵。
通过本公开的上述实施例，可以计算得到第二轮速计尺度测量残差，便于对可移动对象的位姿进行初始化。
根据本公开的实施例，第二车道线测量残差可以表征目标第二车道线特征点在第六滑动窗口所对应的直线和第七滑动窗口所对应的特征点位置形成的距离残差，第六滑动窗口所对应的时刻早于第七滑动窗口所对应的时刻。根据检测到的可移动对象的多个第二传感器信息，确定多个第二实时测量残差可以包括：确定第六滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第四平移量和第三旋转量，以及第七滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第五平移量和第四旋转量。确定目标第二车道线特征点在第六图像帧所对应的归一化平面中的第三位置信息，第六图像帧与第六滑动窗口相对应。确定目标第二车道线特征点在第七图像帧所对应的归一化平面中的第四位置信息，第七图像帧与第七滑动窗口相对应。确定第六图像帧中与目标第二车道线特征点相对应的直线的方向向量。根据第三位置信息、第四平移量、第三旋转量、第五平移量、第四旋转量、第四位置信息、方向向量以及相机的外参和惯性测量单元的外参，确定第一车道线测量残差。
图4A示意性示出了根据本公开一实施例的确定车道线点线关联关系的示意图。
图4B示意性示出了根据本公开另一实施例的确定车道线点线关联关系的示意图。
如图4A和4B所示，实心点例如表示上一帧滑动窗口所采集到的车道线对应的车道线3D点，空心点例如表示当前帧滑动窗口所采集到的车道线对应的车道线3D点。空心点411、412例如表示待匹配点，圆形区域421、422可以表示搜索半径范围，直线413、423可以表示与相关车道线相对应的拟合直线。通过将车道线3D点转换到前几帧中可以获得点线间的关联关系。例如，在一定搜索半径范围421内搜索车道线3D点411，可以获得上一帧滑动窗口所采集到的车道线和当前帧滑动窗口所采集到的车道线之间的关联关系，并确定点线间的匹配对。
根据本公开的实施例，通过车道线的点线匹配对，可以将车道线的帧间约束构建为点到直线的距离残差，设第k个车道线的第l个3D点在第j帧滑动窗口所对应的相机坐标系下的位置为
Figure BDA0003442552080000121
对应的匹配直线的方向向量为V，直线上的点为pl，根据车道线构建的残差rl可以表示为公式(2)所示。
Figure BDA0003442552080000122
在公式(2)中，
Figure BDA0003442552080000123
可以表示相机的外参。
Figure BDA0003442552080000124
可以表示IMU的外参。
Figure BDA0003442552080000125
可以表示第i帧滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第三旋转量。
Figure BDA0003442552080000126
可以表示第i帧滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第四平移量。
Figure BDA0003442552080000127
可以表示第j帧滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第四旋转量。
Figure BDA0003442552080000128
可以表示第j帧滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第五平移量。第i帧滑动窗口可以与第六滑动窗口相对应。第j帧滑动窗口可以与第七滑动窗口相对应。匹配直线可以包括第六滑动窗口所对应的直线。pl可以表示目标第二车道线特征点在第六图像帧所对应的归一化平面中的第三位置信息。
Figure BDA0003442552080000129
可以表示目标第二车道线特征点在第七图像帧所对应的归一化平面中的第四位置信息。V可以表示第六图像帧中与目标第二车道线特征点相对应的直线的方向向量。
通过本公开的上述实施例，可以计算得到第二车道线测量残差，便于对可移动对象的位姿进行初始化。
根据本公开的实施例，第二非车道线测量残差可以表征目标第二非车道线特征点在与第八滑动窗口相对应的第八图像帧中的重投影残差。根据检测到的可移动对象的多个第二传感器信息，确定多个第二实时测量残差可以包括：根据目标第二非车道线特征点在第八滑动窗口所对应的相机坐标系下的第一像素横坐标和第一像素列坐标，确定目标第二非车道线特征点在第八滑动窗口所对应的相机坐标系下的第五位置信息的第二测量值。确定世界坐标系和第八滑动窗口所对应的惯性测量单元坐标系之间的第五旋转量和第六平移量，以及世界坐标系和第九滑动窗口所对应的惯性测量单元坐标系之间的第六旋转量和第七平移量，第九滑动窗口所对应的时刻早于第八滑动窗口所对应的时刻。根据目标第二非车道线特征点在第九滑动窗口所对应的相机坐标系下的第二像素横坐标和第二像素列坐标，以及第五旋转量、第六平移量、第六旋转量、第七平移量、相机的外参和惯性测量单元的外参，确定目标第二非车道线特征点在第八滑动窗口所对应的相机坐标系下的第五位置信息的第二计算值。根据第二测量值和第二计算值，确定第二非车道线测量残差。
根据本公开的实施例，通过图像的特征匹配点对，可以构建点的重投影残差，设第l个特征点分别被第i个图像帧和第j个图像帧观测到，根据图像特征点对构建的重投影残差rp可以表示为公式(3)所示。
Figure BDA0003442552080000131
在公式(3)中，cj可以表示第j帧滑动窗口所对应的相机坐标系。
Figure BDA0003442552080000132
可以表示第l个特征点在第j帧滑动窗口所对应的相机坐标系下的归一化平面坐标的第二测量值。
Figure BDA0003442552080000133
可以表示第l个特征点在第j帧滑动窗口所对应的相机坐标系下的归一化平面坐标的第一像素横坐标。
Figure BDA0003442552080000134
可以表示第l个特征点在第j帧滑动窗口所对应的相机坐标系下的归一化平面坐标的第一像素列坐标。
Figure BDA0003442552080000135
可以表示第l个特征点在第j帧滑动窗口所对应的相机坐标系下的归一化平面坐标的第二计算值。
Figure BDA0003442552080000136
可以表示第j帧滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第五旋转量。
Figure BDA0003442552080000137
可以表示第i帧滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第六旋转量。
Figure BDA0003442552080000138
可以表示表示第j帧滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第六平移量。
Figure BDA0003442552080000139
可以表示表示第i帧滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第七平移量。
Figure BDA00034425520800001310
可以表示第l个特征点在第i帧滑动窗口所对应的相机坐标系下的第二像素横坐标。
Figure BDA00034425520800001311
可以表示第l个特征点在第i帧滑动窗口所对应的相机坐标系下的第二像素列坐标。λl表示第l个特征点的逆深度。第j帧滑动窗口可以与第八滑动窗口相对应。第i帧滑动窗口可以与第九滑动窗口相对应。第l个特征点可以表示目标第二非车道线特征点。
通过本公开的上述实施例，可以计算得到第二非车道线测量残差，便于对可移动对象的位姿进行初始化。
根据本公开的实施例，在通过上述方法计算得到上述第二轮速计尺度测量残差、第二车道线测量残差以及第二非车道线测量残差的基础上，可以将该些残差综合起来，构建得到如公式(4)所示的图优化模型，实现根据多个第二实时测量残差，确定可移动对象的初始化位姿。
Figure BDA0003442552080000141
在公式(4)中，PI可以表示rI残差的协方差矩阵，Pl可以表示rl残差的协方差矩阵，Pp可以表示rp残差的协方差矩阵。
根据本公开的实施例，还可以基于类似的方法计算得到第二车速测量残差和第二惯性测量单元测量残差。并可以根据第二轮速计尺度测量残差、第二车道线测量残差、第二非车道线测量残差、第二车速测量残差以及第二惯性测量单元测量残差构建类似于公式(4)的图优化模型。
根据本公开的实施例，针对如公式(4)所示的图优化模型，可以采用LM方法迭代求解上式，并求得滑动窗口内各帧的初始化位姿和角速度计零偏。通过将轮速计积分得到的位姿作为位姿初始值，用位姿初始值三角化角点得到的逆深度作为逆深度初始值，可以使上式快速收敛。
通过本公开的上述实施例，可以融合IMU、车道线特征和非车道线特征，甚至融合轮速计、车速计等信息进行位姿初始化，可以提高初始化结果的精确度。
根据本公开的实施例，在求得初始化位姿的之后，可以估计初始化角速度计零偏。根据第二车道线特征信息和第二惯性测量单元信息，确定可移动对象的初始化角速度计零偏可以包括：根据惯性测量单元坐标系在相邻两帧滑动窗口所定位的两个时刻形成的第二旋转偏移量的第五测量值和角速度计零偏，确定惯性测量单元坐标系在相邻两帧滑动窗口所定位的两个时刻形成的第二旋转偏移量的第三计算值。根据第三计算值和第五测量值，确定初始化角速度计零偏。
根据本公开的实施例，在视觉惯性对齐的过程中，可以首先进行初始化角速度计零偏的估计。在初始化角速度计零偏的过程中，可以根据相邻两帧滑动窗口间车道线的关联关系，求出惯性测量单元坐标系在相邻两帧滑动窗口所定位的两个时刻形成的第二旋转偏移量的第三计算值
Figure BDA0003442552080000142
通过相邻两帧滑动窗口间的IMU积分，可以得到惯性测量单元坐标系在相邻两帧滑动窗口所定位的两个时刻形成的第二旋转偏移量的第五测量值
Figure BDA0003442552080000151
基于该两个第二旋转偏移量理论上应相同的原理，可以通过对IMU预积分线性化，得到公式(5)。
Figure BDA0003442552080000152
在公式(5)中，表示了上述相邻两帧滑动窗口可以包括第k帧滑动窗口和第k+1帧滑动窗口。J可以表示雅克比矩阵。
根据公式(5)，可以使用最小二乘的方法，结合MAP方法，求出初始化角速度计零偏δbg。该过程可以表示为公式(6)所示。
Figure BDA0003442552080000153
在公式(6)中，β可以表示所有帧滑动窗口所对应的所有图像帧。Q可以表示协方差矩阵。
通过本公开的上述实施例，可以计算得到初始化角速度计零偏，有利于状态向量初始化的进一步完成。
根据本公开的实施例，根据初始化角速度计零偏、第二惯性测量单元信息和初始化位姿，确定初始化轮速计尺度和初始化加速度计零偏其中至少之一可以包括：在位姿量和角速度计零偏均初始化完成的情况下，根据第十滑动窗口所对应的第一目标速度量、第十一滑动窗口所对应的第二目标速度量、第十滑动窗口所对应的时刻和第十一滑动窗口所对应的时刻形成的时间差、重力量、轮速计尺度以及第十滑动窗口和第十一滑动窗口分别对应的位姿量和旋转量，确定惯性测量单元预积分模型。确定与惯性测量单元预积分模型相对应的最小二乘优化模型，最小二乘优化模型中包括待估计速度量、待估计重力量和待估计轮速计尺度其中至少之一。根据预设模长的重力量、惯性测量单元预积分模型和最小二乘优化模型，确定初始化轮速计尺度和初始化加速度计零偏其中至少之一。
根据本公开的实施例，在完成IMU的初始化角速度零偏的估计后，可以对IMU重新积分，得到更准确的帧间IMU预积分值
Figure BDA0003442552080000154
和
Figure BDA0003442552080000155
如公式(7)所示。
Figure BDA0003442552080000161
在公式(7)中，tk可以表示第k帧滑动窗口所对应的时刻和第k+1帧滑动窗口所对应的时刻之间的时间差。
Figure BDA0003442552080000162
可以表示第k帧滑动窗口所对应的第一目标速度量在第k帧滑动窗口所对应的IMU坐标系下的表示。
Figure BDA0003442552080000163
可以表示第k+1帧滑动窗口所对应的第二目标速度量在第k+1帧滑动窗口所对应的IMU坐标系下的表示。gw可以表示重力在世界坐标系下的表示。s可以表示轮速计尺度。第k帧滑动窗口可以与第十滑动窗口相对应。第k+1帧滑动窗口可以与第十一滑动窗口相对应。
Figure BDA0003442552080000164
和
Figure BDA0003442552080000165
可以分别表示第十滑动窗口和第十一滑动窗口分别对应的位姿量。
Figure BDA0003442552080000166
和
Figure BDA0003442552080000167
可以分别表示第十滑动窗口和第十一滑动窗口分别对应的旋转量。
根据本公开的实施例，公式(7)中相应帧滑动窗口所对应的位姿可以由BA优化图像帧位姿确定。通过将公式(7)转换为矩阵表示可以得到惯性测量单元预积分模型，可以表示为公式(8)所示。
Akxk＝Bk
Figure BDA0003442552080000168
在公式(8)中，I3×3可以表示3×3矩阵。03×3可以表示3×3的零矩阵。
根据本公开的实施例，可以采用MAP方法求解速度、重力和轮速计尺度的估计值。在公式(8)的基础上，可以确定相应的待估计变量
Figure BDA0003442552080000169
可以表示为：
Figure BDA00034425520800001610
基于待估计变量
Figure BDA00034425520800001611
可以得到与惯性测量单元预积分模型相对应的最小二乘优化模型，可以表示为公式(9)所示。
Figure BDA00034425520800001612
在公式(9)中，β表示所有帧滑动窗口所对应的所有图像帧，k可以表示第k帧滑动窗口所对应的图像帧。P可以表示协方差矩阵。
根据本公开的实施例，在经由公式(7)～(9)求解得到速度、重力和轮速计尺度的估计值之后，可以进行优化重力、速度和轮速计尺度的过程。在优化重力、速度和轮速计尺度的过程中，可以将重力的模长设为固定值，优化重力的方向，预设模长的重力量可以表示为公式(10)所示。
Figure BDA0003442552080000171
在公式(10)中，||g||可以表示重力的预设模长，
Figure BDA0003442552080000172
可以表示重力单位方向向量，w1和w2为系数，b1和b2可以表示重力所在球面切空间的一对正交基。
根据本公开的实施例，可以将公式(10)带入到公式(8)和公式(9)中，得到关于待估计变量的优化问题，可以表示为公式(11)所示。
Figure BDA0003442552080000173
在公式(11)中，Bkxk-dk可以表示优化问题的线性最小二乘形式，P可以表示轮速计尺度的先验协方差矩阵。
根据本公开的实施例，可以基于公式(11)，进行优化重力、轮速计尺度，估计加速度零偏的过程。在该部分可以仅估计重力、轮速计尺度和加速度零偏，需要根据公式(7)将速度消去，重力的表示形式可以与公式(10)相同，每三个相邻的图像帧可以得到如公式(12)所示的方程。
Figure BDA0003442552080000174
在公式(12)中，M可以表示常值矩阵，η可以表示状态变量，g0可以表示重力初始值。
根据本公开的实施例，由于IMU的零偏通常比较小，将预积分结果线性化可以得到如公式(13)所示的形式。
Figure BDA0003442552080000175
在公式(13)中，αk可以表示
Figure BDA0003442552080000176
αk+1可以表示
Figure BDA0003442552080000177
βk可以表示
Figure BDA0003442552080000178
ba可以表示加速度计零偏，
Figure BDA0003442552080000179
可以表示
Figure BDA00034425520800001710
对ba的雅克比矩阵。
Figure BDA00034425520800001711
可以表示
Figure BDA00034425520800001712
时ba的雅克比矩阵。
Figure BDA00034425520800001713
可以表示
Figure BDA00034425520800001714
对ba的雅克比矩阵。
根据本公开的实施例，通过将线性化形式(13)带入公式(12)中，采用MAP方法，可以得到公式(14)。通过求解公式(14)，例如可以估计得到初始化重力、初始化轮速计尺度和初始化加速度计零偏。
Figure BDA0003442552080000181
在公式(14)中，
Figure BDA0003442552080000182
可以表示最小二乘形式。
通过本公开的上述实施例，可以更准确的计算得到初始化轮速计尺度、初始化加速度计零偏等，实现对轮速计尺度和加速度计零偏的初始化，从而实现对所有状态向量的初始化。
根据本公开的实施例，在针对可移动对象中所有状态向量均初始化完成之后，可以根据惯性测量单元的运动学模型和多个第一实时测量残差，对可移动对象的状态向量进行更新。在此之前，例如可以首先根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差。
根据本公开的实施例，第一车道线测量残差可以表征目标第一车道线特征点在第一滑动窗口所对应的直线和第二滑动窗口所对应的特征点位置形成的距离残差，第一滑动窗口所对应的时刻早于第二滑动窗口所对应的时刻。根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差包括：确定第一图像帧所对应的第一位姿，以及第二图像帧所对应的第二位姿，第一图像帧与第一滑动窗口相对应，第二图像帧与第二滑动窗口相对应。确定目标第一车道线特征点在第二图像帧所对应的归一化平面中的第一位置信息。根据第一位姿、第二位姿、第一位置信息、以及预定义线性模型，确定第一车道线测量残差。
根据本公开的实施例，例如，可以基于公式(15)计算得到上述第一车道线测量残差
Figure BDA0003442552080000183
Figure BDA0003442552080000184
在公式(15)中，
Figure BDA0003442552080000185
可以表示第k个车道线的点根据第i帧滑动窗口所对应的图像帧和第j帧滑动窗口所对应的图像帧构建的距离残差。d为一种线性表示形式。Tj可以表示与第j帧滑动窗口相对应的第一图像帧所对应的第一位姿。
Figure BDA0003442552080000186
可以表示残差
Figure BDA0003442552080000187
对第j帧滑动窗口所对应的第一图像帧位姿的雅克比矩阵。Ti可以表示与第i帧滑动窗口相对应的第二图像帧所对应的第二位姿。
Figure BDA0003442552080000191
可以表示残差
Figure BDA0003442552080000192
对第i帧滑动窗口所对应的第二图像帧位姿的雅克比矩阵。
Figure BDA0003442552080000193
可以表示第k个车道线的特征点在第i帧滑动窗口所对应的归一化平面的第一位置信息。Jp可以表示残差
Figure BDA0003442552080000194
对第k个特征点
Figure BDA0003442552080000195
的雅克比矩阵。第j帧滑动窗口可以与第一滑动窗口相对应。第i帧滑动窗口可以与第二滑动窗口相对应。
Figure BDA0003442552080000196
可以表示目标第一车道线特征点。由于点的位置不进行估计，可以用Jp的左零空间消除和点位置有关的项。
通过本公开的上述实施例，可以计算得到第一车道线测量残差，基于该残差结合其他传感器信息的第一实时测量残差进行视觉惯性里程计定位时，能够得到横向精度高并且平滑的局部定位结果。
根据本公开的实施例，第一非车道线测量残差可以表征目标第一非车道线特征点在与第三滑动窗口相对应的第三图像帧中的重投影残差。根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差可以包括：根据目标第一非车道线特征点在世界坐标系中的世界坐标信息，确定目标第一非车道线特征点在第三图像帧所对应的归一化平面中的第二位置信息的第一测量值。确定世界坐标系和惯性测量单元坐标系之间的第一旋转量和第一平移量。根据世界坐标信息、第一旋转量、第一平移量以及相机的外参和惯性测量单元的外参，确定目标第一非车道线特征点在第三图像帧所对应的归一化平面中的第二位置信息的第一计算值。根据第一测量值和第一计算值，确定第一非车道线测量残差。
根据本公开的实施例，根据点对的关联关系，利用用滑动窗口所对应的图像帧的位姿对特征点进行三角化，可以得到特征点在世界坐标系下的坐标pw。据此可以构建重投影残差rp，如公式(16)所示。
Figure BDA0003442552080000197
在公式(16)中，
Figure BDA0003442552080000201
可以表示第k个特征点在第c帧滑动窗口所对应的第三图像帧的归一化平面中的第二位置信息的第一测量值。h(·)可以表示将相机坐标系下三维坐标投影到相机归一化平面。
Figure BDA0003442552080000202
可以表示第k个特征点在第c帧滑动窗口所对应的第三图像帧图像坐标系下坐标，可对应上述第二位置信息的第一计算值。
Figure BDA0003442552080000203
可以表示第k个特征点在世界坐标系中的世界坐标信息。
Figure BDA0003442552080000204
可以表示IMU坐标系和世界坐标系之间的第一旋转量。
Figure BDA0003442552080000205
可以表示IMU坐标系和世界坐标系之间的第一平移量。
根据本公开的实施例，还可以对公式(16)进行线性化得到误差状态方程，如公式(17)所示。基于公式(17)确定的
Figure BDA0003442552080000206
可应用于MSCKF的滤波过程中，实现对状态向量进行实时更新。
Figure BDA0003442552080000207
在公式(17)中，
Figure BDA0003442552080000208
可以表示测量残差。x可以表示所有状态向量。Hx可以表示残差
Figure BDA0003442552080000209
对状态变量的雅克比矩阵。Hp可以表示残差
Figure BDA00034425520800002010
对特征点坐标的雅克比矩阵。n2可以表示噪声。由于特征点在世界坐标系下的位置不进行估计，可以用Hp的左零空间消除和特征点位置变量有关的项。
通过本公开的上述实施例，可以计算得到第一非车道线测量残差，基于该残差结合其他传感器信息的第一实时测量残差进行视觉惯性里程计定位时，能够得到横向精度高并且平滑的局部定位结果。
根据本公开的实施例，第一轮速计尺度测量残差可以表征可移动对象的每个轮子的尺度测量残差。根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差可以包括：针对可移动对象的每个轮子，根据世界坐标系和惯性测量单元坐标系之间的位姿，世界坐标系下惯性测量单元的速度、轮子的坐标系和惯性测量单元坐标系之间的位移、惯性测量单元检测到的轮子的角速度以及轮子的尺度、半径和速度，确定轮子的尺度测量残差。
根据本公开的实施例，第一车速测量残差可以表征可移动对象的移动速度的测量残差。根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差可以包括：根据可移动对象在相邻时刻内的平均速度和世界坐标系下惯性测量单元的速度，确定第一车速测量残差。
根据本公开的实施例，第一轮速计尺度测量残差和第一车速测量残差可以合并计算。可以针对可以的对象中的每个轮子分别构造残差。例如，可以针对左右轮分别构造残差，以估计左右轮尺度。采用与IMU同频率的更新，或者与图像帧同频率的更新，得到残差e的计算公式例如如公式(18)所示。
Figure BDA0003442552080000211
在公式(18)中，el可以表示左轮的残差。er可以表示右轮的残差。ev可以表示车速的残差。
Figure BDA0003442552080000212
可以表示世界坐标系和IMU坐标系之间的相对位姿。vG可以表示IMU的速度在世界坐标系下的表示。sl可以表示左轮的尺度。rl可以表示车轮半径。vl可以表示左轮角速度。
Figure BDA0003442552080000213
可以表示左轮坐标系和IMU坐标系之间的旋转。ω×可以表示IMU检测到的左轮的角速度。tl可以表示IMU坐标系和左轮坐标系之间的位移。相应地，右轮的相关参数具有相对应的释义。
Figure BDA0003442552080000215
可以表示平均速度。
通过本公开的上述实施例，可以计算得到第一轮速计尺度测量残差和第一车速测量残差，基于该残差结合其他传感器信息的第一实时测量残差进行视觉惯性里程计定位时，能够得到横向精度高并且平滑的局部定位结果。
根据本公开的实施例，在计算第一惯性测量单元测量残差之前，可以首先通过轮速、车速和IMU判断可移动对象是否处于静止状态。然后，计算静止状态下的可移动对象的第一惯性测量单元测量残差，可以包括位姿测量残差和速度测量残差。静止检测可以通过χ2检测完成，χ为预定义阈值。
例如，可以采用轮速和车速进行检测。通过将轮速测量残差和车速测量残差中的速度vG的状态置0，可以得到ZUPT(Zero Velocity Update，零速修正)残差，如公式(19)所示。
Figure BDA0003442552080000214
根据公式(19)，如果残差
Figure BDA0003442552080000221
满足
Figure BDA0003442552080000222
则可以确定可移动对象处于静止状态。
例如，还可以采用IMU进行检测。通过将可移动对象的加速度和角速度的状态置0，可以得到ZIHR(零速更新)残差，如公式(20)所示。
Figure BDA0003442552080000223
在公式(20)中，
Figure BDA0003442552080000224
可以表示IMU的测量残差。am可以表示IMU的加速度测量值。ba可以表示IMU的加速度计零偏。ωm可以表示IMU的角速度测量值。bg可以表示IMU的角速度计零偏。
Figure BDA0003442552080000225
可以表示IMU坐标系和世界坐标系之间的第二旋转量。g可以表示重力量。
根据公式(20)，如果残差
Figure BDA0003442552080000226
满足
Figure BDA0003442552080000227
则可以确定可移动对象处于静止状态。
在公式(19)和公式(20)中，H1、H2为系数矩阵。P1、P2为状态的协方差矩阵，χ1，χ2为阈值，α1、α2表示常数系数，R1可以表示
Figure BDA00034425520800002210
的协方差矩阵，R2可以表示
Figure BDA00034425520800002211
的协方差矩阵。
根据本公开的实施例，第一惯性测量单元测量残差可以包括表征静止状态下的可移动对象的惯性测量单元的位姿测量残差。根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差可以包括：确定静止状态下的可移动对象采集到的第四图像帧所对应的第一航向角，以及采集到的第五图像帧所对应的的第二航向角，第四图像帧与第四滑动窗口相对应，第五图像帧与第五滑动窗口相对应，第四滑动窗口与第五滑动窗口相邻。确定静止状态下的可移动对象在第四滑动窗口所定位的时刻的惯性测量单元坐标系与世界坐标系的第二平移量，以及在第五滑动窗口所定位的时刻的惯性测量单元坐标系与世界坐标系的第三平移量。根据第一航向角、第二航向角、第二平移量和第三平移量，确定位姿测量残差。
根据本公开的实施例，在确定可移动对象处于静止状态的情况下，可以构建位姿测量残差r1，如公式(21)所示。
Figure BDA0003442552080000228
在公式(21)中，r1可以表示位姿的测量残差。
Figure BDA0003442552080000229
可以表示与第i帧滑动窗口相对应的第四图像帧所对应的第一航向角。
Figure BDA0003442552080000231
可以表示与第i+1帧滑动窗口相对应的第五图像帧所对应的第二航向角。第i帧滑动窗口可以与第四滑动窗口相对应，第i+1帧滑动窗口可以与第五滑动窗口相对应。
Figure BDA0003442552080000232
可以表示第i帧滑动窗口所对应的IMU坐标系和世界坐标系的第二平移量。
Figure BDA0003442552080000233
可以表示第i+1帧滑动窗口所对应的IMU坐标系和世界坐标系的第三平移量。
通过本公开的上述实施例，可以计算得到第一惯性测量单元测量残差，基于该残差结合其他传感器信息的第一实时测量残差进行视觉惯性里程计定位时，能够得到横向精度高并且平滑的局部定位结果。
根据本公开的实施例，第一惯性测量单元测量残差还可以包括表征静止状态下的可移动对象的惯性测量单元的速度测量残差。根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差可以包括：确定静止状态下的可移动对象的惯性测量单元的加速度测量值、加速度计零偏、角速度测量值和角速度计零偏。根据加速度测量值、加速度计零偏、角速度测量值、角速度计零偏、惯性测量单元坐标系和世界坐标系之间的第二旋转量以及静止状态下的可移动对象的重力量，确定速度测量残差。
根据本公开的实施例，在确定可移动对象处于静止状态的情况下，可以构建速度测量残差r2，如公式(22)所示。
Figure BDA0003442552080000234
在公式(22)中，r2可以表示IMU的测量残差。
通过本公开的上述实施例，可以计算得到第一惯性测量单元测量残差，基于该残差结合其他传感器信息的第一实时测量残差进行视觉惯性里程计定位时，能够得到横向精度高并且平滑的局部定位结果。
根据本公开的实施例，运动学模型中可以包括旋转状态模型、平移状态模型、速度状态模型、加速度计零偏状态模型和角速度计零偏状态模型。根据惯性测量单元的运动学模型和多个第一实时测量残差，对可移动对象的状态向量进行更新可以包括：根据在前一时刻检测到的旋转状态模型、平移状态模型、速度状态模型、加速度计零偏状态模型和角速度计零偏状态模型，确定在前一时刻的条件下确定的当前时刻的惯性测量单元协方差模型。根据在前一时刻的条件下确定的当前时刻的状态向量、多个第一实时测量残差、惯性测量单元协方差以及在当前时刻确定的量测协方差，确定当前时刻的状态向量，其中，量测协方差包括与可移动对象所检测到的所有物理量相关的协方差。
根据本公开的实施例，IMU的运动学模型例如可以表示为公式(23)所示。
Figure BDA0003442552080000241
在公式(23)中，
Figure BDA0003442552080000242
Figure BDA0003442552080000243
可以表示旋转状态模型。
Figure BDA0003442552080000244
可以表示平移状态模型。
Figure BDA0003442552080000245
可以表示速度状态模型。
Figure BDA0003442552080000246
可以表示角速度计零偏状态模型。
Figure BDA0003442552080000247
可以表示加速度计零偏状态模型。ω(t)可以表示t时刻的角速度。
Figure BDA0003442552080000248
可以表示t时刻的速度。a(t)可以表示t时刻的加速度。
Figure BDA0003442552080000249
可以表示IMU坐标系和世界坐标系之间的旋转。am(t)可以表示t时刻IMU的加速度测量值。g可以表示重力。na(t)可以表示t时刻IMU加速度计的测量噪声。ng(t)可以表示t时刻IMU角速度计的测量噪声。ba(t)可以表示t时刻的加速度计零偏。bg(t)可以表示t时刻的角速度计零偏。
根据本公开的实施例，通过将公式(23)积分可以得到当前帧预测位姿，将公式(23)的微分方程离散化可以得到状态转移模型，可以表示为公式(24)所示。
Figure BDA00034425520800002410
在公式(24)中，
Figure BDA00034425520800002411
可以表示根据在t时刻的条件下确定的t+1时刻IMU的误差状态，Φ(tk+1，tk)可以表示第k时刻到第k+1时刻的状态转移矩阵，Gk可以表示噪声的系数矩阵，n1可以表示测量噪声。
根据本公开的实施例，惯性测量单元协方差模型例如可以表示为公式(25)所示。
Figure BDA0003442552080000251
在公式(25)中，Pk+1|k可以表示在k时刻的条件下得到的k+1时刻的协方差，Q可以表示测量噪声的协方差。
根据本公开的实施例，在计算得到所有第一实时测量残差后，可以采用误差状态卡尔曼滤波的方法对可移动对象系统的状态向量进行量测更新。例如，可以基于如下公式(26)实现该量测更新的过程。
Figure BDA0003442552080000252
在公式(26)中，xk+1|k+1可以表示更新后系统当前时刻的状态。Pk+1|k+1可以表示更新后系统当前时刻的状态协方差，该协方差可以结合公式(24)和公式(25)递推得到。Hk+1可以表示量测对误差状态的雅可比矩阵。rz可以表示所有第一实时测量残差。Rk+1可以表示当前时刻量测的协方差。量测可以包括可移动对象的所有状态的表示。
通过本公开的上述实施例，可以融合IMU、车道线特征和非车道线特征，甚至融合轮速计、车速计等特征实现视觉惯性里程计定位，能够得到横向精度高并且平滑的局部定位结果。
根据本公开的实施例，用于实时检测多个第一传感器信息的滑动窗口存储于用于存储预设数目帧滑动窗口的时序滑动窗口序列中。基于车道线和特征点的定位方法还可以包括：针对存储于时序滑动窗口序列中的每帧滑动窗口，将满足预设条件的滑动窗口确定为关键帧。在接收到包括更新后的状态向量的第十二滑动窗口的情况下，从时序滑动窗口序列中，确定与第十二滑动窗口所对应的时刻最近的第十三滑动窗口和与第十二滑动窗口所对应的时刻最远的第十四滑动窗口，第十二滑动窗口所对应的时刻为当前时刻。在第十三滑动窗口属于关键帧的情况下，将第十四滑动窗口从时序滑动窗口序列中删除，并将第十二滑动窗口存储于时序滑动窗口序列中。在第十三滑动窗口不属于关键帧的情况下，将第十三滑动窗口从时序滑动窗口序列中删除，并将第十二滑动窗口存储于时序滑动窗口序列中。
根据本公开的实施例，在根据新的一帧滑动窗口检测到多个第一传感器信息并根据该多个第一传感器信息完成对状态向量的实时更新后，例如可以相应地移除时序滑动窗口序列中的某一帧。在时序滑动窗口序列中的倒数第二帧不是关键帧的情况下，可以移除该倒数第二帧；否则可以移除时序滑动窗口序列中最旧的帧。
通过本公开的上述实施例，可以在维持时序滑动窗口序列中固定数量的状态的基础上，提高时序滑动窗口序列中的关键帧的占比，可以有利于节省计算资源，并可提高计算准确度。
根据本公开的实施例，将满足预设条件的滑动窗口确定为关键帧可以包括：确定第十二滑动窗口所对应的位姿量和第十三滑动窗口所对应的位姿量的之间的位姿偏移量。在位姿偏移量大于预设阈值的情况下，将第十二滑动窗口确定为关键帧。
根据本公开的实施例，在基于滤波的方法实时更新状态向量的过程中，滤波器的时序滑动窗口序列可以由fix window(固定窗口)和optimization window(优化窗口)两部分组成。最早的几帧滑动窗口所对应的信息可以存储于fix window中，位于fix window的滑动窗口所对应的位姿不再继续更新。剩余的关键帧可以存储于optimization window中，位于optimization window内的滑动窗口所对应的位姿可以不断更新。
根据本公开的实施例，在系统运行中有如下情况可以创建新的关键帧：在相邻帧间的旋转或者平移的变换，即位姿偏移量，大于预设阈值的情况下，创建新的关键帧。
根据本公开的实施例，在系统运行中还可以有如下情况可以创建新的关键帧：当相邻帧间的特征点视差大于一个阈值时，创建新的关键帧。当相邻帧间的匹配特征点对数量小于一个阈值时，创建新的关键帧。
通过本公开的上述实施例，可有效提高时序滑动窗口序列中的关键帧的占比，提高定位准确度。
图5示意性示出了根据本公开实施例的基于车道线和特征点的视觉惯性里程计的工作原理图。
如图5所示，基于车道线和特征点的视觉惯性里程计架构500可以包括输入模块510、MSCKF状态初始化模块520、MSCKF滤波器模块530和输出模块540。
根据本公开的实施例，输入模块510可以基于相同或不同的频率，实时接收多个传感器信息，如轮速信息511、IMU信息512、车速信息513、非车道线图像信息514和3D车道线信息515等，且可不限于此。
根据本公开的实施例，由于里程计模块的坐标系采用局部坐标系，所以在整个系统刚启动时，需要估计出系统的初始化状态，包括实现对IMU零偏(包括角速度计零偏、加速度计零偏)、速度、重力和轮速计尺度等状态向量进行初始化。初始化流程可以包括操作S551～S553。
在操作S551，MSCKF状态初始化模块520融合帧间轮速计积分521、帧间车道线信息522、非车道线图像特征点匹配信息523以及IMU预积分信息524等多个传感器的信息，进行动态或静态的初始化。
在操作S552，初始化是否完成？若是，则执行操作S553；若否，则执行操作S551。
在操作S553，进入MSCKF滤波器模530。在该部分中，还可以输出初始化状态，包括输出初始化IMU零偏、初始化速度、初始化重力和初始化轮速计尺度等。
根据本公开的实施例，在对系统的状态向量初始化完成后，MSCKF滤波器530里的IMU零偏、速度和轮速计尺度可以被赋予相应的初始值。包括初始化状态的MSCKF滤波器530可以实时接收轮速信息511、IMU信息512、车速信息513、非车道线图像信息514和SD车道线信息515等多个传感器信息，并进行处理，以输出估计的可移动对象的位姿。MSCKF滤波器530的处理流程可以包括操作S554～S557。
在操作S554，构建第二实时测量残差。在该部分中，里程计可以实时接收各个传感器的信息，通过对传感器信息进行处理，构建相应的残差。如可以包括对轮速计积分531和车道线信息532进行处理，确定车道线关联关系533，并构建车道线残差534；通过IMU递推及协方差更新确定IMU协方差535；通过提取和匹配非车道线图像特征点536，构建特征点重投影残差537；以及在车辆静止状态下，构建ZUPT、ZIHR残差538等。
在操作S555，MSCKF状态滤波。在该部分中，可以采用卡尔曼滤波的方法，利用车道线残差534、IMU协方差535、特征点重投影残差537以及ZUPT、ZIHR残差538，对状态进行量测更新，不断估计滑动窗口的最新时刻状态。
在操作S556，MSCKF状态边缘化。在该部分中，可以挑选出关键帧作为时序滑动窗口序列中的帧，进行边缘化，维护滑动窗口的大小。
在操作S557，特征点重跟踪。该部分中，在滤波器更新完成后，可以获得更精确的位姿。在当前帧中，对于没有成功匹配的特征点，可以用位姿将特征点重投影回旧的关键帧中，在图像平面重投影点一定半径范围内搜索特征点，当最近的描数子距离小于一个阈值时，认为特征点重新跟踪成功。
根据本公开的实施例，在滤波器完成状态更新后，输出模块540可以根据MSCKF滤波器模块530的更新结果，输出Odometry pose(里程位姿)541和Relative pose(相对位姿)542。Odometry pose可以表征可移动对象在当前时刻相对于起始时刻的位姿，起始时刻可以表示滤波器开启的时刻。Relative pose可以表征时序滑动窗口序列中各滑动窗口所对应的关键帧的IMU坐标系位姿相对于该时序滑动窗口序列中起始滑动窗口所对应的关键帧的IMU坐标系位姿。该两种位姿可用于后续的pose graph(曲线图)模块。便于统计分析。
通过本公开的上述实施例，基于车道线和特征点的视觉惯性里程计采用紧耦合的方法融合IMU、轮速计、车速计、非车道线图像特征点和3D车道线信息，并采用滤波器来实时估计车辆的状态，能够输出横向精度高且平滑的局部定位结果，采用滤波的方法还可实现更高的计算效率。
图6示意性示出了根据本公开实施例的基于车道线和特征点的定位装置的框图。
如图6所示，基于车道线和特征点的定位装置600可以包括第一确定模块610、更新模块620和第二确定模块630。
第一确定模块610，用于根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差，第一传感器信息包括第一惯性测量单元信息、第一车道线特征信息和第一非车道线特征点信息，第一实时测量残差包括第一惯性测量单元测量残差、第一车道线测量残差和第一非车道线测量残差。
更新模块620，用于根据惯性测量单元的运动学模型和多个第一实时测量残差，对可移动对象的状态向量进行更新。
第二确定模块630，用于根据更新后的状态向量中的位姿量，确定可移动对象在与更新后的状态向量相对应时刻的位姿。
根据本公开的实施例，基于车道线和特征点的定位装置还包括第三确定模块、第四确定模块、第五确定模块、第六确定模块和第七确定模块。
第三确定模块，用于根据检测到的可移动对象的多个第二传感器信息，确定多个第二实时测量残差。第二传感器信息包括第二惯性测量单元信息、第二车道线特征信息和第二非车道线特征点信息，第二实时测量残差包括第二惯性测量单元测量残差、第二车道线测量残差和第二非车道线测量残差。
第四确定模块，用于根据多个第二实时测量残差，确定可移动对象的初始化位姿。
第五确定模块，用于根据第二车道线特征信息和第二惯性测量单元信息，确定可移动对象的初始化角速度计零偏。
第六确定模块，用于根据初始化角速度计零偏、第二惯性测量单元信息和初始化位姿，确定初始化轮速计尺度和初始化加速度计零偏其中至少之一。
第七确定模块，用于根据初始化位姿、初始化角速度计零偏、初始化轮速计尺度和初始化加速度计零偏，确定初始化完成的状态向量。
根据本公开的实施例，第一车道线测量残差表征目标第一车道线特征点在第一滑动窗口所对应的直线和第二滑动窗口所对应的特征点位置形成的距离残差，第一滑动窗口所对应的时刻早于第二滑动窗口所对应的时刻。第一确定模块包括第一确定单元、第二确定单元和第三确定单元。
第一确定单元，用于确定第一图像帧所对应的第一位姿，以及第二图像帧所对应的第二位姿。第一图像帧与第一滑动窗口相对应，第二图像帧与第二滑动窗口相对应。
第二确定单元，用于确定目标第一车道线特征点在第二图像帧所对应的归一化平面中的第一位置信息。
第三确定单元，用于根据第一位姿、第二位姿、第一位置信息、以及预定义线性模型，确定第一车道线测量残差。
根据本公开的实施例，第一非车道线测量残差表征目标第一非车道线特征点在与第三滑动窗口相对应的第三图像帧中的重投影残差。第一确定模块包括第四确定单元、第五确定单元、第六确定单元和第七确定单元。
第四确定单元，用于根据目标第一非车道线特征点在世界坐标系中的世界坐标信息，确定目标第一非车道线特征点在第三图像帧所对应的归一化平面中的第二位置信息的第一测量值。
第五确定单元，用于确定世界坐标系和惯性测量单元坐标系之间的第一旋转量和第一平移量。
第六确定单元，用于根据世界坐标信息、第一旋转量、第一平移量以及相机的外参和惯性测量单元的外参，确定目标第一非车道线特征点在第三图像帧所对应的归一化平面中的第二位置信息的第一计算值。
第七确定单元，用于根据第一测量值和第一计算值，确定第一非车道线测量残差。
根据本公开的实施例，第一惯性测量单元测量残差包括表征静止状态下的可移动对象的惯性测量单元的位姿测量残差。第一确定模块包括第八确定单元、第九确定单元和第十确定单元。
第八确定单元，用于确定静止状态下的可移动对象采集到的第四图像帧所对应的第一航向角，以及采集到的第五图像帧所对应的的第二航向角。第四图像帧与第四滑动窗口相对应，第五图像帧与第五滑动窗口相对应，第四滑动窗口与第五滑动窗口相邻。
第九确定单元，用于确定静止状态下的可移动对象在第四滑动窗口所定位的时刻的惯性测量单元坐标系与世界坐标系的第二平移量，以及在第五滑动窗口所定位的时刻的惯性测量单元坐标系与世界坐标系的第三平移量。
第十确定单元，用于根据第一航向角、第二航向角、第二平移量和第三平移量，确定位姿测量残差。
根据本公开的实施例，第一惯性测量单元测量残差包括表征静止状态下的可移动对象的惯性测量单元的速度测量残差。第一确定模块包括第十一确定单元和第十二确定单元。
第十一确定单元，用于确定静止状态下的可移动对象的惯性测量单元的加速度测量值、加速度计零偏、角速度测量值和角速度计零偏。
第十二确定单元，用于根据加速度测量值、加速度计零偏、角速度测量值、角速度计零偏、惯性测量单元坐标系和世界坐标系之间的第二旋转量以及静止状态下的可移动对象的重力量，确定速度测量残差。
根据本公开的实施例，第一实时传感器信息还包括第一轮速计信息，第一实时测量残差还包括第一轮速计尺度测量残差，第一轮速计尺度测量残差表征可移动对象的每个轮子的尺度测量残差。第一确定模块包括第十三确定单元。
第十三确定单元，用于针对可移动对象的每个轮子，根据世界坐标系和惯性测量单元坐标系之间的位姿，世界坐标系下惯性测量单元的速度、轮子的坐标系和惯性测量单元坐标系之间的位移、惯性测量单元检测到的轮子的角速度以及轮子的尺度、半径和速度，确定轮子的尺度测量残差。
根据本公开的实施例，第一实时传感器信息还包括第一车速计信息，第一实时测量残差还包括第一车速测量残差，第一车速测量残差表征可移动对象的移动速度的测量残差。第一确定模块包括第十四确定单元。
第十四确定单元，用于根据可移动对象在相邻时刻内的平均速度和世界坐标系下惯性测量单元的速度，确定第一车速测量残差。
根据本公开的实施例，运动学模型中包括旋转状态模型、平移状态模型、速度状态模型、加速度计零偏状态模型和角速度计零偏状态模型。更新模块包括第十五确定单元和第十六确定单元。
第十五确定单元，用于根据在前一时刻检测到的旋转状态模型、平移状态模型、速度状态模型、加速度计零偏状态模型和角速度计零偏状态模型，确定在前一时刻的条件下确定的当前时刻的惯性测量单元协方差模型。
第十六确定单元，用于根据在前一时刻的条件下确定的当前时刻的状态向量、多个第一实时测量残差、惯性测量单元协方差以及在当前时刻确定的量测协方差，确定当前时刻的状态向量。量测协方差包括与可移动对象所检测到的所有物理量相关的协方差。
根据本公开的实施例，第二车道线测量残差表征目标第二车道线特征点在第六滑动窗口所对应的直线和第七滑动窗口所对应的特征点位置形成的距离残差，第六滑动窗口所对应的时刻早于第七滑动窗口所对应的时刻。第三确定模块包括第十七确定单元、第十八确定单元、第十九确定单元、第二十确定单元和第二十一确定单元。
第十七确定单元，用于确定第六滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第四平移量和第三旋转量，以及第七滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第五平移量和第四旋转量。
第十八确定单元，用于确定目标第二车道线特征点在第六图像帧所对应的归一化平面中的第三位置信息。第六图像帧与第六滑动窗口相对应。
第十九确定单元，用于确定目标第二车道线特征点在第七图像帧所对应的归一化平面中的第四位置信息。第七图像帧与第七滑动窗口相对应。
第二十确定单元，用于确定第六图像帧中与目标第二车道线特征点相对应的直线的方向向量。
第二十一确定单元，用于根据第三位置信息、第四平移量、第三旋转量、第五平移量、第四旋转量、第四位置信息、方向向量以及相机的外参和惯性测量单元的外参，确定第一车道线测量残差。
根据本公开的实施例，第二非车道线测量残差表征目标第二非车道线特征点在与第八滑动窗口相对应的第八图像帧中的重投影残差。第三确定模块包括第二十二确定单元、第二十三确定单元、第二十四确定单元和第二十五确定单元。
第二十二确定单元，用于根据目标第二非车道线特征点在第八滑动窗口所对应的相机坐标系下的第一像素横坐标和第一像素列坐标，确定目标第二非车道线特征点在第八滑动窗口所对应的相机坐标系下的第五位置信息的第二测量值。
第二十三确定单元，用于确定世界坐标系和第八滑动窗口所对应的惯性测量单元坐标系之间的第五旋转量和第六平移量，以及世界坐标系和第九滑动窗口所对应的惯性测量单元坐标系之间的第六旋转量和第七平移量。第九滑动窗口所对应的时刻早于第八滑动窗口所对应的时刻。
第二十四确定单元，用于根据目标第二非车道线特征点在第九滑动窗口所对应的相机坐标系下的第二像素横坐标和第二像素列坐标，以及第五旋转量、第六平移量、第六旋转量、第七平移量、相机的外参和惯性测量单元的外参，确定目标第二非车道线特征点在第八滑动窗口所对应的相机坐标系下的第五位置信息的第二计算值。
第二十五确定单元，用于根据第二测量值和第二计算值，确定第二非车道线测量残差。
根据本公开的实施例，第二传感器信息还包括第二轮速计信息，第二实时测量残差还包括第二轮速计尺度测量残差，第二轮速计尺度测量残差表征相邻两帧滑动窗口所定位的两个时刻之间的轮速计积分残差。第三确定模块包括第二十六确定单元和第二十七确定单元。
第二十六确定单元，用于针对两个时刻中的每个时刻，确定惯性测量单元坐标系与世界坐标系之间的第七旋转量和第八平移量。
第二十七确定单元，用于根据惯性测量单元坐标系在两个时刻形成的第一旋转偏移量的第三测量值、轮速计积分在两个时刻形成的平移偏移量的第四测量值、惯性测量单元的角速度计零偏以及对应于两个时刻中的每个时刻的第七旋转量和第八平移量，确定轮速计积分残差。
根据本公开的实施例，第五确定模块包括第二十八确定单元和第二十九确定单元。
第二十八确定单元，用于根据惯性测量单元坐标系在相邻两帧滑动窗口所定位的两个时刻形成的第二旋转偏移量的第五测量值和角速度计零偏，确定惯性测量单元坐标系在相邻两帧滑动窗口所定位的两个时刻形成的第二旋转偏移量的第三计算值。
第二十九确定单元，用于根据第三计算值和第五测量值，确定初始化角速度计零偏。
根据本公开的实施例，第六确定模块包括第三十确定单元、第三十一确定单元和第三十二确定单元。
第三十确定单元，用于在位姿量和角速度计零偏均初始化完成的情况下，根据第十滑动窗口所对应的第一目标速度量、第十一滑动窗口所对应的第二目标速度量、第十滑动窗口所对应的时刻和第十一滑动窗口所对应的时刻形成的时间差、重力量、轮速计尺度以及第十滑动窗口和第十一滑动窗口分别对应的位姿量和旋转量，确定惯性测量单元预积分模型。
第三十一确定单元，用于确定与惯性测量单元预积分模型相对应的最小二乘优化模型。最小二乘优化模型中包括待估计速度量、待估计重力量和待估计轮速计尺度其中至少之一。
第三十二确定单元，用于根据预设模长的重力量、惯性测量单元预积分模型和最小二乘优化模型，确定初始化轮速计尺度和初始化加速度计零偏其中至少之一。
根据本公开的实施例，用于实时检测多个第一传感器信息的滑动窗口存储于用于存储预设数目帧滑动窗口的时序滑动窗口序列中。基于车道线和特征点的定位装置还包括第八确定模块、第九确定模块、第一处理模块和第二处理模块。
第八确定模块，用于针对存储于时序滑动窗口序列中的每帧滑动窗口，将满足预设条件的滑动窗口确定为关键帧。
第九确定模块，用于在接收到包括更新后的状态向量的第十二滑动窗口的情况下，从时序滑动窗口序列中，确定与第十二滑动窗口所对应的时刻最近的第十三滑动窗口和与第十二滑动窗口所对应的时刻最远的第十四滑动窗口。第十二滑动窗口所对应的时刻为当前时刻。
第一处理模块，用于在第十三滑动窗口属于关键帧的情况下，将第十四滑动窗口从时序滑动窗口序列中删除，并将第十二滑动窗口存储于时序滑动窗口序列中。
第二处理模块，用于在第十三滑动窗口不属于关键帧的情况下，将第十三滑动窗口从时序滑动窗口序列中删除，并将第十二滑动窗口存储于时序滑动窗口序列中。
根据本公开的实施例，第八确定模块包括第三十三确定单元和第三十四确定单元。
第三十三确定单元，用于确定第十二滑动窗口所对应的位姿量和第十三滑动窗口所对应的位姿量的之间的位姿偏移量。
第三十四确定单元，用于在位姿偏移量大于预设阈值的情况下，将第十二滑动窗口确定为关键帧。
根据本公开的实施例，本公开还提供了一种电子设备、一种可读存储介质、一种计算机程序产品和一种自动驾驶车辆。
根据本公开的实施例，一种电子设备，包括：至少一个处理器；以及与至少一个处理器通信连接的存储器；其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行本公开所述的基于车道线和特征点的定位方法。
根据本公开的实施例，一种存储有计算机指令的非瞬时计算机可读存储介质，其中，计算机指令用于使计算机执行本公开所述的基于车道线和特征点的定位方法。
根据本公开的实施例，一种计算机程序产品，包括计算机程序，计算机程序在被处理器执行时实现本公开所述的基于车道线和特征点的定位方法。
图7示出了可以用来实施本公开的实施例的示例电子设备700的示意性框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本公开的实现。
如图7所示，设备700包括计算单元701，其可以根据存储在只读存储器(ROM)702中的计算机程序或者从存储单元708加载到随机访问存储器(RAM)703中的计算机程序，来执行各种适当的动作和处理。在RAM 703中，还可存储设备700操作所需的各种程序和数据。计算单元701、ROM 702以及RAM 703通过总线704彼此相连。输入/输出(I/O)接口705也连接至总线704。
设备700中的多个部件连接至I/O接口705，包括：输入单元706，例如键盘、鼠标等；输出单元707，例如各种类型的显示器、扬声器等；存储单元708，例如磁盘、光盘等；以及通信单元709，例如网卡、调制解调器、无线通信收发机等。通信单元709允许设备700通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
计算单元701可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元701的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。计算单元701执行上文所描述的各个方法和处理，例如基于车道线和特征点的定位方法。例如，在一些实施例中，基于车道线和特征点的定位方法可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元708。在一些实施例中，计算机程序的部分或者全部可以经由ROM 702和/或通信单元709而被载入和/或安装到设备700上。当计算机程序加载到RAM 703并由计算单元701执行时，可以执行上文描述的基于车道线和特征点的定位方法的一个或多个步骤。备选地，在其他实施例中，计算单元701可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行基于车道线和特征点的定位方法。
基于前述的电子设备，电子设备可以对多个第一传感器信息进行分析，并实现确定可移动对象的位姿。
基于前述的电子设备，本公开还提供一种自动驾驶车辆，可以包括电子设备，还可以包括通信部件、用于实现人机界面的显示屏以及用于采集周围环境信息的信息采集设备等。通信部件、显示屏、信息采集设备与电子设备之间可以通信连接。
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、复杂可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本公开的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)和互联网。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，也可以是分布式系统的服务器，或者是结合了区块链的服务器。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本发公开中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本公开公开的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本公开保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本公开的精神和原则之内所作的修改、等同替换和改进等，均应包含在本公开保护范围之内。

Claims (21)

1.一种基于车道线和特征点的定位方法，包括：
根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差，所述第一传感器信息包括第一惯性测量单元信息、第一车道线特征信息和第一非车道线特征点信息，所述第一实时测量残差包括第一惯性测量单元测量残差、第一车道线测量残差和第一非车道线测量残差；
根据惯性测量单元的运动学模型和所述多个第一实时测量残差，对所述可移动对象的状态向量进行更新；以及
根据更新后的状态向量中的位姿量，确定所述可移动对象在与所述更新后的状态向量相对应时刻的位姿。
2.根据权利要求1所述的方法，还包括：
根据检测到的所述可移动对象的多个第二传感器信息，确定多个第二实时测量残差，其中，所述第二传感器信息包括第二惯性测量单元信息、第二车道线特征信息和第二非车道线特征点信息，所述第二实时测量残差包括第二惯性测量单元测量残差、第二车道线测量残差和第二非车道线测量残差；
根据所述多个第二实时测量残差，确定所述可移动对象的初始化位姿；
根据所述第二车道线特征信息和所述第二惯性测量单元信息，确定所述可移动对象的初始化角速度计零偏；
根据所述初始化角速度计零偏、所述第二惯性测量单元信息和所述初始化位姿，确定初始化轮速计尺度和初始化加速度计零偏其中至少之一；以及
根据所述初始化位姿、所述初始化角速度计零偏、所述初始化轮速计尺度和所述初始化加速度计零偏，确定初始化完成的状态向量。
3.根据权利要求1或2所述的方法，其中，所述第一车道线测量残差表征目标第一车道线特征点在第一滑动窗口所对应的直线和第二滑动窗口所对应的特征点位置形成的距离残差，所述第一滑动窗口所对应的时刻早于所述第二滑动窗口所对应的时刻；
所述根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差包括：
确定第一图像帧所对应的第一位姿，以及第二图像帧所对应的第二位姿，其中，所述第一图像帧与所述第一滑动窗口相对应，所述第二图像帧与所述第二滑动窗口相对应；
确定所述目标第一车道线特征点在所述第二图像帧所对应的归一化平面中的第一位置信息；以及
根据所述第一位姿、所述第二位姿、所述第一位置信息、以及预定义线性模型，确定所述第一车道线测量残差。
4.根据权利要求1-3中任一所述的方法，其中，所述第一非车道线测量残差表征目标第一非车道线特征点在与第三滑动窗口相对应的第三图像帧中的重投影残差；
所述根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差包括：
根据所述目标第一非车道线特征点在世界坐标系中的世界坐标信息，确定所述目标第一非车道线特征点在所述第三图像帧所对应的归一化平面中的第二位置信息的第一测量值；
确定世界坐标系和惯性测量单元坐标系之间的第一旋转量和第一平移量；
根据所述世界坐标信息、所述第一旋转量、所述第一平移量以及相机的外参和惯性测量单元的外参，确定所述目标第一非车道线特征点在所述第三图像帧所对应的归一化平面中的第二位置信息的第一计算值；以及
根据所述第一测量值和所述第一计算值，确定所述第一非车道线测量残差。
5.根据权利要求1-4中任一所述的方法，其中，所述第一惯性测量单元测量残差包括表征静止状态下的所述可移动对象的惯性测量单元的位姿测量残差；
所述根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差包括：
确定静止状态下的所述可移动对象采集到的第四图像帧所对应的第一航向角，以及采集到的第五图像帧所对应的的第二航向角，其中，所述第四图像帧与第四滑动窗口相对应，所述第五图像帧与第五滑动窗口相对应，所述第四滑动窗口与所述第五滑动窗口相邻；
确定所述静止状态下的所述可移动对象在所述第四滑动窗口所定位的时刻的惯性测量单元坐标系与世界坐标系的第二平移量，以及在所述第五滑动窗口所定位的时刻的惯性测量单元坐标系与世界坐标系的第三平移量；以及
根据所述第一航向角、所述第二航向角、所述第二平移量和所述第三平移量，确定所述位姿测量残差。
6.根据权利要求1-5中任一所述的方法，其中，所述第一惯性测量单元测量残差包括表征静止状态下的所述可移动对象的惯性测量单元的速度测量残差；
所述根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差包括：
确定静止状态下的所述可移动对象的惯性测量单元的加速度测量值、加速度计零偏、角速度测量值和角速度计零偏；以及
根据所述加速度测量值、所述加速度计零偏、所述角速度测量值、所述角速度计零偏、惯性测量单元坐标系和世界坐标系之间的第二旋转量以及静止状态下的所述可移动对象的重力量，确定所述速度测量残差。
7.根据权利要求1-6中任一所述的方法，其中，所述第一实时传感器信息还包括第一轮速计信息，所述第一实时测量残差还包括第一轮速计尺度测量残差，所述第一轮速计尺度测量残差表征所述可移动对象的每个轮子的尺度测量残差；
所述根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差包括：
针对所述可移动对象的每个轮子，根据世界坐标系和惯性测量单元坐标系之间的位姿，世界坐标系下惯性测量单元的速度、所述轮子的坐标系和惯性测量单元坐标系之间的位移、所述惯性测量单元检测到的所述轮子的角速度以及所述轮子的尺度、半径和速度，确定所述轮子的尺度测量残差。
8.根据权利要求1-7中任一所述的方法，其中，所述第一实时传感器信息还包括第一车速计信息，所述第一实时测量残差还包括第一车速测量残差，所述第一车速测量残差表征所述可移动对象的移动速度的测量残差；
所述根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差包括：
根据所述可移动对象在相邻时刻内的平均速度和世界坐标系下惯性测量单元的速度，确定所述第一车速测量残差。
9.根据权利要求1-8中任一所述的方法，其中，所述运动学模型中包括旋转状态模型、平移状态模型、速度状态模型、加速度计零偏状态模型和角速度计零偏状态模型；
所述根据惯性测量单元的运动学模型和所述多个第一实时测量残差，对所述可移动对象的状态向量进行更新包括：
根据在前一时刻检测到的所述旋转状态模型、平移状态模型、速度状态模型、加速度计零偏状态模型和角速度计零偏状态模型，确定在所述前一时刻的条件下确定的当前时刻的惯性测量单元协方差模型；以及
根据在所述前一时刻的条件下确定的当前时刻的所述状态向量、所述多个第一实时测量残差、所述惯性测量单元协方差以及在所述当前时刻确定的量测协方差，确定所述当前时刻的状态向量，其中，所述量测协方差包括与所述可移动对象所检测到的所有物理量相关的协方差。
10.根据权利要求2所述的方法，其中，所述第二车道线测量残差表征目标第二车道线特征点在第六滑动窗口所对应的直线和第七滑动窗口所对应的特征点位置形成的距离残差，所述第六滑动窗口所对应的时刻早于所述第七滑动窗口所对应的时刻；
所述根据检测到的所述可移动对象的多个第二传感器信息，确定多个第二实时测量残差包括：
确定所述第六滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第四平移量和第三旋转量，以及所述第七滑动窗口所对应的惯性测量单元坐标系和世界坐标系之间的第五平移量和第四旋转量；
确定所述目标第二车道线特征点在第六图像帧所对应的归一化平面中的第三位置信息，其中，所述第六图像帧与所述第六滑动窗口相对应；
确定所述目标第二车道线特征点在第七图像帧所对应的归一化平面中的第四位置信息，其中，所述第七图像帧与所述第七滑动窗口相对应；
确定所述第六图像帧中与所述目标第二车道线特征点相对应的直线的方向向量；以及
根据所述第三位置信息、所述第四平移量、所述第三旋转量、所述第五平移量、所述第四旋转量、所述第四位置信息、所述方向向量以及相机的外参和惯性测量单元的外参，确定所述第一车道线测量残差。
11.根据权利要求2或10所述的方法，其中，所述第二非车道线测量残差表征目标第二非车道线特征点在与第八滑动窗口相对应的第八图像帧中的重投影残差；
所述根据检测到的所述可移动对象的多个第二传感器信息，确定多个第二实时测量残差包括：
根据所述目标第二非车道线特征点在所述第八滑动窗口所对应的相机坐标系下的第一像素横坐标和第一像素列坐标，确定所述目标第二非车道线特征点在所述第八滑动窗口所对应的相机坐标系下的第五位置信息的第二测量值；
确定世界坐标系和所述第八滑动窗口所对应的惯性测量单元坐标系之间的第五旋转量和第六平移量，以及世界坐标系和第九滑动窗口所对应的惯性测量单元坐标系之间的第六旋转量和第七平移量，其中，所述第九滑动窗口所对应的时刻早于所述第八滑动窗口所对应的时刻；
根据所述目标第二非车道线特征点在所述第九滑动窗口所对应的相机坐标系下的第二像素横坐标和第二像素列坐标，以及所述第五旋转量、所述第六平移量、所述第六旋转量、所述第七平移量、相机的外参和惯性测量单元的外参，确定所述目标第二非车道线特征点在所述第八滑动窗口所对应的相机坐标系下的第五位置信息的第二计算值；以及
根据所述第二测量值和所述第二计算值，确定所述第二非车道线测量残差。
12.根据权利要求2或10或11所述的方法，其中，所述第二传感器信息还包括第二轮速计信息，所述第二实时测量残差还包括第二轮速计尺度测量残差，所述第二轮速计尺度测量残差表征相邻两帧滑动窗口所定位的两个时刻之间的轮速计积分残差；
所述根据检测到的所述可移动对象的多个第二传感器信息，确定多个第二实时测量残差包括：
针对所述两个时刻中的每个时刻，确定惯性测量单元坐标系与世界坐标系之间的第七旋转量和第八平移量；以及
根据所述惯性测量单元坐标系在所述两个时刻形成的第一旋转偏移量的第三测量值、轮速计积分在所述两个时刻形成的平移偏移量的第四测量值、惯性测量单元的角速度计零偏以及对应于所述两个时刻中的每个时刻的第七旋转量和第八平移量，确定所述轮速计积分残差。
13.根据权利要求2或10-12中任一所述的方法，其中，所述根据所述第二车道线特征信息和所述第二惯性测量单元信息，确定所述可移动对象的初始化角速度计零偏包括：
根据所述惯性测量单元坐标系在相邻两帧滑动窗口所定位的两个时刻形成的第二旋转偏移量的第五测量值和角速度计零偏，确定所述惯性测量单元坐标系在相邻两帧滑动窗口所定位的两个时刻形成的第二旋转偏移量的第三计算值；以及
根据所述第三计算值和所述第五测量值，确定所述初始化角速度计零偏。
14.根据权利要求2或10-13中任一所述的方法，其中，所述根据所述初始化角速度计零偏、所述第二惯性测量单元信息和所述初始化位姿，确定初始化轮速计尺度和初始化加速度计零偏其中至少之一包括：
在所述位姿量和所述角速度计零偏均初始化完成的情况下，根据第十滑动窗口所对应的第一目标速度量、第十一滑动窗口所对应的第二目标速度量、所述第十滑动窗口所对应的时刻和所述第十一滑动窗口所对应的时刻形成的时间差、重力量、轮速计尺度以及所述第十滑动窗口和所述第十一滑动窗口分别对应的位姿量和旋转量，确定惯性测量单元预积分模型；
确定与所述惯性测量单元预积分模型相对应的最小二乘优化模型，其中，所述最小二乘优化模型中包括待估计速度量、待估计重力量和待估计轮速计尺度其中至少之一；
根据预设模长的重力量、所述惯性测量单元预积分模型和所述最小二乘优化模型，确定所述初始化轮速计尺度和所述初始化加速度计零偏其中至少之一。
15.根据权利要求1至14中任一所述的方法，其中，用于实时检测所述多个第一传感器信息的滑动窗口存储于用于存储预设数目帧滑动窗口的时序滑动窗口序列中；
所述方法还包括：
针对存储于所述时序滑动窗口序列中的每帧滑动窗口，将满足预设条件的滑动窗口确定为关键帧；
在接收到包括更新后的状态向量的第十二滑动窗口的情况下，从所述时序滑动窗口序列中，确定与所述第十二滑动窗口所对应的时刻最近的第十三滑动窗口和与所述第十二滑动窗口所对应的时刻最远的第十四滑动窗口，其中，所述第十二滑动窗口所对应的时刻为当前时刻；
在所述第十三滑动窗口属于关键帧的情况下，将所述第十四滑动窗口从所述时序滑动窗口序列中删除，并将所述第十二滑动窗口存储于所述时序滑动窗口序列中；以及
在所述第十三滑动窗口不属于关键帧的情况下，将所述第十三滑动窗口从所述时序滑动窗口序列中删除，并将所述第十二滑动窗口存储于所述时序滑动窗口序列中。
16.根据权利要求15所述的方法，其中，所述将满足预设条件的滑动窗口确定为关键帧包括：
确定所述第十二滑动窗口所对应的位姿量和第十三滑动窗口所对应的位姿量的之间的位姿偏移量；以及
在所述位姿偏移量大于预设阈值的情况下，将所述第十二滑动窗口确定为所述关键帧。
17.一种基于车道线和特征点的定位装置，包括：
第一确定模块，用于根据检测到的可移动对象的多个第一传感器信息，确定多个第一实时测量残差，所述第一传感器信息包括第一惯性测量单元信息、第一车道线特征信息和第一非车道线特征点信息，所述第一实时测量残差包括第一惯性测量单元测量残差、第一车道线测量残差和第一非车道线测量残差；
更新模块，用于根据惯性测量单元的运动学模型和所述多个第一实时测量残差，对所述可移动对象的状态向量进行更新；以及
第二确定模块，用于根据更新后的状态向量中的位姿量，确定所述可移动对象在与所述更新后的状态向量相对应时刻的位姿。
18.一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-16中任一项所述的方法。
19.一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据权利要求1-16中任一项所述的方法。
20.一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现根据权利要求1-16中任一项所述的方法。
21.一种自动驾驶车辆，包括如权利要求18所述的电子设备。
CN202111637724.3A 2021-12-29 2021-12-29 基于车道线和特征点的定位方法、设备及自动驾驶车辆 Active CN114323033B (zh)

Priority Applications (5)

Application Number Priority Date Filing Date Title
CN202111637724.3A CN114323033B (zh) 2021-12-29 2021-12-29 基于车道线和特征点的定位方法、设备及自动驾驶车辆
JP2022205078A JP2023036796A (ja) 2021-12-29 2022-12-22 レーンラインと特徴点による測位方法及び測位装置、電子機器、記憶媒体、コンピュータプログラム、ならびに自動運転車両
EP22216555.7A EP4155679A3 (en) 2021-12-29 2022-12-23 Positioning method and apparatus based on lane line and feature point
US18/088,954 US20230134569A1 (en) 2021-12-29 2022-12-27 Positioning method based on lane line and feature point, electronic device, and storage medium
KR1020220185242A KR20230008000A (ko) 2021-12-29 2022-12-27 차선 및 특징 포인트에 기초한 위치확정방법 및 장치, 전자장비, 저장매체, 컴퓨터 프로그램 및 자율주행차량

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111637724.3A CN114323033B (zh) 2021-12-29 2021-12-29 基于车道线和特征点的定位方法、设备及自动驾驶车辆

Publications (2)

Publication Number Publication Date
CN114323033A true CN114323033A (zh) 2022-04-12
CN114323033B CN114323033B (zh) 2023-08-29

Family

ID=81016784

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111637724.3A Active CN114323033B (zh) 2021-12-29 2021-12-29 基于车道线和特征点的定位方法、设备及自动驾驶车辆

Country Status (5)

Country Link
US (1) US20230134569A1 (zh)
EP (1) EP4155679A3 (zh)
JP (1) JP2023036796A (zh)
KR (1) KR20230008000A (zh)
CN (1) CN114323033B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115900697A (zh) * 2022-10-28 2023-04-04 北京百度网讯科技有限公司 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112560684B (zh) * 2020-12-16 2023-10-24 阿波罗智联(北京)科技有限公司 车道线检测方法、装置、电子设备、存储介质以及车辆
CN113899363B (zh) * 2021-09-29 2022-10-21 北京百度网讯科技有限公司 车辆的定位方法、装置及自动驾驶车辆
CN118015585B (zh) * 2024-04-08 2024-06-14 奥特酷智能科技(南京)有限公司 用于智轨列车的横向定位系统

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050060069A1 (en) * 1997-10-22 2005-03-17 Breed David S. Method and system for controlling a vehicle
JP2011106929A (ja) * 2009-11-16 2011-06-02 Alpine Electronics Inc ナビゲーション装置及びレーン変更案内方法
CN105783936A (zh) * 2016-03-08 2016-07-20 武汉光庭信息技术股份有限公司 用于自动驾驶中的道路标识制图及车辆定位方法及系统
CN109724615A (zh) * 2019-02-28 2019-05-07 北京经纬恒润科技有限公司 一种车道线识别结果的校验方法及系统
US20190293429A1 (en) * 2018-03-23 2019-09-26 The Boeing Company System and method for dual speed resolver
CN110567480A (zh) * 2019-09-12 2019-12-13 北京百度网讯科技有限公司 车辆定位的优化方法、装置、设备及存储介质
CN110631593A (zh) * 2019-11-25 2019-12-31 奥特酷智能科技(南京)有限公司 一种用于自动驾驶场景的多传感器融合定位方法
CN110954112A (zh) * 2019-03-29 2020-04-03 北京初速度科技有限公司 一种导航地图与感知图像匹配关系的更新方法和装置
CN110954113A (zh) * 2019-05-30 2020-04-03 北京初速度科技有限公司 一种车辆位姿的修正方法和装置
CN111649739A (zh) * 2020-06-02 2020-09-11 北京百度网讯科技有限公司 定位方法和装置、自动驾驶车辆、电子设备和存储介质
CN112304302A (zh) * 2019-07-26 2021-02-02 北京初速度科技有限公司 一种多场景高精度车辆定位方法、装置及车载终端
WO2021070813A1 (ja) * 2019-10-08 2021-04-15 株式会社デンソー 誤差推定装置、誤差推定方法、誤差推定プログラム
CN112683281A (zh) * 2021-03-11 2021-04-20 之江实验室 一种基于车辆运动学的自动驾驶车辆联合定位方法
CN112837352A (zh) * 2021-04-20 2021-05-25 腾讯科技（深圳）有限公司 基于图像的数据处理方法、装置及设备、汽车、存储介质

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050060069A1 (en) * 1997-10-22 2005-03-17 Breed David S. Method and system for controlling a vehicle
JP2011106929A (ja) * 2009-11-16 2011-06-02 Alpine Electronics Inc ナビゲーション装置及びレーン変更案内方法
CN105783936A (zh) * 2016-03-08 2016-07-20 武汉光庭信息技术股份有限公司 用于自动驾驶中的道路标识制图及车辆定位方法及系统
US20190293429A1 (en) * 2018-03-23 2019-09-26 The Boeing Company System and method for dual speed resolver
CN109724615A (zh) * 2019-02-28 2019-05-07 北京经纬恒润科技有限公司 一种车道线识别结果的校验方法及系统
CN110954112A (zh) * 2019-03-29 2020-04-03 北京初速度科技有限公司 一种导航地图与感知图像匹配关系的更新方法和装置
CN110954113A (zh) * 2019-05-30 2020-04-03 北京初速度科技有限公司 一种车辆位姿的修正方法和装置
CN112304302A (zh) * 2019-07-26 2021-02-02 北京初速度科技有限公司 一种多场景高精度车辆定位方法、装置及车载终端
CN110567480A (zh) * 2019-09-12 2019-12-13 北京百度网讯科技有限公司 车辆定位的优化方法、装置、设备及存储介质
WO2021070813A1 (ja) * 2019-10-08 2021-04-15 株式会社デンソー 誤差推定装置、誤差推定方法、誤差推定プログラム
CN110631593A (zh) * 2019-11-25 2019-12-31 奥特酷智能科技(南京)有限公司 一种用于自动驾驶场景的多传感器融合定位方法
CN111649739A (zh) * 2020-06-02 2020-09-11 北京百度网讯科技有限公司 定位方法和装置、自动驾驶车辆、电子设备和存储介质
CN112683281A (zh) * 2021-03-11 2021-04-20 之江实验室 一种基于车辆运动学的自动驾驶车辆联合定位方法
CN112837352A (zh) * 2021-04-20 2021-05-25 腾讯科技（深圳）有限公司 基于图像的数据处理方法、装置及设备、汽车、存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115900697A (zh) * 2022-10-28 2023-04-04 北京百度网讯科技有限公司 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN115900697B (zh) * 2022-10-28 2024-03-29 北京百度网讯科技有限公司 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆

Also Published As

Publication number Publication date
KR20230008000A (ko) 2023-01-13
CN114323033B (zh) 2023-08-29
EP4155679A3 (en) 2023-10-04
JP2023036796A (ja) 2023-03-14
US20230134569A1 (en) 2023-05-04
EP4155679A2 (en) 2023-03-29

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
EP3940421A1 (en) Positioning method and device based on multi-sensor fusion
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN112880674A (zh) 一种行驶设备的定位方法、装置、设备及存储介质
CN114119886A (zh) 高精地图点云重建方法、装置、车辆、设备和存储介质
Pan et al. Tightly-coupled multi-sensor fusion for localization with LiDAR feature maps
CN113052855B (zh) 一种基于视觉-imu-轮速计融合的语义slam方法
CN117685953A (zh) 面向多无人机协同定位的uwb与视觉融合定位方法及系统
CN115900697B (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN117075158A (zh) 基于激光雷达的无人变形运动平台的位姿估计方法及系统
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
CN115792985A (zh) 一种车辆定位方法、装置、电子设备、存储介质及车辆
CN113379915B (zh) 一种基于点云融合的行车场景构建方法
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN113946151A (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN112798020B (zh) 一种用于评估智能汽车定位精度的系统及方法
CN113554705A (zh) 一种变化场景下的激光雷达鲁棒定位方法
Li et al. Exploring the Potential of Deep Learning Aided Kalman Filter for GNSS/INS Integration: A Study on 2D Simulation Datasets
WO2022179047A1 (zh) 一种状态信息估计方法及装置
US20230025579A1 (en) High-definition mapping
CN117687006A (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
GR01 Patent grant
GR01 Patent grant