CN101813780B - 一种集成gps和多传感器的移动目标实时追踪方法 - Google Patents

Abstract

本发明公开了一种集成GPS和多传感器的移动目标实时追踪方法。所述方法包括：构建一种集成GPS设备的多传感器惯性系统，该系统分别安装于追踪对象和被追踪对象上；追踪对象利用该系统采集的数据，并经过卡尔曼滤波校正处理后能实时获得自身及被追踪对象较为准确的位置、高度、速度、方向信息；追踪对象能用跟踪策略，对移动中的被追踪对象实施动态追踪并最终追踪到被追踪对象。

Description

一种集成GPS和多传感器的移动目标实时追踪方法

技术领域

本发明涉及移动通信定位技术领域，尤其涉及一种集成GPS和多传感器的移动目标实时追踪方法。

背景技术

全球定位系统GPS(Global Positioning System)在接收机能收到4颗以上的卫星信号时，能提供运动目标在用户可接受精确度范围内的位置、速度以及时间信息。然而，GPS信号强度与用户所处位置相关，比如在山洞、隧道、城市高楼边就较难收到信号。同时，GPS信号的强度也受到天气的影响。多传感器的惯性系统相对GPS而言，独立性较强，对外界干扰的免疫力较好。但是，由于仪器本身的磨损，传感器经过长期使用后会造成测量信号的偏差、漂移以及缩放比例因子的波动，这都将使多传感器惯性系统的精准度变差。

为此，结合GPS和多传感器惯性系统得到的双重信号并加以比较和校正的解决方案可以获得单一独立系统所不能达到的准确性。例如可以采用GPS获取带有一定噪声的位置信息，然后采用多传感器惯性系统改善该信号的准确度，并且当GPS信号失效时，多传感器惯性系统也能提供位置信息，辅助GPS在失效后快速重新获取信号，从而降低GPS检测和校正的搜索范围。另外，多传感器惯性系统在获取位置、速度以及高度信息时的频率也较高。卡尔曼滤波可以用于优化来自多传感器惯性系统和GPS两者处的位置、速度、方向以及高度信息，并通过误差比较运算对位置、速度、方向以及高度做出调整，从而提高信息的准确度。目前，应用于汽车，步行者的多传感器惯性系统价格依然较高，本发明在减少传感器数量的同时依然能保持合适的定位准确度。

移动目标的追踪通常受到目标位置可移动条件的约束，即在复杂、动态、目标位置信息可实时获取的系统中，追踪对象受路况拓扑结构、障碍物规避、物理动力学模型、移动目标逃逸策略等应用上下文环境的综合约束。追踪对象以可接受的计算代价实时规划出最优或近似最优的追踪路径，并沿此路径移动，进而逼近并追踪到移动的被追踪对象。该方法能解决户外旅游、军事仿真、交通控制、汽车导航等诸多领域中的动态追踪问题，具有较强的通用性和重要的应用价值。

在实际应用中，由于受有限的计算资源、实时性和精确性等条件的限制，对设计实用、高效的移动目标追踪方法提出了更高的要求，具体表现为：1)严格的实时性约束。监控系统采集目标位置的频率应控制在一定范围内才能保证精确性和有效性；2)目标的可移动性。可移动性显著增加了路径规划过程的复杂性和不确定性；3)计算资源需求过高。由于实际交通地图的搜索空间巨大，需消耗大量的内存空间和计算时间；4)对抗搜索。被追踪对象的移动路径的不确定性极大增加了追踪方法的设计难度；5)环境的部分可知。位置获取系统对移动目标所采集的信息具有一定的误差和延迟。目前已有的方法无法满足以上要求，因为它们主要都集中在传统的静态目标车辆导航、智能搜索技术和机器人路径规划等领域，并普遍存在着追踪成功率不高、计算代价较大、收敛速度较慢、追踪行为智能水平较低、性能不稳定、实现复杂等问题，与在动态不确定性环境下的目标追踪要求存在较大差异。

发明内容

本发明的目的在于提供一种集成GPS和多传感器的移动目标实时追踪方法。本发明解决前述的在动态环境下的目标追踪问题所采用的技术方案如下：

1)构建集成GPS设备的多传感器惯性系统。

本发明在追踪对象和被追踪对象上各安装一套集成GPS设备的多传感器惯性系统，该系统具有远程数据传送功能，并包含一个单轴的陀螺仪、一个速度表以及一个高度计，分别获得追踪对象和被追踪对象的方向、速度以及高度信息，并根据追踪对象和被追踪对象的初始位置，经过计算获得两者的运动距离和所在位置信息；同时，利用系统中集成的GPS设备也能获得追踪对象和被追踪对象带有噪声的经、纬度位置、高度、方向以及速度信息；在该系统中采用卡尔曼滤波方法对追踪对象和被追踪对象的方向、速度、高度以及位置进行误差校正，最终可以获得追踪对象和被追踪对象较为准确的方向、速度、高度以及位置信息。

2)实现基于跟踪策略的移动目标追踪方法。

在介绍本方法的实施之前，先对本方法相关的几个概念进行说明：

追踪代价，

追踪代价指追踪对象移至被追踪对象移动轨迹上的任一位置p_t，然后从p_t开始沿被追踪对象运动轨迹移动并最终追踪到被追踪对象，上述两个阶段总共发生的代价度量值，记为

该代价的计算方法如公式1所示：

$\▿\left(p_t\right)=\mathrm{\α}\·\left(\right|p_h^x-p_t^x|+|p_h^y-p_t^y|+|p_h^z-p_t^z\left|\right)+\underset{p_t{p}_g}{\underset{\mathrm{mnr}}{\∫}}{u}_h\mathrm{dt},(\mathrm{\α}>1)---\left(1\right)$

公式1中的p_h、p_t和p_g分别表示追踪对象的当前位置、被追踪对象移动轨迹上的某一位置和被追踪对象的当前位置；p_h ^x、p_h ^y和p_h ^z分别表示追踪对象当前位置平面坐标的X分量、Y分量以及高度值；p_t ^x、p_t ^y和p_t ^z分别表示被追踪对象移动轨迹上任一位置p_t的平面坐标的X值、Y值以及高度值；表示被追踪对象的轨迹从p_t移动至p_g时追踪对象的行程距离，其中u_h表示追踪对象的即时速率；α表示搜索阶段中单位代价的加权比例系数，增大α可以增加搜索代价在追踪代价中的权重，用以调节搜索单位代价和追踪单位代价中的不平衡性。

追踪代价最小的最佳追踪起始位置，

追踪代价最小的最佳追踪起始位置是指可令前述追踪代价最小的追踪起始位置，能使追踪对象快速沿被追踪对象的移动轨迹对被追踪对象实施追踪，该位置的计算方法如公式2所示：

$p_{\mathrm{start}}^{\mathrm{best}}=\arg \underset{p_t\∈\mathrm{Trail}}{\min }\{\▿\left(p_t\right)\}---\left(2\right)$

公式2中的Trail表示追踪对象所记录的被追踪对象的移动轨迹，arg为求解反函数。

策略偏好度，

策略偏好度是指不采用追踪策略的追踪代价与采用追踪策略的追踪代价之间的比值，其计算方法如公式3所示：

$\mathrm{\Θ}=\frac{\mathrm{\α}\·\left(\right|p_f^x-p_g^x|+|p_f^y-p_g^y|+|p_f^z-p_g^z\left|\right)}{\underset{p_f{p}_g}{\underset{\mathrm{mnr}}{\∫}}{u}_h\mathrm{dt}},p_f\∈\mathrm{Trail}---\left(3\right)$

公式3中，p_f为追踪对象实际追踪的起始位置。

本发明中采用如下步骤实现基于跟踪策略的移动目标追踪方法。

A、追踪对象通过集成GPS设备的多传感器惯性系统得到并记录被追踪对象的移动轨迹；

B、追踪对象移至被追踪对象移动轨迹上的某一位置，移至该位置能使追踪对象快速沿被追踪对象的移动轨迹对被追踪对象实施追踪；

步骤B具体为：

B1、全面考虑追踪对象探寻至被追踪对象移动轨迹上的代价和沿被追踪对象轨迹移动至被追踪对象所在位置的代价，选取被追踪对象轨迹上可使采用跟踪策略的总追踪代价最低的位置作为追踪对象当前最佳的追踪起始位置，具体计算方法见公式1和公式2；

B2、然后采用GAA*算法向追踪对象的当前最佳追踪起始位置实时搜索有限步长，并在该过程中忽略被追踪对象的移动，若在限定步骤内搜索到被追踪对象移动轨迹上的任一位置，转步骤B3，否则转步骤B4；

注：GAA*算法描述请见文献S.Koenig，X.Sun，Y.William.Generalized adaptive A*.Proc.7th Int.Conf.Autonom.Agents Multiagent Syst.，2008：469-476

B3、追踪对象停止搜索并移至该位置，该位置即为实际追踪起始位置，然后转步骤C；

B4、追踪对象停止搜索并按已规划好的局部路径移动限定步数，然后转步骤B1重复以上过程。

C、追踪对象对被追踪对象的移动轨迹进行裁剪；

步骤C引入策略偏好度用以指导是否采用被追踪对象当前的移动轨迹。

步骤C具体为：

C1、若策略偏好度大于一个预先设置的经验阈值，说明此时需采用追踪策略，转步骤D，否则转步骤C2；

C2、删除所记录的当前被追踪对象的移动轨迹以释放内存，并转步骤A。

D、追踪对象沿经过裁剪的被追踪对象的轨迹移动，直至追踪到被追踪对象。

其中，所述的步骤A与步骤B-D之间是并行进行的。

以上本发明内容所述的追踪对象和被追踪对象的位置信息包括两者所处平面上的位置值(由经、纬度数值经过换算得到)以及高度值。由本发明所提供的技术方案可以看出，本发明通过记录由集成GPS的多传感器惯性系统所得到的被追踪对象的位置信息，自动为追踪对象实时规划出一条最优或接近最优的基于跟踪策略的追踪路径，只要追踪对象的平均速率大于被追踪对象的平均速率，追踪对象就一定能追踪到移动的被追踪对象。

相对于传统方法，本发明在追踪成功率、计算代价、收敛速度、智能水平、性能稳定性、实现复杂度等方面具有显著优势，方法完全能固化于警用、军用及民用车载或手持的集成GPS设备和多传感器惯性系统的终端中。

附图说明

图1为本发明设计的集成GPS设备的多传感器惯性系统工作流程图；

图2为基于跟踪策略的移动目标追踪方法说明图；

具体实施方式

在实施集成GPS和多传感器的移动目标追踪方法时，将利用本发明中的集成GPS设备和多传感器的惯性系统，实时地将采集到的被追踪对象的位置信息传送给追踪对象，并储存在追踪对象的车载或者手持终端中。之后，采用基于追踪策略的移动目标追踪方法为追踪对象实时规划一条最优或接近最优的追踪路径，以帮助追踪对象对被追踪对象实施追踪。

如图1所示，本发明构建一个集成GPS设备的多传感器惯性系统。

该系统包含一个高度计、一个速度表以及一个陀螺仪，并集成了GPS设备。系统首先根据速度表得到追踪对象和被追踪对象当前的直线速度，然后根据陀螺仪得到的追踪对象和被追踪对象的运动方向，并计算出追踪对象和被追踪对象在水平两个方向上的运动速度分量，通过积分计算可以得到追踪对象和被追踪对象的当前位置，而追踪对象和被追踪对象当前的高度值则由高度计获取。经过卡尔曼滤波器的误差校正后，将被追踪对象和追踪对象当前的位置、速度、方向、高度信息实时输出给后续的移动目标追踪系统。

在本发明中，卡尔曼滤波用松耦合的方式融合惯性导航系统中的传感器得到的位置、速度、方向、高度信息，采用数值统计的方法，计算追踪对象和被追踪对象的高度、位置、速度、方向数值的误差。卡尔曼滤波输出两个重要参数：估计的状态向量X和协方差矩阵P。估计的状态向量X集成了惯性导航系统中多传感器测量得到的高度、位置、速度、方向误差，还增加了传感器本身的随机误差。协方差矩阵P记录了不确定性的估计值，考虑了传感器噪声和动态不确定性对最后的估计误差状态所造成的影响。由于GPS设备得到的位置和速度的测量可能受积累的零均值噪声的影响，通过维护自身不确定性的估计值以及传感器输出的相对不确定性数值，卡尔曼滤波能用优化方法将所要估计数值的误差最小化。

本发明提供的基于跟踪策略的移动目标追踪方法具体包括：

步骤S10、追踪对象GPS终端发送针对被追踪对象的GPS动态定位请求，其中携带目标ID、最大容忍延迟数据；

步骤S11、被追踪对象开始应答，发送GPS动态定位开始响应，其中携带被追踪对象当前的位置、高度、方向以及速度数据；

在某些情况下，请求可能失败。此时返回相应的失败请求后，结束；否则，执行步骤S12和S13。其中，步骤S12和S13是并行处理的。

步骤S12、追踪对象的GPS终端开始持续接收被追踪对象所发来的位置信息数据，将这些位置按时间顺序编号，依序测算出被追踪对象的移动轨迹，并将被追踪对象的移动轨迹以及已接收到的所有位置信息存储在追踪对象终端的内存中；

步骤S13、追踪对象的GPS终端根据步骤S12中存储的被追踪对象的移动轨迹和被追踪对象的当前位置信息规划追踪路径；

在图2中，粗实线表示追踪对象的移动轨迹，虚线表示被追踪对象的移动轨迹，细实线表示追踪对象与被追踪对象移动轨迹重合的部分。追踪对象在A位置开始追踪被追踪对象，此时被追踪对象在A’位置，随后，当追踪对象分别处于B、C、D、E位置时，相应的，被追踪对象分别处于B’、C’、D’、E’位置，最后，追踪对象在位置F处追踪到被追踪对象。踪对象。

如图2所示，S13步骤具体包括：

步骤S131、对步骤S12中存储的每一个被追踪对象的位置信息应用公式1计算相应的追踪代价，然后根据公式2选取追踪代价最小的被追踪对象的位置为最佳追踪起始位置p_start ^best；

步骤S132、采用GAA*算法向p_start ^best实时搜索K个有限步长，并在该过程中忽略被追踪对象的移动，若K步内搜索到被追踪对象轨迹上所覆盖的任一位置p_f(不一定是p_start ^best或步骤S12中存储的其它被追踪对象的位置)，则转步骤S133，否则转S134；

步骤S133、停止搜索并按已规划的路径移至p_f，p_f即为追踪对象实际追踪的起始位置，然后转步骤S135；

步骤S134、停止搜索并按已规划好的局部路径移动K个限定步，然后转步骤S131重复以上过程；

步骤S135、应用公式3计算当前的策略偏好度，若该偏好度大于一个预先设置的经验阈值，说明此时得采用追踪策略，转步骤S136，否则转步骤S137；

步骤S136、追踪对象沿被追踪对象的移动轨迹移动，直至追踪到被追踪对象，结束；

步骤S137、删除步骤S12中所存储的被追踪对象的移动轨迹以及已接收到的所有位置信息，转步骤S131。

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

Claims (1)

1.一种集成GPS和多传感器的移动目标实时追踪方法，其特征在于，所述方法包括以下步骤：

1)为追踪对象和被追踪对象各构建一个集成GPS设备的多传感器惯性系统；所述系统具有远程数据传送功能，并包含一个单轴的陀螺仪、一个速度表以及一个高度计，分别获得追踪对象和被追踪对象的方向、速度以及高度信息，并根据追踪对象和被追踪对象的初始位置，经过计算获得两者的运动距离和所在位置信息；同时，利用系统中集成的GPS设备也能获得追踪对象和被追踪对象带有噪声的经、纬度位置、高度、方向以及速度信息；在该系统中采用卡尔曼滤波方法对追踪对象和被追踪对象的方向、速度、高度以及位置进行误差校正，最终可以获得追踪对象和被追踪对象较为准确的方向、速度、高度以及位置信息；

2)再采取如下步骤：

A、追踪对象通过集成GPS设备的多传感器惯性系统得到并记录被追踪对象的经过卡尔曼滤波校正的移动位置轨迹；

B、追踪对象移至被追踪对象移动轨迹上的某一位置，移至该位置能使追踪对象沿被追踪对象的移动轨迹对被追踪对象实施追踪，且追踪代价最小；

C、追踪对象对被追踪对象的移动轨迹进行裁剪；

D、追踪对象沿经过裁剪的被追踪对象的移动轨迹运动，直至追踪到被追踪对象；

其中，所述的步骤A与步骤B-D之间是并行进行的；

其中，追踪对象的平均速率大于被追踪对象的平均速率；

所述步骤B具体为：

B1、全面考虑追踪对象探寻至被追踪对象移动轨迹上的代价和沿被追踪对象轨迹移动至被追踪对象所在位置的代价，选取被追踪对象轨迹上可使采用追踪策略的总追踪代价最低的位置作为追踪对象当前最佳追踪起始位置；

B2、采用GAA*算法向追踪对象的当前最佳追踪起始位置实时搜索有限步长，并在该过程中忽略被追踪对象的移动，若在限定步骤内搜索到被追踪对象移动轨迹上的任一位置，转步骤B3，否则转步骤B4；

B3、追踪对象停止搜索并移至该位置，该位置即为实际追踪起始位置，然后转步骤C；

B4、追踪对象停止搜索并按已规划好的局部路径移动限定步数，然后转步骤B1重复以上过程；

所述步骤C具体为：

C1、若策略偏好度大于一个预先设置的经验阈值，说明此时需采用追踪策略，转步骤D，否则转步骤C2；

C2、删除所记录的当前被追踪对象的移动轨迹以释放内存，并转步骤A；

所述追踪代价指追踪对象移至被追踪对象移动轨迹上的任一位置p_t，然后从p_t开始沿被追踪对象运动轨迹移动并最终追踪到被追踪对象，上述两个阶段总共发生的代价度量值，记为▽(p_t)，该代价的计算方法如下式所示：

$\▿\left(p_t\right)=\mathrm{\α}\·\left(\right|p_h^x-p_t^x|+|p_h^y-p_t^y|+|p_h^z-p_t^z\left|\right)+\underset{\stackrel{\‾}{p_t{p}_g}}{\∫}{u}_h\mathrm{dt}(\mathrm{\α}>1)$

其中，p_h、p_t和p_g分别表示追踪对象的当前位置、被追踪对象移动轨迹上的某一位置和被追踪对象的当前位置；

和

分别表示追踪对象当前位置平面坐标的X分量、Y分量以及高度值；

和

分别表示被追踪对象移动轨迹上任一位置p_t的平面坐标的X值、Y值以及高度值；

表示被追踪对象的轨迹从p_t移动至p_g时追踪对象的行程距离，其中u_h表示追踪对象的即时速率；α表示搜索阶段中单位代价的加权比例系数，增大α可以增加搜索代价在追踪代价中的权重，用以调节搜索单位代价和追踪单位代价中的不平衡性；

所述最佳追踪起始位置的计算方法如下式所示：

$p_{\mathrm{start}}^{\mathrm{best}}=\arg \underset{p_t\∈\mathrm{Trail}}{\min }\{\▿\left(p_t\right)\}$

其中，Trail表示追踪对象所记录的被追踪对象的移动轨迹，arg为求解反函数；

所述策略偏好度是指不采用追踪策略的追踪代价与采用追踪策略的追踪代价之间的比值，其计算方法如下式所示：

$\mathrm{\Θ}=\frac{\mathrm{\α}\·\left(\right|p_f^x-p_g^x|+|p_f^y-p_g^y|+|p_f^z-p_g^z\left|\right)}{\underset{\stackrel{\‾}{p_f{p}_g}}{\∫}{u}_h\mathrm{dt}},p_f\∈\mathrm{Trail}$

其中，p_f为追踪对象实际追踪的起始位置。

Images