UWB tracking and following system and tracking and following method based on traffic moving cone

Abstract

The invention discloses a UWB tracking and following system and method based on a traffic moving cone, and relates to the technical field of robots. The head moving cone moves according to the initial control instruction, performs real-time obstacle avoidance detection to correct the initial control instruction to obtain a correction control instruction, moves according to the correction control instruction, and sends the correction control instruction to the next adjacent tracking moving cone; and the next tracking moving cone determines a self control instruction, moves according to the self control instruction and the correction control instruction, performs path correction according to the moving path to obtain a self correction control instruction, and moves according to the self correction control instruction. Obstacle detection and automatic path adjustment are carried out on the head moving cone in the execution process, it is ensured that the head moving cone can flexibly bypass obstacles in the complex environment, follow-up tracking moving cones can be prevented from colliding with the obstacles, and then the tracking and following accuracy of the moving cones is improved by correcting the route of each moving cone.

Description

Technical Field

[0001] The invention belongs to the technical field of robots, and in particular relates to a UWB tracking and following system and a tracking and following method based on traffic moving cones. Background Art

[0002] With the continuous development of traffic safety management and intelligent transportation systems, accurate positioning and tracking of traffic signs and obstacles has become increasingly important. In application scenarios such as construction areas, accident sites, temporary road closures or autonomous driving, traffic cones are a common traffic facility that is widely used to alert drivers, pedestrians and construction workers to potential dangers or restrictions on the road. However, traditional traffic cone deployment methods mainly rely on manual settings and manual monitoring, which cannot achieve control and tracking of traffic cones, and it is difficult to effectively respond to rapidly changing traffic conditions.

[0003] In order to solve these problems, UWB technology has emerged. With its high-precision positioning characteristics, especially the centimeter-level positioning accuracy within a short distance, UWB technology has become an ideal choice for real-time and precise tracking. The system controls the movement of the lead vehicle and uses UWB technology for positioning and data transmission. Subsequent vehicles move synchronously according to the movement trajectory of the lead vehicle. When the traffic cones adjust their positions as the fleet moves, the UWB system can track and feedback the positions of the traffic cones in real time, which can not only improve the real-time monitoring capabilities of the traffic cone positions, but also effectively reduce manual intervention and realize automated and intelligent traffic safety management. However, in the actual operation process, the movement path of the traffic cones may deviate due to changes in the vehicle or environment, resulting in the driving route not moving according to the previously sent instructions, making the system tracking and following with low accuracy. Summary of the invention

[0004] The purpose of the present invention is to solve the problem that the moving path of traffic cones may deviate due to changes in vehicles or environment, resulting in the driving route not moving according to the previously sent instructions, resulting in low accuracy of system tracking and following, and a UWB tracking and following system and tracking and following method based on traffic moving cones are proposed.

[0005] In the first aspect of the implementation of the present invention, a UWB tracking and following system based on traffic mobile cones is first proposed. The UWB tracking and following system includes a central control unit and a mobile cone queue. The mobile cone queue includes a head mobile cone and multiple tracking mobile cones. The head mobile cone and each tracking mobile cone are initially in the same direction and are arranged at a first preset distance. The mobile cones in the mobile cone queue are all equipped with two UWB tags and IMU modules. The system includes:

[0006] The central control unit generates an initial control instruction and sends the initial control instruction to the head moving cone;

[0007] The head moving cone moves according to the initial control instruction, and performs real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain a corrected control instruction, moves according to the corrected control instruction, and sends the corrected control instruction to the next adjacent tracking moving cone;

[0008] The target moving cone determines its own control instruction according to the initial distance from the head moving cone, moves according to the self-control instruction and the correction control instruction, and obtains its own correction control instruction by performing path correction according to the moving path, and moves according to the self-correction control instruction. If the target moving cone has an adjacent next moving cone, the correction control instruction is sent to the adjacent next moving cone; the target moving cone is any one of the tracking moving cones.

[0009] In a second aspect of the implementation of the present invention, a UWB tracking and following method based on traffic moving cones is proposed. The UWB tracking and following system includes a central control unit and a moving cone queue. The moving cone queue includes a head moving cone and a plurality of tracking moving cones. The head moving cone and each tracking moving cone initially have the same direction and are arranged at a first preset distance interval. The moving cones in the moving cone queue are all equipped with two UWB tags and an IMU module. The method is applied to the head moving cone and includes:

[0010] Receiving an initial control instruction generated by the central control unit, moving according to the initial control instruction, and performing real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain a corrected control instruction;

[0011] Move according to the correction control instruction, and send the correction control instruction to the adjacent next tracking moving cone; so that the target moving cone determines its own control instruction according to the initial distance from the head moving cone, and moves according to the self-control instruction and the correction control instruction, and corrects the path according to the moving path to obtain its own correction control instruction, moves according to its own correction control instruction, and if the target moving cone has an adjacent next moving cone, the correction control instruction is sent to the adjacent next moving cone; the target moving cone is any one of the tracking moving cones.

[0012] Optionally, moving according to the initial control instruction and performing real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain a corrected control instruction includes:

[0013] Obtain obstacle information at the current location in real time through sensors, and establish a local obstacle map based on the obstacle information;

[0014] According to the local obstacle map, a safe path is generated by a preset algorithm, a deviation value between the path generated by the initial control instruction and the safe path is calculated in real time, and a corrected control instruction is generated according to the deviation value.

[0015] Optionally, obtaining obstacle information at the current position in real time through a sensor, and establishing a local obstacle map according to the obstacle information includes:

[0016] Acquire initial environmental data around the current position in real time through a sensor, and pre-process the initial environmental data to obtain first data;

[0017] The first data is screened according to a preset interval to obtain second data, and the second data is mapped to a two-dimensional grid plane to obtain a local obstacle map.

[0018] Optionally, generating a safe path by a preset algorithm according to the local obstacle map includes:

[0019] According to the local obstacle map, determine whether the route formed by the current initial control instruction collides with an obstacle. If so, generate a safe target point according to the local obstacle map, and calculate the route from the current position to the safe target point by using a wavefront algorithm to obtain a sub-route;

[0020] Move according to the sub-route, and repeatedly generate safe target points according to the local obstacle map until the route formed by the initial control instruction no longer collides with obstacles, and finally generate a safe path.

[0021] In a third aspect of the implementation of the present invention, a UWB tracking and following method based on traffic moving cones is also proposed. The UWB tracking and following system includes a central control unit and a moving cone queue. The moving cone queue includes a head moving cone and a plurality of tracking moving cones. The head moving cone and each tracking moving cone initially have the same direction and are arranged at a first preset distance interval. The moving cones in the moving cone queue are all equipped with two UWB tags and an IMU module. The method is applied to a target moving cone, which is any one of a plurality of tracking moving cones. The method includes:

[0022] Receive the correction control instruction sent by the head moving cone; the generation process of the correction control instruction is that the head moving cone receives the initial control instruction generated by the central control unit, moves according to the initial control instruction, and performs real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain the correction control instruction;

[0023] Determine a self-control instruction according to an initial distance from the head moving cone, move according to the self-control instruction and the correction control instruction, and perform path correction according to the moving path to obtain the self-correction control instruction;

[0024] The moving cone moves according to the self-correction control instruction, and if the target moving cone has an adjacent next moving cone, the correction control instruction is sent to the adjacent next moving cone.

[0025] Optionally, after each tracking moving cone is started, it detects neighboring vehicles according to the UWB tag it carries, determines the front tracking moving cone and the rear tracking moving cone corresponding to the tracking moving cone, and is characterized in that the path correction is performed according to the moving path to obtain the self-correction control instruction, which includes:

[0026] Acquire the relative distances between the target moving cone and the front tracking moving cone and the rear tracking moving cone in real time according to the UWB tag to obtain a first relative distance and a second relative distance;

[0027] The angles between the target moving cone and the front tracking moving cone and the rear tracking moving cone are acquired in real time through the IMU module to obtain a first posture and a second posture;

[0028] Determine the target moving cone position correction instruction according to the first relative distance and the second relative distance;

[0029] The target movement cone angle correction instruction is determined according to the first posture and the second posture, and the self-correction control instruction is obtained according to the position correction instruction and the angle correction instruction.

[0030] Optionally, determining the target moving cone position correction instruction according to the first relative distance and the second relative distance includes:

[0031] Calculating a difference between the first relative distance and the first preset distance to obtain a first error, and calculating a difference between the second relative distance and the first preset distance to obtain a second error;

[0032] If the first error is a positive number and the second error is a negative number, the first error is used as a position correction instruction;

[0033] If the first error is a negative number and the second error is a positive number, the second error is used as a position correction instruction.

[0034] Optionally, determining the target movement cone angle correction instruction according to the first posture and the second posture includes:

[0035] Calculating the difference between the first posture and the preset first posture to obtain a first posture error, and calculating the difference between the second posture and the preset first posture to obtain a second posture error;

[0036] If the first posture error is positive, the first posture error is used as an angle correction instruction;

[0037] If the second posture error is positive, the second posture error is used as the angle correction instruction.

[0038] Beneficial effects of the present invention:

[0039] The present invention proposes a UWB tracking and following system based on traffic moving cones. The UWB tracking and following system comprises a central control unit and a moving cone queue. The moving cone queue comprises a head moving cone and a plurality of tracking moving cones. The head moving cone and each tracking moving cone initially have the same direction and are arranged at intervals of a first preset distance. The moving cones in the moving cone queue are all equipped with two UWB tags and an IMU module. The central control unit generates an initial control instruction and sends the initial control instruction to the head moving cone; the head moving cone moves according to the initial control instruction, and performs real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain a corrected control instruction, moves according to the corrected control instruction, and sends the corrected control instruction to the adjacent next tracking moving cone; the target moving cone determines its own control instruction according to the initial distance from the head moving cone, moves according to its own control instruction and the corrected control instruction, corrects the path according to the moving path to obtain its own corrected control instruction, moves according to its own corrected control instruction, and if the target moving cone has an adjacent next moving cone, the corrected control instruction is sent to the adjacent next moving cone; the target moving cone is any one of the tracking moving cones. By combining the IMU module with UWB, the stability of positioning can be enhanced, providing assistance for subsequent tracking and following. The head moving cone can detect obstacles in real time and automatically adjust the path during execution. By correcting the obstacle avoidance of the initial instructions, dynamic path adjustment can be achieved to ensure that the head moving cone can flexibly bypass obstacles in complex environments, so that subsequent tracking moving cones can avoid colliding with obstacles. By correcting the route of each moving cone, the accuracy of the moving cone tracking and following is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The present invention will be further described below in conjunction with the accompanying drawings.

[0041] Figure 1 A framework diagram of a UWB tracking and following system based on traffic moving cones is provided for an embodiment of the present invention;

[0042] Figure 2 A flow chart of a UWB tracking and following method based on traffic moving cones is provided for an embodiment of the present invention;

[0043] Figure 3 A flowchart of another UWB tracking and following method based on traffic moving cones is provided for an embodiment of the present invention. DETAILED DESCRIPTION

[0044] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The term "and / or" herein is only a description of the association relationship of the associated objects, indicating that there can be three relationships. For example, A and B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, in the present invention, the description of "first", "second", etc. is only used for descriptive purposes, and cannot be understood as indicating or implying its relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first" and "second" can explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but it must be based on the ability of ordinary technicians in the field to implement. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0045] Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative work shall fall within the scope of protection of the present invention.

[0046] The embodiment of the present invention provides a UWB tracking and following system based on traffic moving cones. Figure 1 , Figure 1 A framework diagram of a UWB tracking and following system based on traffic moving cones provided by an embodiment of the present invention, wherein the UWB tracking and following system comprises a central control unit and a moving cone queue, wherein the moving cone queue comprises a head moving cone and a plurality of tracking moving cones, wherein the head moving cone and each tracking moving cone are initially arranged in the same direction at intervals of a first preset distance, and each moving cone in the moving cone queue is equipped with two UWB tags and an IMU module, wherein the system comprises:

[0047] The central control unit generates an initial control command and sends the initial control command to the head moving cone;

[0048] The head moving cone moves according to the initial control instruction, and performs real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain the corrected control instruction, moves according to the corrected control instruction, and sends the corrected control instruction to the next adjacent tracking moving cone;

[0049] The target moving cone determines its own control instruction according to the initial distance from the head moving cone, moves according to its own control instruction and the correction control instruction, and corrects the path according to the moving path to obtain its own correction control instruction, and moves according to its own correction control instruction. If the target moving cone has an adjacent next moving cone, the correction control instruction is sent to the adjacent next moving cone; the target moving cone is any one of the tracking moving cones.

[0050] A UWB tracking and following system based on traffic moving cones is provided based on an embodiment of the present invention. By combining an IMU module with UWB, the stability of positioning can be enhanced to provide assistance for subsequent tracking and following. The head moving cone can detect obstacles in real time and automatically adjust the path during execution. By correcting the obstacle avoidance of the initial instructions, dynamic path adjustment can be achieved to ensure that the head moving cone can flexibly bypass obstacles in a complex environment, so that subsequent tracking moving cones can avoid colliding with obstacles. By correcting the route of each moving cone, the accuracy of mobile cone tracking and following is improved.

[0051] In one implementation, after each mobile cone is started, it will detect neighboring vehicles through the UWB tag it carries to determine the relative position relationship and the front moving cone and rear moving cone relative to the moving cone. The front moving cone of the vehicle is the moving cone that sends control instructions to the moving cone, and the rear moving cone of the vehicle is the moving cone that receives the control instructions sent by the moving cone.

[0052] In one implementation, the head moving cone and each tracking moving cone move in the same initial direction, are arranged at intervals of a first preset distance, and are arranged in a straight line; the initial control instruction is an instruction for the operator to perform an operation, including the moving time and the moving angle; the first preset distance is determined by a technician; if the target moving cone does not have an adjacent next moving cone, it means that the current target moving cone is the last one, and no instruction transmission is required.

[0053] In one implementation, each mobile cone in the system is equipped with a UWB tag, and UWB ultra-wideband technology can be used to achieve real-time positioning with centimeter-level accuracy, so that each mobile cone in the system can perceive the distance and direction of adjacent cones in real time, thereby ensuring precise position control of the queue during movement; the IMU module combined with UWB can enhance the stability of positioning, and when the UWB signal is weak or blocked, the position deviation can still be compensated based on the IMU data, thereby improving the robustness of the system.

[0054] In one implementation, the initial control command issued by the central control unit only indicates the overall path direction. The head moving cone can perform obstacle detection in real time and automatically adjust the path during execution. By correcting the obstacle avoidance of the initial command, dynamic path adjustment can be achieved to ensure that the head moving cone can flexibly bypass obstacles in complex environments. The corrected control command generated by the head moving cone is gradually transmitted to each tracking moving cone in the entire queue. All cones can flexibly adjust their positions according to the current situation, avoid obstacles and ensure the integrity of the queue.

[0055] In one implementation, each tracking moving cone can automatically adjust its position based on the initial distance from the head moving cone combined with its own real-time correction control instructions. This ensures that each cone maintains a reasonable distance from the head moving cone during movement, maintaining the integrity and consistency of the queue.

[0056] In one implementation, the target moving cone determines its own control instruction according to the initial distance from the head moving cone, specifically by determining how many first preset distance intervals there are between the target moving cone and the head moving cone, and determining when the target moving cone receives the moving instruction sent by the head moving cone. That is, the target moving cone first moves straight according to the first preset distance interval with the head moving cone until the target moving cone reaches the starting position of the head moving cone, and then moves further according to the moving instruction sent by the head moving cone.

[0057] Based on the same inventive concept, the embodiment of the present invention also provides a UWB tracking and following method based on traffic moving cones. Figure 2 , Figure 2 A flowchart of a UWB tracking and following method based on traffic moving cones is provided in an embodiment of the present invention. The UWB tracking and following system includes a central control unit and a moving cone queue. The moving cone queue includes a head moving cone and a plurality of tracking moving cones. The head moving cone and each tracking moving cone are initially in the same direction and are arranged at a first preset distance. The moving cones in the moving cone queue are all equipped with two UWB tags and an IMU module. The method is applied to the head moving cone and includes:

[0058] S201, receiving an initial control instruction generated by a central control unit, moving according to the initial control instruction, and performing real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain a corrected control instruction;

[0059] S201, moving according to the modified control instruction, and sending the modified control instruction to the next adjacent tracking moving cone.

[0060] Among them, the correction control instruction is sent to the adjacent next tracking moving cone so that the target moving cone determines its own control instruction according to the initial distance from the head moving cone, and moves according to its own control instruction and the correction control instruction, and corrects the path according to the moving path to obtain its own correction control instruction, and moves according to its own correction control instruction. If the target moving cone has an adjacent next moving cone, the correction control instruction is sent to the adjacent next moving cone; the target moving cone is any one of the tracking moving cones.

[0061] Based on a UWB tracking and following method based on traffic moving cones provided by an embodiment of the present invention, the stability of positioning can be enhanced by combining an IMU module with UWB, and assistance can be provided for subsequent tracking and following. The head moving cone can detect obstacles in real time and automatically adjust the path during execution. By correcting the obstacle avoidance of the initial instructions, dynamic path adjustment can be achieved to ensure that the head moving cone can flexibly bypass obstacles in a complex environment, so that subsequent tracking moving cones can avoid colliding with obstacles, and then each moving cone is corrected on its own route to improve the accuracy of mobile cone tracking and following.

[0062] In one embodiment, moving according to the initial control instruction and performing real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain the corrected control instruction includes:

[0063] Obtain obstacle information at the current location in real time through sensors, and establish a local obstacle map based on the obstacle information;

[0064] According to the local obstacle map, a safe path is generated through a preset algorithm, the deviation value between the path generated by the initial control instruction and the safe path is calculated in real time, and a corrected control instruction is generated according to the deviation value.

[0065] In one implementation, the system uses sensors to scan and identify obstacle information at the current location in real time, generating a current local obstacle map so that it can adapt to dynamic changes in the surrounding environment at any time. In complex or sudden environments, the system can automatically avoid obstacles, reduce the risk of impact or collision, and improve the safety and effectiveness of the mobile cone during movement. The sensor is a lidar.

[0066] In one implementation, based on the local obstacle map, the system can generate a safe path at any time, find a detour even in complex terrain or dynamic environments, and implement flexible path planning; by calculating the deviation value between the initial control instruction path and the safe path in real time, the system can accurately identify the direction and degree of deviation, and provide an accurate correction basis for the control instruction; the deviation value generates a corrected control instruction, and the system dynamically adjusts according to the corrected instruction to make the moving path smoother, avoid large changes in direction, and improve movement efficiency and control stability.

[0067] In one implementation, the current moving cone position, the lateral distance between the initial path and the safety path, and the angular difference between the initial path direction and the safety path direction are calculated, the lateral distance and the angular difference are input into the PID controller, and the calculated control output is used as a correction control instruction.

[0068] In one embodiment, obtaining obstacle information at the current position in real time through a sensor, and establishing a local obstacle map according to the obstacle information includes:

[0069] Acquire initial environmental data around the current position in real time through a sensor, and pre-process the initial environmental data to obtain first data;

[0070] The first data is screened according to a preset interval to obtain second data, and the second data is mapped to a two-dimensional grid plane to obtain a local obstacle map.

[0071] In one implementation, the surrounding environmental data is acquired in real time by the LiDAR sensor to form three-dimensional point cloud data, and the preprocessing is to filter the noise of the three-dimensional point cloud data. For example, noise points that are too far or too close are removed to reduce the impact of environmental changes and improve the accuracy of the map; the first data is screened according to the preset interval to obtain the second data, which is to filter out the point cloud that exceeds the set height range according to the minimum and maximum heights of the point cloud, and only retain the data within the range as the second data. The second data is used to construct a local obstacle map, and the minimum and maximum heights are determined by the technicians. The three-dimensional point cloud data is projected onto a two-dimensional ground plane to form two-dimensional plane data, and the area acquired at the current position is divided into grid units of the target size, and the target size is determined by the technicians. The projected two-dimensional point cloud data is traversed, and whether there are obstacles in the grid unit is determined according to the density or number of points. For example, if there are more than a set number of points in a grid unit, it is marked as an obstacle, and the set number is determined by the technicians, and finally a local obstacle map is obtained.

[0072] In one embodiment, generating a safe path by a preset algorithm according to a local obstacle map includes:

[0073] According to the local obstacle map, determine whether the route formed by the current initial control instruction collides with an obstacle. If so, generate a safe target point according to the local obstacle map, and calculate the route from the current position to the safe target point through the wavefront algorithm to obtain a sub-route;

[0074] Move according to the sub-route and repeatedly generate safe target points according to the local obstacle map until the route formed by the initial control instruction no longer collides with obstacles, and finally a safe path is generated.

[0075] In one implementation, the system can quickly determine whether there are obstacles in the path formed by the initial control instructions by detecting and updating obstacle information in real time, and adjust the route in time to avoid collisions; by calculating the sub-route to the safe target point through the wavefront algorithm, the robot can find a temporary safe path point when the current path is blocked.

[0076] In one implementation, the safe target point is a point in the direction of the vehicle's travel, and the safe target point is usually located at the edge of an obstacle to ensure that the moving cone can approach the target without collision.

[0077] Based on the same inventive concept, the embodiment of the present invention also provides another UWB tracking and following method based on traffic moving cones. Figure 3 , Figure 3 A flowchart of another UWB tracking and following method based on traffic moving cones provided in an embodiment of the present invention, wherein the UWB tracking and following system comprises a central control unit and a moving cone queue, wherein the moving cone queue comprises a head moving cone and a plurality of tracking moving cones, wherein the head moving cone and each tracking moving cone are initially arranged in the same direction and are arranged at intervals of a first preset distance, wherein the moving cones in the moving cone queue are all equipped with two UWB tags and an IMU module, wherein the method is applied to a target moving cone, wherein the target moving cone is any one of the plurality of tracking moving cones, and wherein the method comprises:

[0078] S301, receiving the correction control instruction sent by the head moving cone;

[0079] S302, determining a self-control instruction according to an initial distance from the head moving cone, moving according to the self-control instruction and the correction control instruction, and performing path correction according to the moving path to obtain a self-correction control instruction;

[0080] S303, moving according to the self-corrected control instruction, if the target moving cone has an adjacent next moving cone, sending the corrected control instruction to the adjacent next moving cone.

[0081] Among them, the generation process of the correction control instruction is that the head moving cone receives the initial control instruction generated by the central control unit, moves according to the initial control instruction, and performs real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain the correction control instruction.

[0082] Another UWB tracking and following method based on traffic moving cones provided by an embodiment of the present invention can enhance the stability of positioning by combining the IMU module with UWB, and provide assistance for subsequent tracking and following. The head moving cone can detect obstacles in real time and automatically adjust the path during execution. By correcting the obstacle avoidance of the initial instructions, dynamic path adjustment can be achieved to ensure that the head moving cone can flexibly bypass obstacles in complex environments, so that subsequent tracking moving cones can avoid colliding with obstacles, and then each moving cone is corrected Its own route, the accuracy of mobile cone tracking and following is improved.

[0083] In one embodiment, after each tracking moving cone is started, it detects neighboring vehicles according to the UWB tag it carries, determines the front tracking moving cone and the rear tracking moving cone corresponding to the tracking moving cone, and performs path correction according to the moving path to obtain the self-correction control instruction, including:

[0084] The relative distances between the target moving cone and the front tracking moving cone and the rear tracking moving cone are obtained in real time according to the UWB tag to obtain a first relative distance and a second relative distance;

[0085] The angles between the target moving cone and the front tracking moving cone and the rear tracking moving cone are obtained in real time through the IMU module to obtain the first posture and the second posture;

[0086] Determine a target moving cone position correction instruction based on the first relative distance and the second relative distance;

[0087] The target moving cone angle correction instruction is determined according to the first posture and the second posture, and the self-correction control instruction is obtained according to the position correction instruction and the angle correction instruction.

[0088] In one implementation, UWB tags are used to measure the relative distance to the front and rear tracking moving cones in real time, which can achieve high-precision position determination, keep each tracking moving cone within a preset safe distance, reduce the risk of collision or misoperation, and improve the stability and safety of the system.

[0089] In one implementation, the IMU module is an inertial measurement unit that provides real-time angle information, allowing the system to know the posture of the tracking moving cone at any time, and generate angle correction instructions based on the posture to avoid path deviation due to inconsistent posture, so that each moving cone in the queue can move along the correct path, further enhancing the stability of the path.

[0090] In one implementation, each mobile cone can obtain the distance and angle between itself and the vehicles in front and behind it in real time while moving, ensuring the synchronous update of position and posture in the queue. In this way, the coordination of the entire mobile cone queue can be greatly improved, which helps to achieve smooth queue movement and efficient path following.

[0091] In one implementation, the UWB and IMU-based sensing solution can quickly feedback distance and attitude information, thereby generating position and angle correction instructions, improving the real-time performance of path correction, and enabling the mobile cone fleet to quickly adjust when encountering emergencies or obstacle avoidance needs, thereby reducing errors caused by lags.

[0092] In one embodiment, determining the target moving cone position correction instruction according to the first relative distance and the second relative distance includes:

[0093] Calculating the difference between the first relative distance and the first preset distance to obtain a first error, and calculating the difference between the second relative distance and the first preset distance to obtain a second error;

[0094] If the first error is a positive number and the second error is a negative number, the first error is used as the position correction instruction;

[0095] If the first error is a negative number and the second error is a positive number, the second error is used as the position correction instruction.

[0096] In one implementation, by respectively calculating the relative distance errors with the front and rear moving cones, namely the first error and the second error, the system can quickly identify the position deviation direction of the current moving cone. When it is detected that the error direction is opposite to the front and rear moving cones, that is, the first error is positive and the second error is negative, or the first error is negative and the second error is positive, the system can select the corresponding error for position correction, thereby ensuring the accuracy of the position adjustment and reducing unnecessary path deviation.

[0097] In one implementation, it is only necessary to compare the difference between the current relative distance to the front and rear moving cones and the preset distance, and simply determine whether correction is needed based on the positive or negative value of the difference, thereby reducing the calculation complexity of position correction. Compared with more complex control algorithms, the simplified logic makes the system more advantageous in real-time and response speed, and is particularly suitable for rapidly changing tracking scenarios.

[0098] In one embodiment, determining the target movement cone angle correction instruction according to the first posture and the second posture includes:

[0099] Calculate the difference between the first posture and the preset first posture to obtain a first posture error, and calculate the difference between the second posture and the preset first posture to obtain a second posture error;

[0100] If the first posture error is positive, the first posture error is used as the angle correction instruction;

[0101] If the second posture error is positive, the second posture error is used as the angle correction instruction.

[0102] In one implementation, by separately calculating the attitude error of each moving cone, namely the first attitude error and the second attitude error, the system can accurately identify the specific source of the angle deviation. The generation of correction instructions based on the positive and negative values ​​of the attitude error can quickly correct the deviation, thereby maintaining the correct direction of each moving cone during the movement process and avoiding yaw and deviation.

[0103] In one implementation, by comparing the error between the actual posture and the preset first posture and making adjustments, the system can make the mobile cone queue move in a consistent direction. This consistency is especially important in long-term or large-scale movement, which helps to improve the overall coordination and stability of the queue and ensure that all mobile cones are facing the target direction.

[0104] In one implementation, the posture correction not only ensures the consistency of the direction of the mobile cone queue, but also prevents each mobile cone from moving closer to or farther away from the adjacent mobile cone due to direction deviation. By maintaining the appropriate angle and distance relationship, the system can more stably maintain the structure and order of the queue and prevent the queue from being interrupted or disordered.

[0105] The above is a detailed description of an embodiment of the present invention, but the content is only a preferred embodiment of the present invention and cannot be considered to limit the scope of implementation of the present invention. All equivalent changes and improvements made within the scope of the present invention should still fall within the scope of the patent coverage of the present invention.

Claims

1. A UWB tracking and following system based on traffic moving cones, the UWB tracking and following system comprises a central control unit and a moving cone queue, the moving cone queue comprises a head moving cone and a plurality of tracking moving cones, the head moving cone and each tracking moving cone initially have the same direction, and are arranged at a first preset distance interval, the moving cones in the moving cone queue are all equipped with two UWB tags and an IMU module, characterized in that: The system comprises: The central control unit generates an initial control instruction and sends the initial control instruction to the head moving cone; The head moving cone moves according to the initial control instruction, and performs real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain a corrected control instruction, moves according to the corrected control instruction, and sends the corrected control instruction to the next adjacent tracking moving cone; The target moving cone determines its own control instruction according to the initial distance from the head moving cone, moves according to the self-control instruction and the correction control instruction, and obtains its own correction control instruction by performing path correction according to the moving path, and moves according to the self-correction control instruction. If the target moving cone has an adjacent next moving cone, the correction control instruction is sent to the adjacent next moving cone; the target moving cone is any one of the tracking moving cones.

2. A UWB tracking and following method based on traffic moving cones, wherein the UWB tracking and following system comprises a central control unit and a moving cone queue, wherein the moving cone queue comprises a head moving cone and a plurality of tracking moving cones, wherein the head moving cone and each tracking moving cone are initially arranged in the same direction and are arranged at intervals of a first preset distance, wherein the moving cones in the moving cone queue are all equipped with two UWB tags and an IMU module, wherein: The method is applied to the head moving cone comprising: Receiving an initial control instruction generated by the central control unit, moving according to the initial control instruction, and performing real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain a corrected control instruction; Move according to the correction control instruction, and send the correction control instruction to the adjacent next tracking moving cone; so that the target moving cone determines its own control instruction according to the initial distance from the head moving cone, and moves according to the self-control instruction and the correction control instruction, and corrects the path according to the moving path to obtain its own correction control instruction, moves according to its own correction control instruction, and if the target moving cone has an adjacent next moving cone, the correction control instruction is sent to the adjacent next moving cone; the target moving cone is any one of the tracking moving cones.

3. The UWB tracking and following method based on traffic moving cones according to claim 2 is characterized in that: Moving according to the initial control instruction and performing real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain a corrected control instruction includes: Obtain obstacle information at the current location in real time through sensors, and establish a local obstacle map based on the obstacle information; According to the local obstacle map, a safe path is generated by a preset algorithm, a deviation value between the path generated by the initial control instruction and the safe path is calculated in real time, and a corrected control instruction is generated according to the deviation value.

4. The UWB tracking and following method based on traffic moving cones according to claim 3 is characterized in that: Obtaining obstacle information at the current position in real time through sensors, and establishing a local obstacle map according to the obstacle information includes: Acquire initial environmental data around the current position in real time through a sensor, and pre-process the initial environmental data to obtain first data; The first data is screened according to a preset interval to obtain second data, and the second data is mapped to a two-dimensional grid plane to obtain a local obstacle map.

5. The UWB tracking and following method based on traffic moving cones according to claim 3 is characterized in that: Generating a safe path according to the local obstacle map by a preset algorithm includes: According to the local obstacle map, determine whether the route formed by the current initial control instruction collides with an obstacle. If so, generate a safe target point according to the local obstacle map, and calculate the route from the current position to the safe target point by using a wavefront algorithm to obtain a sub-route; Move according to the sub-route, and repeatedly generate safe target points according to the local obstacle map until the route formed by the initial control instruction no longer collides with obstacles, and finally generate a safe path.

6. A UWB tracking and following method based on traffic moving cones, wherein the UWB tracking and following system comprises a central control unit and a moving cone queue, wherein the moving cone queue comprises a head moving cone and a plurality of tracking moving cones, wherein the head moving cone and each tracking moving cone are initially arranged in the same direction and are arranged at intervals of a first preset distance, wherein the moving cones in the moving cone queue are all equipped with two UWB tags and an IMU module, wherein: The method is applied to a target moving cone, where the target moving cone is any one of a plurality of tracking moving cones, and the method comprises: Receive the correction control instruction sent by the head moving cone; the generation process of the correction control instruction is that the head moving cone receives the initial control instruction generated by the central control unit, moves according to the initial control instruction, and performs real-time obstacle avoidance detection during the movement to correct the initial control instruction to obtain the correction control instruction; Determine a self-control instruction according to an initial distance from the head moving cone, move according to the self-control instruction and the correction control instruction, and perform path correction according to the moving path to obtain the self-correction control instruction; The moving cone moves according to the self-correction control instruction, and if the target moving cone has an adjacent next moving cone, the correction control instruction is sent to the adjacent next moving cone.

7. According to the UWB tracking and following method based on traffic moving cones in claim 6, after each tracking moving cone is started, it detects neighboring vehicles according to the UWB tag it carries, and determines the front tracking moving cone and the rear tracking moving cone corresponding to the tracking moving cone, characterized in that: And according to the moving path, the path correction is performed to obtain the self-correction control instructions including: Acquire the relative distances between the target moving cone and the front tracking moving cone and the rear tracking moving cone in real time according to the UWB tag to obtain a first relative distance and a second relative distance; The angles between the target moving cone and the front tracking moving cone and the rear tracking moving cone are acquired in real time by the IMU module to obtain a first posture and a second posture; Determine the target moving cone position correction instruction according to the first relative distance and the second relative distance; The target movement cone angle correction instruction is determined according to the first posture and the second posture, and the self-correction control instruction is obtained according to the position correction instruction and the angle correction instruction.

8. The UWB tracking and following method based on traffic moving cones according to claim 7 is characterized in that: Determining the target moving cone position correction instruction according to the first relative distance and the second relative distance includes: Calculating a difference between the first relative distance and the first preset distance to obtain a first error, and calculating a difference between the second relative distance and the first preset distance to obtain a second error; If the first error is a positive number and the second error is a negative number, the first error is used as a position correction instruction; If the first error is a negative number and the second error is a positive number, the second error is used as a position correction instruction.

9. The UWB tracking and following method based on traffic moving cones according to claim 7 is characterized in that: Determining the target movement cone angle correction instruction according to the first posture and the second posture includes: Calculating the difference between the first posture and the preset first posture to obtain a first posture error, and calculating the difference between the second posture and the preset first posture to obtain a second posture error; If the first posture error is positive, the first posture error is used as an angle correction instruction; If the second posture error is positive, the second posture error is used as an angle correction instruction.

Images