基于交通移动锥的UWB追踪跟随系统和追踪跟随方法与流程

本发明属于机器人，具体涉及基于交通移动锥的uwb追踪跟随系统和追踪跟随方法。

背景技术：

1、随着交通安全管理和智能交通系统的不断发展，精准的交通标志物与障碍物定位与追踪变得愈加重要。在施工区域、事故现场、临时道路封闭或自动驾驶等应用场景中，交通移动锥作为一种常见的交通设施，广泛用于警示驾驶员、行人及施工人员注意道路上的潜在危险或限制。然而，传统的交通锥部署方式主要依赖人工设置和人工监控，无法实现对交通锥的控制追踪，难以有效应对快速变化的交通状况。

2、为了解决这些问题，出现了uwb技术，uwb技术凭借其高精度的定位特性，尤其在较短距离内可以实现厘米级的定位精度，成为实现实时、精准追踪的理想选择，系统通过控制头车的移动，并利用uwb技术进行定位与数据传输，后续车辆根据头车的移动轨迹进行同步移动，当交通锥随着车队的移动而调整位置时，uwb系统能够实时追踪并反馈交通锥的位置，不仅能够提升交通锥位置的实时监控能力，也能够有效减少人工干预，实现自动化、智能化的交通安全管理；然而，在实际操作的过程中，交通锥的移动路径可能由于车辆或环境变化而发生偏离，导致行驶路线未按照先前发送的指令进行移动，使得系统追踪跟随的精准度低。

技术实现思路

1、本发明的目的就在于解决交通锥的移动路径可能由于车辆或环境变化而发生偏离，导致行驶路线未按照先前发送的指令进行移动，使得系统追踪跟随的精准度低的问题，而提出一种基于交通移动锥的uwb追踪跟随系统和追踪跟随方法。

2、在本发明实施的第一方面，首先提出一种基于交通移动锥的uwb追踪跟随系统，uwb追踪跟随系统包括中控单元和移动锥队列，所述移动锥队列包括头移动锥和多个跟踪移动锥，头移动锥和每个跟踪移动锥初始相同方向，以第一预设距离间隔排列放置，所述移动锥队列中的移动锥都安装有两个uwb标签和i mu模块，其特征在于，所述系统包括：

3、所述中控单元生成初始控制指令，并将所述初始控制指令发送给所述头移动锥；

4、所述头移动锥根据所述初始控制指令进行移动，并在移动过程中进行实时避障检测修正所述初始控制指令得到修正控制指令，根据所述修正控制指令进行移动，并将所述修正控制指令发送给相邻的下一跟踪移动锥；

5、目标移动锥根据与所述头移动锥的初始距离确定自身控制指令，并根据所述自身控制指令和所述修正控制指令进行移动，并根据移动路径进行路径修正得到自身修正控制指令，根据所述自身修正控制指令进行移动，若所述目标移动锥存在相邻的下一移动锥，则将所述修正控制指令发送给相邻的下一移动锥；所述目标移动锥为跟踪移动锥中的任意一个。

6、在本发明实施的第二方面，提出一种基于交通移动锥的uwb追踪跟随方法，uwb追踪跟随系统包括中控单元和移动锥队列，所述移动锥队列包括头移动锥和多个跟踪移动锥，头移动锥和每个跟踪移动锥初始相同方向，以第一预设距离间隔排列放置，所述移动锥队列中的移动锥都安装有两个uwb标签和imu模块，其特征在于，所述方法应用于头移动锥包括：

7、接收所述中控单元生成初始控制指令，根据所述初始控制指令进行移动，并在移动过程中进行实时避障检测修正所述初始控制指令得到修正控制指令；

8、根据所述修正控制指令进行移动，并将所述修正控制指令发送给相邻的下一跟踪移动锥；以使得目标移动锥根据与所述头移动锥的初始距离确定自身控制指令，并根据所述自身控制指令和所述修正控制指令进行移动，并根据移动路径进行路径修正得到自身修正控制指令，根据所述自身修正控制指令进行移动，若所述目标移动锥存在相邻的下一移动锥，则将所述修正控制指令发送给相邻的下一移动锥；所述目标移动锥为跟踪移动锥中的任意一个。

9、可选的，根据所述初始控制指令进行移动，并在移动过程中进行实时避障检测修正所述初始控制指令得到修正控制指令包括：

10、通过传感器实时获取当前位置下的障碍信息，根据所述障碍信息建立局部障碍图；

11、根据所述局部障碍图，通过预设算法生成安全路径，实时计算初始控制指令生成的路径与所述安全路径的偏离值，根据所述偏离值生成修正控制指令。

12、可选的，通过传感器实时获取当前位置下的障碍信息，根据所述障碍信息建立局部障碍图包括：

13、通过传感器实时获取当前位置周围的初始环境数据，对所述初始环境数据进行预处理得到第一数据；

14、根据预设区间对所述第一数据进行筛选得到第二数据，将所述第二数据映射到二维网格平面得到局部障碍图。

15、可选的，根据所述局部障碍图，通过预设算法生成安全路径包括：

16、根据所述局部障碍图，判断当前初始控制指令形成的路线是否碰撞障碍，若碰撞，则根据所述局部障碍图生成安全目标点，并通过波前算法计算从当前位置到所述安全目标点的路线得到子路线；

17、根据所述子路线进行移动，并根据所述局部障碍图重复生成安全目标点，直到初始控制指令形成的路线不再碰撞障碍，最终生成安全路径。

18、在本发明实施的第三方面，还提出一种基于交通移动锥的uwb追踪跟随方法，uwb追踪跟随系统包括中控单元和移动锥队列，所述移动锥队列包括头移动锥和多个跟踪移动锥，头移动锥和每个跟踪移动锥初始相同方向，以第一预设距离间隔排列放置，所述移动锥队列中的移动锥都安装有两个uwb标签和imu模块，其特征在于，所述方法应用于目标移动锥，所述目标移动锥为多个跟踪移动锥中的任意一个，所述方法包括：

19、接收所述头移动锥发送的修正控制指令；所述修正控制指令的生成过程为所述头移动锥接收所述中控单元生成初始控制指令，根据所述初始控制指令进行移动，并在移动过程中进行实时避障检测修正所述初始控制指令得到修正控制指令；

20、根据与所述头移动锥的初始距离确定自身控制指令，并根据所述自身控制指令和所述修正控制指令进行移动，并根据移动路径进行路径修正得到自身修正控制指令；

21、根据所述自身修正控制指令进行移动，若所述目标移动锥存在相邻的下一移动锥，则将所述修正控制指令发送给相邻的下一移动锥。

22、可选的，每辆跟踪移动锥启动后，根据自身携带的uwb标签对邻近车辆进行探测，确定相对于该跟踪移动锥对应的前跟踪移动锥和后跟踪移动锥，其特征在于，并根据移动路径进行路径修正得到自身修正控制指令包括：

23、根据uwb标签实时获取所述目标移动锥与前跟踪移动锥和后跟踪移动锥的相对距离得到第一相对距离和第二相对距离；

24、通过i mu模块实时获取所述目标移动锥与前跟踪移动锥和后跟踪移动锥的角度，得到第一姿态和第二姿态；

25、根据所述第一相对距离和所述第二相对距离确定所述目标移动锥位置修正指令；

26、根据所述第一姿态和所述第二姿态确定所述目标移动锥角度修正指令，根据所述位置修正指令和所述角度修正指令得到自身修正控制指令。

27、可选的，根据所述第一相对距离和所述第二相对距离确定所述目标移动锥位置修正指令包括：

28、计算所述第一相对距离和所述第一预设距离的差值得到第一误差，计算所述第二相对距离和所述第一预设距离的差值得到第二误差；

29、若所述第一误差为正数且所述第二误差为负数，则以所述第一误差为位置修正指令；

30、若所述第一误差为负数且所述第二误差为正数，则以所述第二误差为位置修正指令。

31、可选的，根据所述第一姿态和所述第二姿态确定所述目标移动锥角度修正指令包括：

32、计算所述第一姿态和预设第一姿态的差值得到第一姿态误差，计算所述第二姿态和预设第一姿态的差值得到第二姿态误差；

33、若第一姿态误差为正，则以所述第一姿态误差为角度修正指令；

34、若第二姿态误差为正，则以所述第二姿态误差为角度修正指令。

35、本发明的有益效果：

36、本发明提出了一种基于交通移动锥的uwb追踪跟随系统，uwb追踪跟随系统包括中控单元和移动锥队列，移动锥队列包括头移动锥和多个跟踪移动锥，头移动锥和每个跟踪移动锥初始相同方向，以第一预设距离间隔排列放置，移动锥队列中的移动锥都安装有两个uwb标签和imu模块，中控单元生成初始控制指令，并将初始控制指令发送给头移动锥；头移动锥根据初始控制指令进行移动，并在移动过程中进行实时避障检测修正初始控制指令得到修正控制指令，根据修正控制指令进行移动，并将修正控制指令发送给相邻的下一跟踪移动锥；目标移动锥根据与头移动锥的初始距离确定自身控制指令，并根据自身控制指令和修正控制指令进行移动，并根据移动路径进行路径修正得到自身修正控制指令，根据自身修正控制指令进行移动，若目标移动锥存在相邻的下一移动锥，则将修正控制指令发送给相邻的下一移动锥；目标移动锥为跟踪移动锥中的任意一个。通过将i mu模块结合uwb可以增强定位的稳定性，为后续的追踪跟随提供帮助，头移动锥在执行过程中可以实时进行障碍检测并自动调整路径，通过对初始指令的避障修正，可以实现动态路径调整，确保头移动锥能够在复杂环境中灵活绕开障碍物，使得后续的跟踪移动锥都能够避免碰撞到障碍物，再通过对每一移动锥都进行自身路线矫正，提高了移动锥追踪跟随的精准度。