CN110956665B - 车辆拐弯轨迹双向计算方法、系统、装置 - Google Patents
车辆拐弯轨迹双向计算方法、系统、装置 Download PDFInfo
- Publication number
- CN110956665B CN110956665B CN201911307058.XA CN201911307058A CN110956665B CN 110956665 B CN110956665 B CN 110956665B CN 201911307058 A CN201911307058 A CN 201911307058A CN 110956665 B CN110956665 B CN 110956665B
- Authority
- CN
- China
- Prior art keywords
- frame
- image
- data
- imu
- 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.)
- Active
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 101
- 230000002457 bidirectional effect Effects 0.000 title claims abstract description 35
- 238000005259 measurement Methods 0.000 claims abstract description 88
- 239000011159 matrix material Substances 0.000 claims description 122
- 238000000034 method Methods 0.000 claims description 74
- 238000005457 optimization Methods 0.000 claims description 62
- 239000013598 vector Substances 0.000 claims description 32
- 230000009191 jumping Effects 0.000 claims description 30
- 230000001133 acceleration Effects 0.000 claims description 14
- 230000005484 gravity Effects 0.000 claims description 8
- 230000036544 posture Effects 0.000 claims description 7
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 230000003287 optical effect Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 230000004927 fusion Effects 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 38
- 230000010354 integration Effects 0.000 description 13
- 230000000007 visual effect Effects 0.000 description 12
- 230000008569 process Effects 0.000 description 11
- 238000010276 construction Methods 0.000 description 8
- 238000000691 measurement method Methods 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- FLDSMVTWEZKONL-AWEZNQCLSA-N 5,5-dimethyl-N-[(3S)-5-methyl-4-oxo-2,3-dihydro-1,5-benzoxazepin-3-yl]-1,4,7,8-tetrahydrooxepino[4,5-c]pyrazole-3-carboxamide Chemical compound CC1(CC2=C(NN=C2C(=O)N[C@@H]2C(N(C3=C(OC2)C=CC=C3)C)=O)CCO1)C FLDSMVTWEZKONL-AWEZNQCLSA-N 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 102000020897 Formins Human genes 0.000 description 1
- 108091022623 Formins Proteins 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
-
- 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
- G01C21/165—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 combined with non-inertial navigation 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/20—Instruments for performing navigational calculations
-
- 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
-
- 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/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
-
- 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/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10016—Video; Image sequence
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30241—Trajectory
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
- G06T2207/30256—Lane; Road marking
-
- 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
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Navigation (AREA)
Abstract
本发明属于多传感器融合定位导航技术领域，具体涉及了一种车辆拐弯轨迹双向计算方法、系统、装置，旨在解决现有技术车辆拐弯之前轨迹计算精度较低的问题。本发明包括：获取图片特征点、传感器量测值，存储并更新滑动窗口；进行系统初始化；当新图像加入滑动窗口，优化传感器位姿等参数直到车辆拐弯；当新图像加入滑动窗口，优化传感器位姿和偏移等参数直到加速度计偏移估计值收敛；当新图像加入滑动窗口优化所有参数；开辟反向计算线程，从当前位置开始，按照时间从后向前的顺序计算拐弯之前的轨迹。本发明通过正向线程多次进行拐弯后轨迹优化，并开辟反向线程计算拐弯前轨迹，可以获取拐弯前后高精度轨迹，并可实现无GPS下的准确定位导航。
Description
技术领域
本发明属于多传感器融合定位导航技术领域，具体涉及了一种车辆拐弯轨迹双向计算方法、系统、装置。
背景技术
采用相机和IMU(Inertial Measurement Unit,惯性测量单元)的视觉惯性测程法已经在增强现实、无人机等应用中取得了很好的效果。然而对于视觉惯性测程法在车辆上的应用而言，常常会出现两种困难的场景：第一，当车辆在其自身坐标系下的加速度恒定(更特殊的情况是匀速运动)时，视觉惯性测程法中的尺度估计不准确；第二，当车辆做直线运动时，视觉惯性测程法中的俯仰角和侧滚角估计不准确。对于第一种困难场景，使用现有的紧耦合车辆轮速计数据的方法已经可以得到解决。而相比之下第二种困难场景更难解决，即使有轮速计数据的辅助，在车辆未进行转弯时，俯仰角和侧滚角也无法准确估计。
此外，在车辆拐弯之前，由于场景不能对待估计的状态提供足够的约束，系统处于不稳定的状态，传感器之间的外参数优化也无法进行，否则可能造成系统失败。因此，传感器之间的外参数初始值并不十分准确，在拐弯之前不优化它们的值也会进一步降低拐弯前轨迹精度。
基于滑动窗口优化的视觉惯性测程法大多数使用了边缘化(marginalization)的方法，这种方法可以使历史信息作为约束参加当前窗口的优化，从而在绝大多数情况下，一旦车辆进行拐弯，其后俯仰角和滚转角都可以被准确估计，传感器之间的外参数也可以进行优化，此时得到的车辆轨迹也较为准确。
发明内容
为了解决现有技术中的上述问题，即现有技术车辆拐弯之前的轨迹计算精度较低的问题，本发明提供了一种车辆拐弯轨迹双向计算方法，该轨迹双向计算方法包括：
步骤S10，实时获取相机第t帧图像特征点，并获取第t帧与第t-1帧图像之间传感器的量测值作为第t帧的帧数据，存储并更新滑动窗口；所述传感器包括IMU、轮速计；所述IMU包括陀螺仪、加速度计；
步骤S20，根据状态标记R的数值确定跳转步骤，1、2、3、4分别对应跳转步骤为S40、S50、S60、S70；其中R的初始数值为0；
步骤S30，响应于第一指令，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点，获取第一数据；若初始化成功，将状态标记置为1，并执行步骤S40；
步骤S40，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第一数据，通过预设第一代价函数正向计算获取第二数据；判断获取的车辆转弯角度是否大于设定阈值，若是则将状态标记置为2，并执行步骤S50；
步骤S50，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第二数据，通过预设第二代价函数正向计算获取第三数据；判断获取的加速度计偏移估计值是否收敛，若收敛则将状态标记置为3，并执行步骤S60；
步骤S60，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第三数据，通过预设第三代价函数正向计算获取第四数据；直至该步骤执行次数大于设定次数，将状态标记置为4，并执行步骤S70；
步骤S70，分别基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第四数据以及当前滑动窗口的反向排列数据、第t帧的帧数据、图像特征点、第四数据，通过所述第三代价函数分别正向计算、反向计算获取第五数据；所述第五数据中传感器位置信息构成车辆轨迹。
在一些优选的实施例中，步骤S10中“获取相机第t帧图像特征点”，其方法为：
基于所述相机第t帧图像，通过Shi-Tomasi方法提取特征点，并判断所述相机第t帧图像是否为第一帧图像，若是，则所述Shi-Tomasi特征点为相机第t帧图像特征点；若不是，则通过光流法跟踪第t-1帧图像上的特征点在所述第t帧图像上的匹配特征点，所述匹配特征点与所述Shi-Tomasi特征点为相机第t帧图像特征点。
在一些优选的实施例中，所述第一指令为判断并在达到初始化条件后进行初始化；所述初始化条件包括：
滑动窗口中的图像帧数大于第一设定值；
滑动窗口中存在一帧图像与该窗口内最后一帧图像匹配特征点的数量大于第二设定值，且该两帧图像平均视差大于第三设定值；
滑动窗口中有效本质矩阵数量大于第四设定值。
在一些优选的实施例中，所述有效本质矩阵，其获取方法为：
步骤B10，通过5点法分别计算所述滑动窗口中各相邻帧图像之间的本质矩阵；
步骤B20，分别判断所述本质矩阵对应的相邻帧图像之间匹配特征点的数量是否大于第五设定值，若是则该本质矩阵为有效本质矩阵。
在一些优选的实施例中，步骤S30中“基于第t帧的帧数据以及图像特征点，获取第一数据”，其方法为：
基于所述第t帧的帧数据，通过航位推算获取第一传感器位置和姿态；基于所述第t帧的帧数据以及图像特征点，获取第一陀螺仪偏移值、重力方向、第一传感器速度以及图像特征点在相机坐标系的第一深度；
所述第一传感器位置、姿态、速度，第一陀螺仪偏移值、重力方向以及图像特征点在相机坐标系的第一深度为第一数据。
在一些优选的实施例中，所述第二数据包括通过第一代价函数正向计算获取的第二传感器位置、姿态、速度，第二陀螺仪偏移值以及图像特征点在相机坐标系的第二深度。
在一些优选的实施例中，所述第三数据包括通过第二代价函数正向计算获取的第三传感器位置、姿态、速度，第三陀螺仪偏移值，图像特征点在相机坐标系的第三深度以及第一加速度计偏移值。
在一些优选的实施例中，所述第四数据包括通过第三代价函数正向计算获取的第四传感器位置、姿态、速度，第四陀螺仪偏移值，图像特征点在相机坐标系的第四深度，第二加速度计偏移值，第一传感器外参数。
在一些优选的实施例中，所述第五数据包括分别通过第三代价函数正向计算、反向计算获取的第五传感器位置、姿态、速度，第五陀螺仪偏移值，图像特征点在相机坐标系的第五深度，第三加速度计偏移值，第二传感器外参数。
在一些优选的实施例中，所述车辆转弯角度，其获取方法为：
其中，ωi为时刻ti量测的绕IMU的三个轴中与水平方向夹角最大的轴旋转的角速度，tnow为进行计算时的当前时刻，n1为进行计算的设定历史时间段时长，fIMU为IMU的量测频率。
在一些优选的实施例中，所述代价函数包括重投影误差项、IMU-轮速计误差项、边缘化误差项；所述IMU-轮速计误差项为：
其中，CIMU(x)代表IMU-轮速计误差项；x＝{x1，x2，...，xk}，k为图像在滑动窗口内的帧序号，xk代表第k帧图像的状态向量；∑k，k+1为相邻两帧图像之间IMU与轮速计量测值的协方差矩阵；
在所述第一代价函数中代表第一残差向量、在所述第二代价函数中代表第二残差向量、在所述第三代价函数中代表第三残差向量，/>
代表残差向量的转置。
在一些优选的实施例中，所述第一残差向量为：
其中，
和/>
分别为世界坐标系下第k帧图像曝光时刻传感器的位置和速度；/>
和/>
分别为世界坐标系下第k+1帧曝光时刻传感器的位置和速度；/>
和/>
是第k+1帧和第k帧对应的陀螺仪的偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的陀螺仪偏移；/>
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；/>
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；gw为世界坐标系中的重力加速度，为一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；
是车体坐标系的原点在IMU坐标系中的坐标，/>
是从车体坐标系到IMU坐标系的旋转；
和/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的名义状态；Jk表示第k+1帧图像对应的IMU和轮速计量测值预积分得到的雅可比矩阵，/>
为Jk的第1-3行、第16-18列构成的矩阵，/>
为Jk的第4-6行、第16-18列构成的矩阵，/>
为Jk的第7-9行、第16-18列构成的矩阵，/>
为Jk的第10-12行、第16-18列构成的矩阵。
在一些优选的实施例中，所述第二残差向量为：
其中，
和
分别为世界坐标系下第k帧图像曝光时刻传感器的位置和速度；/>
和/>
分别为世界坐标系下第k+1帧曝光时刻传感器的位置和速度；/>
和/>
是第k+1帧和第k帧对应的加速度计的偏移；/>
和/>
是第k+1帧和第k帧对应的陀螺仪的偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的加速度计偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的陀螺仪偏移；/>
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；/>
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；/>
是车体坐标系的原点在IMU坐标系中的坐标，/>
是从车体坐标系到IMU坐标系的旋转；/>
和/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的名义状态；Jk表示第k+1帧图像对应的IMU和轮速计量测值预积分得到的雅可比矩阵，/>
为Jk的第1-3行、第13-15列构成的矩阵，/>
为Jk的第4-6行、第13-15列构成的矩阵，/>
为Jk的第1-3行、第16-18列构成的矩阵，
为Jk的第4-6行、第16-18列构成的矩阵，/>
是Jk的第7-9行、第16-18列构成的矩阵，/>
是Jk的第10-12行、第16-18列构成的矩阵。
在一些优选的实施例中，所述第三残差向量为：
其中，
和/>
分别为世界坐标系下第k帧图像曝光时刻传感器的位置和速度；/>
和/>
分别为世界坐标系下第k+1帧曝光时刻传感器的位置和速度；/>
和/>
是第k+1帧和第k帧对应的加速度计的偏移；/>
和/>
是第k+1帧和第k帧对应的陀螺仪的偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的加速度计偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的陀螺仪偏移；/>
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；
为/>
的旋转矩阵形式；/>
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；/>
是车体坐标系的原点在IMU坐标系中的坐标，/>
是从车体坐标系到IMU坐标系的旋转，/>
是/>
的四元数形式，/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的从车体坐标系到IMU坐标系的旋转的四元数形式；/>
和/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的名义状态；Jk表示第k+1帧图像对应的IMU和轮速计量测值预积分得到的雅可比矩阵，/>
为Jk的第1-3行、第13-15列构成的矩阵，/>
为Jk的第4-6行、第13-15列构成的矩阵，/>
为Jk的第1-3行、第16-18列构成的矩阵，/>
为Jk的第4-6行、第16-18列构成的矩阵，/>
为Jk的第7-9行、第16-18列构成的矩阵，/>
为Jk的第10-12行、第16-18列构成的矩阵；/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的轮速计位移关于IMU坐标系和车体坐标系之间的旋转外参数的导数矩阵；
表示四元数/>
的向量部分。
本发明的另一方面，提出了一种车辆拐弯轨迹双向计算系统，该轨迹双向计算系统包括输入模块、滑动窗口、状态标记及跳转模块、初始化模块、第一数据优化模块、第二数据优化模块、第三数据优化模块、第四数据优化模块、输出模块；
所述输入模块，配置为实时获取相机第t帧图像特征点，并获取第t帧与第t-1帧图像之间传感器的量测值作为第t帧的帧数据，存储并更新滑动窗口；所述传感器包括IMU、轮速计；所述IMU包括陀螺仪、加速度计；
滑动窗口，配置为存储实时获取的相机第t帧图像特征点、第t帧的帧数据，并依据输入模块获取的数据进行窗口更新；
所述状态标记及跳转模块，配置为根据状态标记R的数值确定跳转模块，1、2、3、4分别对应跳转模块为第一数据优化模块、第二数据优化模块、第三数据优化模块、第四数据优化模块；其中R的初始数值为0；
所述初始化模块，配置为响应于第一指令，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点，获取第一数据；若初始化成功，将状态标记置为1，并跳转第一数据优化模块；
所述第一数据优化模块，配置为基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第一数据，通过预设第一代价函数正向计算获取第二数据；判断获取的车辆转弯角度是否大于设定阈值，若是则将状态标记置为2，并跳转第二数据优化模块；
所述第二数据优化模块，配置为基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第二数据，通过预设第二代价函数正向计算获取第三数据；判断获取的加速度计偏移估计值是否收敛，若是则将状态标记置为3，并跳转第三数据优化模块；
所述第三数据优化模块，配置为基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第三数据，通过预设第三代价函数正向计算获取第四数据；直至该模块执行次数大于设定次数，将状态标记置为4，并跳转第四数据优化模块；
所述第四数据优化模块，配置为分别基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第四数据以及当前滑动窗口的反向排列数据、第t帧的帧数据、图像特征点、第四数据，通过所述第三代价函数分别正向计算、反向计算获取第五数据；
所述输出模块，配置为输出所述第五数据中传感器位置信息构成的车辆轨迹。
本发明的第三方面，提出了一种存储装置，其中存储有多条程序，所述程序适于由处理器加载并执行以实现上述的车辆拐弯轨迹双向计算方法。
本发明的第四方面，提出了一种处理装置，包括处理器、存储装置；所述处理器，适于执行各条程序；所述存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的车辆拐弯轨迹双向计算方法。
本发明的有益效果：
(1)本发明车辆拐弯轨迹双向计算方法，在车辆未完成拐弯和刚完成拐弯的时候控制参与优化的参数个数并且限制边缘化误差项的大小，等到各个参数都能准确估计之后开辟反向计算的线程，按照从后向前的顺序重新计算拐弯之前的轨迹，使得车辆拐弯前后的轨迹都具有较高的精度。
(2)本发明车辆拐弯轨迹双向计算方法，在地下停车场、隧道等没有GPS信号，从而需要其他传感器提供车辆的实时位置进行导航的情况下，使用相机、IMU、轮速计这样比较经济和可靠的设备，融合相机、IMU、轮速计提供的实时位置，通过本发明的双向轨迹计算方法提高在起始阶段计算出的实时位置的精度，从而实现无GPS情况下准确的定位导航。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：
图1是本发明车辆拐弯轨迹双向计算方法的流程示意图；
图2是本发明车辆拐弯轨迹双向计算方法一种实施例的与真值轨迹、单向计算方法轨迹的对比示意图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
本发明的一种车辆拐弯轨迹双向计算方法，该轨迹双向计算方法包括：
步骤S10，实时获取相机第t帧图像特征点，并获取第t帧与第t-1帧图像之间传感器的量测值作为第t帧的帧数据，存储并更新滑动窗口；所述传感器包括IMU、轮速计；所述IMU包括陀螺仪、加速度计；
步骤S20，根据状态标记R的数值确定跳转步骤，1、2、3、4分别对应跳转步骤为S40、S50、S60、S70；其中R的初始数值为0；
步骤S30，响应于第一指令，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点，获取第一数据；若初始化成功，将状态标记置为1，并执行步骤S40；
步骤S40，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第一数据，通过预设第一代价函数正向计算获取第二数据；判断获取的车辆转弯角度是否大于设定阈值，若是则将状态标记置为2，并执行步骤S50；
步骤S50，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第二数据，通过预设第二代价函数正向计算获取第三数据；判断获取的加速度计偏移估计值是否收敛，若收敛则将状态标记置为3，并执行步骤S60；
步骤S60，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第三数据，通过预设第三代价函数正向计算获取第四数据；直至该步骤执行次数大于设定次数，将状态标记置为4，并执行步骤S70；
步骤S70，分别基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第四数据以及当前滑动窗口的反向排列数据、第t帧的帧数据、图像特征点、第四数据，通过所述第三代价函数分别正向计算、反向计算获取第五数据；所述第五数据中传感器位置信息构成车辆轨迹。
为了更清晰地对本发明车辆拐弯轨迹双向计算方法进行说明，下面结合图1对本发明方法实施例中各步骤展开详述。
本发明一种实施例的车辆拐弯轨迹双向计算方法，包括步骤S10-步骤S70，各步骤详细描述如下：
步骤S10，实时获取相机第t帧图像特征点，并获取第t帧与第t-1帧图像之间传感器的量测值作为第t帧的帧数据，存储并更新滑动窗口；所述传感器包括IMU、轮速计；所述IMU包括陀螺仪、加速度计。
实时获取相机图像、IMU和轮速计的量测值；在相机图像上提取并跟踪特征点，并维护一个包含有当前N帧图像帧的滑动窗口；同时把特征点、IMU和轮速计的数据记录下来。本发明一个实施例中，滑动窗口中包含有当前10帧图像帧。本步骤一直执行至最后，其具体执行过程如下：
步骤S101，开辟滑动窗口，记为A；每采集新的一帧图像时，对轮速计量测值进行采样,使采样后的时间戳与IMU量测值时间戳一致。
采集的图像、IMU和轮速计量测值均带有各自的时间戳，IMU量测值和轮子编码器读数的获取频率均显著高于图像的获取频率。
基于相机第t帧图像，通过Shi-Tomasi方法提取特征点，并判断所述相机第t帧图像是否为第一帧图像，若是，则Shi-Tomasi特征点为相机第t帧图像特征点；若不是，则通过光流法跟踪第t-1帧图像上的特征点在所述第t帧图像上的匹配特征点，所述匹配特征点与所述Shi-Tomasi特征点为相机第t帧图像特征点。
通过光流跟踪法获得的前一帧图像特征点与对应的当前帧图像特征点，对应于同一自然地标点，为相互匹配的点，即匹配特征点。
步骤S102，把所述图像与其对应的IMU和轮速计的量测值加入滑动窗口A，如果滑动窗口A内的图像数目已经达到它能容纳的最大数目，那么判断滑动窗口A内倒数第二帧图像是否为关键帧；如果所述图像是关键帧，则把该窗口中第二帧图像对应的IMU和轮速计的量测值和最前面一帧图像丢弃，滑动窗口A内各图像帧序号减1，否则将所述图像丢弃，它对应的IMU和轮速计的量测值保留在滑动窗口A中，滑动窗口A内各图像帧按照时间戳由早到晚的顺序前后排列。
其中，图像对应的IMU和轮速计的量测值是指时间戳位于图像与滑动窗口A内前一张图像的时间戳之间的IMU和轮速计的量测值。
图像是否为关键帧的判断方法具体可参考文献：“Qin T,Li P,Shen S.Vins-mono:A robust and versatile monocular visual-inertial state estimator[J].IEEETransactions on Robotics,2018,34(4):1004-1020.”，本发明在此不进行详细描述。
步骤S103，实时将滑动窗口A中的最后一张图像对应的IMU和轮速计量测值，按照步骤G10中公式进行预积分操作。此时，滑动窗口A中所有图像对应的预积分结果均为已知。
图像对应的预积分结果是指该图像对应的IMU和轮子编码器量测值经过预积分操作得到的位移、速度变化量、旋转等名义状态(nominal state)以及得到的协方差矩阵、雅可比矩阵。
步骤S104，记录所述图像对应的特征点，如果该图像并非系统启动以来的第一张图像，那么同时记录时间戳位于该图像和前一张图像之间的IMU和轮速计的量测值。
步骤S20，根据状态标记R的数值确定跳转步骤，1、2、3、4分别对应跳转步骤为S40、S50、S60、S70；其中R的初始数值为0。
步骤S30，响应于第一指令，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点，获取第一数据；若初始化成功，将状态标记置为1，并执行步骤S40。
每采集新的一帧图像时，使用IMU和轮速计的量测值进行航位推算(deadreckoning)，同时尝试使用上述两种传感器的数据进行初始化，将局部坐标系的z轴方向与重力方向对齐，并对图像上的特征点三角化建立初始地图，具体包括：
步骤S301，开辟新的滑动窗口，记为B，每采集新的一帧图像时，把该图像与其对应的IMU和轮速计的量测值加入所述滑动窗口B；如果此时步骤S102中判断得到该图像加入窗口A前滑动窗口A内最后一帧图像是关键帧，则将滑动窗口B内时间戳比滑动窗口A内第二帧图像更早的图像及其对应的IMU和轮速计量测值丢弃。滑动窗口B内各图像帧按照时间戳由早到晚的顺序前后排列。
其中，图像对应的IMU和轮速计的量测值是指时间戳位于所述图像与滑动窗口B内前一张图像的时间戳之间的IMU和轮速计的量测值。
步骤S302，如果步骤S301中图像并第一张图像，那么用5点法计算该图像与前一帧图像之间的本质矩阵作为该图像对应的本质矩阵，若有超过N1对匹配点满足该矩阵，则该矩阵有效。本发明一个实施例中，N1＝10。
步骤S303，对于滑动窗口B中的最后一张图像对应的IMU和轮子编码器量测值，按照步骤G10进行预积分操作。
步骤S304，查看滑动窗口A中的图像数目是否已经达到它能容纳的最大数目；查看滑动窗口A中是否存在一张图像与该窗口内最后一张图像的匹配特征点的数量和平均视差都超过阈值；判断滑动窗口B中有效本质矩阵个数是否超过阈值。如果上述条件判断结果均为是，则继续往下执行，否则等待S301再次被触发。
步骤S305，分解所述有效本质矩阵，得到旋转四元数以及两帧之间的相对旋转。
步骤S306，使用最小二乘方法计算陀螺仪的偏移，如式(1)所示：
其中，bω为陀螺仪偏移；F表示滑动窗口B中从第二张图像起对应着有效的本质矩阵的图像序号集合；m表示滑动窗口B中图像序号，
为步骤S305中分解本质矩阵得到的旋转四元数，表示从滑动窗口B中第m+1帧图像的相机坐标系到第m帧图像的相机坐标系的旋转；/>
是表示从相机坐标系到IMU坐标系的旋转的四元数；/>
和/>
分别是步骤G10中得到滑动窗口B中的第m帧和第m+1帧图像之间IMU-轮速计预积分的雅可比矩阵的第10至12行、第16至18列构成的矩阵和旋转四元数；/>
表示四元数的乘法。
在所述物理量中，bω是待优化的参数，其他物理量在此阶段均为已知量。
步骤S307，对于滑动窗口A和滑动窗口B中的每一张图像对应的IMU量测值和轮子编码器读数，依次按照步骤G10重新进行预积分操作，其中陀螺仪偏移bω使用步骤S306中得到的值。
步骤S308，根据IMU提供的角速度和轮速计提供的线速度，使用航位推算的方法计算滑动窗口A和滑动窗口B内各图像帧位置和姿态。
步骤S309，对于滑动窗口B中每两帧连续的图像，构造方程，如式(2)所示：
其中，
和/>
为步骤G10计算出的滑动窗口B中第m帧和第m+1帧图像之间IMU-轮子编码器预积分的结果；/>
为四元数/>
对应的旋转矩阵形式，Δtm为滑动窗口B中第m+1帧和第m帧图像之间时间戳的差值；/>
是步骤S308中计算出的滑动窗口内第1图像帧到第m图像帧之间IMU坐标系之间的旋转；/>
是车体坐标系的原点在IMU坐标系下的坐标；/>
是在窗口内第一张图像对应的IMU坐标系下的重力加速度，它的初值可以由/>
计算，/>
是从车体坐标系到IMU坐标系的旋转，g为当地重力加速度的大小；B是/>
的切平面上的一组正交基；/>
为第m帧图像对应的IMU位置在第m帧图像对应的IMU坐标系下的运动速度，/>
为第m+1帧图像对应的IMU位置在第m+1帧图像对应的IMU坐标系下的运动速度；Δg是重力加速度的更新量。
步骤S310，联立步骤S309中所有连续的两帧图像构成的方程，形成一个线性方程组，如式(3)所示：
L(6M-6)×1＝A(6M-6)×(3M+2)X(3M+2)×1 式(3)
其中，矩阵符号的下标表示矩阵的行数和列数；X包含的所有元素均为待优化的变量，L和A的所有元素均为已知。
步骤S311，使用最小二乘法求解minX(AX-L)T(AX-L)，利用X中的Δg按照
更新/>
并重新计算L和A，迭代c次。本发明一个实施例中，c＝4。/>
步骤S312，找到一个旋转矩阵
将计算出的重力方向/>
旋转到与z轴方向平行；对步骤S308中通过航位推算计算出的滑动窗口A内各图像帧的位置和姿态以及步骤S311中计算出的滑动窗口A内各图像帧在滑动窗口B内对应的图像帧的速度做相应的坐标变换作为步骤S401中的状态初值；步骤S307中计算出陀螺仪偏移作为步骤S401中的陀螺仪偏移的初值。
步骤S313，对滑动窗口A中各图像上对应的特征点所对应的自然地标点进行三角化，构建初始地图；至此，步骤S401中所有待优化变量的初值均已得到。
步骤S40，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第一数据，通过预设第一代价函数正向计算获取第二数据；判断获取的车辆转弯角度是否大于设定值，若是则将状态标记置为2，并执行步骤S50。
每采集新的一帧图像时，利用提取到的图像上的特征点、IMU以及轮速计的量测值构造边缘化误差项、IMU-轮速计误差项和重投影误差项，使用基于滑动窗口优化的方法估计传感器的位置、姿态、速度，陀螺仪的偏移以及特征点对应的自然地标点的位置；加速度计偏移、相机和IMU之间的外参数、IMU与轮速计之间的外参数保持不变，不参与优化。每次优化之后，如果各误差项之和超过了阈值Tm，则将边缘化误差项中的残差向量和雅可比矩阵同时乘以系数λ；判断过去n1秒内车辆是否拐了一个超过θ度的弯，若是则转到步骤S50。
本发明一个实施例中，Tm＝1000；λ＝0.85；n1＝20；θ＝45。步骤S40具体包括：
步骤S401，每当新的图像进入滑动窗口A时，按照基于滑动窗口中非线性优化的视觉惯性测程法的通用做法在滑动窗口A中构造代价函数，使用Dogleg算法进行非线性优化求解，得到传感器位姿、速度和偏移以及匹配点深度的估计值。
代价函数包括重投影误差项、IMU-轮速计误差项和边缘化误差项，其中，重投影误差项、边缘化误差项使用基于滑动窗口中非线性优化的视觉惯性测程法的通用做法进行构造，重投影误差项具体参考文献：“Qin T,Li P,Shen S.Vins-mono:A robust andversatile monocular visual-inertial state estimator[J].IEEE Transactions onRobotics,2018,34(4):1004-1020.”，边缘化误差项具体参考文献：“Leutenegger S,Furgale P,Rabaud V,et al.Keyframe-based visual-inertial slam using nonlinearoptimization[J].Proceedings of Robotis Science and Systems(RSS)2013,2013.”，本发明在此不进行详细描述。
IMU-轮速计误差项构造方法如式(4)所示：
其中，CIMU(x)代表IMU-轮速计误差项；x＝{x1，x2，...，xk}，k为图像在滑动窗口内的帧序号，xk代表第k帧图像的状态向量；∑k，k+1为相邻两帧图像之间IMU与轮速计量测值的协方差矩阵；
代表第一残差向量，/>
代表残差向量的转置。
其中，
和/>
分别为世界坐标系下第k帧图像曝光时刻传感器的位置和速度；/>
和/>
分别为世界坐标系下第k+1帧曝光时刻传感器的位置和速度；/>
和/>
是第k+1帧和第k帧对应的陀螺仪的偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的陀螺仪偏移；/>
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；/>
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；gw为世界坐标系中的重力加速度，为一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；
是车体坐标系的原点在IMU坐标系中的坐标，/>
是从车体坐标系到IMU坐标系的旋转；
和/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的名义状态；Jk表示第k+1帧图像对应的IMU和轮速计量测值预积分得到的雅可比矩阵，/>
为Jk的第1-3行、第16-18列构成的矩阵，/>
为Jk的第4-6行、第16-18列构成的矩阵，/>
为Jk的第7-9行、第16-18列构成的矩阵，/>
为Jk的第10-12行、第16-18列构成的矩阵。
另外，每个自然地标点在它出现的第一帧图像上的深度值也是待优化参数，它们参与重投影误差项的构建。
在此步骤中加速度计偏移并不作为参数优化，因此在步骤S40执行的时候，加速度计偏移始终为0。
步骤S402，计算优化后各误差项之和，如果它超过了阈值Tm，则将边缘化误差项中的残差向量和雅可比矩阵同时乘以系数λ。
步骤S403，判断过去n1秒内车辆是否拐了一个超过θ度的弯，其中，过去n1秒内车辆拐弯大小θ′的计算方法如式(6)所示：
其中，ωi为时刻ti量测的绕IMU的三个轴中与水平方向夹角最大的轴旋转的角速度，tnow为进行计算时的当前时刻，n1为进行计算的设定历史时间段时长，fIMU为IMU的量测频率。IMU的三个轴中与水平方向夹角最大的那个轴不会随着车辆运动而变化，因此容易判断。
步骤S50，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第二数据，通过预设第二代价函数正向计算获取第三数据；判断获取的加速度计偏移估计值是否收敛，若是则将状态标记置为3，并执行步骤S60。
每采集新的一帧图像时，利用提取到的图像上的特征点、IMU以及轮速计的数据构造边缘化误差项、IMU-轮速计误差项和重投影误差项，使用基于滑动窗口优化的方法估计传感器的位置、姿态、速度，加速度计和陀螺仪的偏移以及特征点对应的自然地标点的位置；相机和IMU之间的外参数、IMU与轮速计之间的外参数保持不变，不参与优化；每次优化之后，判断过去n2秒内每相邻两个状态加速度计偏移的估计值之差的绝对值的平均值是否小于阈值Tb，若是则转至步骤S60。本发明一个实施例中，n2＝20。步骤S50具体包括：
步骤S501，每当新的图像进入滑动窗口A时，按照基于滑动窗口中非线性优化的视觉惯性测程法的通用做法在滑动窗口A中构造代价函数，使用Dogleg算法进行非线性优化求解，得到传感器位姿、速度和偏移以及匹配点深度的估计值。
其中，代价函数构造方法与步骤S401相同，重投影误差项、IMU-轮速计误差项、边缘化误差项的构造方法也参照步骤S401，本发明在此不再一一详述。
在步骤S501中，IMU-轮速计误差项中
代表第二残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关，如式(7)所示：
其中，
和
分别为世界坐标系下第k帧图像曝光时刻传感器的位置和速度；/>
和/>
分别为世界坐标系下第k+1帧曝光时刻传感器的位置和速度；/>
和/>
是第k+1帧和第k帧对应的加速度计的偏移；/>
和/>
是第k+1帧和第k帧对应的陀螺仪的偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的加速度计偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的陀螺仪偏移；/>
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；/>
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；/>
是车体坐标系的原点在IMU坐标系中的坐标，/>
是从车体坐标系到IMU坐标系的旋转；/>
和/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的名义状态；Jk表示第k+1帧图像对应的IMU和轮速计量测值预积分得到的雅可比矩阵，/>
为Jk的第1-3行、第13-15列构成的矩阵，/>
为Jk的第4-6行、第13-15列构成的矩阵，/>
为Jk的第1-3行、第16-18列构成的矩阵，
为Jk的第4-6行、第16-18列构成的矩阵，/>
是Jk的第7-9行、第16-18列构成的矩阵，/>
是Jk的第10-12行、第16-18列构成的矩阵。
步骤S502，判断过去n2秒内每相邻两个状态加速度计偏移的估计值之差的绝对值的平均值是否小于阈值Tb，若是则转至步骤S60。
阈值Tb的确定方法如式(8)所示：
其中，
是加速度计的随机游走参数，可以由IMU内参数标定获得，f为图像采集频率。
步骤S60，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第三数据，通过预设第三代价函数正向计算获取第四数据；直至该步骤执行次数大于设定次数，将状态标记置为4，并执行步骤S70。
每采集新的一帧图像时，利用提取到的图像上的特征点、IMU以及轮速计的数据构造边缘化误差项、IMU-轮速计误差项和重投影误差项使用基于滑动窗口优化的方法估计传感器的位置、姿态、速度，加速度计和陀螺仪的偏移，特征点对应的自然地标点的位置，相机和IMU之间的外参数以及IMU与轮速计之间的外参数。每次优化后，判断自从步骤S60启动以来是否已经经历了n3秒，若是则启动步骤S70。本发明一个实施例中，n3＝30。步骤S60具体包括：
步骤S601，每当新的图像进入滑动窗口A时，按照基于滑动窗口中非线性优化的视觉惯性测程法的通用做法在滑动窗口A中构造代价函数，使用Dogleg算法进行非线性优化求解，得到传感器位姿、速度和偏移以及匹配点深度的估计值。
其中，代价函数构造方法与步骤S401相同，重投影误差项、IMU-轮速计误差项、边缘化误差项的构造方法也参照步骤S401，本发明在此不再一一详述。
在步骤S601中，IMU-轮速计误差项中
代表第三残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关，如式(9)所示：
其中，
和/>
分别为世界坐标系下第k帧图像曝光时刻传感器的位置和速度；/>
和/>
分别为世界坐标系下第k+1帧曝光时刻传感器的位置和速度；/>
和/>
是第k+1帧和第k帧对应的加速度计的偏移；/>
和/>
是第k+1帧和第k帧对应的陀螺仪的偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的加速度计偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的陀螺仪偏移；/>
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；
为/>
的旋转矩阵形式；/>
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；/>
是车体坐标系的原点在IMU坐标系中的坐标，/>
是从车体坐标系到IMU坐标系的旋转，/>
是/>
的四元数形式，/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的从车体坐标系到IMU坐标系的旋转的四元数形式；/>
和/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的名义状态；Jk表示第k+1帧图像对应的IMU和轮速计量测值预积分得到的雅可比矩阵，/>
为Jk的第1-3行、第13-15列构成的矩阵，/>
为Jk的第4-6行、第13-15列构成的矩阵，/>
为Jk的第1-3行、第16-18列构成的矩阵，/>
为Jk的第4-6行、第16-18列构成的矩阵，/>
为Jk的第7-9行、第16-18列构成的矩阵，/>
为Jk的第10-12行、第16-18列构成的矩阵；/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的轮速计位移关于IMU坐标系和车体坐标系之间的旋转外参数的导数矩阵；
表示四元数/>
的向量部分。
其中，
和/>
均为待优化的参数，其他物理量为已知量，也就是说，传感器之间的外参数被在线标定。
另外，IMU和相机之间的外参数以及每个自然地标点在它出现的第一帧图像上的深度值也是待优化参数，它们参与重投影误差项的构建。
微小的限制IMU和轮速计之间的旋转外参数变动的误差项，是为了防止所述IMU和轮速计之间的旋转外参数在不可观的方向上大幅变化，它是将待估计的所述旋转外参数与该参数在上一次优化中的估计值的差模长的平方作为代价函数值。
步骤S602，判断自从步骤S60启动以来是否已经经历了n3秒，若是则启动步骤S70。本发明一个实施例中，n3＝30。
步骤S70，分别基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第四数据以及当前滑动窗口的反向排列数据、第t帧的帧数据、图像特征点、第四数据，通过所述第三代价函数分别正向计算、反向计算获取第五数据；所述第五数据中传感器位置信息构成车辆轨迹。
新开辟一个用于反向计算轨迹的线程。刚创建该线程时该线程滑动窗口C内待估计的状态以及误差项根据步骤S60对应的线程中的滑动窗口A内的状态和误差项确定。每当上一次优化结束时，将本步骤滑动窗口C中最早的一帧图像与它之前的那一帧图像之间的已经在步骤S30、步骤S40和步骤S10中记录下来的IMU和轮速计的数据通过步骤G10进行预积分，并把滑动窗口C中最早的一帧图像之前的那一帧图像上的已经记录下来的特征点加入滑动窗口C，同时依据跟踪成功的特征点的视差大小将滑动窗口中次早或最晚的图像帧移出窗口。然后构建新的边缘化误差项、IMU-轮速计误差项和重投影误差项，通过非线性优化估计传感器的位置、姿态、速度，加速度计和陀螺仪的偏移，特征点对应的自然地标点的位置，相机和IMU之间的外参数以及IMU与轮速计之间的外参数。按照上述过程，不断将记录下来的特征点与IMU和轮速计的数据按照时间由晚到早的顺序加入滑动窗口并在窗口内进行优化，从而从后往前重新计算出拐弯之前的轨迹。每当该线程中计算出一帧图像的对应位置的时候，保持起点位置不变，调整各帧图像的对应位置，以保持实时位置的平滑性。步骤S70具体包括：
步骤S701，新开辟一个用于反向计算的线程，开辟一个滑动窗口C，与滑动窗口A类似，滑动窗口C中的待估计状态参数包括传感器位姿、速度、加速度计和陀螺仪的偏移、自然地标点的深度和传感器外参数；除自然地标点深度以外，其他参数的初始值与此时滑动窗口A中的估计值相同；考虑到边缘化误差项和IMU-轮速计误差项中不含有自然地标点深度作为参数，初始的所述两个误差项与滑动窗口A中对应的误差项相同；在每个自然地标点被观测到的图像帧构成的序列中，起始帧由原来时间戳最旧的一帧变成现在时间戳最新的一帧；自然地标点在起始帧上的深度也要作相应调整，如式(10)所示：
其中，
和/>
分别是所述时间戳最旧一帧图像对应的传感器姿态和位置；
和/>
别是所述时间戳最新一帧图像对应的传感器姿态和位置；/>
是从相机坐标系到IMU坐标系的旋转，/>
是相机坐标系的原点在IMU坐标系下的坐标，mold是自然地标点在时间戳最旧一帧的图像上对应的特征点的归一化齐次坐标，λold和λnew分别是调整前后自然地标点在起始帧上的深度，/>
起始帧发生变化后，按照基于滑动窗口中非线性优化的视觉惯性测程法的通用做法重新构造重投影误差项。
步骤S702，按照一定的频率以及时间戳由晚到早的顺序依次将步骤S104中记录的图像上的特征点以及所述图像对应的IMU和轮速计量测值加入滑动窗口C中。与滑动窗口A相反，滑动窗口C中的各图像帧按照时间戳由晚到早的顺序前后排列。
其中，所述图像对应的IMU和轮速计的量测值是指时间戳位于所述图像与滑动窗口C内前一张图像的时间戳之间的IMU和轮速计的量测值。
步骤S703，每当有图像加入滑动窗口C，对于滑动窗口C中的最后一张图像对应的IMU和轮速计量测值，按照步骤G10进行预积分操作，此时，滑动窗口C中所有图像对应的预积分结果均为已知。
步骤S704，按照基于滑动窗口中非线性优化的视觉惯性测程法的通用做法在滑动窗口C中构造代价函数，使用Dogleg算法进行非线性优化求解，构建和求解的具体过程与步骤S601完全相同，也就是说传感器位姿、速度、加速度计和陀螺仪的偏移、自然地标点深度和传感器外参数都是待估计的参数。
步骤S705，判断滑动窗口C内倒数第二帧图像是否为关键帧；如果所述图像是关键帧，则把该窗口中最前面一帧图像，和第二帧图像对应的IMU和轮速计的量测值丢弃，滑动窗口C内各图像帧序号减1，否则将所述图像丢弃，它对应的IMU和轮速计的量测值保留在滑动窗口C中。
其中，所述图像对应的IMU和轮速计的量测值是指时间戳位于所述图像与滑动窗口C内前一张图像的时间戳之间的IMU和轮速计的量测值。
步骤S706，重新计算轨迹上各帧位置。
首先计算新的起点位置，如式(11)所示：
p′s＝R′lRl T(ps-pl)+p′l 式(11)
其中，Rl和R′l是刚被所述反向计算的线程更新的图像在更新前后的姿态，pl和p′l是刚被所述反向计算的线程更新的图像在更新前后的位置，ps和p′s是重新计算前后的起点坐标。
对于尚未被反向计算的线程更新位置和姿态的各帧图像，它们的位置首先重新计算，如式(12)所示：
p′j＝R′lRl T(pj-pl)+p′l 式(12)
其中，pj和p′j分别是所述任意一帧图像重新计算前后的坐标。
然后，再减掉新的起点位置p′s；对于轨迹上的其他图像，它们的位置直接减掉p′s。
本发明计算过程中，在不同的阶段均需要通过对图像对应的IMU量测值和轮子编码器量测值进行预积分操作，得到位移、速度变化量、旋转等名义状态(nominal state)以及协方差矩阵、雅可比矩阵。
步骤G10，加速度计偏移ba和陀螺仪偏移bω的初始值均为0，预积分过程为：
i依次取0，2，…s-2，依次通过式(13)-式(21)计算：
Ji+1＝AiJi 式(20)
直到最终结果
∑s-1，Js-1和/>
被计算出来。
其中，
和/>
统称为名义状态，s表示参与预积分的IMU和轮速计量测值的个数，/>
以及/>
是由四元数表示的旋转，/>
表示四元数的乘法，R(·)表示将一个四元数转换成它对应的旋转矩阵的函数，/>
表示第i个IMU量测值中的三轴加速度，/>
表示第i个IMU量测值中的三轴角速度，/>
表示第i个轮速计的读数，ba为当前IMU中加速度计的偏移，bω为当前IMU中陀螺仪的偏移，δti表示第i+1个IMU量测值的时间戳和第i个IMU量测值的时间戳的差值，/>
表示从车体坐标系到IMU坐标系的旋转矩阵在当前的估计值，/>
表示向量/>
对应的反对称矩阵。
其中，车体坐标系的定义为：以左后轮为原点，x轴指向右后轮，y轴指向左前轮，z轴垂直底盘向上。
将本发明方法应用于KAIST Urban Data Set上的14个序列上，并与单向计算一、单向计算二、VINS-stereo进行比较，其结果如表1所示：
表1
如表1所示，14个序列用Urban26-Urban39表示。单向计算一表示不开辟本发明步骤S70提出的反向轨迹计算线程并且不使用本发明提出的步骤S40中所述的将边缘化误差项中的残差向量和雅可比矩阵同时乘以系数λ的技术，其它技术特征与本发明相同；单向计算二表示不开辟本发明步骤S70提出的反向轨迹计算线程并且不使用本发明提出的步骤S40中所述的将边缘化误差项中的残差向量和雅可比矩阵同时乘以系数λ的技术，而且从一开始就优化所有参数；VINS-stereo为目前最先进的开源的视觉惯性测程法之一VINS-Fusion的双目相机+IMU版本。从表1可以看出，本发明提出的方法的精度优于其他方法，尤其是对于车辆在拐弯之前行驶了较长距离的序列，使用本发明提出的方法精度有较大提升。
如图2所示，为本发明车辆拐弯轨迹双向计算方法一种实施例的与真值轨迹、单向计算方法轨迹的对比示意图，图2左图和右图分别是在序列Urban32与序列Urban33上的使用不同方法计算出的轨迹对比图。图中连续曲线代表真值轨迹，断续线段曲线代表本发明方法获取的车辆轨迹，点曲线代表单向计算获取的车辆轨迹；其中，“单向计算”是指不开辟本发明步骤S70提出的反向轨迹计算线程并且不使用本发明提出的步骤S40中所述的将边缘化误差项中的残差向量和雅可比矩阵同时乘以系数λ的技术，其它技术特征与本发明相同的方法，也就是表1中“单向计算一”代表的方法。可以看出，对于车辆在拐弯之前行驶距离较长的序列上，应用本发明提出的双向轨迹计算方法对轨迹精度有明显的提高。
本发明第二实施例的车辆拐弯轨迹双向计算系统，该轨迹双向计算系统包括输入模块、滑动窗口、状态标记及跳转模块、初始化模块、第一数据优化模块、第二数据优化模块、第三数据优化模块、第四数据优化模块、输出模块；
所述输入模块，配置为实时获取相机第t帧图像特征点，并获取第t帧与第t-1帧图像之间传感器的量测值作为第t帧的帧数据，存储并更新滑动窗口；所述传感器包括IMU、轮速计；所述IMU包括陀螺仪、加速度计；
滑动窗口，配置为存储实时获取的相机第t帧图像特征点、第t帧的帧数据，并依据输入模块获取的数据进行窗口更新；
所述状态标记及跳转模块，配置为根据状态标记R的数值确定跳转模块，1、2、3、4分别对应跳转模块为第一数据优化模块、第二数据优化模块、第三数据优化模块、第四数据优化模块；其中R的初始数值为0；
所述初始化模块，配置为响应于第一指令，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点，获取第一数据；若初始化成功，将状态标记置为1，并跳转第一数据优化模块；
所述第一数据优化模块，配置为基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第一数据，通过预设第一代价函数正向计算获取第二数据；判断获取的车辆转弯角度是否大于设定阈值，若是则将状态标记置为2，并跳转第二数据优化模块；
所述第二数据优化模块，配置为基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第二数据，通过预设第二代价函数正向计算获取第三数据；判断获取的加速度计偏移估计值是否收敛，若是则将状态标记置为3，并跳转第三数据优化模块；
所述第三数据优化模块，配置为基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第三数据，通过预设第三代价函数正向计算获取第四数据；直至该模块执行次数大于设定次数，将状态标记置为4，并跳转第四数据优化模块；
所述第四数据优化模块，配置为分别基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第四数据以及当前滑动窗口的反向排列数据、第t帧的帧数据、图像特征点、第四数据，通过所述第三代价函数分别正向计算、反向计算获取第五数据；
所述输出模块，配置为输出所述第五数据中传感器位置信息构成的车辆轨迹。
所属技术领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统的具体工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。
需要说明的是，上述实施例提供的车辆拐弯轨迹双向计算系统，仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的功能模块来完成，即将本发明实施例中的模块或者步骤再分解或者组合，例如，上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块，以完成以上描述的全部或者部分功能。对于本发明实施例中涉及的模块、步骤的名称，仅仅是为了区分各个模块或者步骤，不视为对本发明的不当限定。
本发明第三实施例的一种存储装置，其中存储有多条程序，所述程序适于由处理器加载并执行以实现上述的车辆拐弯轨迹双向计算方法。
本发明第四实施例的一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的车辆拐弯轨迹双向计算方法。
所属技术领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的存储装置、处理装置的具体工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。
本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
术语“第一”、“第二”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。
术语“包括”或者任何其它类似用语旨在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备/装置不仅包括那些要素，而且还包括没有明确列出的其它要素，或者还包括这些过程、方法、物品或者设备/装置所固有的要素。
至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。
Claims (11)
1.一种车辆拐弯轨迹双向计算方法，其特征在于，该轨迹双向计算方法包括：
步骤S10，实时获取相机第t帧图像特征点，并获取第t帧与第t-1帧图像之间传感器的量测值作为第t帧的帧数据，每采集新的一帧图像时，把所述图像与其对应的帧数据加入滑动窗口并更新滑动窗口，滑动窗口内各图像帧按照时间戳由早到晚的顺序前后排列；所述传感器包括IMU、轮速计；所述IMU包括陀螺仪、加速度计；
步骤S20，根据状态标记R的数值确定跳转步骤，1、2、3、4分别对应跳转步骤为S40、S50、S60、S70；其中R的初始数值为0；
步骤S30，响应于第一指令，基于当前滑动窗口中的图像与其对应的帧数据、第t帧的帧数据、图像特征点，获取第一数据；若初始化成功，将状态标记置为1，并执行步骤S40；所述第一指令为判断并在达到初始化条件后进行初始化；所述第一数据包括各图像帧第一位置、各图像帧第一姿态、各图像帧第一速度、陀螺仪第一偏移值、陀螺仪重力方向以及图像特征点在相机坐标系的第一深度；
所述初始化条件包括：
滑动窗口中的图像帧数大于第一设定值；
滑动窗口中存在一帧图像与该窗口内最后一帧图像匹配特征点的数量大于第二设定值，且该两帧图像平均视差大于第三设定值；
滑动窗口中有效本质矩阵数量大于第四设定值；
所述有效本质矩阵，其获取方法为：
步骤B10，通过5点法分别计算所述滑动窗口中各相邻帧图像之间的本质矩阵；
步骤B20，分别判断所述本质矩阵对应的相邻帧图像之间匹配特征点的数量是否大于第五设定值，若是则该本质矩阵为有效本质矩阵；
步骤S40，每当新的图像进入滑动窗口时，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第一数据，通过预设第一代价函数正向计算获取第二数据；判断获取的车辆转弯角度是否大于设定值，若是则将状态标记置为2，并执行步骤S50；所述第二数据包括通过第一代价函数正向计算获取的各图像帧第二位置、各图像帧第二姿态、各图像帧第二速度、陀螺仪第二偏移值以及图像特征点在相机坐标系的第二深度；
步骤S50，每当新的图像进入滑动窗口时，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第二数据，通过预设第二代价函数正向计算获取第三数据；判断获取的加速度计偏移估计值是否收敛，若是则将状态标记置为3，并执行步骤S60；所述第三数据包括通过第二代价函数正向计算获取的各图像帧第三位置、各图像帧第三姿态、各图像帧第三速度、陀螺仪第三偏移值、图像特征点在相机坐标系的第三深度以及加速度计第一偏移值；
步骤S60，每当新的图像进入滑动窗口时，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第三数据，通过预设第三代价函数正向计算获取第四数据；直至该步骤执行次数大于设定次数，将状态标记置为4，并执行步骤S70；所述第四数据包括通过第三代价函数正向计算获取的各图像帧第四位置、各图像帧第四姿态、各图像帧第四速度、陀螺仪第四偏移值、图像特征点在相机坐标系的第四深度、加速度计第二偏移值、IMU和相机之间的第一外参数以及IMU和轮速计之间的第一外参数；
步骤S70，分别基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第四数据以及当前滑动窗口的反向排列数据、第t帧的帧数据、图像特征点、第四数据，通过所述第三代价函数分别正向计算、反向计算获取第五数据；当前滑动窗口的反向排列数据，代表滑动窗口内各图像帧按照时间戳由晚到早的顺序前后排列；所述第五数据包括分别通过第三代价函数正向计算、反向计算获取的各图像帧第五位置、各图像帧第五姿态、各图像帧第五速度、陀螺仪第五偏移值、图像特征点在相机坐标系的第五深度、加速度计第三偏移值、IMU和相机之间的第二外参数以及IMU和轮速计之间的第二外参数；
所述第五数据中传感器位置信息构成车辆轨迹。
2.根据权利要求1所述的车辆拐弯轨迹双向计算方法，其特征在于，步骤S10中“获取相机第t帧图像特征点”，其方法为：
基于所述相机第t帧图像，通过Shi-Tomasi方法提取特征点，并判断所述相机第t帧图像是否为第一帧图像，若是，则所述Shi-Tomasi特征点为相机第t帧图像特征点；若不是，则通过光流法跟踪第t-1帧图像上的特征点在所述第t帧图像上的匹配特征点，所述匹配特征点与所述Shi-Tomasi特征点为相机第t帧图像特征点。
3.根据权利要求1所述的车辆拐弯轨迹双向计算方法，其特征在于，步骤S30中“基于第t帧的帧数据以及图像特征点，获取第一数据”，其方法为：
基于所述第t帧的帧数据，通过航位推算获取各图像帧第一位置和各图像帧第一姿态；基于所述第t帧的帧数据以及图像特征点，获取陀螺仪第一偏移值、陀螺仪重力方向、各图像帧第一速度以及图像特征点在相机坐标系的第一深度。
4.根据权利要求1所述的车辆拐弯轨迹双向计算方法，其特征在于，所述车辆转弯角度，其获取方法为：
其中，ωi为时刻ti量测的绕IMU的三个轴中与水平方向夹角最大的轴旋转的角速度，tnow为进行计算时的当前时刻，n1为进行计算的设定历史时间段时长，fIMU为IMU的量测频率。
5.根据权利要求1所述的车辆拐弯轨迹双向计算方法，其特征在于，所述代价函数包括重投影误差项、IMU-轮速计误差项、边缘化误差项；所述IMU-轮速计误差项为：
其中，CIMU(x)代表IMU-轮速计误差项；x＝{x1,x2,…,xk}，k为图像在滑动窗口内的帧序号，xk代表第k帧图像的状态向量；∑k,k+1为相邻两帧图像之间IMU与轮速计量测值的协方差矩阵；
在所述第一代价函数中代表第一残差向量、在所述第二代价函数中代表第二残差向量、在所述第三代价函数中代表第三残差向量，/>
代表残差向量的转置。
6.根据权利要求5所述的车辆拐弯轨迹双向计算方法，其特征在于，所述第一残差向量为：
其中，
和/>
分别为世界坐标系下第k帧图像曝光时刻传感器的位置和速度；/>
和/>
分别为世界坐标系下第k+1帧曝光时刻传感器的位置和速度；/>
和/>
是第k+1帧和第k帧对应的陀螺仪的偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的陀螺仪偏移；/>
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；
为/>
的旋转矩阵形式；/>
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；gw为世界坐标系中的重力加速度，为一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；/>
是车体坐标系的原点在IMU坐标系中的坐标，/>
是从车体坐标系到IMU坐标系的旋转；
和/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的名义状态；Jk表示第k+1帧图像对应的IMU和轮速计量测值预积分得到的雅可比矩阵，/>
为Jk的第1-3行、第16-18列构成的矩阵，/>
为Jk的第4-6行、第16-18列构成的矩阵，/>
为Jk的第7-9行、第16-18列构成的矩阵，/>
为Jk的第10-12行、第16-18列构成的矩阵。
7.根据权利要求5所述的车辆拐弯轨迹双向计算方法，其特征在于，所述第二残差向量为：
其中，
和/>
分别为世界坐标系下第k帧图像曝光时刻传感器的位置和速度；/>
和/>
分别为世界坐标系下第k+1帧曝光时刻传感器的位置和速度；/>
和/>
是第k+1帧和第k帧对应的加速度计的偏移；/>
和/>
是第k+1帧和第k帧对应的陀螺仪的偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的加速度计偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的陀螺仪偏移；/>
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；/>
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为
的旋转矩阵形式；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；/>
是车体坐标系的原点在IMU坐标系中的坐标，/>
是从车体坐标系到IMU坐标系的旋转；/>
和/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的名义状态；Jk表示第k+1帧图像对应的IMU和轮速计量测值预积分得到的雅可比矩阵，/>
为Jk的第1-3行、第13-15列构成的矩阵，/>
为Jk的第4-6行、第13-15列构成的矩阵，/>
为Jk的第1-3行、第16-18列构成的矩阵，/>
为Jk的第4-6行、第16-18列构成的矩阵，/>
是Jk的第7-9行、第16-18列构成的矩阵，/>
是Jk的第10-12行、第16-18列构成的矩阵。
8.根据权利要求5所述的车辆拐弯轨迹双向计算方法，其特征在于，所述第三残差向量为：
其中，
和
分别为世界坐标系下第k帧图像曝光时刻传感器的位置和速度；/>
和/>
分别为世界坐标系下第k+1帧曝光时刻传感器的位置和速度；/>
和/>
是第k+1帧和第k帧对应的加速度计的偏移；/>
和/>
是第k+1帧和第k帧对应的陀螺仪的偏移；
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的加速度计偏移；/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的陀螺仪偏移；/>
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；/>
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；/>
为/>
的旋转矩阵形式；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；/>
是车体坐标系的原点在IMU坐标系中的坐标，/>
是从车体坐标系到IMU坐标系的旋转，/>
是/>
的四元数形式，/>
是对第k+1帧图像对应的IMU和轮速计量测值进行预积分时的从车体坐标系到IMU坐标系的旋转的四元数形式；/>
和/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的名义状态；Jk表示第k+1帧图像对应的IMU和轮速计量测值预积分得到的雅可比矩阵，/>
为Jk的第1-3行、第13-15列构成的矩阵，/>
为Jk的第4-6行、第13-15列构成的矩阵，/>
为Jk的第1-3行、第16-18列构成的矩阵，/>
为Jk的第4-6行、第16-18列构成的矩阵，/>
为Jk的第7-9行、第16-18列构成的矩阵，/>
为Jk的第10-12行、第16-18列构成的矩阵；/>
为第k+1帧图像对应的IMU和轮速计量测值预积分得到的轮速计位移关于IMU坐标系和车体坐标系之间的旋转外参数的导数矩阵；/>
表示四元数
的向量部分。
9.一种车辆拐弯轨迹双向计算系统，其特征在于，该轨迹双向计算系统包括输入模块、滑动窗口、状态标记及跳转模块、初始化模块、第一数据优化模块、第二数据优化模块、第三数据优化模块、第四数据优化模块、输出模块；
所述输入模块，配置为实时获取相机第t帧图像特征点，并获取第t帧与第t-1帧图像之间传感器的量测值作为第t帧的帧数据，每采集新的一帧图像时，把所述图像与其对应的帧数据加入滑动窗口并更新滑动窗口，滑动窗口内各图像帧按照时间戳由早到晚的顺序前后排列；所述传感器包括IMU、轮速计；所述IMU包括陀螺仪、加速度计；
滑动窗口，配置为存储实时获取的相机第t帧图像特征点、第t帧的帧数据，并依据输入模块获取的数据进行窗口更新；
所述状态标记及跳转模块，配置为根据状态标记R的数值确定跳转模块，1、2、3、4分别对应跳转模块为第一数据优化模块、第二数据优化模块、第三数据优化模块、第四数据优化模块；其中R的初始数值为0；
所述初始化模块，配置为响应于第一指令，基于当前滑动窗口中的图像与其对应的帧数据、第t帧的帧数据、图像特征点，获取第一数据；若初始化成功，将状态标记置为1，并跳转第一数据优化模块；所述第一指令为判断并在达到初始化条件后进行初始化；所述第一数据包括各图像帧第一位置、各图像帧第一姿态、各图像帧第一速度、陀螺仪第一偏移值、陀螺仪重力方向以及图像特征点在相机坐标系的第一深度；
所述初始化条件包括：
滑动窗口中的图像帧数大于第一设定值；
滑动窗口中存在一帧图像与该窗口内最后一帧图像匹配特征点的数量大于第二设定值，且该两帧图像平均视差大于第三设定值；
滑动窗口中有效本质矩阵数量大于第四设定值；
所述有效本质矩阵，其获取方法为：
步骤B10，通过5点法分别计算所述滑动窗口中各相邻帧图像之间的本质矩阵；
步骤B20，分别判断所述本质矩阵对应的相邻帧图像之间匹配特征点的数量是否大于第五设定值，若是则该本质矩阵为有效本质矩阵；
所述第一数据优化模块，配置为每当新的图像进入滑动窗口时，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第一数据，通过预设第一代价函数正向计算获取第二数据；判断获取的车辆转弯角度是否大于设定阈值，若是则将状态标记置为2，并跳转第二数据优化模块；所述第二数据包括通过第一代价函数正向计算获取的各图像帧第二位置、各图像帧第二姿态、各图像帧第二速度、陀螺仪第二偏移值以及图像特征点在相机坐标系的第二深度；
所述第二数据优化模块，配置为每当新的图像进入滑动窗口时，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第二数据，通过预设第二代价函数正向计算获取第三数据；判断获取的加速度计偏移估计值是否收敛，若是则将状态标记置为3，并跳转第三数据优化模块；所述第三数据包括通过第二代价函数正向计算获取的各图像帧第三位置、各图像帧第三姿态、各图像帧第三速度、陀螺仪第三偏移值、图像特征点在相机坐标系的第三深度以及加速度计第一偏移值；
所述第三数据优化模块，配置为每当新的图像进入滑动窗口时，基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第三数据，通过预设第三代价函数正向计算获取第四数据；直至该模块执行次数大于设定次数，将状态标记置为4，并跳转第四数据优化模块；所述第四数据包括通过第三代价函数正向计算获取的各图像帧第四位置、各图像帧第四姿态、各图像帧第四速度、陀螺仪第四偏移值、图像特征点在相机坐标系的第四深度、加速度计第二偏移值、IMU和相机之间的第一外参数以及IMU和轮速计之间的第一外参数；
所述第四数据优化模块，配置为分别基于当前滑动窗口中的数据、第t帧的帧数据、图像特征点、第四数据以及当前滑动窗口的反向排列数据、第t帧的帧数据、图像特征点、第四数据，通过所述第三代价函数分别正向计算、反向计算获取第五数据；当前滑动窗口的反向排列数据，代表滑动窗口内各图像帧按照时间戳由晚到早的顺序前后排列；所述第五数据包括分别通过第三代价函数正向计算、反向计算获取的各图像帧第五位置、各图像帧第五姿态、各图像帧第五速度、陀螺仪第五偏移值、图像特征点在相机坐标系的第五深度、加速度计第三偏移值、IMU和相机之间的第二外参数以及IMU和轮速计之间的第二外参数；
所述输出模块，配置为输出所述第五数据中传感器位置信息构成的车辆轨迹。
10.一种存储装置，其中存储有多条程序，其特征在于，所述程序适于由处理器加载并执行以实现权利要求1-8任一项所述的车辆拐弯轨迹双向计算方法。
11.一种处理装置，包括
处理器，适于执行各条程序；以及
存储装置，适于存储多条程序；
其特征在于，所述程序适于由处理器加载并执行以实现：
权利要求1-8任一项所述的车辆拐弯轨迹双向计算方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911307058.XA CN110956665B (zh) | 2019-12-18 | 2019-12-18 | 车辆拐弯轨迹双向计算方法、系统、装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911307058.XA CN110956665B (zh) | 2019-12-18 | 2019-12-18 | 车辆拐弯轨迹双向计算方法、系统、装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110956665A CN110956665A (zh) | 2020-04-03 |
| CN110956665B true CN110956665B (zh) | 2023-06-23 |
Family
ID=69982511
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911307058.XA Active CN110956665B (zh) | 2019-12-18 | 2019-12-18 | 车辆拐弯轨迹双向计算方法、系统、装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110956665B (zh) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114089739B (zh) * | 2020-05-21 | 2024-06-18 | 深圳市海柔创新科技有限公司 | 导航方法及导航装置 |
| CN111795686B (zh) * | 2020-06-08 | 2024-02-02 | 南京大学 | 一种移动机器人定位与建图的方法 |
| WO2022061799A1 (zh) * | 2020-09-27 | 2022-03-31 | 深圳市大疆创新科技有限公司 | 位姿估计的方法和装置 |
| CN114322918B (zh) * | 2020-09-30 | 2024-02-13 | 广东博智林机器人有限公司 | 可移动设备状态的检测方法、装置和计算机可读存储介质 |
| CN113223161B (zh) * | 2021-04-07 | 2022-04-12 | 武汉大学 | 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法 |
| CN113962115B (zh) * | 2021-12-23 | 2022-04-05 | 深圳佑驾创新科技有限公司 | 车辆轮胎系数图优化标定方法、装置、设备及存储介质 |
| CN114440881B (zh) * | 2022-01-29 | 2023-05-30 | 海南大学 | 一种融合多源传感器信息的无人车定位方法 |
| CN116819973B (zh) * | 2023-08-29 | 2023-12-12 | 北京成功领行汽车技术有限责任公司 | 一种轨迹跟踪控制方法 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108629793A (zh) * | 2018-03-22 | 2018-10-09 | 中国科学院自动化研究所 | 使用在线时间标定的视觉惯性测程法与设备 |
| CN109764880A (zh) * | 2019-02-19 | 2019-05-17 | 中国科学院自动化研究所 | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9658070B2 (en) * | 2014-07-11 | 2017-05-23 | Regents Of The University Of Minnesota | Inverse sliding-window filters for vision-aided inertial navigation systems |
| US9709404B2 (en) * | 2015-04-17 | 2017-07-18 | Regents Of The University Of Minnesota | Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation |
| US10203209B2 (en) * | 2016-05-25 | 2019-02-12 | Regents Of The University Of Minnesota | Resource-aware large-scale cooperative 3D mapping using multiple mobile devices |
-
2019
- 2019-12-18 CN CN201911307058.XA patent/CN110956665B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108629793A (zh) * | 2018-03-22 | 2018-10-09 | 中国科学院自动化研究所 | 使用在线时间标定的视觉惯性测程法与设备 |
| CN109764880A (zh) * | 2019-02-19 | 2019-05-17 | 中国科学院自动化研究所 | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统 |
Non-Patent Citations (2)
| Title |
|---|
| zhenfei yang et al..Monocular visual-inertial state estimation with online initialization and camera-IMU extrinsic calibration.《IEEE transactions on automation science and engineering》.2016,第39-51页. * |
| 孙长库等.运动平台双IMU与视觉组合姿态测量算法.《传感技术学报》.2018,第1365-1372页. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110956665A (zh) | 2020-04-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110956665B (zh) | 车辆拐弯轨迹双向计算方法、系统、装置 | |
| CN111811506B (zh) | 视觉/惯性里程计组合导航方法、电子设备及存储介质 | |
| CN109764880B (zh) | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统 | |
| Ahmed et al. | Accurate attitude estimation of a moving land vehicle using low-cost MEMS IMU sensors | |
| CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| CN109991636A (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
| CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
| CN106814753B (zh) | 一种目标位置矫正方法、装置及系统 | |
| CN108132053B (zh) | 一种行人轨迹构建方法、系统及惯性测量装置 | |
| CN109141411B (zh) | 定位方法、定位装置、移动机器人及存储介质 | |
| CN106709222B (zh) | 基于单目视觉的imu漂移补偿方法 | |
| CN111238535A (zh) | 一种基于因子图的imu误差在线标定方法 | |
| CN113175933A (zh) | 一种基于高精度惯性预积分的因子图组合导航方法 | |
| EP3227634B1 (en) | Method and system for estimating relative angle between headings | |
| Cucci et al. | A flexible framework for mobile robot pose estimation and multi-sensor self-calibration | |
| CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
| CN103105166A (zh) | 一种运动练习拍的运动数据处理方法及系统 | |
| CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
| CN112284381B (zh) | 视觉惯性实时初始化对准方法及系统 | |
| CN116576849A (zh) | 一种基于gmm辅助的车辆融合定位方法及系统 | |
| JP6060548B2 (ja) | 測位装置、測位方法及びプログラム | |
| CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
| CN110864685B (zh) | 一种基于松耦合的车辆单目视觉轮式里程计定位方法 | |
| Martinelli | Closed-form solution to cooperative visual-inertial structure from motion |
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 |