一种多毫米波雷达协同定位跟踪方法
阅读说明：本技术 一种多毫米波雷达协同定位跟踪方法 (Multi-millimeter-wave radar cooperative positioning tracking method ) 是由 陈文亮 袁常顺 王俊 向洪 李晨龙 王峰 周杨 罗雨泉 赵建峰 于 2021-09-09 设计创作，主要内容包括：本发明公开了一种多毫米波雷达协同定位跟踪方法。为了克服现有技术存在因多径现象产生的虚拟目标的问题；本发明包括以下步骤：S1：安装多毫米波雷达协同定位跟踪系统,在定位场景中设置若干波束范围存在重叠的雷达；S2：根据多雷达坐标系参数标定算法,获得各雷达坐标与绝对坐标系的旋转角与平移矩阵；S3：基于雷达坐标系与绝对坐标系的旋转角与平移矩阵,将雷达探测到的点云信息映射到绝对坐标系中；S4：根据雷达探测到的点云信息与预测轨迹点的距离,计算轨迹信息。通过点与轨迹的匹配,在一定程度上能够消除多径、扰动产生的孤立点。(The invention discloses a multi-millimeter wave radar cooperative positioning and tracking method. In order to overcome the problem of virtual target generated by multipath phenomenon in the prior art; the invention comprises the following steps: s1: installing a multi-millimeter wave radar cooperative positioning and tracking system, and setting a plurality of radars with overlapped wave beam ranges in a positioning scene; s2: obtaining rotation angles and translation matrixes of each radar coordinate and an absolute coordinate system according to a multi-radar coordinate system parameter calibration algorithm; s3: mapping point cloud information detected by a radar into an absolute coordinate system based on rotation angles and translation matrixes of the radar coordinate system and the absolute coordinate system; s4: and calculating track information according to the distance between the point cloud information detected by the radar and the predicted track point. Through the matching of the points and the tracks, isolated points generated by multipath and disturbance can be eliminated to a certain extent.)
技术领域
本发明涉及一种雷达定位领域，尤其涉及一种多毫米波雷达协同定位跟踪方法。
背景技术
随着智慧城市，智能家居，智慧物联网等概念的升温，智能家居依赖定位与跟踪技术能给用户带来智能化的体验，提升用户品质生活；同样智慧交通依赖定位与跟踪技术对市民的安全和城市交通秩序维护带来了新的变革。现有定位跟踪技术往往依赖单一传感器进行监控，而单一传感器往往受到环境，功能，作用范围等限制，其适用场景有限。进一步针对单毫米波雷达定位跟踪系统，因其作用距离短、作用范围小、在室内因多径现象会产生的一些虚拟目标等缺点。
例如，一种在中国专利文献上公开的“基于多径利用的雷达目标定位方法”，其公告号CN107918115B，包括：1.产生发射信号得到回波数据；2.对回波数据处理得到不同路径的波达时间；3.建立雷达目标几何定位模型；4.将得到的不同路径波达时间带入建立的几何定位模型中；5.初始化起始搜索点；6.将几何定位模型与起始搜索点输入到搜索函数lsqnonlin，利用搜索函数lsqnonlin，搜索得到准确的目标位置；7.将搜索得到目标位置扩展到三维空间。该方案因多径现象产生的虚拟目标等缺点。
发明内容
本发明主要解决现有技术存在因多径现象产生的虚拟目标的问题；提供一种多毫米波雷达协同定位跟踪方法，消除虚警，目标短暂丢失重续，大范围连贯跟踪。
本发明的上述技术问题主要是通过下述技术方案得以解决的：
一种多毫米波雷达协同定位跟踪方法，包括以下步骤：
S1：安装多毫米波雷达协同定位跟踪系统，在定位场景中设置若干波束范围存在重叠的雷达；
S2：根据多雷达坐标系参数标定算法，获得各雷达坐标与绝对坐标系的旋转角与平移矩阵；
S3：基于雷达坐标系与绝对坐标系的旋转角与平移矩阵，将雷达探测到的点云信息映射到绝对坐标系中；
S4：根据雷达探测到的点云信息与预测轨迹点的距离，计算轨迹信息。
本方案通过点与轨迹的匹配，在一定程度上能够消除多径、扰动产生的孤立点；通过多雷达的协同，能够扩大探测范围实现目标定位与跟踪；通过部分雷达的协同能够实现遮挡下的目标跟踪；根据目标跟踪轨迹的预测，能够在一定程度上做到目标短暂丢失后的重续。
作为优选，所述的多毫米波雷达协同定位跟踪系统包括通信连接的监控单元和若干探测单元；所述探测单元包括雷达、单片机和通讯模块，雷达与单片机的信号输入端连接，通讯模块与单片机的通信端口连接；通讯模块至少有一个为通信网关，用于将其他探测单元的雷达数据发送至监控单元。毫米波雷达为RC7701N32；通讯模块可以是蓝牙、wifi或lora等；监控单元负责数据收集，多毫米波雷达的坐标标定，目标跟踪，显示界面等功能。
作为优选，以A雷达坐标系作为绝对坐标系，B雷达坐标系下的点映射到A雷达坐标系的旋转角矩阵为：
其中，θ为旋转角；
平移矩阵为：
其中，tBAx为x轴平移量；
tBAy为y轴平移量。
作为优选，B雷达坐标系下的点映射到A雷达下的点的表示为：
ABi(RBA，TBA)＝RBABi+TBA
其中，Bi为B雷达坐标系下第i个点的坐标。
由于每个独立的毫米波雷达，探测到的点是基于它自己的坐标系，而不是绝对坐标系下的点。因此为了实现目标的定位，就需要先标定各毫米波雷达的坐标系变换参数。即以其中一个雷达为绝对坐标系，将另一个雷达的坐标系通过旋转平移映射到绝对坐标系之下。
作为优选，建立目标函数：
其中，Ai为A雷达坐标系下第i个点的坐标；
n为雷达探测到的点的总数。
目标函数是求B雷达坐标系下轨迹点映射到A雷达坐标系下的轨迹点与A雷达采样到的轨迹点尽量接近，以均方根误差衡量。
作为优选，根据目标函数获得目标函数对旋转角与平移量的偏导数；
目标函数对旋转角的偏导数为：
其中，R′BA为RBA对θ的导数：
目标函数对平移量的偏导数为：
根据目标函数对旋转角与平移量的偏导数，获得旋转角度与平移量的迭代公式：
其中，m是迭代步数；
η是旋转角θ的迭代步长；
μBA是平移矩阵的迭代步长。
作为优选，所述的步骤S4包括以下步骤：
S401：实时获取雷达采集到的点云数据，雷达点云数据集为S0；
S402：将雷达点云数据集S0中的每个点映射到绝对坐标系下，映射后的点云集合为S1；
S403：计算点云集合S1中的点Pi与所有轨迹预测点的距离，若小于阈值则将该点加入到轨迹的探测点集，若大于阈值则放入孤立点集；
S404：将轨迹的探测点集的平均值定为轨迹的探测点，将该探测点纳入轨迹的历史轨迹点；S405：根据每条轨迹的历史轨迹点，采用卡尔曼滤波算法计算轨迹的预测点；
S406：对孤立点集中的所有点，采用DBscan聚类算法，获得若干类，计算每个类的平均值定为新的轨迹预测点；
S407：判断每条轨迹的上次更新时间与当前时间的时间差是否大于时间阈值，若大于时间阈值则判定为轨迹丢失，删除该轨迹信息；
S408：返回步骤S401。
通过点与轨迹的匹配，在一定程度上能够消除多径、扰动产生的孤立点；通过多雷达的协同，能够扩大探测范围实现目标定位与跟踪；通过部分雷达的协同能够实现遮挡下的目标跟踪；根据目标跟踪轨迹的预测，能够在一定程度上做到目标短暂丢失后的重续。
作为优选，所述的步骤S403中，记所有轨迹预测点集合U中与点云集合S1中的点Pi距离最短的点为Q；若点Pi与点Q的距离小于阈值，则将点Pi加入到点Q所在轨迹的探测点集中，否则放入孤立点集。够消除多径、扰动产生的孤立点。
作为优选，所述的步骤S405具体包括以下步骤：
依次取一条轨迹，根据该轨迹的历史观测点确定卡尔曼滤波算法的系统参数并计算出卡尔曼估计误差的协方差矩阵；
根据卡尔曼滤波算法的系统参数以及卡尔曼估计误差的协方差矩阵，计算出历史轨迹对当前时刻目标出现位置的预测值；
计算卡尔曼增益，同时将轨迹的探测点集的均值记为目标当前观测值，根据当前观测值与卡尔曼增益，预测下一时刻目标出现的位置；
更新卡尔曼估计误差的协方差矩阵。
计算轨迹的预测点的具体过程。
本发明的有益效果是：
1.本发明通过点与轨迹的匹配，在一定程度上能够消除多径、扰动产生的孤立点；
2.通过多雷达的协同，能够扩大探测范围实现目标定位与跟踪；
3.通过部分雷达的协同能够实现遮挡下的目标跟踪；
4.根据目标跟踪轨迹的预测，能够在一定程度上做到目标短暂丢失后的重续。
附图说明
图1是本发明的一种多毫米波雷达协同定位跟踪方法流程图。
具体实施方式
下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。
实施例：
本实施例的一种多毫米波雷达协同定位跟踪方法，如图1所示，包括以下步骤：
S1：安装多毫米波雷达协同定位跟踪系统，在定位场景中设置若干波束范围存在重叠的雷达。
多毫米波雷达协同定位跟踪系统包括通信连接的监控单元和若干探测单元。
探测单元包括雷达、单片机和通讯模块，雷达与单片机的信号输入端连接，通讯模块与单片机的通信端口连接；通讯模块至少有一个为通信网关，用于将其他探测单元的雷达数据发送至监控单元。
在本实施例中，毫米波雷达为RC7701N32；通讯模块可以是蓝牙、wifi或lora等；监控单元负责数据收集，多毫米波雷达的坐标标定，目标跟踪，显示界面等功能。
S2：根据多雷达坐标系参数标定算法，获得各雷达坐标与绝对坐标系的旋转角与平移矩阵。
由于每个独立的毫米波雷达，探测到的点是基于它自己的坐标系，而不是绝对坐标系下的点。因此为了实现目标的定位，就需要先标定各毫米波雷达的坐标系变换参数。即以其中一个雷达为绝对坐标系，将另一个雷达的坐标系通过旋转平移映射到绝对坐标系之下。
以A雷达坐标系作为绝对坐标系，B雷达坐标系下的点映射到A雷达坐标系的旋转角矩阵为：
其中，θ为旋转角；
平移矩阵为：
其中，tBAx为x轴平移量；
tBAy为y轴平移量。
B雷达坐标系下的点映射到A雷达下的点的表示为：
ABi(RBA，TBA)＝RBABi+TBA
其中，Bi为B雷达坐标系下第i个点的坐标。
建立目标函数：
其中，Ai为A雷达坐标系下第i个点的坐标；
n为雷达探测到的点的总数。
目标函数是求B雷达坐标系下轨迹点映射到A雷达坐标系下的轨迹点与A雷达采样到的轨迹点尽量接近，以均方根误差衡量。
根据目标函数获得目标函数对旋转角与平移量的偏导数；
目标函数对旋转角的偏导数为：
其中，R′BA为RBA对θ的导数：
目标函数对平移量的偏导数为：
根据目标函数对旋转角与平移量的偏导数，获得旋转角度与平移量的迭代公式：
其中，m是迭代步数；
η是旋转角θ的迭代步长；
μBA是平移矩阵的迭代步长。
S3：基于雷达坐标系与绝对坐标系的旋转角与平移矩阵，将雷达探测到的点云信息映射到绝对坐标系中。
S4：根据雷达探测到的点云信息与预测轨迹点的距离，计算轨迹信息。
S401：实时获取雷达采集到的点云数据，雷达点云数据集为S0。
S402：将雷达点云数据集S0中的每个点映射到绝对坐标系下，映射后的点云集合为S1。
S403：计算点云集合S1中的点Pi与所有轨迹预测点的距离，若小于阈值则将该点加入到轨迹的探测点集，若大于阈值则放入孤立点集。
记所有轨迹预测点集合U中与点云集合S1中的点Pi距离最短的点为Q；若点Pi与点Q的距离小于阈值，则将点Pi加入到点Q所在轨迹的探测点集中，否则放入孤立点集。
S404：将轨迹的探测点集的平均值定为轨迹的探测点，将该探测点纳入轨迹的历史轨迹点。
S405：根据每条轨迹的历史轨迹点，采用卡尔曼滤波算法计算轨迹的预测点。
依次取一条轨迹，根据该轨迹的历史观测点确定卡尔曼滤波算法的系统参数并计算出卡尔曼估计误差的协方差矩阵；
根据卡尔曼滤波算法的系统参数以及卡尔曼估计误差的协方差矩阵，计算出历史轨迹对当前时刻目标出现位置的预测值；
计算卡尔曼增益，同时将轨迹的探测点集的均值记为目标当前观测值，根据当前观测值与卡尔曼增益，预测下一时刻目标出现的位置；
更新卡尔曼估计误差的协方差矩阵。
S406：对孤立点集中的所有点，采用DBscan聚类算法，获得若干类，计算每个类的平均值定为新的轨迹预测点。
这里轨迹预测点与步骤S405当中的轨迹预测点性质上是一样的，只是产生方式不同，举例来说：要形成一条轨迹必然是由多个历史轨迹点构成的，通过历史轨迹来预测目标可能出现的位置，这就是步骤S405中轨迹预测点的意义，而任何轨迹的产生之初，必然是由孤立点不断积累所形成的，通过DBscan聚类算法分出若干类，每个类的中心记为一条新的轨迹预测点，轨迹预测点的用途是在下一帧的时候将雷达探测到的点云进行汇聚用的。在步骤S403中，点云中的点会依次计算与轨迹预测点集合中每个轨迹预测点的距离，所以步骤S403中提到的轨迹预测点是包含这两类轨迹预测点的。
S407：判断每条轨迹的上次更新时间与当前时间的时间差是否大于时间阈值，若大于时间阈值则判定为轨迹丢失，删除该轨迹信息。
S408：返回步骤S401。
本实施例的方案通过点与轨迹的匹配，在一定程度上能够消除多径、扰动产生的孤立点；通过多雷达的协同，能够扩大探测范围实现目标定位与跟踪；通过部分雷达的协同能够实现遮挡下的目标跟踪；根据目标跟踪轨迹的预测，能够在一定程度上做到目标短暂丢失后的重续。
应理解，实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种基于特征的目标跟踪优化方法、装置和存储介质