一种基于非线性优化的定位方法及系统与流程

本发明涉及系统定位的，尤其涉及一种基于非线性优化的定位方法及系统。

背景技术：

1、对于基本的定位系统，一般是由一个定位服务器、多个接收器以及标签组成的。标签固定在需要定位的对象上，接收器分散安装在场所各处，并在接收器和定位服务器直接建立有通信链路。

2、常见的高精度定位系统包括gps系统、uwb定位系统等，它们的特点是接收器部署密度比较低、一维测距精度不容易受环境因素影响。还有一种定位系统，它有比较高的接收器部署密度，但一维测距精度受环境影响比较大，典型的例子是沙盘上的仿真货车定位系统。

3、对于沙盘上的仿真货车定位系统，沙盘安装在教室固定位置上，沙盘的长宽一般不超过5米，离天花板高度不超过3米。沙盘上有一些道路、停车场、装卸货点等仿真场所，有几十辆仿真货车在上面行驶。在每个仿真货车上都安装了一个标签，在沙盘上方的教室天花板上安装了几十个接收器。这些接收器都通过通信链路，比如2.4g无线铜线、485或以太网等，连接到定位服务器上。

4、这种沙盘可以对所有仿真货车在沙盘上的二维平面或三维空间位置进行低成本的厘米级高精度定位，最成熟的技术是超声测距。具体来说，仿真货车上的标签采用电池供电，标签带有无线接收器和超声发射器。天花板上的接收器带有超声接收器。定位服务器带有一个无线发射器。定位服务器通过无线发射器，按照预定顺序、周期性的给每个标签发送定位命令；当一个标签通过无线接收器接收到定位命令后，就通过标签的超声发射器发射超声定位信号；这个超声定位信号将被多个安装在天花板上的接收器的超声接收器接收到，最后标签将超声定位信号处理后转发给定位服务器进行定位。如图1所示，为对沙盘中一个仿真货车进行定位的示意图，下面的球体代表仿真货车，上面的球体阵列代表接收器。如图2所示，为对沙盘中仿真货车进行定位的系统架构图。如图3所示，为对沙盘中仿真货车进行定位的系统工作示意图。

5、对于超声定位信号从仿真货车上的标签中发射出来到被一个接收器接收到的时间差，与该标签与接收器的距离成正比。空气中声波的传输速度为366米/秒。如果是3米距离，那么有8.196毫秒（即3/366）左右的时间差。超声定位信号一般都采用40khz，所以测距分辨率可以达到0.915厘米（即366/40k）。

6、现有技术中，在通过接收器获取到标签的超声定位信号之后，一般通过三角定位法对标签所在仿真货车的实际位置进行求解。三角定位法的原理为：

7、如图4所示，平面上有三个不共线的接收器a、b、c，和一个未知的对设置于仿真货车上的标签m，并已测出三个接收器到标签m的距离分别为d1，d2,d3，则以三个接收器的坐标为圆心，三个接收器到未知的标签距离为半径可以画出三个相交的圆，如图4所示，未知的标签的坐标即为三个圆的相交点。

8、在实际测量中，往往由于测量的误差，使三个圆并不交于一点，而相交于一块区域，如图5所示，在此种情况下需要采用最小二乘法进行求解。

9、对于有n个接收器的定位系统，建立方程，得到如下方程组：

10、，

11、其中，（x1,y1）...（xn,yn）分别为n个接收器的坐标值，d1...dn为n个接收器到某一个标签的距离。用上述的前n-1个方程一次减去第n个方程，可以得到如下的矩阵表示形式：ax=b，

12、其中：

13、，

14、对其采用最小二乘法求解，则是求ax=b二范数的最小值，即：

15、，

16、得：

17、求得的x便是未知的标签的坐标计算值。

18、三角定位法的优点是很自然就可以推广到三维定位（在距离公式中增加z轴坐标即可），而且可以不局限于只使用3个接收器或3个定位信号，接收器的个数n大于3的情况下，三角定位计算公式依然成立。但是三角定位法也存在几个问题：

19、（1）隐含假设所有测距值的误差分布都是正态分布，而且方差相同，但许多测距技术，比如超声、rssi等，测距误差的分布不一定是高斯，而且方差与距离成正相关。距离越远，方差越大。

20、（2）超声等测距技术，空气密度等对声速有明显影响。如果接收器的部署平面不与标签运动平面在同一个平面上，特别是接收器部署平面都在标签运动平面同一侧的话，那么这种影响造成的定位误差无法抵消，就特别明显。比如，当测距值大于实际值时，相当于标签位置都会远离接收器。另外，风速等对超声测距精度也有影响。

21、在沙盘定位中，设置有标签的仿真货车基本在平面上运行，略有高低起伏，而超声接收器都部署在仿真货车运动平面的同一侧（即天花板）。如果直接使用三角定位法，忽视测距误差分布与距离的相关性、声速不一定是366/秒等问题的话，定位误差就会增加。使用超声定位系统，业内一般只能做到10厘米左右的定位精度。

技术实现思路

1、针对上述问题，本发明的目的在于提供一种基于非线性优化的定位方法及系统,使用非线性最优化方法，通过动态估计环境中空气的实际声速，以及增加每次定位的测距数量，提高了整体系统定位精度。

2、本发明的上述发明目的是通过以下技术方案得以实现的：

3、一种基于非线性优化的定位方法，包括以下步骤：

4、s1：建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空；

5、s2：定位服务器尝试获取一个新的测距结果，其中，所述测距结果为任意一个标签向n个接收器发射定位后计算得到n个测距值的矢量集合；

6、s3：若当前所述测距结果中所有非0值对应的所述接收器，在x-y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。

7、进一步地，在步骤s1中，建立所述测距结果队列，具体为：

8、所述测距结果队列为小于等于k行，n列的矩阵，当所述测距结果队列的行数超过k行时，删除最早的所述测距结果；

9、其中，所述测距结果队列的行数k的取值根据定位的实际应用场景以及所述定位服务器的计算速度进行确定，所述测距结果队列的列数n为所述接收器的数量。

10、进一步地，在步骤s2中，所述定位服务器尝试获取一个新的所述测距结果，具体为：

11、所述定位服务器按照预定顺序，周期性的给每个所述标签发送定位命令，当前所述标签接收到所述定位命令之后，向n个所述接收器发射定位信号，n个所述接收器对所述定位信号进行处理后生成所述标签到所述接收器的测距值，转发给所述定位服务器，生成一条包含n个所述测距值的测距结果。

12、进一步地，在步骤s3中，预先建立包含所述声速校正因子和所述测距值的所述目标函数，具体为：

13、假设n个所述接收器的坐标为yn=(xn,yn,zn)，m个所述标签的坐标为ym=(xm,ym,zm)，其中所述接收器的坐标已知，所述标签的坐标未知，需要求解；

14、第k次的所述测距结果为dkn,=1..n，是一个所述标签发射定位信号后，被每个所述接收器接收到的所述测距值，所述声速校正因子为，为真实的所述测量值；

15、当有k个所述测距结果时，设定非线性最优化的目标函数为：

16、，

17、其中，xk可以存在相同编号的所述标签。

18、进一步地，在步骤s3中，采用所述非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子，具体为：

19、采用matlab中的fminsearch函数作为所述非线性最优化函数对所述目标函数求最优解；

20、在所述目标函数中，所述接收器的坐标yn和所述测距结果dkn为常量，所述声速校正因子和所述标签的坐标xk为需要被估计的变量；

21、采用所述fminsearch函数对所述目标函数寻找所述目标函数的最小值作为局部最优解，并获取最小值时对应的所述声速校正因子和所述标签的坐标xk。

22、进一步地，基于非线性优化的定位方法，还包括：为所述目标函数添加约束条件，假设所有的xk都相等以加快所述fminsearch函数的计算速度，以加快获取所述目标函数的所述局部最优解。

23、进一步地，基于非线性优化的定位方法，还包括：为所述目标函数添加包括预设时间内同一个所述多次的所述测距结果的最大速度和最大加速度约束、声速约束、所述测距结果的范围约束在内的约束条件；

24、在所述目标函数中，对超过所述约束条件的定位值增加惩罚值，以使所述声速校正因子和所述标签的坐标xk的估算值更合理。

25、一种用于执行如上述的基于非线性优化的定位方法的基于非线性优化的定位系统，包括：

26、结果队列建立模块，用于建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空；

27、测距结果获取模块，用于提供给定位服务器等待获取一个新的测距结果，其中，所述测距结果为任意一个标签向n个接收器发射定位后计算得到n个测距值的矢量集合；

28、最优解计算模块，用于若当前所述测距结果中所有非0值对应的所述接收器，在x-y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。

29、一种计算机设备，包括存储器和一个或多个处理器，所述存储器中存储有计算机代码，所述计算机代码被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如上述的方法。

30、一种计算机可读存储介质，所述计算机可读存储介质存储有计算机代码，当所述计算机代码被执行时，如上述的方法被执行。

31、与现有技术相比，本发明包括以下至少一种有益效果是：

32、（1）通过提供一种基于非线性优化的定位方法，包括：s1：建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空；s2：定位服务器尝试获取一个新的测距结果，其中，所述测距结果为任意一个标签向n个接收器发射定位后计算得到n个测距值的矢量集合；s3：若当前所述测距结果中所有非0值对应的所述接收器，在x-y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。上述技术方案，采用非线性最优化方法，通过动态估计环境中空气的实际声速，以及增加每次定位的测距数量，提高了整体定位系统的定位精度。

33、（2）采用非线性最优化方法，可以对超声误差分布进行更加准确的建模，可以对实际声速进行建模，可以对各种约束灵活建模，从而得到更准确的定位结果。