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

权利要求书

1.一种基于匿名相对角度量测的多机器人互定位方法，其特征在于，包括以下步骤：

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

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

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

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

2.根据权利要求1所述的一种基于匿名相对角度量测的多机器人互定位方法，其特征在于，多个机器人表示为R_i，i＝1,2,……,N，机器人R_i相对被量测机器人的多个相对角度量测表示为i_l表示被量测机器人的未知身份；步骤二中用关联矩阵ⁱ[A]∈{0，1，×}^(N-1)×N表示机器人R_i的N-1个相对角度量测与其他多个机器人的关系，其中，1表示该相对角度量测与某个机器人相关联，0表示该相对角度量测与某个机器人不关联，×表示关联关系未知。

3.根据权利要求2所述的一种基于匿名相对角度量测的多机器人互定位方法，其特征在于，步骤三中求解获得多个机器人的身份信息的具体过程包括：

步骤三一、对于任意的三个机器人，从每个机器人中选择2个相对角度量测，检验选择的6个相对角度量测是否满足约束公式，如果满足则得到两个局部解矩阵，每个局部解矩阵中表示该相对角度量测与某个机器人相关联的位置赋值为1，其他位置赋值为0或×；所述约束公式为：

其中，γ表示与量测噪声有关的阈值；Ang表示两个向量之间的夹角；β_iia和β_iib表示来自机器人R_i的相对角度量测；表示来自机器人R_j的相对角度量测；表示来自机器人R_k的相对角度量测；从而获得多个局部解，以构成局部解集；

步骤三二、对于局部解集中的任意两个局部解判断其元素的一致性，若不一致，则将局部解集进行分裂以得到两个子集，每个子集包含两个局部解中的一个；继续判断这两个子集中是否仍然包含不一致的局部解，如果任一个子集含有不一致的局部解，那么这个子集继续分裂，直到子集中所有的局部解是一致的；这些局部解一致的子集称为一致集；

步骤三三、根据一致集的度确定正确的一致集；

步骤三四、合并正确的一致集中的所有局部解，获得关联矩阵的全局解，从而获得多个机器人的身份信息。

4.根据权利要求3所述的一种基于匿名相对角度量测的多机器人互定位方法，其特征在于，步骤三二中对于局部解集中的任意两个局部解判断其元素的一致性的原则为：对于任意两个局部解矩阵A_p和A_q，如果A_p和A_q存在对应元素一个为1，另一个为0，则说明A_p和A_q所表示的量测身份信息矛盾，即A_p和A_q不一致；如果A_p和A_q不存在这种矛盾的对应元素，则A_p和A_q一致。

5.根据权利要求4所述的一种基于匿名相对角度量测的多机器人互定位方法，其特征在于，步骤三三中一致集的度计算公式为：

式中，C表示一致集；sgn表示符号函数；tr表示迹；选择具有最大度的一致集确定为正确的一致集。