具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供的一种基于非线性几何约束优化的多目标室内定位方法，其核心思想是：利用多智能体所构成结构的特点，通过增加标签个数，将精确的先验冗余信息作为非线性约束引入定位算法，将定位问题抽象为带非线性约束的优化问题，进而通过求解该优化问题，以实现精度更高、鲁棒性更强的定位。

本发明提供的一种基于非线性几何约束优化的多目标室内定位方法，具体包括以下步骤：

步骤1、将单个标签的定位问题转化为优化问题。

假设给定有限个待定位标签，这些待定位标签在多智能体系统上组成一个固定形状的结构，其中每个标签p_i可以与位置固定的锚点a_j测距。标签之间不具备通讯及测距能力，但我们将多个标签间的几何形状作为先验信息，即假设标签p_i和p_l间的距离信息(i，l＝1，2...n)，i＝1，2，...，n为已知信息，使用如下公式表示：

本发明中，认为获得的是不存在误差的准确信息。除此以外，锚点坐标j＝1，2，...，m同样被认为是经过标定且准确的信息。主要的误差来源是锚点和标签之间的测距该距离信息是通过UWB信号双边双向飞行测距获得的，通常在视距条件下会产生10～20cm不等的误差。

由此，本发明中假设的已知信息分别为锚点坐标a_j、标签和锚点间的测距以及标签之间的先验距离信息锚点a_j和标签p_i之间的距离采用如下公式计算：

那么，任意标签与所有锚点之间的测距可得公式(3)：

将其展开并且化简得到公式(4)：

其中的取值如公式(5)所示：

其余取值依次类推。

此时，根据上述等式发现，定位问题可以描述为在不存在测距误差的情况下，寻找使上述等式成立的状态值然而实际工程中不可避免地会出现一定误差。因此，当仅存在一个待定位标签时，则可将上述问题表述为如下优化问题：

步骤2、在智能体上设置多个标签，用于描述智能体的几何约束，然后将多个标签的定位问题转换为带非线性几何约束的优化问题；

当多个标签在固定形状的约束下，能够通过增加标签间的非线性约束实现对多个标签的准确定位时，带非线性几何约束的优化问题可以被描述为如下形式：

其中i，l均表示标签的编号，取值范围为[1，n]；j＝1，...，m表示锚点的编号，∈为一个很小的常数，用于避免计算机产生的舍入误差导致的发散。可以看出该规划问题为目标函数和约束条件中都包含非线性函数的优化问题。

步骤3、利用最小二乘算法计算出几个标签的位置，并将该位置作为优化问题的初始值，从而降低优化算法的计算量，提升计算速度。

但是直接对该优化问题进行求解会导致很大的计算量，因此本发明考虑利用最小二乘法首先求解出一个粗略的定位结果作为初始值。具体方案如下：

对于标签p_i，根据其与锚点a_j的距离函数的展开及化简形式，将其写为矩阵形式为：

将上式可以简写为AP_i＝G_i，其中表示标签pi的欧几里得坐标；i＝1，2，…，n。该式的最小二乘形式表示为：

其最优解可表示为：

P_i＝(A^TA)^-1A^TG_i (10)

将分别求解出标签p₁，...，p_n的坐标i＝1，2，…，n作为优化算法的初始值，以降低优化算法的计算量，提高算法的整体性能。

步骤4、利用SQP算法求解步骤3形成的非线性几何约束的优化问题。

目前一种常用的思路是将目标函数与约束条件结合构造出增广的目标函数，进而将该优化问题转换为无约束的优化问题。SQP(序列二次规划)算法是目前被广泛认可的求解非线性约束优化问题的方案之一，其核心思想是将具有复杂非线性约束的优化问题转换为二次规划问题进行求解。其中二次规划问题指的是目标函数为二次函数，约束为线性函数的优化问题。上述优化问题可以简写为如下形式：

min f(X) (11)

s.t.g_u(X)≤0(u＝1，2，…，p)

h_v(X)＝0(v＝1，2，…，m)

将该非线性约束优化问题的目标函数在k步迭代计算值X^k处利用泰勒展开的方法化简为二次函数，并且将非线性约束函数以相同的方法化为线性函数。令S＝X-X^k，上述问题可以转化为：

B_eq＝[h₁(X^k)，h₂(X^k)，…，h_m(X^k)]^T

B＝[g₁(X^k)，g₂(X^k)，…，g_m(X^k)]^T

进一步地，可以将优化问题转换为二次规划问题的一般形式：

s.t.A^kS≤-B^k

对该二次规划问题进行求解，首先根据该二次规划问题的拉格朗日函数：

和多元函数的极值条件可得：

利用消元变换可以方便地求出该方程的唯一解，记作[S^k+1，λ^k+1]^T。根据KKT条件，若此解中的乘子向量不全为0，则S^k+1为二次规划问题的最优解。

将其最优解决作为原问题的下一个搜索方向S^k+1，并在该方向上进行原约束问题目标函数的约束一维搜索，这样就可以得到原约束问题的一个近似解X^k+1。反复这一过程，就可以得到原问题的最优解。

实施例1：

本实施例中，采用本发明提出的一种基于非线性几何约束优化的多目标室内定位方法，实现了对一组无人移动平台编队在智能停车场的定位，应用场景如图1所示。

通常在各个平台上分别设置UWB标签以确定该无人车位置。但是在实际定位过程中，由于测距过程中存在误差，可能会导致定位结果不准确。因此，将多智能体编队中的不同智能体间的距离信息加入定位算法作为冗余信息是一种可行解决方案。另外，依据标签间的准确距离和高度作为先验信息，可以获得更加准确的定位结果。该在本实施例中，具体公开了一种基于非线性几何约束优化的多目标室内定位方法。如图2所示，该定位方法包括：

首先设置给定的锚点位置为(单位：m)：

a₁(0，0，1)、a₂(10，0，1)、a₃(10，10，1)、a₄(0，10，2)

我们认为这些给定的锚点坐标的数值是真实值。假设四个无人移动平台构成拓扑的大小为0.5m×0.5m，将四个标签分别布置在各个无人车上，它们之间的距离为(单位：m)：

除此以外，无人平台的高度也可以提前获得，即标签的高度为(单位：m)：

h₁＝0.3、h₂＝0.3、h₃＝0.3、h₄＝0.3

这些精确的距离信息可以作为算法中的冗余信息，定位算法中可以通过加入这些信息提高定位算法的精确度。对于任意标签其和任意锚点之间的距离可以描述为：

一共有16对测距信息可以获得，将上述方程展开并化简，可以得到：

其中可以表示为：

由于存在误差的部分是UWB锚点和标签进行测距时引入的，即不是精确的距离信息，因此该定位问题可以描述为一个优化问题，寻找合适的标签位置坐标使得如下损失函数最小：

由于存在四个待定位标签，引入标签间的准确距离信息作为非线性约束，引入高度信息作为高度作为线性约束，可以将优化问题描述为如下形式：

过多的等式约束可能由于计算机产生的舍入误差可能导致优化问题发散，因此考虑将该优化问题的部分约束转换为不等式约束如下所示：

此时可以简写该问题并表示为：

min f(X)

s.t.g_u(X)≤0(u＝1，2，…，6)

h_v(X)＝0(v＝1，2，3，4)

在求解该优化问题前，首先对定位初始值进行求解。直接对该优化问题进行求解会导致很大计算量，因此本发明考虑利用最小二乘法首先求解出一个粗略的定位结果作为初始值。具体方案如下：

对于标签p₁、p₂、p₃、p₄，根据其和锚点a₁、a₂、a₃、a₄的距离函数i，j＝1，2，3，4的展开及化简形式，将其写为矩阵形式为：

将上式可以简写为AP_i＝G_i，其中表示标签p_i的欧几里得坐标；i＝1，2，3，4。该式的最小二乘形式可以分别表示为：

最优解可以表示为：

P_i＝(A^TA)^-1A^TG_i

分别求解出标签p₁，...，p₄的坐标i＝1，2，3，4作为优化算法的初始值，以降低优化算法的计算量，提高算法的整体性能。

综上，将该结果作为优化问题的初始值，即：

下面利用SQP算法求解上述优化问题，将该非线性约束优化问题的目标函数在X^k处利用泰勒展开化简为二次函数，并且将非线性约束函数以相同方法化为线性函数。令S＝X-X^k，上述问题可以转化为：

取

B_eq＝[h₁(X^k)，h₂(X^k)，…，h_m(X^k)]^T

B＝[g₁(X^k)，g₂(X^k)，…，g_m(X^k)]^T

进一步地，可以将优化问题转换为二次规划问题的一般形式，

s.t.A^kS≤-B^k

对该二次规划问题进行求解，首先根据该二次规划问题的拉格朗日函数

和多元函数的极值条件可得：

利用消元变换可以方便地求出该方程的唯一解，记作[S^k+1，λ^k+1]^T。根据K-T条件，若此解中的乘子向量不全为0，则S^k+1为二次规划问题的最优解S^*。

将其最优解决S^*作为原问题的下一个搜索方向S^k，并在该方向上进行原约束问题目标函数的约束一维搜索，这样就可以得到原约束问题的一个近似解X^k+1。反复这一过程，就可以得到原问题的最优解。

基于上述定位方法，以下验证利用基于非线性几何约束优化的多目标室内定位方法的精确程度，验证方法如下：

本实例中选择的待定位节点的真实坐标如下所示(单位：m)：

p_1，r(2，2，0.3)、p_2，r(2.5，2，0.3)、p_3，r(2.5，2.5，0.3)、p_4，r(2，2.5，0.3)

直接用标签真实位置的坐标与定位坐标之间的距离表示距离绝对误差E：

本数值实例进行了多组实验，通过在测距阶段设置标准差为20cm的误差，观察本发明提出方法在实际场景中的有效性。

图3是在测距误差的标准差为20cm的情况下采用常见的最小二乘算法与本实例提出方法的定位结果误差的对比。可以看出，在测距误差相同的情况下，本实例所提出的基于几何非线性约束的优化定位方法产生的定位误差小于采用最小二乘法产生的定位误差，说明本发明提出的方案总体性能优于最小二乘法的总体性能。

图4显示了本发明提出方法在测距误差分别为5-25cm情况下，最终定位误差与最小二乘算法在同样测距误差情况下的定位误差的对比。可以看出，随着测距误差的增加，最小二乘方法定位误差明显增加；而对于本方案提出的方法，虽然随着误差增大定位误差出现波动，但是总体而言依然小于最小二乘法产生的定位误差。说明本实例所采用的基于非线性几何约束优化的多目标室内定位方法具有更强的抗扰性和鲁棒性。

通过仿真可以说明，相比市面上常用的定位方案，本实施例提出的基于非线性几何约束优化的多目标室内定位方法可以在智能体的几何形状的约束下，通过增加标签数量，实现精度更高、鲁棒性更强的定位效果；同时由于多个标签构成的拓扑可以对智能体的形状进行描述，更有助于对智能体的精确控制以及在复杂环境下的决策。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。