CN114594421A - 一种基于最小二乘法与卡尔曼滤波器的移动靶点位置计算方法 - Google Patents

Abstract

本发明涉及一种基于最小二乘法与卡尔曼滤波器的移动靶点位置计算方法，包括以下步骤：1、根据确实数据前面与后面数据根据数据的线性关系补全数据；2、时间序列数据中剔除有误差的数据并利用上一步补全该数据；3、设出目标点的坐标，根据靶点到锚点的距离列出等式方程组，通过最小二乘法计算得到目标点坐标，利用卡尔曼滤波器进行靶点的矫正，从而得到精确的靶点坐标；4、用平滑的曲线按时间顺序连接每一时刻的精确靶点坐标得到空间中靶点的运动轨迹。本发明的目的是建立数学模型，利用干扰检测后得到的数据，利用最小二乘法求得靶点坐标，再利用卡尔曼滤波器优化算法精确靶点的坐标，提高算法的精确度。

Description

一种基于最小二乘法与卡尔曼滤波器的移动靶点位置计算 方法

技术领域

本发明涉及室内空间定位领域，具体的讲是一种基于基于最小二乘法和卡尔曼滤波器的计算方法。

背景技术

UWB(Ultra-Wideband)技术也被称之为“超宽带”，又称之为脉冲无线电技术。这是一种无需任何载波，通过发送纳秒级脉冲而完成数据传输的短距离范围内无线通信技术，并且信号传输过程中的功耗仅仅有几十μW。UWB因其独有的特点，使其在军事、物联网等各个领域都有着广阔的应用。其中，基于UWB的定位技术具备实时的室内外精确跟踪能力，定位精度高，可达到厘米级甚至毫米级定位。UWB在室内精确的定位将会对卫星导航起到一个极好的补充作用，可在军事及民用领域有广泛应用，比如：电力、医疗、化工行业、隧道施工、危险区域管控等。

UWB的定位技术有多种方法，本文仅考虑基于飞行时间(Time of Flight,TOF)的测距原理，它是UWB定位法中最常见的定位方法之一。TOF测距技术属于双向测距技术，其通过计算信号在两个模块的飞行时间，再乘以光速求出两个模块之间的距离，这个距离肯定有不同程度的误差，但其精度已经比较高。

在室内定位的应用中，UWB技术可以实现厘米级的定位精度(一般指2维平面定位),并具有良好的抗多径干扰和衰弱的性能以及具有较强的穿透能力。但由于室内环境复杂多变UWB通信信号极易受到遮挡，虽然UWB技术具有穿透能力，但仍然会产生误差，在较强干扰时，数据会发生异常波动(通常是时间延时)，基本无法完成室内定位，甚至会造成严重事故。因此，信号干扰下的超宽带(UWB)精确定位问题成为亟待解决的问题。

发明内容

本发明要解决的技术问题是针对以上不足，提供一种基于最小二乘法与卡尔曼滤波器的移动靶点位置计算方法。

一种基于最小二乘法与卡尔曼滤波器的移动靶点位置计算方法，包括以下步骤：

步骤1、在靶点所在的三维空间空间内建立多个固定锚点，在靶点移动过程中，每个锚点分别检测自身与靶点的距离，存储每一时刻所有锚点检测的距离数据作为单一时刻数据，按时间顺序排列所有单一时刻数据得到时间序列数据；对于时间序列数据中间缺失的部分数据，利用前后数据的相关性，进行，得到完整的时间序列数据；

步骤2、对数据经行甄别，把时间序列中有误差的数据剔除掉，并根据前后数据的相关性把数据补充完整，得到最终的时间序列T1；

步骤3、在每个时刻，利用多边侧距法，根据时间序列T1，确定该时刻靶点到所有锚点的距离，列出等式方程组，通过最小二乘法计算得到靶点坐标；

步骤4、利用卡尔曼滤波器对每个时刻靶点坐标进行矫正，从而得到每一时刻的精确靶点坐标；

步骤5、用平滑的曲线按时间顺序连接每一时刻的精确靶点坐标得到空间中靶点的运动轨迹。

进一步的，所述步骤2中甄别有误数据的方法包括以下步骤：

步骤2.1、设锚点共有四个，一时刻三维空间中四个锚点A0，A1，A2，A3坐标分别为(x₀，y₀，z₀)、(x₁，y₁，z₁)、(x₂，y₂，z₂)、(x₃，y₃，z₃),未知靶点S的坐标为(x，y，z),则三角形A0SA1中，若所测边长满足任意两边之和大于第三边且任意两边之差小于第三边则判断该时刻未知靶点S的坐标为正常数据，否则判断该时刻四个锚点测得的数据中有部分锚点测得的距离有误差，转到下一步；

步骤2.2、对四个锚点中每一个锚点的数据分别减去三个不同的长度的数据，若减去数据后，通过最小二乘法计算得到的靶点与原来靶点坐标在预设误差范围内则说明该锚点的数据存在误差，减去三个数据后得到靶点误差最小的数据即为去掉误差干扰后的数据，利用去掉误差干扰后的数据替换存在误差的数据，即对该时刻的数据修正完成，遍历时间序列T1每个有问题的时刻中每一个数据，进而可以得到修正后的时间序列T3。

进一步的，，所述步骤3包括以下步骤：

步骤3.1、设三维空间中锚点共有四个，一时刻四个A，B，C，D坐标分别为(x₀，y₀，z₀)、(x₁，y₁，z₁)、(x₂，y₂，z₂)、(x₃，y₃，z₃),待求靶点S的坐标为(x，y，z),通过测距得到S到锚点A、B、C和D的距离分别为d₀、d₁、d₂、d₃，则有关系式：

将方程组中的每一个方程依次减去最后一个方程，化简为：

取该方程的前三个可得到一个三元一次线性方程组，将这个方程组写成矩阵形式：AX＝B其中

其中X＝A^-1B；

设X≈X^*，将X^*代入X＝A^-1B，对自变量X^*求导：

故A^TAX^*＝A^TB，即X^*＝(A^TA)^-1(A^TB)，由于X≈X^*，即得到该时刻靶点位置的最优解。

进一步的，，得到i时刻靶点优解(x_i，y_i，z_i)后，取这个样本对应的真实靶点坐标为

以其为球心50mm为半径进行一次筛选，筛掉球以外的点，定义均方误差MSE：

将均方误差的平方根定义为均方根误差RMSE：

进一步的，，所述步骤4包括以下步骤：

步骤4.1、k时刻时，收集现在k状态靶点坐标的测量值Z(k)，设X(k)为现在k状态的预测的靶点坐标，结合预测值和测量值，可以得到现在状态(k)的最优化坐标估算值X(k|k)；

X(k|k)＝X(k|k-1)+kg(k)[Z(k)-HX(k|k-1)]

其中kg为卡尔曼增益：

P(k|k)＝[I-kg(k)H]P(k|k-1)，X(k|k)即为每一时刻的精确靶点坐标；

X(k|k-1)＝AX(k-1|k-1)

用字母P表示协方差：

P(k|k-1)＝AP(k-1|k-1)A′+Q,

在上述公式中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A′表示A的转置矩阵，Q是系统过程的协方差；根据公式X(k)＝AX(k-1)+BU(k)+W(k)和Z(k)＝HX(k)+V(k)对k状态的系统中靶点坐标进行预测，其中X(k)是预测的k时刻的靶点的结果，U(k)是k时刻对系统的控制量，U(k)＝0；A和B是预设系统参数且为矩阵，Z(k)是k时刻的测量值即为步骤3得到的k时刻靶点的坐标，H是预设测量系统的参数,，W(k)和V(k)分别表示过程和测量的噪声，k初始值为1，k＝1时，X(k|k)＝X(k)＝Z(k)；

步骤4.2、令k＝k+1，转到步骤4.1，迭代运算直到求得所有时刻靶点最优化坐标估算值即精确靶点坐标。

为解决以上技术问题，本发明采用以下技术方案：

本发明采用以上技术方案后，与现有技术相比，具有以下优点：

第一本发明只需要利用最小二乘法和卡尔曼滤波器，计算得到靶点坐标的方法，相对传统的用的多变测距，计算得到的靶点坐标精确度更高。

第二相比传统计算过程，此方法在计算之前减小误差，排除了干扰，所得到的时间序列可信度高。

第三在该方法里观察靶点的运动轨迹更方便。

附图说明

图1为本发明的流程示意图；

图2为实测环境具体空间直角坐标系，利用三角形的三边规律用于直观判断所测数据是否有异常；

图3为多变测距法中解存在性的分情况讨论；

图4为筛选过程示意图；

图5为carlman模型图；

图6为代入数据计算结果示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

如图1所示，一种基于最小二乘法和卡尔曼滤波器的计算方法，包括以下步骤：

步骤1、完善数据。

建立空间直角坐标系，数据坐标通过观察和简单计算可以发现附件中所给的数据是存在很多问题的，例如存在数据缺失、重复或相似的情况，这些有问题的数据是不能正常使用的，对含有以上问题的数据进行清洗和预处理，保证得到的数据可以用于后续任务的计算中。对与中间缺失的部分数据，利用前面和后面的数据的相关性，例如线性相关性，补全所缺失的数据，使之成为完整的时间序列，对于常见的时间序列方便后续的处理与加工。

步骤2、干扰检测。

对于锚点所测的时间序列数据，由于设施的在测量中存在各种各样的干扰，部分数据测出来时会偏大，导致所测数据不准，因而要对数据经行相应的甄别，方法一：把数据减去适当固定的长度(300，500，650)，若对所求的靶点结果有明显的变化，则认为它是正常数据；若对所求的靶点结果没有明显的变化，则认为它是异常数据。方法二：通过锚点间的距离与靶点与锚点距离之间的关系，利用构成三角形的基本原理——任意两边之和大于第三边，任意两边之差小于第三边，构建方程来判断该点测得数据是否为异常数据。三维空间中四个锚点A0，A1，A2，A3坐标分别为(x₀，y₀，z₀)、(x₁，y₁，z₁)、(x₂，y₂，z₂)、(x₃，y₃，z₃),未知靶点S的坐标为(x，y，z),如图二，三角形A0SA1中，所测边长就应该满足任意两边之和大于第三边，任意两边只差小于第三边。

(1)找出误差的数据。

在图2中，在三角形A0A1S中，由任意两边之和大于第三边，任意两边之差小于第三边得：

和

和

和

同样的在三角形A0A3S，三角形A1A2S，三角形A2A3S中，都可以用来判断该锚点测得数据是否正常,若不正常则说明该时刻四个锚点测得的数据中有部分锚点测得的距离有误差。

(2)减少误差。

在上述过程中只能找到是那个时刻的的数据有误差，但是不知道是具体时刻的那几个数据有问题，作为精确度的问题，只要想办法减小解存在的可行域的范围，那么利用最小二乘法求解的靶点的精度就会提高。

由于误差可能存在该时刻的四个数据里面，对其中一个数据分别减去300，500，650(在不同的题设条件下自主的选择合适的误差数据)，若减去数据后，通过最小二乘法计算得到的靶点与原来靶点坐标在误差范围内，则说明该数据存在误差。相比较减去三个数据后得到靶点误差最小的数据即为该数据去掉误差干扰后的数据，遍历时间序列T1每个有问题的时刻中每一个数据，进而可以得到修正后的时间序列T3。

步骤3、靶点求解。

设出目标点的坐标，利用多变测距法，根据靶点到锚点的距离列出等式方程组，通过最小二乘法计算得到目标点坐标，得到坐标与实际所给目标点坐标在纵坐标上存在误差，利用卡尔曼滤波器进行靶点的矫正，从而得到精确的靶点坐标。

多边测距下解的存在性分析：

一维情况下，在一条数轴上，由于数轴只有两个方向，根据靶点到锚点的距离可以确定两个可能靶点位置，现设置两个锚点，则可以唯一确定出一个靶点的位置，由于存在信号干扰情况，因此存在距离误差，下面给出靶点可能位置的情况论述。

情况1，如图3-1，两个锚点同时发射信号接触到靶点后信号反馈回锚点，理想状态下，没有受到干扰时，信号接收路线，靶点在以A,B两个锚点为中心的两个闭区间的交点上。

情况2，如图3-2，若有一个锚点发射的信号受到干扰，会导致发射信号到接收信号的时间出现误差而变大，受到干扰的后信号反馈所测出的靶点与锚点的距离会增大，此时如图2，可见，靶点在以A,B两个锚点为中心的两个开邻域的交集中，并且在其交集的闭区间的边界上。

情况3，如图3-3，若两个锚点发射的信号同时受到干扰时，靶点将会在以A,B两个锚点为中心的两个开邻域的交集中间。

下面讨论二维平面中的情况：

情况1，如图3-4，在二维平面内，任意三个不共线的锚点可以唯一确定一个靶点的位置，因为锚点发射的信号可能受到干扰，因此会出现误差，但可以确定出靶点所在位置的一个可能区域。

情况2，如图3-5，若其中一个靶点所发射的信号受到干扰，此时因为发射信号到接收信号所隔时间差变大，导致该锚点所测的靶点位置将会在大于其真实位置的圆周上，但由于另两个锚点的信号没有受到干扰，那么以此两个锚点建立的靶点所在圆周上最多可以有两个交点，即存在两个靶点可能位置，而真实靶点必定在受到干扰的锚点所测的圆周区域内，则可以根据此区域来缩小靶点位置范围，若两个可能靶点只有一个在黑色区域内，则唯一得到一个置即为真实靶点位置，若都在黑色区域内，则需要进一步辨别。

情况3，如图3-6，若其中有两个锚点所发射的信号都收到了干扰，导致有两个锚点所测的靶点位置将会在大于其真实位置的圆周上，此时只有一个锚点发射的信号没有受到干扰，此时的靶点会在该锚点所测距离的圆周上，如图，此时真实靶点在A,C两圆所交的红色区域内，且在B所在的圆周上，此时，得到一条弧线为靶点P所在的位置范围。

情况4，如图3-7，若三个锚点发射点信号同时受到干扰，此时靶点真实位置将会在三个锚点为圆心，所测的距离为半径的三个圆所交的区域内，如图，此时，靶点P点在灰色区域内。

将情况延伸到三维空间中，在三位空间中确定一个靶点的位置需要四个锚点，以每一个锚点为球心，以锚点到靶点的时间差所测的距离作为球的半径作球面，则四个锚点形成四个球面，靶点在每一个球面上，可以得到唯一的一个靶点，并且每三个不共线，同样的，在测距定位时会因为信号干扰导致出现偏差，具体分为以下几种情况：

情况1，如图3-8，若四个靶点都没有受到信号干扰，此时如图，四个球有且仅有一个公共点，下面给出证明。首先，两个没有干扰的锚点所在的球交出一个圆，在该圆上所有点都满足P(靶点)的要求，即到两个锚点的距离等于所测距离。现引入第三个锚点所在的球面与该圆交出两个点，这两个点都满足三个到三个锚点的距离等于其所测距离，再引入第四个锚点所在的球，与该弧线交出仅一个点，即为唯一的一个靶点的位置。

情况2，如图3-9，若有一个靶点发射点信号受到干扰，此时如上图，以三个没有受到信号干扰的锚点为球心，以所测距离为半径形成的球面交于两点，此时第四个受到信号干扰的靶点所构建的球体体积偏大，则有两种情况，第一种是情况是两点包含于该球体内部，此时两个点均可能为靶点。第二种情况是球体将两个可能靶点分开，一个在球面外部一个在球面内部。此时靶点自然应当取球面内部的可能靶点。

情况3，如图3-10，若有两个靶点发射的信号受到干扰，此时如图，以另两个没有受到干扰的锚点为球心，以所测距离为半径形成球面交于一个圆，此时第三个锚点发射的信号受到干扰导致到靶点距离为半径所成的球面偏大，将圆面截出一段圆弧，可知，靶点必定在该段圆弧上。接着另一个受干扰的锚点也对应这这样一个球体也将圆截下一段圆弧，则可知，靶点必定在这两段圆弧的重合之处。

情况4，如图3-11，若有三个靶点发射的信号受到干扰，如图，三个有信号干扰的锚点形成的球体交于一个封闭空间，第四个没有受到干扰的信号所反馈的形成的球体与该公共区域交于一个弧面，因为靶点必定在该球面上某个位置，也必定在该封闭空间内，则靶点必限制在该弧面上。

情况5，若四个靶点发射的信号都受到了干扰，由情况4可知，靶点必在三个球体所包含空间内，由第四个锚点也受到信号干扰形成的球体将该空间区域一分为二，取被包含在第四个球体内的部分空间区域，则靶点被限制在该区域内。

综上所述情况，则可以得到结论，在三维空间中四个锚点发射信号，当发射的信号受到干扰时，可以根据所测的各个靶点到锚点的距离所测值确定出真实靶点所在位置的一个空间范围，即靶点必定限制在这个确定的范围内。因此可以在靶点坐标的估计位置坐标集所在空间范围内取到真实靶点坐标，可以依据这个思想去求出所有靶点可能位置坐标，进而去预测出真实靶点所在位置。

多变测距解的求解：

多边测量法是一种位置估计方法，设三维空间中四个锚点A，B，C，D坐标分别为(x₀，y₀，z₀)、(x₁，y₁，z₁)、(x₂，y₂，z₂)、(x₃，y₃，z₃),未知靶点S的坐标为(x，y，z),经过TOF、RSSI等测距方式获得S到锚点A、B、C和D的距离分别为d₀、d₁、d₂、d₃。则有关系式：

将方程组中的每一个方程依次减去最后一个方程，化简为：

取该方程的前三个可得到一个三元一次线性方程组，将这个方程组写成矩阵形式：AX＝B其中

若有解其中X＝A^-1B (*)

进行分类讨论：

1、固然希望(*)是有解的，因为如果有解的话一定是唯一解，此时的唯一解必然就是我们要找的靶点的位置，这时的解是一个优解。

2、重点讨论(*)无解的情况，因为矩阵A的秩在很多时候都不等于增广矩阵(AB)的秩，此时(*)无解，这时候要想办法找一个近似的最优解，只需要采用最小二乘法对式子AX^*-B进行讨论。

将目标函数函数f(X^*)＝(|AX^*-B|)²定义为靶点精确定位问题中的“误差”，自变量为靶点近似坐标X^*,该问题中，要使得误差尽可能的小，这样找到的靶点的位置才会更加的精确，所以就把该问题转化为了求目标函数取最小值时自变量的值，此时显然最小值就是极小值，对目标函数对自变量X^*求导：

故A^TAX^*＝A^TB，即X^*＝(A^TA)^-1(A^TB)

矩阵A的秩等于3的情况其实包含在情况1)中了，这里讨论的是矩阵A的秩小于3的情况，所以很显然A^TA这个矩阵是不可逆的，式子X^*＝(A^TA)^-1(A^TB)中的(A^TA)^-1在这里称之为伪逆。

定义均方误差(MSE)：

现对清理后的每一个正常数据文件每一组正常数据带入上述模型我们可以得到一个靶点优解(x_i，y_i，z_i),取这个样本对应的真实靶点坐标为

以其为球心50mm为半径进行一次筛选，筛掉球以外的点，定义均方误差(MSE)：

是指估计值与真实值之间差值平方的期望值，则MSE值的大小与模型测出的实验数据误差成正比，MSE值越小精确度越高。将均方误差的平方根定义为均方根误差(RMSE)：

均方根误差可以用于评价UWB室内定位靶点位置，若靶点的估计位置坐标中存在误差较大的异常数据，则需要剔除异常数据，再均方根进行定位算法性能评价，否则会导致均方根误差数值很大，对定位算法性能评价不准确。这也是以上操作中需要依次筛选的原因。

定义精度：

J∈[0，1]可以衡量实验数据误差大小。三维精度则取N＝3，同理二维精度取N＝2,一维精度取N＝1时即可。

有效性分析：

如图4，当未知靶点真实位置时，此时对所有组求得的靶点优解分别对其x,y,z轴坐标取平均数得到中心点

以中心点坐标位置为原点，50mm为半径进行一次筛选，得到N个在球内的靶点优解坐标(x_i，y_i，z_i)，定义均方误差(MSE)：

将均方误差的平方根定义为均方根误差(RMSE)：

karlman滤波器

卡尔曼滤波器介绍：此概念会涉及一些基本的概念知识，包括概率，随机变量，高斯或正态分配(或正态分布)还有State-space Model等等。但对于卡尔曼滤波器的详细证明，这里不一一描述。如图5给出karlman模型图：

Step 1：先引入一个离散控制过程的系统，在此题设中，最小二乘法求得的没有去误差的靶点时间序列T4即为离散控制过程，用线性随机微分方程来描述离散控制过程系统：

X(k)＝AX(k-1)+BU(k)+W(k) (1)

再加上系统的测量值：

Z(k)＝HX(k)+V( (2)

上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，在此问题中，A和B为矩阵。Z(k)是k时刻的测量值即为k时刻靶点的坐标，H是测量系统的参数。W(k)和V(k)分别表示过程和测量的噪声。在一般过程中，时间序列都为平稳的，他们的协方差分别是Q，R(这里假设时间序列不随系统状态变化而变化)。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是要求比较严格的信息处理器。结合公式(1)、(2)和(3)协方差来估算这个系统中靶点的最优化估计值。

Step 2：要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据karlman的系统模型，可以基于系统的上一状态k-1处的坐标而预测出现在状态：

X(k|k-1)＝AX(k-1|k-1)+BU(k) (3)

在公式(3)中，X(k|k-1)是利用上一状态预测的靶点最优化结果而得到的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，在此系统中没有控制量，它可以为0。

Step 3：系统结果已经更新，然而对应于X(k|k-1)的协方差还没更新。用字母P表示协方差：

P(k|k-1)＝AP(k-1|k-1)A′+Q (4)

在公式(4)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A′表示A的转置矩阵，Q是系统过程的协方差。公式(1)、(2)就是卡尔曼滤波器5个公式当中的前两个，也就是对系统中靶点坐标的预测。

Step 4：有现在k状态的预测的靶点的结果，然后再收集现在k状态靶点坐标的测量值。结合预测值和测量值，则可以得到现在状态(k)的最优化坐标估算值X(k|k)：

X(k|k)＝X(k|k-1)+kg(k)[Z(k)-HX(k|k-1)] (5)

其中kg为卡尔曼增益：

Step 5：已经得到了k状态下靶点坐标最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，还需要更新k状态下X(k|k)的协方差：

P(k|k)＝[I-kg(k)H]P(k|k-1) (7)

其中I为1的矩阵，对于单模型单测量，I＝1。当系统进入k+1状态时，P(k|k)就是公式(7)的P(k-1|k-1)。该模型就可以自行回归的运算下去。

表格1

对清洗后的数据画出折线统计图如图6-1和6-2所示：

在图6-1中，可以直观的看出有些波浪地方有干扰情况出现，具体是那些情况出现干扰，对干扰数据经行干扰判断，结果如图6-2所示：在图中纵坐标为零的点是有干扰的点，纵坐标为1的点是没有干扰的点，再利用点的线性性，数据输入线性拟合模型补全缺少数，则可得到高斯白噪声序列，分别将经过干扰检测后得到的靶点时间序列坐标T4时间序列数据带入到Karlman滤波器中，对靶点的坐标进行定位，画出三个运动路径(直接定位，去干扰定位，干扰检测+Karlman滤波器)俯视图也就是靶点在xoy平面的运动轨迹和三维干扰检测+Karlman滤波器路径如图6-3和6-4所示：

对于结果路径图而言，干扰检测+carlman滤波要比直接定位与去干扰要缓和很多，经过这个干扰检测后的数据与路径的可信度和误差范围都提高很多。

具体运算结果以T_n-1，T_n时刻的坐标为例计算，本实例中，H＝1,A＝K_n,B＝1-K_n；

对与计算中的测量误差

根据题目需要可随机生成，在此问题中服从正太分布N～(10，1)，估计误差只需要在T₁时刻输入固定值，后面时刻的估计误差可以迭代更新，

T_n-1时刻的坐标预测T_n时刻的坐标公式：

以上所述为本发明最佳实施方式的举例，其中未详细述及的部分均为本领域普通技术人员的公知常识。本发明的保护范围以权利要求的内容为准，任何基于本发明的技术启示而进行的等效变换，也在本发明的保护范围之内。

Claims (5)

1.一种基于最小二乘法与卡尔曼滤波器的移动靶点位置计算方法，其特征在于，包括以下步骤：

步骤1、在靶点所在的三维空间空间内建立多个固定锚点，在靶点移动过程中，每个锚点分别检测自身与靶点的距离，存储每一时刻所有锚点检测的距离数据作为单一时刻数据，按时间顺序排列所有单一时刻数据得到时间序列数据；对于时间序列数据中间缺失的部分数据，利用前后数据的相关性，进行，得到完整的时间序列数据；

步骤2、对数据经行甄别，把时间序列中有误差的数据剔除掉，并根据前后数据的相关性把数据补充完整，得到最终的时间序列T1；

步骤3、在每个时刻，利用多边侧距法，根据时间序列T1，确定该时刻靶点到所有锚点的距离，列出等式方程组，通过最小二乘法计算得到靶点坐标；

步骤4、利用卡尔曼滤波器对每个时刻靶点坐标进行矫正，从而得到每一时刻的精确靶点坐标；

步骤5、用平滑的曲线按时间顺序连接每一时刻的精确靶点坐标得到空间中靶点的运动轨迹。

2.根据权利要求1所述的基于最小二乘法与卡尔曼滤波器的移动靶点位置计算方法，其特征在于，所述步骤2中甄别有误数据的方法包括以下步骤：

步骤2.1、设锚点共有四个，一时刻三维空间中四个锚点A0，A1，A2，A3坐标分别为(x₀，y₀，z₀)、(x₁，y₁，z₁)、(x₂，y₂，z₂)、(x₃，y₃，z₃)，未知靶点S的坐标为(x，y，z)，则三角形AOSA1中，若所测边长满足任意两边之和大于第三边且任意两边之差小于第三边则判断该时刻未知靶点S的坐标为正常数据，否则判断该时刻四个锚点测得的数据中有部分锚点测得的距离有误差，转到下一步；

步骤2.2、对四个锚点中每一个锚点的数据分别减去三个不同的长度的数据，若减去数据后，通过最小二乘法计算得到的靶点与原来靶点坐标在预设误差范围内则说明该锚点的数据存在误差，减去三个数据后得到靶点误差最小的数据即为去掉误差干扰后的数据，利用去掉误差干扰后的数据替换存在误差的数据，即对该时刻的数据修正完成，遍历时间序列T1每个有问题的时刻中每一个数据，进而可以得到修正后的时间序列T3。

3.根据权利要求1所述的基于最小二乘法与卡尔曼滤波器的移动靶点位置计算方法，其特征在于，所述步骤3包括以下步骤：

步骤3.1、设三维空间中锚点共有四个，一时刻四个A，B，C，D坐标分别为(x₀，y₀，z₀)、(x₁，y₁，z₁)、(x₂，y₂，z₂)、(x₃，y₃，z₃)，待求靶点S的坐标为(x，y，z)，通过测距得到S到锚点A、B、C和D的距离分别为d₀、d₁、d₂、d₃，则有关系式：

将方程组中的每一个方程依次减去最后一个方程，化简为：

取该方程的前三个可得到一个三元一次线性方程组，将这个方程组写成矩阵形式：AX＝B其中

其中X＝A^-1B；

设X≈X^*，将X^*代入X＝A^-1B，对自变量X^*求导：

故A^TAX^*＝A^TB，即X^*＝(A^TA)^-1(A^TB)，由于X≈X^*，即得到该时刻靶点位置的最优解。

4.根据权利要求1所述的基于最小二乘法与卡尔曼滤波器的移动靶点位置计算方法，其特征在于，得到i时刻靶点优解(x_i，y_i，z_i)后，取这个样本对应的真实靶点坐标为

以其为球心50mm为半径进行一次筛选，筛掉球以外的点，定义均方误差MSE：

将均方误差的平方根定义为均方根误差RMSE：

5.根据权利要求4所述的基于最小二乘法与卡尔曼滤波器的移动靶点位置计算方法，其特征在于，所述步骤4包括以下步骤：

步骤4.1、k时刻时，收集现在k状态靶点坐标的测量值Z(k)，设X(k)为现在k状态的预测的靶点坐标，结合预测值和测量值，可以得到现在状态(k)的最优化坐标估算值X(k|k)；

X(k|k)＝X(k|k-1)+kg(k)[Z(k)-HX(k|k-1)]

其中kg为卡尔曼增益：

P(k|k)＝[I-kg(k)H]P(k|k-1)，X(k|k)即为每一时刻的精确靶点坐标；

X(k|k-1)＝AX(k-1|k-1)

用字母P表示协方差：

P(k|k-1)＝AP(k-1|k-1)A′+Q，

在上述公式中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A′表示A的转置矩阵，Q是系统过程的协方差；根据公式X(k)＝AX(k-1)+BU(k)+W(k)和Z(k)＝HX(k)+V(k)对k状态的系统中靶点坐标进行预测，其中X(k)是预测的k时刻的靶点的结果，U(k)是k时刻对系统的控制量，U(k)＝0；A和B是预设系统参数且为矩阵，Z(k)是k时刻的测量值即为步骤3得到的k时刻靶点的坐标，H是预设测量系统的参数，，W(k)和V(k)分别表示过程和测量的噪声，k初始值为1，k＝1时，X(k|k)＝X(k)＝Z(k)；

步骤4.2、令k＝k+1，转到步骤4.1，迭代运算直到求得所有时刻靶点最优化坐标估算值即精确靶点坐标。

Images