Tracking control method and system for engineering machinery by power transmission vehicle based on multiple sensors

Abstract

The invention discloses a tracking control method and system for engineering machinery by a power transmission vehicle based on multiple sensors, and the method comprises the steps: inputting an image containing target engineering machinery into a target detection model for detection, outputting the position and category label of the target engineering machinery, and calculating the azimuth angle of the target engineering machinery; based on the preprocessed three-dimensional point cloud data, calculating to obtain an accurate distance from the power transmission vehicle to the target engineering machinery, and based on the preprocessed three-dimensional point cloud data, predicting a movement track of an obstacle; fusing the azimuth angle, the accurate distance and the course angle of the power transmission vehicle to obtain a target engineering machinery pose; and constructing an MPC model based on the position, the speed and the course angle of the power transmission vehicle, embedding the motion trail of the obstacle into the MPC model, inputting the pose of the target engineering machinery into the MPC model, and dynamically generating a control quantity based on an optimization target. The path of the power transmission vehicle in the following process is optimized through sensing, decision making and control multidirectional design, obstacle avoidance is considered in the following process, and the following automation and positioning precision are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of following control, and in particular to a method and system for controlling a transmission vehicle to follow engineering machinery based on multiple sensors. Background Art

[0002] The statements in this section merely provide background information related to the present disclosure and do not necessarily constitute prior art.

[0003] In large-scale construction scenarios (such as mines and construction sites), transmission vehicles need to dynamically follow construction machinery (such as excavators and loaders) and continuously provide power. Currently, transmission vehicles rely primarily on manual operation or a single sensor system to follow construction machinery, which presents the following problems: (1) Low degree of automation: The existing fully automatic system is insufficient in adaptability to dynamic environments and obstacle recognition accuracy, making it difficult to cope with complex working conditions (such as mines and construction sites).

[0004] (2) Insufficient positioning and path planning accuracy: When traditional lidar or vision systems are used alone, it is difficult to achieve both long-range direction perception and short-range precise positioning, resulting in deviations in tracking trajectory.

[0005] (3) Weak dynamic response capability: Existing path planning algorithms (such as A* and RRT) do not incorporate real-time sensor feedback and cannot adapt to sudden movements of construction machinery or interference from obstacles.

[0006] Existing technologies, such as the proposed ranging method based on the combination of binocular and monocular cameras, have not solved the problems of high matching failure rate, insufficient real-time performance and high hardware cost in weak texture scenes. For example, a binocular camera ranging method based on Yolov5 and an improved tracking algorithm uses Yolov5 and a binocular camera for ranging, but has the defects of poor long-distance accuracy, high computing power requirements and lack of multi-sensor coordination. Summary of the Invention

[0007] To overcome the above-mentioned deficiencies in the prior art, the present invention provides a multi-sensor based transmission vehicle tracking control method and system for construction machinery. The system optimizes the transmission vehicle following process through multi-faceted design of perception, decision-making, and control, and considers obstacle avoidance during the following process, thereby improving the following automation and dynamic adaptation to sudden changes in the speed of the target construction machinery.

[0008] To achieve the above objectives, one or more embodiments of the present invention provide the following technical solutions: In a first aspect, the present invention provides a multi-sensor based method for controlling a transmission vehicle to follow a construction machine, comprising: The monocular camera is used to acquire images containing the target construction machinery, the laser radar is used to scan the environment to obtain 3D point cloud data, and the inertial measurement unit is used to obtain the heading angle of the transmission vehicle. Input the image into a target detection model for detection, output the position and category label of the target engineering machinery, and calculate its azimuth based on the position of the target engineering machinery; preprocess the three-dimensional point cloud data, calculate the precise distance from the transmission vehicle to the target engineering machinery based on the preprocessed three-dimensional point cloud data, and predict the obstacle movement trajectory based on the preprocessed three-dimensional point cloud data; The azimuth angle, precise distance and heading angle of the transmission vehicle are integrated to obtain the target engineering machinery posture; An MPC model is constructed based on the position, speed and heading angle of the transmission vehicle, the obstacle motion trajectory is embedded in the MPC model, the target construction machinery posture is input into the MPC model, and the control variable is dynamically generated based on the optimization target.

[0009] A further technical solution is to calculate the azimuth angle of the target engineering machinery based on its position as follows: the center point of the bottom edge of the bounding box of the target engineering machinery is selected as the calculation point, and the intrinsic parameter matrix of the monocular camera is obtained to calculate the azimuth angle of the target engineering machinery.

[0010] A further technical solution is to calculate the precise distance from the transmission vehicle to the target construction machinery based on the pre-processed 3D point cloud data: Clustering algorithm is used to cluster the pre-processed 3D point cloud data, setting the distance threshold and minimum number of points, and outputting the point cloud cluster of the target construction machinery; Calculate the three-dimensional center coordinates of the target engineering machinery based on the point cloud cluster, transform the three-dimensional center coordinates into the transmission vehicle coordinate system, and obtain the coordinates of the target engineering machinery; The precise distance from the transmission vehicle to the target construction machinery is calculated based on the coordinates of the target construction machinery.

[0011] A further technical solution is to predict the obstacle trajectory based on the pre-processed 3D point cloud data: Clustering algorithm is used to cluster the pre-processed 3D point cloud data, setting the distance threshold and minimum number of points, and outputting the point cloud cluster of the obstacle; Calculate the three-dimensional geometric center coordinates of the obstacle based on its point cloud cluster, transform its three-dimensional geometric center coordinates into the transmission vehicle coordinate system, and obtain the obstacle coordinates; Matching and associating the obstacle bounding box, category label, tracking ID, and semantic label with the obstacle coordinates to obtain a successfully matched obstacle instance; For the obstacle instance, a state estimation algorithm is used to update the obstacle motion state, thereby obtaining a historical state sequence for a set time period; The historical state sequence is input into a pre-trained obstacle motion prediction model for prediction to obtain the obstacle motion trajectory.

[0012] According to a further technical solution, the obstacle motion prediction model is constructed using a long short-term memory network and trained using a multi-scene trajectory dataset.

[0013] In a further technical solution, the optimization objective of the MPC model is expressed as:

[0014] in, represents the length of the prediction time domain, Indicates the first step, represents the weighted sum of squares of state tracking errors, Indicates the The reference state vector of the step, represents the state weight matrix, represents the weighted sum of squares of the control quantities, Indicates the The control vector for the step-by-step prediction, represents the control weight matrix, represents the weighted sum of squares of the control variable change rate, Indicates the The change of step control quantity, Represents the control change rate weight matrix.

[0015] According to a further technical solution, the constraints of the MPC model are control quantity limitations, dynamic constraints and obstacle avoidance constraints.

[0016] In a second aspect, the present invention provides a multi-sensor based transmission vehicle tracking control system for construction machinery, comprising: A data acquisition module is configured to: acquire an image containing the target engineering machinery based on a monocular camera, acquire three-dimensional point cloud data based on a laser radar scanning environment, and acquire a heading angle of the transmission vehicle based on an inertial measurement unit; a parameter calculation module configured to: input the image into a target detection model for detection, output the position and category label of the target engineering machinery, and calculate the azimuth of the target engineering machinery based on the position of the target engineering machinery; preprocess the three-dimensional point cloud data, calculate the precise distance from the transmission vehicle to the target engineering machinery based on the preprocessed three-dimensional point cloud data, and predict the movement trajectory of the obstacle based on the preprocessed three-dimensional point cloud data; a target posture generation module, configured to: fuse the azimuth angle, precise distance, and heading angle of the transmission vehicle to obtain a target engineering machinery posture; The target control quantity generation module is configured to: construct an MPC model based on the position, speed and heading angle of the transmission vehicle, embed the obstacle motion trajectory into the MPC model, input the target construction machinery posture into the MPC model, and dynamically generate the control quantity based on the optimization target.

[0017] In a third aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method for controlling a transmission vehicle to follow construction machinery based on multiple sensors as described in the first aspect.

[0018] In a fourth aspect, the present invention provides a computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of the method for controlling a transmission vehicle to follow construction machinery based on multiple sensors as described in the first aspect are implemented.

[0019] One or more of the above technical solutions have the following beneficial effects: The present invention improves the automation level of following control and the following accuracy through a full-link optimization method of perception, decision-making and control. It can also respond in combination with real-time sensor data, support sudden changes in engineering machinery speed and obstacle interference, and shorten control delays.

[0020] The transmission vehicle in this invention uses a multi-sensor collaboration: a monocular camera provides long-range semantic information, and a lidar achieves high-precision three-dimensional perception, improving environmental modeling efficiency and positioning accuracy. The target construction machinery is first roughly located using monocular camera data, and then precisely located using lidar data. The distance between the current position and the target construction machinery is calculated. The azimuth angle obtained by the monocular camera and the transmission vehicle's heading angle obtained by the inertial measurement unit are combined to determine the target construction machinery's pose, providing input for the MPC model.

[0021] The present invention integrates the semantic labels of obstacle target detection with the obstacle coordinates obtained by the laser radar, and accurately predicts the trajectory of the obstacle in a set time period in the future based on the obstacle motion prediction model, thereby shortening the obstacle avoidance response time of the transmission vehicle.

[0022] The present invention embeds the predicted obstacle trajectory into the constructed MPC model to optimize the steering angle and cable speed in real time; and designs a closed-loop tension control system to reduce cable tension fluctuations and thus reduce the risk of cable breakage; it also introduces a scene complexity weight distribution mechanism to dynamically adjust the sensor data weight according to the scene complexity to enhance the robustness of the system.

[0023] The model predictive control model (MPC) in the present invention combines real-time sensor data to obtain optimized control quantities, realizes the following control of the transmission vehicle on the construction machinery, and realizes real-time obstacle avoidance based on the obstacle motion trajectory generated by the obstacle motion prediction model, thereby optimizing the path smoothness and obstacle avoidance capability.

[0024] In environments such as mines and construction sites, the transmission vehicle of the present invention can autonomously follow construction machinery such as excavators and loaders and adjust the length and position of the cable in real time. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] The accompanying drawings, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0026] Figure 1 This is a flow chart of a method for controlling a transmission vehicle to follow construction machinery based on multiple sensors according to an embodiment of the present invention; Figure 2 This is a flow chart of the MPC model prediction in accordance with an embodiment of the present invention; Figure 3 This is a schematic diagram of the cable tension control principle according to an embodiment of the present invention; Figure 4 It is a flowchart of the dynamic obstacle avoidance process of the transmission vehicle according to an embodiment of the present invention. DETAILED DESCRIPTION

[0027] It should be noted that the following detailed descriptions are exemplary and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which the present invention belongs.

[0028] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0029] In the absence of conflict, the embodiments of the present invention and the features thereof may be combined with each other.

[0030] Example 1 like Figure 1 As shown, this embodiment discloses a multi-sensor based transmission vehicle tracking control method for construction machinery, the method comprising the following steps: S1: Use a monocular camera to acquire an image containing the target construction machinery, use a lidar to scan the environment to obtain 3D point cloud data, and use an inertial measurement unit to obtain the heading angle of the transmission vehicle; In this embodiment, a transmission vehicle is equipped with multiple sensors to perceive target machinery, obstacles, and the environment. These sensors include a monocular camera, a lidar, a global positioning system (GPS), and an inertial measurement unit (IMU). The monocular camera (e.g., with a horizontal field of view ≥120°, 4K resolution, and a frame rate of 30fps) is used for long-range target recognition and semantic segmentation; the lidar (e.g., a 32-line lidar with a detection range ≥100m and an angular resolution of 0.1°) is used to generate high-precision point cloud maps; the GPS and IMU serve as auxiliary sensors. The monocular camera is mounted on the roof of the transmission vehicle with an adjustable pitch angle (-10° to +20°), while the lidar is fixed to the vehicle's anti-vibration mount. All sensors utilize commercially available components, resulting in a lower system cost than custom solutions.

[0031] The monocular camera captures RGB images, which need to be aligned with the lidar data. Data from all sensors must be synchronized in space and time so that the fusion algorithm can correctly correlate the data from different sensors.

[0032] S2: Input the image into a target detection model for detection, output the position and category label of the target construction machinery, and calculate its azimuth based on the position of the target construction machinery; preprocess the three-dimensional point cloud data, calculate the precise distance from the transmission vehicle to the target construction machinery based on the preprocessed three-dimensional point cloud data, and predict the obstacle movement trajectory based on the preprocessed three-dimensional point cloud data; In this embodiment, before the target detection model receives an image, it pre-calculates the target position of the target construction machinery using GPS data. Specifically, the coordinates of the target construction machinery and the transmission vehicle's own coordinates are converted into plane coordinates using UTM projection. The initial azimuth angle of the target construction machinery relative to the transmission vehicle is calculated. The monocular camera's gimbal is driven to steer the target construction machinery within a pre-set detection region of interest (ROI), capturing an image of the target construction machinery within the ROI. The YOLOv8 object detection algorithm is then run within the ROI, significantly improving the real-time detection of the target construction machinery and its robustness in occluded scenes.

[0033] The object detection model uses the YOLOv8 model. RGB images are fed into the YOLOv8 model for object detection. The model outputs bounding boxes (target locations), category labels (identified target types, such as excavators and trucks), and confidence scores for all objects in the image (including target construction machinery and obstacles). During object detection model training, a list of target categories is defined so that the model can identify the target construction machinery to follow based on the category labels and output the bounding box, category label, and confidence score for the target construction machinery.

[0034] The azimuth angle of the target construction machinery is calculated by using the position of the target construction machinery and combining the camera's internal parameters. The specific steps are as follows: (1) Select the calculation point and obtain the camera intrinsic parameter matrix (obtained through calibration); Select the center point of the bottom edge of the bounding box of the target construction machinery As a calculation point, ,in, is the x coordinate of the center point, is the y coordinate of the center point, is the height of the bounding box, Get the y coordinate of the center point of the bottom edge.

[0035] (2) Calculate the azimuth angle of the target construction machinery.

[0036] The calculation formula for the azimuth angle is:

[0037] in, Indicates the azimuth of the target construction machinery, represents the x-coordinate of the camera's optical center in the image, Indicates the focal length of the camera in the x direction.

[0038] In this embodiment, all objects detected by the target detection model are assigned semantic labels of dynamic obstacles or static obstacles based on their category labels and motion states. Specifically: (1) Use target tracking algorithms (such as SORT, IOU tracker) to convert consecutive frames (such as and ) detection box, assigning a unique tracking ID to each object target and maintaining its trajectory.

[0039] (2) Calculate the displacement of the target object. For the successfully tracked target, obtain the coordinates of the center point of its bounding box in the continuous frame and calculate the continuous Frame displacement , .

[0040] (3) Set a displacement threshold (the threshold needs to be adjusted experimentally based on the camera viewing angle, target distance, and application scenario), and compare the calculated displacement of the target object with the displacement threshold. If the displacement exceeds the threshold, it is a dynamic obstacle, and if the displacement is lower than the threshold, it is a static obstacle. The semantic label is obtained based on the comparison result.

[0041] In this embodiment, the 3D point cloud data is preprocessed to obtain preprocessed 3D point cloud data. The preprocessing includes denoising, ground segmentation, and voxel filtering. Denoising is used to remove invalid points, and voxel filtering downsamples the data to reduce the amount of computation.

[0042] The distance calculation process between the transmission vehicle and the target construction machinery is as follows: (1) Clustering the preprocessed three-dimensional point cloud data using a clustering algorithm, setting a distance threshold and a minimum number of points (set according to the size of the target engineering machinery), and outputting the point cloud cluster of the target engineering machinery; this embodiment samples the DBSCAN clustering algorithm.

[0043] (2) Calculate the three-dimensional center coordinates of the target engineering machinery based on the point cloud cluster of the target engineering machinery ; expressed as:

[0044]

[0045]

[0046] in, Represents the number of points in the point cloud cluster of the target engineering machinery, The point cloud cluster representing the target engineering machinery The coordinates of a point.

[0047] (3) Convert the three-dimensional center coordinates of the target engineering machinery to the transmission vehicle coordinate system to obtain the coordinates of the target engineering machinery in the transmission vehicle coordinate system That is, the target engineering machinery coordinates, based on the ground, ignoring the target engineering machinery height, can be simplified to ; (4) Calculate the exact distance from the transmission vehicle to the target construction machinery, expressed as: .

[0048] like Figure 4 As shown in the figure, it also identifies obstacles in the environment and predicts their motion trajectories. The specific steps are: (1) Obstacle point cloud cluster extraction 1) Apply a clustering algorithm (DBSCAN algorithm is used in this embodiment) to the pre-processed point cloud of the entire scene (3D point cloud data, not limited to the target construction machinery area).

[0049] 2) Set a distance threshold (such as 0.5m) and a minimum number of points (such as 5 points) to aggregate spatially adjacent points into clusters.

[0050] 3) Output all identified obstacle point cloud clusters (each cluster represents a potential obstacle).

[0051] (2) Calculation of the geometric center of the obstacle For each obstacle point cloud cluster identified in step (1), calculate its 3D geometric center coordinates: , ,

[0052] in, represents the number of points in the obstacle cluster, Represents the coordinates of the i-th point in the cluster.

[0053] The geometric center coordinates are converted to the transmission vehicle coordinate system to obtain the position of the obstacle in the transmission vehicle coordinate system, that is, the obstacle coordinates.

[0054] (3) Fusion of obstacle semantics and motion status The obstacle bounding box, category label, unique tracking ID, and preliminary motion state (semantic label) information obtained based on visual detection (YOLOv8) and tracking (such as SORT / IOU) are temporally and spatially associated with the lidar obstacle position (obstacle coordinates) calculated in step (2) (this embodiment uses the Hungarian algorithm for matching).

[0055] After successful matching, each obstacle instance is assigned a precise geometric location, a semantic label (such as “excavator”, “truck”, “person”), and a unique tracking ID.

[0056] (4) Obstacle motion state estimation For successfully matched obstacle instances, their historical position sequence (e.g., 5 consecutive frames with a time interval of 100ms) and current frame position are used.

[0057] A state estimation algorithm (in this example, the Extended Kalman Filter (EKF)) is used to update the obstacle's motion state, thereby obtaining a historical state sequence for a set time period. The state vector typically contains position, velocity, and acceleration. The system also estimates the acceleration of the target construction machinery and obstacles in real time, preventing collision risks caused by sudden speed changes between the target construction machinery and dynamic obstacles and triggering emergency path replanning when necessary.

[0058] EKF fuses continuous observation data and outputs the state vector of the optimal estimate of the obstacle at the current moment.

[0059] (5) Obstacle trajectory prediction The obstacle's historical state sequence (such as position and velocity, i.e., 4-dimensional input) over a period of time (e.g., 1 second, corresponding to 20 frames) is input into a pre-trained obstacle motion prediction model to obtain the obstacle motion trajectory.

[0060] The obstacle motion prediction model is constructed using a long short-term memory (LSTM) network and trained using a multi-scenario trajectory dataset. The training data comes from: real mining environments (open-pit mines, 120 hours of video and point clouds), construction site scenes (80 hours of monitoring data), and conversions of public autonomous driving datasets (Waymo, nuScenes). The dataset contains the motion trajectories of targets such as construction machinery, transport vehicles, and construction workers. Each trajectory is annotated with: (1) target category and ID, such as construction machinery movement, excavator / loader; personnel walking, worker / safety officer; vehicle transportation, truck / forklift; (2) continuous frame position coordinates (x, y); and (3) motion state (velocity vx, vy).

[0061] The model input is a trajectory sequence of the past 1 second (20 frames × 4 dimensions) and outputs a sequence of predicted trajectory points for a period of time in the future (e.g., 2.5 seconds, corresponding to 50 points). This predicted trajectory reflects the most likely future movement path of the obstacle.

[0062] S3: Fusing the azimuth angle, precise distance, and heading angle of the transmission vehicle to obtain the target engineering machinery posture; In this embodiment, the extended Kalman filter (EKF) is used to fuse the azimuth , precise distance and the heading angle of the transmission vehicle , output the estimated position of the target in the transmission vehicle coordinate system , combined with the transmission vehicle heading angle Constructing a complete target pose .

[0063] S4: constructing an MPC model based on the position, speed and heading angle of the transmission vehicle, embedding the obstacle motion trajectory into the MPC model, inputting the target construction machinery posture into the MPC model, and dynamically generating control variables based on the optimization target.

[0064] In this embodiment, if Figure 2 As shown in Figure 2, the process of building a model predictive control MPC model is as follows: State variable: Transmission vehicle position ,speed , heading angle , that is, the input vector .

[0065] Control variable: Steering angle , Cable retraction speed , that is, the output vector .

[0066] Discretized model (forward Euler method, step size ):

[0067]

[0068]

[0069]

[0070] in, Indicates the The position coordinates of the transmission vehicle in the global coordinate system at the time step, represents the current discrete time step index, Indicates the The linear speed of the transmission vehicle at each step, The time constant of the inertia of the transmission vehicle speed response, Indicates the The heading angle of the transmission vehicle at the time of step, Indicates the distance between the front and rear axles of the transmission vehicle.

[0071] Optimization goal: minimize tracking error, path curvature, and control variable change rate. The formula is:

[0072] in, Indicates the length of the prediction time domain (number of prediction steps), Indicates the first Step (from the current moment start), represents the weighted sum of squares of state tracking errors, Indicates the The reference state vector of the step, represents the state weight matrix, represents the weighted sum of squares of the control quantities, Indicates the The control vector for the step-by-step prediction, represents the control weight matrix, represents the weighted sum of squares of the control variable change rate, Indicates the The change of step control quantity, Represents the control change rate weight matrix.

[0073] Constraints: Control volume limit:

[0074] Dynamic constraints:

[0075] Obstacle avoidance constraints:

[0076] The current state of the transmission vehicle and the target construction machinery posture are input into the constructed MPC model, and the MPC model predicts the future The state variables of the step are used to solve the optimization objective using a constrained sampling QP solver, and the output control variables are the steering angle and cable retraction and release speed. Based on the output control variables, the cable retraction and release motor executes instructions to adjust the cable length and feeds back the actual tension to the controller to ensure constant tension.

[0077] The cable retraction and extension amount is calculated in real time based on the following distance (the precise distance between the transmission vehicle and the target construction machinery calculated in real time), and the optimal retraction and extension speed is output through MPC. , combined with the tension sensor feedback to form a closed-loop control. The PID controller adjusts the speed of the retracting and releasing mechanism to ensure constant cable tension.

[0078] like Figure 3 As shown, closed-loop control is achieved through a closed-loop tension control system, specifically: (1) The tension sensor collects the actual cable tension value in real time ; (2) Calculation of tension error ( ); (3) The PID controller generates compensation based on the error:

[0079] in, Indicates the adjustment amount added on the basis of MPC output speed, Indicates tension error, that is, the difference between the set tension and the actual tension. Represents the cumulative sum of historical errors , Indicates the rate of change of error.

[0080] (4) The cable speed command output by MPC Corrected to ; (5) The retracting and extending mechanism executes the corrected speed instruction ; (6) When the risk of cable breakage is detected (instantaneous rate of tension drop) ) triggers emergency braking.

[0081] In this embodiment, if Figure 4 As shown in Figure 2, the dynamic obstacle avoidance during the transmission vehicle following process is specifically as follows: Obtain the obstacle prediction trajectory output by the obstacle motion prediction model, and embed the obstacle prediction trajectory into the MPC controller to realize obstacle avoidance decision: (1) Security constraint generation Differentiated safety distances are set based on semantic labels (e.g., 3.0m for construction machinery and 1.8m for personnel), and obstacle avoidance constraint equations are constructed within the MPC prediction domain (k = 1 to 50):

[0082] in, Indicates a safe distance.

[0083] (2) Real-time control solution The qpOASES solver was used to calculate the optimal control variables (e.g., steering angle δ∈[-30°, 30°], cable speed L∈[-2, 2] m / s). Optimization focused on: short-term (0-2s) prioritizing hard obstacle avoidance constraints; long-term (2-5s) minimizing path tracking error. A dynamic response mechanism, MPC rolling optimization, was implemented, using time-domain decomposition control. In the short term, obstacle avoidance maneuvers were intensively optimized with a 100ms step size, while in the long term, global efficiency was optimized with a 500ms step size. Constraints were also injected in real time. When the target construction machinery was approaching, the MPC reference trajectory was updated to an emergency following curve, tightening the dynamic constraints.

[0084] (3) Closed-loop verification and update The actual posture deviation is verified by the IMU and wheel speedometer. If the actual position deviates from the predicted position by more than 0.3m, the trajectory re-prediction is triggered and the obstacle status database is updated (for the next cycle prediction).

[0085] The obstacle motion prediction model updates the predicted path every 50ms and adjusts the control variables based on the predicted obstacle trajectory to ensure a safe distance and achieve rolling optimization of the MPC model.

[0086] Add obstacle avoidance constraints to the optimization objectives of the MPC model, design collision penalty functions and obstacle avoidance hard constraints, and consider the obstacle trajectory during MPC control.

[0087] In this embodiment, an adaptive weight allocation mechanism is also designed to dynamically adjust the sensor data weight according to the scene complexity to enhance the robustness of the system. The specific steps are: (1) Scenario complexity assessment At the beginning of each control cycle (e.g., 50ms), the scene complexity is evaluated based on the current perception data. This embodiment uses the following criteria: Dynamic obstacle density: counts the number of obstacles marked as "dynamic" in the current field of view ; Average obstacle relative speed: calculate the average speed of all dynamic obstacles relative to the transmission vehicle .

[0088] Scene complexity It can be defined as the weighted sum of the two (the weight needs to be determined according to the actual system debugging), expressed as , A larger value indicates a more complex scene (more and faster obstacles).

[0089] (2) Sensor data weight calculation Dynamically adjust the trust weights of different sensor data in subsequent fusion (such as EKF in S3) and MPC optimization based on scene complexity. Define basic weights: lidar distance weight, monocular azimuth weight, and visual semantics / obstacle information weight.

[0090] Weight adjustment strategy example: When the weight is low (simple scene): maintain or slightly increase the weight of monocular azimuth (long-distance rough information is more reliable), maintain the weight of lidar distance and visual semantics / obstacle information; when When the value is high (complex scenes with many dynamic obstacles in the near distance): significantly increase the weight of lidar distance and visual semantic / obstacle information (accurate geometry and real-time semantic information at close range are crucial), and reduce the weight of monocular azimuth angle (which may be affected by occlusion and dynamic objects).

[0091] Specific weight mapping function It can be determined experimentally or based on rules (such as piecewise linear function, lookup table).

[0092] (3) Weight application In pose fusion (S3 EKF), the calculated lidar range weight and monocular azimuth weight are used as the inverse of the corresponding observation noise covariance matrix in the EKF (or directly as observation weights), affecting the fusion result. Sensor data with higher weights has a greater impact on the final fused pose.

[0093] In MPC optimization, the visual semantics / obstacle information weights are incorporated into the processing of obstacle-related constraints. For example, for dynamic obstacle prediction trajectories with high confidence visual semantics / obstacle information weights, the corresponding obstacle avoidance constraints or safety distances are in the optimization target. The weight in can be higher.

[0094] Example 2 This embodiment discloses a multi-sensor based transmission vehicle tracking control system for construction machinery, including: A data acquisition module is configured to: acquire an image containing the target engineering machinery based on a monocular camera, acquire three-dimensional point cloud data based on a laser radar scanning environment, and acquire a heading angle of the transmission vehicle based on an inertial measurement unit; a parameter calculation module configured to: input the image into a target detection model for detection, output the position and category label of the target engineering machinery, and calculate the azimuth of the target engineering machinery based on the position of the target engineering machinery; preprocess the three-dimensional point cloud data, calculate the precise distance from the transmission vehicle to the target engineering machinery based on the preprocessed three-dimensional point cloud data, and predict the movement trajectory of the obstacle based on the preprocessed three-dimensional point cloud data; a target posture generation module, configured to: fuse the azimuth angle, precise distance, and heading angle of the transmission vehicle to obtain a target engineering machinery posture; The target control quantity generation module is configured to: construct an MPC model based on the position, speed and heading angle of the transmission vehicle, embed the obstacle motion trajectory into the MPC model, input the target construction machinery posture into the MPC model, and dynamically generate the control quantity based on the optimization target.

[0095] Example 3 The purpose of this embodiment is to provide a computing device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method of embodiment 1 when executing the program.

[0096] Example 4 The purpose of this embodiment is to provide a computer-readable storage medium, a computer-readable storage medium having a computer program stored thereon, which performs the steps of the method of embodiment 1 when executed by a processor.

[0097] The steps involved in the apparatuses of Examples 3 and 4 above correspond to those of Method Example 1. For detailed implementation, please refer to the relevant description of Example 1. The term "computer-readable storage medium" should be understood to mean a single medium or multiple media containing one or more instruction sets; it should also be understood to include any medium capable of storing, encoding, or carrying an instruction set for execution by a processor and causing the processor to perform any of the methods of the present invention.

[0098] Those skilled in the art will appreciate that the modules or steps of the present invention described above can be implemented using a general-purpose computer device. Alternatively, they can be implemented using program code executable by a computing device, which can then be stored in a storage device and executed by the computing device. Alternatively, they can be fabricated into separate integrated circuit modules, or multiple modules or steps can be fabricated into a single integrated circuit module for implementation. The present invention is not limited to any specific combination of hardware and software.

[0099] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

[0100] Although the above describes the specific embodiments of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without any creative work are still within the scope of protection of the present invention.

Claims

1. A multi-sensor based transmission vehicle tracking control method for construction machinery, characterized in that: include: The monocular camera is used to acquire images containing the target construction machinery, the laser radar is used to scan the environment to obtain 3D point cloud data, and the inertial measurement unit is used to obtain the heading angle of the transmission vehicle. Inputting the image into a target detection model for detection, outputting the position and category label of the target engineering machinery, and calculating the azimuth angle of the target engineering machinery based on the position of the target engineering machinery; Preprocessing the three-dimensional point cloud data, calculating the precise distance between the transmission vehicle and the target engineering machinery based on the preprocessed three-dimensional point cloud data, and predicting the obstacle's motion trajectory based on the preprocessed three-dimensional point cloud data; The azimuth angle, precise distance and heading angle of the transmission vehicle are integrated to obtain the target engineering machinery posture; An MPC model is constructed based on the position, speed and heading angle of the transmission vehicle, the obstacle motion trajectory is embedded in the MPC model, the target construction machinery posture is input into the MPC model, and the control variable is dynamically generated based on the optimization target.

2. The multi-sensor based transmission vehicle to construction machinery following control method according to claim 1, characterized in that: Calculating the azimuth angle of the target engineering machinery based on its position is specifically as follows: selecting the center point of the bottom edge of the bounding box of the target engineering machinery as the calculation point, obtaining the intrinsic parameter matrix of the monocular camera, and calculating the azimuth angle of the target engineering machinery.

3. The multi-sensor based transmission vehicle to construction machinery following control method according to claim 1, characterized in that: The precise distance from the transmission vehicle to the target construction machinery is calculated based on the pre-processed 3D point cloud data: Clustering algorithm is used to cluster the pre-processed 3D point cloud data, setting the distance threshold and minimum number of points, and outputting the point cloud cluster of the target construction machinery; Calculate the three-dimensional center coordinates of the target engineering machinery based on the point cloud cluster, transform the three-dimensional center coordinates into the transmission vehicle coordinate system, and obtain the coordinates of the target engineering machinery; The precise distance from the transmission vehicle to the target construction machinery is calculated based on the coordinates of the target construction machinery.

4. The multi-sensor based transmission vehicle tracking control method for construction machinery according to claim 1, characterized in that: The obstacle trajectory is predicted based on the preprocessed 3D point cloud data as follows: Clustering algorithm is used to cluster the pre-processed 3D point cloud data, setting the distance threshold and minimum number of points, and outputting the point cloud cluster of the obstacle; Calculate the three-dimensional geometric center coordinates of the obstacle based on its point cloud cluster, transform its three-dimensional geometric center coordinates into the transmission vehicle coordinate system, and obtain the obstacle coordinates; Matching and associating the obstacle bounding box, category label, tracking ID, and semantic label with the obstacle coordinates to obtain a successfully matched obstacle instance; For the obstacle instance, a state estimation algorithm is used to update the obstacle motion state, thereby obtaining a historical state sequence for a set time period; The historical state sequence is input into a pre-trained obstacle motion prediction model for prediction to obtain the obstacle motion trajectory.

5. The multi-sensor based transmission vehicle to construction machinery following control method according to claim 4, characterized in that: The obstacle motion prediction model is constructed using a long short-term memory network and trained using a multi-scene trajectory dataset.

6. The multi-sensor based transmission vehicle tracking control method for construction machinery according to claim 1, characterized in that: The optimization objective of the MPC model is expressed as: in, represents the length of the prediction time domain, Indicates the first step, represents the weighted sum of squares of state tracking errors, Indicates the The reference state vector of the step, represents the state weight matrix, represents the weighted sum of squares of the control quantities, Indicates the The control vector for the step-by-step prediction, represents the control weight matrix, represents the weighted sum of squares of the control variable change rate, Indicates the The change of step control quantity, Represents the control change rate weight matrix.

7. The multi-sensor based transmission vehicle tracking control method for construction machinery according to claim 6, characterized in that: The constraints of the MPC model are control quantity limitation, dynamics constraint and obstacle avoidance constraint.

8. A multi-sensor based transmission vehicle to construction machinery tracking control system, characterized in that: include: A data acquisition module is configured to: acquire an image containing the target engineering machinery based on a monocular camera, acquire three-dimensional point cloud data based on a laser radar scanning environment, and acquire a heading angle of the transmission vehicle based on an inertial measurement unit; a parameter calculation module configured to: input the image into a target detection model for detection, output the position and category label of the target engineering machinery, and calculate the azimuth angle of the target engineering machinery based on the position of the target engineering machinery; Preprocessing the three-dimensional point cloud data, calculating the precise distance between the transmission vehicle and the target engineering machinery based on the preprocessed three-dimensional point cloud data, and predicting the obstacle's motion trajectory based on the preprocessed three-dimensional point cloud data; a target posture generation module, configured to: fuse the azimuth angle, precise distance, and heading angle of the transmission vehicle to obtain a target engineering machinery posture; The target control quantity generation module is configured to: construct an MPC model based on the position, speed and heading angle of the transmission vehicle, embed the obstacle motion trajectory into the MPC model, input the target construction machinery posture into the MPC model, and dynamically generate the control quantity based on the optimization target.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps of the method for controlling a power transmission vehicle to follow a construction machine based on multiple sensors as claimed in any one of claims 1 to 7 are implemented.

10. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the steps of the method for controlling a power transmission vehicle to follow a construction machine based on multiple sensors according to any one of claims 1 to 7 are implemented.

Images