CN116222583B - 一种基于滑动窗口的协同导航快速计算方法 - Google Patents

一种基于滑动窗口的协同导航快速计算方法 Download PDF

Info

Publication number
CN116222583B
CN116222583B CN202310518829.XA CN202310518829A CN116222583B CN 116222583 B CN116222583 B CN 116222583B CN 202310518829 A CN202310518829 A CN 202310518829A CN 116222583 B CN116222583 B CN 116222583B
Authority
CN
China
Prior art keywords
time
constraint
moment
dead reckoning
agent
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310518829.XA
Other languages
English (en)
Other versions
CN116222583A (zh
Inventor
蔡庆中
涂勇强
牛皓飞
杨功流
李健
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202310518829.XA priority Critical patent/CN116222583B/zh
Publication of CN116222583A publication Critical patent/CN116222583A/zh
Application granted granted Critical
Publication of CN116222583B publication Critical patent/CN116222583B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于滑动窗口的协同导航快速计算方法，步骤为：S1、基于各智能体配备有惯性传感器、里程计和超宽带测距传感器，以及能够与智能体建立通信的超宽带基站，构建对各智能体的传感器观测模型；S2、基于各智能体的航位推算约束和UWB相对测距约束，设定因子图模型的图顶点以及边的连接形式，并构建各智能体的因子图模型；S3、基于各协同智能体的因子图模型及滑动窗口优化法，采用LM算法对目标函数进行求解，以得到各节点在k时刻下的导航结果；该方法解决了基于因子图模型的目标函数优化求解过程中计算量大，计算时间长，协同导航实时性差的问题，不仅实现了进一步提高导航精度，且大幅提升了导航解算效率，操作简便、实用性好。

Description

一种基于滑动窗口的协同导航快速计算方法
技术领域
本发明涉及多智能体协同导航与定位技术领域，特别涉及一种基于滑动窗口的协同导航快速计算方法。
背景技术
智能无人集群系统由无人机、智能体、机器人等多智能体通过彼此之间的信息交互构成，可以智能协同控制，完成单个无人系统无法实现的复杂工作，在军事和民用领域具有广阔应用前景。协同导航技术是一项旨在多智能体系统下提高个体定位与导航精度的关键技术。在多智能体系统中，通过系统中各个子节点之间的导航状态共享与相互观测的方式来为子节点提供更丰富的观测信息，各个节点相互协同，可实现在低成本条件下的导航定位精度提升。
已公开专利申请CN114838732A提出了一种基于图优化的通信受限环境下协同导航方法，提高复杂通信受限环境下集群综合导航精度。然而，该方法在对基于因子图模型的目标函数优化求解导航结果时，需要计算节点各个时刻的状态变量，导致计算量大，计算时间长，影响了协同导航的实时性。
发明内容
本发明的目的是提供一种解决上述现有技术中存在的对基于因子图模型的目标函数优化求解导航结果时，需要计算节点各个时刻的状态变量，导致计算量大，计算时间长，协同导航实时性差的问题的基于滑动窗口的协同导航快速计算方法。
为此，本发明技术方案如下：
一种基于滑动窗口的协同导航快速计算方法，其具体步骤如下：
S1、基于各智能体配备有惯性传感器、里程计和超宽带测距传感器，能够与智能体建立通信的超宽带基站，构建对各智能体的传感器观测模型；其中，在传感器观测模型由航位推算约束和UWB相对测距约束构成，智能体的状态变量包括智能体位置坐标（）、航向角/>和天向陀螺仪零偏/>；航位推算约束根据智能体自身基于惯性传感器的航位推算信息建立而成，包括时间约束和状态约束；UWB相对测距约束根据各智能体之间相对测距信息及智能体与锚点之间的相对测距信息建立而成；
S2、基于各智能体的航位推算约束和UWB相对测距约束，设定因子图模型的图顶点以及边的连接形式，并构建各智能体的因子图模型；
S3、基于各协同智能体的因子图模型及滑动窗口优化法，采用LM算法对目标函数：进行求解，以得到各节点在k时刻下的位置坐标（包括x轴位置坐标和y轴位置坐标）、航向角、陀螺仪零偏的优化结果；其中，/>为航位推算的状态约束，/>为航位推算的时间约束，/>为UWB相对测距约束；其中，在步骤S3中，对任一智能体在k时刻状态变量的求解步骤为：
1）定义滑动窗口的大小为N，其物理含义为滑动窗口中包含智能体的N个时刻的状态变量；
2）设定起始时刻T=1的状态变量初值X 1 并代入至因子图模型中，基于初值X 1 和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数进行求解，得到T=1时刻的优化结果；
3）在T=2时刻下，将步骤2）得到的、以及基于/>推算得到的T=2时刻的预测值X 2 作为T=2时刻的初值并代入至因子图模型中，基于航位推算的时间约束对应的雅可比矩阵、航位推算的状态约束对应的雅可比矩阵和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数进行求解，得到T=2时刻的优化结果/>；
4）重复步骤3），直至解算得到T=N时刻的优化结果：；
5）在T=N+1时刻下，将自X T-N 至之前的各时刻状态变量转化为一个先验因子；其中，先验因子的表达式为：/>，式中，/>为先验因子的矩阵，其表达式为：/>；/>，，/>为智能体待估计的k时刻x轴位置坐标，/>为智能体待估计的k时刻y轴位置坐标，/>为智能体待估计的k时刻航向角，/>为智能体待估计的k时刻天向陀螺零偏，/>为航位推算解算周期，/>为前一时刻的速率观测值；(/>,/>为其它协同智能体或锚点的坐标；/>为陀螺仪角速率随机噪声，/>的表达式为：/>)，/>为100 (°//>)2；/>为UWB的相对测距误差噪声，/>的表达式为：/>)，/>为0.1 m2；k= T-N；
进而，视先验因子为X’ T-N，并将N+1个状态变量X’ T-N,…,X N+1作为T= N+1时刻下的初值代入至因子图模型中，采用与步骤3）相同的方法进行解算，得到T= N+1时刻的优化结果/>；
6）依次类推，在接下来的每个时刻重复步骤5），以不断优化并解算得到当前时刻T=k下的状态变量；/>即为当前时刻T=k的状态量，即导航结果。
进一步地，在步骤S1中，智能体航位推算约束包括航位推算的时间约束和航位推算的状态约束，其构建步骤为：
1）设定无人机自身惯性传感器提供的天向陀螺仪k时刻的角速率为，里程计输出的k时刻的速率观测值为/>，采样间隔时间为/>，且设定陀螺零偏不随时间变化；则，智能体在k-1时刻与k时刻之间的状态递推方程为：
，
，
，
式中，为智能体k时刻的航向角，/>为k-1时刻的航向角，/>为k-1时刻的角速率，/>为k时刻的x轴位置坐标，/>为k-1时刻的x轴位置坐标，/>为k-1时刻的速率，/>为k时刻的y轴位置坐标，/>为k-1时刻的y轴位置坐标；/>为k时刻的天向陀螺零偏；
2）设待估计状态量为：/>，式中，/>为智能体待估计的k时刻x轴位置坐标，/>为智能体待估计的k时刻y轴位置坐标，/>为智能体待估计的k时刻航向角，/>为智能体待估计的k时刻天向陀螺零偏；
3）构建航位推算的时间约束，其表达式为：
，
，
，
，
式中，为k-1到k时刻之间关于x轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于y轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于航向角的时间约束残差，/>为k-1到k时刻之间关于天向陀螺零偏的时间约束残差，/>为智能体待估计的k-1时刻x轴位置坐标，/>为智能体待估计的k-1时刻y轴位置坐标，/>为智能体待估计的k-1时刻航向角，/>为智能体待估计的k-1时刻天向陀螺零偏；
4）构建航位推算的状态约束，其表达式为：
，
，
，
式中，为k时刻关于x轴位置坐标与航向角的状态约束残差，/>为k时刻关于x轴位置坐标与航向角的状态约束残差；/>为k时刻关于航向角的状态约束残差，/>为航向角的初值，/>为历史某一时刻。
进一步地，在步骤S1中，UWB相对测距约束构建为：，式中，/>为k时刻的UWB相对测距约束残差，/>为k时刻智能体通过UWB相对测距获得的其与其它协同智能体之间的距离观测值、或其与锚点之间的距离观测值；(/>,/>为其它协同智能体或锚点的坐标。
进一步地，对于任一智能体，步骤S2的具体实施步骤为：
S201、根据步骤S1构建的航位推算的时间约束、航位推算的状态约束和UWB相对测距约束分别推导出与之对应的雅可比矩阵；
S202、对任一智能体的因子图模型的架构方式进行设定：
1）设智能体系统优化变量为所有时刻的状态变量，其表达式为：，各时刻下的待估计状态量作为因子图模型的图顶点，并按照时序排布；
2）设定每相邻时刻之间的状态变量通过航位推算因子进行连线；其中，航位推算因子由航位推算的状态约束：和航位推算的时间约束：构成；智能体每两个相邻时刻下的x轴位置坐标之间、节点y轴位置坐标之间、航向角之间、以及天向陀螺仪零偏/>之间均通过航位推算的时间约束进行连线；智能体同一时刻下的天向陀螺仪零偏与航向角之间通过航位推算的状态约束进行连线，同时航向角分别与节点x轴位置坐标、节点y轴位置坐标之间通过航位推算的状态约束进行连线；
3）对各状态变量添加总UWB因子：；各状态变量均与当前状态对应的总UWB因子进行连线；
S203、基于步骤S202中对图顶点以及边的连接形式的设定，构建得到各协同节点相应的因子图模型。
进一步地，在步骤S3的UWB相对测距约束确定中，智能体之间通信关系基于“高通信精度节点向低精度节点进行信息传输”的原则进行，通信信息包括位置信息和相对距离观测值。
进一步地，在步骤S3中，N为正整数，其取值范围为2~5。
与现有技术相比，该基于滑动窗口的协同导航快速计算方法解决了现有技术中基于因子图模型的目标函数优化求解导航结果时，需要计算节点各个时刻的状态变量，导致计算量大，计算时间长，协同导航实时性差的问题，通过在导航结果优化解算过程中设置滑动窗口，即将滑动窗口前各个时刻的状态变量转化为先验因子代入解算中，对解算过程进行了优化，不仅实现了进一步提高导航精度，且大幅提升了导航解算效率，操作简便、省时，具有很好的实用性。
附图说明
图1为本发明的基于滑动窗口的协同导航快速计算方法的流程图；
图2（a）为本发明的航位推算时间约束因子图模型；
图2（b）为本发明的航位推算状态约束因子图模型；
图3为本发明建立的系统中智能体导航定位的完整因子图模型；
图4为本发明建立的图优化算法结构图；
图5为本发明的实施例中仿真实验的典型场景示意图；
图6为本发明的实施例中采用本申请的方法对三个节点进行导航解算得到的导航精度结果图；
图7为本发明的实施例中分别采用本申请的方法和现有技术方法对三个节点进行导航解算得到的总耗时结果图。
具体实施方式
下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。
参见图1，该基于滑动窗口的协同导航快速计算方法的具体实施步骤如下：
S1、基于各智能体配备的惯性传感器、里程计和超宽带测距传感器（即UWB测距传感器），以及能够与智能体建立通信的通信基站（即超宽带基站），构建由智能体的航位推算约束和UWB相对测距约束构成的传感器观测模型；其中，航位推算约束根据智能体自身基于惯性传感器的航位推算信息建立而成；UWB相对测距约束根据各智能体之间相对测距信息及智能体与锚点之间的相对测距信息建立而成；
具体地，步骤S1的观测模型构建的具体步骤为：
S101、建立智能体航位推算约束：
设定智能体自身惯性传感器提供的天向陀螺仪k时刻的角速率为，里程计输出的k时刻的速率观测值为/>，采样间隔时间为/>，且设定陀螺零偏不随时间变化；则，智能体在惯导坐标系下k-1时刻与k时刻之间的状态递推方程为：
，
，
，
式中，为智能体k时刻的航向角，/>为k-1时刻的航向角，/>为k-1时刻的角速率，/>为k时刻的x轴位置坐标，/>为k-1时刻的x轴位置坐标，/>为k-1时刻的速率，/>为k时刻的y轴位置坐标，/>为k-1时刻的y轴位置坐标；/>为k时刻的天向陀螺零偏；
S102、根据k-1时刻与k时刻之间的状态变化量构建航位推算约束，包括航位推算的时间约束和航位推算的状态约束；
具体地，时间约束基于两时刻之间状态增量建立，状态约束基于同一时刻下不同状态量之间满足的约束关系建立；基于此，
设待估计状态量为：/>，
式中，为智能体待估计的k时刻x轴位置坐标，/>为智能体待估计的k时刻y轴位置坐标，/>为智能体待估计的k时刻航向角，/>为智能体待估计的k时刻天向陀螺零偏；
进而，构建航位推算的时间约束，其表达式为：
，
，
，
，
式中，为k-1到k时刻之间关于x轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于y轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于航向角的时间约束残差，/>为k-1到k时刻之间关于天向陀螺零偏的时间约束残差，/>为智能体待估计的k-1时刻x轴位置坐标，/>为智能体待估计的k-1时刻y轴位置坐标，/>为智能体待估计的k-1时刻航向角，/>为智能体待估计的k-1时刻天向陀螺零偏；如图2（a）所示为航位推算的时间约束的构建方式示意图；
构建航位推算的状态约束，其表达式为：
，
，
式中，为k时刻关于x轴位置坐标与航向角的状态约束残差，/>为k时刻关于x轴位置坐标与航向角的状态约束残差；
其中，由于天向陀螺零偏影响航向角的误差，因此以当前k时刻的航向角估计值与量测值建立残差，即可得到航向角状态约束的表达式为：
，
式中，为k时刻关于航向角的状态约束残差，/>为航向角的初值，/>为历史某一时刻；如图2（b）所示为航位推算的状态约束的构建方式示意图；
其中，以带有*的物理量代表该变量参与优化，其余不带*的物理量作为已知量测值输入，不参与优化；
S103、基于超宽带测距传感器（简称为UWB）输出距离观测值，构建UWB相对测距约束，其表达式可为：
，
式中，为k时刻的UWB相对测距约束残差，/>为k时刻智能体通过UWB相对测距获得的其与其它协同智能体（或其与通信基站）之间的距离观测值；(/>,/>为其它协同智能体（或通信基站）的坐标。
S2、基于步骤S1建立的智能体的航位推算约束和UWB相对测距约束，设定因子图模型的图顶点以及边的连接形式，进而构建智能体的因子图模型；
具体地，步骤S2的实施步骤如下：
S201、根据步骤S1构建的航位推算的时间约束，推导出其对应的雅可比矩阵形式：
，
根据步骤S1构建的航位推算的状态约束，推导出其对应的雅可比矩阵形式：
，
根据步骤S1构建的UWB相对测距约束，推导出其对应的雅可比矩阵形式：
，
S202、对因子图模型的架构方式进行设定：
对于处于协同系统中任一协同智能体，状态变量包括智能体位置坐标（）、航向角/>和天向陀螺仪零偏/>；因此，如图3所示，对于任一智能体：
1）基于步骤S1中设定的待估计状态量（/>），设系统优化变量为所有时刻的状态变量，其表达式为：/>，各时刻下的待估计状态量作为因子图模型的图顶点，并按照时序排布；
2）设定航位推算因子由航位推算的状态约束和航位推算的时间约束构成，
航位推算的状态约束为：，
航位推算的时间约束为：；
具体来说，对于各状态变量来说，两个相邻时刻的智能体x轴位置坐标之间、智能体y轴位置坐标之间、航向角之间、以及天向陀螺仪零偏之间均通过航位推算的时间约束（即图3中的三角形标识）进行连线，表示该两个时刻的状态量之间存在航位推算的时间约束关系；而在同一时刻下，智能体的天向陀螺仪零偏与航向角之间通过航位推算的状态约束进行连线，表示二者之间存在航位推算的状态约束关系（即图3中的方形标识），而航向角分别与智能体x轴位置坐标、智能体y轴位置坐标之间通过航位推算的状态约束进行连线，表示其分别与智能体x轴位置坐标、智能体y轴位置坐标之间存在航位推算的状态约束的关系（即图3中的方形标识）；
3）对各状态变量添加总UWB因子；具体地，由于k时刻任一协同智能体可能存在m个UWB相对测距约束，每个UWB相对测距约束为一个UWB因子，因此，设定在k时刻下任一协同智能体对应有由m个UWB相对测距约束构成的总UWB因子，具体地，总UWB因子表示为：；
S203、基于步骤S202中对图顶点以及边的连接形式的设定，构建得到各协同智能体相应的因子图模型，即如图3所示。
S3、基于协同系统中各智能体的因子图模型及滑动窗口优化法，采用LM算法对目标函数：进行求解，以得到各智能体在k时刻下的位置坐标（包括x轴位置坐标和y轴位置坐标）、航向角、陀螺仪零偏的优化结果；其中，/>为航位推算的状态约束，/>为航位推算的时间约束，/>为UWB相对测距约束。
该步骤S3与已公开专利 CN114838732A中的步骤S3不同；具体来说，在已公开专利CN114838732A的步骤S3中，对任一智能体自k时刻至k+1时刻的状态变量优化结果求解过程中，状态变量的初始值为在先全部时刻的状态量，即为了得到k+1时刻的状态变量，需要把X 1 ，X 2 ，……，直到X k+1 的结果都要重新优化计算，才能得到k+1时刻下各状态变量优化值；上述过程在实际应用中，计算量巨大，非常浪费时间；然而，对于导航结果来说，导航有实时性的要求，我们关心的只有实时的结果，即是k+1时刻下我们真正需要的是知道k+1时刻下的状态量X k+1 ，而按照已公开利 CN114838732A的方式将之前的时刻状态变量全部再算一遍，计算量大了，计算时间就长了，实时性肯定就不好了，因此，本申请即对这一缺陷进行改进。
如图4所示为系统中任一协同智能体基于因子图模型图优化算法结构图；相应地，在该步骤S3中，以协同系统中的任一智能体i为例，对其获取待估计状态量的实施优化计算步骤进行具体描述如下：
S301、定义滑动窗口的大小为N，其物理含义为滑动窗口中包含智能体i的N个时刻的状态变量；其中，N为正整数，其取值范围为2~5；
在本实施例中，N=2，即滑动窗口中包含智能体i的2个时刻的状态变量；
S302、设定起始时刻（T=1）状态变量的初值X 1 ，并将其代入至因子图模型中，得到T=1时刻的优化结果；
具体地，由于在初始时刻状态变量仅受相对测距因子约束，因此基于初值X 1 和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数：（k=1）进行求解，得到；
S303、将步骤S302计算得到的添加到T=2时刻下第一个状态的图顶点上（即沿图4中的黑色虚线路径），作为历史时刻状态的初值X 1 ；将T=2时刻的优化结果/>代入步骤S101的状态递推方程中，通过航位推算得到下一时刻（k=2）的预测值X 2 ，该预测值X 2 作为T=2时刻的优化初始值；进而，通过将T=2时刻下的初值X 1 和X 2 代入至因子图模型中，得到T=2时刻的优化结果/>；
具体地，由于在T=2时刻下，初值X 1 和X 2 不仅各自受到相对测距因子约束，同时初值X 1 和X 2 之间还受到航位推算因子的约束，因此，基于初值X 1 和X 2 ，以及航位推算的时间约束对应的雅可比矩阵、航位推算的状态约束对应的雅可比矩阵和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数：（k=2）进行求解，得到优化后的/>；
其中，在该步骤S303中，对于任一协同智能体在某一时刻下的相对测距因子约束可能包括有该智能体与其能够构建通信的锚点之间形成的约束、以及该智能体与其接收通信的协同智能体之间形成的约束；其中，
1）对于智能体与其能够构建通信的锚点之间形成的UWB相对测距约束，其表达式为：；其中，/>和/>为锚点的x轴位置坐标和y轴位置坐标；/>为k时刻智能体通过UWB相对测距获得的其与锚点之间的距离观测值；
2）对于智能体与其接收通信的协同智能体之间形成的UWB相对测距约束，其表达式为：；其中，/>和/>为另外协同智能体的x轴位置坐标和y轴位置坐标；/>为k时刻智能体通过UWB相对测距获得的其与另外协同的智能体之间的距离观测值；
其中，协同智能体之间的通信关系基于“高通信精度智能体向低精度智能体进行信息传输”的原则，即通信精度高的智能体向低于其通信精度的智能体发送通信信息，通信信息包括位置信息（包括当前时刻的x轴位置坐标和y轴位置坐标）以及相对距离观测值；
S304、重复步骤S303，直至计算至T=N时刻的优化结果：；
S305、在T=N+1时刻下，将位于X T-N 后的N个时刻状态变量置于滑动窗口内，并将自X T-N 至之前的各个时刻的状态变量转化为一个先验因子；其中，
先验因子的表达式为：
，
式中，为先验因子矩阵，其表达式为：/>；
其中，，，
上二式中，为智能体待估计的k时刻x轴位置坐标，/>为智能体待估计的k时刻y轴位置坐标，/>为智能体待估计的k时刻航向角，/>为智能体待估计的k时刻天向陀螺零偏，/>为航位推算解算周期，/>为前一时刻的速率观测值；(/>,/>为其它协同智能体或锚点的坐标；/>为陀螺仪角速率随机噪声，/>的表达式为：/>)，/>为100 (°/)2；/>为UWB的相对测距误差噪声，/>的表达式为：/>)，/>为0.1 m2；k=T-N；
进而，视先验因子为X’ T-N，并将N+1个状态变量X’ T-N,…, X N+1作为T= N+1时刻下的初值代入至因子图模型中，采用与步骤S303相同的方法进行解算，得到T=N+1时刻的优化结果：/>；
在该步骤S301中，将滑动窗口之前各个时刻的状态变量转化为先验因子限制，也即将自X 1 至X T-N 的各个时刻状态变量变为先验因子限制；这是因为如果将滑动窗口之前的各个时刻的状态变量直接抹除，则无法利用先前状态变量信息，导致导航精度的下降，因此，在该步骤中，通过将滑动窗口之前的各个时刻的状态变量转换为先验因子，以保证优化结果依然能够保持很好的精度；
S306、依次类推，在接下来的每个时刻重复步骤S305，以不断优化并解算得到当前时刻T=k下的状态变量；/>即为当前时刻T=k的状态量，即导航结果。
在本实施例中，N=2，因此，以T=5为当前时刻为例，将k值取3时对应计算得到视为X’ 3，将X’ 3、X 4和X 5作为初始值代入因子图模型中，得到优化结果]；同理，对于下一时刻T=6，则将k值取4时对应计算得到/>视为X’ 4，将X’ 4、X 5和X 6作为初始值代入因子图模型中，得到优化结果/>]；
在该步骤S3中，通过对滑动窗口的N个时刻状态变量进行优化，得到优化结果；其中，/>即为当前时刻T=k的状态量，即导航结果。相对于已公开专利CN114838732A存在的计算量大，实时性效果较差的问题，本申请的方法，引入了滑动窗口，通过将滑动窗口之前的时刻化为一个先验因子，以仅计算包含在滑动窗口内的各时刻的状态变量，使导航精度相较现有技术略有提升的同时，极大地缩短了计算时间，提高了实时性。
为了进一步证明本申请在实际应用中，相对于现有技术在导航精度和导航时间上的优势，将本申请与现有技术应用于仿真场景中，并与实测场景中的结果进行对比。
如图5所示为本实施例应用的仿真场景；在该仿真场景中，初始状态下，场景分为位于房间内的室内场景和位于房间外的室外场景；室内场景中布置有两个智能体，具体为一辆无人车和一名侦查员，室外场景中布置有一个智能体，具体为一辆无人车；其中，室内场景布置为：房间西侧靠北设有第一进出门，房间南侧自西向东间隔地设有三个进出门，分别为第二进出门、第三进出门和第四进出门，在靠近房间东侧设有一面与第一进出门相对的遮挡墙；三个通信基站分别命名为锚点1、锚点2和锚点3，锚点1和锚点2设置于室外南侧，且二者的设置位置至房间的距离一致，锚点1与第二进出门和第三进出门的连线中点相对，锚点2与第四进出门和第三进出门的连线中点相对，锚点3设置与室外西侧并与第一进出门相对；锚点1、锚点2和锚点3均可用于在通信不受阻的情况下与智能体、侦查员建立实时通信；室外的智能体命名为节点1，其初始位于第二进出门的西侧；节点1的运动轨迹设定为先从室外初始位置行驶至室内，并沿南侧墙体自西侧向东侧行驶。该三个智能体构成协同系统，每个智能体均配置有惯性传感器、里程计和超宽带测距传感器。
在整个运动过程中，节点1能够与室外南侧的两个通信基站（即锚点1和锚点2）建立通信；室内的智能体命名为节点2，其初始位于第一进出门的东侧且靠近北侧墙体处；节点2的运动轨迹设定为自室内北侧行驶至室内南侧；在整个运动过程中，节点2能够与位于室外西侧的通讯基站（即锚点3）建立通信；室内的侦查员命名为节点3，其初始位于遮挡墙西侧且靠近北侧墙体处；节点3的运动轨迹设定为自室内北侧朝南行驶一段距离至遮挡墙端头、再转弯朝向室内东侧行驶；在整个运动过程中，节点3无法与任何通讯基站建立通信。
分别采用本申请的方法和现有技术（已公开专利CN114838732A）的方法，对上述仿真过程进行协同导航解算，并与安装在各个节点的高精度惯导基准进行对比。具体测试结果如下表1所示。
表1：
导航方法 节点1 节点2 节点3
本申请定位精度 0.01m 0.09m 0.375m
现有技术定位精度 0.02m 0.1m 0.4m
从表1的测试结果可以看出，采用本申请的方法进行导航解算得到的导航定位精度相对于现有技术实现了进行一步提升，其中，节点1的导航定位精度提升了50%，节点2的导航定位精度提升了10%，节点3的导航定位精度提升了6.25%。如图6所示为采用本实施例的方法对三个节点进行导航解算得到的导航精度结果图。
如图7为采用本实施例的方法分别采用本申请的方法和现有技术方法对三个节点进行导航解算得到的总耗时结果图；
从导航解算时间上进行对比可知，采用现有技术方法实现导航解算所需的时间为646s，而采用本申请的方法实现导航解算所需的时间为38.44s，可见本申请的方法在解算效率上相比于现有技术的方法提升了94.5%，即证明了本申请的方法在导航效率有较大提升，提高了协同导航的实时性，实用性佳。
本发明未详细公开的部分属于本领域的公知技术。尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化时显而易见的，一切利用本发明构思的发明创造均为保护之列。

Claims (6)

1.一种基于滑动窗口的协同导航快速计算方法，其特征在于，步骤如下：
S1、基于各智能体配备有惯性传感器、里程计和超宽带测距传感器，以及能够与智能体建立通信的超宽带基站，构建对各智能体的传感器观测模型；其中，在传感器观测模型由航位推算约束和UWB相对测距约束构成，智能体的状态变量包括智能体位置坐标（）、航向角/>和天向陀螺仪零偏/>；航位推算约束根据智能体自身基于惯性传感器的航位推算信息建立而成，包括时间约束和状态约束；UWB相对测距约束根据各智能体之间相对测距信息及智能体与锚点之间的相对测距信息建立而成；
S2、基于各智能体的航位推算约束和UWB相对测距约束，设定因子图模型的图顶点以及边的连接形式，并构建各智能体的因子图模型；
S3、基于各协同智能体的因子图模型及滑动窗口优化法，采用LM算法对目标函数：进行求解，以得到各节点在k时刻下的位置坐标，包括x轴位置坐标和y轴位置坐标、航向角、陀螺仪零偏的优化结果；其中，/>为航位推算的状态约束，/>为航位推算的时间约束，/>为UWB相对测距约束；其中，在步骤S3中，对任一智能体在k时刻状态变量的求解步骤为：
1）定义滑动窗口的大小为N，其物理含义为滑动窗口中包含智能体的N个时刻的状态变量；
2）设定起始时刻T=1的状态变量初值X 1 并代入至因子图模型中，基于初值X 1 和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数进行求解，得到T=1时刻的优化结果；
3）在T=2时刻下，将步骤2）得到的、以及基于/>推算得到的T=2时刻的预测值X 2 作为T=2时刻的初值并代入至因子图模型中，基于航位推算的时间约束对应的雅可比矩阵、航位推算的状态约束对应的雅可比矩阵和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数进行求解，得到T=2时刻的优化结果/>；
4）重复步骤3），直至解算得到T=N时刻的优化结果：；
5）在T=N+1时刻下，将位于X T-N 后N个时刻的状态变量置于滑动窗口内，并将自X T-N 至之前的各时刻状态变量转化为一个先验因子；先验因子的表达式为：，式中，/>为先验因子的矩阵，/>；，/>，/>为智能体待估计的k时刻x轴位置坐标，/>为智能体待估计的k时刻y轴位置坐标，/>为智能体待估计的k时刻航向角，/>为智能体待估计的k时刻天向陀螺零偏，/>为航位推算解算周期，/>为前一时刻的速率观测值；(/>,/>为其它协同智能体或锚点的坐标；/>为陀螺仪角速率随机噪声，/>的表达式为：/>)，/>为100 (°//>)2；/>为UWB的相对测距误差噪声，/>的表达式为：/>)，/>为0.1 m2；k=T-N；
进而，视先验因子为X’ T-N，并将N+1个状态变量X’ T-N,…, X N+1作为T= N+1时刻下的初值代入至因子图模型中，采用与步骤3）相同的方法进行解算，得到T= N+1时刻的优化结果/>；
6）依次类推，在接下来的每个时刻重复步骤5），以不断优化并解算得到当前时刻T=k下的状态变量；/>即为当前时刻T=k的状态量，即导航结果。
2.根据权利要求1所述的基于滑动窗口的协同导航快速计算方法，其特征在于，在步骤S1中，航位推算的时间约束和航位推算的状态约束的具体构建步骤为：
1）设定无人机自身惯性传感器提供的天向陀螺仪k时刻的角速率为，里程计输出的k时刻的速率观测值为/>，采样间隔时间为/>，且设定陀螺零偏不随时间变化；则，智能体在k-1时刻与k时刻之间的状态递推方程为：
，
，
，
式中，为智能体k时刻的航向角，/>为k-1时刻的航向角，/>为k-1时刻的角速率，/>为k时刻的x轴位置坐标，/>为k-1时刻的x轴位置坐标，/>为k-1时刻的速率，/>为k时刻的y轴位置坐标，/>为k-1时刻的y轴位置坐标；/>为k时刻的天向陀螺零偏/>；
2）设待估计状态量为：/>，式中，/>为智能体待估计的k时刻x轴位置坐标，/>为智能体待估计的k时刻y轴位置坐标，/>为智能体待估计的k时刻航向角，/>为智能体待估计的k时刻天向陀螺零偏；
3）构建航位推算的时间约束，其表达式为：
，
，
，
，
式中，为k-1到k时刻之间关于x轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于y轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于航向角的时间约束残差，/>为k-1到k时刻之间关于天向陀螺零偏的时间约束残差，/>为智能体待估计的k-1时刻x轴位置坐标，/>为智能体待估计的k-1时刻y轴位置坐标，/>为智能体待估计的k-1时刻航向角，/>为智能体待估计的k-1时刻天向陀螺零偏；
4）构建航位推算的状态约束，其表达式为：
，
，
，
式中，为k时刻关于x轴位置坐标与航向角的状态约束残差，/>为k时刻关于x轴位置坐标与航向角的状态约束残差；/>为k时刻关于航向角的状态约束残差，/>为航向角的初值，/>为历史某一时刻。
3.根据权利要求1所述的基于滑动窗口的协同导航快速计算方法，其特征在于，在步骤S1中，UWB相对测距约束构建为：，式中，/>为k时刻的UWB相对测距约束残差，/>为k时刻智能体通过UWB相对测距获得的其与其它协同智能体之间的距离观测值、或其与锚点之间的距离观测值；(/>,/>为其它协同智能体或锚点的坐标。
4.根据权利要求1所述的基于滑动窗口的协同导航快速计算方法，其特征在于，对于任一智能体，步骤S2的具体实施步骤为：
S201、根据步骤S1构建的航位推算的时间约束、航位推算的状态约束和UWB相对测距约束分别推导出与之对应的雅可比矩阵；
S202、对任一智能体的因子图模型的架构方式进行设定：
1）设智能体系统优化变量为所有时刻的状态变量，其表达式为：，各时刻下的待估计状态量作为因子图模型的图顶点，并按照时序排布；
2）设定每相邻时刻之间的状态变量通过航位推算因子进行连线；其中，航位推算因子由航位推算的状态约束：和航位推算的时间约束：构成；智能体每两个相邻时刻下的x轴位置坐标之间、节点y轴位置坐标之间、航向角之间、以及天向陀螺仪零偏/>之间均通过航位推算的时间约束进行连线；智能体同一时刻下的天向陀螺仪零偏与航向角之间通过航位推算的状态约束进行连线，同时航向角分别与节点x轴位置坐标、节点y轴位置坐标之间通过航位推算的状态约束进行连线；
3）对各状态变量添加总UWB因子：；各状态变量均与当前状态对应的总UWB因子进行连线；
S203、基于步骤S202中对图顶点以及边的连接形式的设定，构建得到各协同节点相应的因子图模型。
5.根据权利要求1所述的基于滑动窗口的协同导航快速计算方法，其特征在于，在步骤S3的UWB相对测距约束确定中，智能体之间通信关系基于“高通信精度节点向低精度节点进行信息传输”的原则进行，通信信息包括位置信息和相对距离观测值。
6.根据权利要求1所述的基于滑动窗口的协同导航快速计算方法，其特征在于，在步骤S3中，N为正整数，其取值范围为2~5。
CN202310518829.XA 2023-05-10 2023-05-10 一种基于滑动窗口的协同导航快速计算方法 Active CN116222583B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310518829.XA CN116222583B (zh) 2023-05-10 2023-05-10 一种基于滑动窗口的协同导航快速计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310518829.XA CN116222583B (zh) 2023-05-10 2023-05-10 一种基于滑动窗口的协同导航快速计算方法

Publications (2)

Publication Number Publication Date
CN116222583A CN116222583A (zh) 2023-06-06
CN116222583B true CN116222583B (zh) 2023-07-21

Family

ID=86570012

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310518829.XA Active CN116222583B (zh) 2023-05-10 2023-05-10 一种基于滑动窗口的协同导航快速计算方法

Country Status (1)

Country Link
CN (1) CN116222583B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116753963B (zh) * 2023-08-16 2023-12-01 湖南大学 一种基于信赖域算法的室内移动机器人定位方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111272192A (zh) * 2020-02-11 2020-06-12 清华大学 用于滑移转向机器人的联合位姿确定方法及装置
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
WO2022156755A1 (zh) * 2021-01-21 2022-07-28 深圳市普渡科技有限公司 一种室内定位方法、装置、设备和计算机可读存储介质
CN114838732A (zh) * 2022-05-18 2022-08-02 北京航空航天大学 一种基于图优化的通信受限环境下协同导航方法
CN115655268A (zh) * 2022-11-01 2023-01-31 江苏崇创嘉航智能科技有限公司 一种基于图优化算法的地下矿井掘进机导航定位方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115082549A (zh) * 2021-03-10 2022-09-20 北京图森智途科技有限公司 位姿估计方法及装置、以及相关设备和存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111272192A (zh) * 2020-02-11 2020-06-12 清华大学 用于滑移转向机器人的联合位姿确定方法及装置
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
WO2022156755A1 (zh) * 2021-01-21 2022-07-28 深圳市普渡科技有限公司 一种室内定位方法、装置、设备和计算机可读存储介质
CN114838732A (zh) * 2022-05-18 2022-08-02 北京航空航天大学 一种基于图优化的通信受限环境下协同导航方法
CN115655268A (zh) * 2022-11-01 2023-01-31 江苏崇创嘉航智能科技有限公司 一种基于图优化算法的地下矿井掘进机导航定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
A new visual/inertial integrated navigation algorithm based on sliding-window factor graph optimisation;Haiying Liu,et al.;The Journal of Navigation;第74卷(第2期);343-359 *
基于滑动窗口滤波器的无误差累积位姿估计;宋佳儒;胡士强;杨永胜;;计算机仿真(第04期);全文 *
基于滑动窗迭代最大后验估计的多源组合导航因子图融合算法;徐昊玮;廉保旺;刘尚波;;兵工学报(第04期);全文 *

Also Published As

Publication number Publication date
CN116222583A (zh) 2023-06-06

Similar Documents

Publication Publication Date Title
Yang et al. A novel NLOS error compensation method based IMU for UWB indoor positioning system
CN114838732B (zh) 一种基于图优化的通信受限环境下协同导航方法
CN116222583B (zh) 一种基于滑动窗口的协同导航快速计算方法
CN102692223B (zh) 用于wsn/ins组合导航的多级非线性滤波器的控制方法
Xu et al. Spatial-temporal constrained particle filter for cooperative target tracking
Xu et al. Improving tightly-coupled model for indoor pedestrian navigation using foot-mounted IMU and UWB measurements
Cao et al. Improving positioning accuracy of UWB in complicated underground NLOS scenario using calibration, VBUKF, and WCA
Chu et al. Vehicle localization via cooperative channel mapping
WO2021248798A1 (zh) 路径跟踪方法、系统、装置及计算机可读存储介质
CN114111802A (zh) 一种行人航迹推算辅助uwb的定位方法
Ren et al. Improved extended kalman filter based on fuzzy adaptation for slam in underground tunnels
Cheng et al. UWB/INS fusion positioning algorithm based on generalized probability data association for indoor vehicle
CN112947481B (zh) 一种家庭服务机器人自主定位控制方法
Wang et al. FedINS2: A Federated-Edge-Learning-Based Inertial Navigation System With Segment Fusion
Uluskan et al. A geometrical closed form solution for RSS based far-field localization: Direction of Exponent Uncertainty
CN104535963A (zh) 基于距离测量的多移动节点实现协同目标定位的方法
Liu et al. Multi-robot cooperative localization with range-only measurement by UWB
Xue et al. A simulated fusion localization algorithm with adaptive error covariance matrix for closed corridor seamless positioning
Wilfinger et al. Indoor position determination using location fingerprinting and vehicle sensor data
CN115639585A (zh) 一种gps/imu和激光雷达的多传感器融合定位方法
Zhong et al. Collaborative navigation in V2I network based on Chan-Taylor joint iterative algorithm
CN116380130A (zh) 一种基于置信度传播的分布式协同导航定位精度评价方法
CN114485623A (zh) 一种聚焦距离的相机-imu-uwb融合精准定位方法
Lu et al. Ultra Wideband indoor positioning method based on Kalman filter and Taylor algorithm
Yue et al. Research on fusion and location method of indoor targeted spraying robots based on UWB\IMU

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant