【技术实现步骤摘要】
本专利技术涉及机器人定位，具体涉及一种基于变换的多机器人一致性协同定位方法及系统。
技术介绍
1、对于多机器人系统而言，确定每个机器人在同一参考系的位置和方向，对于诸如监视和侦察、搜索和救援以及编队控制等各种任务至关重要。虽然全球定位系统(gps)为多机器人系统的定位提供了简单明了的解决方案，但在gps受限或gps干扰的环境(如水下、地下或室内环境)中不可靠。因此，许多研究已投入到协同定位(cl)中，在这种情况下，机器人通过利用来自本体传感器的自运动信息和来自外部传感器的相对机器人之间的测量值共同估计它们的姿态。
2、针对协同定位，国内外学者提出了许多方法，包括扩展卡尔曼滤波器(ekf)、粒子滤波器和基于优化的技术。在这些方法中，ekf由于其显著的效率和竞争性的准确性而成为最受欢迎的方法。然而，研究表明，ekf在多机器人协同定位的背景下应用时存在不一致性。如果估计器满足下面两个条件：估计误差应当具有零均值，并且其协方差应小于等于估计器计算的协方差，则状态估计器被称为一致的。在不一致的情况下，估计器计算的协方差往往过于自信，导致错误的信息增益，从而影响状态估计的准确性。更糟糕的是，状态估计的准确性未知，使得估计器不可靠。为了解决由于可观性错配导致的不一致性，国内外学者提出了许多方法，这些方法大致可以分为三类：基于可观性约束的滤波器、以机器人为中心的滤波器和基于矩阵李群的滤波器。基于可观性约束的滤波器明确修改估计器的线性化系统，以确保适当的可观性。在这些方法中，首次估计雅可比法通过调整线性化点保证了正确的可观性。尽管基于
3、后两类估计器则努力采用定制的状态表示，以确保估计器的线性化系统自动保持正确的可观性特性。以机器人为中心的方法使用以机器人为中心的坐标来构建状态向量，而基于矩阵李群的滤波器(如不变ekf)利用矩阵李群表示来构建状态和不确定性。这些方法在准确性和一致性方面表现出更好的性能，但设计缺乏一个典范表示，且需要深入理解其基础原理，使其在实践中变得复杂。
技术实现思路
1、鉴于以上问题，本专利技术提出一种基于变换的多机器人一致性协同定位方法及系统。
2、根据本专利技术的一方面，提出一种基于变换的多机器人一致性协同定位方法，该方法包括以下步骤：
3、获取多个机器人的自身运动信息和机器人之间的相对测量信息；
4、基于所述自身运动信息和相对测量信息建立线性化误差状态系统，所述线性化误差状态系统的不可观子空间保持时间不变，独立于线性化点；
5、基于所述线性化误差状态系统，设计具有可观一致性的变换扩展卡尔曼滤波器，并利用所述变换扩展卡尔曼滤波器进行多机器人协同定位。
6、进一步地，所述线性化误差状态系统建立如下：
7、
8、式中，表示线性化状态误差；表示线性化量测误差；分别表示状态、噪声、测量的雅可比矩阵值函数，表示k-1时刻方向角ψi，k的旋转矩阵，pi，k和ψi，k分别表示机器人i在全局参考系下k时刻时的位置和方向，i表示单位阵；nk表示所有机器人噪声列向量堆叠；wk表示所有机器人测量噪声列向量堆叠。
9、进一步地，所述线性化误差状态系统中所述测量的雅可比矩阵值函数为：
10、
11、式中，col表示矩阵进行列拼接；表示克罗内克积；
12、i2表示二阶单位阵。
13、进一步地，所述利用变换扩展卡尔曼滤波器进行多机器人协同定位包括：
14、对状态向量和误差协方差进行初始化并对下一时刻状态进行估计，包括：在每个时间步k，基于上一步获得的状态估计向量和最新控制输入向量计算获得当前时间步的状态预测向量误差协方差通过下式预测获得：
15、
16、式中，e表示期望；
17、将获得的状态预测向量和新的观测向量进行融合，采用变换拓展卡尔曼滤波器估计系统状态向量，获得多个机器人在全局参考系下的位置和偏航角，具体包括：
18、状态更新方程为：
19、
20、式中，δek表示卡尔曼修正量，表示待求卡尔曼增益，yk表示量测值，h表示量测方程，表示的逆，是估计的变换矩阵：
21、
22、误差协方差更新方程为：
23、
24、式中，是的估计值；rk表示量测噪声的协方差。
25、进一步地，所述待求卡尔曼增益的计算公式为：
26、
27、根据本专利技术的另一方面，提出一种基于变换的多机器人一致性协同定位系统，该系统包括：
28、信息获取模块，其配置成获取多个机器人的自身运动信息和机器人之间的相对测量信息；
29、系统方程建立模块，其配置成基于所述自身运动信息和相对测量信息建立线性化误差状态系统，所述线性化误差状态系统的不可观子空间保持时间不变，独立于线性化点；
30、协同定位模块，其配置成基于所述线性化误差状态系统，设计具有可观一致性的变换扩展卡尔曼滤波器，并利用所述变换扩展卡尔曼滤波器进行多机器人协同定位。
31、进一步地，所述系统方程建立模块中所述线性化误差状态系统建立如下：
32、
33、式中，表示线性化状态误差；表示线性化量测误差；分别表示状态、噪声、测量的雅可比矩阵值函数，表示k-1时刻方向角ψi，k的旋转矩阵，pi，k和ψi，k分别表示机器人i在全局参考系下k时刻时的位置和方向，i表示单位阵；nk表示所有机器人噪声列向量堆叠；wk表示所有机器人测量噪声列向量堆叠。
34、进一步地，所述线性化误差状态系统中所述测量的雅可比矩阵值函数为：
35、
36、式中，col表示矩阵进行列拼接；表示克罗内克积；i2表示二阶单位阵。
37、进一步地，所述协同定位模块中所述利用变换扩展卡尔曼滤波器进行多机器人协同定位包括：
38、对状态向量和误差协方差进行初始化并对下一时刻状态进行估计，包括：在每个时间步k，基于上一步获得的状态估计向量和最新控制输入向量计算获得当前时间步的状态预测向量误差协方差通过下式预测获得：
39、
40、式中，e表示期望；
41、将获得的状态预测向量和新的观测向量进行融合，采用变换拓展卡尔曼滤波器估计系统状态向量，获得多个机器人在全局参考系下的位置和偏航角，具体包括：
42、状态更新方程为：
43、
44、式中，δek表示卡尔曼修正量，表示待求卡尔曼增益，yk表示量测值，h表示量测方程，表示的逆，是估计的变换矩阵：
45、
46、误差协方差更新方程为：
47、
48、式中，是的估计值；rk表示量测噪声的协方差。
49、进一步地，所述本文档来自技高网...
【技术保护点】
1.一种基于变换的多机器人一致性协同定位方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于变换的多机器人一致性协同定位方法，其特征在于，所述线性化误差状态系统建立如下：
3.根据权利要求2所述的一种基于变换的多机器人一致性协同定位方法，其特征在于，所述线性化误差状态系统中所述测量的雅可比矩阵值函数为：
4.根据权利要求3所述的一种基于变换的多机器人一致性协同定位方法，其特征在于，所述利用变换扩展卡尔曼滤波器进行多机器人协同定位包括：
5.根据权利要求4所述的一种基于变换的多机器人一致性协同定位方法，其特征在于，所述待求卡尔曼增益的计算公式为：
6.一种基于变换的多机器人一致性协同定位系统，其特征在于，包括：
7.根据权利要求6所述的一种基于变换的多机器人一致性协同定位系统，其特征在于，所述系统方程建立模块中所述线性化误差状态系统建立如下：
8.根据权利要求7所述的一种基于变换的多机器人一致性协同定位系统，其特征在于，所述线性化误差状态系统中所述测量的雅可比矩阵值函数为：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。