Millimeter-level positioning automatic docking method based on heterogeneous sensor tight coupling

Abstract

The invention discloses a millimeter-level positioning automatic docking method based on heterogeneous sensor tight coupling, which realizes high-precision automatic docking in a complex environment by fusing laser radar, vision and IMU (Inertial Measurement Unit) data and combining a dynamic error compensation and anti-interference control algorithm. According to the system, a binocular camera and an Aruco code are adopted for target detection, a high-precision pose is obtained through a PnP algorithm and RTK, a dual PID controller is designed to cooperatively adjust the linear speed and the angular speed, and the pose is corrected in real time in combination with lane line recognition. The system has millimeter-level positioning capability and strong anti-interference performance, and is suitable for the fields of logistics, industrial automation and the like.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving technology, and specifically relates to a millimeter-level positioning automatic docking method based on tight coupling of heterogeneous sensors. Background Art

[0002] With the rapid development of mobile robot technology, automatic docking systems are increasingly being used in fields such as service robots, AGVs (automated guided vehicles), and drones. The core of automatic docking is to achieve high-precision positioning and stable control to ensure that robots can accurately and reliably complete tasks such as charging and material exchange. However, existing technologies still have the following shortcomings in practical applications: Limitations of single sensors: LiDAR solutions: Although they have high ranging accuracy, positioning errors increase significantly in the presence of low-reflectivity targets (such as black charging docks) or in environments with strong light interference, with errors exceeding ±5cm in typical scenarios. Inertial navigation solutions: Due to integral drift, positioning errors accumulate significantly after long periods of operation, making it difficult to meet millimeter-level accuracy requirements. Inadequate multi-sensor fusion technology: Loosely coupled fusion: Existing technologies often use loosely coupled methods (such as Kalman filtering), which fail to fully utilize the complementarity between sensors, resulting in insufficient positioning stability in dynamic environments. Data asynchrony: The data acquisition frequency and transmission delay of heterogeneous sensors vary significantly, making traditional timestamp alignment methods difficult to meet high-precision positioning requirements. Poor environmental adaptability: Existing systems experience significant performance degradation in complex environments such as electromagnetic interference, vibration, and temperature fluctuations. For example, in industrial scenarios, magnetic field interference can cause positioning errors to increase to over ±10cm. Low mechanical docking reliability: Traditional docking mechanisms lack adaptive capabilities, and minor deviations during contact can easily lead to docking failure or equipment damage. Summary of the Invention

[0003] The purpose of the present invention is to overcome the above-mentioned shortcomings and provide a millimeter-level positioning automatic docking system and method based on tight coupling of heterogeneous sensors, which achieves high-precision and high-reliability automatic docking in complex environments by deeply integrating multi-source data of lidar, vision, and IMU, combined with dynamic error compensation and anti-interference control algorithms.

[0004] The present invention provides a millimeter-level positioning automatic docking method based on tight coupling of heterogeneous sensors; wherein; comprises the following steps;

[0005] 1) Target point detection and pose estimation

[0006] a. Binocular camera and Aruco code recognition

[0007] Image acquisition and preprocessing: The binocular camera simultaneously captures images from both the left and right perspectives. After camera calibration, the images are dedistorted to provide accurate images for subsequent feature detection and matching.

[0008] Aruco code detection: Use OpenCV and other libraries to detect pre-defined Aruco codes in the image and extract their corner point information. Assume that the size of the Aruco code is known in the physical world, and the model of the corner points in three-dimensional coordinates is Pi = (Xi, Yi, Zi). Usually, Zi = 0 in the plane case. Pose solution: By matching the Aruco code corner points detected in the image with the corresponding 3D coordinates in the real world, an algorithm is used to solve the rotation matrix Rc and translation vector tc from the camera to the Aruco code. The formula form is as follows:

[0009] solvePnP∶Rc,tc=f({pi},{Pi},K) where {pi} is the coordinate of the corner point detected on the image. The Rc,tc obtained after solution describes the pose information of the Aruco code in the camera coordinate system.

[0010] Target point determination: According to actual needs, the target point is set at the center of the Aruco code or according to one of the offsets; if the center of the Aruco code is the target point, then:

[0011]

[0012] If conversion to the vehicle or global coordinate system is required, the rigid transformation of the camera and vehicle mounting needs to be further utilized;

[0013] b. Coordinate system transformation

[0014] From camera coordinate system to vehicle / global coordinate system: If you need to operate in the global or vehicle coordinate system, you can perform coordinate transformation using the known camera installation pose:

[0015]

[0016] Get the position of the target point in the global coordinate system;

[0017] 2) Obtaining the initial position of the car

[0018] RTK receiving message: By receiving high-precision positioning data through the RTK module, the current vehicle posture information is obtained:

[0019] Initial pose: P0 = (x0, y0, θ0)

[0020] 3).Dual PID control motion regulation

[0021] a. Target error calculation

[0022] Distance error; according to the initial position P0 and the target point P t =(x t ,y t ), calculate the Euclidean distance error:

[0023]

[0024] Angular error:

[0025] When the vehicle needs to adjust its direction, the target direction θ is calculated t (can be calculated by connecting two points) and the difference between the current heading θ0:

[0026] e θ =θ t -θ0

[0027] b.PID controller design

[0028] A dual PID solution is used: one set of PID controllers is used for linear speed control; the linear speed v of the vehicle is adjusted according to the distance error:

[0029]

[0030] Another set of PID controllers is used for angular velocity (or rudder angle) control.

[0031] Adjust vehicle steering based on angle error:

[0032]

[0033] Or directly output it as the rudder angle correction value δ;

[0034] 4) Lane recognition and vehicle posture correction

[0035] a. Fisheye camera lane detection

[0036] When the vehicle is in motion, the installed fisheye camera is used to collect images of the road ahead, and the lane lines are extracted through image processing, thereby obtaining the lane centerline.

[0037] b. Lane centerline as a reference

[0038] Calculate the lateral deviation d between the lane centerline and the vehicle's current position lane :

[0039] d lane = distance from the center of the vehicle to the center of the lane

[0040] c. Posture correction

[0041] The lane deviation is added as a correction to the steering PID control to obtain the final corrected steering angle: lane =w(t)+K lane d lane

[0042] where K laneis the lane correction gain, which is used to balance the effects of motion control and lane tracking.

[0043] In step 1), when the vehicle needs to adjust its direction, the target direction θ is calculated. t (can be calculated by connecting two points) and the difference between the current heading θ0:

[0044] e θ =θ t -θ0

[0045] Normalization is required here to keep the angle error within the range of [-π,π].

[0046] In step 1), during the movement of the vehicle, an image of the road ahead is captured using an installed fisheye camera, and lane lines are extracted through image processing including Canny edge detection, Hough transform, or a deep learning-based method to obtain the lane centerline.

[0047] Compared with existing technologies, the present invention offers significant advantages. The technical solution outlined above demonstrates the following: A binocular camera is used to capture scene images and perform image correction using calibration parameters (intrinsic and extrinsic). Aruco codes in the images are detected using libraries such as OpenCV, thereby obtaining the Aruco code's pose information and, in turn, determining the target point's position. High-precision positioning data is then received via the RTK module to obtain the vehicle's current pose information. A dual PID solution is employed: one PID controller is used for linear velocity control, and another for angular velocity (or steering angle) control. While the vehicle is in motion, an installed fisheye camera captures images of the road ahead. Lane lines are extracted through image processing, and the lane centerline is determined. The lateral deviation between the lane centerline and the vehicle's current pose is calculated, and this lane deviation is used as a correction factor and added to the steering PID control to obtain the final corrected steering angle. The system continuously updates RTK and vision sensor data, adjusting the PID control output in real time to ensure a smooth approach to the target point and maintain lane stability. A spatiotemporal joint optimization algorithm achieves deep fusion of lidar, vision, and IMU data, significantly improving system robustness. Experiments have shown that even in the event of a single sensor failure, positioning errors can still be maintained within ±2mm. By deeply integrating multi-source data from lidar, vision, and IMU, combined with dynamic error compensation and anti-interference control algorithms, high-precision and high-reliability automatic docking is achieved in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 It is the workflow of the code operation of the present invention;

[0049] Figure 2 It is a structural diagram of the system of the present invention. DETAILED DESCRIPTION

[0050] Example 1:

[0051] The present invention provides a millimeter-level positioning automatic docking method based on tight coupling of heterogeneous sensors; wherein; comprises the following steps;

[0052] 1) Target point detection and pose estimation

[0053] a. Binocular camera and Aruco code recognition

[0054] Image acquisition and preprocessing: The binocular camera simultaneously captures images from the left and right perspectives. After camera calibration (obtaining the intrinsic parameter matrix KKK and distortion parameters), the image is dedistorted.

[0055] Provide accurate images for subsequent feature detection and matching;

[0056] Aruco code detection: Use libraries such as OpenCV to detect pre-defined Aruco codes in an image and extract their corner point information. Assume that the dimensions of the Aruco code are known in the physical world, and the corner point model in three-dimensional coordinates is Pi = (Xi, Yi, Zi). Zi is usually set to 0 in the planar case. Pose solution (PnP algorithm): By matching the Aruco code corner points detected in the image with the corresponding 3D coordinates in the real world, an algorithm is used to solve the rotation matrix Rc and translation vector tc from the camera to the Aruco code. The formula is as follows:

[0057] solvePnP∶Rc,tc=f({pi},{Pi},K) where {pi} is the coordinate of the corner point detected on the image. The Rc,tc obtained after solution describes the pose information of the Aruco code in the camera coordinate system.

[0058] Target point determination: Based on actual needs, the target point is set at the center of the Aruco code or according to a certain offset; if the center of the Aruco code is set as the target point, then:

[0059]

[0060] If conversion to the vehicle or global coordinate system is required, the rigid transformation of the camera and vehicle mounting needs to be further utilized;

[0061] b. Coordinate system transformation

[0062] From camera coordinate system to vehicle / global coordinate system: If you need to operate in the global or vehicle coordinate system, you can perform coordinate transformation using the known camera installation pose:

[0063]

[0064] Get the position of the target point in the global coordinate system;

[0065] 2) Obtaining the initial position of the car

[0066] RTK receiving message: By receiving high-precision positioning data through the RTK module, the current vehicle posture information is obtained:

[0067] Initial pose: P0 = (x0, y0, θ0)

[0068] 3).Dual PID control motion regulation

[0069] a. Target error calculation

[0070] Distance error; according to the initial position P0 and the target point P t =(x t ,y t ), calculate the Euclidean distance error:

[0071]

[0072] Angular error:

[0073] When the vehicle needs to adjust its direction, the target direction θ is calculated t (can be calculated by connecting two points) and the difference between the current heading θ0:

[0074] e θ =θ t -θ0

[0075] Note: Normalization may be required here to keep the angle error within the range of [-π,π];

[0076] b.PID controller design

[0077] A dual PID solution is used: one set of PID controllers is used for linear speed control; the linear speed v of the vehicle is adjusted according to the distance error:

[0078]

[0079] Another set of PID controllers is used for angular velocity (or rudder angle) control.

[0080] Adjust vehicle steering based on angle error:

[0081]

[0082] Or directly output it as the rudder angle correction value δ;

[0083] 4) Lane recognition and vehicle posture correction

[0084] a. Fisheye camera lane detection

[0085] During vehicle movement, the installed fisheye camera is used to capture images of the road ahead, and lane lines are extracted through image processing (Canny edge detection, Hough transform, or deep learning-based methods) to obtain the lane centerline.

[0086] b. Lane centerline as a reference

[0087] Calculate the lateral deviation d between the lane centerline and the vehicle's current position lane :

[0088] d lane = distance from the center of the vehicle to the center of the lane

[0089] c. Posture correction

[0090] The lane deviation is added as a correction to the steering PID control to obtain the final corrected steering angle: lane =w(t)+K lane d lane

[0091] where K lane is the lane correction gain, which is used to balance the effects of motion control and lane tracking.

Claims

1. A millimeter-level positioning and automatic docking method based on tight coupling of heterogeneous sensors; in; The following steps are included: 1) Target point detection and pose estimation a. Binocular camera and Aruco code recognition Image acquisition and preprocessing: The binocular camera simultaneously captures images from both the left and right perspectives. After camera calibration, the images are dedistorted to provide accurate images for subsequent feature detection and matching. Aruco code detection: Use OpenCV and other libraries to detect pre-defined Aruco codes in the image and extract their corner point information. Assume that the size of the Aruco code is known in the physical world, and the model of the corner points in three-dimensional coordinates is Pi=(Xi, Yi, Zi). Usually, Zi=0 is set in the plane case. Pose solution: By matching the Aruco code corner points detected in the image with the corresponding 3D coordinates in the real world, an algorithm is used to solve the rotation matrix Rc and translation vector tc from the camera to the Aruco code. The formula form is as follows: solvePnP: Rc, tc=f({pi}, {Pi}, K) where {pi} is the coordinates of the corner points detected on the image. The Rc and tc obtained after solution describe the pose information of the Aruco code in the camera coordinate system. Target point determination: According to actual needs, the target point is set at the center of the Aruco code or according to one of the offsets; if the center of the Aruco code is the target point, then: If conversion to the vehicle or global coordinate system is required, the rigid transformation of the camera and vehicle mounting must be further utilized; b. Coordinate system transformation From camera coordinate system to vehicle / global coordinate system: If you need to operate in the global or vehicle coordinate system, you can perform coordinate transformation using the known camera installation pose: Get the position of the target point in the global coordinate system; 2) Obtaining the initial position of the car RTK receiving message: By receiving high-precision positioning data through the RTK module, the current vehicle posture information is obtained: Initial pose: 3).Dual PID control motion regulation a. Target error calculation Distance error; based on the initial position and target point , calculate the Euclidean distance error: Angular error: When the vehicle needs to adjust its direction, the target direction is calculated (can be calculated by connecting two points) and the current heading The difference: b. PID controller design A dual PID solution is used: one set of PID controllers is used for linear speed control; the linear speed of the vehicle is adjusted according to the distance error. : Another set of PID controllers is used for angular velocity (or rudder angle) control. Adjust vehicle steering based on angle error: Or directly output it as the rudder angle correction ; 4) Lane recognition and vehicle posture correction a. Fisheye camera lane detection When the vehicle is in motion, the installed fisheye camera is used to collect images of the road ahead, and the lane lines are extracted through image processing, thereby obtaining the lane centerline. b. Lane centerline as a reference Calculate the lateral deviation between the lane centerline and the vehicle's current position : = distance from the center of the vehicle to the center of the lane c. Posture correction The lane deviation is added as a correction to the steering PID control to obtain the final corrected steering angle: in is the lane correction gain, which is used to balance the effects of motion control and lane tracking.

2. The millimeter-level positioning automatic docking method based on tight coupling of heterogeneous sensors according to claim 1; ； Step 1) When the vehicle needs to adjust its direction, calculate the target direction (calculated by connecting two points) and the current heading The difference: Normalization is required here to keep the angle error within the range of [-π,π].

3. The millimeter-level positioning automatic docking method based on tight coupling of heterogeneous sensors according to claim 1; ； In step 1), during vehicle movement, an image of the road ahead is captured using an installed fisheye camera, and lane lines are extracted through image processing including Canny edge detection, Hough transform, or a deep learning-based method to obtain the lane centerline.

Images