Target tracking sanitation vehicle self-following system and method thereof

Abstract

The invention relates to the technical field of sanitation vehicle navigation, in particular to a target tracking sanitation vehicle self-following system and method, and the system comprises a sanitation vehicle body, a sensor input unit, a sensing planning unit, and a motion control unit. The light sanitation vehicle body comprises a whole vehicle control unit and a vehicle body, and the whole vehicle control unit is connected with the vehicle body. The sensor input unit comprises a three-dimensional laser radar, a rotary camera device and a combined navigation system which are arranged on the vehicle body; the perception planning unit comprises a target tracking and positioning module and a path planning and dynamic obstacle avoidance module; wherein the sensor input unit, the sensing planning unit and the motion control unit are all arranged on the sanitation vehicle body. According to the target tracking sanitation vehicle self-following system and method provided by the invention, through the color depth image including the following target and the obstacle obtained by the depth camera and the point cloud information obtained by the three-dimensional laser radar, after the target path is obtained, following and automatic obstacle avoidance are completed, and the environmental cleaning requirement of a sanitation vehicle is met.

Description

Technical Field

[0001] The present invention relates to the technical field of sanitation vehicle navigation, and in particular to a target tracking self-following system and method for a sanitation vehicle. Background Art

[0002] Unmanned self-driving sanitation vehicles have become an important participant in urban sanitation. Through unmanned, low-speed and high-quality cleaning work, they have further liberated urban sanitation workers and drivers. With the update and iteration of unmanned driving technology, unmanned sanitation vehicles have changed the traditional sanitation operation mode in a more intelligent way. The traditional sanitation operation mode is that drivers or sanitation workers perform routine cleaning work every day along a fixed route.

[0003] However, in the current sanitation scenarios, unmanned autonomous driving sanitation vehicles still face many technical difficulties in practical application scenarios such as fine edge cleaning and clearing potholes. In addition, the digital map generation, environmental perception, self-positioning, path planning, etc. in the current autonomous driving technology have certain technical difficulties that need to be solved in specific road conditions and emergencies, and cannot meet the requirements of complete independence.

[0004] Therefore, there is a need for a self-following system for sanitation vehicles that can independently follow and avoid obstacles in a variety of scenarios. Summary of the Invention

[0005] The main purpose of the present invention is to provide a target tracking sanitation vehicle self-following system and method thereof, aiming to solve the problem that the existing sanitation vehicle following method is difficult to meet the requirements of complete independence in actual application scenarios.

[0006] To achieve the above-mentioned object, the present invention proposes a target tracking sanitation vehicle self-following system, comprising a sanitation vehicle body, a sensor input unit, a perception planning unit, and a motion control unit;

[0007] The sanitation vehicle body includes a vehicle control unit and a vehicle body, and the vehicle control unit is connected to the vehicle body;

[0008] The sensor input unit includes a three-dimensional laser radar, a rotating camera device and a combined navigation system, wherein the three-dimensional laser radar, the depth camera and the rotating camera device are all arranged on the vehicle body;

[0009] The perception planning unit also includes a target tracking and positioning module, a path planning and dynamic obstacle avoidance module, and the target tracking and positioning module is signal-connected to the path planning and dynamic obstacle avoidance module;

[0010] The motion control unit is connected to the vehicle control unit by signals;

[0011] Among them, the sensor input unit, the perception planning unit and the motion control unit are all arranged on the sanitation vehicle body.

[0012] Preferably, the rotating camera device includes a depth camera mount, an embedded development board and an inertial measurement unit. The depth camera mount is arranged at the front end of the vehicle body, the depth camera is arranged on the vehicle body through the depth camera mount, and the depth camera mount is fixedly connected to the inertial measurement unit. The inertial measurement unit, the depth camera and the three-dimensional laser radar are all signal-connected to the embedded development board.

[0013] Preferably, the rotating camera device also includes a stepper motor, one end of the stepper motor is connected to the camera mounting seat, and the other end of the stepper motor is connected to the vehicle body. The camera mounting seat is movably connected to the vehicle body through the stepper motor, and a signal connection is established between the stepper motor and the embedded development board.

[0014] The present invention further discloses a target tracking sanitation vehicle self-following method, which is applied to a target tracking sanitation vehicle self-following system as described in any one of the above technical solutions, and comprises the following steps:

[0015] Receive scene information acquired by the sensor and obtain the two-dimensional coordinates of the tracking target and obstacles;

[0016] The acquired scene information is used to obtain the motion paths of targets and obstacles through the target position detection algorithm;

[0017] The motion paths of the tracked targets and obstacles are calculated by the local path planner to output the required speed and front angle of the sanitation vehicle.

[0018] Based on the calculated required speed and front turning angle of the sanitation vehicle, the signal is transmitted to the vehicle control unit, enabling the sanitation vehicle to follow the motion path of the tracking target and avoid obstacles.

[0019] Preferably, the step of receiving scene information acquired by the sensor and acquiring the two-dimensional coordinates of the tracking target and the obstacle includes:

[0020] Use 3D laser radar to scan the surrounding scene and generate point cloud data;

[0021] Use a depth camera to obtain color images and depth images of the tracking target or obstacle;

[0022] Obtain the three-dimensional coordinates of the target in the camera coordinate system through the color image and depth image;

[0023] The point cloud data obtained by the 3D lidar scan and the 3D coordinate data obtained by the depth camera are processed to form a joint target tracking method;

[0024] After continuously tracking the target and obstacles by labeling the point cloud clusters, the two-dimensional coordinates of the target and obstacles are continuously output, and the three-dimensional coordinates of the target obtained by the depth camera at that moment are projected onto the horizontal plane to form two-dimensional coordinates;

[0025] Calculate the bi-norm of the plane two-dimensional coordinate vector output by point cloud target tracking and the plane two-dimensional coordinate vector output by visual target detection. When the bi-norm is less than the set threshold, it is marked as a target as the output of the final target coordinates.

[0026] Preferably, the step of obtaining the three-dimensional coordinates of the target in the camera coordinate system through the color image and the depth image includes:

[0027] Get the three-dimensional coordinates of the target center relative to the color camera coordinate system;

[0028] The three-dimensional coordinates of the target center relative to the navigation coordinate system are obtained through the conversion matrix from the color camera coordinate system to the color camera initial coordinate system after camera rotation, the conversion matrix from the lidar coordinate system to the navigation system coordinate system, and the conversion matrix from the depth camera initial coordinate system to the lidar coordinate system;

[0029] By obtaining the conversion matrix from the navigation coordinate system output by the integrated navigation system to the navigation initial coordinate system, the three-dimensional coordinates of the target center relative to the navigation initial coordinate system are obtained.

[0030] Preferably, the step of using a three-dimensional laser radar to scan the surrounding scene and generate point cloud data includes:

[0031] Using the three-dimensional laser radar on the sanitation vehicle to scan the surrounding scene, establishing a radar coordinate system with the sanitation vehicle as the center, and obtaining local point cloud information within the radar coordinate system;

[0032] Remove outliers from the local point cloud information and project the local point cloud information onto the horizontal plane. Then, use the DBSCAN clustering algorithm to divide the horizontal plane point cloud information into multiple clusters. For the point cloud in each cluster, calculate a maximum enclosing ellipse. The center coordinates of each ellipse are calibrated as the two-dimensional coordinates of the target or obstacle in the global coordinate system.

[0033] The coordinate transformation point cloud output by the integrated navigation system is mapped to the global coordinate system to generate global point cloud data.

[0034] The present invention provides a target tracking self-following system and method for sanitation vehicles. The system uses a rotating camera to obtain a color image including a target sanitation worker and a local point cloud obtained by a laser radar, and then tracks the target sanitation worker with high robustness through a combined target tracking and positioning algorithm. After obtaining the target path, the system follows the target sanitation worker and performs autonomous obstacle avoidance control, which can meet the cleaning requirements of the sanitation vehicle in a human-machine collaborative mode. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the processes shown in these drawings without paying any creative work.

[0036] Figure 1 This is a flow chart of an embodiment of the target tracking method for a sanitation vehicle;

[0037] Figure 2 This is a schematic diagram of the framework of an embodiment of the target tracking sanitation vehicle self-following system. DETAILED DESCRIPTION

[0038] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0039] It should be noted that all directional indications in the embodiments of the present invention (such as up, down, left, right, front, back, etc.) are only used to explain the relative position relationship, movement status, etc. between the various components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0040] In addition, the descriptions of "first", "second", etc. in the present invention are for descriptive purposes only and should not be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of such features. In addition, the technical solutions between the various embodiments can be combined with each other, but this must be based on the fact that they can be implemented by ordinary technicians in this field. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0041] When sanitation vehicles used in existing technologies in sanitation scenarios follow target sanitation workers, they may face the problem of losing the target in the picture due to the large deviation between the movement direction and the target sanitation worker during the movement, and the field of view range limitation of the fixed visual capture equipment. The existing method for identifying and positioning sanitation workers is an algorithm that follows the target task based on pure vision. This method is easily affected by differences in outdoor light intensity and fog and rain, resulting in deviations in the position information of the target person being followed in outdoor sanitation work, or even the inability to detect the target person, making tracking ineffective. In the existing technology, the movement of the sanitation vehicle is constrained by imposing restrictions to ensure that the target is moderately within the image frame. In the process of planning the path, this method greatly reduces the number of feasible path points due to the dual constraints of field of view and obstacle avoidance, greatly increases the calculation time, and even makes it impossible to generate a feasible path, causing the sanitation vehicle to stagnate and affecting the cleaning efficiency.

[0042] The human-machine collaboration mode of the sanitation vehicle involved in the present invention is a working mode in which a sanitation worker performs garbage cleaning operations in front of the sanitation vehicle and collaborates with the sanitation vehicle carrying a garbage collection device and cleaning tools. The sanitation vehicle can move autonomously following the designated sanitation worker, meeting the current requirements for the safety and effectiveness of the autonomous work of sanitation vehicles and is easy to industrialize.

[0043] Based on this, the embodiment of the present application provides a target tracking sanitation vehicle self-following system and method thereof, referring to Figures 1 to 2 , Figure 1 This is a flow chart of an embodiment of the target tracking method for sanitation vehicles. Figure 2 This is a framework diagram of an embodiment of the target tracking sanitation vehicle self-following system.

[0044] The present invention provides a target tracking self-following system for sanitation vehicles, which includes: a sanitation vehicle body, a sensor input unit, a perception planning unit, and a motion control unit, specifically as follows:

[0045] The sanitation vehicle body includes a vehicle control unit and a vehicle body, and the vehicle control unit is connected to the vehicle body; the sensor input unit includes a three-dimensional laser radar, a rotating camera device and a combined navigation system, and the three-dimensional laser radar, depth camera and rotating camera device are all arranged on the vehicle body; the perception planning unit also includes a target tracking and positioning module, a path planning and dynamic obstacle avoidance module, and the target tracking and positioning module is signal-connected to the path planning and dynamic obstacle avoidance module; the motion control unit, the motion control unit is signal-connected to the vehicle control unit; among them, the sensor input unit, the perception planning unit and the motion control unit are all arranged on the sanitation vehicle body.

[0046] In this embodiment, the sensor input unit includes a three-dimensional laser radar, a depth camera, a rotating camera device, and an integrated navigation system. The sensor input unit acquires environmental information surrounding the sanitation vehicle. The three-dimensional laser radar inputs point cloud data, the rotating camera device inputs correct image data, and the integrated navigation system inputs the coordinate transformation (TF transformation) of the sanitation vehicle relative to the global coordinate system. This information yields the specific coordinates of the sanitation vehicle and surrounding environmental data. The perception and planning unit comprises a target tracking and positioning module and a path planning and dynamic obstacle avoidance module. The target tracking and positioning module uses the point cloud data, image data, and TF transformation data acquired by the sensor input unit. It then uses a rotating camera and laser radar on an industrial computer mounted on the sanitation vehicle's body to combine target position detection algorithms to determine the motion paths of targets and obstacles. The local path planning phase calculates and outputs key motion data, such as the speed and front wheel angle, required by the sanitation vehicle, enabling the sanitation vehicle to follow the target's trajectory and avoid obstacles. The motion control unit uses the speed and front wheel angle data output by the perception and planning unit to input the controller into the sanitation vehicle's vehicle control unit (VCU) to control the vehicle's motion.

[0047] In detail, the integrated navigation system includes the Global Navigation Satellite System GNSS, RTK (Real-time kinematic, real-time dynamic carrier phase differential technology) and inertial measurement devices. Through combined operations, the sanitation vehicle can obtain the precise positioning of itself in the map.

[0048] In one embodiment, the rotating camera device includes a depth camera mount, an embedded development board and an inertial measurement unit. The depth camera mount is arranged at the front end of the vehicle body, the depth camera is arranged on the vehicle body through the depth camera mount, and the depth camera mount is fixedly connected to the inertial measurement unit. The inertial measurement unit, the depth camera and the three-dimensional laser radar are all connected to the embedded development board for signal connection.

[0049] In this embodiment, the rotating camera device includes a depth camera, an IMU (Inertial Measurement Unit) fixedly connected to the depth camera, and an embedded development board, wherein the embedded development board is an STM32 development board. The depth camera is a depth camera with a depth range of 0.2 to 10m, and the depth camera is capable of acquiring color images and depth images. The rotating camera device is installed at the front end of the sanitation vehicle, and the height difference from the lowest point of the wheel is 0.8m. The motor driver and the embedded development board are installed on the left front of the sanitation vehicle.

[0050] Specifically, the rotating camera device of the present invention is equipped with a rotating camera object detection algorithm, specifically an industrial computer object detection program. The industrial computer object detection program primarily runs the YOLO-V10 object detection program, outputting three-dimensional coordinates in the camera coordinate system and sending steering instructions to the microcontroller serial port, while also recording the theoretical number of left and right turns since the initial moment.

[0051] In one embodiment, the rotating camera device also includes a stepper motor, one end of the stepper motor is connected to the camera mounting bracket, and the other end of the stepper motor is connected to the vehicle body. The camera mounting bracket is movably connected to the vehicle body through the stepper motor, and a signal connection is established between the stepper motor and the embedded development board.

[0052] In this embodiment, the rotating camera target detection algorithm also includes a microcontroller serial port communication and a motor drive program. The microcontroller serial port communication and motor drive program is an embedded program pre-burned in the microcontroller for reading serial port received data and driving the stepper motor through the HAL library function. It is understandable that, considering that the movement speed of sanitation workers and other moving objects identified as obstacles is usually slow, when the sanitation vehicle is not in a high-speed usage scenario, the movement mode of the sanitation vehicle is to follow the movement trajectory of the target person (i.e., the sanitation worker). During the movement, even if the target is blocked by dynamic obstacle avoidance or other dynamic objects, the speed at which the target deviates from the camera screen will not be too fast. Therefore, the embedded program is designed to drive the stepper motor to a fixed horizontal rotation angle of 10° each time, and when the x-direction pixel ratio from the center of the target frame in the camera screen is less than or equal to 0.3 or greater than or equal to 0.7, the industrial computer target detection program will send instructions for turning and rotation direction. The serial communication data is also the rotation direction of the stepper motor. After receiving the rotation direction, the microcontroller drives the stepper motor to rotate the corresponding direction angle of 10°, thereby completing the camera's deflection of the tracking target, so that the sanitation vehicle can obtain accurate and stable target three-dimensional coordinates without frequently controlling the motor rotation. After the stepper motor controls the rotation of the depth camera, the industrial computer obtains the actual rotation angle data of the camera's IMU. This actual rotation angle is compared with the theoretical rotation angle obtained by converting the number of turns in each direction recorded by the target detection program. If the angle difference is within 3°, the two angles are considered equal, meeting the detection conditions. The target's three-dimensional coordinates in the color camera coordinate system are then obtained and converted to global three-dimensional coordinates through subsequent coordinate transformation. The YOLO-V10 target detection program trains its model by collecting a large number of images of sanitation workers wearing specific work clothes in different environments and lighting conditions. It uses the color camera in the depth camera for target detection, obtains the target center position, and then uses a built-in algorithm to obtain the depth value of the center position. Finally, it can feedback the three-dimensional coordinates of the target center in the color camera coordinate system.

[0053] The present invention also discloses a target tracking sanitation vehicle self-following method, which is applied to the target tracking sanitation vehicle self-following system of any one of the above technical solutions, and includes the following steps:

[0054] S001. Receive scene information acquired by the sensor and obtain the two-dimensional coordinates of the tracking target and obstacles;

[0055] S002. Using the acquired scene information through a target position detection algorithm to obtain the motion paths of the target and obstacles;

[0056] S003. The motion paths of the tracked target and obstacles are calculated by a local path planner to output the required speed and front turning angle of the sanitation vehicle;

[0057] S004. Based on the calculated required speed and front turning angle of the sanitation vehicle, a signal is transmitted to the vehicle control unit to enable the sanitation vehicle to follow the motion path of the tracking target and avoid obstacles.

[0058] In this embodiment, the sanitation vehicle obtains the three-dimensional coordinates of the target at intervals of 1 second, which constitute the set of path points that the sanitation vehicle needs to track. These are also the cleaning path points guided by the target personnel that the sanitation vehicle needs to track. For obstacles, the sanitation vehicle uses the three-dimensional coordinates of the obstacles obtained in the previous and next frames and then uses Kalman filtering to predict a total of 20 trajectory coordinate points at intervals of 0.1 seconds within 1 second after the current moment. The MPC-CPF (model predictive control and control obstacle function) can be used to calculate the optimal control variables at the current moment, including the speed and front wheel angle of the sanitation vehicle. The specific public disclosure of MPC-CPF is as follows:

[0059]

[0060] stx k+1 =f(x k ,u k ),k=0,...,N-1

[0061] x k ∈K,u k ∈U,k=0,...,N-1

[0062] x t | t =x t

[0063] x t+N | t ∈X f

[0064] Δh(x k ,u k )≥-γh(xk ),k=0,...,N-1

[0065] x k+1 =f(x k ,u k )

[0066] in, is the total cost, N is the number of predicted steps, p(x t+N|t ) is the terminal state cost, q(x k ,u k ) is the state cost and control cost of the prediction step, x k+1 =f(x k ,u k ) is the state space equation, x t|t =x t is the initial input state, and h is a continuously differentiable function.

[0067] In detail, 0<γ≤1 is defined as a CBF attenuation coefficient. When γ=1, it means there is no attenuation, the function has no effect, and MPC-CPF degenerates into MPC-DC. The smaller γ is, the stronger the function "repels" obstacles. This inequality is used as an inequality constraint for the planning problem. As long as this constraint is observed, the state quantity x k It will not enter the obstacle area and can control the sanitation vehicle to stay away from obstacles at an exponential level.

[0068] In one embodiment, the step of S001, receiving scene information acquired by a sensor and acquiring two-dimensional coordinates of a tracking target and an obstacle, includes:

[0069] S011. Use 3D laser radar to scan the surrounding scene and generate point cloud data;

[0070] S012. Acquire a color image and a depth image of a tracking target or obstacle using a depth camera;

[0071] S013. Obtain the three-dimensional coordinates of the target in the camera coordinate system through the color image and the depth image;

[0072] S014. Processing the point cloud data obtained by the 3D laser radar scanning and the 3D coordinate data acquired by the depth camera to form a joint target tracking method;

[0073] S015. After continuously tracking the target and obstacles by labeling the point cloud clusters, the two-dimensional coordinates of the target and obstacles are continuously output, and the three-dimensional coordinates of the target acquired by the depth camera at that moment are projected onto the horizontal plane to form two-dimensional coordinates;

[0074] S016. Calculate the bi-norm of the plane two-dimensional coordinate vector output by point cloud target tracking and the plane two-dimensional coordinate vector output by visual target detection. When the bi-norm is less than a set threshold, mark it as a target as the output of the final target coordinate.

[0075] In this embodiment, during the process of tracking sanitation workers by the laser radar, the continuous tracking of the follow-up target and obstacles is achieved through point cloud cluster labeling, and the planar two-dimensional coordinates of the follow-up target and obstacles can be continuously output. The target three-dimensional coordinates in the global coordinate system feedbacked by the visual target detection synchronized at this moment are projected onto the horizontal plane to form planar two-dimensional coordinates. The two-norm of the planar two-dimensional coordinate vector output by the point cloud target tracking and the two-norm of the planar two-dimensional coordinate vector output by the visual target detection is calculated. When the two-norm is less than the set threshold, the two-norm is output as the coordinate of the final target.

[0076] The present invention processes radar target point cloud data and visual image target three-dimensional coordinate data by adopting data post-fusion to form a joint target tracking method. By fusing information from multiple data sources, it can make up for the shortcomings of a single data source, improve the accuracy of decision-making or evaluation, and reduce the impact of abnormalities or anomalies of a single data source on the overall result, thereby enhancing robustness.

[0077] In one embodiment, S013, the step of obtaining the three-dimensional coordinates of the target in the camera coordinate system through the color image and the depth image, includes:

[0078] S131, obtaining the three-dimensional coordinates of the target center relative to the color camera coordinate system;

[0079] S132. Obtain the three-dimensional coordinates of the target center relative to the navigation coordinate system by using the conversion matrix from the color camera coordinate system to the color camera initial coordinate system after camera rotation, the conversion matrix from the lidar coordinate system to the navigation system coordinate system, and the conversion matrix from the depth camera initial coordinate system to the lidar coordinate system;

[0080] S133: Obtain the three-dimensional coordinates of the target center relative to the initial navigation coordinate system by acquiring a conversion matrix from the navigation coordinate system output by the integrated navigation system to the initial navigation coordinate system.

[0081] In this embodiment, the color camera is a depth camera capable of acquiring color images, and the three-dimensional coordinates of the target center relative to the color camera coordinate system are represented as The transformation matrix from the color camera coordinate system to the color camera initial coordinate system after the camera rotates is recorded as The transformation matrix from the laser radar coordinate system to the navigation system coordinate system is recorded as The transformation matrix from the initial color camera coordinate system to the lidar coordinate system is recorded as Combining the above coordinate systems can obtain the three-dimensional coordinates of the target center relative to the navigation coordinate system, and then obtain the conversion matrix from the navigation coordinate system output by the combined navigation system to the navigation initial coordinate system Finally, the three-dimensional coordinates of the target center relative to the initial navigation coordinate system can be obtained. The initial navigation coordinate system can locate the global coordinate system. During the movement of the sanitation vehicle, the global three-dimensional coordinates of the target center detected by the camera at each moment can be obtained. The specific formula is as follows:

[0082]

[0083] Among them, P w It is the three-dimensional coordinate value of the center point of the target detection in the global coordinate system.

[0084] The present invention obtains the target center position by following target detection in the depth camera image, then the perception planning unit outputs the rotation angle, the motion control unit controls the camera rotation, and finally the rotation angle is verified by the onboard IMU, forming a closed-loop control of camera rotation, ensuring the correctness of the calculated global three-dimensional coordinates of the target center.

[0085] In one embodiment, the step of S011, scanning the surrounding scene using a three-dimensional laser radar and generating point cloud data, includes:

[0086] S111. Scan the surrounding scene using the three-dimensional laser radar on the sanitation vehicle, establish a radar coordinate system with the sanitation vehicle as the center, and obtain local point cloud information within the radar coordinate system;

[0087] S112, removing outliers from the local point cloud information, projecting the local point cloud information onto a horizontal plane, and then dividing the horizontal plane point cloud information into multiple clusters using the DBSCAN clustering algorithm. Calculating a maximum enclosing ellipse for the point clouds in each cluster, and calibrating the center coordinates of each ellipse as the two-dimensional coordinates of the target or obstacle in the global coordinate system;

[0088] S113 , mapping the coordinate transformation point cloud output by the integrated navigation system to a global coordinate system to generate global point cloud data.

[0089] In this embodiment, a lidar acquires a local point cloud within a rectangular frame 10m long and 10m wide in the radar coordinate system, centered on the sanitation vehicle. The point cloud is then mapped to the global coordinate system using the TF transformation output by the integrated navigation system. During the sanitation vehicle's motion, the local point cloud in the global coordinate system includes the point cloud of the target person and the point cloud of dynamic obstacles. This local point cloud is first filtered using a SOR filter to remove outliers, projected onto a horizontal plane, and then divided into multiple clusters using the DBSCAN clustering algorithm. The point cloud in each cluster generates a maximum enclosing ellipse, and the center coordinates of each ellipse locate the two-dimensional coordinates of the target or obstacle in the global coordinate system. Each ellipse is then assigned a numerical label, allowing the sanitation vehicle to continuously acquire local point clouds for each frame in chronological order during its motion. Each local point cloud frame includes multiple ellipses distributed at different locations. The target and obstacle ellipses of the previous frame are positionally associated with the target and obstacle ellipses of the current frame using the Hungarian algorithm. The Hungarian algorithm calculates the distance similarity between targets to construct an edge weight matrix for the bipartite graph, thereby finding the optimal matching solution.

[0090] It can be understood that in the Hungarian algorithm, the digital labels of the previous frame are inherited for the objects that meet the distance requirements after matching, and new digital labels are assigned for the objects that do not meet the distance requirements, thereby enabling the sanitation vehicle to continuously track the following targets and obstacles.

[0091] Specifically, the initial marking method for the following target is as follows: the target person stands in front of the sanitation vehicle, and only one frame of the target person's point cloud appears in the local point cloud to complete the marking of the initial following target. In each subsequent frame of the local point cloud, all point cloud clusters except the point cloud cluster marked as the target are marked as obstacle point clouds.

[0092] In combination with all of the above embodiments, the present invention provides a target-tracking self-following system and method for sanitation vehicles. By using a sanitation vehicle equipped with a high-precision combined navigation system, a laser radar, and a high-definition camera, sensor data acquisition and processing can be used to accurately identify and locate the designated sanitation personnel or other obstacles. The present invention can ensure that the sanitation vehicle can continuously acquire images containing the target being followed under the constraints of obstacle avoidance motion, thereby increasing the effective time of visual target detection. At the same time, the method of combined target tracking using a camera and a laser radar improves the robustness of target tracking, ensuring that the sanitation vehicle can use the sanitation personnel's historical motion path, i.e., the guided cleaning path, as a reference when performing following or obstacle avoidance motion in sanitation scenarios, thereby achieving efficient and reliable semi-unmanned sanitation cleaning operations.

[0093] The above description is only a preferred embodiment of the present invention and does not limit the patent scope of the present invention. All equivalent structural transformations made by using the contents of the present invention description and drawings under the inventive concept of the present invention, or direct / indirect application in other related technical fields are included in the patent protection scope of the present invention.

Claims

1. A target tracking sanitation vehicle self-following system, characterized in that: It includes the sanitation vehicle body, sensor input unit, perception planning unit, and motion control unit; The sanitation vehicle body includes a vehicle control unit and a vehicle body, and the vehicle control unit is connected to the vehicle body; The sensor input unit includes a three-dimensional laser radar, a rotating camera device and a combined navigation system, wherein the three-dimensional laser radar, the depth camera and the rotating camera device are all arranged on the vehicle body; The perception planning unit also includes a target tracking and positioning module, a path planning and dynamic obstacle avoidance module, and the target tracking and positioning module is signal-connected to the path planning and dynamic obstacle avoidance module; The motion control unit is connected to the vehicle control unit by signals; Among them, the sensor input unit, the perception planning unit and the motion control unit are all arranged on the sanitation vehicle body.

2. The target tracking self-following system for sanitation vehicles according to claim 1, characterized in that: The rotating camera device includes a depth camera mount, an embedded development board and an inertial measurement unit. The depth camera mount is arranged at the front end of the vehicle body, the depth camera is arranged on the vehicle body through the depth camera mount, and the depth camera mount is fixedly connected to the inertial measurement unit. The inertial measurement unit, the depth camera and the three-dimensional laser radar are all signal-connected to the embedded development board.

3. The target tracking self-following system for sanitation vehicles according to claim 2, characterized in that: The rotating camera device also includes a stepper motor, one end of which is connected to the camera mounting seat, and the other end of which is connected to the vehicle body. The camera mounting seat is movably connected to the vehicle body through the stepper motor, and a signal connection is established between the stepper motor and the embedded development board.

4. A target tracking sanitation vehicle self-following method, characterized in that: The target tracking sanitation vehicle self-following method is applied to the target tracking sanitation vehicle self-following system according to any one of claims 1 to 3, comprising the following steps: Receive scene information acquired by the sensor and obtain the two-dimensional coordinates of the tracking target and obstacles; The acquired scene information is used to obtain the motion paths of targets and obstacles through the target position detection algorithm; The motion paths of the tracked targets and obstacles are calculated by the local path planner to output the required speed and front angle of the sanitation vehicle. Based on the calculated required speed and front turning angle of the sanitation vehicle, the signal is transmitted to the vehicle control unit, enabling the sanitation vehicle to follow the motion path of the tracking target and avoid obstacles.

5. The target tracking self-following method for sanitation vehicles according to claim 4, characterized in that: The step of receiving scene information acquired by the sensor and acquiring the two-dimensional coordinates of the tracking target and the obstacle includes: Use 3D laser radar to scan the surrounding scene and generate point cloud data; Use a depth camera to obtain color images and depth images of the tracking target or obstacle; Obtain the three-dimensional coordinates of the target in the camera coordinate system through the color image and depth image; The point cloud data obtained by the 3D lidar scan and the 3D coordinate data obtained by the depth camera are processed to form a joint target tracking method; After continuously tracking the target and obstacles by labeling the point cloud clusters, the two-dimensional coordinates of the target and obstacles are continuously output, and the three-dimensional coordinates of the target obtained by the depth camera at that moment are projected onto the horizontal plane to form two-dimensional coordinates; Calculate the bi-norm of the plane two-dimensional coordinate vector output by point cloud target tracking and the plane two-dimensional coordinate vector output by visual target detection. When the bi-norm is less than the set threshold, it is marked as a target as the output of the final target coordinates.

6. The target tracking self-following method for sanitation vehicles according to claim 5, characterized in that: The step of obtaining the three-dimensional coordinates of the target in the camera coordinate system through the color image and the depth image includes: Get the three-dimensional coordinates of the target center relative to the color camera coordinate system; The three-dimensional coordinates of the target center relative to the navigation coordinate system are obtained through the conversion matrix from the color camera coordinate system to the color camera initial coordinate system after camera rotation, the conversion matrix from the lidar coordinate system to the navigation system coordinate system, and the conversion matrix from the depth camera initial coordinate system to the lidar coordinate system; By obtaining the conversion matrix from the navigation coordinate system output by the integrated navigation system to the navigation initial coordinate system, the three-dimensional coordinates of the target center relative to the navigation initial coordinate system are obtained.

7. The target tracking self-following method for sanitation vehicles according to claim 6, characterized in that: The step of using a three-dimensional laser radar to scan the surrounding scene and generate point cloud data includes: Using the three-dimensional laser radar on the sanitation vehicle to scan the surrounding scene, establishing a radar coordinate system with the sanitation vehicle as the center, and obtaining local point cloud information within the radar coordinate system; Remove outliers from the local point cloud information and project the local point cloud information onto the horizontal plane. Then, use the DBSCAN clustering algorithm to divide the horizontal plane point cloud information into multiple clusters. For the point cloud in each cluster, calculate a maximum enclosing ellipse. The center coordinates of each ellipse are calibrated as the two-dimensional coordinates of the target or obstacle in the global coordinate system. The coordinate transformation point cloud output by the integrated navigation system is mapped to the global coordinate system to generate global point cloud data.

Images