基于窄带物联网的反制机器人随动式云台自动追踪方法与流程

本发明涉及机器人控制，尤其涉及基于窄带物联网的反制机器人随动式云台自动追踪方法。

背景技术：

1、随着反制机器人在低空目标巡查、区域警戒及移动处置场景中的应用不断增加，依托窄带物联网链路实现目标观测信息传输和随动式云台自动追踪的技术受到广泛关注。现有反制机器人主要依据图像检测结果计算目标位置偏差，并通过固定时延补偿和线性预测控制云台转动，但是在实际应用中普遍存在以下问题：

2、机器人运动引起的姿态变化会使图像坐标与云台坐标之间产生动态偏差，目标观测数据、机器人姿态及云台状态缺少统一时序配准，导致目标视线偏差和变化速度计算不准确，云台容易出现跟踪滞后及往复摆动；窄带物联网链路存在带宽受限、报文重传及缓存排队现象，通信完成时长随信号接收功率和链路状态变化，传统固定时延补偿难以表征通信完成时长与云台机械响应时长共同形成的动作完成时长分布；目标快速移动与通信静默窗叠加时，传统线性外推方法难以同时考虑云台可达范围和视场边界，目标视线预测轨迹容易越出移动视场安全域，造成目标丢失，并降低随动式云台连续追踪的稳定性和准确性。

3、因此，如何提供基于窄带物联网的反制机器人随动式云台自动追踪方法是本领域技术人员亟需解决的问题。

技术实现思路

1、本发明的一个目的在于提出基于窄带物联网的反制机器人随动式云台自动追踪方法，本发明充分利用窄带物联网链路分析、改进型ornstein-uhlenbeck模型和云台预测控制技术，实现目标视线预测及云台连续追踪，具有抗通信时延能力强、追踪稳定性高和控制精度高的优点。

2、根据本发明实施例的基于窄带物联网的反制机器人随动式云台自动追踪方法，包括如下步骤：

3、步骤一：采集目标观测数据和窄带物联网链路数据，获取机器人姿态及云台视场参数与云台状态，形成追踪数据集；

4、步骤二：对追踪数据集执行时序配准和坐标映射，生成目标视线偏差和变化速度构成的指向误差状态；

5、步骤三：以指向误差状态构建改进型ornstein-uhlenbeck模型，由追踪数据集生成动作完成时长分布；

6、步骤四：依据动作完成时长分布和云台状态构建终端捕获域，生成终端捕获分布；

7、步骤五：基于动作完成时长分布和终端捕获分布处理模型转移密度，生成终端捕获势函数，由终端捕获势函数状态梯度重构漂移项；

8、步骤六：由追踪数据集生成有效报文到达分布，依据指向误差状态和云台视场参数构建移动视场安全域；

9、步骤七：计算指向误差状态离开移动视场安全域的首次退出时间，结合有效报文到达分布生成通信静默窗生存函数，由通信静默窗生存函数状态梯度更新漂移项；

10、步骤八：按漂移项求解模型，生成目标视线预测轨迹并转换为云台控制量，驱动云台追踪目标。

11、可选的，所述步骤一具体为：

12、采集目标中心像素坐标和目标框尺度并记录目标采样时刻，将目标中心像素坐标及目标框尺度与目标采样时刻关联，形成目标观测数据；

13、记录窄带物联网报文的发送时刻和接收时刻，采集报文重传次数、信号接收功率及缓存排队时长，形成窄带物联网链路数据；

14、采集机器人航向角、机器人俯仰角及机器人横滚角，形成机器人姿态，获取云台水平视场角及云台垂直视场角，形成云台视场参数，采集云台方位角、云台俯仰角、云台角速度及云台角加速度，形成云台状态；

15、依据目标采样时刻、报文发送时刻及报文接收时刻关联目标观测数据和窄带物联网链路数据，并将机器人姿态、云台视场参数及云台状态绑定至目标观测数据，形成追踪数据集。

16、可选的，所述步骤二具体为：

17、按照目标采样时刻对追踪数据集进行排序，将目标中心像素坐标与对应的机器人姿态及云台状态建立时刻关联，生成时序配准记录；

18、计算目标中心像素坐标相对成像中心的水平像素偏移量及垂直像素偏移量，按照云台视场参数将水平像素偏移量及垂直像素偏移量转换为目标视线方位角及目标视线俯仰角，生成目标视线角；

19、将机器人姿态转换为姿态旋转矩阵，采用姿态旋转矩阵对目标视线角执行坐标旋转，将坐标旋转结果与云台状态中的云台方位角及云台俯仰角执行角度差计算，生成目标视线偏差；

20、计算连续目标采样时刻对应的目标视线偏差之差及目标采样时刻之差，以目标视线偏差之差除以目标采样时刻之差生成变化速度，将目标视线偏差和变化速度按照目标采样时刻组合，生成指向误差状态。

21、可选的，所述步骤三具体为：

22、按照目标采样时刻排列指向误差状态，将目标视线偏差和变化速度组合为状态向量序列，计算状态向量序列的均值回归中心及相邻状态向量增量；

23、以状态向量偏离均值回归中心的偏差作为自变量，以相邻状态向量增量作为因变量执行最小二乘拟合，生成漂移矩阵，计算拟合误差协方差并分解生成扩散矩阵，将均值回归中心、漂移矩阵和扩散矩阵写入ornstein-uhlenbeck状态方程，构建改进型ornstein-uhlenbeck模型并生成模型转移密度；

24、从追踪数据集提取目标采样时刻、报文接收时刻、报文重传次数、信号接收功率及云台状态，以报文接收时刻减去目标采样时刻生成通信完成时长，基于目标视线偏差和云台状态计算云台角位移达到目标视线偏差时的云台机械响应时长；

25、将通信完成时长和云台机械响应时长相加生成动作完成时长样本，按照报文重传次数取值和信号接收功率区间划分动作完成时长样本，统计每一划分区间内动作完成时长样本的出现频次并执行概率归一化，生成动作完成时长分布。

26、可选的，所述步骤四具体为：

27、从动作完成时长分布中提取动作完成时长区间及区间概率，利用云台方位角、云台俯仰角、云台角速度及云台角加速度计算动作完成时长区间对应的方位可达角度、俯仰可达角度、方位可达速度及俯仰可达速度；

28、将目标视线偏差分别与方位可达角度及俯仰可达角度进行比较，将变化速度分别与方位可达速度及俯仰可达速度进行比较，筛选满足角度可达条件和速度可达条件的指向误差状态，生成捕获子域；

29、按照动作完成时长区间归并捕获子域，将区间概率配置至对应捕获子域，生成由目标视线偏差范围、变化速度范围及区间概率构成的终端捕获域；

30、计算终端捕获域内每一指向误差状态对应的目标视线偏差幅值及变化速度幅值，根据目标视线偏差幅值及变化速度幅值生成状态权重，将状态权重与区间概率相乘并执行概率归一化，生成终端捕获分布。

31、可选的，所述步骤五具体为：

32、从动作完成时长分布中提取动作完成时长区间及区间概率，从终端捕获分布中提取终端捕获域内指向误差状态对应的状态概率；

33、以当前指向误差状态作为起始状态，以终端捕获域内指向误差状态作为终止状态，将动作完成时长区间代入模型转移密度，计算起始状态转移至终止状态的状态转移概率；

34、将状态转移概率、状态概率及区间概率相乘，按照终端捕获域和动作完成时长区间进行求和，生成终端捕获势函数，对终端捕获势函数相对于指向误差状态每一维分量进行偏导计算，生成终端捕获势函数状态梯度；

35、将扩散矩阵与扩散矩阵的转置矩阵相乘，将乘积矩阵与终端捕获势函数状态梯度相乘，生成终端捕获漂移增量，将终端捕获漂移增量叠加至漂移矩阵作用于指向误差状态与均值回归中心差值形成的基础漂移量，完成漂移项重构。

36、可选的，所述模型转移密度以起始指向误差状态、终止指向误差状态及状态转移时长作为输入，依据均值回归中心、漂移矩阵及起始指向误差状态计算状态转移均值，依据漂移矩阵、扩散矩阵及状态转移时长计算状态转移协方差，根据终止指向误差状态与状态转移均值之间的状态偏差及状态转移协方差计算概率密度，用于表征起始指向误差状态在状态转移时长内转移至终止指向误差状态的概率密度。

37、可选的，所述步骤六具体为：

38、从追踪数据集中提取目标采样时刻及报文接收时刻，将报文接收时刻与目标采样时刻进行时序匹配，保留与目标观测数据建立关联的报文，生成有效报文记录；

39、按照报文接收时刻排列有效报文记录，计算相邻报文接收时刻之差，生成有效报文到达时长样本，按照报文重传次数取值及信号接收功率区间划分有效报文到达时长样本，统计有效报文到达时长样本的出现频次并执行概率归一化，生成有效报文到达分布；

40、从指向误差状态中提取目标视线偏差及变化速度，将云台水平视场角的一半确定为方位视场边界，将云台垂直视场角的一半确定为俯仰视场边界；

41、将有效报文到达分布中的到达时长区间与变化速度相乘，生成预测偏移量，将预测偏移量与目标视线偏差相加，保留相加结果位于方位视场边界及俯仰视场边界内的指向误差状态，并按照到达时长区间及区间概率归并指向误差状态，生成移动视场安全域。

42、可选的，所述步骤七具体为：

43、以当前指向误差状态作为初始状态，采用改进型ornstein-uhlenbeck模型的重构后的漂移项及扩散矩阵推进指向误差状态，比较各推进时刻的指向误差状态与移动视场安全域边界，将指向误差状态首次越过移动视场安全域边界的推进时刻确定为首次退出时间；

44、从有效报文到达分布中提取到达时长区间及区间概率，以首次退出时间不小于到达时长区间上限值作为生存条件，基于模型转移密度计算生存条件对应的生存概率，将生存概率与区间概率相乘并按照到达时长区间求和，生成通信静默窗生存函数；

45、对通信静默窗生存函数执行自然对数变换，计算自然对数变换结果相对于指向误差状态各维分量的偏导，生成通信静默窗生存函数状态梯度；

46、将扩散矩阵与扩散矩阵的转置矩阵相乘，将乘积矩阵与通信静默窗生存函数状态梯度相乘，生成视场生存漂移增量，将视场生存漂移增量叠加至重构后的漂移项，完成漂移项更新。

47、可选的，所述步骤八具体为：

48、以当前指向误差状态作为初始状态，将更新后的漂移项及扩散矩阵代入改进型ornstein-uhlenbeck模型，按照目标采样时刻间隔在每一动作完成时长区间内推进指向误差状态，生成动作完成时长区间对应的预测指向误差状态序列；

49、从预测指向误差状态序列中提取目标视线偏差，将目标视线偏差与云台方位角及云台俯仰角进行角度叠加，生成动作完成时长区间对应的目标视线角序列，采用区间概率对目标视线角序列进行加权汇总，生成目标视线预测轨迹；

50、计算目标视线预测轨迹中相邻目标视线角之间的角度差及目标采样时刻之差，以角度差除以目标采样时刻之差生成云台角速度，将目标视线预测轨迹中的目标视线角和云台角速度组合，生成云台控制量；

51、将云台控制量输入云台，控制云台方位角及云台俯仰角按照目标视线预测轨迹变化，并按照云台角速度调节转动过程，驱动云台追踪目标。

52、本发明的有益效果是：

53、本发明通过对目标观测数据、窄带物联网链路数据、机器人姿态、云台视场参数及云台状态进行时序配准和坐标映射，建立由目标视线偏差和变化速度构成的指向误差状态，并采用改进型ornstein-uhlenbeck模型描述目标视线状态的随机演化过程。通过通信完成时长与云台机械响应时长构建动作完成时长分布，再结合云台可达角度、云台可达速度及终端捕获分布生成终端捕获势函数，能够将链路传输时延、云台机械响应能力及目标运动状态统一纳入预测过程，降低固定时延补偿和线性外推造成的追踪滞后及往复摆动，提高目标视线预测结果与云台实际执行能力之间的匹配程度。

54、本发明进一步依据有效报文到达分布构建移动视场安全域，通过首次退出时间和通信静默窗生存函数对漂移项进行更新，使改进型ornstein-uhlenbeck模型在窄带物联网报文间隔增大、报文重传及信号接收功率变化条件下，仍能够约束指向误差状态位于云台视场范围内。更新后的漂移项用于生成目标视线预测轨迹并转换为云台控制量，从而在机器人姿态变化和通信链路波动环境中保持云台连续转动与目标稳定跟踪，减少目标越出视场引起的跟踪中断，提高随动式云台自动追踪的稳定性、准确性及抗通信时延能力。