多移动智能体在目标跟踪中的惯导误差补偿方法及系统

1.本发明涉及目标跟踪与惯性导航领域，具体地，涉及一种多移动智能体在目标跟踪中的惯导误差补偿方法及系统。


背景技术：

2.在目标跟踪领域，多智能体系统利用相机或雷达进行目标跟踪十分常见。与单一智能体的目标跟踪相比，多智能体目标跟踪具有跟踪精度高，稳定性好的特性，弥补了单一智能体目标跟踪观测范围有限、观测角度单一等缺点。对于相机等单一的纯角度测量传感器存在能观性差问题，也可以利用多智能体的协同进行解决。
3.在实际场景中，智能体往往自身也在运动，如果智能体自身的定位存在误差，将对目标跟踪精度造成不利影响。在难以利用外界导航物标进行定位的情况下，智能体只能依靠自身的惯导装置进行自身定位，而惯导固有的累积误差将导致自身定位误差越来越大。由于传感器在智能体平台上进行目标测量，而智能体在移动，导致在测量模型中目标跟踪与智能体定位存在耦合，因而惯导累积带来的定位误差将导致目标跟踪精度的下降。在不增加额外传感器的情况下，依靠多智能体配备的雷达或相机对目标的跟踪以及智能体集群间的通信，融合目标跟踪信息，并实现惯导误差的估计和补偿，对提高多智能体定位精度和目标跟踪精度有重要的现实意义。


技术实现要素：

4.针对惯性导航多智能体系统自身定位存在累积误差的问题，本发明提出一种用于目标跟踪背景下的惯导误差补偿方法，包括：
5.统一多智能体的测量数据为距离和角度形式的测量信息；
6.每个智能体在本地执行卡尔曼滤波；
7.融合所述测量信息和卡尔曼滤波的预测信息，进行目标跟踪以获得目标状态估计信息，同时估计惯导误差补偿值；
8.每个智能体与通信范围内的智能体通信所述目标状态估计信息，通过一致性融合获得目标跟踪信息。
9.优选地，所述统一多智能体的测量数据为距离和角度形式的测量信息，包括：
10.获取配备相机的智能体采集的原始图像数据，通过对图像中的目标进行检测，得到目标相对于相机的角度信息；
11.获取配备雷达的智能体采集的点云，通过对原始点云数据进行聚类处理，得到目标相对于雷达的距离和角度信息；
12.以同一时刻相机测量的目标信息和雷达测量的目标信息为输入，得到角度测量的伪距离估计，统一配备相机和雷达的智能体的测量信息都为距离和角度形式。
13.优选地，所述获取配备相机的智能体采集的原始图像数据，通过对图像中的目标进行检测，得到目标相对于相机的角度信息，包括：
14.利用图像的目标检测方法检测目标在图像中的位置；
15.根据所述位置和图像中心点的相对位置关系；
16.得到目标相对于相机的角度。
17.优选地，所述聚类方法包括dbscan和k-means聚类算法，得到测量的目标相对于雷达的距离和角度信息。
18.优选地，所述以同一时刻相机测量的目标信息和雷达测量的目标信息为输入，得到角度测量的伪距离估计，统一配备相机和雷达的智能体的测量信息都为距离和角度形式，需要与其他智能体通信目标的测量信息，包括相机和雷达俩类测量中的至少一类，使用最小二乘法计算目标的位置估计值。
19.优选地，所述伪距离估计，包括：
20.对于配备相机的智能体i，根据其目标位置和相机的角度测量，经过变换可以得到线性方程组：
[0021][0022]
其中分别表示智能体i相机测量得到目标相对于自身的方位角和高低角，(xi,yi,zi)表示智能体i的位置，(x,y,z)表示目标位置；
[0023]
将所有相机测量的线性方程组联立记为：
[0024]ac
x＝bc[0025]
对于配备雷达的智能体j，根据其位置和雷达测量，得到线性方程组
[0026][0027]
其中分别表示智能体j雷达测量的目标相对于自身的距离、方位角和高低角，(xj,yj,zj)表示智能体j的位置，(x,y,z)表示目标位置；
[0028]
将所有雷达测量的方程组联立记为：
[0029]al
x＝b
l
[0030]
联立相机和雷达测量的方程组得到：
[0031][0032]
利用最小二乘法求解目标位置估计值：
[0033][0034]
计算配备相机的智能体i到的目标的伪距离测量估计
[0035]
[0036]
其中‖
·
‖表示欧几里得范数，xi智能体i的位置。
[0037]
优选地，所述卡尔曼滤波包括线性和非线性卡尔曼滤波方法。
[0038]
所述每个智能体与通信范围内的智能体通信所述目标状态估计信息，所述融合测量信息和卡尔曼滤波的预测信息；进行目标跟踪以获得目标状态估计信息，同时估计惯导误差补偿值。以目标状态和惯导误差补偿值作为状态变量，步骤包括：
[0039]
a.采样过程：对于k-1时刻状态变量估计值和协方差p
k-1|k-1
，通过ut变换得到sigma点集合其中l为采样点个数，为相应采样点的权重；
[0040]
b.预测过程：给定状态方程f(
·
)，测量方程h(
·
)和过程噪声方差矩阵q
t
；
[0041][0042][0043][0044][0045][0046]
通过预测过程得到了k时刻的状态变量的预测值状态变量协方差p
k|k-1
，和对测量值的预测
[0047]
c.测量更新：给定测量噪声方差矩阵r
t
：
[0048][0049][0050][0051][0052][0053]
通过测量更新得到了k时刻的状态变量的估计值和状态变量协方差p
k|k
。
[0054]
优选地，所述每个智能体与通信范围内的智能体通信所述目标状态估计信息，通过一致性融合获得目标跟踪信息，包括：
[0055]
每个智能体的状态变量x包含目标状态xa和惯导误差补偿值xb两部分，本步骤仅对
目标状态xa进行一致性融合。每个智能体将目标在k时刻的后验状态估计和协方差转换为信息向量和信息矩阵的形式：
[0056][0057][0058]
智能体i向通信邻域的智能体发送并与通信邻域智能体传来的信息向量和信息矩阵进行一致性加权融合：
[0059][0060][0061]
τ
←
τ+1
[0062]
其中w为一致性权重，ni为智能体i邻域智能体的序号的集合，τ为一致性迭代次数。
[0063]
得到本地状态估计(目标跟踪信息)和协方差：
[0064][0065][0066]
根据本发明的第二个方面，提供一种惯性导航多移动智能体系统在目标跟踪应用中的惯导误差补偿系统，包括：
[0067]
图像数据处理模块，其获取配备相机的智能体采集的原始图像数据，通过对图像中的目标进行检测，得到目标相对于相机的角度信息。
[0068]
点云数据处理模块，其获取配备雷达的智能体采集的点云，通过对原始点云数据进行聚类处理，得到目标相对于雷达的距离和角度信息；
[0069]
角度测量的伪距离估计模块，其以同一时刻相机测量的目标信息和雷达测量的目标信息为输入，得到角度测量的伪距离估计，统一配备相机和雷达的智能体的测量信息都为距离和角度形式；
[0070]
基于分布式卡尔曼滤波的目标跟踪模块，其以目标的距离和角度作为输入信息，每个智能体在本地执行卡尔曼滤波；融合卡尔曼滤波的预测信息和所述输入信息，进行目标跟踪以获得目标状态估计信息，同时估计惯导误差补偿值；
[0071]
所述目标状态一致性融合模块，每个智能体仅与通信范围内的智能体通信所述目标状态估计信息，通过一致性融合获得目标跟踪信息。
[0072]
本发明相对于现有技术，其有益效果为：
[0073]
本发明能够应用于数量不定、具有两种传感器类型的多移动智能体在目标跟踪中的惯导误差补偿方法和系统，可以融合雷达与相机的目标跟踪信息，一定程度上抑制惯导误差的累积，实现多智能体系统在目标跟踪应用中的惯导误差补偿，进而提高目标跟踪精度；其仅依靠智能体集群对目标的跟踪实现多智能体惯导定位误差的补偿与目标的稳定跟
踪。
附图说明
[0074]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0075]
图1是本发明一实施例的多移动智能体在目标跟踪中的惯导误差补偿方法和系统应用于目标跟踪场景中；
[0076]
图2是本发明一实施例的多移动智能体在目标跟踪中的惯导误差补偿方法的流程图；
[0077]
图3是本发明一实施例的多智能体定位均方根误差对比；
[0078]
图4是本发明一实施例的多智能体目标跟踪均方根误差对比。
[0079]
其中，1为雷达，2为智能体，3为惯性测量单元，4为相机，5为目标。
具体实施方式
[0080]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
[0081]
参照图1所示，是本发明一实施例的多移动智能体在目标跟踪中的惯导误差补偿方法和系统应用于目标跟踪场景中的示意图，图中包括雷达1、智能体2、惯性测量单元3、相机4、目标5。其中对于配备相机的智能体：将相机4固定在智能体2上，智能体2内置惯性测量单元3，配备相机的智能体数量为2；对于配备雷达的智能体：将雷达1固定在智能体2上，智能体2内置惯性测量单元3，惯性测量单元能够测量智能体本体的速度，进而根据速度和时间推算出当前所在位置；配备雷达的智能体数量为2。所有智能体共同跟踪目标5，同时向目标5移动，环境中没有参考锚点，移动过程仅依靠惯性测量单元进行定位。在这一实施例中，利用惯性导航多智能体系统的惯导误差补偿方法，能够一定程度上抑制惯导误差的累积，实现多智能体系统自身定位的惯导误差补偿，进而提高目标跟踪精度。
[0082]
在一优选实施例中，用于多移动智能体在目标跟踪中的惯导误差补偿方法和系统应用于目标跟踪场景的流程图如图2所示。具体过程为：
[0083]
s100所有智能体使用各自的传感器跟踪目标。配备相机的智能体采集目标的图像，通过对图像的预处理和目标检测方法，检测目标在图像中的位置，进而得到目标相对于相机的角度；配备雷达的智能体采集目标的点云，通过对点云的聚类处理，得到雷达测量的目标距离和角度信息。
[0084]
s200配备相机的智能体利用伪距离估计，将测量统一为距离和角度的形式。具体的，包括：
[0085]
s201与其他智能体通信目标的测量信息，包括相机和雷达俩类测量中的至少一类；
[0086]
对于配备相机的智能体i，根据其位置和相机的角度测量，经过变换可以得到以下线性方程组：
[0087][0088]
其中分别表示智能体i相机测量得到目标相对于自身的方位角和高低角，(xi,yi,zi)表示智能体i的位置，(x,y,z)表示目标位置。将所有相机测量的方程组联立记为：
[0089]ac
x＝bc[0090]
对于配备雷达的智能体j，根据其位置和雷达测量，可以得到以下线性方程组
[0091][0092]
其中分别表示智能体j雷达测量的目标相对于自身的距离、方位角和高低角，(xj,yj,zj)表示智能体j的位置，(x,y,z)表示目标位置。将所有雷达测量的方程组联立记为：
[0093]al
x＝b
l
[0094]
再联立相机和雷达测量的方程组得到：
[0095][0096]
s202使用最小二乘法计算目标的位置估计值：
[0097][0098]
s203计算配备相机的智能体i到的目标的伪距离测量估计
[0099][0100]
其中‖
·
‖表示欧几里得范数，xi智能体i的位置。
[0101]
利用本实施例上述方法，解决相机测量缺少距离信息造成的目标跟踪能观性差的问题；利用多智能体合作，将测量信息统一为距离和角度形式，有助于进一步的目标跟踪和信息融合。
[0102]
s300每个智能体在本地利用无迹卡尔曼滤波对目标进行跟踪，以融合预测模型和相机或雷达的测量信息。将目标状态(指目标在xyz三个方向上的位置和速度)与惯导误差补偿值作为状态变量，对目标状态与惯导误差补偿值进行估计。具体的，包括：
[0103]
无迹卡尔曼滤波是一种非线性卡尔曼滤波方法，其步骤包括采样过程、预测过程和更新过程，在k时刻，无迹卡尔曼滤波的步骤如下所示：
[0104]
a.采样过程，对于k-1时刻状态变量估计值和协方差p
k-1|k-1
，通过ut变换得到sigma点集合其中l为采样点个数，为相应采样点的
权重；
[0105]
b.预测过程，给定状态方程f(
·
)，测量方程h(
·
)和过程噪声方差矩阵q
t
，公式如下：
[0106][0107][0108][0109][0110][0111]
通过预测过程得到了k时刻的状态变量的预测值状态变量协方差p
k|k-1
，和对测量值的预测
[0112]
c.测量更新，给定测量噪声方差矩阵r
t
，公式如下：
[0113][0114][0115][0116][0117][0118]
利用无迹卡尔曼滤波方法，得到了k时刻的状态变量的估计值和协方差p
k|k
。
[0119]
本实施例将目标状态和惯导误差补偿值共同作为卡尔曼滤波的状态变量，二者在该系统中具有相关性；在状态变量中引入并估计惯导误差补偿值，意味着对目标状态的估计也更加准确。
[0120]
s400智能体与通信范围内的智能体通信目标状态信息，分布式地对目标状态信息进行一致性融合，从而实现集群的合作目标跟踪。具体的，包括:
[0121]
s401每个智能体将目标在k时刻的后验状态估计和协方差转换为信息向量和信息矩阵的形式,以智能体i为例：
[0122][0123]
[0124]
s402智能体i向通信邻域的智能体发送并与通信邻域智能体传来的信息向量和信息矩阵进行一致性加权融合：
[0125][0126][0127]
τ
←
τ+1
[0128]
其中w为一致性权重，ni为智能体i邻域智能体的序号的集合，τ为一致性迭代次数。
[0129]
s403得到本地状态估计和协方差：
[0130][0131][0132]
本发明提供一个优选实施例验证本发明提供的实施例的有效性，对效果进行评估。
[0133]
智能体定位的评价指标为所有智能体的平均定位均方根误差，即：
[0134][0135]
其中表示第m次蒙特卡洛仿真中智能体n位置真实值，真实值由仿真程序生成，表示的估计值，m表示蒙特卡洛仿真总次数，n表示智能体总数。
[0136]
目标跟踪的评价指标为均方根误差，即：
[0137][0138]
其中表示第m次蒙特卡洛仿真中目标位置真实值，真实值由仿真程序生成，表示的估计值，m表示蒙特卡洛仿真总次数。
[0139]
在本发明的应用实施例中，进行100次蒙特卡洛仿真实验，移动智能体集群定位均方根误差对比如图3所示，移动智能体集群目标跟踪均方根误差对比如图4所示。结果表明，在该实施例的背景下，本发明能够有效降低移动智能体集群的定位均方根误差和目标跟踪均方根误差。
[0140]
本发明上述实施例的用于多智能体系统在目标跟踪应用中的惯导误差补偿方法，可以提高惯性导航智能体集群的定位精度，进而提高目标跟踪精度，在许多实际场景中具有应用前景。
[0141]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影
响本发明的实质内容。