CN111308415B - 一种基于时间延迟的在线估计位姿的方法和设备 - Google Patents
一种基于时间延迟的在线估计位姿的方法和设备 Download PDFInfo
- Publication number
- CN111308415B CN111308415B CN201911060513.0A CN201911060513A CN111308415B CN 111308415 B CN111308415 B CN 111308415B CN 201911060513 A CN201911060513 A CN 201911060513A CN 111308415 B CN111308415 B CN 111308415B
- Authority
- CN
- China
- Prior art keywords
- time delay
- measurement result
- result
- measurement
- pose
- 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 44
- 238000005259 measurement Methods 0.000 claims abstract description 146
- 238000004364 calculation method Methods 0.000 claims abstract description 30
- 238000013507 mapping Methods 0.000 claims abstract description 10
- 238000005457 optimization Methods 0.000 claims description 67
- 238000012545 processing Methods 0.000 claims description 23
- 230000000007 visual effect Effects 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 2
- 238000013473 artificial intelligence Methods 0.000 abstract 1
- 238000005096 rolling process Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 9
- 230000006870 function Effects 0.000 description 6
- 239000013598 vector Substances 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 5
- 230000002093 peripheral effect Effects 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 4
- 238000004891 communication Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 230000033001 locomotion Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000003111 delayed effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- HBBGRARXTFLTSG-UHFFFAOYSA-N Lithium ion Chemical compound [Li+] HBBGRARXTFLTSG-UHFFFAOYSA-N 0.000 description 1
- 239000002253 acid Substances 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000012512 characterization method Methods 0.000 description 1
- 230000001934 delay Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 239000000446 fuel Substances 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 229910001416 lithium ion Inorganic materials 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 238000001228 spectrum Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0247—Determining attitude
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C9/00—Measuring inclination, e.g. by clinometers, by levels
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/16—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using electromagnetic waves other than radio waves
- G01S5/163—Determination of attitude
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
本申请涉及人工智能，应用于移动机器人领域，提供了一种基于时间延迟的在线估计位姿的方法和设备，可以提高位姿估计的精确度。该方法包括：获取第一测量结果，第一测量结果为惯性传感器的测量结果，并根据该第一测量结果确定目标设备在多个测量时刻的位姿；获取第二测量结果，第二测量结果为视觉传感器的测量结果，该第二测量结果与第一测量结果之间具有时间延迟；对目标设备在多个测量时刻的位姿进行基于时间延迟的三维特殊正交群SO(3)指数映射插值计算，以得到插值计算后的位姿；将插值计算后的位姿与第二测量结果对齐，以得到对齐结果；根据对齐结果进行优化，以得到优化后的结果，优化后的结果包括优化后的位姿。
Description
技术领域
本申请涉及移动机器人领域，并且更具体地，涉及一种基于时间延迟的在线估计位姿的方法和设备。
背景技术
在自动驾驶、手机终端、无人机等机器人应用中，视觉传感器与惯性传感器(inertial measurement unit，IMU)结合可以给机器人提供更加精确的位姿估计。随着技术发展，视觉传感器从全局快门(global shutter)工业相机转向卷帘快门(rollingshutter)相机。然而，卷帘快门相机与IMU之间具有一定的时间延迟，例如，由于触发延迟、传输延迟和非同步时钟所带来的延迟，或者由卷帘快门效应引入的额外时间延迟。所以，卷帘快门相机与IMU没有严格的时间同步，导致位姿估计结果与真实位姿之间存在偏差。
因此，视觉传感器与IMU结合进行位姿估计的精确度需要进一步提高。
发明内容
本申请提供一种基于时间延迟的在线估计位姿的方法和设备，可以提高位姿估计的精确度。
第一方面，提供了一种基于时间延迟的在线估计位姿的方法，包括：获取第一测量结果，该第一测量结果为惯性传感器的测量结果，并根据该第一测量结果确定目标设备在多个测量时刻的位姿；获取第二测量结果，该第二测量结果为视觉传感器的测量结果，该第二测量结果与第一测量结果之间具有时间延迟；对目标设备在多个测量时刻的位姿进行基于时间延迟的三维特殊正交群SO(3)指数映射插值计算，以得到插值计算后的位姿；将插值计算后的位姿与第二测量结果对齐，以得到对齐结果；根据对齐结果进行优化，以得到优化后的结果，优化后的结果包括优化后的位姿。
由于惯性传感器的测量结果与视觉传感器的测量结果之间有时间延迟，如果直接基于这两者的测量结果进行位姿估计，所得位姿估计与真实位姿之间具有较大偏差。本申请实施例对根据惯性传感器测量得到的多个测量时刻的位姿进行基于时间延迟的插值计算，因此得到的优化后的位姿是考虑了时间延迟后的位姿，这样的位姿估计结果更加精确。
另外，本申请实施例采用SO(3)指数映射插值计算，相比于线性插值，SO(3)指数映射插值计算无需进行线性化处理和近似假设，可以提高插值结果的精确度，从而提高位姿估计结果的精确度。
结合第一方面，在第一方面的某些实现方式中，插值计算包括位置测量插值和姿态测量插值。
本申请实施例中的位姿包括位置和姿态。对位置测量结果和姿态测量结果分别进行SO(3)指数映射插值计算，相比于线性插值，无需进行线性化处理和近似假设，插值计算结果更加准确。
结合第一方面，在第一方面的某些实现方式中，优化为光束法平差优化。
本申请实施例提供了光束法平差优化方法来对对齐结果进行优化。这种优化方法得到的结果更加精确。
结合第一方面，在第一方面的某些实现方式中，优化后的结果还包括优化后的时间延迟。
本申请实施例中，插值计算结果和重投影误差方程包含时间延迟(作为未知量)，因此重投影误差也包含时间延迟，则对重投影误差进行优化后输出的优化后的结果也可以包括优化后的时间延迟。对于固定的惯性传感器和视觉传感器，时间延迟是一个固定但未知的量。根据本申请的上述实施例，可以在线标定得到上述固定时间延迟。
结合第一方面，在第一方面的某些实现方式中，将插值计算后的位姿与第二测量结果对齐，以得到对齐结果，包括：根据插值计算后的位姿建立重投影误差方程；根据插值计算后的位姿获取第二测量结果的特征测量值；将第二测量结果的特征测量值与第二测量结果代入重投影误差方程，计算第二测量结果的特征测量值与第二测量结果的偏差，该偏差为重投影误差，该重投影误差为对齐结果。
本申请实施例中，对重投影误差进行优化。理想情况下，重投影误差应当趋近于零。当重投影误差在预定范围内时，则可以认为此时的优化结果达到此次测量的最佳结果，则输出此时的优化结果，优化结果包括优化后的位姿估计。
结合第一方面，在第一方面的某些实现方式中，还包括：根据优化后的结果判断优化后的时间延迟是否收敛；如果优化后的时间延迟收敛，则确定所述优化后的位姿可信。
本申请实施例中，由于时间延迟是一个固定但未知的量，因此对于一次测量优化后得到的优化后的时间延迟不一定是真实值，可以对该次得到的优化后的时间延迟是否收敛做判断，如果该次得到的优化后的时间延迟是收敛的，则可以确定该次优化后的位姿可信。位姿的可信度信息可以供用户进一步参考使用，例如进行后续自动驾驶策略判断等。
结合第一方面，在第一方面的某些实现方式中，根据优化后的结果判断优化后的时间延迟是否收敛，包括：将上一次优化后的时间延迟代入根据此次插值计算后的位姿建立的重投影误差方程，获取第一重投影误差；将上一次优化后的时间延迟代入根据此次优化后的位姿建立的重投影误差方程，获取第二重投影误差；计算第一重投影误差和第二重投影误差的平均值；根据平均值，判定上一次优化后的时间延迟是否收敛。
结合第一方面，在第一方面的某些实现方式中，根据平均值，判定上一次优化后的时间延迟是否收敛，包括：若平均值在预定范围内，则判定上一次优化后的时间延迟收敛。
本申请实施例中，该预定范围可以是根据经验规定的一个范围，也可以是根据技术需要(例如精确程度的需要、计算速度的需要等)计算得到的范围。将上一次优化后的时间延迟分别代入此次优化前后的重投影误差方程中，若得到的重投影误差平均值在预定范围内，即第二测量结果的特征测量值与第二测量结果已经足够接近，也就是说，在考虑了上一次优化后的时间延迟的情况下，上一次优化后的位姿足够接近真实位姿，此次优化后的位姿也足够接近真实位姿，即上一次优化后的时间延迟已经足够接近真实的时间延迟固定值，因此可以认为上一次优化后的时间延迟收敛。
第二方面，提供了一种基于时间延迟的在线估计位姿设备，包括获取模块和处理模块，可以执行第一方面或第一方面的任一可选的实现方式中的方法。
第三方面，提供了一种电子设备，包括存储器和处理器，所述存储器上存储有可以用于指示执行上述第一或第一方面的任意可选的实现方式的程序代码，当所述代码被执行时，所述处理器可以实现方法中终端设备执行各个操作。
第四方面，提供了一种计算机存储介质，包括计算机程序，当计算机程序在计算机上运行时，使得计算机可以执行第一方面或第一方面的任一可选的实现方式中的方法。
附图说明
图1是本申请实施例可应用的一种示例车辆100的一种功能框图。
图2是本申请实施例的一种基于时间延迟的在线估计位姿的方法200的示意性流程图。
图3是本申请实施例中根据位姿Ik和位姿Ik+1进行插值计算的插值方式示意图。
图4是本申请实施例的一种根据时间延迟收敛判断优化后的估计位姿可信的方法400的示意性流程图。
图5是本申请实施例的一种基于时间延迟的在线估计位姿的设备500的示意性框图。
图6是根据本申请实施例的电子设备600的示意性框图。
具体实施方式
下面将结合附图，对本发明实施例中的技术方案进行描述。
为了便于理解本申请实施例，首先对本申请中涉及到的几个名词作简单说明。
1、特征点
图像处理中，特征点指的是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点(即两个边缘的交点)。图像特征点在基于特征点的图像匹配算法中有着十分重要的作用。图像特征点能够反映图像本质特征，能够标识图像中目标物体。通过特征点能够完成图像的匹配。
2、视觉惯性里程计(visual-inertial odometry，VIO)
系统利用视觉传感器(相机)和惯性传感器(IMU)采集到的数据，对自身运动状态进行估计。
3、三维特殊正交群(SO(3))
SO(3)也可以称为三维旋转特殊正交群。它也是一种李群，指具有连续光滑性质的群，具有群结构的实流形或者复流形，并且群中的加法运算和逆元运算也是流形中的解析映射。
图1是本申请实施例可应用的一种示例车辆100的一种功能框图。但是应当注意，本申请实施例不仅仅可以应用于车辆系统，也可以应用于其他需要进行位姿估计的系统或设备，例如无人机或自动机器人等。本申请对所应用的具体系统或设备的名称不作限制。
耦合到车辆100或包括在车辆100中的组件可包括推进系统110、传感器系统120、控制系统130、外围设备140、电源101、计算装置107以及用户接口108。计算装置103可包括处理器102、收发器103和存储器104。计算装置103可以是车辆100的控制器或控制器的一部分。存储器104可包括处理器102可运行的指令106，并且还可存储地图数据105。车辆100的组件可被配置为以与彼此互连和/或与耦合到各系统的其它组件互连的方式工作。例如，电源101可向车辆100的所有组件提供电力。计算装置103可被配置为从推进系统110、传感器系统120、控制系统130和外围设备140接收数据并对它们进行控制。计算装置103可被配置为在用户接口108上生成图像的显示并从用户接口108接收输入。
在其它示例中，车辆100可包括更多、更少或不同的系统，并且每个系统可包括更多、更少或不同的组件。此外，示出的系统和组件可以按任意种的方式进行组合或划分。
推进系统102可用于车辆100提供动力运动。如图所示，推进系统102包括引擎/发动机114、能量源113、传动装置(transmission)112和车轮/轮胎111。
推进系统102可以额外地或可替换地包括除了所示出的那些以外的组件。
传感器系统104可包括用于感测关于车辆100所位于的环境的信息的若干个传感器。如图所示，传感器系统的传感器包括全球定位系统GPS126、惯性测量单元(inertialmeasurement unit，IMU)125、激光雷达传感器124、相机传感器123、毫米波雷达传感器122以及用于为修改传感器的位置和/或朝向的致动器121。传感器系统120也可包括额外的传感器，包括例如监视车辆100的内部系统的传感器(例如，O2监视器、燃油量表、机油温度，等等)。传感器系统120也可以包括其它传感器。
GPS模块126可以为用于估计车辆100的地理位置的任何传感器。为此，GPS模块126可能包括收发器，基于卫星定位数据，估计车辆100相对于地球的位置。在示例中，计算装置103可用于结合地图数据105使用GPS模块126来估计车辆100可在其上行驶的道路上的车道边界的位置。GPS模块126也可采取其它形式。
IMU 125可以是用于基于惯性加速度及其任意组合来感测车辆100的位置和朝向变化。在一些示例中，传感器的组合可包括例如加速度计和陀螺仪。传感器的其它组合也是可能的。
激光雷达传感器(LIDAR)124可以被看作物体检测系统，该传感器使用光感测或检测车辆100所位于的环境中的物体。通常，LIDAR是可通过利用光照射目标来测量到目标的距离或目标的其它属性的光学遥感技术。作为示例，LIDAR单元124可包括被配置为发射激光脉冲的激光源和/或激光扫描仪，和用于为接收激光脉冲的反射的检测器。例如，LIDAR单元124可包括由转镜反射的激光测距仪，并且以一维或二维围绕数字化场景扫描激光，从而以指定角度间隔采集距离测量值。在示例中，LIDAR单元124可包括诸如光(例如，激光)源、扫描仪和光学系统、光检测器和接收器电子器件之类的组件，以及位置和导航系统。LIDAR单元124通过扫描一个物体上反射回来的激光确定物体的距离，可以形成精度高达厘米级的3D环境图。
相机传感器123可以用于获取车辆100所位于的环境的图像的任何相机(例如，静态相机、视频相机等)。为此，相机可被配置为检测可见光，或可被配置为检测来自光谱的其它部分(诸如红外光或紫外光)的光。其它类型的相机也是可能的。相机123可以是二维检测器，或可具有三维空间范围。在一些示例中，相机123例如可以是距离检测器，其被配置为生成指示从相机123到环境中的若干点的距离的二维图像。为此，相机123可使用一种或多种距离检测技术。例如，相机123可被配置为使用结构光技术，其中车辆100利用预定光图案，诸如栅格或棋盘格图案，对环境中的物体进行照射，并且使用相机134检测从物体的预定光图案的反射。基于反射的光图案中的畸变，车辆100可被配置为检测到物体上的点的距离。预定光图案可包括红外光或其它波长的光。
毫米波雷达传感器(millimeter-wave radar)122通常指波长为1～10mm的物体检测传感器，频率大致范围是10GHz～200GHz。毫米波雷达测量值具备深度信息，可以提供目标的距离；其次，由于毫米波雷达有明显的多普勒效应，对速度非常敏感，可以直接获得目标的速度，通过检测其多普勒频移可将目标的速度提取出来。目前主流的两种车载毫米波雷达应用频段分别为24GHz和77GHz，前者波长约为1.25cm，主要用于短距离感知，如车身周围环境、盲点、泊车辅助、变道辅助等；后者波长约为4mm，用于中长距离测量，如自动跟车、自适应巡航(ACC)、紧急制动(AEB)等。
控制系统130可被配置为控制车辆100及其组件的操作。为此，控制系统130可包括转向单元136、油门135、制动单元134、传感器融合算法133、计算机视觉系统132、导航或路线控制(pathing)系统131。
控制系统106可以额外地或可替换地包括除了所示出的那些以外的组件。
外围设备140可被配置为允许车辆100与外部传感器、其它车辆和/或用户交互。为此，外围设备140可包括例如无线通信系统144、触摸屏143、麦克风142和/或扬声器141。
外围设备140可以额外地或可替换地包括除了所示出的那些以外的组件。
电源101可被配置为向车辆100的一些或全部组件提供电力。为此，电源110可包括例如可再充电锂离子或铅酸电池。在一些示例中，一个或多个电池组可被配置为提供电力。其它电源材料和配置也是可能的。在一些示例中，电源110和能量源113可一起实现，如一些全电动车中那样。
包括在计算装置103中的处理器102可包括一个或多个通用处理器和/或一个或多个专用处理器(例如，图像处理器、数字信号处理器等)。就处理器102包括多于一个处理器而言，这种处理器可单独工作或组合工作。计算装置103可实现基于通过用户接口107接收的输入控制车辆100的功能。
收发器103用于该计算装置103与各个系统间的通信。
存储器104进而可包括一个或多个易失性存储组件和/或一个或多个非易失性存储组件，诸如光、磁和/或有机存储装置，并且存储器104可全部或部分与处理器102集成。存储器104可包含可由处理器102运行的指令106(例如，程序逻辑)，以运行各种车辆功能，包括本文中描述的功能或方法中的任何一个。
车辆100的组件可被配置为以与在其各自的系统内部和/或外部的其它组件互连的方式工作。为此，车辆100的组件和系统可通过系统总线、网络和/或其它连接机制通信地链接在一起。
应理解，图1中的车辆只是本发明实施例的一种应用场景，并不对本发明实施例构成限定。事实上，本发明实施例的应用范围并不限于自动驾驶车辆，还可以应用于无人机和其他移动机器人以及其他目标设备等。
图2是本发明实施例的一种基于时间延迟的在线估计位姿方法200的示意性流程图。如图2所示，该方法200包括以下内容。
S201，获取第一测量结果，并根据第一测量结果确定目标设备在多个测量时刻的位姿。
例如，当本发明实施例的一种基于时间延迟的在线估计位姿方法应用于自动驾驶车辆，则该位姿为自动驾驶车辆的位姿；当本发明实施例的一种基于时间延迟的在线估计位姿方法应用于无人机和其他移动机器人以及其他目标设备时，则该位姿为无人机和其他移动机器人以及其他目标设备的位姿。
该第一测量结果为惯性传感器的测量结果。例如，可根据该惯性传感器的测量结果，确定对应于时刻tk的位姿Ik和对应于时刻tk+1的位姿Ik+1。
可选地，惯性传感器与机体固连。
作为一个非限制性的具体实例，可将惯性传感器IMU在时刻tk的状态向量
表示为：
其中，
表示IMU在tk时刻的状态向量，k＝1,2,...,n。该状态向量包括位置
速度
姿态
和偏差ba，bω；上标{w}表示世界坐标系框架。
具体地，以IMU包括陀螺仪和加速度计的情况为例，IMU的测量结果可以包括陀螺仪输出的角速率和加速度计输出的加速度：
其中，
为加速度计的测量值，
为陀螺仪的测量值，它们还包含各项误差。at，ωt分别为加速度和角速率的实际值；
是世界坐标系到机体坐标系的旋转矩阵；gw是重力加速度；nat，nωt是零均值的高斯白噪声；偏差被建模为随机游走bat，bωt。
确定对应于时刻tk的位姿Ik和对应于时刻tk+1的位姿Ik+1可采用IMU预积分的方法。
具体地，对应于时刻tk的位姿Ik和对应于时刻tk+1的位姿Ik+1计算如下：
其中，
为IMU在tk+1时刻输出的位置、速度和姿态信息，由tk时刻的状态信息和IMU预积分计算得到。tk至tk+1时刻位置
速度
姿态
的预积分表示如下：
由此，可得到对应于时刻tk的位姿Ik为
和对应于时刻tk+1的位姿Ik+1为
是对应于时刻tk的姿态旋转矩阵。
S202，获取第二测量结果，第二测量结果与第一测量结果之间具有时间延迟。该第二测量结果为视觉传感器的测量结果。
此时，仅仅理解第一测量结果和第二测量结果之间存在时间延迟，但是该时间延迟的具体值是未知的，需要根据后续步骤得到优化后的时间延迟结果。
应注意，本申请实施例对S201和S202的执行顺序不作限制。例如，可以先进行S201，后进行S202；或者，可以先进行S202，后进行S201；或者，可以部分或者全部地同时进行S201和S202。
举例来说，视觉传感器的测量结果可包括实际场景的图像。该图像可被转为灰度图，用于下一步的特征提取。
例如，视觉传感器的测量结果可包括对应于图像触发时刻tk+td的坐标zk，即图像的触发时刻比IMU的测量时刻tk延迟td，其中td为未知量。
当视觉传感器为卷帘快门相机时，视觉传感器的测量结果还可能包括对应于时刻tk+td+tm的图像特征点。即图像特征点的获取时刻比IMU的测量时刻tk延迟td+tm，其中，tm由卷帘快门相机的逐行读取时间决定。例如，如果卷帘快门相机每一行的读取时间为tr，则图像的第m行的获取时刻比整个图像的初始获取时刻延迟tm＝tr*m。对于特定的卷帘快门相机，tr或tm应该是一个固定值，可以预先确定得到或者标定得到。
具体地，将S202中获取的图像进行处理。首先采用加速段特征检测(featuresfrom accelerated segment test，FAST)方法提取图像特征点，再采用光流法进行特征点追踪，取得连续被多个帧同时观测到的特征点并记录所取得的特征点。
可选地，将得到的特征点放在滑动数据窗口里进行管理，滑动数据窗口里储存的全部都是关键帧，并且每个特征点都可以被其中的多个关键帧观测到。每次获得新的图像测量信息时，滑动窗口内的特征都会进行观测更新。
S203，对目标设备在多个测量时刻的位姿进行基于时间延迟的SO(3)指数映射插值计算。
具体地，根据位姿Ik和位姿Ik+1进行插值计算，以得到对应于时刻tk+td+tm的位姿Ik+t。
对于连续两帧图像，IMU测量计算出对应于tk的位姿Ik和对应于时刻tk+1的位姿Ik+1。但是由于存在传感器之间的时间延迟，以及卷帘快门相机逐行曝光的影响，连续两帧图像中的前一帧图像的触发时刻比IMU的测量时刻tk延迟td，连续两帧图像中的前一帧图像中的特征点的获取时刻比IMU的测量时刻tk延迟td+tm，因此该特征点对应的IMU观测也不是Ik框架下的位姿，而是Ik+t。
图3是根据位姿Ik和位姿Ik+1进行插值计算的插值方式示意图。
具体地，可以先根据时间延迟td(或将所有时间延迟整体表示为td+tm)来计算比例系数，用于插值计算。比例系数λ的计算方法如下：
本发明实施例采用在SO(3)空间下，将姿态旋转矩阵映射到指数空间去进行插值计算，无需进行小角度假设和线性化处理，直接在相邻测量时刻之间进行插值，来对应带有时间延迟的特征点的测量信息。
(1)位置测量插值：
(2)姿态测量插值：
在SO(3)空间下，Ik相对于世界坐标系旋转矩阵
和对应的旋转矢量
之间的关系为：
其中
为
对应的反对称矩阵。
根据上述插值方法，可以得到Ik+t框架下的位姿为
S204，将插值计算后的位姿与第二测量结果对齐。
具体地，特征点fj在相机坐标系下的表征
为：
其中，j＝1,2,...,l，为特征点数目；
为相机和IMU之间的相对位姿，由标定得到。
量测模型为：
其中，
为特征点坐标，直接通过图像获取；h(·)为相机成像模型，将相机坐标系转换至特征点坐标系，相关参数可以通过相机标定获得；
为零均值高斯白噪声。
消除
后，可以得到通过IMU测量值计算得到的特征点坐标预测值
与特征点坐标真实值
之间的误差
由此可以建立重投影误差方程为：
S205，根据对齐结果进行优化。
可选地，优化采用光束法平差(bundle adjustment，BA)优化方式。
具体地，根据位姿Ik+t和图像特征点的坐标zk进行BA优化。
例如，将IMU测量值和计算值以及重投影误差进行BA优化，优化框架为包含IMU传播因子和重投影因子的目标函数：
其中，
是IMU传播的偏差，
是相机重投影误差，其中包括时间偏移td+tm；B和C分别是IMU和相机的测量信息。χ表示状态向量的集合，系统的整体状态向量为：
使用高斯牛顿法优化非线性最小二乘成本函数，记录每次优化后的结果。其中，每次优化后的结果包括优化后的位姿，优化后的位姿是对运动状态的实时估计。另外，在将时间延迟td作为未知量代入优化过程时，上述优化后的结果还可以包括优化后的时间延迟。
相比于不考虑时间延迟直接根据惯性传感器测量结果和视觉传感器测量结果进行的位姿估计，上述优化后的位姿更加接近真实的位姿。
本申请实施例中，由于时间延迟td是一个固定但未知的量，因此对于一次测量优化后得到的优化后的时间延迟不一定是真实的固定值，可以对该次得到的优化后的时间延迟是否收敛做判断，如果该次得到的优化后的时间延迟是收敛的，则可以进一步确定该次优化后的位姿可信。
由于tm由卷帘快门相机的逐行读取时间决定，一般是一个固定值，因此，在本发明实施例中，可以只判断td是否收敛。当然，也可以等价地将所有时间延迟td+tm作为整体考虑，代入上述优化方法，所得的结果也是等价的。
图4为本申请实施例的一种根据时间延迟收敛判断优化后的估计位姿可信的方法400的示意性流程图，该方法400包括如下内容。
S401，将上一次优化后的时间延迟代入根据此次插值计算后的位姿建立的重投影误差方程，获取第一重投影误差。
例如，上一次优化后的时间延迟为td1，此次优化后的时间延迟为td2。将td1代入根据此次插值计算后的位姿建立的重投影误差方程，此次插值计算后的位姿是根据此次获取的惯性传感器的测量结果确定的，由此可以计算得到第一重投影误差。
S402，将上一次优化后的时间延迟代入根据此次优化后的位姿建立的重投影误差方程，获取第二重投影误差。
将td1代入根据此次优化后的位姿建立的重投影误差方程，此次优化后的位姿是根据此次获取的惯性传感器的测量结果和视觉传感器测量结果确定的，由此可以计算得到第二重投影误差。
S403，根据第一重投影误差和第二重投影误差的平均值判断上一次优化后的时间延迟是否收敛。
计算第一重投影误差和第二重投影误差的平均值，若该平均值在预定范围内，例如，预定范围为(-0.2,0.2)，则判定该时间延迟td1已经收敛至稳定值。判定理由如下：如果第一重投影误差和第二重投影误差的平均值在预定范围内，即基于时间延迟td1确定的第二测量结果的特征测量值与第二测量结果在优化前后都已经足够接近，也就是说，在考虑了时间延迟td1的情况下，上一次优化后的位姿足够接近真实位姿，此次优化后的位姿也足够接近真实位姿，即时间延迟td1已经足够接近真实的时间延迟固定值，不因具体某一次测量获取的测量结果不同而导致位姿估计变化，因此可以认为时间延迟td1收敛。
S404，判断上一次优化后的位姿估计是否可信。
根据上述判定理由，如果时间td1收敛，则可以认为上一次优化后的位姿估计可信。另外，如果时间不td1收敛，则可以认为上一次优化后的位姿估计不可信，但由于相比于不考虑时间延迟直接根据惯性传感器测量结果和视觉传感器测量结果进行的位姿估计，该优化后的位姿已经更加接近真实的位姿，因此该优化后的位姿可以保留或者舍弃，本申请实施例在此不做限定。
图5是本发明实施例的一种基于时间延迟的在线估计位姿设备500的示意性框图。如图5所示，该设备500包括获取模块501和处理模块502。设备500可以是图1的车辆(或其他系统或设备)100的一个示例，也可以构成车辆100的一部分。设备500的各个模块可以实现上述图1至图4的各个实施例，为了避免重复，不再详细描述。
获取模块501，用于获取第一测量结果，该第一测量结果为惯性传感器的测量结果。
该获取模块501，还用于获取第二测量结果，该第二测量结果为视觉传感器的测量结果，该第二测量结果与第一测量结果之间具有时间延迟。
处理模块502，用于根据第一测量结果确定目标设备在多个测量时刻的位姿。
该处理模块502，还用于对目标设备在多个测量时刻的位姿进行基于时间延迟的三维特殊正交群SO(3)指数映射插值计算，以得到插值计算后的位姿。
该处理模块502，还用于将所述插值计算后的位姿与所述第二测量结果对齐，以得到对齐结果；
该处理模块502，还用于根据对齐结果进行优化，以得到优化后的结果，优化后的结果包括优化后的位姿。
图6示出了本发明实施例提供的电子设备600的示意性框图。如图6所示，所述电子设备600包括存储器601、处理器602和收发器603。电子设备600可以是图1的车辆(或其他系统或设备)100的一个示例，也可以构成车辆100的一部分。电子设备600的各个模块可以实现上述图1至图4的各个实施例，为了避免重复，不再详细描述。
存储器601，用于存储程序，所述程序包括代码；
处理器602，用于执行存储器601中的程序代码；
收发器603，用于和其他设备进行通信。
应理解，所述电子设备600可以是图1所示的一种示例车辆100的一种功能框图中的计算装置107。
可选地，当所述代码被执行时，所述处理器602可以实现方法200的各个操作，为了简洁，在此不再赘述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本发明实施例所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。
Claims (16)
1.一种基于时间延迟的在线估计位姿的方法，其特征在于，包括：
获取第一测量结果，所述第一测量结果为惯性传感器的测量结果，并根据所述第一测量结果确定目标设备在多个测量时刻的位姿；
获取第二测量结果，所述第二测量结果为视觉传感器的测量结果，所述第二测量结果与所述第一测量结果之间具有时间延迟；
对所述目标设备在所述多个测量时刻的位姿进行基于所述时间延迟的三维特殊正交群SO(3)指数映射插值计算，以得到插值计算后的位姿；
根据所述插值计算后的位姿建立重投影误差方程；
根据所述插值计算后的位姿获取所述第二测量结果的特征测量值；
将所述第二测量结果的特征测量值与所述第二测量结果代入所述重投影误差方程，计算所述第二测量结果的特征测量值与所述第二测量结果的偏差，所述偏差为重投影误差，所述重投影误差为对齐结果；
根据所述对齐结果进行优化，以得到优化后的结果，所述优化后的结果包括优化后的位姿。
2.根据权利要求1所述的方法，其特征在于，所述插值计算包括位置测量插值和姿态测量插值。
3.根据权利要求1或2所述的方法，其特征在于，所述优化为光束法平差优化。
4.根据权利要求1或2所述的方法，其特征在于，所述优化后的结果还包括优化后的时间延迟。
5.根据权利要求4所述的方法，其特征在于，还包括：
根据所述优化后的结果判断所述优化后的时间延迟是否收敛；
如果所述优化后的时间延迟收敛，则确定所述优化后的位姿可信。
6.根据权利要求5所述的方法，其特征在于，所述根据所述优化后的结果判断所述优化后的时间延迟是否收敛，包括：
将上一次优化后的时间延迟代入根据此次插值计算后的位姿建立的重投影误差方程，获取第一重投影误差；
将上一次优化后的时间延迟代入根据此次优化后的位姿建立的重投影误差方程，获取第二重投影误差；
计算所述第一重投影误差和所述第二重投影误差的平均值；
根据所述平均值，判定所述上一次优化后的时间延迟是否收敛。
7.根据权利要求6所述的方法，其特征在于，所述根据所述平均值，判定所述上一次优化后的时间延迟是否收敛，包括：
若所述平均值在预定范围内，则判定所述上一次优化后的时间延迟收敛。
8.一种基于时间延迟的在线估计位姿的设备，其特征在于，包括：
获取模块，用于获取第一测量结果，所述第一测量结果为惯性传感器的测量结果；
所述获取模块，还用于获取第二测量结果，所述第二测量结果为视觉传感器的测量结果，所述第二测量结果与所述第一测量结果之间具有时间延迟；
处理模块，用于根据所述第一测量结果确定目标设备在多个测量时刻的位姿；
所述处理模块，还用于对所述目标设备在所述多个测量时刻的位姿进行基于所述时间延迟的三维特殊正交群SO(3)指数映射插值计算，以得到插值计算后的位姿；
所述处理模块，还用于根据所述插值计算后的位姿建立重投影误差方程；
所述处理模块，还用于根据所述插值计算后的位姿获取所述第二测量结果的特征测量值；
所述处理模块，还用于将所述第二测量结果的特征测量值与所述第二测量结果代入所述重投影误差方程，计算所述第二测量结果的特征测量值与所述第二测量结果的偏差，所述偏差为重投影误差，所述重投影误差为对齐结果；
所述处理模块，还用于根据所述对齐结果进行优化，以得到优化后的结果，所述优化后的结果包括优化后的位姿。
9.根据权利要求8所述的设备，其特征在于，所述处理模块进行的所述插值计算包括位置测量插值和姿态测量插值。
10.根据权利要求8或9所述的设备，其特征在于，所述处理模块进行的所述优化为光束法平差优化。
11.根据权利要求8或9所述的设备，其特征在于，所述处理模块得到的所述优化后的结果还包括优化后的时间延迟。
12.根据权利要求11所述的设备，其特征在于，所述处理模块还用于：
根据所述优化后的结果判断所述优化后的时间延迟是否收敛；
如果所述优化后的时间延迟收敛，则确定所述优化后的位姿可信。
13.根据权利要求12所述的设备，其特征在于，所述处理模块根据所述优化后的结果判断所述优化后的时间延迟是否收敛，包括：
将上一次优化后的时间延迟代入根据此次插值计算后的位姿建立的重投影误差方程，获取第一重投影误差；
将上一次优化后的时间延迟代入根据此次优化后的位姿建立的重投影误差方程，获取第二重投影误差；
计算所述第一重投影误差和所述第二重投影误差的平均值；
根据所述平均值，判定所述上一次优化后的时间延迟是否收敛。
14.根据权利要求13所述的设备，其特征在于，所述处理模块根据所述平均值，判定所述上一次优化后的时间延迟是否收敛，包括：
若所述平均值在预定范围内，则判定所述上一次优化后的时间延迟收敛。
15.一种移动机器人，包括权利要求8至14中任一项所述的设备、惯性传感器和视觉传感器。
16.一种计算机存储介质，其特征在于，包括计算机程序，当所述计算机程序在计算机上运行时，使得所述计算机执行权利要求1至7中任一项所述的基于时间延迟的在线估计位姿的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911060513.0A CN111308415B (zh) | 2019-11-01 | 2019-11-01 | 一种基于时间延迟的在线估计位姿的方法和设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911060513.0A CN111308415B (zh) | 2019-11-01 | 2019-11-01 | 一种基于时间延迟的在线估计位姿的方法和设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111308415A CN111308415A (zh) | 2020-06-19 |
| CN111308415B true CN111308415B (zh) | 2022-09-02 |
Family
ID=71158067
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911060513.0A Active CN111308415B (zh) | 2019-11-01 | 2019-11-01 | 一种基于时间延迟的在线估计位姿的方法和设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111308415B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114812608A (zh) * | 2020-11-16 | 2022-07-29 | 浙江商汤科技开发有限公司 | 视觉惯性系统的参数标定方法及装置、电子设备和介质 |
| CN112833791B (zh) * | 2021-02-02 | 2021-11-19 | 浙江大学 | 一种自旋转线结构光扫描系统时空标定方法 |
| CN113075713B (zh) * | 2021-03-29 | 2024-06-18 | 北京理工大学重庆创新中心 | 一种车辆相对位姿测量方法、系统、设备及存储介质 |
| CN113591015B (zh) * | 2021-07-30 | 2024-07-23 | 北京小狗吸尘器集团股份有限公司 | 时间延迟的计算方法、装置、存储介质及电子设备 |
| CN114459506B (zh) * | 2022-02-28 | 2023-08-08 | 清华大学深圳国际研究生院 | 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统 |
| CN115908586B (zh) * | 2023-01-05 | 2024-11-26 | 中国人民解放军军事科学院国防科技创新研究院 | 时空标定方法、装置和存储介质 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9300880B2 (en) * | 2013-12-31 | 2016-03-29 | Google Technology Holdings LLC | Methods and systems for providing sensor data and image data to an application processor in a digital image format |
| CN108629793B (zh) * | 2018-03-22 | 2020-11-10 | 中国科学院自动化研究所 | 使用在线时间标定的视觉惯性测程法与设备 |
| CN109544638B (zh) * | 2018-10-29 | 2021-08-03 | 浙江工业大学 | 一种用于多传感器融合的异步在线标定方法 |
| CN110223348B (zh) * | 2019-02-25 | 2023-05-23 | 湖南大学 | 基于rgb-d相机的机器人场景自适应位姿估计方法 |
| CN110018691B (zh) * | 2019-04-19 | 2022-04-15 | 天津大学 | 小型多旋翼无人机飞行状态估计系统和方法 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
-
2019
- 2019-11-01 CN CN201911060513.0A patent/CN111308415B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN111308415A (zh) | 2020-06-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111308415B (zh) | 一种基于时间延迟的在线估计位姿的方法和设备 | |
| JP7297017B2 (ja) | オンボードセンサの外部パラメータを較正する方法及び装置、並びに関連する車両 | |
| CN109887057B (zh) | 生成高精度地图的方法和装置 | |
| US10436884B2 (en) | Calibration of laser and vision sensors | |
| CN108762245B (zh) | 数据融合方法以及相关设备 | |
| AU2018282302B2 (en) | Integrated sensor calibration in natural scenes | |
| CN110032180A (zh) | 激光雷达定位 | |
| US7463340B2 (en) | Ladar-based motion estimation for navigation | |
| EP2133662B1 (en) | Methods and system of navigation using terrain features | |
| CN112005079B (zh) | 用于更新高清地图的系统和方法 | |
| CN111156998A (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
| US20180075614A1 (en) | Method of Depth Estimation Using a Camera and Inertial Sensor | |
| CN110889808A (zh) | 一种定位的方法、装置、设备及存储介质 | |
| RU2767949C2 (ru) | Способ (варианты) и система для калибровки нескольких лидарных датчиков | |
| US12140975B2 (en) | Devices, systems and methods for navigating a mobile platform | |
| CN114111776A (zh) | 定位方法及相关装置 | |
| KR100901312B1 (ko) | 자율이동플랫폼의 지형데이터 생성 장치, 이를 구비하는 자율이동플랫폼 및 자율이동플랫폼의 지형데이터 생성 방법 | |
| CN112752693B (zh) | 车辆数据弹道估计 | |
| WO2022256976A1 (zh) | 稠密点云真值数据的构建方法、系统和电子设备 | |
| WO2021056283A1 (en) | Systems and methods for adjusting a vehicle pose | |
| US10249056B2 (en) | Vehicle position estimation system | |
| WO2021035748A1 (zh) | 位姿获取方法、系统和可移动平台 | |
| CN111712855A (zh) | 地面信息处理方法、装置和无人驾驶车辆 | |
| WO2022037370A1 (zh) | 一种运动估计方法及装置 | |
| CN118489238A (zh) | 数据采集方法、装置、以及智能驾驶设备 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |