一种基于谱图方法的多机器人互定位可观性判别方法及系统技术方案

一种基于谱图方法的多机器人互定位可观性判别方法及系统，涉及多机器人互定位技术领域，用以解决任意相对位置测量构型和运动约束条件下多机器人互定位系统的可观性判别问题。本发明专利技术的技术要点包括：首先为多机器人互定位系统定义了拓展相对位置测量图，包括相对位置测量配置和运动约束条件，进而给出了快速判别系统可观或不可观的充分条件；然后构造了基于拓展相对位置测量图的可观性判别矩阵，并基于该判别矩阵给出可观性判别的充要条件。本发明专利技术简化了判别基于相对位置测量的多机器人互定位可观性的逻辑与计算量。本发明专利技术可应用于判别相对位置测量的多机器人互定位可观性和多机器人互定位系统相对位置量测拓扑结构设计中。器人互定位系统相对位置量测拓扑结构设计中。器人互定位系统相对位置量测拓扑结构设计中。

【技术实现步骤摘要】
一种基于谱图方法的多机器人互定位可观性判别方法及系统


[0001]本专利技术涉及多机器人互定位
，具体涉及一种基于谱图方法的多机器人互定位可观性判别方法及系统。

技术介绍

[0002]多机器人进行互定位的目的是获得一组机器人在某一共同参考系下的位姿。对于许多多机器人协同任务，如协同监视、侦察、搜索、救援和编队控制等，多机器人互定位具有重要的作用。传统的定位方法，如基于GNSS和动作捕捉定位系统，能够获得各机器人在全局坐标系下的绝对位姿，进而求出机器人之间的相对位姿，但是这些定位方法的使用受到成本和应用环境的制约。基于地图匹配的方法需要应用环境的先验信息，在无纹理环境中性能表现较差。近年来，基于相机、激光雷达、UWB等相对量测传感器和编码器、罗盘、IMU等自运动量测传感器的互定位方法得到了广泛关注。
[0003]本质上，互定位问题是非线性状态估计问题。首先需要解决的是可观性判别问题，它保证了状态估计误差的有界性。然而，对于由n个机器人组成的多机器人系统，进行互定位的可观性分析是一个非常困难的问题。首先，n个机器人之间可能存在的n(n
‑
1)个相对测量，使得多机器人系统存在2
n(n
‑
1)
种可能的相对测量构型，增加了可观性分析的复杂性；其次，机器人的运动也会影响多机器人系统的可观性，现有的方法很难分析不同的相对测量构型和运动条件下的多机器人系统的可观性。
[0004]目前，多机器人互定位系统的可观性判别方法主要是基于Pairwise的方法，该方法将多机器人互定位的可观性问题视为一系列解耦的成对可观性问题。首先推导出两个机器人的可观性判别条件，然后通过假设系统中的相邻机器人是可观的，进而推出系统中的n个机器人都是可观的。然而，该方法只给出了可观性的充分条件，许多相对测量构型和运动约束条件下的多机器人系统的可观性无法通过Pairwise的方法判别。

技术实现思路

[0005]鉴于以上问题，本专利技术提出一种基于谱图方法的多机器人互定位可观性判别方法及系统，用以解决任意相对位置测量构型和运动约束条件下多机器人互定位系统的可观性判别问题。
[0006]根据本专利技术的一方面，提供一种基于谱图方法的多机器人互定位可观性判别方法，该方法包括以下步骤：
[0007]步骤一、每个机器人R
i
的状态为：
[0008][0009]其中，和分别表示机器人R
i
的机体坐标系相对于自运动坐标系f
i
的位置和偏航角；和分别表示机器人R
i
的自运动坐标系f
i
相对于公共参考坐标系f0的位置和偏航角；建立多机器人互定位系统的状态方程和量测方程；其中，机器人R
i
的状态方程为：
[0010][0011]其中，表示机器人R
i
的机体坐标系相对机器人R
i
的自运动坐标系f
i
的线速度；表示机器人R
i
的机体坐标系相对机器人R
i
的自运动坐标系f
i
的角速度；
[0012]相对位置的量测方程为：
[0013][0014]其中，表示机器人R
i
与机器人R
j
的相对位置；表示机器人R
i
的机体坐标系相对于公共参考坐标系f0的偏航角；表示旋转矩阵,表示旋转矩阵,表示机器人R
j
的机体坐标系相对于公共参考坐标系f0的位置；表示机器人R
i
的机体坐标系相对于公共参考坐标系f0的位置；
[0015]步骤二、根据状态方程和量测方程构建用于描述多机器人之间相对位置测量构型的拓展相对位置测量图，具体包括：
[0016]定义多机器人互定位系统的相对位置测量图表示由n个机器人节点组成的节点集，ε表示相对位置测量边集，相对位置测量边e
ij
∈ε表示机器人R
i
和机器人R
j
之间存在相对位置测量对其进行拓展，则拓展相对位置测量图为：
[0017][0018]其中，ε
E
＝ε
P
∪ε
M
是有向边集，ε
P
＝ε是相对位置测量边集，ε
M
表示由多条运动输入伴随边组成的运动输入伴随边集，是e
ij
的伴随边，当且仅当e
ij
满足运动约束时存在，且与e
ij
方向相反；是边权集，其元素与有向边集中的元素一一对应，表示各有向边的权重；
[0019]所述运动约束定义为：若两个机器人R
i
和R
j
之间存在相对位置测量即e
ij
∈ε,且满足则称机器人R
i
和R
j
存在运动约束；反之，若两个机器人R
j
和R
i
之间存在相对位置测量即e
ji
∈ε，且满足则称机器人R
j
和R
i
存在运动约束；
[0020]步骤三、根据拓展相对位置测量图的拓扑结构获得多机器人互定位系统是否可观的充分条件，以对系统可观性进行判别，从而获得判别结果。
[0021]进一步地，在步骤三之后还包括步骤四：若根据所述充分条件无法确定多机器人互定位系统是否可观，则基于拓展相对位置测量图构造可观性判别矩阵，从而获得系统是否可观的充要条件，以对系统可观性进行再次判别，获得判别结果。
[0022]进一步地，步骤二中边权集中相对位置测量边e
ij
∈ε
P
的权重为：
[0023][0024]其中：
[0025][0026]运动输入伴随边的权重为：
[0027][0028]其中，表示机器人R
j
的自运动坐标系f
j
相对于公共参考坐标系f0的偏航角；表示机器人R
j
的机体坐标系相对机器人R
i
的自运动坐标系f
i
的角速度。
[0029]进一步地，步骤三中判别系统是可观的充分条件为：如果是连通的且任意相邻的机器人节点R
i
和R
j
间至少存在2条方向相反的边，则称系统是局部弱可观的；判别系统是不可观的充分条件为：如果满足如下条件之一，则称系统是不可观的：是不连通的；中存在一个出度为0的节点，即存在一个对其他机器人没有相对位置测量的机器人。
[0030]进一步地，步骤四中可观性判别矩阵为：
[0031][0032]其中，I3表示3
×
3的单位矩阵，L3表示3
×
1的全1矩阵；表示的权值矩阵，是一个对角矩阵，对角线元素是ε
E
中对应边的权值对角阵；表示的关联矩阵，中的元素a
i本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于谱图方法的多机器人互定位可观性判别方法，其特征在于，包括以下步骤：步骤一、每个机器人R
i
的状态为：其中，和分别表示机器人R
i
的机体坐标系相对于自运动坐标系f
i
的位置和偏航角；和分别表示机器人R
i
的自运动坐标系f
i
相对于公共参考坐标系f0的位置和偏航角；建立多机器人互定位系统的状态方程和量测方程；其中，机器人R
i
的状态方程为：其中，表示机器人R
i
的机体坐标系相对机器人R
i
的自运动坐标系f
i
的线速度；表示机器人R
i
的机体坐标系相对机器人R
i
的自运动坐标系f
i
的角速度；相对位置的量测方程为：其中，表示机器人R
i
与机器人R
j
的相对位置；表示机器人R
i
的机体坐标系相对于公共参考坐标系f0的偏航角；表示旋转矩阵,表示旋转矩阵,表示机器人R
j
的机体坐标系相对于公共参考坐标系f0的位置；表示机器人R
i
的机体坐标系相对于公共参考坐标系f0的位置；步骤二、根据状态方程和量测方程构建用于描述多机器人之间相对位置测量构型的拓展相对位置测量图，具体包括：定义多机器人互定位系统的相对位置测量图表示由n个机器人节点组成的节点集，ε表示相对位置测量边集，相对位置测量边e
ij
∈ε表示机器人R
i
和机器人R
j
之间存在相对位置测量对其进行拓展，则拓展相对位置测量图为：其中，ε
E
＝ε
P
∪ε
M
是有向边集，ε
P
＝ε是相对位置测量边集，ε
M
表示由多条运动输入伴随边组成的运动输入伴随边集，是e
ij
的伴随边，当且仅当e
ij
满足运动约束时存
在，且与e
ij
方向相反；是边权集，其元素与有向边集中的元素一一对应，表示各有向边的权重；所述运动约束定义为：若两个机器人R
i
和R
j
之间存在相对位置测量即e
ij
∈ε,且满足则称机器人R
i
和R
j
存在运动约束；反之，若两个机器人R
j
和R
i
之间存在相对位置测量即e
ji
∈ε，且满足则称机器人R
j
和R
i
存在运动约束；步骤三、根据拓展相对位置测量图的拓扑结构获得多机器人互定位系统是否可观的充分条件，以对系统可观性进行判别，从而获得判别结果。2.根据权利要求1所述的一种基于谱图方法的多机器人互定位可观性判别方法，其特征在于，在步骤三之后还包括步骤四：若根据所述充分条件无法确定多机器人互定位系统是否可观，则基于拓展相对位置测量图构造可观性判别矩阵，从而获得系统是否可观的充要条件，以对系统可观性进行再次判别，获得判别结果。3.根据权利要求2所述的一种基于谱图方法的多机器人互定位可观性判别方法，其特征在于，步骤二中边权集中相对位置测量边e
ij
∈ε
P
的权重为：其中：运动输入伴随边的权重为：其中，表示机器人R
j
的自运动坐标系f
j
相对于公共参考坐标系f0的偏航角；表示机器人R
j
的机体坐标系相对机器人R
i
的自运动坐标系f
i
的角速度。4.根据权利要求3所述的一种基于谱图方法的多机器人互定位可观性判别方法，其特征在于，步骤三中判别系统是可观的充分条件为：如果是连通的且任意相邻的机器人节点R
i
和R
j
间至少存在2条方向相反的边，则称系统是局部弱可观的；判别系统是不可观的充分条件为：如果满足如下条件之一，则称系统是不可观的：是不连通的；中存在一个出度为0的节点，即存在一个对其他机器人没有相对位置测量的机器人。5.根据权利要求4所述的一种基于谱图方法的多机器人互定位可观性判别方法，其特征在于，步骤四中可观性判别矩阵为：其中，I3表示3
×
3的单位矩阵，L3表示3
×
1的全1矩阵；表示的权值矩阵，
是一个对角矩阵，对角线元素是ε
E
中对应边的权值对角阵；表示的关联矩阵，中的元素a
ij
定义如下：6.根据权利要求5所述的一种基于谱图方法的多机器人互定位可观性判别方法，其特征在于，步骤四中所述充要条件为：判断可观性判别矩阵的秩是否等于4(n
‑
1)，若等于则判...

【专利技术属性】
技术研发人员：郝宁，贺风华，田春耕，姚昊迪，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：