一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统与流程

本发明涉及超声定位，尤其涉及一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统。

背景技术：

1、基本的定位系统由一个定位服务器、多个接收器、标签组成。标签固定在定位对象上，接收器分散安装在场所各处，并与定位服务器有通信链路，在定位系统中，标签常称为终端，接收器常称为基站。

2、利用超声等测距技术进行测距时，空气温度、密度等对声速有明显影响。如果基站的部署平面不与终端运动平面在同一个平面上，特别是基站部署平面都在终端运动平面的同一侧的话，那么这种影响造成的定位误差无法抵消。比如，当测距值大于实际值时，相当于定位推算得到的终端位置都会远离基站。在沙盘定位中，仿真对象（即终端）如果在平面上进行运动且略有高低起伏，而超声接收器（即基站）按现有的超声定位系统进行定位，存在定位误差大，精准度不够的技术问题。

技术实现思路

1、发明的目的是针对上述技术问题，提出一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统，本发明的目的可通过下列技术方案来实现：

2、本发明提供一种使用扩展卡尔曼滤波估计声速的超声定位方法，用于定位服务器通过多个基站定位终端，包括以下步骤：

3、在预设阶段，根据终端的空间位置坐标和预设声速校准因子得到表示终端初步定位的四维向量；

4、根据终端的空间位置坐标确认终端与基站的第一估计距离；

5、根据声速校准因子和第一估计距离确定第二估计距离并更新终端的四维雅可比矩阵；

6、定位服务器接收到各个基站发送的终端位置信息进行定位时，结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第一估计距离，后计算当前时刻的第二估计距离，再更新当前时刻的四维雅可比矩阵，利用更新的四维所述雅可比矩阵计算四维矩阵的卡尔曼增益，结合前一时刻的变化计算当前时刻的卡尔曼增益和观测噪声协方差矩阵，预测出当前时刻的所述终端精准定位。

7、较佳地“更新四维的所述雅可比矩阵”中更新雅可比矩阵具体为：

8、确认终端x的估计位置与基站i的第一估计距离；

9、，

10、其中，k为终端x的编号，i为基站的编号，是所述终端x在空间中的位置坐标，是基站i在空间中的位置坐标；

11、确定按照以下公式更新所述终端的雅可比矩阵，所述雅可比矩阵为m*4维的雅可比矩阵；

12、；其中，k为终端x的编号，i为基站的编号，是终端x在空间中的位置坐标；是基站i在空间中的位置坐标。

13、确定第二估计距离进一步包括：

14、确定终端x的估计位置与基站i的第二估计距离，，

15、其中，为终端x的估计位置与基站i的第一估计距离为预设声速校准因子。

16、较佳地，对协方差矩阵在内的信息进行状态初始化：当前时刻k=0时，初始化观测噪声协方差矩阵、确定过程噪声矩阵和测量噪声矩阵在内的常数矩阵，其具体为：

17、初始化观测噪声协方差矩阵，设置为4x4维的单位对角阵：，

18、其中；

19、确认过程噪声矩阵，设置为4x4维的常数矩阵，单位对角阵：；

20、初始化四维向量，设置为：；

21、确认测量噪声矩阵，设置为单位对角阵：，所述r为m*m维的常数矩阵。

22、较佳的，结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第一估计距离，后计算当前时刻的第二估计距离，更新当前时刻的四维的雅可比矩阵，进一步包括：

23、定位服务器接收到各个基站传送的当前时刻k中所述终端的空间坐标信息，并获得各个基站对应的每一基站空间坐标信息；计算、第二估计距离和雅可比矩阵。

24、结合前一时刻的变化更新当前时刻的卡尔曼增益和观测噪声协方差矩阵，预测出当前时刻的所述终端精准定位进一步包括：

25、计算当前时刻k的卡尔曼增益，，所述为上一时刻k-1时的观测噪声协方差矩阵，r为测量噪声矩阵，为上一时刻k-1时的雅可比矩阵；

26、计算时刻k的观测噪声协方差矩阵：，为观测噪声协方差矩阵的单位矩阵，为时刻k时的卡尔曼增益，为当前时刻k的雅可比矩阵，q为所述过程噪声矩阵；

27、计算当前时刻k终端精准估计位置，，为上一时刻k-1时终端x的精准估计位置，为时刻k的卡尔曼增益，为时刻k时的测距结果；为上一时刻k-1时的第二估计距离。

28、较佳的，还包括：k=k+1,重新计算下一个时刻的终端x的精准定位：进行结合前一时刻(k-1)的变化预测当前时刻k的终端x的位置信息和协方差矩阵，更新当前时刻k的卡尔曼增益, 得到当前时刻k的终端x的精准定位。

29、较佳的，定位服务器接收到各个所述基站传送当前时刻k中所述终端的空间坐标信息，并获得各个所述基站对应的每一所述基站空间坐标信息之后，还包括：定位服务器对基站共线的判断，如果基站共线，k=k+1，计算下一个时刻的终端x的精准定位。

30、本发明还提供了一种使用扩展卡尔曼滤波估计声速的超声定位系统，包括定位服务器、多个基站、至少一终端，进一步包括：

31、初定位预定单元：用于根据终端的空间位置坐标和预设声速校准因子得到表示终端初步定位的四维向量；

32、估计距离确定单元：根据终端的空间位置坐标确认终端与基站的第一估计距离；根据声速校准因子和第一估计距离确定第二估计距离；

33、观测信息确定单元，根据第二估计距离，更新观测信息，观测信息包括四维的雅可比矩阵；

34、定位计算单元：用于结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第一估计距离，计算当前时刻的第二估计距离，更新当前时刻的雅可比矩阵，利用更新四维的雅可比矩阵计算四维矩阵的卡尔曼增益，结合前一时刻的变化计算观测噪声协方差矩阵和当前时刻的终端精准定位。

35、较佳的，定位服务器还包括：接收单元，用于接收基站传送当前时刻终端的空间坐标信息和各个基站对应的每一基站空间坐标信息。

36、与现有技术相比，本发明存在以下至少一种技术效果：

37、与传统的定位系统相比，使用扩展卡尔曼滤波算法能够更准确的处理好非线性问题，通过引入声速校正因子作为状态变量，更新观测信息，包括更新m*4维的雅可比矩阵，再利用更新m*4维的雅可比矩阵计算四维矩阵的卡尔曼增益，根据第二估计距离、雅可比矩阵和卡尔曼增益，结合前一时刻的变化计算当前时刻观测噪声协方差矩阵，后结合第二估计距离计算当前时刻的所述终端精准定位信息，整个过程把声速校正完合整入扩展卡尔曼滤波估计超声定位的计算过程中，特别是终端在略有高低起伏的平面上运行，可以对实际环境中的声速进行在线估计，从而得到更准确的定位结果。与此同时本发明能够应用于各种场景，包括医疗，物流，工业等领域，具有广泛的应用前景。

技术特征：

1.一种使用扩展卡尔曼滤波估计声速的超声定位方法，用于定位服务器通过多个基站定位终端，其特征在于，包括以下步骤：

2.由权利要求1所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，更新四维的所述雅可比矩阵具体为：

3.由权利要求2所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于：确定所述第二估计距离进一步包括：

4.由权利要求3所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，还包括对协方差矩阵在内的信息进行状态初始化：当前时刻k=0时，初始化所述观测噪声协方差矩阵、确定过程噪声矩阵和测量噪声矩阵在内的常数矩阵，其具体为：

5.由权利要求4所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，结合当前时刻所述终端的空间位置坐标计算与各个所述基站的当前时刻的所述第一估计距离，后计算当前时刻的所述第二估计距离，更新当前时刻的四维的所述雅可比矩阵，进一步包括：

6.由权利要求1或5所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，利用更新四维的所述雅可比矩阵计算四维矩阵的所述卡尔曼增益，结合前一时刻的变化计算所述观测噪声协方差矩阵和当前时刻的所述终端精准定位进一步包括：

7.由权利要求6所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，还包括：k=k+1,重新计算下一个时刻的所述终端x的精准定位：进行结合前一时刻(k-1)的变化预测当前时刻k的所述终端x的位置信息和所述协方差矩阵，更新当前时刻k的卡尔曼增益, 得到当前时刻k的所述终端x的精准定位。

8.由权利要求6所述的使用扩展卡尔曼滤波估计声速的超声定位方法，所述定位服务器接收到各个所述基站传送当前时刻k中所述终端的空间坐标信息，并获得各个所述基站对应的每一所述基站空间坐标信息之后，还包括：

9.一种使用扩展卡尔曼滤波估计声速的超声定位系统，包括定位服务器、多个基站、至少一终端，其特征在于，所述定位服务器进一步包括：

10.由权利要求9所述的一种使用扩展卡尔曼滤波估计声速的超声定位系统，其特征在于，定位服务器还包括：

技术总结
本发明涉及超声定位技术领域，尤其涉及一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统，包括定位服务器、多个基站、至少一终端，定位服务器接收到各个基站发送的终端位置信息进行定位时，结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第二估计距离，更新当前时刻的四维的雅可比矩阵，计算卡尔曼增益，结合前一时刻的变化计算观测噪声协方差矩阵和当前时刻的终端精准定位。本发明将声速校正完合整入扩展卡尔曼滤波估计超声定位的计算过程中，可以对实际环境中的声速进行在线估计，从而得到更准确的定位结果。与此同时本发明能够应用于各种场景，包括医疗，物流，工业等领域，具有广泛的应用前景。

技术研发人员：董雁适,朱长华,张婧,杨雯
受保护的技术使用者：至控科技（湖州）创新研究院有限公司
技术研发日：
技术公布日：2024/1/13