视觉+惯性传感器的空间定位方法_算法实现

视觉+惯性传感器的空间定位方法1内容•各种空间定位方法简介–基于各种硬件传感器–Outside-in–Inside-out.•Vision+IMU融合的SLAM算法–IMU数学模型–基于优化的Vision+IMU融合算法–基于滤波的Vision+IMU融合算法2空间定位方法简介3空间定位技术的应用场景•军事–导弹–飞机•室外–自动驾驶–无人机•室内–机器人–吸尘器4空间定位和AR/VR5VR对SLAM的要求•对用户体验影响最大的几项指标–延时–相对位置精度–稳定性•相对不太重要的指标–Loopclosing–绝对误差–绝对方向6方向传感器机械陀螺仪激光陀螺仪MEMs陀螺仪7加速度传感器•重物通过弹簧固定在框架上•感知到加速度时，重物会发生相对运动•测量这种运动，并转化为电信号输出，得到加速度值•为了得到物体的实际加速度，还要减掉重力加速度8磁场传感器9RazerHydra可以感应磁场方向和强度手机里的数字罗盘芯片感应磁场方向硬件传感器比较精度缺点陀螺仪精度高，0.01度采样率：200Hz(手机)，1KHz(GearVR)加速度计精度较低，0.1m/s2用来估计位置误差很大磁场传感器精度低，容易受干扰采样率低：50Hz(手机)位置精度和速度都达不到VR的要求10Outside-In•通过外置的传感器进行定位•代表设备：Oculus，HTCVIVE，PSVR11HTVVIVELightHouse•设备上的红外传感器可以精确输出被照亮的时间•这个时间可以转换为相对于发射器的角度，可以理解为从发射器位置看到的所有亮点的2D坐标•相对于外置红外相机（Oculus）的好处–这些亮点的ID都是已知的，不需要做跟踪或匹配–只要时间测量足够精确，就可以达到非常高的精度，不受相机分辨率的限制X扫描开始X扫到y扫描开始y扫到12Inside-Out•传感器都集成在设备内部，通过观测外部场景确定自身的位置•代表设备：Hololens，Qualcomm,uSens13HololensQualcommVR820/835uSensPowerFingo空间定位算法比较精度优缺点基于传感器角度精度高，0.01度位置估计不了只能输出3DoFOutside-In精度高，小于1mm精度高，延迟小工作范围小成本高Inside-Out精度略低，约1mm不受场景限制精度稍低于Outside-in方法，延迟略高14视觉+IMU融合的SLAM算法15视觉+IMU融合16视觉方法单目相机无法解决尺度问题在纹理少的区域无法工作快速运动时图像模糊动态场景会有错误匹配IMU纯IMU预测会有漂移角度漂移相对时间t是线性的位置漂移是t2级别的视觉+IMU融合•代表产品：Hololens，GoogleTango,QualcommVR•好处–通过IMU减小图像匹配的搜索范围头快速旋转时可以达到每秒180度的速度–可以去掉错误的匹配点、错误的跟踪结果–可以在很少或没有特征点时继续跟踪–可以在图像帧间和之后预测位置，ATW17基础知识•四元数Quaternion–用欧拉角表示旋转，无法保证表达的连续性•SO(3)，SE(3)，Liegroup，LieAlgebra–SO(3)是3D空间中旋转变化构成的群。–SE(3)是3D空间中的刚性变换构成的群。用SE(3)表示变换，可以简化公式计算。–so(3)是SO(3)的局部线性展开•各种非线性方程组求解算法–GradientDescent，Gaussian-Newton，Levenberg-Marquadrt18基础知识•SO(3)上的高斯牛顿法19VisualSLAM算法•VisualSLAM算法一般包含两部分–Localization是指已知特征点的3D坐标，计算相机的3D位置–Mapping是指估计特征点3D坐标的过程•两种视觉特征–基于特征点的SLAM方法，依赖于场景中的特征点检测和匹配•在室内场景，主要特征都是线和边缘（墙的边缘、桌子和家具边缘）。特征点数目比较少。–DirectMethod，是直接通过图像像素匹配来优化相机位置•代表方法：LSD-SLAM，DSO-SLAM•两类优化方法：–基于滤波的方法，ExtendedKalmanFilter–基于优化的方法，OptimizationbasedMethod20IMU传感器•有用的数据–陀螺仪可以输出采样时刻的角速度w–加速度计可以输出采样时刻的加速度a•没用的数据–磁场传感器、GPS在室内实时定位中基本不用–有些IMU芯片上有自己的融合算法，可以直接输出融合数据。但是延迟和误差都比较大21IMU模型22数学模型状态量：R（相机角度），p（相机位置），v（运动速度），b（IMU的漂移）方程：从当前时刻的状态量，根据IMU数据，得到下一时刻的状态量滤波算法+IMU融合•在Filter方法中加入IMU信息–IMU信息作为predict过程中的控制量–每收到一个IMU数据，就更新states和covariance•传统KalmanFilter方法•MSCKF方法23优化算法+IMU融合•IMU信息作为约束项，加入优化目标函数•但是IMU采样速度很高，每个时刻的pose都优化的话，变量维数太高•Pre-Integration24优化算法+IMU融合•Pre-Integration25ChristianForster,etc.IMUPreintegrationonManifoldforEfficientVisual-InertialMaximum-a-PosterioriEstimationChristianForster,etc.On-ManifoldPreintegrationforReal-TimeVisual-InertialOdometry优化算法+IMU融合•Pre-Integration–将IMU信息转为跟关键帧状态无关的帧间约束26优化算法+IMU融合•Pre-Integration深入学习–噪声模型–Bias模型•如何求解bias•Bias更新后，不需要重新计算27Visual+IMU实际系统•难点284.IMU输出数据很快，1KHz相机帧率很慢，60Hz3.IMU和相机并不在同一位置2.加速度计输出的是物体自身加速度和重力加速度的和。而且误差比较大，0.1m/s2。a+GaG1.IMU在不同温度下有漂移。采样率不够高，可能会丢失信息。Visual+IMU实际系统29对于实际系统，IMU的速度可以达到1000Hz。而相机一般是30~60Hz。一个图像周期内会有很多IMU数据。手机上的IMU，情况又不一样。手机IMU速率为100Hz，但是采样时间和图像时间并不相同。需要精确得到图像和IMU数据的时间差。Visual+IMU实际系统•IMU-Camera坐标系变换•以IMU坐标系为主，对投影方程进行修改30𝑢𝑣~𝐾∙𝑅𝑐𝑎𝑚𝑖𝑚𝑢∙(𝑅𝑖𝑚𝑢∙𝑋−𝑡𝑖𝑚𝑢−𝑡𝑐𝑎𝑚𝑖𝑚𝑢)IMU坐标系Camera坐标系Visual+IMU实际系统•ATW是重要的后处理步骤，可以有效减少眩晕•用户看到图像的时间，实际上是图像时刻+处理时间+渲染时间的最终时刻。因此需要告诉渲染模块，在最终时刻相机所处的方向R和位置p31当前图像时间，可以得到准确的R,p通过IMU数据估计得到R,p，相对准确渲染结果显示时刻的R,p，没有IMU数据，完全靠预测算法耗时渲染耗时显示耗时Demo32