A multi-source data fusion positioning and autonomous navigation method for component recycling unmanned transfer vehicle

Abstract

The application discloses a kind of multi-source data fusion positioning and autonomous navigation method for component recycling unmanned transfer vehicle, including obtaining multi-source data, and based on this, multi-source data fusion positioning is carried out, with multi-source data fusion positioning result as starting pose, with docking preparation point pose or unloading point pose as target pose, the global path planning of transfer vehicle is carried out, and finally the cruise motion control of transfer vehicle is carried out.The scheme fuses GNSS / RTK, IMU and wheeled odometry, obtains global pose through double extended Kalman filtering and unified map reference conversion, generates global path containing reverse section based on occupancy grid map and minimum turning radius constraint using hybrid A*;Path tracking control is used in cruise phase, and relative pose closed-loop control is switched in docking phase;Combined with local cost map obstacle avoidance of ring-view laser radar, the positioning accuracy, path trackability and docking success rate can be improved.

Description

Technical Field

[0001] This invention belongs to the field of autonomous mobile robot control, specifically relating to a multi-source data fusion positioning and autonomous navigation method for unmanned transport vehicles for component recycling. Background Technology

[0002] After the decommissioning of large photovoltaic power plant modules, multiple collaborative operations, including dismantling, transfer, and unloading, need to be completed within the park. Transfer vehicles often use semi-Ackerman chassis, which are subject to minimum turning radius constraints. Existing indoor AGV positioning solutions are ill-suited to the open environment of the park; single laser SLAM is prone to degradation in open areas; ordinary grid A* does not incorporate kinematic constraints, making it difficult for semi-Ackerman transfer vehicles to track the planned path; and if path tracking is still used during the docking phase, it is difficult to meet the centimeter-level docking accuracy requirements.

[0003] Therefore, there is a need for an autonomous navigation method for unmanned transfer vehicles that is suitable for component recycling scenarios, takes into account positioning in open environments, allows for traceable path planning, and enables precise docking control. Summary of the Invention

[0004] This invention provides a multi-source data fusion positioning and autonomous navigation method for unmanned transport vehicles used for component recycling.

[0005] The specific technical solution for achieving the objective of this invention is as follows:

[0006] A multi-source data fusion positioning and autonomous navigation method for unmanned transport vehicles used for component recycling includes the following steps:

[0007] Step 1: Obtain multi-source data;

[0008] Step 2: Perform multi-source data fusion positioning based on multi-source data;

[0009] Step 3: Using the multi-source data fusion positioning result as the starting pose and the docking preparation point pose or unloading point pose as the target pose, perform global path planning for the transfer vehicle.

[0010] Step 4: Based on the global path planning results, perform cruise motion control of the transfer vehicle.

[0011] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0012] (1) This solution is designed for open environments in the park. By performing time alignment and dual extended Kalman filtering fusion on the RTK, IMU and wheel speed odometer of the transfer vehicle, the accuracy and continuity of the global pose estimation of the transfer vehicle can be improved.

[0013] (2) This scheme uses a hybrid A* algorithm with embedded semi-Ackerman constraints on the grid map to generate a global path that includes a reversing segment and can be tracked, overcoming the problem that ordinary A* paths cannot be executed. At the same time, through the distance-triggered segmented control strategy, the transport vehicle adopts pure tracking control during the cruise phase and switches to PID control relative to the dismantling vehicle during the docking phase, which can improve the docking accuracy at the end.

[0014] (3) This scheme combines the rolling cost map to perform real-time obstacle expansion processing on lidar data, which can ensure the safe driving of the transport vehicle without interrupting the main path planning.

[0015] The present invention will be further described below with reference to specific embodiments. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of the multi-source data fusion positioning and autonomous navigation method for unmanned transfer vehicles for component recycling in this solution.

[0017] Figure 2 This is a flowchart illustrating the multi-source data fusion and positioning process of this solution.

[0018] Figure 3 This is a schematic diagram illustrating the hierarchical relationship between the map coordinate system, the odometer coordinate system, and the vehicle coordinate system in this scheme.

[0019] Figure 4 This is a flowchart illustrating the path planning process using the hybrid A* algorithm in this scheme.

[0020] Figure 5 This is a schematic diagram of the segmented motion control strategy of this scheme. Detailed Implementation

[0021] Example

[0022] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. The described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0023] As indicated in this application and claims, unless the context clearly indicates otherwise, the words "a," "an," "an," and / or "the" do not specifically refer to the singular and may also include the plural. Generally speaking, the terms "comprising" and "including" only indicate the inclusion of explicitly identified steps and elements, which do not constitute an exclusive list, and the method or apparatus may also include other steps or elements.

[0024] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps described in these embodiments do not limit the scope of this application. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following drawings denote similar items; therefore, once an item is defined in one drawing, it need not be further discussed in subsequent drawings.

[0025] Combination Figures 1 to 5 A multi-source data fusion positioning and autonomous navigation method for unmanned transport vehicles used for component recycling, wherein the unmanned transport vehicle for component recycling has a semi-Ackerman structure and is used to transport dismantled photovoltaic modules from the dismantling work station to the unloading point within a photovoltaic power plant component recycling park, including the following steps:

[0026] Step 1: Obtain multi-source data, including:

[0027] Satellite positioning data is acquired using a GNSS / RTK receiver installed on the transport vehicle;

[0028] The inertial measurement unit acquires the angular velocity and linear acceleration data of the three axes of the transport vehicle;

[0029] Wheel speed and odometer data of the transport vehicle are obtained through wheel encoders, and environmental point cloud or scan data around the transport vehicle are obtained through surround-view lidar.

[0030] In addition, the actual parameters in this embodiment are as follows:

[0031] The transfer vehicle has a wheelbase L = 1.2 m and a maximum steering angle δ. max =30°, minimum turning radius R min =L / tan(δ max )≈2.08m, drive wheel radius r w =0.25 m. Docking distance threshold d dock =4 m, Position Precision Factor Threshold PDOP max =3, control cycle ΔT=0.05s, the park map reference point (lat0,lon0) is shared by the transfer vehicle and the dismantling vehicle.

[0032] Step 2: Multi-source data fusion and localization based on multi-source data:

[0033] Step 2-1: Data timestamp alignment; Let the control period be ΔT, at each time t k The acquired satellite positioning data, IMU data, and wheel speed odometer data are interpolated or matched to nearest neighbors according to timestamps to obtain the synchronization data packet D(t). k )={z RTK (t k ), z IMU (t k ), z odom (t k )};

[0034] In this embodiment, at time t k The RTK outputs at 5Hz, the IMU outputs at 100Hz, and the wheel speed odometer is synchronized with the RTK. Linear interpolation is performed on the observed z(t) at t(k):

[0035]

[0036] In the formula, t a ≤ t k ≤ t b z(t) represents the observation value of the corresponding sensor of the transport vehicle;

[0037] Step 2-2, Map Baseline Conversion: Using the set reference point (lat0, lon0) as the origin, convert the latitude and longitude (lat, lon) of the transport vehicle's satellite positioning data into map planar coordinates (x, y). m ,y m ):

[0038]

[0039]

[0040] In the formula, R e The radius of the Earth;

[0041] Only if the transfer vehicle RTK is a fixed solution and PDOP < PDOP max At that time, the position observation vector z map =[x m , y m ]ᵀ is valid and serves as an update observation for the second extended Kalman filter; otherwise, the RTK position observation is invalid, and EKF map Skip RTK updates and only perform predictions, then combine the most recent valid map→odom transformation with the current odom pose to continue outputting the global pose of the map system.

[0042] Steps 2-3: Use the first extended Kalman filter to perform a prediction-update loop to obtain the local pose estimate of the transport vehicle in the odometer coordinate system;

[0043] The first extended Kalman filter uses the transport vehicle state vector X=[x,y,θ,v,ω] T The object to be estimated is defined as follows: x and y represent the planar position of the transport vehicle in the odometer coordinate system, θ is the heading angle of the transport vehicle, v is the linear velocity of the transport vehicle, and ω is the angular velocity of the transport vehicle. The prediction input of the first extended Kalman filter is the linear velocity v measured by the odometer of the transport vehicle. o With angular velocity ω o The state transition is performed according to the planar kinematics model; the local pose estimation of the transport vehicle in the odometer coordinate system is obtained by using the angular velocity and wheel speed observations of the inertial measurement unit as update quantities and the extended Kalman filter standard equation;

[0044] The prediction equation is:

[0045]

[0046] The covariance propagation equation is:

[0047]

[0048] In the formula, F k Let Q be the state transition Jacobian matrix. k Obtained from IMU noise calibration.

[0049] Observation vector z odom =[v o ,ω o ]ᵀ. Calculate the observed Jacobian H(k) = ∂h / ∂X| X(k) Kalman gain:

[0050]

[0051] State update equation:

[0052]

[0053] The local pose estimate of the transport vehicle in the odometer coordinate system is obtained from (x, y, θ) in state X.

[0054] A second extended Kalman filter is used to obtain the global pose estimate of the transport vehicle in the map coordinate system, with the local pose estimate as the prediction input and the position observation as the update quantity.

[0055] Specifically, the second extended Kalman filter uses the local pose estimate of the transport vehicle in the odometer coordinate system output by the first extended Kalman filter as the input to the prediction step, and performs a confidence judgment on the satellite positioning data obtained in step 2. When the RTK position observation is determined to be valid, the map plane coordinates (x, y, y) of the transport vehicle are used as the input to the prediction step.m ,y m The system performs updates for location observations; when RTK location observations are invalid, it only performs prediction steps and maintains or updates the transformation relationship from the map coordinate system to the odometer coordinate system, so that the global pose estimation of the transport vehicle in the map coordinate system remains continuously available.

[0056] Specifically, in this embodiment:

[0057] When RTK location observations are invalid, skip RTK updates, perform only predictions, and press... Calculate the pose of the transport vehicle in the map coordinate system, where The transformation obtained from the most recent valid RTK update. For EKF odom The current odometry pose is output to ensure that subsequent planning and control can still obtain the global pose of the map system; after RTK is restored, it is re-adjusted using (x m ,y m Updated to fix drift.

[0058] The transfer vehicle and the dismantling vehicle use the same reference point (lat0, lon0) for map baseline conversion to ensure that the global pose of the two vehicles is in the same map coordinate system, which facilitates collaborative docking.

[0059] Step 3: Using the multi-source data fusion positioning result as the starting pose and the docking preparation point pose or unloading point pose as the target pose, perform global path planning for the transfer vehicle. That is, on the occupied grid map, use the hybrid A* algorithm to perform node expansion, cost accumulation, heuristic sorting, collision detection and path backtracking to generate a global path that satisfies the semi-Ackerman minimum turning radius constraint and includes optional reversing segments.

[0060] Specifically, the process of using the hybrid A* algorithm for path planning includes:

[0061] (1) Initialization: Let N be the starting node of the planned transfer vehicle. s =(x s ,y s ,θ s ,g s ,h s ,par s ), where (x s ,y s ,θ s ) represents the starting position of the transfer vehicle, g s Starting node N s The cumulative actual cost, since the starting node has no preceding path, is g. s =0, h s Heuristic cost for planning the journey from the origin to the destination node for the transfer vehicle, pars This represents the parent node index of the starting node. If the starting node has no parent node, then `par` is used. s Empty; the resulting child node N after expansion j `par` points to its corresponding parent node `N`. i Used for path backtracking; let the target node N be... g =(x g ,y g ,θ g ); Open list OPEN={N s The} list is used to store nodes to be expanded; the CLOSE list (=∅) is used to store nodes that have already been expanded.

[0062] In this embodiment, the cost function is selected as follows:

[0063]

[0064] In the formula, w σ w δ As a penalty weight, d sw For gear shift indicator f j =g j+ h j Used for node sorting.

[0065] (2) Node expansion loop:

[0066] When the open list OPEN is not empty, the evaluation function f is selected from the open list OPEN. i =g i +h i The smallest node N i Dequeue and add to the close list CLOSE, where g i To reach N from the starting point along the generated path i The cumulative actual cost, h i For N i To the target node N g Heuristic cost estimation, if N i With N g The positional deviation is less than the set threshold ε p And the heading deviation is less than the set threshold ε θ If the search fails, the search will terminate and backtracking will begin;

[0067] Otherwise, for each steering angle δ in the pre-defined set of discrete steering angles m By generating motion primitives with a fixed step size Δs, candidate nodes N are obtained. j =(x j ,y j ,θ j ), where δ mΔs is a parameter pre-discretely set according to the semi-Ackerman constraint; (x) is obtained by discrete integration of the semi-Ackerman kinematics. j ,y j ,θ j And distinguish between forward and reverse gears, eliminating N gears that meet any of the following conditions. j :

[0068] a) The grid corresponding to the candidate node overlaps with the grid of the obstacle, i.e., the grid occupies the collision.

[0069] b. Equivalent turning radius R = L / tan(|δ m |) is less than the minimum turning radius R of the transfer vehicle min Where L is the wheelbase of the transport vehicle;

[0070] c. The node is already in the closed list (CLOSE) and the current path cost is not better;

[0071] Node N to be retained j Add to the OPEN list and repeat this step until the termination condition is met;

[0072] (3) Backtracking and post-processing: Backtracking from the target node along the parent node index par of each node to obtain the discrete path point sequence, then resampling by arc length and calculating the curvature κ of each point. j Output the global path that the transfer vehicle can track {(x j ,y j ,θ j ,κ j )}.

[0073] Step 4: Based on the global path planning results, perform cruise motion control of the transfer vehicle. When the transfer vehicle is far from the dismantling vehicle, it cruises along the global path; after entering the docking area, it switches to precise docking control relative to the dismantling vehicle, specifically:

[0074] Step 4-1, Mode Switching Judgment: Let the distance between the transfer vehicle and the dismantling vehicle be d, and the docking distance threshold be d. dock At the beginning of each control cycle, calculate the distance d between the transfer vehicle and the dismantling vehicle. When d > d dock When d ≤ d, execute steps 4-2 and 4-3. dock Then execute steps 4-4 and 4-5;

[0075] Step 4-2, Cruise - Pure Tracking: Let the current linear velocity of the transport vehicle be v, and the aiming distance be L. d =k d •v, select a pre-aiming point P on the global path that is L_d away from the current pose arc length of the transport vehicle. p =(x p ,y p), calculate the aiming angle α = atan2(y p -y c ,x p -x c )-θ c Given the curvature κ of the current path point, determine the steering angle δ of the front wheels of the transport vehicle: δ = atan(2L·sin(α) / L d ) and linear velocity command v cmd =min(v set ,v max / (1+|κ|)), where L is the wheelbase of the transport vehicle, v set For the desired cruising speed, v max The maximum permissible speed is achieved by automatically reducing the speed of the transfer vehicle when the path curvature |κ| increases.

[0076] Step 4-3, Cruise-Semi-Ackermann Inverse Solution: Relate δ and v cmd Converted to the rotational speed of the transport vehicle's drive wheels n=(v cmd / (2πr w ))·60 and the rear axle angular velocity ω=v cmd ·tan(δ) / L, where r w The radius of the drive wheel;

[0077] Step 4-4, Precision Docking—Error Calculation: In the coordinate system of the disassembly vehicle, calculate the longitudinal error e between the current pose of the transfer vehicle and the relative docking pose of the target. x Horizontal error e y and heading error e θ ;

[0078] Steps 4-5, Fine-tuning—PID Output: Press v d =K px ·e x +K ix ·∫e x dt、ω d =K py ·e y +K pθ ·e θ Output transfer vehicle control commands, and control |v d |Implement upper limit; where K px K ix For the longitudinal scaling and integral gain, K py K pθ For the lateral and directional proportional gain, when |e x |、|e y |and|e θ |Continuously meet the preset threshold for T hold At that time, the precision docking is determined to be complete; preferred |v d |≤0.3 m / s; when |ex |<0.2 m、|e y |<0.2 m、|e θ |<3° continuous T hold The docking was completed in 1 second.

[0079] In addition, during the motion control of the transport vehicle's cruise or docking, a rolling cost map is constructed and updated based on the surround-view lidar data; the lidar point cloud is projected onto a rolling window map centered on the transport vehicle, and after being written through the obstacle layer, it is expanded according to the expansion radius rinf; when the cost in the front safe area exceeds a preset threshold, the linear velocity and angular velocity control commands of the transport vehicle are executed to decelerate or stop for correction.

[0080] In this embodiment, the above modules were implemented in the Gazebo / ROS environment. The measured positioning RMSE of the transport vehicle was <0.1 m, the lateral error of the path was <0.3 m, the docking position error was <0.2 m, and the mission success rate was >95%.

[0081] In addition, this solution also provides a multi-source data fusion positioning and autonomous navigation system for unmanned transfer vehicles used for component recycling, including the following modules:

[0082] Data acquisition module: used to acquire multi-source data from the transfer vehicle in real time;

[0083] Fusion positioning module: used for multi-source data fusion positioning of transfer vehicles based on multi-source data;

[0084] Path planning module: Used to perform global path planning for the transfer vehicle, with the starting pose as the multi-source data fusion positioning result and the docking preparation point pose or unloading point pose as the target pose.

[0085] Control module: Used to control the cruise motion of the transfer vehicle based on the global path planning results.

[0086] This solution also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that the processor performs the following steps when executing the computer program:

[0087] Step 1: Obtain multi-source data;

[0088] Step 2: Perform multi-source data fusion positioning based on multi-source data;

[0089] Step 3: Using the multi-source data fusion positioning result as the starting pose and the docking preparation point pose or unloading point pose as the target pose, perform global path planning for the transfer vehicle.

[0090] Step 4: Based on the global path planning results, perform cruise motion control of the transfer vehicle.

[0091] The embodiments described above are merely one implementation method of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A multi-source data fusion positioning and autonomous navigation method for unmanned transfer vehicles used for component recycling, characterized in that, Includes the following steps: Step 1: Obtain multi-source data; Step 2: Perform multi-source data fusion positioning based on multi-source data; Step 3: Using the multi-source data fusion positioning result as the starting pose and the docking preparation point pose or unloading point pose as the target pose, perform global path planning for the transfer vehicle. Step 4: Based on the global path planning results, perform cruise motion control of the transfer vehicle.

2. The multi-source data fusion positioning and autonomous navigation method for unmanned transfer vehicles for component recycling according to claim 1, characterized in that, The multi-source data in step 1 includes: Satellite positioning data is acquired using a GNSS / RTK receiver installed on the transport vehicle; The inertial measurement unit acquires the angular velocity and linear acceleration data of the three axes of the transport vehicle; Wheel speed and odometer data of the transport vehicle are obtained through wheel encoders, and environmental point cloud or scan data around the transport vehicle are obtained through surround-view lidar.

3. The multi-source data fusion positioning and autonomous navigation method for unmanned transfer vehicles for component recycling according to claim 2, characterized in that, The multi-source data fusion positioning in step 2 specifically involves: Step 2-1: Data timestamp alignment; Let the control period be ΔT, at each time t k The acquired satellite positioning data, IMU data, and wheel speed odometer data are interpolated or matched to nearest neighbors according to timestamps to obtain the synchronization data packet D(t). k )={z RTK (t k ), z IMU (t k ), z odom (t k )}; Step 2-2, Map Baseline Conversion: Using the set reference point (lat0, lon0) as the origin, convert the latitude and longitude (lat, lon) of the transport vehicle's satellite positioning data into map planar coordinates (x, y). m ,y m ); Steps 2-3: Use the first extended Kalman filter to perform a prediction-update loop to obtain the local pose estimate of the transport vehicle in the odometer coordinate system; The global pose estimate of the transport vehicle in the map coordinate system is obtained by using the local pose estimate as the prediction input and the position observation as the update quantity using the second extended Kalman filter.

4. The multi-source data fusion positioning and autonomous navigation method for unmanned transfer vehicles for component recycling according to claim 3, characterized in that, The first extended Kalman filter in steps 2-3 uses the transport vehicle state vector X=[x,y,θ,v,ω] T The object to be estimated is defined as follows: x and y represent the planar position of the transport vehicle in the odometer coordinate system, θ is the heading angle of the transport vehicle, v is the linear velocity of the transport vehicle, and ω is the angular velocity of the transport vehicle. The prediction input of the first extended Kalman filter is the linear velocity v measured by the odometer of the transport vehicle. o With angular velocity ω o The state transition is performed according to the planar kinematics model; the local pose estimation of the transport vehicle in the odometer coordinate system is obtained by using the angular velocity and wheel speed observations of the inertial measurement unit as update quantities and the extended Kalman filter standard equation; The second extended Kalman filter uses the local pose estimate of the transport vehicle in the odometer coordinate system output by the first extended Kalman filter as the input to the prediction step, and performs a confidence judgment on the satellite positioning data obtained in step 2. When the RTK position observation is determined to be valid, the map plane coordinates (x, y, y) of the transport vehicle are used as the input to the prediction step. m ,y m The system performs updates for location observations; when RTK location observations are invalid, it only performs prediction steps and maintains or updates the transformation relationship from the map coordinate system to the odometer coordinate system, so that the global pose estimation of the transport vehicle in the map coordinate system remains continuously available. The transfer vehicle and the dismantling vehicle use the same reference point (lat0, lon0) for map baseline conversion to ensure that the global pose of the two vehicles is in the same map coordinate system, which facilitates collaborative docking.

5. The multi-source data fusion positioning and autonomous navigation method for unmanned transfer vehicles for component recycling according to claim 3, characterized in that, The global path planning in step 3 specifically includes: Using the global pose estimation of the transfer vehicle as the starting pose and the docking preparation point pose or unloading point pose of the transfer vehicle as the target pose, a hybrid A* algorithm is used on the occupied grid map to perform node expansion, cost accumulation, heuristic sorting, collision detection and path backtracking to generate a global path that satisfies the semi-Ackerman minimum turning radius constraint and includes optional reversing segments.

6. The multi-source data fusion positioning and autonomous navigation method for unmanned transfer vehicles for component recycling according to claim 5, characterized in that, The process of path planning using the hybrid A* algorithm includes: (1) Initialization: Let N be the starting node of the planned transfer vehicle. s =(x s ,y s ,θ s ,g s ,h s ,par s ), where (x s ,y s ,θ s ) represents the starting position of the transfer vehicle, g s Starting node N s The cumulative actual cost, since the starting node has no preceding path, is g. s =0, h s Heuristic cost for planning the journey from the origin to the destination node for the transfer vehicle, par s This represents the parent node index of the starting node. If the starting node has no parent node, then `par` is used. s Empty; the resulting child node N after expansion j `par` points to its corresponding parent node `N`. i Used for path backtracking; let the target node N be... g =(x g ,y g ,θ g ); Open list OPEN={N s The} list is used to store nodes to be expanded; the CLOSE list (=∅) is used to store nodes that have already been expanded. (2) Node expansion loop: When the open list OPEN is not empty, the evaluation function f is selected from the open list OPEN. i =g i +h i The smallest node N i Dequeue and add to the close list CLOSE, where g i To reach N from the starting point along the generated path i The cumulative actual cost, h i For N i To the target node N g Heuristic cost estimation, if N i With N g The positional deviation is less than the set threshold ε p And the heading deviation is less than the set threshold ε θ If the search fails, the search will terminate and backtracking will begin; Otherwise, for each steering angle δ in the pre-defined set of discrete steering angles m By generating motion primitives with a fixed step size Δs, candidate nodes N are obtained. j =(x j ,y j ,θ j ), where δ m Δs is a parameter pre-discretely set according to the semi-Ackerman constraint; (x) is obtained by discrete integration of the semi-Ackerman kinematics. j ,y j ,θ j And distinguish between forward and reverse gears, eliminating N gears that meet any of the following conditions. j : a) The grid corresponding to the candidate node overlaps with the grid of the obstacle, i.e., the grid occupies the collision. b. Equivalent turning radius R = L / tan(|δ m |) is less than the minimum turning radius R of the transfer vehicle min Where L is the wheelbase of the transport vehicle; c. The node is already in the closed list (CLOSE) and the current path cost is not better; Node N to be retained j Add to the OPEN list and repeat this step until the termination condition is met; (3) Backtracking and post-processing: Backtracking from the target node along the parent node index par of each node to obtain the discrete path point sequence, then resampling by arc length and calculating the curvature κ of each point. j Output the global path that the transfer vehicle can track {(x j ,y j ,θ j ,κ j )}.

7. The multi-source data fusion positioning and autonomous navigation method for unmanned transfer vehicles for component recycling according to claim 1, characterized in that, In step 4, during the cruise motion control of the transfer vehicle, when the transfer vehicle is far from the dismantling vehicle, it cruises along the global path; after entering the docking area, it switches to precise docking control relative to the dismantling vehicle, specifically: Step 4-1, Mode Switching Judgment: Let the distance between the transfer vehicle and the dismantling vehicle be d, and the docking distance threshold be d. dock At the beginning of each control cycle, calculate the distance d between the transfer vehicle and the dismantling vehicle. When d > d dock When d ≤ d, execute steps 4-2 and 4-3. dock Then execute steps 4-4 and 4-5; Step 4-2, Cruise - Pure Tracking: Let the current linear velocity of the transport vehicle be v, and the aiming distance be L. d =k d •v, select a pre-aiming point P on the global path that is L_d away from the current pose arc length of the transport vehicle. p =(x p ,y p ), calculate the aiming angle α = atan2(y p -y c ,x p -x c )-θ c Given the curvature κ of the current path point, determine the steering angle δ of the front wheels of the transport vehicle: δ = atan(2L·sin(α) / L d ) and linear velocity command v cmd =min(v set ,v max / (1+|κ|)), where L is the wheelbase of the transport vehicle, v set For the desired cruising speed, v max The maximum permissible speed is achieved by automatically reducing the speed of the transfer vehicle when the path curvature |κ| increases. Step 4-3, Cruise-Semi-Ackermann Inverse Solution: Relate δ and v cmd Converted to the rotational speed of the transport vehicle's drive wheels n=(v cmd / (2πr w ))·60 and the rear axle angular velocity ω=v cmd ·tan(δ) / L, where r w The radius of the drive wheel; Step 4-4, Precision Docking—Error Calculation: In the coordinate system of the disassembly vehicle, calculate the longitudinal error e between the current pose of the transfer vehicle and the relative docking pose of the target. x Horizontal error e y and heading error e θ ; Steps 4-5, Fine-tuning—PID Output: Press v d =K px ·e x +K ix ·∫e x dt、ω d =K py ·e y +K pθ ·e θ Output transfer vehicle control commands, and control |v d |Implement upper limit; where K px K ix For the longitudinal scaling and integral gain, K py K pθ For the lateral and directional proportional gain, when |e x |、|e y |and|e θ |Continuously meet the preset threshold for T hold At that time, it was determined that the precision docking was completed.

8. The multi-source data fusion positioning and autonomous navigation method for unmanned transfer vehicles for component recycling according to claim 7, characterized in that, In the motion control of the transport vehicle's cruise or docking, a rolling cost map is constructed and updated based on surround-view lidar data; the lidar point cloud is projected onto a rolling window map centered on the transport vehicle, written through the obstacle layer, and then expanded according to the radius r. inf Expand; when the cost in the frontal safe area exceeds the preset threshold, execute deceleration or stop correction on the linear speed and angular velocity control commands of the transfer vehicle.

9. A multi-source data fusion positioning and autonomous navigation system for unmanned transfer vehicles for component recycling, characterized in that, Includes the following modules: Data acquisition module: used to acquire multi-source data from the transfer vehicle in real time; Fusion positioning module: used for multi-source data fusion positioning of transfer vehicles based on multi-source data; Path planning module: Used to perform global path planning for the transfer vehicle, with the starting pose as the multi-source data fusion positioning result and the docking preparation point pose or unloading point pose as the target pose. Control module: Used to control the cruise motion of the transfer vehicle based on the global path planning results.

10. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1-8.

Images