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
矩形优化排样
主要研究矩形零件的排样方法，遗传算法的用途在此处体现的淋漓尽致本文算子的选择是有效的为进一步验证算法,对零件数量从16~97的不同算例进行试验,每类有3个例子,其最优排放图均已知,各个算例的基本试验数据(零件数量,板材尺寸)和本文试验最好结果见表2,表2算例2的基本试验数据及本文试验结果Tab 2 Dimension of second exampleand best result given by this paper问题零件最优高度原最优板材所得最低高度()SA+文算法结果(b)SA+最低水平线算法结果种类数量mm尺寸/mmmm2020×2020图1算例1的排放图40×15Fig 1 Layout of first example2860×3032表1本文算法与最低水平线算法排样结果对比C4496060×6064Tab. 1 Difference between our algorithm and7360×90the lowest outline algorithm9712080×120129最小高度最小高度最大高度平均高度运行时间图2给出了部分算例的最好排放结果。 Hopper算法/mm出现频次/mm/mm/ms8. Turton对以上规模不同的矩形件采用BL、BLFSA+最低水平线481/5053.716算法进行排放,允许零件旋转90°,GA、NE、SA、HCSA+本文方法483/505 I48.6等算法搜索排放顺序。文中指出采用BLF排放效果优于BL算法10%~30%,采用SA+BLF算法所得4.2算例2结果最优,见表3。(a)C11(b)C41(c)C61图2算例2采用本文算法所得的排放图ig 2 Best layout of second example with our algorithm表3各类别实例的相对距离百分比1表4各实例运行时间对比表Tab3 Relative distance of best solution toTab 4 Average elapsed time foroptimum height for six cases%six cases with different algorithm问题种类BIBLF SA+BLF本文算法问题A+BLFSA+本文算法174种类ms162.824126.7C41816132120C657.5注:1)表中值表示所得最好结果U与最优值lO)pt差值的白分比C61528189447(U-Op:)/lOpt。宇航材料工艺2007年第4期17对比表2、表3知,本文算法和文献[6]中采用图3表明:矩形排放耗时10ms,经人机交互调BLF解码的综合算法结果相近,并且在零件数量较整后材料利用率为86.4%,比人工排样提高约11少(如n=16)时能获得最优解,与埋论分析一致;由8%。表4知,本文算法的运行时间大大少于BLF算法,这5结论是因为在排放R;时只需搜索当前轮廓线段,比BLF实际算例表明最低轮廓线搜索算法能有效地进算法(搜索所有空域区域)搜索空间减少,因此效率明行矩形件排放,与模拟退火算法相结合,能在较短时显提高。由于文献[6的运行环境是:处理器奔腾间内获得与BLF算法相近的排放结果,并且在零件200MHκ,RAM65M, Windows nt4.0;而本文运行数量较少时能获得最优解,是解决大规模矩形件排放环境为:CPU2.8GHz,RAM512M,其速度大约是问题的有效方法200MHz处理器的15倍,因此表4所给BLF混合算参考文献法的运行时间做了相应处理。可见采用轮廓搜索法1张丽萍,张春丽,蒋寿伟.皮料优化排样的有效方法与BLF算法可获得相近的排放效果,但前者效率明软件学报,2005;16(2):316~323显高于后者。文献[7采用启发式递归(HR)算法对2曹炬,周济,余俊.矩形件排样优化的背包算法.中国以上算例进行求解,大大提高了运行效率,但在零件机械工程,1994;5(2):11~12数量较多时其速度也明显低于本文算法。因此最低3曹炬.二维异形切割件优化排样的拟合算法.中国机轮廓搜索法可用于求解大规模矩形件的排样问题。械工程,2000;11(4):438~4414.3应用举例1 Jakobs S On genetic algorithms for the packing of针对不规则复合材料铺层,采用矩形包络法求出 polygons,Eur. of oper,Res.,1996881):165-181其包络矩形,然后采用上述算法进行排放。图3是飞5贾志欣.面向发电设备制造的下料优化排样原理与关机坐舱罩顶棚的铺层展开数据采用以上策略获得的键技术,四川大学博士学位论文,2002排放图。6 Hopper E, Turton B C H. An empirical investigationof meta-heuristic and heuristic algorithms for a 2D packingproblem. EurJ of Oper Res, 2001; 128(1): 34577 Zhang Defu, Kang Yan, Deng Ansheng. A new heuristicrecursive algorithm for the strip rectangular packing problemComputers &. Operations Research, 2006; 33(8): 2209-2 217图3复合材料铺层排放实例(编辑李洪泉)ig. 3 Layout for composites plys18宇航材料工艺2007年第4期矩形件优化排样的研究旧万数据WANFANG DATA文献链接作者:邓冬梅,厝米水,安鲁陵,王桂宾, Deng Dongmei, Zhou laishui, An Luling,Wang guibin作者单位:南京航空航天大学机电学院,南京,210016刊名宇航材料工艺sTc|PKU英文刊名:AEROSPACe mATERIALS technology年,卷(期):2007,37(4)被引用次数4次惨考文献(条)1.张丽萍.张春丽.蒋寿伟皮料优化排样的有效方法[期刊论文]软件学报2005(02)2.曹炬.周济.余俊矩形件排样优化的背包算法[期刊论文]中国机械工程1994(02)3.曹炬二维异形切割件优化排样的拟合算法「期刊论文]中国机械工程2000(044.Jakobs S On geretic algorithms for the packing of polygons 1996 (05.贾志欣面向发电设备制造的下料优化排样原理与关键技术[学位论文]20026. Hopper E Turton B C H An empirical investigation of meta-heuristic and heuristic algorithms for a 2Dpacking problem 2001(01)7. Zhang Defu. Kang Yan. Deng Ansheng A new heuristic recursive algorithm for the strip rectangularpacking problem 2006 (08)相似文献(1条)1.学位论文邓冬梅复合材料铺层排样抆术硏究与开发2007复合材料因其比强度高、比模量大、材料的刚度和强度可设汁等一系列优点,在航空航天领域得到广泛应用,但高昂的价咯成了复合材料应用的最大壁垒。国外的硏究和应用成果表明数字化技术是降低复合材料构件制造成本、提高构件性能的有效途径。目前国内主要还以手工没计和手工制造为主、自动化程度不高,不仅浪费人力、物力,而且产品质量难以保证,因此有必要对复合材料数字化技术进行研究。优化排样是复合材料构件数字化生产过程中的重要环节。本文在研宄各种排样算法的基础上,提岀丁新的矩形件排样算法、优化算法以及不规则样片的排样算法,并与复合材料铺层排样的特点相结合开发了复合材料铺层排样软仁。主要研究内容和创新点如下矩形件排样不仅适用于矩形样片的排放,也是不规则样片排咩的基础。本文在建立矩形件排样数学模型的基础上,介绍了各种常见的定序列矩形件排样算法并分析其特点,提出了一种新的启发式排样算法——最低轮廓线搜索算法。该算法满足“最下最左”条件,克服了其他排样算法对某些排栏图不能给出排列的缺点,实验结果表明该算法排样效果好于最低水平线算法和最下最左(BL)算法。利用该算法实现了大量不同规格图纸的集中出图,省时省力,节约氏张2050%。矩形件排样问题具有图形运算和组合优化两方面的特性,单纯的排样算法只能解决图形运算问题,样片的排放顺序对排样结果同样重要。针对较小规模(一般少于100个图形)的矩形件排样问题,本文提岀了模拟退火与最低轮廓线搜索算法相结合的综合优化算法。对于十多个图形的排样,该算法可短时间内求得最优舾:对于近百个图形的排样,在排样效果相当旳情冮下,该亥算法比其基于模拟退火的综合优化算法效率提髙百以饣。针对大规模矩形件排样问题本文提出了蚁群笪法与最低轸廓线搜索算法相结合旳综合优化算法,该算法比模拟退火与最低轮廓线算法相结合的综合优化算法效率提高十倍以上。不规则图形排栏是所有排样研究中的热点和难点。本文将不规则样片简化成多边形进行排样,提出了两种不同的解法方法:一是基于矩形的排样方法,二是直接对多边形进行排样。基于矩形求解不规则样片排样时,将图形运算、矩形件排样算法及交互调整相结合,提出了基于矩形的多边形综合排样算法。通过各种优化组合策略,对单一样片和多种样片进行组合求其最小包络矩形,从而将不规则形状样片排样转化为矩形件排样进行求解。直接冄放多边形时,重点研究两个多边形的临界多边形(NFP)的求解。首先对基于倾斜图法的NFP求解法进行了改进和优化,完善了凹、凸两多边形NFP的求解,然后提出了适用于任意两多边形N求解的边界绕行法,该方法比基于倾斜图的求解方法适用范围广,计算简单、效率高。根据复合材料构件数宇化生产的主要过程,分析总结了复合材料铺层排样的特点,并将伉化排样算法与复合材料铺层排样的特点相结合,设计丌发了复合材料构件铺层排栏软件系统。引证文献(3条)1.卢远志杨建新.文桂林.周兵.钟志华基于排样思想的工程图坐标尺寸防干涉方法[期刊论文]中南大学学报(自然科学版)2010(2)2.张伟.安鲁陵.邵挠眀.郑盈一种矩形件分层排样算法[期刊论文]宇航材料工艺2010(1)3.陈婷.许超钣金零件排样技术及其发展[期刊论文]锻压装备与制造技术2008(4)本文链接http://d.wanfangdata.comcn/periodicAlyhclgy200704005.aspx授权使用:广东工业大学图书馆( gdgydxtsg),授权号:4flc88c5-bfdd-4dec-8ebf-9ec501113fe6下载时间:2011年4月14日
- 2020-12-09下载
- 积分：1
