CN112347205B - 一种车辆误差状态的更新方法和装置 - Google Patents
一种车辆误差状态的更新方法和装置 Download PDFInfo
- Publication number
- CN112347205B CN112347205B CN201910723045.4A CN201910723045A CN112347205B CN 112347205 B CN112347205 B CN 112347205B CN 201910723045 A CN201910723045 A CN 201910723045A CN 112347205 B CN112347205 B CN 112347205B
- Authority
- CN
- China
- Prior art keywords
- matrix
- coordinate system
- target
- projection
- image
- 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
- 238000000034 method Methods 0.000 title claims abstract description 91
- 239000011159 matrix material Substances 0.000 claims abstract description 276
- 238000006243 chemical reaction Methods 0.000 claims abstract description 80
- 238000004422 calculation algorithm Methods 0.000 claims description 10
- 238000005259 measurement Methods 0.000 claims description 10
- 238000001914 filtration Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims 2
- 238000005096 rolling process Methods 0.000 abstract description 18
- 238000004364 calculation method Methods 0.000 description 21
- 230000008447 perception Effects 0.000 description 19
- 239000000047 product Substances 0.000 description 19
- 230000000694 effects Effects 0.000 description 7
- 238000012549 training Methods 0.000 description 7
- 238000004590 computer program Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 238000010276 construction Methods 0.000 description 4
- 238000005457 optimization Methods 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 102000002274 Matrix Metalloproteinases Human genes 0.000 description 1
- 108010000684 Matrix Metalloproteinases Proteins 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 239000012467 final product Substances 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/29—Geographical information databases
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/23—Updating
- G06F16/2358—Change logging, detection, and notification
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Databases & Information Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Multimedia (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明实施例公开了一种车辆误差状态的更新方法和装置，该方法包括：在接收到感知图像时，从预设导航地图中确定与感知图像中的目标图像元素存在匹配关系的目标地图元素；基于目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将第一位置转换为在目标图像元素特征点的曝光时刻，基于相机坐标系的第二位置；将转换得到的第二位置对应的目标地图元素投影到所述感知图像所在平面，并利用投影后的目标地图元素与目标图像元素之间的重投影残差，对车辆的误差状态矩阵进行更新。通过采用上述技术方案，在利用卷帘快门相机进行图像感知以对车辆位姿进行定位时，提升了自动驾驶车辆的定位精度。
Description
技术领域
本发明涉及自动驾驶技术领域，具体涉及一种车辆误差状态的更新方法和装置。
背景技术
目前国内外做SLAM(Simultaneous Localization and Mapping，即时定位与地图构建)和惯性导航的采集设备大多基于camera相机、IMU(惯性测量单元)和GPS(GlobalPositioning System，全球定位系统)构成的定位模块。
如果相机为rolling shutter(卷帘快门)相机，利用该相机所采集的每一行像素的曝光时刻不同，将rolling shutter相机安装于自动驾驶的车辆上时，随着车辆的运动，该相机的不同行像素实际是在车辆的不同位置下曝光的，这会导致采集到的图像有整体的倾斜。图1为现有技术提供的一种利用rolling shutter相机拍摄的图像。如图1所示，图1中的交通牌1和路灯杆2存在明显的倾斜状态。因此，在利用高精度地图中3D语义信息到相机的投影和相机实际拍摄特征的一致性进行车辆定位时，相对于画面中心的曝光时刻，画面不同行上的曝光时间发生在不同时刻，即不是车辆在同一位置所对应的像素。此时，如果仍然按照画面中心曝光时间进行3D特征的重投影，则会在重投影残差中引入额外的偏差，不利于无人车在高精度语义地图中进行定位。
发明内容
本发明实施例公开一种车辆误差状态的更新方法和装置，在利用rollingshutter相机进行图像感知以对车辆位姿进行定位时，提升了自动驾驶车辆的定位精度。
第一方面，本发明实施例公开了一种车辆误差状态的更新方法，应用于自动驾驶，该方法包括：
在接收到感知图像时，从预设导航地图中确定与所述感知图像中的目标图像元素存在匹配关系的目标地图元素；
基于所述目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将所述第一位置转换为在目标图像元素特征点的曝光时刻，基于所述相机坐标系的第二位置；
将转换得到的第二位置对应的目标地图元素投影到所述感知图像所在平面，并利用投影后的目标地图元素与所述目标图像元素之间的重投影残差，对车辆的误差状态矩阵进行更新；
其中，所述第一目标转换关系中包含有车辆的速度、所述误差状态量、惯性测量单元IMU采集的角速度，以及目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差。
可选的，利用投影后的目标地图元素与所述目标图像元素之间的重投影残差，对车辆的误差状态量进行更新，包括：
计算投影后的目标地图元素与所述目标图像元素之间的重投影残差；
基于扩展卡尔曼滤波算法，根据所述重投影残差与所述误差状态量所形成的误差状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新；
其中，所述线性关系中的系数矩阵中包含有：利用所述目标转换关系中的第二位置对各误差状态变量求导后得到的雅克比矩阵。
可选的，所述目标地图元素为路灯杆；
相应的，所述投影后的目标地图元素所对应的投影直线以参数方程的形式表示；
相应的，所述计算投影后的目标地图元素与所述目标图像元素之间的重投影残差，包括：
根据所述路灯杆对应投影直线的参数方程和所述感知图像中对应的目标路灯杆端点的像素坐标，将所述目标路灯杆端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述目标地图元素为交通牌；
相应的，所述投影后的目标地图元素为：将所述交通牌的每条边投影到所述感知图像所在平面时得到的投影直线，其中，各条边对应的投影直线以参数方程的形式表示；
相应的，所述计算投影后的目标地图元素与所述目标图像元素之间的重投影残差，包括：
对于所述交通牌的任意一条边，根据该条边的端点在所述感知图像中的像素坐标和该条边投影后对应投影直线的参数方程，将该条边的端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述目标地图元素为车道线曲线；
相应的，所述方法还包括：
根据投影后的车道线曲线对应各投影点的行数坐标，将所述感知图像中对应的车道线曲线划分为多个区间，其中，投影后的车道线曲线在每个区间内所对应的投影直线以参数方程的形式表示；
对于每一个区间，根据感知图像中车道线曲线端点的像素坐标和该区间内投影后的车道线直线对应的参数方程，将所述车道线曲线端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述目标地图元素为车道线虚线段；
相应的，在转换为所述第二位置之前，所述方法还包括：
将预设导航地图中的车道线虚线段投影到所述感知图像所在平面，并确定投影后的车道线虚线段中，两个虚线端点到所述感知图像中对应端点的像素距离；
如果所述像素距离大于设定阈值，则将转换得到的第二位置对应的车道线虚线段投影到所述感知图像所在平面时，投影后的车道线虚线段通过投影直线的参数方程的形式表示；
相应的，所述计算投影后的目标地图元素与所述目标图像元素之间的重投影残差，包括：
根据所述感知图像中虚线段端点坐标和投影后的虚线段对应的参数方程，将感知图像中虚线段端点到所述参数方程之间的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述方法还包括：
如果所述像素距离小于设定阈值，则基于所述车道线虚线段端点在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第二目标转换关系，将所述第一位置转换为在感知图像中对应车道线虚线段端点的曝光时刻，基于所述相机坐标系的第二位置，其中，所述第二目标转换关系中包含有车辆的速度、误差状态量、角速度以及所述车道线虚线段端点的曝光时刻与感知图像的中心行曝光时刻之间的时间差；
相应的，将转换得到的第二位置对应的车道线虚线段投影到所述感知图像所在平面时，投影后的车道线虚线段通过投影端点的像素坐标的形式表示；
相应的，所述计算投影后的目标地图元素与所述目标图像元素之间的重投影残差，包括：
将所述感知图像中车道线虚线段端点到对应投影端点的像素坐标的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第三矩阵和雅克比矩阵的乘积；其中，所述第三矩阵是利用所述重投影残差，对基于相机坐标系的车道线虚线段端点的第二位置进行求导后得到的。
可选的，根据所述重投影残差与所述误差状态量所形成的误差状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新，包括：
按照如下公式，对车辆的误差状态矩阵进行更新：
其中，表示重投影残差；/>表示从k时刻到k+1时刻的误差状态矩阵；R为感知图像的观测噪声参数；H表示系数矩阵。
可选的，所述方法还包括：按照如下公式对车辆估计位姿对应的协方差矩阵进行更新：
P+＝(I-KH)P-
其中，P-表示更新前的协方差矩阵；P+表示更新后的协方差矩阵；I为单位矩阵；K表示卡尔曼增益，K＝P-HT(HP-HT+R)-1；H表示所述线性关系中的系数矩阵。
可选的，所述第一目标转换关系为：
其中，Lc表示目标地图元素在相机坐标系下的直线参数；表示目标地图元素在世界坐标系下的直线坐标表示；Nc表示相机坐标系原点与目标地图元素的两个特征点构成的平面的法向量；Vc是目标地图元素基于相机坐标系的方向向量；Nw表示世界坐标系原点与目标地图元素的两个特征点构成的平面的法向量；VW表示目标地图元素基于世界坐标系的方向向量；/>和/>分别为图像元素到来时刻imu状态量的姿态和位置；/>和/>均为imu和相机之间的外参；ωi为imu测量的角速度；vi为车辆的速度；Δt为目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差。
可选的，所述第二目标转换关系为：
其中，是车道线虚线段的端点在相机坐标系下的三维坐标，/>是车道线虚线段的端点在世界坐标系下的三维坐标；vi车辆的速度；Δt为车道线虚线段端点的曝光时刻与感知图像的中心行曝光时刻之间的时间差；/>和/>为车辆的误差状态量，/>和/>分别表示imu在世界坐标系和在相机坐标系下的旋转矩阵；/>表示imu坐标系相对于camera坐标系的位置。
第二方面，本发明实施例还提供了一种车辆误差状态的更新装置，应用于自动驾驶，该装置包括：
目标地图元素确定模块，被配置为在接收到感知图像时，从预设导航地图中确定与所述感知图像中的目标图像元素存在匹配关系的目标地图元素；
坐标系转换模块，被配置为基于所述目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将所述第一位置转换为在目标图像元素特征点的曝光时刻，基于所述相机坐标系的第二位置；
更新模块，被配置为将转换得到的所述第二位置对应的目标地图元素投影到所述感知图像所在平面，并利用投影后的目标地图元素与所述目标图像元素之间的重投影残差，对车辆的误差状态矩阵进行更新；
其中，所述第一目标转换关系中包含有车辆的速度、所述各误差状态量、惯性测量单元IMU采集的角速度，以及目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差。
可选的，所述更新模块包括投影单元和更新单元；
所述投影单元，具体被配置为：将转换得到的第二位置对应的目标地图元素投影到所述感知图像所在平面；
所述更新单元包括：
重投影残差计算子单元，被配置为计算投影后的目标地图元素与所述目标图像元素之间的重投影残差；
误差状态矩阵更新子单元，被配置为基于扩展卡尔曼滤波算法，根据所述重投影残差与所述误差状态量所形成的误差状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新；
其中，所述线性关系中的系数矩阵中包含有：利用所述目标转换关系中的第二位置对各误差状态变量求导后得到的雅克比矩阵。
可选的，所述目标地图元素为路灯杆；
相应的，所述投影后的目标地图元素所对应的投影直线以参数方程的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
根据所述路灯杆对应投影直线的参数方程和所述感知图像中对应的目标路灯杆端点的像素坐标，将所述目标路灯杆端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述目标地图元素为交通牌；
相应的，所述投影后的目标地图元素为：将所述交通牌的每条边投影到所述感知图像所在平面时得到的投影直线，其中，各条边对应的投影直线以参数方程的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
对于所述交通牌的任意一条边，根据该条边的端点在所述感知图像中的像素坐标和该条边投影后对应投影直线的参数方程，将该条边的端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述目标地图元素为车道线曲线；
相应的，所述装置还包括：
区间划分模块，被配置为根据投影后的车道线曲线对应各投影点的行数坐标，将所述感知图像中对应的车道线曲线划分为多个区间，其中，投影后的车道线曲线在每个区间内所对应的投影直线以参数方程的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
对于每一个区间，根据感知图像中车道线曲线端点的像素坐标和该区间内投影后的车道线直线对应的参数方程，将所述车道线曲线端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述目标地图元素为车道线虚线段；
相应的，所述装置还包括：像素距离确定模块，被配置为在转换为所述第二位置之前，将预设导航地图中的车道线虚线段投影到所述感知图像所在平面，并确定投影后的车道线虚线段中，两个虚线端点到所述感知图像中对应端点的像素距离；
如果所述像素距离大于设定阈值，则将转换得到的第二位置对应的车道线虚线段投影到所述感知图像所在平面时，投影后的车道线虚线段通过投影直线的参数方程的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
根据所述感知图像中虚线段端点坐标和投影后的虚线段对应的参数方程，将感知图像中虚线段端点到所述参数方程之间的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述装置还包括：
位置转换模块，被配置为如果所述像素距离小于设定阈值，则基于所述车道线虚线段端点在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第二目标转换关系，将所述第一位置转换为在感知图像中对应车道线虚线段端点的曝光时刻，基于所述相机坐标系的第二位置，其中，所述第二目标转换关系中包含有车辆的速度、误差状态量、角速度以及所述车道线虚线段端点的曝光时刻与感知图像的中心行曝光时刻之间的时间差；
相应的，将转换得到的第二位置对应的车道线虚线段投影到所述感知图像所在平面时，投影后的车道线虚线段通过投影端点的像素坐标的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
将所述感知图像中车道线虚线段端点到对应投影端点的像素坐标的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第三矩阵和雅克比矩阵的乘积；其中，所述第三矩阵是利用所述重投影残差，对基于相机坐标系的车道线虚线段端点的第二位置进行求导后得到的。
可选的，所述误差状态矩阵更新子单元，具体被配置为：
按照如下公式，对车辆的误差状态矩阵进行更新：
其中，表示重投影残差；/>表示从k时刻到k+1时刻的误差状态矩阵；R为感知图像的观测噪声参数；H表示系数矩阵。
可选的，所述装置还包括：协方差矩阵更新模块，被配置为：
按照如下公式对车辆估计位姿对应的协方差矩阵进行更新：
P+＝(I-KH)P-
其中，P-表示更新前的协方差矩阵；P+表示更新后的协方差矩阵；I为单位矩阵；K表示卡尔曼增益，K＝P-HT(HP-HT+R)-1；H表示所述线性关系中的系数矩阵。
可选的，所述第一目标转换关系为：
其中，Lc表示目标地图元素在相机坐标系下的直线参数；表示目标地图元素在世界坐标系下的直线坐标表示；Nc表示相机坐标系原点与目标地图元素的两个特征点构成的平面的法向量；Vc是目标地图元素基于相机坐标系的方向向量；Nw表示世界坐标系原点与目标地图元素的两个特征点构成的平面的法向量；VW表示目标地图元素基于世界坐标系的方向向量；/>和/>分别为图像元素到来时刻imu状态量的姿态和位置；/>和/>均为imu和相机之间的外参；ωi为imu测量的角速度；vi为车辆的速度；Δt为目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差。
可选的，所述第二目标转换关系为：
其中，是车道线虚线段的端点在相机坐标系下的三维坐标，/>是车道线虚线段的端点在世界坐标系下的三维坐标；vi车辆的速度；Δt为车道线虚线段端点的曝光时刻与感知图像的中心行曝光时刻之间的时间差；/>和/>为车辆的误差状态量，/>和/>分别表示imu在世界坐标系和在相机坐标系下的旋转矩阵；/>表示imu坐标系相对于camera坐标系的位置。
第三方面，本发明实施例还提供了一种车载终端，包括：
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行本发明任意实施例所提供的车辆误差状态的更新方法的部分或全部步骤。
第四方面，本发明实施例还提供了一种计算机可读存储介质，其存储计算机程序，所述计算机程序包括用于执行本发明任意实施例所提供的车辆误差状态的更新方法的部分或全部步骤的指令。
第五方面，本发明实施例还提供了一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行本发明任意实施例所提供的车辆误差状态的更新方法的部分或全部步骤。
本实施例提供的技术方案，在接收到感知图像时，从预设导航地图中确定与感知图像中的目标图像元素存在匹配关系的目标地图元素。由于第一目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的目标转换关系，是在现有转换关系的基础上，利用车辆的速度、惯性测量单元IMU采集的角速度，以及目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差进行修正后得到的，因此，利用该第一目标转换关系可将第一位置转换为在目标图像元素特征点的曝光时刻，基于相机坐标系的第二位置。相对于现有技术，在利用图像中心行曝光时刻的相机姿态，将世界坐标系中的目标地图元素转换到相机坐标系的坐标转换方式，本实施例这样设置对于rolling shutter相机而言，能够获取到更加精确的匹配关系，从而在后续利用投影后的目标地图元素与目标图像元素之间的重投影残差去更新车辆位姿时，能够有效提升车辆的定位精度。
本发明的发明点包括：
1、通过利用车辆的速度、惯性测量单元IMU采集的角速度，以及目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差，对现有技术中地图元素从世界坐标系到相机坐标系的转换关系进行修正，可以使得预设导航地图中的目标地图元素投影到对应目标图像元素曝光时刻对应的相机位姿下，即消除了rolling shutter效应的影响，从而可以使无人驾驶车辆在预设导航地图中使用地图元素与深度学习感知出的图像元素进行定位时的定位精度得到大幅提升，是本发明的发明点之一。
2、本发明实施例提供的技术方案，分别针对路灯杆、交通牌、车道线曲线和重投影残差小于设定阈值的车道线虚线段，提供了点到直线的模型来进行重投影匹配，以获得更加准确的匹配关系。利用该匹配关系可提升车辆的定位精度，是本发明的发明点之一。
3、本发明实施例的技术方案中，针对重投影残差大于设定阈值的车道线虚线段，提供了点到点的模型来进行重投影匹配，以获得更加准确的匹配关系。利用该匹配关系可提升车辆的定位精度，是本发明的发明点之一。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为现有技术提供的一种利用rolling shutter相机拍摄的图像；
图2为本发明实施例提供的一种系统从k-1时刻的后验状态到k时刻的后验状态流程图；
图3为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图；
图4为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图；
图5为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图；
图6a为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图；
图6b为本发明实施例提供的一种投影后的车道线与感知图像中对应的车道线的示意图；
图7为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图；
图8为本发明实施例提供的一种车辆误差状态的更新装置的结构示意图；
图9是本发明实施例提供的一种车载终端的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。
为了更加清楚、明白地解释各实施例的内容，下面先对本发明实施例提供的技术方案的工作原理进行简单介绍：
本发明的目的是，利用高精度地图中的地图元素，例如路灯杆、交通牌、车道线和车道线虚线端点等，对车辆的位姿进行定位。具体是将这些地图元素投影到感知图像所在平面，并利用投影后地图元素与图像元素之间重投影残差的大小修正车辆的位姿。
在重投影过程中，由于通过rolling shutter相机采集的图像各行像素不是在同一时刻曝光的(简称为rolling shutter效应)，因此，如果不考虑rolling shutter效应的影响，在将地图元素投影到感知图像后，重投影残差会存在一定的偏差，例如地图中的路灯杆有上下两个端点，上端点投影到感知图像中是位于第100行像素，下端点投影到感知图像中是位于第500行像素。现有技术在计算重投影残差时，是将第100行和第500行像素的曝光时刻按同一时刻进行计算的，这样就会导致车辆的定位精度受到影响。
本发明实施例提供的计算方案中，在确定地图元素与图像元素之间的对应匹配关系后，在进行重投影时，可将感知图像上观测到的图像元素的不同的特征点，例如路灯杆的上端点和下端点看作是车辆在不同位姿下获取的图像信息。一般情况下，为了便于运算，可将图像中心行的曝光时间作为基点。基于该基点，可计算出感知图像的其他行与其中心行的时间间隔Δt。在自动驾驶车辆行驶的过程中，通过利用速度、角速度和目标地图元素的特征点的曝光时刻与中心行曝光时刻之间的时间间隔Δt，可对地图元素在世界坐标系与其在相机坐标系中的转换关系进行修正，即对rolling shutter效应进行补偿，以使得世界坐标系中的某个地图元素在投影到感知图像时，能够投影到该地图元素对应图像元素的曝光时刻的相机位姿下，从而将基于相机坐标系的地图元素投影到感知图像所在平面。按照这样的方法，在利用投影后的地图元素与感知图像中图像元素之间的重投影残差对车辆的位姿进行定位时，可提高车辆的定位精度。
下面，从车辆位姿状态变化的角度，对车辆状态量的更新过程进行说明，图2为本发明实施例提供的一种系统从k-1时刻的后验状态到k时刻的后验状态流程图。如图2所示，表示k-1时刻车辆全状态量的后验状态；/>表示k-1时刻车辆全状态量对应的后验协方差矩阵；/>表示k-1时刻车辆的误差状态量的后验状态；/>表示k时刻车辆全状态量的先验状态；/>表示k时刻车辆全状态量对应的先验协方差矩阵；/>表示k时刻车辆的误差状态量的先验状态；/>表示k时刻车辆全状态量的后验状态；/>表示k时刻车辆全状态量的后验协方差矩阵；/>表示k时刻车辆的误差状态量的后验状态。
其中，是利用IMU(Inertial measurement unit，惯性测量单元)测量出的角速度和加速度以及估计出的角速度零偏和加速度零偏，由k-1时刻车辆的后验状态/>预测得到的。/>和/>是利用如下误差转移方程，并根据k-1时刻的误差状态量的先验状态和先验协方差矩阵/>预测得到的：
其中，符号“∧”代表状态估计值，“～”代表状态误差值。误差状态矩阵在误差状态矩阵/>中，各元素的含义依次为：imu在世界坐标系下的姿态、imu坐标系相对于世界坐标系的速度、imu在世界坐标系下的位置、imu角速度零偏、imu加速度零偏、imu坐标系在camera坐标系下的姿态和imu坐标系相对于camera(相机)坐标系的位置。此外，上述误差状态转移方程中，系数0均代表3×3的零矩阵。ng,na为imu量测角速度和线加速度的过程噪声，ngw，naw为imu零偏游走噪声；/>表示imu在世界坐标系下的姿态的旋转矩阵的表示形式。
本发明实施例提供的技术方案中，当获取到感知图像中的观测信息，即图像元素到来时，利用该观测信息和高精度地图中的地图元素之间的重投影残差z和观测方程可对K时刻的误差状态量的先验状态/>进行更新后，可得到K时刻的后验误差状态量/>其中，在观测方程中，R为图像的观测噪声参数，系数矩阵H是通过如下过程得到：利用车辆的速度、角速度和目标地图元素的特征点的曝光时刻与中心行曝光时刻之间的时间间隔Δt，对地图元素在世界坐标系与其在相机坐标系中的转换关系进行修正后，即对rolling shutter效应进行补偿后，利用该补偿后的转换关系，对各个误差状态量求雅可比矩阵后得到。类似的，K时刻的后验协方差矩阵/>可利用方程P+＝(I-KH)P-对K时刻的先验协方差矩阵P-更新后得到。其中，K＝P-HT(HP-HT+R)-1。
在对K时刻的误差状态量更新后，可将K时刻的后验误差状态量反馈到全状态量/>上，到全状态量/>即/>反馈完成后/>的数学期望值变为/>向量。对于姿态/>的反馈优选采用四元数加法，其他误差状态量可采用普通的加法。通过采用上述方法，可使得估计出的车辆的位姿精度得到提高。
下面各实施例将对利用不同地图元素，对车辆误差状态的更新过程进行详细介绍。
实施例一
请参阅图3，图3为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图。该方法应用于自动驾驶用rolling shutter相机作为感知器件的应用场景下，可由车辆误差状态量的更新装置来执行，该装置可通过软件和/或硬件的方式实现，一般可集成在车载电脑、车载工业控制计算机(Industrial personal Computer，IPC)等车载终端中，本发明实施例不做限定。如图3所示，本实施例提供的方法具体包括：
110、在接收到感知图像时，从预设导航地图中确定与感知图像中的目标图像元素存在匹配关系的目标地图元素。
其中，感知图像是利用预设感知模型对摄像头采集的包含道路信息的图像进行识别后得到的。预设感知模型可以预先采用大量标注有图像语义特征的道路样本图像对感知模型进行训练。其中，图像语义特征可包括交通牌、路灯杆、车道线、车道线虚线端点等。通过将包含有道路信息的道路图像输入至训练好的预设感知模型，基于预设感知模型的识别结果，即可得到道路图像中的图像语义特征。其中，预设感知模型可以通过以下方式得到：
构建训练样本集，该训练样本集包括多组训练样本数据，每组训练样本数据包括道路样本图像和对应的标注有图像语义特征的道路感知样本图像；基于训练样本集对搭建的初始神经网络进行训练得到预设感知模型，该预设感知模型使得每组训练样本数据中的道路样本图像与对应的标注有图像语义特征的道路感知样本图像相关联。模型输出的即可称之为感知图像。感知图像中的各种道路信息可称为地图元素，也可称为图像观测数据。
本实施例中，导航地图是指应用于自动驾驶的误差级别为厘米级的高精度导航地图。高精度导航地图中具有交通牌、路灯杆、车道线和车道线虚线端点等元素的3D位置信息。
需要说明的是，当自动驾驶车辆刚刚驶入预设导航地图所覆盖的与先验位置对应的区域时，系统通过一系列算法将预设导航地图中的路灯杆、交通牌、车道线以及车道线虚线端点等地图元素与车载相机获取的图像上的路灯杆、交通牌、车道线以及车道线虚线端点等感知元素进行一对一的匹配，输出正确的感知元素与地图元素匹配对，以形成感知图像与预设导航地图之间正确的匹配关系。步骤110是在图像观测数据到达系统时，获取存在匹配关系的目标地图元素和目标图像元素，例如获取高精度地图中的路灯杆和与其存在一一对应关系的感知图像中的路灯杆。
120、基于目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将第一位置转换为在目标图像元素特征点的曝光时刻，基于相机坐标系的第二位置。
现有技术中，在将世界坐标系中的目标地图元素转换到相机坐标系中时，是利用图像中心行曝光时刻的相机姿态进行转换的。对于rolling shutter相机而言，图像中心行的曝光时刻并不是该每行像素的实际曝光时刻，这样在后续利用投影后的目标地图元素与目标图像元素之间的重投影残差去更新车辆位姿时，会对车辆位姿的定位精度造成影响。相对于现有技术中的转换关系，本实施例中，目标转换关系是利用车辆的速度、IMU采集的角速度以及目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差，对现有转换关系修正后得到的。利用本实施例中的目标转换关系，可将基于世界坐标系的目标地图元素，投影到曝光目标图像元素时刻相机坐标系下的图像平面中，从而提升了后续利用重投影残差对车辆位姿的更新精度。
具体的，以路灯杆为例进行说明。从预设导航地图中可以得到路灯杆上下端点在世界坐标系的三维坐标，设Aw和Bw是高精地图中一根路灯杆的两个端点的三维坐标，则路灯杆在世界坐标系中的直线坐标Lw(第一位置)可表示为：
其中，Nw是世界坐标系原点与A点和B点构成的平面的法向量，Vw是路灯杆直线的方向向量。设图像感知元素到来时刻系统imu状态量位姿为和/>imu和camera之间的外参为/>和/>则路灯杆直线在camera坐标系下的直线参数Lc(第二位置)为：
其中，Nc表示相机坐标系原点与目标地图元素的两个特征点构成的平面的法向量；Vc是目标地图元素基于相机坐标系的方向向量。
本实施例中，为例消除rolling shutter效应带来的影响，将预设导航地图中的路灯杆直线投影到像素曝光时刻对应的camera位姿下，可将Lc和Lw之间的第一目标转换关系修正为：
其中，ωi为imu测量的角速度；vi为车辆的速度；Δt为目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差。其中，Δt可通过公如下公式来计算：
其中，为路灯杆在图像上对应的直线的一个端点a对应的带畸变的像素，该像素的像素坐标/>tl为感知图像的一行曝光时刻与相邻行曝光时刻间隔；rows为感知图像的所有行数。
130、将转换得到的第二位置对应的目标地图元素投影到感知图像所在平面，并利用投影后的目标地图元素与目标图像元素之间的重投影残差，对车辆的误差状态矩阵进行更新。
由于目标地图元素的不同，投影后的目标地图元素与目标图像元素之间的重投影残差的计算方式也不同。
示例性的，如果目标地图元素为路灯杆，重投影残差的计算方式为计算感知图像中路灯杆的端点到投影到感知图像中的投影直线的观测距离，并将该观测距离作为重投影残差。如果目标地图元素为车道线虚线端点，如果判断出投影到图像的两个虚线端点与各自对应的图像中识别到的虚线的端点的重投影残差的像素距离小于设定阈值，则将感知图像中虚线端点到投影后对应端点之间的距离作为重投影残差。
本实施例中，在计算出投影后的目标地图元素与目标图像元素之间的重投影残差后，可基于扩展卡尔曼滤波算法，根据重投影残差与各误差状态量所形成的状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新。
具体的，重投影残差与各误差状态量所形成的状态矩阵之间的线性关系可表示为：
其中，表示重投影残差；/>表示从k时刻到k+1时刻的误差状态矩阵；R为感知图像的观测噪声参数。其中，在系数矩阵H中包含有：利用上述修正后的第一目标转换关系中的第二位置对各误差状态变量求导后得到的雅克比矩阵。
由于在车辆的误差状态矩阵更新的过程中，车辆估计位子对应的协方差矩阵也可相应得到更新，因此，考虑到协方差矩阵的更新过程，该协方差矩阵用于说明车辆位姿状态的估计精度，协方差越小，位姿估计的越准确。上述线性关系可变形为：
其中，表示更新前各误差状态量所形成的误差状态矩阵；/>表示更新后各误差状态量所形成的误差状态矩阵；/>表示重投影残差；K表示卡尔曼增益，其中，K＝P-HT(HP-HT+R)-1。H为系数矩阵，该系数矩阵中包含有利用上述第二位置对各误差状态变量求导后得到的雅克比矩阵。P为协方差矩阵，R为感知图像的观测噪声参数。
具体的，本实施例提供的技术方案在更新车辆的误差状态时，可按照如下公式对协方差矩阵进行更新：
P+＝(I-KH)P-
其中，P-表示更新前的协方差矩阵；P+表示更新后的协方差矩阵；I为单位矩阵；K表示卡尔曼增益；H表示所述线性关系中的系数矩阵。具体的，车辆误差状态矩阵在不同时刻的更新过程，以及对车辆的误差状态矩阵更新后，全状态量的更新过程可参照上文中从k-1时刻的后验状态到k时刻的后验状态的更新流程，本实施例不再赘述。
本实施例提供的技术方案，在接收到感知图像时，从预设导航地图中确定与感知图像中的目标图像元素存在匹配关系的目标地图元素。由于第一目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的目标转换关系，是在现有转换关系的基础上，利用车辆的速度、惯性测量单元IMU采集的角速度，以及目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差进行修正后得到的，因此，利用该第一目标转换关系可将第一位置转换为在目标图像元素特征点的曝光时刻，基于相机坐标系的第二位置。相对于现有技术，在利用图像中心行曝光时刻的相机姿态，将世界坐标系中的目标地图元素转换到相机坐标系的坐标转换方式，本实施例这样设置对于rolling shutter相机而言，能够获取到更加精确的匹配关系，从而在后续利用投影后的目标地图元素与目标图像元素之间的重投影残差去更新车辆位姿时，能够有效提升车辆的定位精度。
实施例二
请参阅图4，图4为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图。本实施例在上述实施例的基础上，将路灯杆作为目标地图元素，对重投影残差的计算以及系数矩阵求解的过程进行了优化。如图4所示，该方法包括：
210、在接收到感知图像时，从预设导航地图中确定与感知图像中的目标图像元素存在匹配关系的目标地图元素。
本实施例中，目标地图元素和对应的目标感知元素优选为路灯杆。
220、基于目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将第一位置转换为在目标图像元素特征点的曝光时刻，基于相机坐标系的第二位置。
其中，目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系为上文中对Lc和Lw之间修正后的转换关系。利用该关系，可将在世界坐标系下预设导航地图中的路灯杆的坐标(第一位置)转换到感知图像中对应路灯杆端点曝光时刻时在相机坐标系下的坐标(第二位置)，以消除rolling shutter效应带来的影响。
230、根据投影后的路灯杆所对应投影直线的参数方程和感知图像中对应的目标路灯杆端点的像素坐标，将目标路灯杆端点到投影直线的观测距离作为重投影残差。
本实施例中，当目标地图元素为路灯杆时，投影后的目标地图元素所对应的投影直线以参数方程的形式表示，具体参数方程可表示为：其中，K′＝det(K)K-T，K是相机内参矩阵。地图中的路灯杆在感知图像中对应目标路灯杆端点为路灯杆的上端点和下端点。
相应的，在计算投影后的路灯杆与感知图像中对应的路灯杆的重投影残差时，可根据感知图像中目标路灯杆端点的像素坐标和对应投影直线的参数方程，将目标路灯杆端点到投影直线的观测距离作为重投影残差，此过程可概括为基于点到直线模型来进行重投影匹配优化。
240、基于扩展卡尔曼滤波算法，根据重投影残差与各误差状态量所形成的误差状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新。
本实施例中，关于点到直线模型，线性关系中的系数矩阵中除了上述雅克比矩阵之外，还包含有：利用重投影残差对投影直线对应的参数方程和上述转换后得到的第二位置，分别求导后得到的第一矩阵和第二矩阵，而系数矩阵则为第一矩阵、第二矩阵和雅克比矩阵的乘积。
具体的，如果感知图像上观测到的路灯杆的一个端点a的像素坐标为a＝[ua va 1]T,则该端点到投影后的投影直线l的观测距离(重投影残差)为：
其中，第一矩阵为：其中/>/>
第二矩阵为：
Lc分别对状态量vi，/>bg，ba，/>求雅克比可得：
若设备的每一行与相邻行的曝光时刻差值tl是已经的，则可以直接使用上述方程对状态更新，如果tl是未知的需要实时估计，则需将其加入状态量中，并使用观测误差对其求雅克比矩阵。如下：
其中n是畸变像素所在行的索引减图像中间行索引得到的差值。
综上所述，系数矩阵为：H＝J1J2[Jθwi Jvi Jpwi 03×3 03×3Jθci Jtci Jtl]。在得到系数矩阵之后，可利用上文中的公式对车辆的误差状态矩阵进行更新。
一般的，感知图像中的一根路灯杆可以得到两个端点的像素，因此，分别对两个图像像素点重复上述过程，直到遍历完一帧图像上的所有杆的匹配关系，可完成利用路灯杆对误差状态量的更新。
本实施例在上述实施例的基础上，将目标地图元素优化为路灯杆，并给出了针对路灯杆的对应的点到直线模型的方案，来构建重投影残差和雅克比矩阵。通过将预设导航地图中的路灯杆投影到对应感知地图路灯杆端点的曝光时刻对应的相机位姿下，可利用投影后得到路灯杆和感知图像中对应路灯杆之间的重投影残差来更新车辆的误差状态矩阵，提升了车辆位姿的定位精度。
实施例三
请参阅图5，图5为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图。本实施例在上述实施例的基础上，将交通牌作为目标地图元素，对重投影残差的计算以及系数矩阵求解的过程进行了优化。如图5所示，该方法包括：
310、在接收到感知图像时，从预设导航地图中确定与感知图像中的目标图像元素存在匹配关系的目标地图元素。
本实施例中，目标地图元素和对应的目标感知元素优选为交通牌。
320、基于目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将第一位置转换为在目标图像元素特征点的曝光时刻，基于相机坐标系的第二位置。
330、对于交通牌的任意一条边，根据该条边的端点在感知图像中的像素坐标和该条边投影后对应投影直线的参数方程，将该条边的端点到投影直线的观测距离作为重投影残差。
本实施例中，当目标地图元素为交通牌时，投影后的目标地图元素为将交通牌的每条边均投影到感知图像所在平面得到的投影直线，其中，各条边对应的投影直线均可以上述参数方程的形式表示。
在计算计算投影后的交通牌与感知图像中对应交通牌之间的重投影残差时，对于交通牌的任意一条边，可仍可基于上述路灯杆采用的点到直线的模型进行计算。
340、基于扩展卡尔曼滤波算法，根据重投影残差与各误差状态量所形成的状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新。
本实施例中，在利用交通牌更新车辆的误差状态量时，对于交通牌的任意一条边，可按照路灯杆对应的计算公式，确定系数矩阵H，并可根据重投影残差与各误差状态量所形成的状态矩阵之间的线性关系对车辆的误差状态矩阵进行更新，具体可参照上述实施例提供的误差状态矩阵的更新公式，本实施例不再赘述。由于交通牌有四条边，可分别对每条边重复上述过程，直到遍历完一帧图像上的所有交通牌的匹配关系，以完成利用交通牌对误差状态量的更新。
本实施例在上述实施例的基础上，将目标地图元素优化为了交通牌，该交通牌也可采用点到直线模型的方案来构建重投影残差和雅克比矩阵。通过将预设导航地图中的交通牌中的某条边投影到对应感知地图交通牌某条边端点的曝光时刻对应的相机位姿下，可利用投影后得到交通牌和感知图像中对应交通牌之间的重投影残差来更新车辆的误差状态量，从而提升车辆位姿的定位精度。
实施例四
请参阅图6a，图6a为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图。本实施例在上述实施例的基础上，将车道线曲线作为目标地图元素，对重投影残差的计算以及系数矩阵求解的过程进行了优化。如图6a所示，该方法包括：
410、在接收到感知图像时，从预设导航地图中确定与感知图像中的目标图像元素存在匹配关系的目标地图元素。
本实施例中，目标地图元素和对应的目标感知元素为车道线曲线。
420、基于目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的目标转换关系，将第一位置转换为在目标图像元素特征点的曝光时刻，基于相机坐标系的第二位置。
430、根据投影后的车道线曲线对应各投影点的行数坐标，将感知图像中对应的车道线曲线划分为多个区间。
其中，投影后的车道线曲线在每个区间内所对应的投影直线以参数方程的形式表示。
440、对于每一个区间，根据感知图像中车道线曲线端点的像素坐标和该区间内投影后的车道线直线对应的参数方程，将车道线曲线端点到投影直线的观测距离作为重投影残差。
本实施例中，当目标地图元素为车道线曲线时，仍可基于上述路灯杆采用的点到直线的模型进行重投影优化匹配。
具体的，图6b为本发明实施例提供的一种投影后的车道线与感知图像中对应的车道线的示意图。如图6b所示，车道线1中的各点表示预设导航地图中的车道线上的点投影到感知图像中的位置，相邻两点构成一条直线；车道线2中的各点表示的是感知图像上观测到的车道线离散出的点的像素位置；横线3表示根据预设地图车道线投影点的行数坐标将观测到的车道线分割成多个区间的分割线。对于分割后的任意一个区间，落在该区间的图像观测点与落在对应区间的重投影直线段可按照路灯杆杆对应的计算公式，确定系数矩阵H，并可根据重投影残差与误差状态量所形成的状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新具体可参照上述实施例提供的误差状态矩阵的更新公式，本实施例不再赘述。
450、基于扩展卡尔曼滤波算法，根据重投影残差与车辆的各误差状态量所形成的误差状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新。
本实施例在上述实施例的基础上，将目标地图元素优化为了车道线曲线，并给出了针对车道线曲线对应的点到直线模型的方案，来构建残差和雅克比矩阵。通过将预设导航地图中的车道线曲线投影到对应感知地图中对应车道线曲线的曝光时刻对应的相机位姿下，可利用投影后得到车道线曲线和感知图像中对应车道线曲线之间的重投影残差来更新车辆的误差状态量，从而提升车辆位姿的定位精度。
实施例五
请参阅图7，图7为本发明实施例提供的一种车辆误差状态的更新方法的流程示意图。本实施例在上述实施例的基础上，将车道线虚线段作为目标地图元素，并对重投影残差的计算以及系数矩阵求解的过程进行了优化。如图7所示，该方法包括：
501、在接收到感知图像时，从预设导航地图中确定与感知图像中的目标图像元素存在匹配关系的目标地图元素。
本实施例中，目标地图元素和目标图像元素优选为车道线虚线段。
502、将预设导航地图中的车道线虚线段投影到感知图像所在平面，并判断投影后的两个虚线端点到感知图像中对应端点的像素距离是否大于设定阈值，若是，则执行步骤503；否则，执行步骤504。
如果上述距离小于设定距离阈值，说明车道线虚线段的起点(离车头近的一端)未被车头遮挡，此时，可将感知图像中虚线段端点到对应投影端点的像素坐标的观测距离作为重投影残差，简称为基于点到点模型进行重投影匹配优化。如果上述距离大于设定距离阈值，则仍基于上述实施例提供的点到直线模型计算重投影残差和构建雅克比矩阵。
503、基于车道线虚线段在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将第一位置转换为在感知图像中对应车道线虚线段端点的曝光时刻，基于相机坐标系的第二位置，继续执行步骤505。
504、基于车道线虚线段在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第二目标转换关系，将第一位置转换为在目标图像元素特征点的曝光时刻，基于相机坐标系的第二位置，继续执行步骤506。
其中，所述第二目标转换关系中包含有车辆的速度、误差状态量、角速度以及车道线虚线段端点的曝光时刻与感知图像的中心行曝光时刻之间的时间差，利用该第二目标转换关系，可将世界坐标系中的车道线虚线端点转换到对应观测的曝光时刻相机位姿下。
505、将转换得到的第二位置对应的车道线虚线段投影到感知图像所在平面，投影后的车道线虚线段通过投影直线的参数方程的形式表示，继续执行步骤507。
506、将转换得到的第二位置对应的车道线虚线段投影到感知图像所在平面，投影后的车道线虚线段通过投影端点的像素坐标的形式表示，继续执行步骤508。
507、根据感知图像中虚线段端点坐标和投影后的虚线段对应的参数方程，将感知图像中虚线段端点到所述参数方程之间的观测距离作为重投影残差，继续执行步骤509。
本实施例中，当像素距离大于设定阈值，则投影后的虚线段通过投影直线的参数方程的形式表示，此时，重投影残差和雅克比矩阵的构建可按照如路灯杆对应的点到直线的模型计算，此处不再赘述。
508、根据感知图像中虚线段端点坐标和对应投影后的投影端点的像素坐标，将感知图像中车道线虚线段端点到对应投影端点的像素坐标的观测距离作为重投影残差，继续执行步骤509。
509、基于扩展卡尔曼滤波算法，根据重投影残差与各误差状态量所形成的状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新。
本实施例中，对于点到点模型，线性关系中的系数矩阵中还包含有：利用所述重投影残差，对待投影的车道线虚线段端点的第二位置进行求导后得到的第三矩阵，而系数矩阵则为第三矩阵和雅克比矩阵的乘积。具体重投影残差和雅克比矩阵的构建过程如下：
将世界坐标系中的车道线虚线段的某个段点转换到对应观测的曝光时刻相机位姿下，即第二目标转换关系对应的方程为：
其中，是一点在相机坐标系下的三维坐标(第二位置)，/>是一点在世界坐标系下的三维坐标(第一位置)。图像上对应的观测点的像素坐标为a＝[uava]T。将/>重投影到感知图像所在平面的方程为：/>
其中，fx、fy、/>cx和cy为相机的内参。
重投影残差e
对于点对点模型，在系数矩阵中，除包含有雅克比矩阵之外，还包含有：利用重投影残差e，对待投影的车道线虚线段端点的第二位置进行求导后得到的第三矩阵J1，具体公式如下：
此外，雅克比矩阵的构建过程与点到直线模型的雅克比矩阵的构建过程类似，为第二位置对各误差状态变量求导后得到，具体为：
若相机的每一行与相邻行的曝光时刻差值tl是已知的，则可以直接使用上述方程对状态更新，如果tl是未知的需要实时估计，则需将其加入状态量中，并使用观测误差对其求雅克比矩阵，具体过程如下：
其中n是畸变像素所在行的索引减图像中间行索引得到的差值。
综上所述，根据重投影残差与所述各误差状态量所形成的状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新时，线性关系中的线性矩阵H如下：
H＝J1[Jθwi Jvi Jpwi 03×3 03×3 Jθci Jtci Jtl]
若状态更新前系统误差状态量为协方差矩阵为P-，图像观测噪声协方差为R，则量测更新后的误差状态量为：/>
协方差为：P+＝(I-KH)P-。
本实施例在上述实施例的基础上，将目标地图元素优化为了车道线虚线段，并针对重投影距离小于设定阈值的车道线虚线段提供了对应的点到点模型的方案，来构建残差和雅克比矩阵。通过将预设导航地图中的车道线虚线段投影到对应感知地图中对应车道线虚线端点的曝光时刻对应的相机位姿下，可利用投影后得到车道线虚线段端点和感知图像中对应观测端点之间的重投影残差来更新车辆的误差状态量，从而提升车辆位姿的定位精度。
需要注意的是，本发明实施例提出的基于点到直线模型和点到点模型的重投影匹配优化方法，该方法能推广到点和近似直线特征的重投影优化上，而不仅限于交通牌、路灯杆、车道线曲线和车道线虚线段等现有语义信息。
实施例六
请参阅图8，图8为本发明实施例提供的一种车辆误差状态的更新装置的结构示意图。如图8所示，该装置包括：目标地图元素确定模块610、坐标系转换模块620和更新模块630；
其中，目标地图元素确定模块610，被配置为在接收到感知图像时，从预设导航地图中确定与所述感知图像中的目标图像元素存在匹配关系的目标地图元素；
坐标系转换模块620，被配置为基于所述目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将所述第一位置转换为在目标图像元素特征点的曝光时刻，基于所述相机坐标系的第二位置；
更新模块630，被配置为将转换得到的所述第二位置对应的目标地图元素投影到所述感知图像所在平面，并利用投影后的目标地图元素与所述目标图像元素之间的重投影残差，对车辆的误差状态矩阵进行更新；
其中，所述第一目标转换关系中包含有车辆的速度、所述各误差状态量、惯性测量单元IMU采集的角速度，以及目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差。
可选的，所述更新模块包括投影单元和更新单元；
所述投影单元，具体被配置为：将转换得到的第二位置对应的目标地图元素投影到所述感知图像所在平面；
所述更新单元包括：
重投影残差计算子单元，被配置为计算投影后的目标地图元素与所述目标图像元素之间的重投影残差；
误差状态矩阵更新子单元，被配置为基于扩展卡尔曼滤波算法，根据所述重投影残差与所述误差状态量所形成的误差状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新；
其中，所述线性关系中的系数矩阵中包含有：利用所述目标转换关系中的第二位置对各误差状态变量求导后得到的雅克比矩阵。
可选的，所述目标地图元素为路灯杆；
相应的，所述投影后的目标地图元素所对应的投影直线以参数方程的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
根据所述路灯杆对应投影直线的参数方程和所述感知图像中对应的目标路灯杆端点的像素坐标，将所述目标路灯杆端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述目标地图元素为交通牌；
相应的，所述投影后的目标地图元素为：将所述交通牌的每条边投影到所述感知图像所在平面时得到的投影直线，其中，各条边对应的投影直线以参数方程的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
对于所述交通牌的任意一条边，根据该条边的端点在所述感知图像中的像素坐标和该条边投影后对应投影直线的参数方程，将该条边的端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述目标地图元素为车道线曲线；
相应的，所述装置还包括：
区间划分模块，被配置为根据投影后的车道线曲线对应各投影点的行数坐标，将所述感知图像中对应的车道线曲线划分为多个区间，其中，投影后的车道线曲线在每个区间内所对应的投影直线以参数方程的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
对于每一个区间，根据感知图像中车道线曲线端点的像素坐标和该区间内投影后的车道线直线对应的参数方程，将所述车道线曲线端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述目标地图元素为车道线虚线段；
相应的，所述装置还包括：像素距离确定模块，被配置为将预设导航地图中的车道线虚线段投影到所述感知图像所在平面时，并确定投影后的车道线虚线段中，两个虚线端点到所述感知图像中对应端点的像素距离；
如果所述像素距离大于设定阈值，则将转换得到的第二位置对应的车道线虚线段投影到所述感知图像所在平面时，投影后的车道线虚线段通过投影直线的参数方程的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
根据所述感知图像中虚线段端点坐标和投影后的虚线段对应的参数方程，将感知图像中虚线段端点到所述参数方程之间的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
可选的，所述装置还包括：
位置转换模块，被配置为如果所述像素距离小于设定阈值，则基于所述车道线虚线段端点在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第二目标转换关系，将所述第一位置转换为在感知图像中对应车道线虚线段端点的曝光时刻，基于所述相机坐标系的第二位置，其中，所述第二目标转换关系中包含有车辆的速度、误差状态量、角速度以及所述车道线虚线段端点的曝光时刻与感知图像的中心行曝光时刻之间的时间差；
相应的，将转换得到的第二位置对应的车道线虚线段投影到所述感知图像所在平面时，投影后的车道线虚线段通过投影端点的像素坐标的形式表示；
相应的，所述重投影残差计算子单元，具体被配置为：
将所述感知图像中车道线虚线段端点到对应投影端点的像素坐标的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第三矩阵和雅克比矩阵的乘积；其中，所述第三矩阵是利用所述重投影残差，对基于相机坐标系的车道线虚线段端点的第二位置进行求导后得到的。
可选的，所述误差状态矩阵更新子单元，具体被配置为：
按照如下公式，对车辆的误差状态矩阵进行更新：
其中，表示重投影残差；/>表示从k时刻到k+1时刻的误差状态矩阵；R为感知图像的观测噪声参数；H表示系数矩阵。
可选的，所述装置还包括：协方差矩阵更新模块，被配置为：
按照如下公式对车辆估计位姿对应的协方差矩阵进行更新：
P+＝(I-KH)P-
其中，P-表示更新前的协方差矩阵；P+表示更新后的协方差矩阵；I为单位矩阵；K表示卡尔曼增益，K＝P-HT(HP-HT+R)-1；H表示所述线性关系中的系数矩阵。
可选的，所述第一目标转换关系为：
其中，Lc表示目标地图元素在相机坐标系下的直线参数；表示目标地图元素在世界坐标系下的直线坐标表示；Nc表示相机坐标系原点与目标地图元素的两个特征点构成的平面的法向量；Vc是目标地图元素基于相机坐标系的方向向量；Nw表示世界坐标系原点与目标地图元素的两个特征点构成的平面的法向量；VW表示目标地图元素基于世界坐标系的方向向量；/>和/>分别为图像元素到来时刻imu状态量的姿态和位置；/>和/>均为imu和相机之间的外参；ωi为imu测量的角速度；vi为车辆的速度；Δt为目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差。
可选的，所述第二目标转换关系为：
其中，是车道线虚线段的端点在相机坐标系下的三维坐标，/>是车道线虚线段的端点在世界坐标系下的三维坐标；vi车辆的速度；Δt为车道线虚线段端点的曝光时刻与感知图像的中心行曝光时刻之间的时间差；/>和/>为车辆的误差状态量，/>和/>分别表示imu在世界坐标系和在相机坐标系下的旋转矩阵；/>表示imu坐标系相对于camera坐标系的位置。
本发明实施例所提供的车辆误差状态的更新装置可执行本发明任意实施例所提供的车辆误差状态的更新方法，具备执行方法相应的功能模块和有益效果。未在上述实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的车辆误差状态的更新方法。
实施例七
请参阅图9，图9是本发明实施例提供的一种车载终端的结构示意图。如图9所示，该车载终端可以包括：
存储有可执行程序代码的存储器701；
与存储器701耦合的处理器702；
其中，处理器702调用存储器701中存储的可执行程序代码，执行本发明任意实施例所提供的车辆误差状态的更新方法。
本发明实施例公开一种计算机可读存储介质，其存储计算机程序，其中，该计算机程序使得计算机执行本发明任意实施例所提供的车辆误差状态的更新方法。
本发明实施例公开一种计算机程序产品，其中，当计算机程序产品在计算机上运行时，使得计算机执行本发明任意实施例所提供的车辆误差状态的更新方法的部分或全部步骤。
在本发明的各种实施例中，应理解，上述各过程的序号的大小并不意味着执行顺序的必然先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
在本发明所提供的实施例中，应理解，“与A相应的B”表示B与A相关联，根据A可以确定B。但还应理解，根据A确定B并不意味着仅仅根据A确定B，还可以根据A和/或其他信息确定B。
另外，在本发明各实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
上述集成的单元若以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可获取的存储器中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或者部分，可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干请求用以使得一台计算机设备(可以为个人计算机、服务器或者网络设备等，具体可以是计算机设备中的处理器)执行本发明的各个实施例上述方法的部分或全部步骤。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(CompactDisc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。
以上对本发明实施例公开的一种车辆误差状态的更新方法和装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。
Claims (12)
1.一种车辆误差状态矩阵的更新方法，应用于自动驾驶，其特征在于，包括：
在接收到感知图像时，从预设导航地图中确定与所述感知图像中的目标图像元素存在匹配关系的目标地图元素；
基于所述目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将所述第一位置转换为在目标图像元素特征点的曝光时刻，基于所述相机坐标系的第二位置；
将转换得到的第二位置对应的目标地图元素投影到所述感知图像所在平面，并利用投影后的目标地图元素与所述目标图像元素之间的重投影残差，对车辆的误差状态矩阵进行更新；
其中，所述第一目标转换关系中包含有车辆的速度、误差状态量、惯性测量单元IMU采集的角速度，以及目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差，其中，所述误差状态矩阵是由各误差状态量形成的矩阵，所述各误差状态量包括：IMU在世界坐标系下的姿态、IMU坐标系相对于世界坐标系的速度、IMU在世界坐标系下的位置、IMU角速度零偏、IMU加速度零偏、IMU坐标系在相机坐标系下的姿态和IMU坐标系相对于相机坐标系的位置。
2.根据权利要求1所述的方法，其特征在于，利用投影后的目标地图元素与所述目标图像元素之间的重投影残差，对车辆的误差状态矩阵进行更新，包括：
计算投影后的目标地图元素与所述目标图像元素之间的重投影残差；
基于扩展卡尔曼滤波算法，根据所述重投影残差与所述误差状态量所形成的误差状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新；
其中，所述线性关系中的系数矩阵中包含有：利用所述第一目标转换关系中的第二位置对各误差状态变量求导后得到的雅克比矩阵。
3.根据权利要求2所述的方法，其特征在于，所述目标地图元素为路灯杆；
相应的，所述投影后的目标地图元素所对应的投影直线以参数方程的形式表示；
相应的，所述计算投影后的目标地图元素与所述目标图像元素之间的重投影残差，包括：
根据投影后的路灯杆所对应投影直线的参数方程和所述感知图像中对应的目标路灯杆端点的像素坐标，将所述目标路灯杆的端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
4.根据权利要求2所述的方法，其特征在于，所述目标地图元素为交通牌；
相应的，所述投影后的目标地图元素为：将所述交通牌的每条边投影到所述感知图像所在平面时得到的投影直线，其中，各条边对应的投影直线以参数方程的形式表示；
相应的，所述计算投影后的目标地图元素与所述目标图像元素之间的重投影残差，包括：
对于所述交通牌的任意一条边，根据该条边的端点在所述感知图像中的像素坐标和该条边投影后对应投影直线的参数方程，将该条边的端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
5.根据权利要求2所述的方法，其特征在于，所述目标地图元素为车道线曲线；
相应的，所述方法还包括：
根据投影后的车道线曲线对应各投影点的行数坐标，将所述感知图像中对应的车道线曲线划分为多个区间，其中，投影后的车道线曲线在每个区间内所对应的投影直线以参数方程的形式表示；
相应的，所述计算投影后的目标地图元素与所述目标图像元素之间的重投影残差，包括：
对于每一个区间，根据感知图像中车道线曲线端点的像素坐标和该区间内投影后的车道线直线对应的参数方程，将所述车道线曲线的端点到所述投影直线的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
6.根据权利要求2所述的方法，其特征在于，所述目标地图元素为车道线虚线段；
相应的，在转换为所述第二位置之前，所述方法还包括：
将预设导航地图中的车道线虚线段投影到所述感知图像所在平面，并确定投影后的车道线虚线段中，两个虚线端点到所述感知图像中对应端点的像素距离；
如果所述像素距离大于设定阈值，则将转换得到的第二位置对应的车道线虚线段投影到所述感知图像所在平面时，投影后的车道线虚线段通过投影直线的参数方程的形式表示；
相应的，所述计算投影后的目标地图元素与所述目标图像元素之间的重投影残差，包括：
根据所述感知图像中虚线段端点坐标和对应投影后的虚线段对应的参数方程，将感知图像中虚线段端点到所述参数方程之间的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第一矩阵、第二矩阵和所述雅克比矩阵的乘积；其中，所述第一矩阵和所述第二矩阵是利用所述重投影残差对所述参数方程和所述第二位置分别求导后得到的。
7.根据权利要求6所述的方法，其特征在于，所述方法还包括：
如果所述像素距离小于设定阈值，则基于所述车道线虚线段端点在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第二目标转换关系，将所述第一位置转换为在感知图像中对应车道线虚线段端点的曝光时刻，基于所述相机坐标系的第二位置，其中，所述第二目标转换关系中包含有车辆的速度、误差状态量、角速度以及所述车道线虚线段端点的曝光时刻与感知图像的中心行曝光时刻之间的时间差；
相应的，将转换得到的第二位置对应的车道线虚线段投影到所述感知图像所在平面时，投影后的车道线虚线段通过投影端点的像素坐标的形式表示；
相应的，所述计算投影后的目标地图元素与所述目标图像元素之间的重投影残差，包括：
根据感知图像中虚线段端点坐标和对应投影后的投影端点的像素坐标，将所述感知图像中车道线虚线段端点到对应投影端点的像素坐标的观测距离作为重投影残差；
其中，所述线性关系中的系数矩阵为第三矩阵和雅克比矩阵的乘积；其中，所述第三矩阵是利用所述重投影残差，对基于相机坐标系的车道线虚线段端点的第二位置进行求导后得到的。
8.根据权利要求2-7任一所述的方法，其特征在于，根据所述重投影残差与所述误差状态量所形成的误差状态矩阵之间的线性关系，对车辆的误差状态矩阵进行更新，包括：
按照如下公式，对车辆的误差状态矩阵进行更新：
其中，表示k+1时刻的重投影残差；/>表示从k时刻到k+1时刻的误差状态矩阵；R为感知图像的观测噪声参数；H表示系数矩阵。
9.根据权利要求2所述的方法，其特征在于，所述方法还包括：按照如下公式对车辆估计位姿对应的协方差矩阵进行更新：
P+＝(I-KH)P-
其中，P-表示更新前的协方差矩阵；P+表示更新后的协方差矩阵；I为单位矩阵；K表示卡尔曼增益，K＝P-HT(HP-HT+R)-1；H表示所述线性关系中的系数矩阵。
10.根据权利要求1-6任一所述的方法，其特征在于，所述第一目标转换关系为：
其中，Lc表示目标地图元素在相机坐标系下的直线参数；表示目标地图元素在世界坐标系下的直线坐标表示；Nc表示相机坐标系原点与目标地图元素的两个特征点构成的平面的法向量；Vc是目标地图元素基于相机坐标系的方向向量；Nw表示世界坐标系原点与目标地图元素的两个特征点构成的平面的法向量；VW表示目标地图元素基于世界坐标系的方向向量；/>和/>分别为图像元素到来时刻imu状态量的姿态和位置；/>和/>均为imu和相机之间的外参；ωi为imu测量的角速度；vi为车辆的速度；Δt为目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差。
11.根据权利要求7所述的方法，其特征在于，所述第二目标转换关系为：
其中，是车道线虚线段的端点在相机坐标系下的三维坐标，/>是车道线虚线段的端点在世界坐标系下的三维坐标；vi车辆的速度；Δt为车道线虚线段端点的曝光时刻与感知图像的中心行曝光时刻之间的时间差；/>和/>为车辆的误差状态量，/>和/>分别表示imu在世界坐标系和在相机坐标系下的旋转矩阵；/>表示imu坐标系相对于camera坐标系的位置。
12.一种车辆误差状态的更新装置，应用于自动驾驶，其特征在于，包括：
目标地图元素确定模块，被配置为在接收到感知图像时，从预设导航地图中确定与所述感知图像中的目标图像元素存在匹配关系的目标地图元素；
坐标系转换模块，被配置为基于所述目标地图元素在世界坐标系中的第一位置，与其在相机坐标系中位置之间的第一目标转换关系，将所述第一位置转换为在目标图像元素特征点的曝光时刻，基于所述相机坐标系的第二位置；
更新模块，被配置为将转换得到的所述第二位置对应的目标地图元素投影到所述感知图像所在平面，并利用投影后的目标地图元素与所述目标图像元素之间的重投影残差，对车辆的误差状态矩阵进行更新；
其中，所述第一目标转换关系中包含有车辆的速度、误差状态量、惯性测量单元IMU采集的角速度，以及目标图像元素特征点的曝光时刻与感知图像的中心行曝光时刻之间的时间差，其中，所述误差状态矩阵是由各误差状态量形成的矩阵，所述各误差状态量包括：IMU在世界坐标系下的姿态、IMU坐标系相对于世界坐标系的速度、IMU在世界坐标系下的位置、IMU角速度零偏、IMU加速度零偏、IMU坐标系在相机坐标系下的姿态和IMU坐标系相对于相机坐标系的位置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910723045.4A CN112347205B (zh) | 2019-08-06 | 2019-08-06 | 一种车辆误差状态的更新方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910723045.4A CN112347205B (zh) | 2019-08-06 | 2019-08-06 | 一种车辆误差状态的更新方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112347205A CN112347205A (zh) | 2021-02-09 |
| CN112347205B true CN112347205B (zh) | 2023-09-19 |
Family
ID=74367256
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910723045.4A Active CN112347205B (zh) | 2019-08-06 | 2019-08-06 | 一种车辆误差状态的更新方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112347205B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113884089B (zh) * | 2021-09-09 | 2023-08-01 | 武汉中海庭数据技术有限公司 | 一种基于曲线匹配的相机杆臂补偿方法及系统 |
| CN114140538B (zh) * | 2021-12-03 | 2022-09-27 | 禾多科技（北京）有限公司 | 车载相机位姿调整方法、装置、设备和计算机可读介质 |
| CN114659531B (zh) * | 2022-05-16 | 2022-09-23 | 苏州挚途科技有限公司 | 车辆的地图定位方法、装置和电子设备 |
| CN117490727B (zh) * | 2023-12-27 | 2024-03-29 | 合众新能源汽车股份有限公司 | 定位精度评价方法、装置和电子设备 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2003337947A (ja) * | 2002-05-21 | 2003-11-28 | Iwane Kenkyusho:Kk | 画像表示方法、画像表示装置、及び画像表示方法を記録した記憶媒体 |
| CN108765498A (zh) * | 2018-05-30 | 2018-11-06 | 百度在线网络技术（北京）有限公司 | 单目视觉跟踪方法、装置及存储介质 |
| CN109186592A (zh) * | 2018-08-31 | 2019-01-11 | 腾讯科技（深圳）有限公司 | 用于视觉惯导信息融合的方法和装置以及存储介质 |
| CN109729278A (zh) * | 2018-11-19 | 2019-05-07 | 魔门塔(苏州)科技有限公司 | 可设定各通道传输速率的远程多传感器同步接收装置 |
| CN110030994A (zh) * | 2019-03-21 | 2019-07-19 | 东南大学 | 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 |
-
2019
- 2019-08-06 CN CN201910723045.4A patent/CN112347205B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2003337947A (ja) * | 2002-05-21 | 2003-11-28 | Iwane Kenkyusho:Kk | 画像表示方法、画像表示装置、及び画像表示方法を記録した記憶媒体 |
| CN108765498A (zh) * | 2018-05-30 | 2018-11-06 | 百度在线网络技术（北京）有限公司 | 单目视觉跟踪方法、装置及存储介质 |
| CN109186592A (zh) * | 2018-08-31 | 2019-01-11 | 腾讯科技（深圳）有限公司 | 用于视觉惯导信息融合的方法和装置以及存储介质 |
| CN109729278A (zh) * | 2018-11-19 | 2019-05-07 | 魔门塔(苏州)科技有限公司 | 可设定各通道传输速率的远程多传感器同步接收装置 |
| CN110030994A (zh) * | 2019-03-21 | 2019-07-19 | 东南大学 | 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 |
Non-Patent Citations (1)
| Title |
|---|
| 刘康.大尺度视觉SLAM的光束平差算法研究.《中国优秀硕士学位论文全文数据库 信息科技辑》.2018,全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112347205A (zh) | 2021-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112347205B (zh) | 一种车辆误差状态的更新方法和装置 | |
| CN110567469B (zh) | 视觉定位方法、装置、电子设备及系统 | |
| CN110146099B (zh) | 一种基于深度学习的同步定位与地图构建方法 | |
| CN111076733B (zh) | 一种基于视觉与激光slam的机器人室内建图方法及系统 | |
| CN108489486B (zh) | 二维码以及用于机器人的视觉-惯性组合导航系统及方法 | |
| CN111932887B (zh) | 车道级轨迹数据的生成方法及设备 | |
| CN111912416B (zh) | 用于设备定位的方法、装置及设备 | |
| CN110570449B (zh) | 一种基于毫米波雷达与视觉slam的定位与建图方法 | |
| CN103761737A (zh) | 基于稠密光流的机器人运动估计方法 | |
| CN111207762B (zh) | 地图生成方法、装置、计算机设备和存储介质 | |
| CN115082549A (zh) | 位姿估计方法及装置、以及相关设备和存储介质 | |
| CN110827353B (zh) | 一种基于单目摄像头辅助的机器人定位方法 | |
| CN110766760B (zh) | 用于相机标定的方法、装置、设备和存储介质 | |
| CN113920198B (zh) | 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法 | |
| CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
| CN113223064B (zh) | 一种视觉惯性里程计尺度的估计方法和装置 | |
| CN112446915B (zh) | 一种基于图像组的建图方法及装置 | |
| CN112254728A (zh) | 基于关键路标增强ekf-slam全局优化的方法 | |
| CN112577479A (zh) | 基于地图元素数据的多传感器融合车辆定位方法及装置 | |
| CN112862818B (zh) | 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法 | |
| CN114529585A (zh) | 基于深度视觉和惯性测量的移动设备自主定位方法 | |
| CN111862146B (zh) | 一种目标对象的定位方法及装置 | |
| WO2021063756A1 (en) | Improved trajectory estimation based on ground truth | |
| JP2018116147A (ja) | 地図作成装置、地図作成方法及び地図作成用コンピュータプログラム | |
| CN112530270A (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 | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20220228 Address after: 100083 unit 501, block AB, Dongsheng building, No. 8, Zhongguancun East Road, Haidian District, Beijing Applicant after: BEIJING MOMENTA TECHNOLOGY Co.,Ltd. Address before: 100083 room 28, 4 / F, block a, Dongsheng building, 8 Zhongguancun East Road, Haidian District, Beijing Applicant before: BEIJING CHUSUDU TECHNOLOGY Co.,Ltd. |
|
| TA01 | Transfer of patent application right | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |