[发明专利]一种基于匿名相对角度量测的多机器人互定位方法及系统

说明书

一种基于匿名相对角度量测的多机器人互定位方法及系统，涉及机器人定位技术领域，用以解决现有的基于相对量测的协同定位方法在机器人位姿的先验信息未知的情况下无法实现机器人准确定位的问题。本发明的技术要点包括：提出一种数据关联算法来获得相对角度量测的身份信息，具体包括：获取多个机器人的匿名量测信息；构建用于描述匿名量测信息与多个机器人之间对应关系的关联矩阵；根据多个机器人相对角度量测之间的几何约束对关联矩阵求解，以获得多个机器人的身份信息；然后使用拓展卡尔曼滤波器来融合里程计信息和相对角度量测，以估计系统状态即机器人之间的相对位姿信息。本发明适用于解决雷达、相机等匿名相对角度传感器的数据关联问题中。

技术领域

本发明涉及机器人定位技术领域，具体涉及一种基于匿名相对角度量测的多机器人互定位方法及系统。

背景技术

协同定位旨在估计一个多机器人系统的机器人在世界坐标系下的位姿。它在协作探索、监视和编队控制等许多协作任务中具有重要的作用。在世界坐标系中定位一组机器人，一种简单的方法是使用全球导航卫星系统(GNSS)或动作捕捉系统来获取机器人的位姿。但在某些环境下，如GNSS拒止环境，此类系统不可用。一些方法通过在系统中的机器人之间交换环境特征来估计姿势，但是，这些方法不能在无纹理环境中使用。还有一些方法利用机器人之间的相对测量来实现协同定位，该种方法不需要全局信息和环境特征。

现有的基于相对量测的协同定位方法几乎都假设相对测量包括被量测测机器人的身份，然而，在某些实际场景中，相对测量可能没有被测机器人的身份，这在机器人学文献中被称为未知数据关联或匿名测量。例如，相机无法区分具有相似视觉外观的同类机器人。假设机器人位姿的先验信息已知，数据关联问题可以很容易地得到解决，利用基于机器人姿态估计结果的最近邻(NN)算法和耦合概率数据关联过滤器(CPDAF)来解决数据关联问题。然而，当没有可用的机器人姿态先验信息时，基于贝叶斯的关联方法(包括NN和CPDAF)和基于最大似然的关联方法都无法工作。此外，存在一类利用不同机器人的测量之间的几何约束来解决匿名相对量测的方法(MRAB)，此类方法不依赖机器人的位姿的先验信息，但是它的计算量会随着机器人数量的增加而爆炸式增长。

发明内容

鉴于以上问题，本发明提出一种基于匿名相对角度量测的多机器人互定位方法及系统，用以解决现有的基于相对量测的协同定位方法在机器人位姿的先验信息未知的情况下无法实现机器人准确定位的问题。

根据本发明的一方面，提供一种基于匿名相对角度量测的多机器人互定位方法，该方法包括以下步骤：

步骤一、获取多个机器人的匿名量测信息；所述匿名量测信息包括每个机器人相对其他机器人的相对角度量测；

步骤二、构建用于描述匿名量测信息与多个机器人之间对应关系的关联矩阵；

步骤三、根据多个机器人相对角度量测之间的几何约束对关联矩阵求解，以获得多个机器人的身份信息；所述身份信息为每个机器人在其他机器人匿名量测信息中所对应的位置编号；

步骤四、根据身份信息和多机器人互定位系统模型方程获得每个机器人在世界坐标系下的位置和偏航角。

进一步地，多个机器人表示为R_i，i＝1，2，……，N，机器人R_i相对被量测机器人的多个相对角度量测表示为i_l表示被量测机器人的未知身份；步骤二中用关联矩阵ⁱ[A]∈{0，1，×}^(N-1)×N表示机器人R_i的N-1个相对角度量测与其他多个机器人的关系，其中，1表示该相对角度量测与某个机器人相关联，0表示该相对角度量测与某个机器人不关联，×表示关联关系未知。

进一步地，步骤三中求解获得多个机器人的身份信息的具体过程包括：