• 四川郎酒股份有限公司获第十二届人民企业社会责任奖年度环保奖 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
    • / 18
    • 下载费用:30 金币  

    重庆时时彩首页网址: 一种基于秩滤波的农业机器人组合导航信息融合方法.pdf

    关 键 词:
    一种 基于 滤波 农业 机器人 组合 导航 信息 融合 方法
      专利查询网所有资源均是用户自行上传分享,仅供网友学习交流,未经上传用户书面授权,请勿作他用。
    摘要
    申请专利号:

    CN201611059641.X

    申请日:

    2016.11.25

    公开号:

    CN106772524A

    公开日:

    2017.05.31

    当前法律状态:

    实审

    有效性:

    审中

    法律详情: 实质审查的生效IPC(主分类):G01S 19/49申请日:20161125|||公开
    IPC分类号: G01S19/49(2010.01)I 主分类号: G01S19/49
    申请人: 安徽科技学院
    发明人: 王磊; 李双喜; 张玉山; 王岳
    地址: 233199 安徽省滁州市凤阳县东华路9号
    优先权:
    专利代理机构: 南京苏高专利商标事务所(普通合伙) 32204 代理人: 肖明芳
    PDF完整版下载: PDF下载
    法律状态
    申请(专利)号:

    CN201611059641.X

    授权公告号:

    |||

    法律状态公告日:

    2017.06.23|||2017.05.31

    法律状态类型:

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

    摘要

    本发明公开了一种基于秩滤波的农业机器人组合导航信息融合方法,采用交互式多模型滤波为算法结构,通过上一个时刻各个模型滤波器的输出结果计算当前时刻滤波的初值,将秩滤波器作为模型滤波步骤中的子滤波器进行并行滤波,利用模型滤波中得到的残差、残差协方差对模型概率进行更新,对各个秩滤波器估值进行加权融合从而得到交互输出结果。由导航计算机根据组合导航系统的滤波模型及算法流程,完成组合导航的数据处理及解算工作。本发明能够提高复杂环境下组合导航系统的滤波精度,使得农业机器人具有适应范围广和抗干扰能力强的优点。

    权利要求书

    1.一种基于秩滤波的农业机器人组合导航信息融合方法,其特征在于,包括如下步骤:
    步骤1,根据农业机器人所处环境,将土地平整度和卫星信号强度分级,设定土地平整
    度因子和卫星信号强度因子ξ,1≤ξ≤ξmax,ξ∈Z,Z为整数集,ξmax分
    别表示最大的土地不平整级别和最大的卫星信号衰落级别;
    步骤2,由惯性导航系统INS和全球卫星导航系统GNSS所测量的数据,测量各级别土地
    平整度和卫星信号衰落情形所对应的量测噪声ν,将量测噪声ν及其协方差矩阵R作为可变
    参数建立组合导航系统模型集合M,设模型个数为r,各模型之间跳变的Markov
    转移概率为πji,下标i,j分别表示第i个模型和第j个模型;
    步骤3,由k-1时刻的模型概率与先验的Markov转移概率πji进行交互,计算混合概率
    k≥1;
    步骤4,对于第j个模型,j=1,2,…,r,利用k-1时刻的状态估计和协方差阵
    计算k时刻的状态初值与协方差阵初值
    步骤5,在组合导航系统获得新的量测zk之后,利用状态初值与协方差阵初值
    采用秩滤波算法进行滤波,得到k时刻的状态估计和协方差阵
    步骤6,利用混合概率残差估值和残差协方差计算当前时刻的匹配模型对
    应的残差估值残差协方差似然函数和模型概率
    步骤7,利用各个模型滤波器k时刻的状态估计协方差阵和模型概率计算
    导航参数的全局估计及其估计误差的协方差阵Pk|k;
    步骤8,返回步3,重复步骤3至步骤7,直至接收到停止导航控制指令。
    2.根据权利要求1所述方法,其特征在于,步骤3中,通过如下公式计算混合概率
    <mrow> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> </msubsup> <mo>=</mo> <mfrac> <mrow> <msub> <mi>&pi;</mi> <mrow> <mi>j</mi> <mi>i</mi> </mrow> </msub> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> </mrow> <mrow> <munderover> <mo>&Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msub> <mi>&pi;</mi> <mrow> <mi>j</mi> <mi>i</mi> </mrow> </msub> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> </mrow> </mfrac> <mo>.</mo> </mrow>
    3.根据权利要求2所述方法,其特征在于,步骤4中,通过如下公式计算k时刻的状态初
    值与协方差阵初值
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> </msubsup> <mo>{</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>+</mo> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>}</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow>
    4.根据权利要求3所述方法,其特征在于,步骤5包括如下步骤:
    步骤5-1,利用k-1时刻的状态估计一维标准分布概率pβ对应的分位点和第β
    层采样点的修正系数rβ,通过秩采样机制求取采样点集
    <mrow> <msubsup> <mi>X</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>+</mo> <msub> <mi>r</mi> <mi>&beta;</mi> </msub> <msub> <mi>&lambda;</mi> <msub> <mi>p</mi> <mi>&beta;</mi> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> </msqrt> <mo>)</mo> </mrow> <mi>l</mi> </msub> <mo>,</mo> <mi>l</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mi>n</mi> <mo>,</mo> </mrow>
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>&alpha;</mi> <mo>=</mo> <mrow> <mo>(</mo> <mi>&beta;</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mi>n</mi> <mo>+</mo> <mi>l</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>&beta;</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mi>&rho;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&alpha;</mi> <mo>=</mo> <mrow> <mo>(</mo> <mi>&beta;</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> <mi>n</mi> <mo>+</mo> <mi>l</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>&beta;</mi> <mo>=</mo> <mi>&rho;</mi> <mo>+</mo> <mn>2</mn> <mo>,</mo> <mi>&rho;</mi> <mo>+</mo> <mn>3</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mn>2</mn> <mi>&rho;</mi> <mo>+</mo> <mn>1</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
    其中,n为状态向量x的维数,ρ为采样点层数,α,β分别表示第α个采样点和第β层采样
    点,l取值为[1,n]中的整数,表示矩阵平方根的第l列;
    步骤5-2,将采样点集进行非线性传播,得到非线性变换后点集
    <mrow> <msubsup> <mi>X</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>,</mo> <mi>&omega;</mi> <mo>)</mo> </mrow> <mo>,</mo> </mrow>
    其中,f(·)为状态方程非线性函数,ω为过程噪声向量;
    步骤5-3,进一步预测k时刻的状态估计和协方差阵
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>&rho;</mi> <mi>n</mi> </mrow> </mfrac> <munderover> <mo>&Sigma;</mo> <mrow> <mi>&alpha;</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>&rho;</mi> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>X</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&tau;</mi> </mfrac> <munderover> <mo>&Sigma;</mo> <mrow> <mi>&alpha;</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>&rho;</mi> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <msubsup> <mi>r</mi> <mi>&alpha;</mi> <mo>*</mo> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mo>&times;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>}</mo> <mo>+</mo> <mi>Q</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
    其中,为第α个采样点对应的协方差修正系数,取值为1,Q为过程噪声方差矩阵,τ为协
    方差权重系数,通过如下公式计算:
    <mrow> <mi>&tau;</mi> <mo>=</mo> <munderover> <mo>&Sigma;</mo> <mrow> <mi>&beta;</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mi>&beta;</mi> <mo>&NotEqual;</mo> <mi>&rho;</mi> <mo>+</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>&rho;</mi> <mo>+</mo> <mn>1</mn> </mrow> </munderover> <msubsup> <mi>r</mi> <mi>&beta;</mi> <mn>2</mn> </msubsup> <msubsup> <mi>&lambda;</mi> <msub> <mi>p</mi> <mi>&beta;</mi> </msub> <mn>2</mn> </msubsup> <mo>;</mo> </mrow>
    步骤5-4,通过秩采样机制求取采样点集
    <mrow> <msubsup> <mi>X</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>+</mo> <msub> <mi>r</mi> <mi>&beta;</mi> </msub> <msub> <mi>&lambda;</mi> <msub> <mi>p</mi> <mi>&beta;</mi> </msub> </msub> <msub> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>l</mi> </msub> <mo>,</mo> </mrow>
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>&alpha;</mi> <mo>=</mo> <mrow> <mo>(</mo> <mi>&beta;</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mi>n</mi> <mo>+</mo> <mi>l</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>&beta;</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mi>&rho;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&alpha;</mi> <mo>=</mo> <mrow> <mo>(</mo> <mi>&beta;</mi> <mo>-</mo> <mn>2</mn> <mo>)</mo> </mrow> <mi>n</mi> <mo>+</mo> <mi>l</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>&beta;</mi> <mo>=</mo> <mi>&rho;</mi> <mo>+</mo> <mn>2</mn> <mo>,</mo> <mi>&rho;</mi> <mo>+</mo> <mn>3</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mn>2</mn> <mi>&rho;</mi> <mo>+</mo> <mn>1</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
    步骤5-5,将采样点集引入观测方程并进行非线性传播,得到预测的量测点集

    <mrow> <msubsup> <mi>Z</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>,</mo> <msup> <mi>v</mi> <mi>j</mi> </msup> <mo>)</mo> </mrow> <mo>,</mo> </mrow>
    其中,h(·)为观测方程非线性函数,νj为模型j对应的量测噪声向量;
    步骤5-6,通过预测的量测点集计算量测估计
    <mrow> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mi>&rho;</mi> <mi>n</mi> </mrow> </mfrac> <munderover> <mo>&Sigma;</mo> <mrow> <mi>&beta;</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>&rho;</mi> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>Z</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>;</mo> </mrow>
    步骤5-7,通过如下公式计算新息协方差矩阵和互协方差阵
    <mrow> <msubsup> <mi>P</mi> <mrow> <mi>z</mi> <mi>z</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&tau;</mi> </mfrac> <munderover> <mo>&Sigma;</mo> <mrow> <mi>&alpha;</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>&rho;</mi> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <msubsup> <mi>r</mi> <mi>&alpha;</mi> <mo>*</mo> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>Z</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mo>&times;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>Z</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>}</mo> <mo>+</mo> <msup> <mi>R</mi> <mi>j</mi> </msup> <mo>,</mo> </mrow>
    <mrow> <msubsup> <mi>P</mi> <mrow> <mi>x</mi> <mi>y</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&tau;</mi> </mfrac> <munderover> <mo>&Sigma;</mo> <mrow> <mi>&alpha;</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>&rho;</mi> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <msubsup> <mi>r</mi> <mi>&alpha;</mi> <mo>*</mo> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mo>&times;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>Z</mi> <mrow> <mi>&alpha;</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>j</mi> <mo>*</mo> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>}</mo> <mo>,</mo> </mrow>
    其中Rj为模型j对应的量测噪声方差矩阵;
    步骤5-8,由新息协方差矩阵和互协方差阵计算增益

    k时刻组合导航系统的量测值zk到来之后,通过如下公式计算k时刻的状态估计和协
    方差阵
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>+</mo> <msubsup> <mi>W</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>-</mo> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>-</mo> <msubsup> <mi>W</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msubsup> <mi>P</mi> <mrow> <mi>z</mi> <mi>z</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>W</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow>
    5.根据权利要求4所述方法,其特征在于,步骤6中,通过如下方法计算当前时刻匹配模
    型对应的残差估值残差协方差似然函数和模型概率
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>&epsiv;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <msup> <mi>z</mi> <mi>k</mi> </msup> <mo>-</mo> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>S</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <mi>E</mi> <mo>&lsqb;</mo> <mrow> <mo>(</mo> <msubsup> <mi>&epsiv;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>&epsiv;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&rsqb;</mo> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>z</mi> <mi>z</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>&Lambda;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <msup> <mrow> <mo>&lsqb;</mo> <msup> <mrow> <mo>(</mo> <mn>2</mn> <mi>&pi;</mi> <mo>)</mo> </mrow> <mi>r</mi> </msup> <mo>|</mo> <msubsup> <mi>S</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>|</mo> <mo>&rsqb;</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> <mi>exp</mi> <mo>{</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msup> <mrow> <mo>(</mo> <msubsup> <mi>&epsiv;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>S</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>&epsiv;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>}</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mrow> <msubsup> <mi>&Lambda;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <munderover> <mo>&Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msub> <mi>&pi;</mi> <mrow> <mi>j</mi> <mi>i</mi> </mrow> </msub> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> </msubsup> </mrow> <mrow> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msubsup> <mi>&Lambda;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <munderover> <mo>&Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msub> <mi>&pi;</mi> <mrow> <mi>j</mi> <mi>i</mi> </mrow> </msub> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> </msubsup> </mrow> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
    其中E[·]表示数学期望。
    6.根据权利要求5所述方法,其特征在于,步骤7中,通过如下公式计算导航参数的全局
    估计及其估计误差的协方差阵Pk|k:
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mo>&Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <msubsup> <mi>&mu;</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>&lsqb;</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&rsqb;</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow>
    7.根据权利要求6所述的方法,其特征在于,步骤5-1和步骤5-4中,采用中位秩计算第β
    层采样点对应的一维标准分布概率pβ,计算表达式为pβ=(β-0.3)/(2ρ+1.4)。

    说明书

    一种基于秩滤波的农业机器人组合导航信息融合方法

    技术领域

    本发明属于多传感器信息融合技术领域,尤其涉及一种基于秩滤波的农业机器人
    组合导航信息融合方法。

    背景技术

    农业机器人工作在复杂的开放式非结构化农田环境中(障碍物、土地松软程度不
    一高低不平、遮挡、光照变化等),它需要通过自身携带的传感器感知周围的环境,对感知到
    的信息进行处理,获取自身的位置和姿态信息,然后实时进行路径规划和导航控制,完成各
    项作业任务。因此,导航技术是实现农业机器人自动化作业的关键。Kalman滤波器一般适用
    于系统为线性、噪声统计特性服从高斯分布并完全已知的情况;传统的扩展卡尔曼滤波
    (extended Kalman filter,EKF)及其改进方法只能处理弱非线性系统及高斯噪声条件下
    的状态估计问题。为此,人们提出了一系列以贝叶斯滤波理论为框架、基于数值积分近似的
    非线性状态估计方法,其中具有代表性的方法包括中心差分卡尔曼滤波、无??ǘ瞬?、
    求积分卡尔曼滤波、容积卡尔曼滤波等。它们的共同特点是均假设系统中噪声服从高斯分
    布,即为高斯滤波器。秩滤波器(rank Kalman filter,RKF)具有与高斯滤波器相同的滤波
    结构,它通过秩统计量原理确定采样点和权值,因此还适用于具有非高斯噪声的系统。交互
    式多模型(Interacting Multiple Model,IMM)算法以广义伪贝叶斯算法为基础,利用
    Markov切换系数实现各个模型之间的交互,兼具计算复杂度低和估计精度高的特点。

    发明内容

    本发明提出了一种基于秩滤波的农业机器人组合导航信息融合方法,该方法的主
    要思路是在交互式多模型滤波框架下,采用秩滤波器替代传统算法中的卡尔曼滤波器,突
    破传统算法中系统噪声必须为高斯分布的条件限制,增强各个子滤波器状态估计的稳定
    性,提高复杂环境下组合导航系统的滤波精度,增强农业机器人自主导航定位的性能。

    本发明采用交互式多模型滤波为算法结构,将秩滤波器作为模型滤波步骤中的子
    滤波器进行并行滤波,本发明具体包括如下步骤:

    步骤1,根据农业机器人所处环境,将土地平整度和卫星信号强度分级,设定土地
    平整度因子和卫星信号强度因子ξ,1≤ξ≤ξmax,ξ∈Z,Z为整数集,
    ξmax分别表示最大的土地不平整级别和最大的卫星信号衰落级别;

    步骤2,由惯性导航系统INS和全球卫星导航系统GNSS所测量的数据,测量各级别
    土地平整度和卫星信号衰落情形所对应的量测噪声ν,将量测噪声ν及其协方差矩阵R作为
    可变参数建立组合导航系统模型集合M,设模型个数为r,各模型之间跳变的
    Markov转移概率为πji,下标i,j分别表示第i个模型和第j个模型;

    步骤3,由k-1时刻的模型概率与先验的Markov转移概率πji进行交互,计算混
    合概率k≥1;

    步骤4,对于第j个模型,j=1,2,…,r,利用k-1时刻的状态估计和协方差阵
    计算k时刻的状态初值与协方差阵初值

    步骤5,在组合导航系统获得新的量测zk之后,利用状态初值与协方差阵初
    值采用秩滤波算法进行滤波,得到k时刻的状态估计和协方差阵

    步骤6,利用混合概率残差估值和残差协方差计算当前时刻的匹配模
    型对应的残差估值残差协方差似然函数和模型概率

    步骤7,利用各个模型滤波器k时刻的状态估计协方差阵和模型概率
    计算导航参数的全局估计及估计误差的协方差阵Pk|k;

    步骤8,返回步骤3,重复步骤3至步骤7,直至接收到停止导航控制指令;

    本发明步骤3中,通过如下公式计算混合概率


    本发明步骤4中,通过如下公式计算k时刻的状态初值与协方差阵初值


    本发明步骤5包括如下步骤:

    步骤5-1,利用k-1时刻的状态估计一维标准分布概率pβ对应的分位点
    和第β层采样点的修正系数rβ,通过秩采样机制求取采样点集



    其中,n为状态向量x的维数,ρ为采样点层数,α,β分别表示第α个采样点和第β层采
    样点,l取值为[1,n]中的整数,表示矩阵平方根的第l列;

    步骤5-2,将采样点集进行非线性传播,得到非线性变换后点集


    其中,f(·)为状态方程非线性函数,ω为过程噪声向量;

    步骤5-3,进一步预测k时刻的状态估计和协方差阵


    其中,为第α个采样点对应的协方差修正系数,取值为1,Q为过程噪声方差矩阵,
    τ为协方差权重系数,通过如下公式计算:


    步骤5-4,通过秩采样机制求取采样点集



    步骤5-5,将采样点集引入观测方程并进行非线性传播,得到预测的量测
    点集


    其中,h(·)为观测方程非线性函数,νj为模型j对应的量测噪声向量;

    步骤5-6,通过预测的量测点集计算量测估计


    步骤5-7,通过如下公式计算新息协方差矩阵和互协方差阵



    其中Rj为模型j对应的量测噪声方差矩阵;

    步骤5-8,由新息协方差矩阵和互协方差阵计算增益


    k时刻组合导航系统的量测值zk到来之后,通过如下公式计算k时刻的状态估计
    和协方差阵


    本发明步骤6中,通过如下方法计算当前时刻匹配模型对应的残差估值残差协
    方差似然函数和模型概率



    其中E[·]表示数学期望。

    本发明步骤7中,通过如下公式计算导航参数的全局估计及其估计误差的协方
    差阵Pk|k:


    本发明步骤5-1和步骤5-4中,采用中位秩计算第β层采样点对应的一维标准分布
    概率pβ,计算表达式为pβ=(β-0.3)/(2ρ+1.4)。

    本发明针对现有技术的不足,将秩滤波器与交互式多模型算法相结合,解决非线
    性、非高斯系统中的状态估计问题,提高了农业机器人组合导航算法估计性能。本发明内容
    同样适用于惯性组合导航、目标跟踪与识别、图像处理等其他多传感器信息融合及多源数
    据处理应用领域。

    有益效果:本发明相对于现有技术的优点为:

    (1)本发明采用交互式多模型信息融合算法结构,通过设计多个模型来逼近系统
    复杂的时变或非线性过程,能够使在建模条件下分析得到的系统性能保持或接近最优,相
    比于广义伪贝叶斯算法,兼具了计算复杂度低和估计精度高的优点。

    (2)本发明将秩滤波器引入到交互式多模型信息融合算法中,秩滤波器通过秩统
    计量相关原理确定采样点和权值,因此,它不受高斯分布条件限制,对于未知噪声干扰具有
    更好的适应性,从而使组合导航系统具有更好的稳定性和抗干扰能力。

    附图说明

    下面结合附图和具体实施方式对本发明做更进一步的具体说明,本发明的上述或
    其他方面的优点将会变得更加清楚。

    图1基于秩滤波的交互式多模型算法结构图;

    图2农业机器人仿真运行轨迹;

    图3俯仰角估计误差曲线;

    图4横摇角估计误差曲线;

    图5航向角估计误差曲线;

    图6东向速度估计误差曲线;

    图7北向速度估计误差曲线;

    图8纬度估计误差曲线;

    图9经度估计误差曲线。

    具体实施方式

    下面结合附图对发明的技术内容进行详细说明:

    本发明的基于秩滤波的农业机器人组合导航信息融合方法,以交互式多模型算法
    为滤波框架,采用秩滤波器替代传统算法中的卡尔曼滤波器,如图1所示为基于秩滤波的交
    互式多模型算法结构图。现以微电子机械(Micro-Electro-Mechanical System,MEMS)捷联
    惯性导航系统和实时动态(Real-time kinematic,RTK)全球定位系统RTK GPS构成的农业
    机器人组合导航系统为例来说明所提供的信息融合算法,具体步骤如下:

    步骤1,根据农业机器人所处环境,将土地平整度和卫星信号强度分级,设定土地
    平整度因子和卫星信号强度因子ξ(1≤ξ≤ξmax,ξ∈Z),Z为整数集,
    ξmax分别表示最大的土地不平整级别和卫星信号衰落级别;

    步骤2,以东北天坐标系为导航坐标系,右前上坐标系为载体坐标系,根据先验知
    识与农业机器人所处环境(例如障碍物、土地平整度、遮挡、光照等),建立MEMS捷联惯性导
    航系统、RTK GPS组合导航系统模型:


    其中,状态向量X=[φE φN φU δVE δVN δL δλ εbx εby εbz ▽bx ▽by ▽bz]T,
    φE、φN、φU为东向、北向和天向失准角;δVE、δVN为东向、北向速度误差;δL、δλ为纬度、经度
    误差;εbx、εby、εbz为陀螺的常值漂移;▽bx、▽by、▽bz为加速度计常值漂移。以MEMS-INS和GPS
    测得的速度、位置之差作为观测量,VINS和VGNSS为东北天坐标系下MEMS-
    INS和GPS测得的速度,PINS和PGNSS为东北天坐标系下MEMS-INS和GPS测得的位置;F和H分别
    为状态转移矩阵、观测矩阵;ω为状态噪声向量,协方差为Q;为量测噪声向量,考虑农田复
    杂环境对传感器测量精度的影响,建立以量测噪声向量ν及其协方差矩阵R为变化参数的模
    型集合M,设模型个数为r,Markov转移概率为πji,下标i,j分别表示第i个模型和第j个模型。

    步骤3,由k-1(k≥1)时刻的模型概率与先验的Markov转移概率πji进行交互,
    计算混合概率


    步骤4,对于第j=1,2,…,r个模型,利用k-1时刻的状态估计和协方差阵
    计算k时刻的状态初值与协方差阵初值


    步骤5,在组合导航系统获得新的量测zk之后,利用步骤4中计算得到的状态初值
    与协方差阵初值采用秩滤波算法进行滤波,得到k时刻的状态估计协方差
    阵残差估值以及残差协方差具体流程为:

    步骤5-1,以α,β为下标分别表示第α个采样点和第β层采样点,利用k-1时刻的状态
    估计一维标准分布概率pβ对应的分位点和第β层采样点的修正系数rβ,通过秩采
    样机制求取采样点集



    其中,n为状态向量x的维数,ρ为采样点层数,l取值为[1,n]中的整数,
    表示矩阵平方根的第l列,第β层采样点对应的概率pβ采用中位秩计算得到,计算表达
    式为pβ=(β-0.3)/(2ρ+1.4)。

    步骤5-2,将采样点集进行非线性传播,得到非线性变换后点集


    其中,f(·)为状态方程非线性函数,ω为过程噪声向量。

    步骤5-3,进一步预测状态估计和协方差阵


    其中,为第α个采样点对应的协方差修正系数,取值为1;Q为过程噪声方差矩阵;
    τ为协方差权重系数,其计算公式为:


    步骤5-4,通过秩采样机制求取采样点集



    其中,第β层采样点对应的概率pβ采用中位秩计算得到,计算表达式为pβ=(β-
    0.3)/(2ρ+1.4)。

    步骤5-5,将采样点集引入观测方程并进行非线性传播,得到预测的量测
    点集


    其中,h(·)为观测方程非线性函数,νj为模型j对应的量测噪声向量。

    步骤5-6,通过预测的量测点集计算量测估计


    步骤5-7,计算新息协方差矩阵和互协方差阵



    其中Rj为模型j对应的的量测噪声方差矩阵。

    步骤5-8,由新息协方差矩阵和互协方差阵计算增益


    k时刻组合导航系统的量测值zk到来之后,计算k时刻的状态估计和协方差阵
    具体计算公式为:


    步骤6,通过如下方法计算当前时刻匹配模型对应的残差估值残差协方差
    似然函数和模型概率:



    步骤7,利用步骤5中各模型滤波器所得到的状态估计协方差阵和步骤6中
    获取的模型概率计算融合输出结果:


    步骤8,返回步骤3,重复步骤3至步骤7,直至接收到停止导航控制指令。

    实施例

    为验证本发明的可行性,在Matlab下进行了仿真,滤波初始值及仿真参数设置如
    下:

    陀螺随机常值漂移为1°/h,白噪声随机漂移为0.1°/h;

    加速度计偏置误差为1mg,白噪声随机漂移为0.1mg;

    MEMS捷联惯性导航系统初始水平姿态角误差为1°,航向角误差为3°;

    初始速度为0m/s,初始速度误差为0m/s;

    初始位置为东经118°,北纬32°,高度100m,初始位置误差为0m;

    RTK GPS位置测量精度为0.1米;

    MEMS捷联惯性导航系统采样周期为1ms,RTK GPS采样周期为0.1s;

    最大的土地不平整级别最大卫星信号衰落级别ξmax=1,则建立的模型集合
    M中模型个数r为3,Markov转移概率为

    仿真时间为3600s,假设农业机器人在农田中模拟“割草机”运动,轨迹如图2所示。
    在同等条件下,导航计算机根据传感器数据和系统模型,分别采用交互式多模型扩展卡尔
    曼滤波(IMM-EKF)、交互式多模型无??ǘ瞬?IMM-UKF)和本发明提出的交互式多模型
    秩滤波(IMM-RKF)算法进行滤波估计,对比三种算法在组合导航系统中的估计结果。图3为
    俯仰角估计误差曲线,图4为横摇角估计误差曲线图,图5为航向角估计误差曲线,可以看
    出,三种算法所得到的水平姿态角误差均能收敛到很小的值并且收敛速度较快,航向角误
    差的修正效果不是很明显,但也能看出逐渐收敛的趋势,收敛速度较慢。从航向角误差的收
    敛过程可以看出,本发明提出的IMM-RKF算法估计效果优于其它两种算法;图6、图7分别为
    东向、北向速度估计误差曲线,由于实时动态GPS提供的速度信息对SINS的速度误差进行修
    正,采用IMM-RKF算法速度误差基本保持在0.2m/s以内,明显优于其它两种算法;图8、图9分
    别为纬度、经度估计误差曲线,可以看出IMM-UKF与IMM-RKF算法位置误差较为接近,IMM-
    EKF算法误差较大。进一步的,将三种算法得到的位置估计误差进行数值比较,采用IMM-EKF
    算法得到的经度估计误差均方差为0.051m,最大达到0.095m;纬度估计误差均方差为
    0.045m,最大达到0.102m。采用IMM-UKF算法得到的经度估计误差为0.042m,最大为0.049m;
    纬度估计误差均方差为0.008m,最大为0.061m。采用IMM-RKF算法得到的经度估计误差为
    0.027m,最大为0.032m;纬度估计误差均方差为0.005m,最大为0.039m。从数据对比结果可
    以看出,本发明提出的基于秩滤波的农业机器人组合导航信息融合方法性能优于现有的其
    它两种交互式多模型信息融合算。

    本发明提供了一种基于秩滤波的农业机器人组合导航信息融合方法,具体实现该
    技术方案的方法和途径很多,以上所述仅是本发明的优选实施方式,应当指出,对于本技术
    领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这
    些改进和润饰也应视为本发明的?;し段?。本实施例中未明确的各组成部分均可用现有技
    术加以实现。

       内容来自专利网重庆时时彩单双窍门 www.4mum.com.cn转载请标明出处

    关于本文
    本文标题:一种基于秩滤波的农业机器人组合导航信息融合方法.pdf
    链接地址://www.4mum.com.cn/p-6001487.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
  • 安徽快3基本走势图 众彩票论坛 北京快三跨度走势图一定牛 体育彩票7位数 双色球奖金分配 麻将游戏单机版 ↙微乐龙江麻将 股票分析软件排行榜 股票分析软件哪个好 双色球开奖视频 江苏快三开奖结果老 英超积分榜2017一2018 真人麻将四人麻将河北 信阳股票配资 极速飞艇开奖 qq篮彩 天天捕鱼2赢话费