CN112683281A - 一种基于车辆运动学的自动驾驶车辆联合定位方法 - Google Patents

一种基于车辆运动学的自动驾驶车辆联合定位方法 Download PDF

Info

Publication number
CN112683281A
CN112683281A CN202110263051.3A CN202110263051A CN112683281A CN 112683281 A CN112683281 A CN 112683281A CN 202110263051 A CN202110263051 A CN 202110263051A CN 112683281 A CN112683281 A CN 112683281A
Authority
CN
China
Prior art keywords
vehicle
imu
information
kinematics
constraint
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
CN202110263051.3A
Other languages
English (en)
Other versions
CN112683281B (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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202110263051.3A priority Critical patent/CN112683281B/zh
Publication of CN112683281A publication Critical patent/CN112683281A/zh
Application granted granted Critical
Publication of CN112683281B publication Critical patent/CN112683281B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Navigation (AREA)
  • Image Analysis (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

本发明公开一种基于车辆运动学的自动驾驶车辆联合定位方法，该方法利用双目相机及IMU获取环境及车辆运动信息进行车辆状态估计，借助车辆总线获取车辆侧偏角构建运动约束，该方法通过引入车辆本身的运动学模型，将其作为车辆在利用双目视觉和IMU惯性测量单元进行定位过程中本身运动的约束，构建车辆位姿状态估计的优化模型，迭代求解获得符合车辆运动学模型的车辆定位结果，该方法在定位优化过程中借助于运动学约束，将车辆运动学与状态估计的优化模型相结合，得到适应车辆自身运动的定位轨迹信息，有效提高了视觉惯性联合定位优化模型本身的鲁棒性和精确性，适用于自动驾驶车辆在复杂环境下的定位问题。

Description

一种基于车辆运动学的自动驾驶车辆联合定位方法
技术领域
本发明涉及自动驾驶车辆定位领域，具体是一种基于车辆运动学的自动驾驶车辆联合定位方法。
背景技术
随着国家发改委《智能汽车创新发展战略》的出台，在政府力量和社会资本的不断推动下，自动驾驶领域得到如火如荼的发展。在高级辅助驾驶不断得到落地的今天，高等级自动驾驶实现在产业界和高校的努力下也逐渐看到了曙光。越来越多的企业如百度、阿里、文远知行等也在不断推出自己的自动驾驶产品。但是完全自动驾驶距离走进千家万户仍有诸多的障碍。为了实现完全自动驾驶，定位感知规划控制高精度地图五大模块缺一不可，在这其中，定位模块作为自动驾驶车辆的位置信息来源，对自动驾驶的实现起着基础性作用。
当前自动驾驶车辆定位的实现强依赖于GNSS组合导航技术，车辆通过获取经过RTK基站解算的定位信息实现高精度定位，但是在隧道桥梁等GNSS信号弱或消失环境下应用效果不佳。因此，基于各种传感器的车辆定位技术逐渐得到重视，如通过摄像头、IMU等设备结合高精度地图完成车辆定位。虽然视觉惯性联合定位技术已经在学术界有了较多的研究，但是由于其鲁棒性差和精确度不高的问题，其并没有在车辆定位中得到实际落地。并且目前已有的视觉惯性联合定位技术过于通用化，并没有有效利用车辆本身的运动特性对其进行优化，这也是限制视觉惯性联合定位在车辆应用的原因所在。因此，如何有效的将车辆本身运动特性应用于视觉惯性联合定位以增强车辆定位的鲁棒性和精确性成为亟待解决的技术问题。
发明内容
针对上述现有技术存在的问题，本发明提供一种基于车辆运动学的自动驾驶车辆联合定位方法，可以有效的解决上述问题。
为了实现上述目的，本发明通过以下技术方案实现：一种基于车辆运动学的自动驾驶车辆联合定位方法，所述自动驾驶车辆上搭载有双目相机和IMU惯性测量单元，所述自动驾驶车辆借助于车辆总线完成车辆内部通讯；
所述基于车辆运动学的自动驾驶车辆联合定位方法包括以下步骤：
（1）传感器采集获取自动驾驶车辆当前信息，包括通过双目相机获得车辆前方图像信息、通过IMU惯性测量单元获得车辆当前速度状态信息、通过车辆总线获取车辆侧偏角信息；
（2）车辆利用步骤（1）所述的车辆前方图像信息和所述车辆当前速度状态信息进行车辆的状态估计，获得当前车辆的位姿信息；
（3）利用步骤（2）所述的位姿信息，结合步骤（1）所述的车辆侧偏角信息建立运动学模型，构建运动学约束下的车辆位姿优化模型完成车辆定位；所述的优化模型为基于相机测量误差、IMU测量误差构建的误差函数，所述的运动学约束为自动驾驶车辆运动所需要满足的单车运动模型。
优选的，所述步骤（1）中，传感器采集获取自动驾驶车辆当前信息的具体过程如下：
传感器利用双目相机的左右相机同时采集获取车辆前方图像信息，并利用相机标定所述车辆前方图像信息的内外参数、利用单应矩阵对原始图像进行校正，使得图像处于同一平面且互相平行，再对像素点匹配从而获得每个点的深度，得到当前图像帧信息
Figure 100002_DEST_PATH_IMAGE001
；
车辆利用IMU 惯性测量单元采集车辆当前的速度状态信息
Figure 378219DEST_PATH_IMAGE002
，即车辆线加速度
Figure 100002_DEST_PATH_IMAGE003
和角速度
Figure 365767DEST_PATH_IMAGE004
，其中
Figure 590075DEST_PATH_IMAGE003
表示i时刻车辆的加速度信息，
Figure 651571DEST_PATH_IMAGE004
表示i时刻车辆角速度信息；
利用车辆总线获取车辆当前时刻的车轮侧偏角信息，其
Figure 100002_DEST_PATH_IMAGE005
，其中
Figure 529398DEST_PATH_IMAGE006
和
Figure 100002_DEST_PATH_IMAGE007
分别表示车辆前轮和后轮的侧偏角。
优选的，所述步骤（2）中，获得当前车辆的位姿信息的具体过程如下：
（2.1）利用FAST算法对双目相机采集到的图像帧进行检测，提取图像帧的角点作为图像特征点，利用LK光流算法追踪所述图像特征点，利用ICP算法处理所述当前图像帧信息
Figure 473083DEST_PATH_IMAGE008
，以此来恢复相机的帧间运动。
（2.2）利用所述车辆当前速度状态信息
Figure 100002_DEST_PATH_IMAGE009
，采用IMU预积分模型对车辆的运动状态进行估计，得到车辆预积分模型如下：
Figure 32240DEST_PATH_IMAGE010
；
Figure 100002_DEST_PATH_IMAGE011
；
Figure 213823DEST_PATH_IMAGE012
；
其中，
Figure 100002_DEST_PATH_IMAGE013
表示IMU坐标系下车辆从i时刻到j时刻的位移距离；
Figure 669075DEST_PATH_IMAGE014
表示IMU坐标系下车辆从i时刻到j时刻的速度变化；
Figure 100002_DEST_PATH_IMAGE015
表示IMU坐标系下从i时刻到j时刻的旋转量，
Figure 427952DEST_PATH_IMAGE016
表示IMU坐标系下从i时刻到t时刻的旋转量；
Figure 994063DEST_PATH_IMAGE017
表示t时刻下的IMU坐标系，通过IMU惯性测量单元得到旋转矩阵
Figure 100002_DEST_PATH_IMAGE018
和平移矩阵
Figure 561310DEST_PATH_IMAGE019
。
优选的，所述步骤（3）中，基于相机测量误差、IMU测量误差构建误差函数的具体过程如下：
相机测量误差定义为相机坐标系下在归一化平面上特征点坐标预测值与特征点坐标测量值之间的偏差，特征点m在第i帧的相机测量误差具体定义如下：
Figure 100002_DEST_PATH_IMAGE020
；
IMU测量误差，即IMU坐标系下时刻i与时刻j之间的预积分误差定义为：
Figure 921885DEST_PATH_IMAGE021
；
其中，
Figure 100002_DEST_PATH_IMAGE022
为利用IMU测量值进行两次积分后所获得的车辆位移，其中
Figure 371321DEST_PATH_IMAGE023
表示世界坐标系到IMU坐标系的转换；
Figure 100002_DEST_PATH_IMAGE024
为通过对IMU进行积分获得的车辆速度；
Figure 537860DEST_PATH_IMAGE025
表示利用IMU获得的车辆旋转信息。
优选的，所述步骤（3）中，所述运动学约束的具体过程如下：
将车辆运动学模型加入位姿优化的模型当中，将车辆简化为单车运动模型，即车辆满足的运动关系为：
Figure 100002_DEST_PATH_IMAGE026
；
其中，
Figure 428455DEST_PATH_IMAGE027
，
Figure 100002_DEST_PATH_IMAGE028
分别表示车辆在世界坐标系下沿X轴、Y轴方向的速度；
Figure 287827DEST_PATH_IMAGE029
表示世界坐标系下单位时间内车辆航向角的变化量，即车辆的旋转角速度；
Figure 100002_DEST_PATH_IMAGE030
为车辆的滑移角，
Figure 896663DEST_PATH_IMAGE031
；
Figure 100002_DEST_PATH_IMAGE032
，
Figure 70155DEST_PATH_IMAGE033
分别为前轮偏角和后轮偏角；
Figure 100002_DEST_PATH_IMAGE034
，
Figure 80836DEST_PATH_IMAGE035
分别为前悬长度和后悬长度；当车辆为前轮驱动时，假设
Figure 111109DEST_PATH_IMAGE033
恒为0；低速条件下，时间内认为车辆匀速转向，即
Figure 100002_DEST_PATH_IMAGE036
，则车辆的运动模型可简化为：
Figure 269558DEST_PATH_IMAGE037
；
因此，当前时刻i车辆的旋转矩阵
Figure 100002_DEST_PATH_IMAGE038
和平移矩阵
Figure 981162DEST_PATH_IMAGE039
可表示为：
Figure 100002_DEST_PATH_IMAGE040
；
Figure 846350DEST_PATH_IMAGE041
；
由上易得，车辆满足如下约束：
Figure 100002_DEST_PATH_IMAGE042
；
其中，
Figure 985207DEST_PATH_IMAGE043
，
Figure 100002_DEST_PATH_IMAGE044
，
Figure 630952DEST_PATH_IMAGE045
表示i时刻的车辆滑移角；
并且由于受到车辆自身结构限制，车辆前轮侧偏角满足约束
Figure 100002_DEST_PATH_IMAGE046
；由此构建了车辆在视觉惯性联合定位当中的旋转和平移之间的约束。
优选的，所述步骤（3）中，构建运动学约束下的车辆位姿优化模型的具体过程如下：
针对所述相机测量误差，IMU测量误差和车辆运动学约束，为了得到车辆最佳的位姿估计，利用所述相测量像误差和IMU测量误差建立位姿优化模型，在运动学约束下最小化位姿估计误差，将上述约束优化问题转换为无约束优化，问题形式化为：
Figure 880668DEST_PATH_IMAGE047
；
其中，
Figure 100002_DEST_PATH_IMAGE048
为特征点m在第i帧的测量误差，
Figure 865942DEST_PATH_IMAGE049
为IMU测量误差，
Figure 100002_DEST_PATH_IMAGE050
为等式约束条件，
Figure 175700DEST_PATH_IMAGE051
为对应的约束系数，
Figure 100002_DEST_PATH_IMAGE052
为不等式约束条件，
Figure 308741DEST_PATH_IMAGE053
为对应的约束系数。
本发明的有益效果为：本方法通过引入借助于车辆侧偏角建立的车辆自身运动学约束，将其与视觉惯性联合定位所获得的位姿优化模型相结合，所得到的位姿优化结果与车辆自身运动学相匹配，大幅度提高位姿优化结果在车辆执行方面的可行性；同时，通过考虑车辆自身运动约束，在位姿优化模型求解过程中也有效提高了视觉惯性联合定位的鲁棒性和精确度。
附图说明
图1为本发明的自动驾驶车辆联合定位方法的流程图；
图2为本发明的自动驾驶车辆联合定位方法的定位优化模型构建示意图；
图3为本发明的自动驾驶车辆联合定位方法的车辆运动学模型示意图。
具体实施方式
下面结合附图对本发明做进一步说明。
如图1至图3所示，一种基于车辆运动学的自动驾驶车辆联合定位方法，所述自动驾驶车辆上搭载有双目相机和IMU惯性测量单元，所述自动驾驶车辆借助于车辆总线完成车辆内部通讯；
所述基于车辆运动学的自动驾驶车辆联合定位方法包括以下步骤：
（1）传感器采集获取自动驾驶车辆当前信息，包括通过双目相机获得车辆前方图像信息、通过IMU惯性测量单元获得车辆当前速度状态信息、通过车辆总线获取车辆侧偏角信息；
（2）车辆利用所述的车辆前方图像信息和所述车辆当前速度状态信息进行车辆的状态估计，获得当前车辆的位姿信息；
（3）利用步骤（2）所述的位姿信息，结合步骤（1）所述的车辆侧偏角信息建立运动学模型，构建运动学约束下的车辆位姿优化模型完成车辆定位；所述的优化模型为基于相机测量误差、IMU测量误差构建的误差函数，所述的运动学约束为自动驾驶车辆运动所需要满足的单车运动模型。
所述步骤（1）中，传感器采集获取自动驾驶车辆当前信息的具体过程如下：
传感器利用双目相机的左右相机同时采集获取车辆前方图像信息，并利用相机标定所述车辆前方图像信息的内外参数、利用单应矩阵对原始图像进行校正，使得图像处于同一平面且互相平行，再对像素点进行匹配从而获得每个点的深度，得到当前图像帧信息
Figure 362148DEST_PATH_IMAGE001
；
车辆利用IMU 惯性测量单元采集车辆当前的速度状态信息
Figure 100002_DEST_PATH_IMAGE054
，即车辆线加速度
Figure 201928DEST_PATH_IMAGE003
和角速度
Figure 682588DEST_PATH_IMAGE004
，利用车辆总线获取车辆当前时刻的车轮侧偏角信息
Figure 975029DEST_PATH_IMAGE055
，其中
Figure DEST_PATH_IMAGE056
和
Figure 894444DEST_PATH_IMAGE007
分别表示车辆前轮和后轮的侧偏角。
所述步骤（2）中，获得当前车辆的位姿信息的具体过程如下：
（2.1）利用FAST算法对双目相机采集到的图像帧进行检测，提取图像帧的角点作为图像特征点。所采用的图像帧不限于左目或右目相机，若提取到的特征点数量不符合要求，则重新提取；
利用LK光流算法追踪所述图像特征点，基于灰度不变假设，同一窗口中像素具有相同的运动，确定帧间图像特征点
Figure 588730DEST_PATH_IMAGE057
，
Figure DEST_PATH_IMAGE058
的匹配关系；
利用ICP算法处理所述当前图像帧信息
Figure 240291DEST_PATH_IMAGE059
，以此来恢复相机的帧间运动，具体处理过程如下：
利用ICP算法对相邻两帧提取得到的匹配特征点
Figure 285608DEST_PATH_IMAGE057
，
Figure 680817DEST_PATH_IMAGE058
进行计算，首先计算两组点云的质心位置
Figure DEST_PATH_IMAGE060
，
Figure 291927DEST_PATH_IMAGE061
，进而得到每个点的去质心坐标：
Figure DEST_PATH_IMAGE062
，
定义W矩阵为
Figure 379969DEST_PATH_IMAGE063
，对其进行SVD分解，即
Figure DEST_PATH_IMAGE064
，则求解得到旋转矩阵
Figure 647002DEST_PATH_IMAGE065
和平移矩阵
Figure DEST_PATH_IMAGE066
为
Figure 908219DEST_PATH_IMAGE067
，
对i，j相邻两帧点云进行迭代计算，使误差
Figure DEST_PATH_IMAGE068
小于预定值，得到最终的旋转矩阵
Figure 311518DEST_PATH_IMAGE065
和平移矩阵
Figure 570461DEST_PATH_IMAGE066
。
（2.2）利用所述车辆当前速度状态信息
Figure 324791DEST_PATH_IMAGE069
，采用IMU预积分模型对车辆的运动状态进行估计，得到车辆预积分模型如下：
Figure 61803DEST_PATH_IMAGE010
；
Figure 647505DEST_PATH_IMAGE011
；
Figure DEST_PATH_IMAGE070
；
其中，
Figure 77349DEST_PATH_IMAGE013
表示IMU坐标系下车辆从i时刻到j时刻的位移距离；
Figure 53395DEST_PATH_IMAGE014
表示IMU坐标系下车辆从i时刻到j时刻的速度变化；
Figure 594098DEST_PATH_IMAGE015
表示IMU坐标系下从i时刻到j时刻的旋转量；
Figure 237569DEST_PATH_IMAGE071
表示IMU坐标系下从i时刻到t时刻的旋转量，
Figure 572735DEST_PATH_IMAGE017
表示t时刻下的IMU坐标系，通过IMU惯性测量单元得到旋转矩阵
Figure 363974DEST_PATH_IMAGE018
和平移矩阵
Figure DEST_PATH_IMAGE072
。
所述步骤（3）中，基于相机测量误差、IMU测量误差构建误差函数的具体过程如下：
相机测量误差定义为相机坐标系下在归一化平面上特征点坐标预测值与特征点坐标测量值之间的偏差，特征点m在第i帧的相机测量误差具体定义如下：
Figure 442788DEST_PATH_IMAGE020
；
IMU测量误差，即IMU坐标系下时刻i与时刻j之间的预积分误差定义为：
Figure 940766DEST_PATH_IMAGE021
；
其中，
Figure 446833DEST_PATH_IMAGE022
为利用IMU测量值进行两次积分后所获得的车辆位移，其中
Figure 663051DEST_PATH_IMAGE023
表示世界坐标系到IMU坐标系的转换；
Figure 545557DEST_PATH_IMAGE024
为通过对IMU进行积分获得的车辆速度；
Figure 960357DEST_PATH_IMAGE025
表示利用IMU获得的车辆旋转信息。
所述步骤（3）中，所述运动学约束的具体过程如下：
将车辆运动学模型加入位姿优化的模型当中，将车辆简化为单车运动模型，即车辆满足的运动关系为：
Figure 902906DEST_PATH_IMAGE026
；
其中，
Figure 340840DEST_PATH_IMAGE027
，
Figure 761457DEST_PATH_IMAGE028
分别表示车辆在世界坐标系下沿X轴、Y轴方向的速度；
Figure 234027DEST_PATH_IMAGE029
表示世界坐标系下单位时间内车辆航向角的变化量，即车辆的旋转角速度；
Figure 81897DEST_PATH_IMAGE030
为车辆的滑移角，
Figure 7128DEST_PATH_IMAGE031
；
Figure 293753DEST_PATH_IMAGE032
，
Figure 620829DEST_PATH_IMAGE033
分别为前轮偏角和后轮偏角；
Figure 905180DEST_PATH_IMAGE034
，
Figure 317706DEST_PATH_IMAGE035
分别为前悬长度和后悬长度；当车辆为前轮驱动时，假设
Figure 80126DEST_PATH_IMAGE033
恒为0；低速条件下，时间内认为车辆匀速转向，即
Figure 261709DEST_PATH_IMAGE036
，则车辆的运动模型可简化为：
Figure 716961DEST_PATH_IMAGE037
；
因此，当前时刻i车辆的旋转矩阵
Figure 679101DEST_PATH_IMAGE038
和平移矩阵
Figure 245211DEST_PATH_IMAGE039
可表示为：
Figure 281300DEST_PATH_IMAGE040
；
Figure 907454DEST_PATH_IMAGE041
；
由上易得，车辆满足如下约束：
Figure 294573DEST_PATH_IMAGE042
；
其中，
Figure 398795DEST_PATH_IMAGE043
，
Figure 554970DEST_PATH_IMAGE044
，
Figure 414341DEST_PATH_IMAGE045
表示i时刻的车辆滑移角；
并且由于受到车辆自身结构限制，车辆前轮侧偏角满足约束
Figure 23177DEST_PATH_IMAGE046
；由此构建了车辆在视觉惯性联合定位当中的旋转和平移之间的约束。
所述步骤（3）中，构建运动学约束下的车辆位姿优化模型的具体过程如下：
针对所述相机测量误差，IMU测量误差和车辆运动学约束，为了得到车辆最佳的位姿估计，需要利用所述相机测量误差和IMU测量误差建立位姿优化模型，在运动学约束下最小化位姿估计误差为：
Figure 196670DEST_PATH_IMAGE073
；
Figure DEST_PATH_IMAGE074
；
Figure 207351DEST_PATH_IMAGE075
；
将上述约束优化问题转换为无约束优化，问题形式化为：
Figure DEST_PATH_IMAGE076
；
其中，
Figure 175307DEST_PATH_IMAGE048
为特征点m在第i帧的测量误差，
Figure 333756DEST_PATH_IMAGE049
为IMU预积分误差，
Figure 779781DEST_PATH_IMAGE077
为等式约束条件，
Figure 910548DEST_PATH_IMAGE051
为对应的约束系数，
Figure 49405DEST_PATH_IMAGE052
为不等式约束条件，
Figure 632833DEST_PATH_IMAGE053
为对应的约束系数。利用Gaussian-Newton法对该无约束优化问题进行迭代求解，得到符合车辆运动学的位姿优化结果。
通过引入借助于车辆侧偏角建立的车辆自身运动学约束，将其与视觉惯性联合定位所获得的位姿优化模型相结合，所得到的位姿优化结果与车辆自身运动学相匹配，大幅度提高位姿优化结果在车辆执行方面的可行性；同时，通过考虑车辆自身运动约束，在位姿优化模型求解过程中也有效提高了视觉惯性联合定位的鲁棒性和精确度。

Claims (6)

1.一种基于车辆运动学的自动驾驶车辆联合定位方法，其特征在于，所述自动驾驶车辆上搭载有双目相机和IMU惯性测量单元，所述自动驾驶车辆借助于车辆总线完成车辆内部通讯；
所述基于车辆运动学的自动驾驶车辆联合定位方法包括以下步骤：
（1）传感器采集获取自动驾驶车辆当前信息，包括通过双目相机获得车辆前方图像信息、通过IMU惯性测量单元获得车辆当前速度状态信息、通过车辆总线获取车辆侧偏角信息；
（2）车辆利用步骤（1）所述的车辆前方图像信息和所述车辆当前速度状态信息进行车辆的状态估计，获得当前车辆的位姿信息；
（3）利用步骤（2）所述的位姿信息，结合步骤（1）所述的车辆侧偏角信息建立运动学模型，构建运动学约束下的车辆位姿优化模型完成车辆定位；所述的优化模型为基于相机测量误差、IMU测量误差构建的误差函数，所述的运动学约束为自动驾驶车辆运动所需要满足的单车运动模型。
2.根据权利要求1所述的基于车辆运动学的自动驾驶车辆联合定位方法，其特征在于，所述步骤（1）中，传感器采集获取自动驾驶车辆当前信息的具体过程如下：
传感器利用双目相机的左右相机同时采集获取车辆前方图像信息，并利用相机标定所述车辆前方图像信息的内外参数、利用单应矩阵对原始图像进行校正，使得图像处于同一平面且互相平行，再对像素点进行匹配从而获得每个点的深度，得到当前图像帧信息
Figure DEST_PATH_IMAGE001
；
车辆利用IMU 惯性测量单元采集车辆当前的速度状态信息
Figure 501873DEST_PATH_IMAGE002
，即车辆线加速度
Figure DEST_PATH_IMAGE003
和角速度
Figure 419014DEST_PATH_IMAGE004
，其中
Figure 261068DEST_PATH_IMAGE003
表示i时刻车辆的加速度信息，
Figure 656277DEST_PATH_IMAGE004
表示i时刻车辆角速度信息；
利用车辆总线获取车辆当前时刻的车轮侧偏角信息
Figure DEST_PATH_IMAGE005
，其中
Figure 470649DEST_PATH_IMAGE006
和
Figure DEST_PATH_IMAGE007
分别表示车辆前轮和后轮的侧偏角。
3.根据权利要求2所述的基于车辆运动学的自动驾驶车辆联合定位方法，其特征在于，所述步骤（2）中，获得当前车辆的位姿信息的具体过程如下：
（2.1）利用FAST算法对双目相机采集到的图像帧进行检测，提取图像帧的角点作为图像特征点，利用LK光流算法追踪所述图像特征点，利用ICP算法处理所述当前图像帧信息
Figure 293112DEST_PATH_IMAGE008
，以此来恢复相机的帧间运动；
（2.2）利用所述车辆当前速度状态信息
Figure DEST_PATH_IMAGE009
，采用IMU预积分模型对车辆的运动状态进行估计，得到车辆预积分模型如下：
Figure 622462DEST_PATH_IMAGE010
Figure DEST_PATH_IMAGE011
Figure 821362DEST_PATH_IMAGE012
其中，
Figure DEST_PATH_IMAGE013
表示IMU坐标系下车辆从i时刻到j时刻的位移距离；
Figure 490241DEST_PATH_IMAGE014
表示IMU坐标系下车辆从i时刻到j时刻的速度变化；
Figure DEST_PATH_IMAGE015
表示IMU坐标系下从i时刻到j时刻的旋转量；
Figure 483605DEST_PATH_IMAGE016
表示IMU坐标系下从i时刻到t时刻的旋转量；
Figure 300251DEST_PATH_IMAGE017
表示t时刻下的IMU坐标系，通过IMU惯性测量单元得到旋转矩阵
Figure DEST_PATH_IMAGE018
和平移矩阵
Figure 37263DEST_PATH_IMAGE019
。
4.根据权利要求1所述的基于车辆运动学的自动驾驶车辆联合定位方法，其特征在于，所述步骤（3）中，基于相机测量误差、IMU测量误差构建误差函数的具体过程如下：
相机测量误差定义为相机坐标系下在归一化平面上特征点坐标预测值与特征点坐标测量值之间的偏差，特征点m在第i帧的相机测量误差具体定义如下：
Figure DEST_PATH_IMAGE020
IMU测量误差，即IMU坐标系下时刻i与时刻j之间的预积分误差定义为：
Figure 560648DEST_PATH_IMAGE021
其中，
Figure DEST_PATH_IMAGE022
为利用IMU测量值进行两次积分后所获得的车辆位移，其中
Figure 990492DEST_PATH_IMAGE023
表示世界坐标系到IMU坐标系的转换；
Figure DEST_PATH_IMAGE024
为通过对IMU进行积分获得的车辆速度；
Figure 294435DEST_PATH_IMAGE025
表示利用IMU获得的车辆旋转信息。
5.根据权利要求1所述的基于车辆运动学的自动驾驶车辆联合定位方法，其特征在于，所述步骤（3）中，所述运动学约束的具体过程如下：
将车辆运动学模型加入位姿优化的模型当中，将车辆简化为单车运动模型，即车辆满足的运动关系为：
Figure DEST_PATH_IMAGE026
其中，
Figure DEST_PATH_IMAGE027
，
Figure DEST_PATH_IMAGE028
分别表示车辆在世界坐标系下沿X轴、Y轴方向的速度；
Figure 569558DEST_PATH_IMAGE029
表示世界坐标系下单位时间内车辆航向角的变化量，即车辆的旋转角速度；
Figure DEST_PATH_IMAGE030
为车辆的滑移角，
Figure 275346DEST_PATH_IMAGE031
；
Figure DEST_PATH_IMAGE032
，
Figure 610512DEST_PATH_IMAGE033
分别为前轮偏角和后轮偏角；
Figure DEST_PATH_IMAGE034
，
Figure 339434DEST_PATH_IMAGE035
分别为前悬长度和后悬长度；当车辆为前轮驱动时，假设
Figure 418248DEST_PATH_IMAGE033
恒为0；低速条件下，
Figure DEST_PATH_IMAGE036
时间内认为车辆匀速转向，即
Figure 978543DEST_PATH_IMAGE037
，则车辆的运动模型可简化为：
Figure DEST_PATH_IMAGE038
因此，当前时刻i车辆的旋转矩阵
Figure 750190DEST_PATH_IMAGE039
和平移矩阵
Figure DEST_PATH_IMAGE040
可表示为
Figure 700828DEST_PATH_IMAGE041
Figure DEST_PATH_IMAGE042
由上易得，车辆满足如下约束：
Figure 583333DEST_PATH_IMAGE043
；
其中，
Figure DEST_PATH_IMAGE044
，
Figure 998134DEST_PATH_IMAGE045
，
Figure DEST_PATH_IMAGE046
表示i时刻的车辆滑移角；
并且由于受到车辆自身结构限制，车辆前轮侧偏角满足约束
Figure 940683DEST_PATH_IMAGE047
；由此构建了车辆在视觉惯性联合定位当中的旋转和平移之间的约束。
6.根据权利要求5所述的基于车辆运动学的自动驾驶车辆联合定位方法，其特征在于，所述步骤（3）中，构建运动学约束下的车辆位姿优化模型的具体过程如下：
针对所述相机测量误差，IMU测量误差和车辆运动学约束，为了得到车辆最佳的位姿估计，利用所述相机测量误差和IMU测量误差建立位姿优化模型，在运动学约束下最小化位姿估计误差，将上述约束优化问题转换为无约束优化，问题形式化为：
Figure DEST_PATH_IMAGE048
其中，
Figure 378617DEST_PATH_IMAGE049
为特征点m在第i帧的测量误差，
Figure DEST_PATH_IMAGE050
为IMU测量误差，
Figure 861551DEST_PATH_IMAGE051
为等式约束条件，
Figure DEST_PATH_IMAGE052
为对应的约束系数，
Figure 334121DEST_PATH_IMAGE053
为不等式约束条件，
Figure DEST_PATH_IMAGE054
为对应的约束系数。
CN202110263051.3A 2021-03-11 2021-03-11 一种基于车辆运动学的自动驾驶车辆联合定位方法 Active CN112683281B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110263051.3A CN112683281B (zh) 2021-03-11 2021-03-11 一种基于车辆运动学的自动驾驶车辆联合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110263051.3A CN112683281B (zh) 2021-03-11 2021-03-11 一种基于车辆运动学的自动驾驶车辆联合定位方法

Publications (2)

Publication Number Publication Date
CN112683281A true CN112683281A (zh) 2021-04-20
CN112683281B CN112683281B (zh) 2021-06-04

Family

ID=75458341

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110263051.3A Active CN112683281B (zh) 2021-03-11 2021-03-11 一种基于车辆运动学的自动驾驶车辆联合定位方法

Country Status (1)

Country Link
CN (1) CN112683281B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113156963A (zh) * 2021-04-29 2021-07-23 重庆大学 基于监督信号引导的深度强化学习自动驾驶汽车控制方法
CN113503873A (zh) * 2021-07-14 2021-10-15 北京理工大学 一种多传感器融合的视觉定位方法
CN113946151A (zh) * 2021-10-20 2022-01-18 北京百度网讯科技有限公司 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN114323033A (zh) * 2021-12-29 2022-04-12 北京百度网讯科技有限公司 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN116664698A (zh) * 2023-07-25 2023-08-29 江苏集萃清联智控科技有限公司 车载双目相机与gnss/imu的自动标定方法
CN117553819A (zh) * 2024-01-10 2024-02-13 齐鲁空天信息研究院 无人叉车室外装卸路径规划方法及装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20030191568A1 (en) * 2002-04-09 2003-10-09 Breed David S. Method and system for controlling a vehicle
CN101099076A (zh) * 2004-12-07 2008-01-02 萨基姆防务安全公司 基于运动学模型的组合惯性导航系统
CN102954783A (zh) * 2012-11-05 2013-03-06 清华大学 基于全球定位系统数据采集的车身侧偏角计算方法
CN102999759A (zh) * 2012-11-07 2013-03-27 东南大学 一种基于光流的车辆运动状态估计方法
CN106256606A (zh) * 2016-08-09 2016-12-28 浙江零跑科技有限公司 一种基于车载双目相机的车道偏离预警方法
CN108426581A (zh) * 2018-01-08 2018-08-21 深圳市易成自动驾驶技术有限公司 车辆位姿确定方法、装置及计算机可读存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20030191568A1 (en) * 2002-04-09 2003-10-09 Breed David S. Method and system for controlling a vehicle
CN101099076A (zh) * 2004-12-07 2008-01-02 萨基姆防务安全公司 基于运动学模型的组合惯性导航系统
CN102954783A (zh) * 2012-11-05 2013-03-06 清华大学 基于全球定位系统数据采集的车身侧偏角计算方法
CN102999759A (zh) * 2012-11-07 2013-03-27 东南大学 一种基于光流的车辆运动状态估计方法
CN106256606A (zh) * 2016-08-09 2016-12-28 浙江零跑科技有限公司 一种基于车载双目相机的车道偏离预警方法
CN108426581A (zh) * 2018-01-08 2018-08-21 深圳市易成自动驾驶技术有限公司 车辆位姿确定方法、装置及计算机可读存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
冯祎 等: "双目视觉辅助GNSS在恶劣环境下导航定位", 《全球定位系统》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113156963A (zh) * 2021-04-29 2021-07-23 重庆大学 基于监督信号引导的深度强化学习自动驾驶汽车控制方法
CN113503873A (zh) * 2021-07-14 2021-10-15 北京理工大学 一种多传感器融合的视觉定位方法
CN113503873B (zh) * 2021-07-14 2024-03-12 北京理工大学 一种多传感器融合的视觉定位方法
CN113946151A (zh) * 2021-10-20 2022-01-18 北京百度网讯科技有限公司 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN114323033A (zh) * 2021-12-29 2022-04-12 北京百度网讯科技有限公司 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN114323033B (zh) * 2021-12-29 2023-08-29 北京百度网讯科技有限公司 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN116664698A (zh) * 2023-07-25 2023-08-29 江苏集萃清联智控科技有限公司 车载双目相机与gnss/imu的自动标定方法
CN116664698B (zh) * 2023-07-25 2023-12-22 江苏集萃清联智控科技有限公司 车载双目相机与gnss/imu的自动标定方法
CN117553819A (zh) * 2024-01-10 2024-02-13 齐鲁空天信息研究院 无人叉车室外装卸路径规划方法及装置

Also Published As

Publication number Publication date
CN112683281B (zh) 2021-06-04

Similar Documents

Publication Publication Date Title
CN112683281B (zh) 一种基于车辆运动学的自动驾驶车辆联合定位方法
EP3940421A1 (en) Positioning method and device based on multi-sensor fusion
CN109029417B (zh) 基于混合视觉里程计和多尺度地图的无人机slam方法
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109946732B (zh) 一种基于多传感器数据融合的无人车定位方法
US10867409B2 (en) Methods and systems to compensate for vehicle calibration errors
CN109676604B (zh) 机器人曲面运动定位方法及其运动定位系统
CN109885080B (zh) 自主控制系统及自主控制方法
CN110849362B (zh) 一种基于车载惯性的激光雷达与视觉组合导航算法
US8213706B2 (en) Method and system for real-time visual odometry
CN107941217B (zh) 一种机器人定位方法、电子设备、存储介质、装置
CN111595333A (zh) 视觉惯性激光数据融合的模块化无人车定位方法及系统
US20190033867A1 (en) Systems and methods for determining a vehicle position
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
CN111065043B (zh) 一种基于车路通信的隧道内车辆融合定位系统及方法
CN113819914A (zh) 一种地图构建方法及装置
CN109976344B (zh) 巡检机器人姿态矫正方法
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN113819905A (zh) 一种基于多传感器融合的里程计方法及装置
CN113223161A (zh) 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法
CN112819711A (zh) 一种基于单目视觉的利用道路车道线的车辆反向定位方法
CN115015956A (zh) 一种室内无人车的激光与视觉slam系统
CN111708010B (zh) 一种可移动设备的定位方法、装置、系统及可移动设备
CN114964276A (zh) 一种融合惯导的动态视觉slam方法
CN114475581A (zh) 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法

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