[发明专利]一种适应于时变通信拓扑的多机器人分布式协作定位方法

权利要求书

1.一种适应于时变通信拓扑的多机器人分布式协作定位方法，其特征是：采用无味卡尔曼滤波UKF作为分布式协作定位的技术基础，利用一组sigma点来近似表示变量的概率密度分布，针对服从正态分布的变量，即用一些采样点来等价描述变量的估计值和该估计的方差，该过程称为U变换；此处，假设在k时刻，N个机器人的整体位置估计为：表示机器人R_i的位置估计，整体位置估计对应的方差为：

那么该估计的U变换为：

式中ξ⁽⁰⁾(k)Lξ^(4N)(k)即为sigma点，λ为可调整量；P(k)为对称矩阵，表示的第p列，T表示矩阵转置；

保障单机器人获得所有的块元素P_ij(k)以构建P(k)，以获得以使单机器人R_i获得局部sigma点：

基于向上看Cholesky分解来解决协方差矩阵的分布式获得问题，令：P＝AA^T，A中每一块元素A_ij可根据公式(2)(3)求解：

上式中：1)每一块元素A_ij的获取只依赖A_ij(i＞j)和P_ij(i＞j)，而非所有元素块，避免了所有机器人对信息交换的需求；2)由上至下递推而出，并将该特点与通信顺序结合；

利用向上看Cholesky矩阵分解块元素A_ij自上而下的递推特点，将分布式协方差矩阵分解与通信条件相结合，使矩阵分解从对通信“提需求”转变为自动顺应，从而避免分布式协作定位对通信拓扑条件的高度依赖问题；各机器人依靠获得的局部信息量，即可完成自身的位置更新；包括位置预测和位置后验估计两步，两步输出结果互为输入；

当机器人群内没有机器人测量到其它队友时，各个机器人按照一定的步长不断预测着自己的位置；

当一个机器人测量到队友，并测量到与队友的相对信息后，所有机器人的位置预测值都将被该测量更新，在完成后验估计更新后，继续进行位置预测；

在协作定位中单个机器人的工作流程分四步进行；针对安装有线速度测量设备、相对距离测量设备，无线通信设备的多个机器人，对编号为i的机器人，简写为R_i；具体实施步骤如下：

步骤一：开始协作定位任务；

任务开始后，机器人按照δ步长不断进行对自己位置预测；同时机器人实时监测队友，能否监测到与所处位置、视场角有关和侦听友邻发送的信息；

在某一时刻，若机器人发现队友，则执行步骤二，若机器人接收到定位信息，则执行步骤三；否则，继续预估自身位置，并继续对队友侦察和对外界信息的侦听，若接收到协作定位结束指令，则停止队友监测；

步骤二：发现队友，生成协作定位信息；

假设在k_m时刻，R_i发现队友，根据它与被发现者的相对测量值，若同时发现多个，则只处理与发现者最近的一个，计算生成定位信息S_i(k_m)，定位信息S_i(k_m)之后重新进入步骤一；

步骤三：接收到的队友信息，更新自身位置；

假设在k_r时刻，R_j接收到邻近机器人R_i信息，R_j首先判断该信息是否为协作定位信息，如果是则利用它进行协作定位；

在利用该协作定位信息前，先进行预处理：首先判断是否曾经接收过S_i(k_m)，若已接收过，则放弃，否则，继续利用公式(4)判断定位信息是否有效：

k_m+δ＞k_r (4)

其中k_r表示接收时刻；

若公式(4)不成立，则放弃，且不向邻近机器人传播，否则，R_j利用定位信息S_i(k_m)进行位置更新，之后重新进入步骤一；

步骤四：协作定位任务结束；

机器人在接收到协作定位终止命令后，机器人不再监测队友。