CN113084801A - 基于半正定规划优化的多机器人初始位姿相对定位方法 - Google Patents

Abstract

本发明公开了一种基于半正定规划优化的多机器人初始位姿相对定位方法，包括步骤：获取目标机器人的航位推测信息、邻接机器人的航位推测信息以及目标机器人和邻接机器人之间的量测距离；构建二次约束二次规划问题，通过优化松弛技术获得目标机器人的半正定优化方程；根据半正定优化方程，确定目标机器人的目标变量；在最大似然估计框架下，更新目标变量得到更新的目标变量，直至更新的目标变量满足终止条件时，得到估计协方差。将多机器人初始位姿相对定位问题建模成半正定规划问题，不需要根据设计特定的求解工具对优化问题进行求解，直接利用现有工具即可，可以广泛地移植到多个应用平台下，可移植性强。

Description

基于半正定规划优化的多机器人初始位姿相对定位方法

技术领域

本发明涉及多机器人定位技术领域，尤其涉及的是一种基于半正定规划优化的多机器人初始位姿相对定位方法。

背景技术

多机器人协同可以广泛应用于编组控制、合作运输、周边监视和态势感知等多个领域，完成单个机器人无法实现的任务。其中，多机器人之间能够相互准确定位，即多机器人系统中任意一部机器人实时确定出其它机器人的相对于自身的位姿(包括位置和方位)，是上述系统正常工作的前提。

现有技术中，现有多机器人相互定位方式主要包括依赖外部设备(如运动捕获系统)辅助定位、利用全球定位设备全局定位以及根据无线传感器协同定位几种。然而，通过建立和求解以几何关系构建的带约束加权最小二乘优化问题，可以获得最大似然意义下最优的位姿估计结果。然而，由于求解的目标函数是强非线性的函数，因此需要使用特殊的优化求解工具，且容易获得不一致的估计结果，存在可移植性差的问题。

因此，现有技术还有待于改进和发展。

发明内容

本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种基于半正定规划优化的多机器人初始位姿相对定位方法，旨在解决现有技术中多机器人相互定位存在可移植性差的问题。

本发明解决技术问题所采用的技术方案如下：

一种基于半正定规划优化的多机器人初始位姿相对定位方法，其中，包括步骤：

获取目标机器人的航位推测信息、邻接机器人的航位推测信息以及所述目标机器人和所述邻接机器人之间的量测距离；其中，所述邻接机器人为所述目标机器人的邻域内的机器人；

根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，构建二次约束二次规划问题，通过优化松弛技术获得所述目标机器人的半正定优化方程；

根据所述半正定优化方程，确定所述目标机器人的目标变量；

在最大似然估计框架下，更新所述目标变量得到更新的目标变量，直至所述更新的目标变量满足终止条件时，得到估计协方差。

所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其中，所述根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，构建二次约束二次规划问题，通过优化松弛技术获得所述目标机器人的半正定优化方程，包括：

根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，确定所述目标机器人的线性矩阵和协方差矩阵；

根据所述线性矩阵和所述协方差矩阵，确定所述目标机器人的目标矩阵；

根据所述目标矩阵，构建所述目标机器人的半正定优化方程。

所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其中，所述根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，确定所述目标机器人的线性矩阵和协方差矩阵，包括：

根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，确定所述目标机器人的线性矩阵；

根据量测噪声协方差矩阵和所述量测距离，确定所述目标机器人的协方差矩阵。

所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其中，所述根据所述半正定优化方程，确定所述目标机器人的目标变量，包括：

根据所述半正定优化方程，确定所述半正定优化方程的优化结果；

根据所述优化结果，确定目标变量。

所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其中，所述根据所述优化结果，确定目标变量，包括：

对所述优化结果进行奇异值分解，确定最大奇异值；

根据所述最大奇异值，得到目标变量。

所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其中，所述对所述优化结果进行奇异值分解，确定最大奇异值，包括：

对所述优化结果进行奇异值分解，得到奇异值；

根据所述奇异值，确定最大奇异值。

所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其中，所述在最大似然估计框架下，更新所述目标变量得到更新的目标变量，直至所述更新的目标变量满足终止条件时，根据所述目标变量，得到估计协方差，包括：

在最大似然估计框架下，将所述目标变量作为目标值的泰勒展开的初始值得到近似值；

根据所述目标值和所述近似值，更新所述目标变量得到更新的目标变量；

当所述更新的目标变量不满足终止条件时，将所述更新的目标变量作为目标变量，并继续执行将所述目标变量作为目标值的泰勒展开的初始值得到近似值的步骤；

当所述更新的目标变量满足终止条件时，得到估计协方差。

所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其中，所述目标变量包括：所述目标机器人的偏向变量、所述目标机器人的方位变量以及所述目标机器人的航位推测变量，所述邻接机器人的航位推测变量；

所述目标机器人的偏向变量根据所述目标机器人的偏向角确定；

所述目标机器人的方位变量根据所述目标机器人的方位角确定。

所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其中，所述终止条件包括：

更新次数大于预设更新阈值，和/或

所述更新的目标变量与所述目标变量的差值小于或等于门限。

一种基于半正定规划优化的多机器人初始位姿相对定位系统，包括存储器和处理器，所述存储器存储有计算机程序，其中，所述处理器执行所述计算机程序时实现上述任一项所述的基于半正定规划优化的多机器人初始位姿相对定位方法的步骤。

有益效果：本发明将多机器人初始位姿相对定位问题建模成一个半正定规划问题，根据机器人的航位推测信息和量测距离，构建机器人的半正定优化方程，并对该方程进行求解，得到优化结果。由于求解半正定优化方程时，不需要根据设计特定的求解工具对优化问题进行求解，直接利用现有工具即可，可以广泛地移植到多个应用平台下，可移植性强。

附图说明

图1是本发明在二维场景下利用航位推测信息和测距量测确定初始位姿的几何示意图。

图2是本发明中基于半正定规划优化的多机器人初始位姿相对定位方法的第一流程示意图。

图3是本发明在实验仿真条件下与基于几何关系和基于最大似然算法在弱噪声干扰下，偏向角和方位角的估计精度对比图。

图4是本发明在实验仿真条件下与基于几何关系和基于最大似然算法在强噪声干扰下，偏向角和方位角精度对比图。

图5是本发明算法在真实实验环境下与基于几何关系和基于最大似然算法，偏向角和方位角精度对比图。

图6是本发明中基于半正定规划优化的多机器人初始位姿相对定位方法的第二流程示意图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

请同时参阅图1-图6，本发明提供了一种基于半正定规划优化的多机器人初始位姿相对定位方法的一些实施例。

如图1-图2以及图6所示，本发明的一种基于半正定规划优化的多机器人初始位姿相对定位方法，包括以下步骤：

步骤S100、获取目标机器人的航位推测信息、邻接机器人的航位推测信息以及所述目标机器人和所述邻接机器人之间的量测距离；其中，所述邻接机器人为所述目标机器人的邻域内的机器人。

具体地，多个机器人在二维平面上随机移动，以多个机器人中任意一个机器人作为目标机器人，例如，机器人的数量为N个，目标机器人为

i＝1,2,3,…,N，该目标机器人的领域内的机器人的集合为

则目标机器人的邻接机器人为

且

目标机器人的领域可以根据目标机器人的通信距离d_com确定，例如，以目标机器人为圆心，以通信距离d_com为半径的圆为目标机器人的邻域。当然，目标机器人的邻域内至少有一个邻接机器人。

在得到机器人的航位推测信息时，将N个机器人安放在监测环境的任意位置处，根据实际定位需求设定每部机器人航位推测系统和测距传感器的采样周期均为T。每个机器人均沿任意方向进行移动，并以周期T采集和记录航位推测信息，例如，目标机器人的航位推测信息为q_i＝[q_i,1,…,q_i,l,…,q_i,N]，目标机器人的第l个航位推测信息为q_i,l＝[x_i,ly_i,l]^T，x_i,l表示目标机器人在第l个航位推测下的x轴坐标，y_i,l表示目标机器人在第l个航位推测下的y轴坐标，T表示转置，邻接机器人的航位推测信息为q_j＝[q_j,1,…,q_j,l,…,q_j,N]，邻接机器人的第l个航位推测信息为q_j,l＝[x_j,l y_j,l]^T，x_j,l表示目标机器人在第l个航位推测下的x轴坐标，y_j,l表示目标机器人在第l个航位推测下的y轴坐标，目标机器人

和邻接机器人

在第l个航位推测时的量测距离为d_l，其中，l＝1,2,3,…,L，L表示航位推测数量，可以根据航位推测系统确定航位推测数量，例如，L＝5。需要说明的是，如图1所示，根据目标机器人航位推测信息和邻接机器人的航位推测信息的不同，量测距离也不相同。d₀表示目标机器人

和邻接机器人

在初始位置时的量测距离。量测距离可以根据机器人上的测距传感器得到。每个机器人都有自身的坐标系，

表示目标机器人的坐标系，

表示邻接机器人的坐标系，机器人自身的坐标系是以机器人正面朝向和侧面方向建立的直角坐标系。

步骤S200、根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，构建二次约束二次规划问题，通过优化松弛技术获得所述目标机器人的半正定优化方程。

具体地，本发明将多机器人初始位姿相对定位问题建模成一个半正定规划问题，根据机器人的航位推测信息和量测距离，构建机器人的半正定优化方程，并对该方程进行求解，得到优化结果。由于求解半正定优化方程时，不需要根据设计特定的求解工具对优化问题进行求解，直接利用现有工具即可，可以广泛地移植到多个应用平台下，可移植性强。

具体地，步骤S200包括：

步骤S210、根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，确定所述目标机器人的线性矩阵和协方差矩阵。

具体地，根据目标机器人的航位推测信息、邻接机器人的航位推测信息以及量测距离，确定目标机器人的线性矩阵和协方差矩阵，以便根据线性矩阵和协方差矩阵确定目标机器人的目标矩阵。

具体地，步骤S210包括：

步骤S211、根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，确定所述目标机器人的线性矩阵。

具体地，根据每一个航位推测，根据目标机器人在该航位推测下的航位推测信息q_i,l，以及邻接机器人在该航位推测下的航位推测信息q_j,l，确定χ_l，并根据q_i,l、q_j,l、该航位推测下的量测距离d_l以及初始位置时的量测距离d₀，确定ε_l；然后根据d₀、q_i,l、χ_l、q_j,l以及ε_l确定线性矩阵A。

具体地，线性矩阵为：

其中，χ_l＝[-2(x_i,lx_j,l+y_i,ly_j,l) 2(x_i,ly_j,l-y_i,lx_j,l)]^T，

A表示线性矩阵，χ_l表示第l个第一中间变量，ε_l表示第l个第二中间变量。

步骤S212、根据量测噪声协方差矩阵和所述量测距离，确定所述目标机器人的协方差矩阵。

具体地，在本发明中考虑到噪声的存在引入量测噪声协方差，从而可以在强噪声环境下进行准确定位，本发明方法的抗噪声干扰能力强。具体地，量测噪声协方差矩阵为：

其中，R表示测量噪声协方差矩阵，

表示第l个航位推测下的量测噪声。

根据量测噪声协方差矩阵和所述量测距离，确定协方差矩阵，具体地，协方差矩阵为：

其中，

R′表示协方差矩阵，

表示近似，Σ表示中间矩阵，Σ_ll表示中间矩阵Σ中ll位置(即第l行第l列)处的元素，R_ll表示测量噪声协方差矩阵R中ll位置(即第l行第l列)处的元素。

由上可知，协方差矩阵为：

步骤S220、根据所述线性矩阵和所述协方差矩阵，确定所述目标机器人的目标矩阵。

步骤S230、根据所述目标矩阵，构建所述目标机器人的半正定优化方程。

具体地，根据线性矩阵A和协方差矩阵R′，确定目标矩阵，具体地，目标矩阵可表示为M₀以及M₁，M₂，…，M_l，…，M_L，b，其中，M₀＝A^TR′A，M_l＝sparse(h,k,s,m,n)，符号sparse(h,k,s,m,n)借用了matlab的标记方式，h和k分别表示了元素的行号和列号，s表示该元素的真实值，m和n表示该矩阵的大小，b＝[b₁ … b_l … b_L]^T。以L＝5为例进行说明，M₁，M₂，…，M_l，…，M_L，b具体地如下：

M₁＝sparse([1 2],[1 2],[1 1],6,6) M₂＝sparse([3 4],[3 4],[1 1],6,6)

M₃＝sparse([5 6],[5 6],[1 1],6,6) M₄＝sparse([1 2 6],[3 4 5],[1 1 -1],6,6)

M₅＝sparse([1 2 6],[4 3 6],[1 1 -1],6,6) b＝[b₁ … b₅]^T＝[1 1 1 0 0]

可知，M₁，M₂，…，M_l，…，M_L，b根据M₀确定。

采用最小平方距离误差准则和松弛理论，根据目标矩阵构建半正定优化方程，具体地，半正定优化方程为：

s.t.Tr(M_lX)＝b_l,l＝1,...,L

其中，X＝xx^T，argmin(·)表示函数取最小值时变量值，Tr(·)表示矩阵的求迹操作，X表示优化结果，x表示目标变量，

表示半正定。

步骤S300、根据所述半正定优化方程，确定所述目标机器人的目标变量。

具体地，在确定半正定优化方程，求解半正定优化方程，得到优化结果，根据优化结果得到目标变量。

具体地，步骤S300包括：

步骤S310、根据所述半正定优化方程，确定所述半正定优化方程的优化结果。

具体地，利用优化工具，如CVX、SeDuMi或者SDPT3求解下面的优化问题，获得半正定优化方程的优化结果X。

步骤S320、根据所述优化结果，确定目标变量。

具体地，根据优化结果X，确定目标变量，所述目标变量包括：所述目标机器人的偏向变量、所述目标机器人的方位变量以及所述目标机器人的航位推测变量，所述邻接机器人的航位推测变量；所述目标机器人的偏向变量根据所述目标机器人的偏向角确定；所述目标机器人的方位变量根据所述目标机器人的方位角确定。具体地，偏向变量为x_s，方位变量为x_r，目标机器人的航位推测变量为q₁，邻接机器人的航位推测变量为q₂，其中，偏向变量x_s根据所述目标机器人的偏向角θ确定，方位变量x_r根据所述目标机器人的方位角φ确定。如图1所示，方位角φ为目标机器人的航位推测信息与目标机器人正面朝向的坐标轴之间的夹角，偏向角θ为邻接机器人与目标机器人正面朝向的坐标轴之间的夹角。

具体地，步骤S320包括：

步骤S321、对所述优化结果进行奇异值分解，确定最大奇异值。

具体地，对优化结果X进行奇异值分解，确定最大奇异值。

具体地，步骤S321包括：

步骤S3211、对所述优化结果进行奇异值分解，得到奇异值。

步骤S3212、根据所述奇异值，确定最大奇异值。

对优化结果X进行奇异值分解可以得到优化结果为：

其中，Σ表示求和符号，i表示序号，r表示奇异值的数量，σ_i表示优化结果X作分解之后获得的第i个奇异值，u_i表示第i个奇异值对应的左奇异值向量，v_i表示第i个奇异值对应的右奇异值向量。

从σ_i中确定最大值σ_max，从u_i中确定最大值u_max，从v_i中确定最大值v_max，根据最大奇异值σ_max，u_max以及v_max，确定估计优化结果

具体地，估计优化结果为：

其中，

表示估计优化结果，σ_max表示优化结果X作分解之后获得的最大奇异值，u_max表示最大奇异值对应的左奇异值向量，v_max表示最大奇异值对应的右奇异值向量。

步骤S322、根据所述最大奇异值，得到目标变量。

具体地，根据最大奇异值对应的左奇异值向量和大奇异值对应的右奇异值向量，得到目标变量。目标变量为：

步骤S400、在最大似然估计框架下，更新所述目标变量得到更新的目标变量，直至所述更新的目标变量满足终止条件时，得到估计协方差。

具体地，目标变量并不能准确体现目标机器人的位姿，因此需要对目标变量进行更新，得到更新的目标变量，当更新的目标变量满足终止条件时，则根据更新的目标变量，确定估计协方差；当更新的目标变量不满足终止条件时，则需要继续执行更新目标变量得到更新的目标变量的步骤，直至更新的目标变量满足终止条件，然后根据更新的目标变量，确定估计协方差。通过不断迭代更新，使更新的目标变量能准确体现目标机器人的位姿。

所述终止条件包括：更新次数大于预设更新阈值，和/或所述更新的目标变量与所述目标变量的差值小于或等于门限。预设更新阈值N_max可以根据需要设定，门限可以根据需要设定，由于目标变量有：偏向变量为x_s，方位变量为x_r，目标机器人的航位推测变量为q₁，邻接机器人的航位推测变量为q₂，门限有Tsh_s,Tsh_r,

其中，Tsh_s用于判断更新的偏向变量

与偏向变量

的差值大小，Tsh_r用于判断更新的方位变量

与方位变量

的差值大小，

用于判断更新的目标机器人的航位推测变量

与目标机器人的航位推测变量

的差值大小，

用于判断更新的邻接机器人的航位推测变量

与邻接机器人的航位推测变量

的差值大小，其中，ε表示更新次数。

具体地，步骤S400包括：

步骤S410、在最大似然估计框架下，将所述目标变量作为目标值的泰勒展开的初始值得到近似值。

具体地，对目标值z进行泰勒展开，并将目标变量作为目标值的泰勒展开的初始值得到近似值

具体地，目标值的泰勒展开如下：

其中，x_s＝[cosθ sinθ]^T,x_r＝[cosφ sinφ]^T，

其中，z表示目标值，h_d表示量测对应的非线性方程，v表示量测噪声，

表示偏向变量x_s求导下的雅克比矩阵，

表示方位变量x_r求导下的雅克比矩阵，

表示目标机器人的航位推测变量q₁求导下的雅克比矩阵，

表示邻接机器人的航位推测变量为q₂求导下的雅克比矩阵，u表示第一引入的变量，

表示矩阵参数，C表示第二引入的变量，w_l表示在l时刻，目标机器人与邻接机器人之间的相对位置向量。

步骤S420、根据所述目标值和所述近似值，更新所述目标变量得到更新的目标变量。

具体地，根据目标值z和近似值

对目标变量进行更新，得到更新的目标变量。在更新时，可以对

以及

进行更新，还可以对

以及

进行更新。

具体地，根据目标值z和近似值

确定更新值，更新值有：

以及

其中，

其中，H表示雅克比矩阵，

表示偏向变量更新值，

表示方位变量更新值。

具体地，根据目标值z和近似值

确定更新值，更新值有：

以及

其中，

其中，H表示雅克比矩阵，

表示偏向变量更新值，

表示方位变量更新值，

表示目标机器人的航位推测变量更新值，

表示邻接机器人的航位推测变量更新值。

由于在不同更新次数下，得到近似值

不相同，那么得到的更新值也就不同，因此，在第ε次更新时，更新值有：

在更新时，有：

其中，

表示在第ε+1次更新时的偏向变量，

表示在第ε次更新时的偏向变量，

表示在第ε次更新时的偏向变量更新值，

表示在第ε+1次更新时的方位变量，

表示在第ε次更新时的方位变量，

表示在第ε次更新时的方位变量更新值，

表示在第ε+1次更新时的目标机器人的航位推测变量，

表示在第ε次更新时的目标机器人的航位推测变量，

表示在第ε次更新时的目标机器人的航位推测变量更新值，

表示在第ε+1次更新时的邻接机器人的航位推测变量，

表示在第ε次更新时的邻接机器人的航位推测变量，

表示在第ε次更新时的邻接机器人的航位推测变量更新值。

在第ε次更新时，对

进行更新得到

对

进行更新得到

对

进行更新得到

对

进行更新得到

步骤S430、当所述更新的目标变量不满足终止条件时，将所述更新的目标变量作为目标变量，并继续执行将所述目标变量作为目标值的泰勒展开的初始值得到近似值的步骤。

具体地，更新的目标变量不满足终止条件，也就是说，更新的目标变量与目标变量的差值大于门限，和/或更新次数小于或等于预设更新阈值，则将更新的目标变量作为目标变量，返回到步骤S410。

步骤S440、当所述更新的目标变量满足终止条件时，得到估计协方差。

具体地，所述更新的目标变量满足终止条件，也就是说，更新次数ε大于预设更新阈值N_max，和/或更新的目标变量与目标变量的差值小于或等于门限，则得到估计协方差。具体地，终止条件可以表示为：

和/或

ε＞N_max。

估计协方差为：

其中，P_ini表示估计协方差，E表示取期望操作。

也就是说，在更新的目标变量满足终止条件后，确定估计协方差。

图3为本发明方法在实验仿真条件下与基于几何关系和基于最大似然方法在弱噪声干扰下，偏向角和方位角的估计精度对比图；其中，航位推测信息σ_p和测距量测的噪声误差σ_l设定在[10^-8,10^-4]m的范围内。图3分别展示了偏向角θ和方位角φ与不同的σ_p和σ_l相结合的情况。由于估计误差较小，这里使用的是对数误差值。从图3中可以看出，基于几何关系方法在噪声处于[10^-8,2·10^-6]m的范围内时，具有更好的估计精度，这主要是由于基于最大似然和基于半正定松弛的方法存在近似和松弛的过程，在理论上并不是绝对最优。但是，当噪声处于2·10^-6m以上时，可以看到后两种方法的估计精度远远高于基于几何关系的方法。并且容易发现，基于几何关系方法的估计精度呈指数型衰减，这比线性衰减趋势的另两种方法严重得多。此外，基于半正定松弛的方法和基于最大似然方法的估计方法获得的参数估计精度基本一致。

图4为本发明方法在实验仿真条件下与基于几何关系和基于最大似然方法在强噪声干扰下，偏向角和方位角精度的对比图；这里的噪声设置在[10^-4,10^-1]m范围内。与图3相同，同样将偏向角θ和方位角φ与不同的σ_p和σ_l相结合。从图4中可以看到，尽管噪声误差设置到了比较高的量级，例如10^-1m，基于半正定松弛方法仍具有较高的估计精度，最大只有0.15弧度的误差。相比而言，在强噪声环境下，基于几何关系方法在估计精度上表现不佳，这进一步支持了本发明方法具有鲁棒性好的这一优点。

图5为本发明方法在真实实验环境下与基于几何关系和基于最大似然方法，偏向角和方位角精度对比图；实验数据包括航位推测信息和测距量测。其中，航位推测信息主要通过IMU传感器，轮式编码器获取得到，测距量测则通过UWB模块DWM1000获取得到。其中，UWB模块具有30m范围的通信能力，测距数据在STM32F103控制模块下进行处理，并且UWB和航位推测数据采样速率设置为1赫兹。获得采样数据后，数据在Intel核i7，3.6GHz处理器电脑下利用matlab2013a进行处理。

实验环境设置如下：两部机器人在一个4×6m的矩形空间内，以0.1m/s的速度作直线运动。通过运动捕获平台(具有16个摄像头，测距误差小于0.05mm，角度误差小于0.01°)，可以获取每部机器人真实的运动位置(真实偏向角)和方位角。

如表1所示，前两行对应实验一，后两行对应实验二。针对实验一，首先固定方位角φ＝45.23°，随后改变12次机器人的偏向角，每次设置的偏向角大小可以从表1中找到；针对实验二，首先固定偏向角θ＝44.99°，同样进行12次实验，分别对应不同的方位角，如表1的第4行所示。在此之后，利用基于几何关系、基于最大似然和基于半正定松弛几种方法对θ和φ进行估计。如图5所示，由于实测数据中航位推测信息和测距量测的噪声较大，基于几何关系的方法无法利用实测数据获取正确的方位角和偏向角估计值。相反，基于最大似然和基于半正定松弛方法具有较小的估计误差，进一步验证了本方法鲁棒性强的优点。

表1 方位角和偏向角

本发明采用一种基于半正定规划的多机器人初始位姿相对定位方法，首先根据实际利用航位推测信息和距离量测生成相互定位需要的关键参数和关键矩阵，并根据最小平方距离误差准则和松弛理论构建半正定优化方程，然后通过SeDuMi工具求解和奇异值分解、奇异值分解等手段获得初始位姿的估计值，之后通过泰勒展开获得的结果对初始位姿的估计值进行更新直至获得最大似然估计结果，最后求取相应的估计协方差用于后续的持续定位，有效地解决了多机器人初始位姿相对定位问题；并且具备以下优点：

(1)本发明的方法可以广泛应用于非结构环境下的环境。由于该方法不依赖于任何锚节点和环境先验信息，因此适用于大量未知和变化的环境，可以广泛地应用到不同场景中，包括灾难救援、野外作战以及太空探索等；

(2)本发明的方法操作简单，只需要进行简单的参数配置和矩阵生成工作，通过现有工具箱SeDuMi等即可实现初始位姿的精确求解；

(3)本发明的方法抗噪声干扰能力强。不同于基于几何关系方法，该方法在设计之初就考虑了噪声的存在，从理论上可以获得最小误差的估计结果，因此可以在强噪声环境下进行准确定位，在定位精度上与现有最先进方法一致。

本发明的方法可移植性强且鲁棒性强。具体地，本方法将多机器人初始位姿相对定位问题建模成一个标准的半正定规划问题，不需要根据设计特定的求解工具对优化问题进行求解，直接利用现有工具即可，可以广泛地移植到多个应用平台下，并且标准化的工具能够获得更加准确和稳定的求解结果，因此还具有鲁棒性强的优点。

基于上述实施例的基于半正定规划优化的多机器人初始位姿相对定位方法，本发明还提供了一种基于半正定规划优化的多机器人初始位姿相对定位系统的实施例。

本发明中基于半正定规划优化的多机器人初始位姿相对定位系统包括：包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如下步骤：

获取目标机器人的航位推测信息、邻接机器人的航位推测信息以及所述目标机器人和所述邻接机器人之间的量测距离；其中，所述邻接机器人为所述目标机器人的邻域内的机器人；

根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，构建所述目标机器人的半正定优化方程；

根据所述半正定优化方程，确定所述目标机器人的目标变量；

更新所述目标变量得到更新的目标变量，直至所述更新的目标变量满足终止条件时，根据所述目标变量，得到估计协方差。

应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (10)

1.一种基于半正定规划优化的多机器人初始位姿相对定位方法，其特征在于，包括步骤：

获取目标机器人的航位推测信息、邻接机器人的航位推测信息以及所述目标机器人和所述邻接机器人之间的量测距离；其中，所述邻接机器人为所述目标机器人的邻域内的机器人；

根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，构建二次约束二次规划问题，通过优化松弛技术获得所述目标机器人的半正定优化方程；

根据所述半正定优化方程，确定所述目标机器人的目标变量；

在最大似然估计框架下，更新所述目标变量得到更新的目标变量，直至所述更新的目标变量满足终止条件时，得到估计协方差。

2.根据权利要求1所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其特征在于，所述根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，构建二次约束二次规划问题，通过优化松弛技术获得所述目标机器人的半正定优化方程，包括：

根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，确定所述目标机器人的线性矩阵和协方差矩阵；

根据所述线性矩阵和所述协方差矩阵，确定所述目标机器人的目标矩阵；

根据所述目标矩阵，构建所述目标机器人的半正定优化方程。

3.根据权利要求2所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其特征在于，所述根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，确定所述目标机器人的线性矩阵和协方差矩阵，包括：

根据所述目标机器人的航位推测信息、所述邻接机器人的航位推测信息以及所述量测距离，确定所述目标机器人的线性矩阵；

根据量测噪声协方差矩阵和所述量测距离，确定所述目标机器人的协方差矩阵。

4.根据权利要求1所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其特征在于，所述根据所述半正定优化方程，确定所述目标机器人的目标变量，包括：

根据所述半正定优化方程，确定所述半正定优化方程的优化结果；

根据所述优化结果，确定目标变量。

5.根据权利要求4所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其特征在于，所述根据所述优化结果，确定目标变量，包括：

对所述优化结果进行奇异值分解，确定最大奇异值；

根据所述最大奇异值，得到目标变量。

6.根据权利要求5所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其特征在于，所述对所述优化结果进行奇异值分解，确定最大奇异值，包括：

对所述优化结果进行奇异值分解，得到奇异值；

根据所述奇异值，确定最大奇异值。

7.根据权利要求1所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其特征在于，所述在最大似然估计框架下，更新所述目标变量得到更新的目标变量，直至所述更新的目标变量满足终止条件时，得到估计协方差，包括：

在最大似然估计框架下，将所述目标变量作为目标值的泰勒展开的初始值得到近似值；

根据所述目标值和所述近似值，更新所述目标变量得到更新的目标变量；

当所述更新的目标变量不满足终止条件时，将所述更新的目标变量作为目标变量，并继续执行将所述目标变量作为目标值的泰勒展开的初始值得到近似值的步骤；

当所述更新的目标变量满足终止条件时，得到估计协方差。

8.根据权利要求7所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其特征在于，所述目标变量包括：所述目标机器人的偏向变量、所述目标机器人的方位变量以及所述目标机器人的航位推测变量，所述邻接机器人的航位推测变量；

所述目标机器人的偏向变量根据所述目标机器人的偏向角确定；

所述目标机器人的方位变量根据所述目标机器人的方位角确定。

9.根据权利要求1所述的基于半正定规划优化的多机器人初始位姿相对定位方法，其特征在于，所述终止条件包括：

更新次数大于预设更新阈值，和/或

所述更新的目标变量与所述目标变量的差值小于或等于门限。

10.一种基于半正定规划优化的多机器人初始位姿相对定位系统，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至9中任一项所述的基于半正定规划优化的多机器人初始位姿相对定位方法的步骤。

Images