VINS论文推倒及代码解析
VINS 的功能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程,分别是:前端图像跟踪、后端非线性优化(其中初始化和 IMU 预积分在这个线程中)、闭环检测、闭环优化。、总体框架Measurement PreprocessingInitializationCamera(30hz)Feature Detectionnd rackerVisual-lnertialInitializedis- onlySfMAlignmentIMU (100hMU Pre-integrationLocal Visual-Inertial: OldestSliting WindowNewestNonli+、 Keyframe?OptimizationBundle Adjustment II Loop detectionwith RelocalizationStates from Loop ClosureFealure retrievel oop Deleted二二1---11------22===Global Pose Graph4-DoF Pose Graph OptimizationKeyframe DatabaseOptimization图1VINS框架ⅵINS的玏能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线稈,分别是:前端图像跟踪、后端非线性伉化(其中初始化和IMU预积分在这个线程中)、闭环检测、闭环优化各个功能模块的作用上要有:1.I图像和MU预处理●图像:提取图像 Harris角点,利用金字塔光流跟踪相邻帧,通过 RANSAC去除异常点,最后将跟踪到的特征点push到图像队列中,并通知后端进行处理●IU:将IMU数据进行积分,得到当前时刻的位置、速度和旋转(PVQ),同时计算在后端优化中将用到的相邻帧的预积分增量,及预积分误差的 Jacobian矩阵和协方差项。1.2初始化首先,利用SFM进行纯视觉佔计滑窗內所有帧的位姿及3D点逆深度,最后与IMU预积分进行对齐求解初始化参数1.3后端滑窗优化将视觉约束、IMU约束和闭环约束放在·个大的目标函数中进行非线性优化,求解滑窗内所有帧的PVQ、bias等。L M States in the sliding windowIMU:k States from loop clos1Camera:冷 MU measurements>visual measurements★ Catur图2滑窗优化示意图14闭环检测和优化利用D)BoW进行闭环检测,当检测成功后进行重定位,最后对整个相机轨迹进行闭环优化。U预积分VisionIMUVision图3MU预积分示意图21当前时刻pVQ的连续形式将第k唢和第kl帧之间的所有IMU进行积分,可得第kHI帧的位置、速度和旋转(PVQ),作为视觉估计的初始值,这里的旋转采用的四元数。v△t+k+1∈[k,k+1]rW(at-ba ) -owletbk JtE[k, k+1]n(,-bdt∈[k,k+1]其中,a2和O为ⅠMU测量的加速度和角速度,是在Body自身坐标系, world坐标系是IMU所在的惯导系,上式的旋转公式推导可参考附录10.1。22当前时刻PVQ的中值法离散形式公式(1)给出的是连续吋刻的相机当前PVR的达代公式,为了跟代码致,下面给出基于中值法的公式,这与 Estimator:; processIMg(O函数中的Ps]、Rs]和Vs是一致的,IMU积分出来的第j时刻的物理量可以作为第j帧图像的初始值。tr t+a26t(2)ka,St其中q(a1-ba)-g"+q:+1(a+1-ba)(a;+o;+1)2.3两帧之间PVQ增量的连续形式通过观察公式(1)可知,IvU的预积分需要依赖与第k帧的ν和R,当我们在后端进行非线性优化时,需要迭代更新第κ唢的ν和R,这将导致我们需要根据每次迭代后值重新进行积分,这将非常耗吋。因此,我们考虑将优化变量从第k帧到第κ+1帧的IU预积分项中分离开来,通过对公式(1)左右两侧各乘Rb,可化简为:R(+p2k-=2△)+ak+1b其中DtElk, k+1t∈[k,k+1R k(at-bar)ldt)Wendtt∈[kk+1这样我们就得到了连续时刻的MU预积分公式,可以发现,上式得到的MU预积分的值只与不同时刻的a2和o相关。这里我们需要重新讨论下公式(5)预积分公式,以ab,为例,我们发现它是与MU的bias相关的,而bias也是我们需要优化的变量,这将导致的问题是,当每次迭代时,我们得到一个新的bias,又得根据公式(巧5)重新对第k帧和第k+1帧之间的IMU预积分,非常耗时。这里假设预积分的变化量与bias是线性关系,可以写成:ab,+/6n6ba+/16b+8 8ba +p(6)k+1sb24两帧之间PVQ增量的欧拉法离散形式面给出离散时刻的IMU预积分公式,首先按照论文中采用的欧拉法,给出第i个MU时刻与第i1个IMU时刻的变量关系为b+1k+的6t+元R(P)(1+R(P")(a2-bbn)δt25两帧之间PⅤQ增量的中值法离散形式卜面给出代码中采用的基」中值法的IMU预积分公式,这与 Estimator: processIMUO函数中的 Integration Base: push backo上是一致的。注意这里跟公式(2)是不一样的,这里积分出来的是前后两顿之间的IU增量信息,而公式(2)给出的当前帧时刻的物理量信息+1+B k St +=a, &tbb+1Bi + au1其中a,=slqilai-bai)+qiDi t aitl2.6连续形式下PVQ增量的误差、协方差及 JacobianIMU在每个吋刻积分出来的值是有误差的,下面我们对误差进行分析。首先我们直接给出在t时刻误差项的导数为:sa00016a000000-82(066hkR;0006|=00-(a-bh)0-1192k|+|000|mLL000016ba00101n000018b000F+ozk+ Gt其中:F25×15,G215×2,62x1,n12×,上式推导可参考附录102。下面我们讨论它的作用,将其可以简写为:6之k=F62z+Gtnt根据导数定义可知:62b=1m24-6262+8=62+628t=(+F6t)6z+(Gt6t)nt(11)这里我们对公式(1)的IMU误差运动方程再说明,将上式和EKF对比可知,上式恰好给出了如EKF一般对非线性系统线性化的过程,这里的意义是表示下一个时刻的IMU测量误差与上一个时刻的成线性关系,这样我们根据当前时刻的值,可以预测出下一个时刻的均值和协方差,而公式(1)给出的是均值预测,协方差预测公式如下Pb+6=(1+Ft)P(+Fl6t)7+(G,t)Q(G18t)ot(12)上式给出了协方差的选代公式,初始值Pk=0。其中,Q为表示噪声项的对角协方差矩阵000003000另外根据(11)式可获得诀差项的 Jacobian的迭代公式:(I+F26t)(14)其中 Jacobian的初始值为bk=12.7离散形式的PVQ增量误差分析我们首先直接给出PVQ增量误差在离散形式下的矩阵形式,为了与代码一致,我们修改下变量顺序,这和代码中 midPointIntegration(函数是一致的。(但不知为何计算的V中与前四个噪声项相关的差个负号?)1t fo660f106t‖loeBk+1=0f211f20016bδb0[6b102001rnot000kRkotk+1(15006t0n0000δt其中,推导可参考附录10.3:stE(ak-ba)02-4B+1(kk+121k+1b.)6t|6t2(Rr+ rk+18t2Stn=71=Rk+1(a+1-b)6tWr+ wf1=Ik+11+Gb。)δt-Rn+1(ak+121=-2配+1Stl st21(RK+Ruts)4rula1RrotstStR+1(a1R,+114/+11t28离散形式的PVQ增量误差的 Jacobian和协方差将公式(15)简写为:k+1F15×158215×1+V15×13Q则 Jacobian的迭代公式为k+15×15=F/k(16)其中, Jacobian的初始值为/k=l。这里计算出来的k+1只是为了给后面提供对bias的acoblar。协方差的迭代公式为P+15×15=FPFr+vQv(17)其中,初始值P=0。Q为表示噪声项的对角协方差矩阵:00000000aa000Q18×180a00(18)000000三、后端非线性优化31状态向量状态向量共包括滑动窗口内的n+l1个所有相机的状态(包括位置、朝向、速度、加速度计bias和陀螺仪bias)、 Camera到IMU的外参、m+1个3D点的逆深度X=[xr=pw,vb bpc,q3.2目标函数吗+(喻,2)+2(19)其中三个残差项即误差项分别为边缘化的先验信息、IMU测量残差、视觉的重投影残差。三种残差都是用马氏距离表示。根据《十四讲》中高斯牛顿法,若要计算目标函数的最小值,可以理解为,当优化变量有一个增量后,目标函数值最小,以IU残差为例,可写成如下所示:nin lre2bk, X+8Xrk x)+HSⅩDk+1oXk+1k+1其中HB,为B关于 XIK Jacobian,将上式展开并令关于6X的导数为0,可得增量δx的计算公式:H k 8X=k+1TB那么,公式(28)可写成+∑+∑Tk∑1rc上式中,B为MU预积分噪声项的协方差,P为vual观测的噪声协方差。当MU的噪声协方差P越大时,其信息矩阵Pk,将越小,意味着该MU观测越不可信,换句话说,因MU噪声较大,越不可信IMU预积分数据,而更加相信 visual观测。注意,这里的IMU和vsua协方差的绝对值没有意义,因为考虑得是两者的相对性可将上式继续简化为:(Ap+AB +Acox=bp +bB +bc其中,Ap,AB和Ac为 Hessian矩阵,上述方程称之为增量方程。33MU约束1)残差:两帧之间的PVQ和bias的变化量的差△tx+k+1bk qbk+1bR+1 xyz+g"△t)-Bk(20)sbbbb其中各增量关于bias的 Jacobian可从公式(16)的大 Jacobian中的相应位置获得。上面与代码中 Integration base: evaluateD对应,2)优化变量pb, 0W, Svb ,8ba:,bor Opb,, 80W ,Swb,, bakr, Sba3)Jacobian:计算 Jacobian时,残差对应求偏导对象分别为p6e,6vB,6h,ba],6b,6b
- 2020-12-07下载
- 积分:1
CX-Protocol协议宏操作手册(中文)
CX-Protocol协议宏操作手册(中文)欧姆龙协议通信声明:OMRON公司生产的产品应由合格的操作员按正确步骤使用,并且仅用于本手册中所描述的用途本手册中的以下约定用于指明手册中的注意事项并对其进行分类。请务必注意这些约定所规定的情况。疏忽这些事项会导致人身伤害或产品损坏。危险表示若不遵守,将很有可能导致死亡或严重受伤的事项。此外,还可能会造成严重的财产损害。警告表示若不遵守,将有可能导致死亡或严重受伤的事项。此外,还可能会造成严重的财产损害。注意表示若不遵守,将有可能导致相对较严重或轻度受伤、产品损坏或故障运行的事项。OMRON产品附注在本手册中,所有 OMRON产品均以大写字母表示。当“Unit”用于指称 OMRON产品名称时,不管它是否是产品的正式名称,也以大写字母表示。缩写“Ch”出现在某些显示和某些 OMRON产品中时,往往表示“字”。在这个意义上,文件中缩写为“Wd”。缩写“PLC”表示“可编程序控制器”,缩写“PC”表示个人计算机并且不月作任何其它词语的缩写。缩写“PMSU”表示“协议宏支持单元”,是指用于C200HX/HG/HE的通信板、用于CS的串行通信板和用于CSCJ的串行通信单元。直观标题出现在本手册左侧的下列标题用于帮助区分不同类型的信息。注表示对产品操作的有效性和方便性特别重要的信息。犭,23..1.表示各个类别的列举说明,例如操作步骤、检查表等。C OMRON, 2008版权所有。未经 OMRON公司事先书面允许,不得将本出版物的任何部分以任何形式或任何方式(机械、电子、照相、录制或其它方式)进行复制、存入检索系统或传送。使用本手册所包含的信息不负专利责任。由于 OMRON公司始终致力于改进其高质量产品,所以本手册所包含的信息可随时改变而不另行通知。虽然在编制本手册时收录了每种可能的注意事项,但对于仍然可能出现的错误或遗漏, OMRON公司不承担任何责任。同样, OMRON公司对于因使用本手册所包含的信息而造成的损害也不承担任何责任。目录注意事项■曹■■D■■XXIII阅读对象XXI2般注意事项3安全注意事项4运行环境注意事项xxx5应用注意事项6软件操作步骤第1章1-1CX- Protocol概述1-2特点.1-3支持的PLC型号和个人计算机系统配置1-5协议宏1-6协议宏结构1-7由CX- Protocol创建的数据171-8CX- Protocol的主画面181-9项目创建概要211-10整合的标准系统协议1-11协议宏使用的基本步骤1-12规格301-13协议宏之间的差异32第2章安装/卸载/启动/结束352-1连接到PLC362-2安装和卸载软件39启动392-4关闭402-5用户界面概要40第3章协议宏513-1协议宏概要3-2序列属性(所有步通用).3-3步属性783-4通信报文属性893-5创建矩阵1053-6标准系统协议示例1083-7通信序列示例1103-8执行一个已创建的通信序列CS/CJ3-9执行一个已创建的通信序列(C20HXHG/HE)1203-10辅助区和数据分配区..126第4使用协议宏功能1394-1协议宏功能的应用范围.1404-2协议创建过程1424-3传送控制模式设置目录4梯形图编程方法1474-5监测时间的计算方法1586运行确认1604-7协议执行时发生的错误1614-8通信响应时间性能1694-9循环时间执行l81第5章创建目标1835-1创建项目和协议1845-2创建序列和步····1885-3创建报文和被动响应1895-4系统协议显示和编辑191第6章项目和协议编辑.1936-1编辑项目.1946-2编辑协议196第7章设定和编辑序列,,,,,,,,,,,,,,,,,,,,,1977-1设定序列1987-2编辑序列第8设定和编辑步2078-1步设定2088-2步编辑.214第9章设定和编辑报文和矩阵列表,,2179-1创建报文9-2创建矩阵2329-3报文和矩阵编辑233第10章通信PLC设置和在线237l0-1PLC系统构成.10-2个人计算机和PLC之间的通信设定23910-3在线与模式变更24610-4调制解调器连接10-5创建IO表10-6PMSU通信端口设定10-7将通信端口设定数据传送至PLC第11章传送和打印协议数据26111-1计算机和PMSU之间传送和读取协议数据.26211-2打印协议26711-3从PST/PSS文件导入协议数据26911-4CSCJ协议和C200HXHG/HE协议271目录第12章跟踪和监测27312-1跟踪传送线路27412-2PLC存储器窗口概要27812-3ⅠO存储器显示和编辑12-4IO存储器监测28812-5IO存储器传送和对比293第13章错误和出错日志显示297第14故障诊断305第15章帮助313附录附录A创建协议应用程序附录BPLC设置和PMSU设定329附录C对RS-232C电缆连接器进行配线335修订记录,,,,,,,,,,,,,339目录关于本手册:CⅩ- Protocol是协议宏功能的支持软件。本手册对CX- Protocol的安装和操作进行说明。手册中包含下述章节。本手册供下列人员使用负责安装FA设备的人员设计FA系统的人员管理FA设施的人员名称样本编号内容提要CXONE-AL C-V4/AL D-V4W344描述使用CX- Protocol来生成协议宏作为通信序列,从而与外部设CX-Protocol本手册)备进行通信的方法。(本手册)操作手册CXONE-AL C-V4/AL D-V4W463CX- One fa整合工具包的安装和概要。CX-One设置手册SYSMAC CS/C J系列W336描述使用串行通信单元和通信板与外部设备进行串行通信的方法CSIW-SCB 1-V1 CSIW-SCU1-V1(包括使用 OMRON产品的标准系统协议)CJIW-SCU 1-VI CJIW-SCU 2串行通信板和串行通信单元操作手册SYSMAC CS/CJ/CP/NS.J系列W342描述 CS/CJ/CP系列PC和NSJ系列控制器使用的C系列(上位链CSIG/H-CPU H接)和FIⅠNS通信命令。SIG/H-CPU -EVICSID-CPUCSIW-SCU -VIC J2H-CPU6 -EIPC. 12H-CPU6C J2M-CPU1C 2M-CPU3C J1H-CPU H-RCJIC/H CPU HCJIG-CPU PCJIG-CPUC JIM-CPUC -V1CPIL-MILCPIH-XCPIH-XACPIH-YCPlE-ED一CPlE-ND一NSI-(B)-G5DBM3D通信指令参考手册SYSMAC CJ系列W472提供CJ系列C2CPU单元的设计、安装、维护和其它基本操作的CJ2H-CPU6 -EIP概要及详细描述。C J2H-CPU6包含下述信息C 2M-CPUl概述和功能C J2M-CPU3系统配置可编程序控制器安装和配线硬件操作手册故障诊断请将本手册与W473结合使用SYSMAC CI系列W473描述使用CJ2CPU单元的编程和使用其功能的其它方法。C J2H-CPU6 -EIP包含下述信息C J2H-CPU6C J2M-CPUl内部存储区C J2M-CPU3可编程序控制器软件操作手册CPU单元内置功能请将本手册与W472结合使用。SYSMAC CS系列W339描述CS系列PLC的安装和操作。CSIG/H-CPU HCSIG/H-CPU -EVI可编程序控制器操作手册名称样本编号内容提要SYSMAC CJ系列W393描述CJ系列PIC的安装和操作CJlH- CPU H-E、CJ1G/H-CPUH、CJ1G-CPUP、CJ1G-CPUCJIM-CPU可编程序控制器SYSMAC CS/CJ/NS.系列W394描述CSCJ/NSJ系列PLC的编程和使用其功能的其它方法。CSI G/H-CPU H. CSIG/H-CPU -EVI包含下述信息CSID-CPUH、CS1D-CPUS编程CJIH-CPUH-R、CJ1G/H-CPUH、任务CJlG-CPUP、CJlG-CPU文件存储器CJ1M-CPU其它功能B)-M3D将本手册与《 SYSMAC CS系列操作手册》(w339)或《 SYSMAC可编程序控制器CJ系列操作手册》(W393结合使用。扁程手册YSMAC CS/CI系列W474描述CsCJ系列或NSJ系列PLC所支持的梯形图编程指令。CSIG/H-CPU编程时,请将本手册与《操作手册》或《硬件操作手册》(CSICSIG/H-CPU HW339,CJ1:W393或C』2W472)和《编程手册》或《软件操作CSID-CPUH、CSlD-CPU手册》CSl/CJ:W394或CJ2:W473)结合使用。CJ2H-CPU6 -EIP, C.J2H-CPU6CJ2M-CPUI、CJ2M-CPU3CJ1H-CPU H-RCJ1G-CPU CJIG/H-CPU HCJ1G-CPUP、 CJIM-CPUYSMAC One Ns.J系列B)-G5D(B)-M3D可编程序控制器令参考手YSMAC CP系列450对CP系列提供了以下几个方面的信息CPIH-X概述/特性CPIH-XA·系统配置CPIH-Y安装和配线I/O存储器分配CPIH CPU单元操作手册故障诊断将本手册与《CPIH可编程序控制器编程手册》(W451)结合使用。系列W452对NSJ系列NSJ控制器提供了以下几个方面的信息NSJ5-tQ B)-G5D, NSJ5-SQ (B)-G5D概述和功能NSJ8-TV(B)-G5D, NSJ10-TV(B)-G设计系统配置NSJ12-Ts(B)-G5D、NS5-TQ(B)-M3D、安装和配线SJ5sQ(B)M3D、 NSJ8 T存储器分配NSJW-ETN21、NSJW-CLK21-V1、故障诊断与维护NSJW-IC1Ol请将本手册与下述手册结合使用:《 SYSMAC CS系列操作手册》操作手册W339《SMAC CJ系列操作手册》YSMAC CS/C.系列编程于册》(W394)和《Ns-V1/V2系列设置于册)(V083)SYSMAC CP系列W451对CP系列提供了以下几个方面的信息CPIH-X40D一编程指CPIH-XA40D·编程方法CPlH-Y2ODT-D任务CPIL-I14D一文件存储器功能CPIL L2OD请将本手册与《CP系列 CPIH CPU单元操作手册》(W450)和《CPCPIL-M3OD系列 CPIL CPU单元操作手册》(W462)结合使用。CPIL-M4ODCPIH和 CPIL CPU单元编程手册XONE-AL C-V4/AL D-V4W446提供了有关如何使用 CX-Programmer(一种支持CS(J系列PLC的编程设备)以及CX- Programmer中所包含的CX-Net的信息手册AC CS/CJ系列W341提供有关如何使用编程器对CSCJ系列PLC进行编程和操作的信CQMIH-PRO-EIC200H-PRO27-E CQMI-PROOI-E扁稈器作手册
- 2020-12-10下载
- 积分:1