• 四川郎酒股份有限公司获第十二届人民企业社会责任奖年度环保奖 2019-05-13
  • 银保监会新规剑指大企业多头融资和过度融资 2019-05-12
  • 韩国再提4国联合申办世界杯 中国网友无视:我们自己来 2019-05-11
  • 中国人为什么一定要买房? 2019-05-11
  • 十九大精神进校园:风正扬帆当有为 勇做时代弄潮儿 2019-05-10
  • 粽叶飘香幸福邻里——廊坊市举办“我们的节日·端午”主题活动 2019-05-09
  • 太原设禁鸣路段 设备在测试中 2019-05-09
  • 拜耳医药保健有限公司获第十二届人民企业社会责任奖年度企业奖 2019-05-08
  • “港独”没出路!“梁天琦们”该醒醒了 2019-05-07
  • 陈卫平:中国文化内涵包含三方面 文化复兴表现在其中 2019-05-06
  • 人民日报客户端辟谣:“合成军装照”产品请放心使用 2019-05-05
  • 【十九大·理论新视野】为什么要“建设现代化经济体系”?   2019-05-04
  • 聚焦2017年乌鲁木齐市老城区改造提升工程 2019-05-04
  • 【专家谈】上合组织——构建区域命运共同体的有力实践者 2019-05-03
  • 【华商侃车NO.192】 亲!楼市火爆,别忘了买车位啊! 2019-05-03
    • / 16
    • 下载费用:30 金币  

    重庆时时彩历史网: 基于航向辅助分布式SLAM的机器人自主导航方法.pdf

    摘要
    申请专利号:

    重庆时时彩单双窍门 www.4mum.com.cn CN201610154634.1

    申请日:

    2016.03.17

    公开号:

    CN105606104A

    公开日:

    2016.05.25

    当前法律状态:

    实审

    有效性:

    审中

    法律详情: 实质审查的生效IPC(主分类):G01C 21/20申请日:20160317|||公开
    IPC分类号: G01C21/20 主分类号: G01C21/20
    申请人: 北京工业大学
    发明人: 裴??? 程雨航; 武小平; 严鸿
    地址: 100124 北京市朝阳区平乐园100号
    优先权:
    专利代理机构: 北京思海天达知识产权代理有限公司 11203 代理人: 沈波
    PDF完整版下载: PDF下载
    法律状态
    申请(专利)号:

    CN201610154634.1

    授权公告号:

    |||

    法律状态公告日:

    2016.06.22|||2016.05.25

    法律状态类型:

    实质审查的生效|||公开

    摘要

    本发明公开了基于航向辅助分布式SLAM的机器人自主导航方法,属于机器人自主导航的范畴。本方法旨在结合机器人运动模型及观测模型的特点,在分布式滤波运算的框架下,分别比较粒子滤波与扩展卡尔曼滤波的诸如计算量等对系统性能有较大影响的因素,考虑到实时系统中对计算量的要求,采用分布式EKF算法设计滤波器而非采用PF算法,为了同时保证系统的可观测性,在传统的里程计+激光传感器作为传感器组合方案之外,引入磁罗盘作为辅助传感器从而给系统加入机器人的航向信息。该设计针对分布式滤波结构的观测模型,通过引入航向信息重新规划了滤波方法从而达到实用化机器人自主导航的目的,从而增加系统了的稳定性和精度。

    权利要求书

    1.基于航向辅助分布式SLAM的机器人自主导航方法,其特征在于:在每个路标点信息
    送入各自子滤波器的方式下,改变观测模型的结构,在不添加每个分布式子滤波器中路标
    点个数的条件下,将航向信息作为状态量融合到整个的扩展卡尔曼滤波过程中,下文将论
    述分布式架构中粒子滤波与卡尔曼滤波实现的差异,在采用分布式EKF的条件下,若需保证
    精度,与粒子滤波不同的是需要引入航向信息作为观测量,从而在减少计算量同时保证可
    观性的条件下,本方法通过航向引入的方式建立分布式扩展卡尔曼滤波的模型,设计相应
    的滤波器实现方式,并通过实验验证本算法的真实有效性;
    首先,引入航向信息的系统模型阐述如下:
    关于引入航向信息的机器人状态矩阵描述如下:xv=[xv,yv,φv,xL,yL]T,其中,xv,yv表
    示机器人坐标值,φv表示机器人航向角(φv=0表示指向x轴正向,φv=π/2表示指向y轴正
    向);xL,yL表示探测得到的路标点坐标值,则系统的状态转移模型如下:
    X L ( t + 1 ) = x v ( t + 1 ) y v ( t + 1 ) φ v ( t + 1 ) x L ( t + 1 ) y L ( t + 1 ) = f ( X L ( t ) ) + ω = x v ( t ) + Δ T ( v c cos ( φ v ) - v c L tan ( φ v ) ( a sin ( φ v ) + b cos ( φ v ) ) ) y v ( t ) + Δ T ( v c sin ( φ v ) - v c L tan ( φ v ) ( a sin ( φ v ) - b cos ( φ v ) ) ) φ v ( t ) + Δ T v c L tan ( α ) x v ( t ) + r * cos ( φ v ( t ) + α ) y v ( t ) + r * sin ( φ v ( t ) + α ) + ω - - - ( 1 ) ]]>
    以上转移矩阵中的参数与车体及里程计的固定参数有关,式中xv=[xv,yv,φvv,xL,yL]T
    为机器人在t时刻的位姿及路标点位置,ΔT为时间变化量,vc为机器人移动速度,α为机器
    人车轮转角,L为两轮轴间距,γ为高斯白噪声;
    扩展卡尔曼滤波的量测更新结果是由激光传感器测得的路标点距离信息及磁罗盘测
    得的本体航向信息计算得到的,针对激光传感器及磁罗盘给出机器人的观测模型,观测量
    分别t时刻第k个路标点相对于传感器的距离,角度以及所测得的航向;获得路标点的观
    测模型如下:
    z t k = z r k z θ k z φ k = ( m L , x - x v ) 2 + ( m L , y - y v ) 2 φ v - a t a n ( - m L , y - y v m L , x - x v ) + π 2 φ v + v - - - ( 2 ) ]]>
    其中,mL,x,mL,y表示测量到的路标点相对于xv,yv的测量信息;
    在分布式框架下,带有航向信息的各子滤波器的模型如下:
    x v = f ( x v ) + γ z t 1 = h ( x v ( t ) , m 1 ) + v 1 ( t ) ]]>
    x v = f ( x v ) + γ z t 2 = h ( x v ( t ) , m 2 ) + v 2 ( t ) ]]>
    …………
    x v = f ( x v ) + γ z t n = h ( x v ( t ) , m n ) + v n ( t ) - - - ( 3 ) ]]>
    每个子滤波器中对应的SLAM状态向量分为机器人位姿估计和路标估计共五维状态,即
    每次状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决定;其
    中各子滤波器状态向量xv=[xv,yv,φv,xL,yL]T,h(xv(t),m1)表示t时刻,根据该时刻机器人
    状态向量xv(t)和对应的测量信息mn所估计的第n个路标点的测量信息;
    分布式结构的滤波方式主要分为粒子滤波(PF)和扩展卡尔曼滤波(EKF),PF属于非线
    性滤波方法,解决SLAM这种非线性模型问题具有精度上的优势,但其计算量较大,并较难移
    植到实时性较高的移动平台上;
    对于PF,根据各子滤波器中粒子群的预测分布,针对各种传感器的模型近似利用蒙特
    卡洛法模拟粒子分布,然后对每个子滤波器中的粒子进行重要性权值估计如式(4)所示;
    η t i = 1 ( 2 π ) n v / 2 det R t * e - 1 2 ( z t - h t ( x t i * ) ) T R t - 1 ( z t - h t ( x t i * ) ) - - - ( 4 ) ]]>
    根据公式(4)得到的权值可见,权值反映各个子滤波器中粒子的分布情况,并且由观
    测信息zt和根据状态向量估计的所近似的分布计算得来,每个子滤波器虽然只利用
    单一路标点进行滤波估计,但由于PF的权值由概率分布模型决定,并不是基于线性模型迭
    代计算得到,而是模拟概率统计的模型得到,故可观测性可以得到保证;
    根据权重值更新,可输出t时刻第j个子滤波器的位姿估计结果如式(5)
    x t j = Σ i = 1 N x t i * η t i - - - ( 5 ) ]]>
    根据子滤波器位姿结果,进行主滤波器的融合;如式(6)及式(7)所示;
    x t = Σ j = 1 M x t j * η t j - - - ( 6 ) ]]>
    η t j = 1 / Σ i = 1 N ( w t j ) 2 Σ j = 1 M ( 1 / Σ i = 1 N ( w t j ) 2 ) - - - ( 7 ) ]]>
    其中,最终位姿估计xt由子滤波器估计结果融合而来,融合系数由各子滤波器有
    效粒子数决定;
    根据以上各式可知,PF算法整个计算过程均与子滤波器估计的概率分布有关,可观测
    性的问题并不像EKF算法影响显著,但其精度随着滤波器所使用的粒子数的增加而增加,所
    以,若想要保证良好的精度,往往滤波器所需的粒子数也较多,即计算的次数要远远大于其
    他非统计型的滤波方法;
    考虑到计算量的问题,选用相对计算量可控的EKF算法,提高系统的实时性,而分布式
    架构又能很好的提高系统的容错性及计算性能;故采用分布式架构上使用EKF的方法可以
    充分发挥两者优势,即每个滤波器采用固定的路标点个数,防止过多动态重构的发生,并且
    每个滤波器使用EKF进行滤波;该种解决方案虽然最大程度地发挥了两者的优点,但不可忽
    视的是,EKF方法将非线性模型转化为线性模型,根据线性模型的可观测性的特质,如果滤
    波器中只有一个观测点或多个观测点但共线时,系统的可观测性会受到影响,无法满足滤
    波的可观要求,为了兼容每个滤波器中输入路标点相对较少且固定,并降低其对可观测性
    的影响,加入航向对可观测性进行改善,当每个子滤波器中存在一个路标点且存在当前时
    刻机器人本体的航向时,整个系统是可观的;综上,以下描述基于分布式架构下,结合航向
    信息辅助的分布式EKF算法;
    首先,卡尔曼滤波解决的是线性系统模型,而SLAM模型属于非线性模型,将其进行线性
    化处理如下,航向信息作为观测量引入:
    h X = h r X h θ X h φ X = z r ( x v , y v , φ v , { x L , y L } ) z θ ( x v , y v , φ v , { x L , y L } ) z φ ( x v , y v , φ v , { x L , y L } ) - - - ( 8 ) ]]>
    其中
    h r X = 1 Δ [ - Δ x , - Δ y , 0 , 0 , 0 ] - - - ( 9 ) ]]>
    h θ X = [ Δ y Δ 2 , - Δ x Δ 2 , - 1 , 0 , 0 ] - - - ( 10 ) ]]>
    h φ X = [ 0 , 0 , 1 , 0 , 0 ] - - - ( 11 ) ]]>
    Δx=[xv-xL]Δy=[yv-yL](12)
    Δ = Δx 2 + Δy 2 - - - ( 13 ) ]]>
    在模型的线性化操作后,针对每个路标点建立相应的子滤波器,并匹配相关t-1时刻的
    路标点,将各个子滤波器中代入相应的航向值;子滤波器的位姿估计过程为:
    P ^ i ( t | t - 1 ) = P ^ i ( t - 1 ) + Q i ( k ) , i = 1 , 2 ... n , m - - - ( 14 ) ]]>
    X ^ i ( t | t - 1 ) = X ^ i ( t - 1 ) , i = 1 , 2 ... n , m - - - ( 15 ) ]]>
    根据将公式(1)线性化后的姿态转移矩阵进行以上的位姿预测估计,得到的结果作为
    以下量测更新的输入;
    量测更新的过程在各子滤波器中进行,结合线性化后的观测模型进行以下的卡尔
    曼滤波量测更新过程,更新卡尔曼滤波增益Ki(t)和协方差
    K i ( t ) = P ^ i ( t | t - 1 ) H i T ( k ) [ H ( k ) P ^ i ( t | t - 1 ) H i T ( k ) + R i ( k ) ] - 1 , i = 1 , 2 ... n - - - ( 16 ) ]]>
    P ^ i ( t ) = [ I - K i ( t ) H i ( k ) ] P ^ i ( t | t - 1 ) , i = 1 , 2 ... n - - - ( 17 ) ]]>
    X ^ i ( t ) = X ^ i ( t - 1 ) + K i ( t ) [ Z i ( t ) - H i ( t ) X ^ i ( t | t - 1 ) ] , i = 1 , 2 ... n - - - ( 18 ) ]]>
    得到子滤波器更新的结果后,根据协方差矩阵进行融合过程,从而输出最终的位置:
    P ^ m ( t ) = P ^ m ( t | t - 1 ) = Σ i = 1 n η i p ^ i ( t ) , i = 1 , 2 ... n - - - ( 19 ) ]]>
    X ^ m ( t ) = X ^ m ( t | t - 1 ) = Σ i = 1 n η i X ^ i ( t ) , i = 1 , 2 ... n - - - ( 20 ) ]]>
    其中ηi为加权系数,加权平均法将来自不同子滤波器的机器人位姿估计信息计算权值,
    进行加权平均后的结果作为融合值。
    2.根据权利要求1所述的基于航向辅助分布式SLAM的机器人自主导航方法,其特征在
    于:
    步骤1:模型的建立
    系统模型的建立是基于机器人、环境特征和传感器这三个坐标系来定义的,通过状态
    转移方程和坐标系建立里程计预测模型,本发明选用笛卡尔坐标系统作为直角坐标的表示
    形式,把传感器坐标系统一到机器人坐标系中,只采用全局坐标系和机器人坐标系;状态转
    移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决定;即xv=[xL,yL,
    φL,xi,yi]T,建立运动模型和观测模型见公式(1)(2),两者分别利用里程计的输出和激光
    传感器的输出来估计相应的机器人及路标点位置信息;
    步骤2:滤波初始化及数据预处理
    设定卡尔曼滤波的P/Q/R矩阵初值,并进行全局坐标下机器人位置的初始化,对准磁罗
    盘激光传感器等传感器时钟以及读取数据路径;设置匹配路标点阈值等初始化参数;此外,
    对传感器数据进行预处理,根据每一时刻扫描的点云(511个)中设计函数提取激光传感器
    特征点,并进行航向弧度值的计算等;
    步骤3:路标点匹配
    在机器人运行过程中,将t时刻观测到路标点与已在全局地图中存储的路标点相匹配,
    进行相关的数据关联运算,若匹配成功,则送入所匹配上的t-1时刻对应路标点的子滤波器
    进行后续运算;若没有匹配成功,则说明该路标点为新增路标点,直接将其添加至全局地图
    并建立相关子滤波器;对于t时刻未能观测到t-1时刻路标点的子滤波器,进行删除;
    步骤4:滤波器运行过程
    将运动模型及观测模型进行线性化处理,将传感器输入的结果通过线性化的模型处理
    后进行卡尔曼滤波,每个路标点的信息及t时刻的机器人航向信息送入同一个子滤波器,通
    过卡尔曼滤波算法对上述步骤中建立的各个子滤波器的状态向量和协方差进行处理,分别
    进行状态转移矩阵和相应协方差矩阵的迭代更新,每个子滤波器中进行如下两个关键的计
    算步骤,:
    预测过程:
    首先将t-1时刻的每个子滤波器的估计状态和协方差矩阵,结合t时刻里程计的输出数
    据,代入运动模型(1)中,再经过一步预测方程(14)和(15)进行一步递推,得到由里程计和
    状态转移矩阵推算出的机器人位置;
    量测更新过程:
    通过第一步递推的方式,得到机器人位置的预测值后,根据预测值的结果,加入激光传
    感器和磁罗盘的观测信息进行量测更新的计算,具体见式(16),(17)和(18),输出每个子滤
    波器对位置的量测修正值及路标点的更新值,并同时输出相应的协方差矩阵供后续迭代;
    每次的滤波重复以上两个过程,能够持续观测到的路标点所建立的滤波器,在t时刻继
    承t-1时刻的协方差矩阵作为t时刻的初始输入矩阵,新观测到的路标点对应的子滤波器则
    采用t-1时刻最终估计得协方差矩阵作为初始输入矩阵;
    步骤5:滤波器融合输出最终结果
    将各个子滤波器的估计结果输入到主滤波器中,根据各子滤波器协方差确定各子滤波
    器在主滤波器中所占的权重并归一化,最后利用信息融合公式(20)输出最终位置估计结
    果。

    说明书

    基于航向辅助分布式SLAM的机器人自主导航方法

    技术领域

    本发明涉及机器人同时定位与制图(SLAM)属于机器人自主导航的范畴,针对自主
    移动机器人如何确定自身位置及感知外界环境的问题,SLAM系统旨在通过机器人系统模
    型,结合相应的滤波方法完成自主移动机器人的定位与周围环境的制图。

    背景技术

    SLAM(SimultaneousLocalizationandMapping)即同步定位与地图构建其基本
    原理是:当机器人处于未知环境中时,通过自身传感器收集自身运动状况及周围环境情况,
    无论采用何种滤波方法进行位置的估计,都需经历预测与更新的过程,传统2D-SLAM的设
    计,一般采用里程计采集的信息作为预测过程的输入,激光传感器采集的信息作为更新过
    程的输入,具体的滤波器设计可以有多种衍生,传统的以集中矩阵形式描述机器人位姿及
    路标点状态,本设计采用系统模型的分布化处理,建立由每个有效路标点单独构成子滤波
    器的模型结构,由各个子滤波器融合为统一的地图估计结果,基于此框架下,对其观测模型
    特性进行分析,通过其可观测性及收敛性的总结,引入磁罗盘的航向作为辅助信息加入至
    分布式观测环节的滤波过程中,从而达到改善系统可观测性的目的,系统的输出结果由里
    程计,激光传感器及磁罗盘的信息进行滤波生成。

    由于集中式结构中,描述系统状态的矩阵维数动态时变,并且随着观测路标点的
    增加,矩阵维度也随之增加,只用单一滤波器完成计算过程会带来诸如计算量大,稳定性差
    和容错性差等问题,本设计使用的分布式结构的SLAM算法相对于传统集中式算法,所建立
    的每个滤波器都包含位姿信息及所对应固定路标点的信息,有效减小了计算量并提高了稳
    定性与容错性。对于分布式算法,有诸多提高精度的方法,主要通过改善滤波的方法及融合
    的算法来提高系统属性,而单纯地将集中式结构改成分布式结构而不加入其他传感器信
    息,同样面临可观测性降低的问题,由于单一路标点观测性无法得到保证,针对此类问题,
    主要有提高子滤波器中观测点个数等方法解决。针对现有技术的缺陷详述如下。

    分布式结构的精度缺陷及解决方案:

    分布式结构虽然状态矩阵维数相对固定能够解决计算量和不稳定的问题,但是由
    于子滤波器中的路标点较少,可观测性得不到有效的保证,即当只有一个观测点的信息时,
    无法保证系统在长时间运行过程中都是可观的,粒子滤波的方法对可观测性影响较小,但
    计算量的问题导致它不便于应用到实时系统中。当针对两个及以上不共线的观测点建立子
    滤波器时,可有效保证系统可观性,但此方案会带来子滤波器重构等不稳定因素,影响系统
    稳定性,故若试图同时考虑计算量,可观性,稳定性,考虑加入航向信息进行扶助,磁罗盘所
    提供的机器人本体航向信息能够解决可观测性的问题并且不会带来很频繁的子滤波器重
    构,规避了以上两者的缺陷,同时选用EKF作为滤波方法。综上所述,制定相关的基于航向辅
    助的分布式EKF算法作为有效解决方案。

    发明内容

    本方法旨在结合机器人运动模型及观测模型的特点,在分布式滤波运算的框架
    下,分别比较粒子滤波(PF)与扩展卡尔曼滤波(EKF)的诸如计算量等对系统性能有较大影
    响的因素,考虑到实时系统中对计算量的要求,采用分布式EKF算法设计滤波器而非采用PF
    算法,为了同时保证系统的可观测性,在传统的里程计+激光传感器作为传感器组合方案之
    外,引入磁罗盘作为辅助传感器从而给系统加入机器人的航向信息。该设计针对分布式滤
    波结构的观测模型,通过引入航向信息重新规划了滤波方法从而达到实用化机器人自主导
    航的目的。

    本发明从分布式粒子滤波带来的计算量问题及分布式卡尔曼滤波所面临的可观
    测性问题入手,引入磁罗盘提供航向辅助信息,提出一种结合分布式算法结构与航向辅助
    的SLAM方法,并搭建移动平台进行算法精度和收敛性的验证。在每个路标点信息送入各自
    子滤波器的方式下,改变观测模型的结构,在不添加每个分布式子滤波器中路标点个数的
    条件下,将航向信息作为状态量融合到整个的扩展卡尔曼滤波过程中,下文将论述分布式
    架构中粒子滤波与卡尔曼滤波实现的差异,在采用分布式EKF的条件下,若需保证一定的精
    度,与粒子滤波不同的是需要引入航向信息作为观测量,从而在减少计算量同时保证可观
    性的条件下,本文通过航向引入的方式建立分布式扩展卡尔曼滤波的模型,设计相应的滤
    波器实现方式,并通过实验验证本算法的真实有效性。

    首先,引入航向信息的系统模型阐述如下:

    关于引入航向信息的机器人状态矩阵描述如下:xv=[xv,yv,φv,xL,yL]T,其中,xv,
    yv表示机器人坐标值,φv表示机器人航向角(φv=0表示指向x轴正向,φv=π/2表示指向y
    轴正向)。xL,yL表示探测得到的路标点坐标值,则系统的状态转移模型如下:


    X L ( t + 1 ) = x v ( t + 1 ) y v ( t + 1 ) φ v ( t + 1 ) x L ( t + 1 ) y L ( t + 1 ) = f ( X L ( t ) ) + ω = x v ( t ) + Δ T ( v c cos ( φ v ) - v c L tan ( φ v ) ( a sin ( φ v ) + b cos ( φ v ) ) ) y v ( t ) + Δ T ( v c sin ( φ v ) - v c L tan ( φ v ) ( a sin ( φ v ) - b cos ( φ v ) ) ) φ v ( t ) + Δ T v c L tan ( α ) x v ( t ) + r * cos ( φ v ( t ) + α ) y v ( t ) + r * sin ( φ v ( t ) + α ) + ω - - - ( 1 ) ]]>

    以上转移矩阵中的参数与车体及里程计的固定参数有关,式中xv=[xv,yv,φv,xL,
    yL]T为机器人在t时刻的位姿及路标点位置,ΔT为时间变化量,vc为机器人移动速度,α为机
    器人车轮转角,L为两轮轴间距,γ为高斯白噪声。

    扩展卡尔曼滤波的量测更新结果是由激光传感器测得的路标点距离信息及磁罗
    盘测得的本体航向信息计算得到的,针对激光传感器及磁罗盘给出机器人的观测模型,观
    测量分别t时刻第k个路标点相对于传感器的距离,角度以及所测得的航向?;竦寐繁甑?br />的观测模型如下:

    z t k = z r k z θ k z φ k = ( m L , x - x v ) 2 + ( m L , y - y v ) 2 φ v - a t a n ( - m L , y - y v m L , x - x v ) + π 2 φ v + v - - - ( 2 ) ]]>

    其中,mL,x,mL,y表示测量到的路标点相对于xv,yv的测量信息。

    在分布式框架下,带有航向信息的各子滤波器的模型如下:

    x v = f ( x v ) + γ z t 1 = h ( x v ( t ) , m 1 ) + v 1 ( t ) x v = f ( x v ) + γ z t 2 = h ( x v ( t ) , m 2 ) + v 2 ( t ) ............ x v = f ( x v ) + γ z t n = h ( x v ( t ) , m n ) + v n ( t ) - - - ( 3 ) ]]>

    每个子滤波器中对应的SLAM状态向量分为机器人位姿估计和路标估计共五维状
    态,即每次状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息决
    定。其中各子滤波器状态向量xv=[xv,yv,φv,xL,yL]T,h(xv(t),m1)表示t时刻,根据该时刻
    机器人状态向量xv(t)和对应的测量信息mn所估计的第n个路标点的测量信息。

    分布式结构的滤波方式主要分为粒子滤波(PF)和扩展卡尔曼滤波(EKF),PF属于
    非线性滤波方法,解决SLAM这种非线性模型问题具有精度上的优势,但其计算量较大,并较
    难移植到实时性较高的移动平台上。

    对于PF,根据各子滤波器中粒子群的预测分布,针对各种传感器的模型近似利用
    蒙特卡洛法模拟粒子分布,然后对每个子滤波器中的粒子进行重要性权值估计如式(4)所
    示。

    η t i = 1 ( 2 π ) n v / 2 detR t * e - 1 2 ( z t - h t ( x t i * ) ) T R t - 1 ( z t - h t ( x t i * ) ) - - - ( 4 ) ]]>

    根据公式(4)得到的权值可见,权值反映各个子滤波器中粒子的分布情况,并
    且由观测信息zt和根据状态向量估计的所近似的分布计算得来,每个子滤波器虽然
    只利用单一路标点进行滤波估计,但由于PF的权值由概率分布模型决定,并不是基于线性
    模型迭代计算得到,而是模拟概率统计的模型得到,故可观测性可以得到保证。

    根据权重值更新,可输出t时刻第j个子滤波器的位姿估计结果如式(5)

    x t j = Σ i = 1 N x t i * η t i - - - ( 5 ) ]]>

    根据子滤波器位姿结果,进行主滤波器的融合。如式(6)及式(7)所示。

    x t = Σ j = 1 M x t j * η t j - - - ( 6 ) ]]>

    η t j = 1 / Σ i = 1 N ( w t j ) 2 Σ j = 1 M ( 1 / Σ i = 1 N ( w t j ) 2 ) - - - ( 7 ) ]]>

    其中,最终位姿估计xt由子滤波器估计结果融合而来,融合系数由各子滤波
    器有效粒子数决定。

    根据以上各式可知,PF算法整个计算过程均与子滤波器估计的概率分布有关,可
    观测性的问题并不像EKF算法影响显著,但其精度随着滤波器所使用的粒子数的增加而增
    加,所以,若想要保证良好的精度,往往滤波器所需的粒子数也较多,即计算的次数要远远
    大于其他非统计型的滤波方法。

    考虑到计算量的问题,选用相对计算量可控的EKF算法,提高系统的实时性,而分
    布式架构又能很好的提高系统的容错性及计算性能。故采用分布式架构上使用EKF的方法
    可以充分发挥两者优势,即每个滤波器采用固定的路标点个数,防止过多动态重构的发生,
    并且每个滤波器使用EKF进行滤波。该种解决方案虽然最大程度地发挥了两者的优点,但不
    可忽视的是,EKF方法将非线性模型转化为线性模型,根据线性模型的可观测性的特质,如
    果滤波器中只有一个观测点或多个观测点但共线时,系统的可观测性会受到影响,无法满
    足滤波的可观要求,为了兼容每个滤波器中输入路标点相对较少且固定,并降低其对可观
    测性的影响,加入航向对可观测性进行改善,当每个子滤波器中存在一个路标点且存在当
    前时刻机器人本体的航向时,整个系统是可观的。综上,以下描述基于分布式架构下,结合
    航向信息辅助的分布式EKF算法。

    首先,卡尔曼滤波解决的是线性系统模型,而SLAM模型属于非线性模型,将其进行
    线性化处理如下,航向信息作为观测量引入:

    h X = h r X h θ X h φ X = z r ( x v , y v , φ v , { x L , y L } ) z θ ( x v , y v , φ v , { x L , y L } ) z φ ( x v , y v , φ v , { x L , y L } ) - - - ( 8 ) ]]>

    其中

    h r X = 1 Δ [ - Δ x , - Δ y , 0 , 0 , 0 ] - - - ( 9 ) ]]>

    h θ X = [ Δ y Δ 2 , - Δ x Δ 2 , - 1 , 0 , 0 ] - - - ( 10 ) ]]>

    h φ X = [ 0 , 0 , 1 , 0 , 0 ] - - - ( 11 ) ]]>

    Δx=[xv-xL]Δy=[yv-yL](12)

    Δ = Δx 2 + Δy 2 - - - ( 13 ) ]]>

    在模型的线性化操作后,针对每个路标点建立相应的子滤波器,并匹配相关t-1时
    刻的路标点,将各个子滤波器中代入相应的航向值。子滤波器的位姿估计过程为:

    P ^ i ( t | t - 1 ) = P ^ i ( t - 1 ) + Q i ( k ) , i = 1 , 2 ... n , m - - - ( 14 ) ]]>

    X ^ i ( t | t - 1 ) = X ^ i ( t - 1 ) , i = 1 , 2 ... n , m - - - ( 15 ) ]]>

    根据将公式(1)线性化后的姿态转移矩阵进行以上的位姿预测估计,得到的结果
    作为以下量测更新的输入。

    量测更新的过程在各子滤波器中进行,结合线性化后的观测模型进行以下的
    卡尔曼滤波量测更新过程,更新卡尔曼滤波增益Ki(t)和协方差

    K i ( t ) = P ^ i ( t | t - 1 ) H i T ( k ) [ H i ( k ) P ^ i ( t | t - 1 ) H i T ( k ) + R i ( k ) ] - 1 , i = 1 , 2 ... n - - - ( 16 ) ]]>

    P ^ i ( t ) = [ I - K i ( t ) H i ( k ) ] P ^ i ( t | t - 1 ) , i = 1 , 2 ... n - - - ( 17 ) ]]>

    X ^ i ( t ) = X ^ i ( t - 1 ) + K i ( t ) [ Z i ( t ) - H ( t ) X ^ i ( t | t - 1 ) ] , i = 1 , 2 ... n - - - ( 18 ) ]]>

    得到子滤波器更新的结果后,根据协方差矩阵进行融合过程,从而输出最终的位
    置:

    P ^ m ( t ) = P ^ m ( t | t - 1 ) = Σ i = 1 n η i p ^ i ( t ) , i = 1 , 2 ... n - - - ( 19 ) ]]>

    X ^ m ( t ) = X ^ m ( t | t - 1 ) = Σ i = 1 n η i X ^ i ( t ) , i = 1 , 2 ... n - - - ( 20 ) ]]>

    其中ηi为加权系数,加权平均法将来自不同子滤波器的机器人位姿估计信息计算
    权值,进行加权平均后的结果作为融合值。

    附图说明

    图1:机器人的状态转移过程图;

    图2:机器人实际坐标系;

    图3:基于航向辅助分布式SLAM算法流程图;

    图4:基于航向辅助分布式程序设计流程图

    图5:移动小车平台实物图;

    图6:移动小车平台实验环境;

    图7:移动小车平台形实验结果图。

    具体实施方式

    步骤1:模型的建立

    系统模型的建立是基于机器人、环境特征和传感器这三个坐标系来定义的,通过
    图1和图2中的状态转移方程和坐标系建立里程计预测模型,本发明选用笛卡尔坐标系统作
    为直角坐标的表示形式,把传感器坐标系统一到机器人坐标系中,只采用全局坐标系和机
    器人坐标系。状态转移矩阵包含的信息由机器人本体的位置信息与一个路标点的位置信息
    决定。即xv=[xL,yL,φL,xi,yi]T,建立运动模型和观测模型见公式(1)(2),两者分别利用里
    程计的输出和激光传感器的输出来估计相应的机器人及路标点位置信息。

    步骤2:滤波初始化及数据预处理

    设定卡尔曼滤波的P/Q/R矩阵初值,并进行全局坐标下机器人位置的初始化,对准
    磁罗盘激光传感器等传感器时钟以及读取数据路径。设置匹配路标点阈值等初始化参数。
    此外,对传感器数据进行预处理,根据每一时刻扫描的点云(511个)中设计函数提取激光传
    感器特征点,并进行航向弧度值的计算等。

    步骤3:路标点匹配

    在机器人运行过程中,将t时刻观测到路标点与已在全局地图中存储的路标点相
    匹配,进行相关的数据关联运算,若匹配成功,则送入所匹配上的t-1时刻对应路标点的子
    滤波器进行后续运算;若没有匹配成功,则说明该路标点为新增路标点,直接将其添加至全
    局地图并建立相关子滤波器。对于t时刻未能观测到t-1时刻路标点的子滤波器,进行删除。

    步骤4:滤波器运行过程

    将运动模型及观测模型进行线性化处理,将传感器输入的结果通过线性化的模型
    处理后进行卡尔曼滤波,每个路标点的信息及t时刻的机器人航向信息送入同一个子滤波
    器,通过卡尔曼滤波算法对上述步骤中建立的各个子滤波器的状态向量和协方差进行处
    理,分别进行状态转移矩阵和相应协方差矩阵的迭代更新,每个子滤波器中进行如下两个
    关键的计算步骤,:

    预测过程:

    首先将t-1时刻的每个子滤波器的估计状态和协方差矩阵,结合t时刻里程计的输
    出数据,代入运动模型(1)中,再经过一步预测方程(14)和(15)进行一步递推,得到由里程
    计和状态转移矩阵推算出的机器人位置。

    量测更新过程:

    通过第一步递推的方式,得到机器人位置的预测值后,根据预测值的结果,加入激
    光传感器和磁罗盘的观测信息进行量测更新的计算,具体见式(16),(17)和(18),输出每个
    子滤波器对位置的量测修正值及路标点的更新值,并同时输出相应的协方差矩阵供后续迭
    代。

    每次的滤波重复以上两个过程,能够持续观测到的路标点所建立的滤波器,在t时
    刻继承t-1时刻的协方差矩阵作为t时刻的初始输入矩阵,新观测到的路标点对应的子滤波
    器则采用t-1时刻最终估计得协方差矩阵作为初始输入矩阵。

    步骤5:滤波器融合输出最终结果

    将各个子滤波器的估计结果输入到主滤波器中,根据各子滤波器协方差确定各子
    滤波器在主滤波器中所占的权重并归一化,最后利用信息融合公式(20)输出最终位置估计
    结果。

    通过实际场景实验对航向辅助的分布式SLAM进行验证。实验平台是基于ROS操作
    系统操控制的两轮小车,传感器使用URG-04LX的激光传感器,磁罗盘使用迈科传感公司生
    产的HCM365B全姿态三维电子罗盘,移动小车平台的实物图如图4。实验的测试环境中,以圆
    柱形筒作为路标点分别放置于实验跑道两侧,实验环境如图6所示,通过使用本发明算法和
    未加入航向的EKF算法对比,证明本算法在精度及保持可观测性的优势。

    实验数据在Matlab中进行绘图,结果如图7所示,在里程计数据无法保证系统收敛
    性的要求的情况下,图中给出了传统集中式EKF算法(CEKF)和本发明所应用的基于航向辅
    助的分布式EKF算法(DH-EKF)的运算结果,对比图中DH-EKF和传统CEKF算法,可以看出加入
    航向辅助的算法更平滑稳定且估计结果一致性和收敛性更好,误差更小。证明加入航向修
    正后,对于系统可能存在的动态干扰和观测的不确定性均有很好的稳定性表现。并且,由于
    航向的引入,以往分布式结构中单纯路标点扫描带来的可观测性问题也能得到很好解决,
    从而增加系统了的稳定性和精度。

    关 键 词:
    基于 航向 辅助 分布式 SLAM 机器人 自主 导航 方法
      专利查询网所有资源均是用户自行上传分享,仅供网友学习交流,未经上传用户书面授权,请勿作他用。
    关于本文
    本文标题:基于航向辅助分布式SLAM的机器人自主导航方法.pdf
    链接地址://www.4mum.com.cn/p-5886363.html
    关于我们 - 网站声明 - 网站地图 - 资源地图 - 友情链接 - 网站客服客服 - 联系我们

    [email protected] 2017-2018 www.4mum.com.cn网站版权所有
    经营许可证编号:粤ICP备17046363号-1 
     


    收起
    展开
  • 四川郎酒股份有限公司获第十二届人民企业社会责任奖年度环保奖 2019-05-13
  • 银保监会新规剑指大企业多头融资和过度融资 2019-05-12
  • 韩国再提4国联合申办世界杯 中国网友无视:我们自己来 2019-05-11
  • 中国人为什么一定要买房? 2019-05-11
  • 十九大精神进校园:风正扬帆当有为 勇做时代弄潮儿 2019-05-10
  • 粽叶飘香幸福邻里——廊坊市举办“我们的节日·端午”主题活动 2019-05-09
  • 太原设禁鸣路段 设备在测试中 2019-05-09
  • 拜耳医药保健有限公司获第十二届人民企业社会责任奖年度企业奖 2019-05-08
  • “港独”没出路!“梁天琦们”该醒醒了 2019-05-07
  • 陈卫平:中国文化内涵包含三方面 文化复兴表现在其中 2019-05-06
  • 人民日报客户端辟谣:“合成军装照”产品请放心使用 2019-05-05
  • 【十九大·理论新视野】为什么要“建设现代化经济体系”?   2019-05-04
  • 聚焦2017年乌鲁木齐市老城区改造提升工程 2019-05-04
  • 【专家谈】上合组织——构建区域命运共同体的有力实践者 2019-05-03
  • 【华商侃车NO.192】 亲!楼市火爆,别忘了买车位啊! 2019-05-03