一种多毫米波雷达协同定位跟踪方法技术

本发明专利技术公开了一种多毫米波雷达协同定位跟踪方法。为了克服现有技术存在因多径现象产生的虚拟目标的问题；本发明专利技术包括以下步骤：S1：安装多毫米波雷达协同定位跟踪系统，在定位场景中设置若干波束范围存在重叠的雷达；S2：根据多雷达坐标系参数标定算法，获得各雷达坐标与绝对坐标系的旋转角与平移矩阵；S3：基于雷达坐标系与绝对坐标系的旋转角与平移矩阵，将雷达探测到的点云信息映射到绝对坐标系中；S4：根据雷达探测到的点云信息与预测轨迹点的距离，计算轨迹信息。通过点与轨迹的匹配，在一定程度上能够消除多径、扰动产生的孤立点。扰动产生的孤立点。扰动产生的孤立点。

【技术实现步骤摘要】
一种多毫米波雷达协同定位跟踪方法


[0001]本专利技术涉及一种雷达定位领域，尤其涉及一种多毫米波雷达协同定位跟踪方法。

技术介绍

[0002]随着智慧城市，智能家居，智慧物联网等概念的升温，智能家居依赖定位与跟踪技术能给用户带来智能化的体验，提升用户品质生活；同样智慧交通依赖定位与跟踪技术对市民的安全和城市交通秩序维护带来了新的变革。现有定位跟踪技术往往依赖单一传感器进行监控，而单一传感器往往受到环境，功能，作用范围等限制，其适用场景有限。进一步针对单毫米波雷达定位跟踪系统，因其作用距离短、作用范围小、在室内因多径现象会产生的一些虚拟目标等缺点。
[0003]例如，一种在中国专利文献上公开的“基于多径利用的雷达目标定位方法”，其公告号CN107918115B，包括：1.产生发射信号得到回波数据；2.对回波数据处理得到不同路径的波达时间；3.建立雷达目标几何定位模型；4.将得到的不同路径波达时间带入建立的几何定位模型中；5.初始化起始搜索点；6.将几何定位模型与起始搜索点输入到搜索函数lsqnonlin，利用搜索函数lsqnonlin，搜索得到准确的目标位置；7.将搜索得到目标位置扩展到三维空间。该方案因多径现象产生的虚拟目标等缺点。

技术实现思路

[0004]本专利技术主要解决现有技术存在因多径现象产生的虚拟目标的问题；提供一种多毫米波雷达协同定位跟踪方法，消除虚警，目标短暂丢失重续，大范围连贯跟踪。
[0005]本专利技术的上述技术问题主要是通过下述技术方案得以解决的：一种多毫米波雷达协同定位跟踪方法，包括以下步骤：S1：安装多毫米波雷达协同定位跟踪系统，在定位场景中设置若干波束范围存在重叠的雷达；S2：根据多雷达坐标系参数标定算法，获得各雷达坐标与绝对坐标系的旋转角与平移矩阵；S3：基于雷达坐标系与绝对坐标系的旋转角与平移矩阵，将雷达探测到的点云信息映射到绝对坐标系中；S4：根据雷达探测到的点云信息与预测轨迹点的距离，计算轨迹信息。
[0006]本方案通过点与轨迹的匹配，在一定程度上能够消除多径、扰动产生的孤立点；通过多雷达的协同，能够扩大探测范围实现目标定位与跟踪；通过部分雷达的协同能够实现遮挡下的目标跟踪；根据目标跟踪轨迹的预测，能够在一定程度上做到目标短暂丢失后的重续。
[0007]作为优选，所述的多毫米波雷达协同定位跟踪系统包括通信连接的监控单元和若干探测单元；所述探测单元包括雷达、单片机和通讯模块，雷达与单片机的信号输入端连接，通讯模块与单片机的通信端口连接；通讯模块至少有一个为通信网关，用于将其他探测
单元的雷达数据发送至监控单元。毫米波雷达为RC7701N32；通讯模块可以是蓝牙、wifi或lora等；监控单元负责数据收集，多毫米波雷达的坐标标定，目标跟踪，显示界面等功能。
[0008]作为优选，以A雷达坐标系作为绝对坐标系，B雷达坐标系下的点映射到A雷达坐标系的旋转角矩阵为：其中，θ为旋转角；平移矩阵为：其中，t
BAx
为x轴平移量；t
BAy
为y轴平移量。
[0009]作为优选，B雷达坐标系下的点映射到A雷达下的点的表示为：A
Bi
(R
BA
，T
BA
)＝R
BA
B
i
+T
BA
其中，B
i
为B雷达坐标系下第i个点的坐标。
[0010]由于每个独立的毫米波雷达，探测到的点是基于它自己的坐标系，而不是绝对坐标系下的点。因此为了实现目标的定位，就需要先标定各毫米波雷达的坐标系变换参数。即以其中一个雷达为绝对坐标系，将另一个雷达的坐标系通过旋转平移映射到绝对坐标系之下。
[0011]作为优选，建立目标函数：其中，A
i
为A雷达坐标系下第i个点的坐标；n为雷达探测到的点的总数。
[0012]目标函数是求B雷达坐标系下轨迹点映射到A雷达坐标系下的轨迹点与A雷达采样到的轨迹点尽量接近，以均方根误差衡量。
[0013]作为优选，根据目标函数获得目标函数对旋转角与平移量的偏导数；目标函数对旋转角的偏导数为：其中，R
′
BA
为R
BA
对θ的导数：目标函数对平移量的偏导数为：
根据目标函数对旋转角与平移量的偏导数，获得旋转角度与平移量的迭代公式：根据目标函数对旋转角与平移量的偏导数，获得旋转角度与平移量的迭代公式：其中，m是迭代步数；η是旋转角θ的迭代步长；μ
BA
是平移矩阵的迭代步长。
[0014]作为优选，所述的步骤S4包括以下步骤：S401：实时获取雷达采集到的点云数据，雷达点云数据集为S0；S402：将雷达点云数据集S0中的每个点映射到绝对坐标系下，映射后的点云集合为S1；S403：计算点云集合S1中的点P
i
与所有轨迹预测点的距离，若小于阈值则将该点加入到轨迹的探测点集，若大于阈值则放入孤立点集；S404：将轨迹的探测点集的平均值定为轨迹的探测点，将该探测点纳入轨迹的历史轨迹点；S405：根据每条轨迹的历史轨迹点，采用卡尔曼滤波算法计算轨迹的预测点；S406：对孤立点集中的所有点，采用DBscan聚类算法，获得若干类，计算每个类的平均值定为新的轨迹预测点；S407：判断每条轨迹的上次更新时间与当前时间的时间差是否大于时间阈值，若大于时间阈值则判定为轨迹丢失，删除该轨迹信息；S408：返回步骤S401。
[0015]通过点与轨迹的匹配，在一定程度上能够消除多径、扰动产生的孤立点；通过多雷达的协同，能够扩大探测范围实现目标定位与跟踪；通过部分雷达的协同能够实现遮挡下的目标跟踪；根据目标跟踪轨迹的预测，能够在一定程度上做到目标短暂丢失后的重续。
[0016]作为优选，所述的步骤S403中，记所有轨迹预测点集合U中与点云集合S1中的点P
i
距离最短的点为Q；若点P
i
与点Q的距离小于阈值，则将点P
i
加入到点Q所在轨迹的探测点集中，否则放入孤立点集。够消除多径、扰动产生的孤立点。
[0017]作为优选，所述的步骤S405具体包括以下步骤：依次取一条轨迹，根据该轨迹的历史观测点确定卡尔曼滤波算法的系统参数并计算出卡尔曼估计误差的协方差矩阵；根据卡尔曼滤波算法的系统参数以及卡尔曼估计误差的协方差矩阵，计算出历史轨迹对当前时刻目标出现位置的预测值；计算卡尔曼增益，同时将轨迹的探测点集的均值记为目标当前观测值，根据当前观测值与卡尔曼增益，预测下一时刻目标出现的位置；更新卡尔曼估计误差的协方差矩阵。
[0018]计算轨迹的预测点的具体过程。
[0019]本专利技术的有益效果是：
1.本专利技术通过点与轨迹的匹配，在一定程度上能够消除多径、扰动产生的孤立点；2.通过多雷达的协同，能够扩大探测范围实现目标定位与跟踪；3.通过部分雷达的协同能够实现遮挡下的目标跟踪；4.根据目标跟踪轨迹的预测，能够在一定本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种多毫米波雷达协同定位跟踪方法，其特征在于，包括以下步骤：S1：安装多毫米波雷达协同定位跟踪系统，在定位场景中设置若干波束范围存在重叠的雷达；S2：根据多雷达坐标系参数标定算法，获得各雷达坐标与绝对坐标系的旋转角与平移矩阵；S3：基于雷达坐标系与绝对坐标系的旋转角与平移矩阵，将雷达探测到的点云信息映射到绝对坐标系中；S4：根据雷达探测到的点云信息与预测轨迹点的距离，计算轨迹信息。2.根据权利要求1所述的一种多毫米波雷达协同定位跟踪方法，其特征在于，所述的多毫米波雷达协同定位跟踪系统包括通信连接的监控单元和若干探测单元；所述探测单元包括雷达、单片机和通讯模块，雷达与单片机的信号输入端连接，通讯模块与单片机的通信端口连接；通讯模块至少有一个为通信网关，用于将其他探测单元的雷达数据发送至监控单元。3.根据权利要求1或2所述的一种多毫米波雷达协同定位跟踪方法，其特征在于，以A雷达坐标系作为绝对坐标系，B雷达坐标系下的点映射到A雷达坐标系的旋转角矩阵为：其中，θ为旋转角；平移矩阵为：其中，t
BAx
为x轴平移量；t
BAy
为y轴平移量。4.根据权利要求3所述的一种多毫米波雷达协同定位跟踪方法，其特征在于，B雷达坐标系下的点映射到A雷达下的点的表示为：A
Bi
(R
BA
，T
BA
)＝R
BA
B
i
+T
BA
其中，B
i
为B雷达坐标系下第i个点的坐标。5.根据权利要求4所述的一种多毫米波雷达协同定位跟踪方法，其特征在于，建立目标函数：其中，A
i
为A雷达坐标系下第i个点的坐标；n为雷达探测到的点的总数。6.根据权利要求5所述的一种多毫米波雷达协同定位跟踪方法，其特征在于，根据目标函数获得目标函数对旋转角与平移量的偏导数；目标函数对旋转角的偏导数为：
其中，R
′
BA
为R
BA
对θ的导数：目标函数对平移量的偏导数为：根据目标...

【专利技术属性】
技术研发人员：陈文亮，袁常顺，王俊，向洪，李晨龙，王峰，周杨，罗雨泉，赵建峰，
申请(专利权)人：北京航空航天大学杭州创新研究院，
类型：发明
国别省市：