CN114003051B - 一种用于混合少星条件的递归协同定位方法 - Google Patents
一种用于混合少星条件的递归协同定位方法 Download PDFInfo
- Publication number
- CN114003051B CN114003051B CN202111196909.5A CN202111196909A CN114003051B CN 114003051 B CN114003051 B CN 114003051B CN 202111196909 A CN202111196909 A CN 202111196909A CN 114003051 B CN114003051 B CN 114003051B
- Authority
- CN
- China
- Prior art keywords
- members
- unmanned aerial
- collaborative
- positioning
- cooperative
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 230000004044 response Effects 0.000 claims abstract description 11
- 238000005259 measurement Methods 0.000 claims description 52
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 11
- 230000008447 perception Effects 0.000 claims description 6
- 239000013598 vector Substances 0.000 claims description 6
- 230000005540 biological transmission Effects 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000004891 communication Methods 0.000 claims description 4
- 239000000470 constituent Substances 0.000 claims description 2
- 238000012937 correction Methods 0.000 claims description 2
- 238000013500 data storage Methods 0.000 claims description 2
- 238000012216 screening Methods 0.000 claims description 2
- 125000004122 cyclic group Chemical group 0.000 claims 1
- 238000012163 sequencing technique Methods 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 10
- 238000004088 simulation Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000015572 biosynthetic process Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 230000008520 organization Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000036962 time dependent Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
- G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
- G05D1/0833—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability using limited authority control
-
- 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
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Computer Security & Cryptography (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种用于混合少星条件的递归协同定位方法,通过对周围所有同类型无人机,发送协同请求,接收其他无人机回应信号,进行信息提取;对能协同的无人机进行排序和选择,使无人机能够获得最优协同无人机群;考虑到无人机的少星对定位结果的影响,综合利用无人机与卫星之间距离和无人机与无人机之间距离来解算位置;无人机在获得位置之后对其他无人机进行广播自己位置与信息,使无人机集群里面可用信息增加,更多无人机获得位置,最后可能所有无人机均能得到定位。本发明可以使无人机在接收卫星信号不足的情况下,利用无人机之间的相互信息协同使之解算出精确的位置坐标,大大提高无人机集群的抗干扰定位能力与任务精确执行能力。
Description
技术领域
本发明涉及一种用于混合少星条件的递归协同定位方法,属于定位与导航技术领域。
背景技术
近年来,无人机集群协同技术日益受到国内外的关注。无人机集群具有工作范围大、可靠性高、可同时执行多重任务、整体效率高等优点,可用于灾害勘测和救援等领域。无人机集群技术,即多架无人机为适应任务要求而进行的某种队形排列和任务分配的组织模式,它既包括集群飞行的队形产生、保持和变化,也包括集群飞行任务的规划和组织,是未来无人机飞行技术发展的一个重要趋势。
传统无人机在接收卫星不足四颗或者在战场等恶劣环境中仅凭借惯导是无法长时间精确定位的,这对于在执行任务的无人机集群影响巨大,无法获得准确位置便不能准确完成任务。因此,在无人机接收卫星信号不足的情况下,提高无人机集群的抗干扰定位能力与任务精确执行能力,是亟需解决的问题。
发明内容
本发明所要解决的技术问题是:提供一种用于混合少星条件的递归协同定位方法,克服集群无人机中部分无人机在接收卫星不足四颗或者在战场等恶劣环境下无法定位问题,让集群无人机之间进行信息共享通过协同来获得各自位置,使其在恶劣环境也能得到良好的运用。
本发明为解决上述技术问题采用以下技术方案:
一种用于混合少星条件的递归协同定位方法,包括如下步骤:
步骤1,构建无人机集群协同导航定位系统,对集群中所有成员即无人机进行编号;
步骤2,集群中各成员记录集群定位所需的量测信息,包括各成员接收到的导航卫星个数、接收到的导航卫星的坐标以及各成员通过自带的卫星导航接收机模块测量与接收到的导航卫星之间的伪距;
步骤3,集群中各成员感知周围的成员,不满足独立定位条件的成员即待定位成员与其感知到的所有成员进行通信,发送协同请求信号;
步骤4,待定位成员接收其感知到的所有成员的回应信号,并进行混合少星条件下量测信息提取;所述回应信号包括感知到的成员的编号、感知到的成员接收导航卫星个数、感知到的成员接收导航卫星的坐标以及感知到的成员与其接收到的导航卫星之间的伪距;
步骤5,根据混合少星条件的几何约束,实时构建集群定位协同体;具体过程如下:
步骤5.1,对集群内所有成员进行分类,将接收到信号的导航卫星数大于等于4的成员归为集合UF,将接收到信号的导航卫星数等于3的成员归为集合U3,将接收到信号的导航卫星数等于2的成员归为集合U2,将接收到信号的导航卫星数等于1的成员归为集合U1,将无法接收到任何导航卫星信号的成员归为集合U0;
步骤5.2,按照U3、U2、U1、U0的顺序,优先选择前一集合中的成员加入集群定位协同体,当前一集合中的成员选择完后,再从后一集合中开始选择;且每次迭代增加一个成员进入集群定位协同体;且在各个集合中进行成员选择时,优先选择收到前一集合中最多成员回应信号的成员;
步骤5.3,在集合UF中筛选与当前协同体内成员能够进行相对测量的成员,将筛选出的成员加入协同体;
步骤5.4,协同体内各成员根据所接收到的协同请求信号,向协同体内对应成员共享自身位置信息并进行相对测量,构造间接感知通路;
步骤5.5,判断步骤5.2所构建集群定位协同体是否满足协同定位约束条件,若是则继续执行步骤6,否则返回步骤5.2;
步骤6,根据步骤5所构建集群定位协同体,进行递归协同定位;
步骤7,判断集群内所有成员是否均已实现定位,若是则返回步骤2进行下一时刻协同定位;否则返回步骤5.2继续进行协同定位。
本发明采用以上技术方案与现有技术相比,具有以下技术效果:
1、本发明的一种用于混合少星条件的递归协同定位方法,对周围所有同类型无人机发送协同请求,接收其他无人机回应信号,进行信息提取,使得无人机之间的信息能够共享利用;同时对能协同的无人机进行排序和选择,使得无人机能够获得最优协同无人机群,大大减小信息处理能力与缩短位置解算时间;考虑到无人机的少星对定位结果的影响,综合利用无人机与卫星之间距离和无人机与无人机之间距离来解算位置;同时无人机在获得位置之后对其他无人机进行广播自己位置与信息,使得无人机集群里面可用信息增加,越来越多无人机获得位置,最后可能所有无人机均能得到定位。和未采用混合少星的集群无人机协同定位算法相比,本发明能够解算出集群无人机的位置,适合实际应用。
2、本发明所使用的设备简单,价格低廉,能很好解算出各无人机位置,适合于大规模集群无人机的装备。
附图说明
图1是本发明一种用于混合少星条件的递归协同定位方法的原理流程示意图。
图2是本发明方法的无人机之间通信、感知、测距示意图。
图3是本发明方法的集群中的单个协同体示意图。
图4是采用本发明方法待定位无人机(设置为编号1)解算得到的经、纬、高误差曲线图。
图5是采用本发明方法无人机可见星数目随时间变化图。
图6是采用本发明方法待定位无人机可见卫星编号变化图。
图7是采用本发明方法待定位无人机与其他无人机协同编号变化图。
图8是采用本发明方法仿真第190s时待定位无人机与其他无人机协同仿真示意图。
具体实施方式
下面详细描述本发明的实施方式,所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的,仅用于解释本发明,而不能解释为对本发明的限制。
本发明通过对集群中无人机信息互相传递与共享,使得集群中可用信息增加,从而综合利用集群定位信息解算出各无人机位置。同时,解算出来位置后向其他无人机广播自己位置信息,让更多少星无人机能够定位。
如图1所示,为本发明提出的一种用于混合少星条件的递归协同定位方法的流程图,包括以下步骤:
步骤1,构建集群协同导航定位系统,对集群中所有成员进行编号;
设有n个成员组成成员,每架成员自带有数据传输模块、数据存储模块、计算模块和卫星导航接收机模块,同时在每个成员上还安装有相对感知模块、相对测量模块和通信模块,对集群中所有成员按照1,2,3,…,n的序号进行唯一的编号。
步骤2,集群中各成员记录集群定位所需的量测信息,包括各成员接收到的导航卫星个数、接收到的导航卫星的坐标以及各成员通过自带的卫星导航接收机模块测量与接收到的导航卫星之间的伪距。图2为无人机之间通信、感知、测距示意图。
步骤3,集群中各成员感知周围的成员,不满足独立定位条件的成员即待定位成员与其感知到的所有成员进行通信,发送协同请求信号;
集群中各成员通过相对感知模块感知周围的成员,建立各成员与其感知到的所有成员之间的间接感知通路,利用相对测量模块获得各成员与其感知到的所有成员之间的相对测量值,不满足独立定位条件的成员即接收到信号的导航卫星数小于4的成员与其感知到的所有成员进行通信,发送协同请求信号。
步骤4,待定位成员接收其感知到的所有成员的回应信号,并进行混合少星条件下量测信息提取;所述回应信号包括感知到的成员的编号、感知到的成员接收导航卫星个数、感知到的成员接收导航卫星的坐标以及感知到的成员与其接收到的导航卫星之间的伪距。
步骤5,根据混合少星条件的几何约束,实时构建集群定位协同体,包括以下子步骤:
步骤5.1,对集群内所有成员进行分类,将接收到信号的导航卫星数大于等于4的成员归为集合UF,将接收到信号的导航卫星数等于3的成员归为集合U3,将接收到信号的导航卫星数等于2的成员归为集合U2,将接收到信号的导航卫星数等于1的成员归为集合U1,将无法接收到任何导航卫星信号的成员归为集合U0;
步骤5.2,按照U3、U2、U1、U0的顺序,优先选择前一类中的成员加入协同体,当前一类中的成员选择完后,再从后一类中开始选择;且每次迭代增加一个成员进入协同体;且在各个集合中进行成员选择时,优先选择收到前一集合中更多成员回应信号的成员;图3为集群中单个协同体示意图。
步骤5.3,在集合UF中筛选与当前协同体内成员可进行相对测量的成员,将筛选出的成员加入协同体;
步骤5.4,协同体内各成员根据所接收到的协同请求信号,向协同体内对应成员共享自身位置信息并进行相对测量,构造间接感知通路;
步骤5.5,判断步骤5.2所构建集群定位协同体是否满足协同定位约束条件,若是则继续执行步骤6,否则返回步骤5.2;
步骤5.51,根据当前协同体中相对观测关系,建立递归协同定位特征矩阵M和方向余弦系数矩阵H,其表达式为:
H=diag[… hi p … hjk …]T
其中,表示编号为i的成员获得的编号为p的卫星伪距观测量,zjk表示编号为j的成员和编号为k的成员之间的相对距离观测量,xi、xj、xk分别为编号为i、j、k的成员k的坐标误差和钟差引起的等效距离误差状态量;
步骤5.52,根据步骤5.51得到的递归协同定位特征矩阵M和方向余弦系数矩阵H,判断是否达到协同要求,其表达式为:
其中,ei表示成员i的未知数个数。
步骤6,根据步骤5所构建协同体,进行递归协同定位,包括以下子步骤:
步骤6.1,建立步骤5所构建协同体内各成员的间接感知通路量测方程;
步骤6.1的具体过程如下:
步骤6.11,根据步骤5确定的协同体,利用协同体内各成员与其可接收到信号的卫星之间的伪距,建立协同体的伪距量测方程;
成员i自带的卫星导航接收机模块输出的对应某颗卫星p的测量伪距为其表达式为:
其中,为卫星p到成员i的距离真值,/>(xi,yi,zi)为成员i位置真值,δtui为成员i等效时钟误差相应的距离,/>为伪距测量误差;
由成员i机载惯导系统获得的成员i位置(xIi,yIi,zIi)对应与某颗卫星p的计算伪距为ρIip,其表达式为:
其中,(xsp,ysp,zsp)为卫星p的位置;
将计算伪距相对于成员i位置真值(xi,yi,zi)处展开成泰勒级数,取前两项然后减去测量伪距/>构成相对某颗卫星p的伪距测量差/>矢量方程,其表达式为:
则协同体中成员i相对某颗卫星p的伪距量测方程如下:
其中,
xi=[δxi δyi δzi δti]T
其中,i表示成员编号,p表示成员i可接收到信号的卫星的编号,为协同体中成员i相对某颗卫星p的观测量,/>为协同体中成员i相对某颗卫星p的观测方程,xi为成员i的坐标误差和钟差引起的等效距离误差(δxi,δyi,δzi,δti),/>表示成员i与卫星p之间伪距测量误差,/>为成员i相对某颗卫星p的伪距测量差;
步骤6.12,利用成员与其感知到的成员之间的距离测量值,建立协同体的距离量测方程;
成员j与成员k的测量距离为dMjk,其表达式为:
dMjk=Djk+δtuj-δtuk-vjk
其中,Djk为成员j与成员k的距离真值,(xj,yj,zj)为成员j位置真值,(xk,yk,zk)为成员k位置真值,δtuj和δtuk分别为成员j和k钟差,vjk为成员j和k之间距离测量误差;
由成员j机载惯导系统获得的成员j位置(xIj,yIj,zIj)与由成员k机载惯导系统获得的成员j位置(xIk,yIk,zIk)之间计算距离为dIjk,其表达式为:
将计算距离dIjk相对于成员位置真值处展开成泰勒级数,取前两项然后减去测量距离值dMjk,构成成员j与k的距离测量差δdjk矢量方程,其表达式为:δdjk=dIjk-dMjk=ejkxδxj+ejkyδyj+ejkzδzj-ejkxδxk-ejkyδyk-ejkzδzk+δtuj-δtuk+vjk
则协同体的距离量测方程如下:
zjk=hjk[I -I][xj xk]T+vjk
其中,
zjk=dIjk-dMjk
hjk=[ejkx ejky ejkz 1]
xj=[δxj δyj δzj δtuj]T,xk=[δxk δyk δzk δtuk]T
其中,j、k表示当前协同体中一对能够相互测距的成员编号,zjk为协同体中成员j与成员k的相对测距观测量,hjk为协同体中成员j与成员k的相对测距方向余弦系数矢量,xj、xk分别为成员j和成员k的坐标误差及钟差引起的等效距离误差状态量,vjk表示成员j与成员k之间距离测量噪声。
步骤6.2,根据步骤6.1所构建协同体内各成员的间接感知通路量测方程,构建协同体定位模型;
步骤6.2的具体过程如下:
根据步骤6.11得到的伪距量测方程和步骤6.12得到的距离量测方程,将两种量测方程整合成协同体定位模型,其表达式为:
Z=HMX+V
X=[… xi … xj … xk …]T
其中,协同体定位模型中zjk、/>hjk、xi、xj、xk、/>vjk的下标与上标分别代表成员编号i,j,k∈{1,2,…,n}和对应成员可接收到卫星编号p;/>表示编号为i的成员获得的编号为p的卫星伪距观测量,zjk表示编号为j的成员和编号为k的成员之间的相对距离观测量,xi、xj、xk分别为编号为i、j、k的成员k的坐标误差和钟差引起的等效距离误差状态量;/>表示编号为i的成员获得的编号为p的卫星伪距观测噪声,vjk表示编号为j的成员和编号为k的成员之间的相对距离观测噪声。
步骤6.3,迭代进行协同体中各成员协同定位,完成协同体定位初始化;
步骤6.31,根据步骤6.2得到的集群定位初始化模型,通过解算得到坐标的误差,其表达式为:
X=(MTHTHM)-1MTHTZ
根据X矩阵得到各坐标的误差,加上设定的相对应的初始值坐标得到解算坐标,其表达式为:
其中,(xIi,yIi,zIi)为随机设定或由惯性导航系统获得的成员i初始位置坐标值,(δxi,δyi,δzi)为成员i位置坐标误差值,为修正后的成员i位置坐标值;
步骤6.32,循环迭代若干次同样的解算过程,每次将上一次解算得到的位置坐标值作为这一次的成员位置坐标设定值重新解算得到新的成员位置坐标误差值;最后循环解算后得到协同体内所有成员的精确三维坐标。
步骤6.4,协同体内所有成员通过步骤6.3获得的精确三维坐标后,将协同体内的成员加入步骤5.1建立的集合UF中,并向其他成员共享自己的位置信息。
步骤7,判断集群内所有成员是否均已实现定位,即U3、U2、U1、U0是否均为空集,若是则返回步骤2进行下一时刻协同定位;否则返回步骤5.2继续进行协同定位。
为了验证本发明所提出的一种用于混合少星条件的递归协同定位方法的有效性,进行数字仿真分析。仿真中采用的无人机均设置一段飞行轨迹,在一段时间内不能完整接收到四颗卫星导航信号,无法通过接收机获取自己准确位置,但它们之间可以进行测距及相互感知、传递信息。
由图8可以看出,本发明方法可以从无人机集群众多成员中筛选出最佳协同体进行位置解算。图4为在采用本发明方法待定位无人机(设置为编号1)解算得到的经、纬、高误差曲线图,一共仿真210s,周期为1s,其中待定位无人机前面170s(竖直线处)观星数量为4颗及以上,其中0~94s观星条件良好,后面95~170为观星条件较差。170~210s无法观测到4颗卫星及以上,需要进行少星协同,通过本发明方法解算,通过图4可以看到,此算法能解出待定位无人机位置,误差符合实际任务需求。图5为采用本发明方法无人机可见星数目随时间变化图,图6为采用本发明方法待定位无人机可见卫星编号变化图,图7为采用本发明方法待定位无人机与其他无人机协同编号变化图。采用本发明方法进行集群协同定位,能够有效提高混合少星场景集群下的协同定位性能,具有良好的应用价值。
以上实施例仅为说明本发明的技术思想,不能以此限定本发明的保护范围,凡是按照本发明提出的技术思想,在技术方案基础上所做的任何改动,均落入本发明保护范围之内。
Claims (3)
1.一种用于混合少星条件的递归协同定位方法,其特征在于,包括如下步骤:
步骤1,构建无人机集群协同导航定位系统,对集群中所有成员即无人机进行编号;
步骤2,集群中各成员记录集群定位所需的量测信息,包括各成员接收到的导航卫星个数、接收到的导航卫星的坐标以及各成员通过自带的卫星导航接收机模块测量与接收到的导航卫星之间的伪距;
步骤3,集群中各成员感知周围的成员,不满足独立定位条件的成员即待定位成员与其感知到的所有成员进行通信,发送协同请求信号;
步骤4,待定位成员接收其感知到的所有成员的回应信号,并进行混合少星条件下量测信息提取;所述回应信号包括感知到的成员的编号、感知到的成员接收导航卫星个数、感知到的成员接收导航卫星的坐标以及感知到的成员与其接收到的导航卫星之间的伪距;
步骤5,根据混合少星条件的几何约束,实时构建集群定位协同体;具体过程如下:
步骤5.1,对集群内所有成员进行分类,将接收到信号的导航卫星数大于等于4的成员归为集合UF,将接收到信号的导航卫星数等于3的成员归为集合U3,将接收到信号的导航卫星数等于2的成员归为集合U2,将接收到信号的导航卫星数等于1的成员归为集合U1,将无法接收到任何导航卫星信号的成员归为集合U0;
步骤5.2,按照U3、U2、U1、U0的顺序,优先选择前一集合中的成员加入集群定位协同体,当前一集合中的成员选择完后,再从后一集合中开始选择;且每次迭代增加一个成员进入集群定位协同体;且在各个集合中进行成员选择时,优先选择收到前一集合中最多成员回应信号的成员;
步骤5.3,在集合UF中筛选与当前协同体内成员能够进行相对测量的成员,将筛选出的成员加入协同体;
步骤5.4,协同体内各成员根据所接收到的协同请求信号,向协同体内对应成员共享自身位置信息并进行相对测量,构造间接感知通路;
步骤5.5,判断步骤5.2所构建集群定位协同体是否满足协同定位约束条件,若是则继续执行步骤6,否则返回步骤5.2;
判断所构建集群定位协同体是否满足协同定位约束条件,具体过程如下:
步骤5.51,根据当前协同体中相对观测关系,建立递归协同定位特征矩阵M和方向余弦系数矩阵H,其表达式为:
其中,表示协同体中成员i相对导航卫星p的伪距观测量,zjk表示协同体中成员j与成员k的相对测距观测量,xi、xj、xk分别为成员i、j、k的坐标误差及等效时钟误差引起的等效距离误差状态量,I为4维单位矩阵,/>为协同体中成员i相对导航卫星p的观测方程,hjk为协同体中成员j与成员k的相对测距方向余弦系数矢量;
步骤5.52,根据步骤5.51得到的递归协同定位特征矩阵M和方向余弦系数矩阵H,判断是否达到协同要求,其表达式为:
其中,ei表示成员i的未知数个数;
步骤6,根据步骤5所构建集群定位协同体,进行递归协同定位;具体过程如下:
步骤6.1,建立步骤5所构建协同体内各成员的间接感知通路量测方程;具体过程如下:
步骤6.11,根据步骤5确定的集群定位协同体,利用协同体内各成员与其可接收到信号的导航卫星之间的伪距,建立协同体的伪距量测方程;
成员i自带的卫星导航接收机模块输出的对应某颗导航卫星p的测量伪距为其表达式为:
其中,ri p为导航卫星p到成员i的距离真值,(xi,yi,zi)为成员i位置真值,(xsp,ysp,zsp)为卫星p的位置真值,δtui为成员i等效时钟误差相应的距离,/>为成员i与导航卫星p之间伪距观测噪声;
由成员i机载惯导系统获得的成员i位置(xIi,yIi,zIi)与某颗导航卫星p的计算伪距为其表达式为:
其中,(xsp,ysp,zsp)为导航卫星p的位置;
将计算伪距相对于成员i位置真值(xi,yi,zi)处展开成泰勒级数,取前两项然后减去测量伪距/>构成相对导航卫星p的伪距测量差/>矢量方程,其表达式为:
其中,为导航卫星p相对于成员i的方向余弦,(δxi,δyi,δzi)为成员i位置坐标误差值;
则协同体中成员i相对导航卫星p的伪距量测方程如下:
其中,
xi=[δxi δyi δzi δti]T
其中,i表示成员编号,p表示成员i可接收到信号的导航卫星的编号,δti为成员i的等效时钟误差引起的等效距离误差,为成员i相对导航卫星p的伪距测量差;
步骤6.12,利用成员与其感知到的成员之间的距离测量值,建立协同体的距离量测方程;
成员j与成员k的测量距离为dMjk,其表达式为:
dMjk=Djk+δtuj-δtuk-vjk
其中,Djk为成员j与成员k的距离真值,(xj,yj,zj)为成员j位置真值,(xk,yk,zk)为成员k位置真值,δtuj和δtuk分别为成员j和k等效时钟误差,vjk为成员j和k之间距离测量误差;
由成员j机载惯导系统获得的成员j位置(xIj,yIj,zIj)与由成员k机载惯导系统获得的成员j位置(xIk,yIk,zIk)之间计算距离为dIjk,其表达式为:
将计算距离dIjk相对于成员位置真值处展开成泰勒级数,取前两项然后减去测量距离值dMjk,构成成员j与k的距离测量差δdjk矢量方程,其表达式为:
δdjk=dIjk-dMjk=ejkxδxj+ejkyδyj+ejkzδzj-ejkxδxk-ejkyδyk-ejkzδzk+δtuj-δtuk+vjk
其中,(ejkx,ejky,ejkz)为成员k相对于成员j的方向余弦,(δxj,δyj,δzj)为成员j位置坐标误差值,(δxk,δyk,δzk)为成员k位置坐标误差值;
则协同体的距离量测方程如下:
zjk=hjk[I -I][xj xk]T+vjk
其中,
zjk=dIjk-dMjk
hjk=[ejkx ejky ejkz 1]
xj=[δxj δyj δzj δtuj]T,xk=[δxk δyk δzk δtuk]T
其中,j、k表示当前协同体中一对能够相互测距的成员编号,vjk表示成员j与成员k之间的相对距离观测噪声;
步骤6.2,根据步骤6.1所构建协同体内各成员的间接感知通路量测方程,构建协同体定位模型;具体过程如下:
根据步骤6.11得到的伪距量测方程和步骤6.12得到的距离量测方程,将两种量测方程整合成协同体定位模型,其表达式为:
Z=HMX+V
X=[… xi … xj … xk …]T
其中,Z表示协同体定位联合观测量,i,j,k代表成员编号,i,j,k∈{1,2,…,n},n为成员数量,p代表可接收到的导航卫星编号;
步骤6.3,迭代进行协同体中各成员协同定位,完成协同体定位初始化;具体过程如下:
步骤6.31,根据步骤6.2得到的协同体定位模型,通过解算得到坐标的误差,其表达式为:
X=(MTHTHM)-1MTHTZ
其中,X为协同体定位联合误差状态量,Z表示协同体定位联合观测量;
根据X矩阵得到各坐标的误差,加上设定的相对应的初始值坐标得到解算坐标,其表达式为:
其中,(xIi,yIi,zIi)为随机设定或由惯性导航系统获得的成员i初始位置坐标值,(δxi,δyi,δzi)为成员i位置坐标误差值,为修正后的成员i位置坐标值;
步骤6.32,循环迭代若干次同样的解算过程,每次将上一次解算得到的位置坐标值作为这一次的成员位置坐标设定值重新解算得到新的成员位置坐标误差值;最后循环解算后得到协同体内所有成员的精确三维坐标;
步骤6.4,协同体内所有成员通过步骤6.3获得三维坐标后,将协同体内的成员加入步骤5.1建立的集合UF中,并向其他成员共享自己的位置信息;
步骤7,判断集群内所有成员是否均已实现定位,若是则返回步骤2进行下一时刻协同定位;否则返回步骤5.2继续进行协同定位。
2.根据权利要求1所述用于混合少星条件的递归协同定位方法,其特征在于,所述步骤1的具体过程如下:
设有n架无人机组成无人机集群,每架无人机自带有数据传输模块、数据存储模块、计算模块和卫星导航接收机模块,同时在每架无人机上还安装有相对感知模块、相对测量模块和通信模块,对集群中所有成员按照1,2,3,…,n的序号进行唯一的编号。
3.根据权利要求1所述用于混合少星条件的递归协同定位方法,其特征在于,所述步骤3的具体过程如下:
集群中各成员通过相对感知模块感知周围的成员,建立各成员与其感知到的所有成员之间的间接感知通路,利用相对测量模块获得各成员与其感知到的所有成员之间的相对测量值,不满足独立定位条件的成员即接收到的卫星个数小于4的成员与其感知到的所有成员进行通信,发送协同请求信号。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111196909.5A CN114003051B (zh) | 2021-10-14 | 2021-10-14 | 一种用于混合少星条件的递归协同定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111196909.5A CN114003051B (zh) | 2021-10-14 | 2021-10-14 | 一种用于混合少星条件的递归协同定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114003051A CN114003051A (zh) | 2022-02-01 |
CN114003051B true CN114003051B (zh) | 2023-10-31 |
Family
ID=79922805
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111196909.5A Active CN114003051B (zh) | 2021-10-14 | 2021-10-14 | 一种用于混合少星条件的递归协同定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114003051B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114567930A (zh) * | 2022-03-02 | 2022-05-31 | 湖州快飞智能科技有限公司 | 一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107918398A (zh) * | 2017-10-23 | 2018-04-17 | 南京航空航天大学 | 一种基于多重优化的集群无人机协同定位方法 |
CN111208544A (zh) * | 2020-03-04 | 2020-05-29 | 南京航空航天大学 | 一种用于无人机蜂群协同导航的完好性保护水平优化方法 |
CN112146650A (zh) * | 2020-09-18 | 2020-12-29 | 南京航空航天大学 | 一种用于无人蜂群协同导航的构型优化方法 |
CN113376664A (zh) * | 2021-05-25 | 2021-09-10 | 南京航空航天大学 | 一种无人蜂群协同导航多故障检测方法 |
-
2021
- 2021-10-14 CN CN202111196909.5A patent/CN114003051B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107918398A (zh) * | 2017-10-23 | 2018-04-17 | 南京航空航天大学 | 一种基于多重优化的集群无人机协同定位方法 |
CN111208544A (zh) * | 2020-03-04 | 2020-05-29 | 南京航空航天大学 | 一种用于无人机蜂群协同导航的完好性保护水平优化方法 |
CN112146650A (zh) * | 2020-09-18 | 2020-12-29 | 南京航空航天大学 | 一种用于无人蜂群协同导航的构型优化方法 |
CN113376664A (zh) * | 2021-05-25 | 2021-09-10 | 南京航空航天大学 | 一种无人蜂群协同导航多故障检测方法 |
Non-Patent Citations (2)
Title |
---|
基于增广伪距辅助的容错导航层次滤波方法;李传意 等;航空计算技术;第50卷(第2期);全文 * |
基于改进麻雀搜索算法的无人机集群置信传播协同定位方法;熊智 等;中国惯性技术学报;第29卷(第2期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114003051A (zh) | 2022-02-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107864007B (zh) | 面向区域目标的多星多地面站资源协同分配管理方法 | |
CN110244323B (zh) | 微、轻型无人机gnss反欺骗系统及欺骗信号检测与导航方法 | |
Zumberge et al. | Precise point positioning for the efficient and robust analysis of GPS data from large networks | |
CN105866812B (zh) | 一种车辆组合定位算法 | |
Bonnifait et al. | Cooperative localization with reliable confidence domains between vehicles sharing GNSS pseudoranges errors with no base station | |
CN105813194A (zh) | 基于指纹数据库二次校正的室内定位方法 | |
CN114003051B (zh) | 一种用于混合少星条件的递归协同定位方法 | |
CN112327340B (zh) | 终端定位精度评估方法、装置、设备以及介质 | |
CN112230247B (zh) | 一种用于城市复杂环境下gnss完好性监测方法 | |
CN107071893B (zh) | 一种蜂窝网络rtk定位方法及系统 | |
CN110662163A (zh) | 基于rss和aoa的三维无线传感网络协作定位方法 | |
CN108535746B (zh) | 一种探测gnss卫星轨道机动的方法 | |
CN114689015B (zh) | 一种提高光学卫星立体影像dsm高程精度的方法 | |
CN113791431B (zh) | 基于p2p技术构建的对等安全卫星高精度增强网络方法 | |
CN108253942B (zh) | 一种提高倾斜摄影测量空三质量的方法 | |
CN107607908A (zh) | 一种基于信号时延特征时空分布的定位方法 | |
Moratuwage et al. | A hierarchical approach to the multi-vehicle SLAM problem | |
Shen et al. | Multi-UAV cluster-based cooperative navigation with fault detection and exclusion capability | |
CN111722250B (zh) | 基于gnss时间序列的地壳形变影像共模误差提取方法 | |
Rohani et al. | Vehicular cooperative map matching | |
CN111025364A (zh) | 一种基于卫星辅助的机器视觉定位系统及方法 | |
CN113310489B (zh) | 一种基于互观测虚拟参考域的无人集群协同导航优化方法 | |
CN111856530B (zh) | 一种连续运行参考站系统的组合优化方法 | |
CN114167469A (zh) | 一种基于5g/gnss组合的车辆导航信息监测方法和装置 | |
CN111027646B (zh) | 一种基于多源基础信息的无人机点迹融合方法 |
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 |