A charging robot autonomous positioning method and system based on multi-sensor fusion of binocular camera and laser radar

Abstract

The present invention provides a charging robot autonomous positioning method and system based on multi-sensor fusion of binocular camera and laser radar, belonging to the field of image processing and autonomous positioning. To solve the problem that the existing perception system uses laser point cloud data, is insensitive to detection scenes under occlusion or multiple target overlapping scenes; or uses visual detection algorithms, and cannot effectively obtain the target orientation and posture characteristics. This method uses YOLOv5 to detect electric vehicles, obtains the position coordinates of the target in three-dimensional space, and combines the laser radar to extract the complete point cloud of the target, effectively solving the problems of inaccurate detection by a single sensor, the influence of obstacle occlusion, and incomplete acquisition of position and posture information; through point cloud feature extraction and two-stage registration, the interference of wrong matching pairs in the registration process is effectively eliminated, and the matching between real-time point cloud and pre-recorded point cloud is achieved, and the position and posture orientation information of the target vehicle is obtained, paving the way for subsequent charging robot autonomous navigation and plug-in tasks.

Description

Technical Field

[0001] The present invention relates to the technical field of image processing and autonomous positioning, and in particular to an autonomous positioning method and system for a charging robot based on multi-sensor fusion of a binocular camera and a laser radar. Background Art

[0002] When the charging robot realizes autonomous charging, it needs to manually determine the position and posture of the target electric vehicle. In this case, due to the error in the personnel's control of the position and posture of the electric vehicle, the charging gun and the charging port will physically interfere when the charging robot docks with the charging port, which may easily damage the charging gun and the electric vehicle. Therefore, in future electric vehicle parking lots, the charging robot needs to realize autonomous positioning according to the target vehicle and adjust the lateral and heading angle deviations. Before the charging robot performs the autonomous plug-in task, its own system needs to determine the position of the target electric vehicle and be able to actively extract the vehicle's orientation and posture information. This requires the selection of appropriate sensor data to realize real-time detection and posture extraction of the target vehicle, and then calculate the route to the plug-in target point that the charging robot needs to reach.

[0003] For the perception system, the use of laser point cloud data for target vehicle detection can accurately extract the spatial position of the target, because the radar can obtain the point cloud information of the target vehicle. According to the three-dimensional point cloud extraction, the position features of the vehicle's head, tail, door, etc. can be obtained. However, since the detection features extracted by the radar are single, the detection accuracy is unstable, and it is not sensitive to occluded target detection and detection scenes with multiple overlapping targets. The target spatial position extracted by the visual detection algorithm is not accurate enough, and the target's orientation and posture characteristics cannot be obtained, especially when facing target detection scenes with a large screen area and rich posture features, such as vehicle detection scenes at close range. Summary of the invention

[0004] The technical problems to be solved by the present invention are:

[0005] In order to solve the problem that the existing perception system or the use of laser point cloud data is insensitive to detection scenarios under occlusion or multiple target overlapping scenarios; or the use of visual detection algorithms cannot effectively obtain the target orientation and posture characteristics.

[0006] The present invention adopts the following technical solutions to solve the above technical problems:

[0007] The present invention provides a charging robot autonomous positioning method based on multi-sensor fusion of binocular camera and laser radar, comprising the following steps:

[0008] S100, multi-sensor position calibration, obtaining the spatial position between the image acquisition tool including the binocular camera and the point cloud acquisition tool laser radar, completing the calibration between the perception system coordinate system, the camera coordinate system and the radar coordinate system, and obtaining the position transformation moment;

[0009] S200, target vehicle detection, using a binocular camera to collect RGB images of the target vehicle, using the YOLOv5 network to detect the target vehicle in the image data set, and obtain its image center information in the camera coordinate system and coordinate information in three-dimensional space;

[0010] S300, extract the point cloud model of the target vehicle, design a spherical filter and a kd-tree search tree to segment and divide the point cloud clusters collected by the laser radar, combine the progressive morphological filtering method to remove the point cloud from the ground in the point cloud set, obtain the local point clouds of the target vehicle in multiple directions and perform registration, and then obtain the point cloud model of the whole vehicle;

[0011] S400, registering the real-time point cloud with the pre-recorded vehicle point cloud, and after obtaining the vehicle point cloud model obtained in step S300, using the SIFT feature conversion algorithm to extract feature points and feature vectors in the two point cloud clusters and perform feature data enhancement processing;

[0012] S500, using the FPFH algorithm to describe the local area where the feature point is located as a whole, and quantify it into a feature descriptor, and then extract the point cloud feature vector;

[0013] S600. After obtaining the eigenvalues ​​of each key point in the measured point cloud and the recorded point cloud, the eigenvalues ​​of the two parts of the point cloud are respectively formed into sets; the elements in the eigenvalue set of the measured point cloud are traversed, and the random sampling consistency algorithm is used to eliminate erroneous feature point matching pairs to complete the rough alignment of the point cloud features, and the ICP is used to perform fine alignment on the matching pairs after the rough alignment to obtain the state transfer matrix, and the state transfer matrix is ​​used as the posture relationship between the charging robot and the target vehicle to realize the autonomous positioning of the charging robot.

[0014] Further, in step S100, the position transformation matrix is:

[0015]

[0016] in, S C T is the transformation matrix from the camera coordinate system to the perception system coordinate system; R C is the camera attitude rotation matrix, which is obtained from the angle information during the installation of the binocular camera; P C is the camera position translation matrix, which is obtained from the relative position information during the camera installation process; SL T is the transformation matrix from the radar coordinate system to the perception system coordinate system, which is obtained from the angle information during the radar installation process; R L is the radar attitude rotation matrix, P L is the radar position translation matrix, which is obtained from the relative position information during the radar installation process.

[0017] Furthermore, in step S200, it specifically includes:

[0018] S210, network model training, using a binocular camera to collect an image data set of the target vehicle, and divide the data set into a training set and a test set, input the data set and the classification label into the YOLOv5 network model for training verification, use a convolutional neural network to extract features, and mark the detection box with an output confidence greater than a threshold as "car";

[0019] S220, image input, inputting each frame of the image collected by the binocular camera in the video stream into the trained YOLOv5 network model for target detection, and obtaining the category of the target in the picture and the coordinate parameters of the bounding box;

[0020] S230, image detection, in the trained YOLOv5 model, judge whether there is an object in the input real-time RGB image, and if there is an object, output the bounding box information of the object (x l ,y l ,x r ,y r ), and calculate the coordinate information of its center point in the image coordinate system as:

[0021]

[0022] Among them, u, v are the position coordinates of the center pixel of the target vehicle in the image coordinate system; x l ,y l ,x r ,y r They are the horizontal and vertical coordinates of the upper left vertex and the lower right vertex of the target vehicle bounding box respectively;

[0023] Convert the acquired position coordinates in the image coordinate system to the pixel coordinate system according to the camera intrinsic parameters:

[0024]

[0025] Among them, d x ,d y is the horizontal and vertical dimensions of a single pixel point on the XOY plane of the pixel coordinate system; u0, v0 is the position of the focus in the pixel coordinate system; x p ,y pis the position coordinate of the target vehicle in the pixel coordinate system; I P T is the transformation matrix from the image coordinate system to the pixel coordinate system;

[0026] S240, three-dimensional spatial positioning, extracting the spatial position of the target vehicle based on the binocular parallax method, and calculating the spatial position of each pixel in the image.

[0027] Furthermore, in step S240, it specifically includes:

[0028] The spatial position of the center point of the target vehicle in the camera coordinate system is solved by establishing a stereo vision model of the binocular camera. cl and O cr are the aperture centers of the left and right cameras respectively, and the distance between the two aperture centers is the baseline length b of the binocular camera; the center point of the target vehicle is located at P in the imaging plane of the left and right cameras respectively. l and P r Position, get P through the stereo camera internal reference l and P r The position coordinates (y l ,z l ) and (y r ,z r ); the position of the target center point in the camera coordinate system is calculated according to the parallax between the left and right cameras, as shown in formula (4):

[0029]

[0030] Among them, y l ,z l is the position coordinate of the center point of the target vehicle in the left camera pixel coordinate system; r ,z r is the position coordinate of the center point of the target vehicle in the pixel coordinate system of the right camera; c ,y c ,z c is the spatial position of the center point of the target vehicle in the binocular camera coordinate system; f is the focal length of the binocular camera.

[0031] Furthermore, in step S300, it specifically includes:

[0032] S310: After obtaining the coordinate position of the center of the target vehicle in the camera space through step S240, the transformation matrix obtained in step S100 is used S C T converts the description in the camera coordinate system to the perception system coordinate system, and divides the spherical filter range with the target vehicle center point in the perception system coordinate system as the sphere center to filter the radar point cloud data which is also converted to the perception system coordinate system;

[0033] S320, using a kd-tree search tree to segment and divide the point cloud clusters collected by the laser radar, filtering the ordered point cloud data using the outer surface of the spherical filter range obtained in step S310 as a segmentation plane to exclude environmental point clouds, and obtaining a point cloud data set within the spherical range, and then using a progressive morphological filtering method to remove point clouds from the ground in the point cloud set, and the point cloud cluster composed of the remaining point clouds retains the local point cloud information of the target vehicle;

[0034] S330, using laser radar to collect local point clouds of the target vehicle from multiple directions and perform registration to obtain a point cloud model of the entire vehicle.

[0035] Furthermore, the multiple directions are eight directions, and the eight directions are based on the front of the target vehicle as the collection point No. ①, the center of the target vehicle as the origin, and rotate counterclockwise to select a point every 45° as a collection point.

[0036] Furthermore, in step S400, it specifically includes:

[0037] S410, scale transform the target point cloud in the three-dimensional space, use the Gaussian filter function to perform fuzzy processing on the point cloud cluster, and obtain the description of the point cloud information in other scale spaces:

[0038]

[0039] Among them, L(x, y, z, σ) is the point cloud cluster after scale transformation; G(x, y, z, σ) is the Gaussian filter function; I(x, y, z, σ) is the initial position information of the point cloud cluster; (x, y, z) is the position coordinate of the vehicle center in the base coordinate system; σ is the standard deviation parameter;

[0040] S420, performing convolution operations on Gaussian filter operators of different scales and pre-recorded point cloud information to obtain transformation results of the target point cloud at the scale; stacking the fuzzification results of different scales in the depth direction to obtain a data tensor composed of point cloud models of different scales, that is, a Gaussian pyramid model;

[0041] In order to search for local extreme points that meet scale stability in the point cloud cluster, it is necessary to perform differential operations on the point cloud description information in two adjacent layers of transformation space in the Gaussian pyramid model and stack them in the same way to obtain a Gaussian difference pyramid model.

[0042] Calculate the membrane of the gradient of each point in the pyramid and determine the extreme points as feature points, that is:

[0043]

[0044] S430, constructing a feature vector, performing data enhancement on the feature point information obtained by searching in the target point cloud cluster, and obtaining a complete description of the overlapping area of ​​the two point clouds.

[0045] Furthermore, in step S500, it specifically includes:

[0046] Construct the corresponding normal vector angle matrix between two points to characterize the relative position relationship between the neighborhood points and the feature points in the local area, and add the consideration of the influence of the neighborhood points in the local area to the feature descriptor measurement formula of the feature point itself.

[0047] Perform weighted average processing on the influencing parameters of each neighborhood point:

[0048]

[0049] Among them, S(p i ) is the relative position relationship between the local neighborhood point and the feature point; k is the total number of neighborhood points in the local area; ω is the weight coefficient of each neighborhood point, which is inversely proportional to the spatial distance between the neighborhood point and the feature point;

[0050] Furthermore, in step S600, it specifically includes:

[0051] S610, feature matching is performed on the two point clouds based on the feature points in the overlapping area, and the positional relationship between the measured point cloud and the recorded point cloud is modeled using scale invariance. The description of the same position point in the two point cloud clusters satisfies the following relationship:

[0052]

[0053] Among them, R ij is the rotation transformation matrix coefficient between the measured point cloud and the recorded point cloud coordinate direction; t x ,t y ,t z It is the translation transformation matrix coefficient between the measured point cloud and the recorded point cloud coordinate origin;

[0054] S620, selecting multiple groups of feature matching pairs for the position data in step S610 to construct an initial position model, and fitting a pair of rotation matrix and translation matrix models using SVD decomposition, denoted as T; using T to filter all elements in the feature point matching set, and using valid matching pairs to perform secondary fitting on the model, denoted as T';

[0055] S630. Based on the point cloud matching pair after T' filtering, the ICP algorithm is used for precise registration. The two points with similar spatial distances in the two point cloud clusters are regarded as the expression of the same point cloud in different spatial coordinate systems, and the overlapping areas of the two point cloud clusters are registered accordingly. The position of the measured point cloud after transformation is regarded as the initial state to be registered, recorded as M. All points in the set are traversed, and the point cloud with a spatial position close to the current query point is searched in the recorded point cloud set. The two form a new matching pair, and the SVD algorithm is used to fit the state transfer matrix, and the measured point cloud position is updated by the new transformation matrix, recorded as M'; the target error function is used to evaluate the transformed position, and the state transfer matrix is ​​updated. The above process is repeated until the target error between the two point cloud clusters is less than the threshold, which proves that the two point clouds are completely overlapped at this time. The state transfer matrix between the measured point cloud and the recorded point cloud can be obtained by matrix operation:

[0056]

[0057] Among them, R r ,P r The rotation matrix and translation matrix obtained by rough registration respectively; R i ,P i are the rotation matrix and translation matrix of the i-th iteration respectively; M i+1 is the measured point cloud after i-times position transformation; T F The state transfer matrix result obtained by precise alignment;

[0058] The obtained state transfer matrix is ​​used as the posture relationship between the charging robot and the target electric vehicle to achieve autonomous positioning of the charging robot.

[0059] A charging robot autonomous positioning system based on multi-sensor fusion of binocular cameras and laser radars, the system has program modules corresponding to the above steps, and executes the steps in the above-mentioned charging robot autonomous positioning method based on multi-sensor fusion of binocular cameras and laser radars during operation.

[0060] Compared with the prior art, the present invention has the following beneficial effects:

[0061] The present invention discloses an autonomous positioning method and system for a charging robot based on multi-sensor fusion of binocular camera and laser radar. YOLOv5 is selected to detect electric vehicles to obtain the position coordinates of the target in three-dimensional space. The laser radar is combined to extract the complete point cloud of the target and the data information of the camera and the radar are fused, which effectively solves the problems of inaccurate detection by a single sensor, the influence of obstacle occlusion, and incomplete acquisition of position and posture information. Through point cloud feature extraction and two-stage alignment, the interference of erroneous matching pairs in the alignment process is effectively eliminated, the matching between real-time point cloud and pre-recorded point cloud is achieved, and the position and posture orientation information of the target vehicle is obtained, which paves the way for subsequent autonomous navigation and plug-in tasks of the charging robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 This is a flow chart of a method for autonomous positioning of a charging robot based on multi-sensor fusion of a binocular camera and a laser radar in an embodiment of the present invention;

[0063] Figure 2 A diagram for establishing a perception system coordinate system, a camera coordinate system, and a radar coordinate system in an embodiment of the present invention;

[0064] Figure 3 is a coordinate information diagram of the target vehicle in the camera coordinate system in an embodiment of the present invention;

[0065] Figure 4 Schematic diagram of the relationship between the pixel coordinate system and the image coordinate system;

[0066] Figure 5 This is a binocular vision ranging model diagram in an embodiment of the present invention;

[0067] Figure 6 This is a positioning effect diagram of a target vehicle in three-dimensional space in an embodiment of the present invention;

[0068] Figure 7 The eight directions and the local point cloud images of the vehicle respectively obtained in the eight directions during the point cloud recording process in the embodiment of the present invention;

[0069] Figure 8 is a pre-recorded point cloud model diagram of the target vehicle in an embodiment of the present invention;

[0070] Fig. 9 This is a diagram showing the registration results of the measured point cloud and the recorded point cloud of the target vehicle in an embodiment of the present invention. DETAILED DESCRIPTION

[0071] In the description of the present invention, it should be noted that the terminology nouns in the various embodiments, such as "up", "down", "front", "back", "left", "right", etc., which indicate directions, are only for simplifying the description of the positional relationship based on the drawings in the specification, and do not mean that the referred elements and devices must be operated according to the specific directions and defined operations and methods and structures in the specification. Such directional nouns do not constitute a limitation on the present invention.

[0072] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0073] Specific implementation plan 1: Combine Figures 1 to 9 As shown, the present invention provides a charging robot autonomous positioning method based on multi-sensor fusion of binocular camera and laser radar, comprising the following steps:

[0074] S100, multi-sensor position calibration: obtain the spatial position between the image acquisition tool including the binocular camera and the point cloud acquisition tool laser radar, and complete the perception system coordinate system ∑ S , camera coordinate system∑ C and radar coordinate system∑ L Calibrate between and get the position transformation matrix:

[0075]

[0076] in, S C T is the transformation matrix from the camera coordinate system to the perception system coordinate system, R C is the camera attitude rotation matrix, which is obtained from the angle information during the installation of the binocular camera, P C is the camera position translation matrix, which is obtained from the relative position information of the binocular camera during installation. S L T is the transformation matrix from the radar coordinate system to the perception system coordinate system, which is obtained from the angle information during the radar installation process. L is the radar attitude rotation matrix, P L is the radar position translation matrix, which is obtained from the relative position information during the radar installation process;

[0077] Combination Figure 2As shown, the perception system coordinate system takes the center point of the base of the mobile mechanical arm of the charging robot as the origin, selects the forward direction of the central axis of the mobile mechanical arm as the positive direction of the X-axis, selects the direction perpendicular to the central axis to the left as the positive direction of the Y-axis, and selects the vertical upward direction as the positive direction of the Z-axis to construct the perception system coordinate system; the camera coordinate system takes the centers of the two cameras of the binocular camera as the origin, and the XYZ axis direction is the same as the XYZ axis direction of the perception system coordinate system; the radar coordinate system takes the center of the laser radar as the origin, and the XYZ axis direction is the same as the XYZ axis direction of the perception system coordinate system;

[0078] The image acquisition tool uses the ZED2i (polarized version) binocular camera developed by Stereolabs, which has four image resolutions. The highest resolution reaches 4416×122 pixels, which can sense the depth information of 0.2m to 20m, and the maximum field of view can reach 110° (H)×70° (V)×120° (D). The point cloud acquisition tool uses the DJI Livox Mid-70 hybrid solid-state laser radar, whose circular field of view angle can reach 70.4° in both horizontal and vertical directions, and the near blind area is greatly reduced to 5 cm.

[0079] S200, target vehicle detection: Use a binocular camera to collect RGB images of the target vehicle, use the YOLOv5 network to detect the target vehicle (electric vehicle) in the image using the collected image data set, and obtain its image center information in the camera coordinate system and coordinate information in three-dimensional space, specifically including:

[0080] S210, network model training: collect image data sets of target vehicles, and divide the data sets into training sets and test sets, input the data sets and classification labels into the YOLOv5 network model for training verification, use convolutional neural network to extract features, and mark the detection boxes with output confidence greater than the threshold as "car";

[0081] S220, image input: Input each frame of the image collected by the binocular camera into the trained YOLOv5 network model for target detection. The category of the target in the picture and the coordinate parameters of the bounding box can be obtained. The actual effect is shown in the figure below. Figure 3 As shown;

[0082] S230, image detection: In the trained YOLOv5 model, the input real-time RGB image is judged to see whether there is an object. If there is an object, the bounding box information of the object is output (x l ,y l ,x r ,y r ), and calculate the coordinate information of its center point in the image coordinate system as:

[0083]

[0084] Among them, u, v are the position coordinates of the center pixel of the target vehicle in the image coordinate system; x l ,y l ,x r ,y r They are the horizontal and vertical coordinates of the upper left vertex and the lower right vertex of the target vehicle bounding box respectively;

[0085] Combination Figure 4 As shown, ∑ P is the pixel coordinate system, the horizontal rightward in the image is the u(X) axis, the vertical downward is the v(Y) axis, ∑ I is the image coordinate system, horizontal to the right is X I Axis, vertical downward is Y I Axis, convert the position coordinates in the image coordinate system to the pixel coordinate system according to the camera intrinsic parameters:

[0086]

[0087] Among them, d x ,d y is the horizontal and vertical dimensions of a single pixel point on the XOY plane of the pixel coordinate system, which is the camera internal parameter; u0, v0 are the position coordinates of the focus in the pixel coordinate system, which is the camera internal parameter; x p ,y p is the position coordinate of the target vehicle in the pixel coordinate system; I P T is the transformation matrix from the image coordinate system to the pixel coordinate system;

[0088] S240, three-dimensional spatial positioning: extract the spatial position of the target vehicle based on the binocular parallax method, and calculate the spatial position of each pixel in the image, specifically including:

[0089] Combination Figure 5 As shown in the figure, the spatial position of the center point of the target vehicle in the camera coordinate system is solved by establishing a stereo vision model of the binocular camera. cl and O cr are the aperture centers of the left and right cameras respectively, and the distance between the two aperture centers is the baseline length b of the binocular camera; the center point of the target vehicle is located at P in the imaging plane of the left and right cameras respectively. l and P r Position, get P through the stereo camera internal reference l and P r The position coordinates (y l ,z l ) and (y r ,z r); According to the parallax between the left and right cameras, the position of the target center point in the camera coordinate system can be calculated, as shown in formula (4):

[0090]

[0091] Among them, y l ,z l is the position coordinate of the center point of the target vehicle in the left camera pixel coordinate system; r ,z r is the position coordinate of the center point of the target vehicle in the pixel coordinate system of the right camera; c ,y c ,z c is the spatial position of the center point of the target vehicle in the binocular camera coordinate system; b is the baseline length of the binocular camera; f is the focal length of the binocular camera; the left camera pixel coordinate system here refers to the coordinate system with the center of the left camera aperture as the origin, and the right camera pixel coordinate system refers to the coordinate system with the center of the right camera aperture as the origin;

[0092] S300, target vehicle point cloud model extraction: Design spherical filters and kd-tree search trees to segment and divide the point cloud clusters collected by the laser radar, combine the progressive morphological filtering method to eliminate the point clouds from the ground in the point cloud set, obtain the local point clouds of the target vehicle in 8 directions and perform registration, and then obtain the point cloud model of the whole vehicle, including:

[0093] S310: After obtaining the coordinate position of the center point of the target vehicle in the camera space through step S240, the conversion matrix from the camera coordinate system to the perception system coordinate system obtained in step S100 is used. S C T converts the description in the camera coordinate system to the perception system coordinate system, and divides the spherical filter range with the target vehicle center point in the perception system coordinate system as the sphere center. The radius of the spherical filter range is determined by the bounding box (x l ,y l ,x r ,y r ), the radar point cloud data which is also converted to the perception system coordinate system is filtered, and the actual effect is as follows Figure 6 As shown;

[0094] S320, using the kd-tree search tree to segment and divide the point cloud clusters collected by the laser radar, specifically including:

[0095] By using the outer surface of the spherical filter range obtained in step S310 as a segmentation plane to filter the ordered point cloud data, so as to exclude the environmental point cloud that is not from the target vehicle, a point cloud data set within the spherical range can be obtained, and then the point cloud from the ground in the point cloud set is eliminated by using the progressive morphological filtering method. The point cloud cluster composed of the remaining point clouds retains the local point cloud information of the target vehicle and can be used for the subsequent registration link;

[0096] S330, to ensure that the collected point cloud clusters have sufficient overlapping areas for registration, the laser radar is used to collect local point clouds of the target vehicle from eight directions and perform registration to obtain a point cloud model of the entire vehicle;

[0097] Among them, the eight directions of collection are as follows Figure 7 As shown in the figure, the front of the target vehicle is taken as the collection point ①, the center of the target vehicle is taken as the origin, and the counterclockwise rotation is performed. Every 45° point is selected as a collection point. The actual vehicle point cloud model is as follows: Figure 7 As shown;

[0098] S400, registering the real-time point cloud with the pre-recorded vehicle point cloud. After obtaining the vehicle point cloud model obtained in step S300, the SIFT feature conversion algorithm is used to extract the feature points and feature vectors in the two point cloud clusters and perform feature data enhancement processing, specifically including:

[0099] S410, scale transform the target point cloud in the three-dimensional space, use the Gaussian filter function to perform fuzzy processing on the point cloud cluster, and obtain the description of the point cloud information in other scale spaces:

[0100]

[0101] Wherein, L(x, y, z, σ) is the point cloud cluster after scale transformation, G(x, y, z, σ) is the Gaussian filter function, I(x, y, z, σ) is the initial position information of the point cloud cluster, (x, y, z) is the position coordinate of the vehicle center in the base coordinate system, the base coordinate system takes the base center as the origin, the origin as the starting point, the horizontal forward direction is the x-axis, the horizontal left direction is the y-axis, the vertical upward direction is the z-axis, and σ is the standard deviation parameter;

[0102] S420, performing convolution operations on Gaussian filter operators of different scales and pre-recorded point cloud information to obtain transformation results of the target point cloud at the scale; stacking the blurring results of different scales in the depth direction to obtain a data tensor composed of point cloud models of different scales, namely, a Gaussian pyramid model;

[0103] In order to search for local extreme points that meet scale stability in the point cloud cluster, it is necessary to perform differential operations on the point cloud description information in two adjacent layers of transformation space in the Gaussian pyramid model and stack them in the same way to obtain a Gaussian difference pyramid model.

[0104] Calculate the membrane of the gradient of each point in the pyramid and determine the extreme points as feature points, that is:

[0105]

[0106] S430, constructing a feature vector, performing data enhancement on the feature point information obtained by searching in the target point cloud cluster, so as to obtain a complete description of the overlapping area of ​​the two point clouds; considering the parameters that should have scale stability during the feature vector construction process, including the position coordinates, scale parameters and main direction parameters of the feature, and considering the influence of the neighboring points of the feature point on it, completing the acquisition of the feature vector of the feature point;

[0107] S500, using the FPFH algorithm to describe the local area where the feature point is located as a whole, and quantify it into a feature descriptor, taking into account the influence of the neighboring points, and extracting the point cloud feature vector, specifically including:

[0108] Construct the corresponding normal vector angle matrix between two points to characterize the relative position relationship between the neighborhood points and the feature points in the local area. Add the consideration of the influence of the neighborhood points in the local area to the feature descriptor metric formula of the feature point itself. The influence factor of the neighborhood points close to the feature point is relatively large; on the contrary, the influence factor of the neighborhood points farther away from the feature point is smaller.

[0109] Perform weighted average processing on the influencing parameters of each neighborhood point:

[0110]

[0111] Among them, S(p i ) is the relative position relationship between the local neighborhood point and the feature point, k is the total number of neighborhood points in the local area, and ω is the weight coefficient of each neighborhood point, which is inversely proportional to the spatial distance between the neighborhood point and the feature point;

[0112] S600, after obtaining the eigenvalues ​​of each key point of the measured point cloud and the pre-recorded point cloud, the eigenvalues ​​of the two parts of the point cloud are respectively formed into sets; the elements in the eigenvalue set of the measured point cloud are traversed, and the wrong feature point matching pairs are eliminated by using the random sampling consistency algorithm to complete the rough registration of the point cloud features, and the matching pairs after the rough registration are finely registered by using ICP to obtain the state transfer matrix, which specifically includes:

[0113] S610, feature matching is performed on the two point clouds based on the feature points in the overlapping area. The relative distance and position relationship between the point clouds of the same target vehicle at the same position will not change. The position relationship between the measured point cloud and the recorded point cloud is modeled based on scale invariance. The description of the same position point in the two point cloud clusters satisfies the following relationship:

[0114]

[0115] Among them, R ij is the rotation transformation matrix coefficient between the measured point cloud and the recorded point cloud coordinate direction; t x ,t y ,t z It is the translation transformation matrix coefficient between the measured point cloud and the recorded point cloud coordinate origin;

[0116] S620, selecting multiple groups of feature matching pairs for the position data in step S610 to construct an initial position model, and fitting a pair of rotation matrix and translation matrix models using SVD decomposition, denoted as T; using T to filter all elements in the feature point matching set, and using valid matching pairs to perform secondary fitting on the model, denoted as T';

[0117] S630. Based on the point cloud matching pair after T' filtering, the ICP algorithm is used for precise registration. The two points with similar spatial distances in the two point cloud clusters are regarded as the expression of the same point cloud in different spatial coordinate systems, and the overlapping areas of the two point cloud clusters are registered accordingly. The position of the measured point cloud after transformation is regarded as the initial state to be registered, recorded as M. All points in the set are traversed, and the point cloud with a spatial position close to the current query point is searched in the recorded point cloud set. The two form a new matching pair, and the SVD algorithm is used to fit the state transfer matrix, and the measured point cloud position is updated by the new transformation matrix, recorded as M'; the target error function is used to evaluate the transformed position, and the state transfer matrix is ​​updated. The above process is repeated until the target error between the two point cloud clusters is less than the threshold, which proves that the two point clouds are completely overlapped at this time. The state transfer matrix between the measured point cloud and the recorded point cloud can be obtained by matrix operation:

[0118]

[0119] Among them, R r ,P r are the rotation matrix and translation matrix obtained by coarse registration, R i ,P i are the rotation matrix and translation matrix of the i-th iteration respectively; M i+1 is the measured point cloud after i-times position transformation; T F The state transfer matrix result obtained by precise alignment;

[0120] The obtained state transfer matrix is ​​used as the posture relationship between the charging robot and the target electric vehicle to achieve autonomous positioning of the charging robot.

[0121] Specific implementation scheme two: The present invention provides a charging robot autonomous positioning system based on multi-sensor fusion of binocular cameras and laser radars. The system has a program module corresponding to the above steps, and executes the steps in the above-mentioned charging robot autonomous positioning method based on multi-sensor fusion of binocular cameras and laser radars during operation.

[0122] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0123] Although the present invention is disclosed as above, the protection scope of the present invention is not limited thereto. Those skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention, and these changes and modifications will fall within the protection scope of the present invention.

Claims

1. A charging robot autonomous positioning method based on multi-sensor fusion of binocular camera and laser radar, characterized in that: The following steps are involved: S100, multi-sensor position calibration, obtain the spatial position between the image acquisition tool including the binocular camera and the point cloud acquisition tool laser radar, complete the calibration between the perception system coordinate system, the camera coordinate system and the radar coordinate system, and obtain the position transformation matrix, the position transformation matrix is: in, S C T is the transformation matrix from the camera coordinate system to the perception system coordinate system; R C is the camera attitude rotation matrix, which is obtained from the angle information during the installation of the binocular camera; P C is the camera position translation matrix, which is obtained from the relative position information during the camera installation process; S L T is the transformation matrix from the radar coordinate system to the perception system coordinate system, which is obtained from the angle information during the radar installation process; R L is the radar attitude rotation matrix, P L is the radar position translation matrix, which is obtained from the relative position information during the radar installation process; S200, target vehicle detection, using a binocular camera to collect RGB images of the target vehicle, using the collected image data set to detect the target vehicle in the image using the YOLOv5 network, and obtain its image center information in the camera coordinate system and coordinate information in three-dimensional space, specifically including: S210, network model training, using a binocular camera to collect an image dataset of the target vehicle, and divide the dataset into a training set and a test set, input the dataset and classification labels into the YOLOv5 network model for training verification, use a convolutional neural network to extract features, and mark the output detection box with a confidence level greater than a threshold as "car"; S220, image input, inputting each frame of the image collected by the binocular camera in the video stream into the trained YOLOv5 network model for target detection, and obtaining the category of the target in the picture and the coordinate parameters of the bounding box; S230, image detection, in the trained YOLOv5 model, judge whether there is an object in the input real-time RGB image, and if there is an object, output the bounding box information of the object (x l ,y l ,x r ,y r ), and calculate the coordinate information of its center point in the image coordinate system as: Among them, u, v are the position coordinates of the center pixel of the target vehicle in the image coordinate system; x l ,y l ,x r ,y r They are the horizontal and vertical coordinates of the upper left vertex and the lower right vertex of the target vehicle bounding box respectively; Convert the acquired position coordinates in the image coordinate system to the pixel coordinate system according to the camera intrinsic parameters: Among them, d x ,d y is the horizontal and vertical dimensions of a single pixel point on the XOY plane of the pixel coordinate system; u0, v0 are the position coordinates of the focus in the pixel coordinate system; x p ,y p is the position coordinate of the target vehicle in the pixel coordinate system; I P T is the transformation matrix from the image coordinate system to the pixel coordinate system; S240, three-dimensional spatial positioning, extracting the spatial position of the target vehicle based on the binocular parallax method, and calculating the spatial position of each pixel in the image, specifically including: The spatial position of the center point of the target vehicle in the camera coordinate system is solved by establishing a stereo vision model of the binocular camera. cl and O cr are the aperture centers of the left and right cameras respectively, and the distance between the two aperture centers is the baseline length b of the binocular camera; the center point of the target vehicle is located at P in the imaging plane of the left and right cameras respectively. l and P r Position, get P through the stereo camera internal reference l and P r The position coordinates (y l ,z l ) and (y r ,z r ); the position of the target center point in the camera coordinate system is calculated according to the parallax between the left and right cameras, as shown in formula (4): Among them, y l ,z l is the position coordinate of the center point of the target vehicle in the left camera pixel coordinate system; r ,z r is the position coordinate of the center point of the target vehicle in the pixel coordinate system of the right camera; c ,y c ,z c is the spatial position of the center point of the target vehicle in the binocular camera coordinate system; f is the focal length of the binocular camera; S300, extract the point cloud model of the target vehicle, design a spherical filter and a kd-tree search tree to segment and divide the point cloud clusters collected by the laser radar, combine the progressive morphological filtering method to remove the point cloud from the ground in the point cloud set, obtain the local point cloud of the target vehicle in multiple directions and perform registration, and then obtain the point cloud model of the whole vehicle, including: S310: After obtaining the coordinate position of the center point of the target vehicle in the camera space through step S240, the transformation matrix obtained in step S100 is used S C T converts the description in the camera coordinate system to the perception system coordinate system, and divides the spherical filter range with the target vehicle center point in the perception system coordinate system as the sphere center to filter the radar point cloud data which is also converted to the perception system coordinate system; S320, using a kd-tree search tree to segment and divide the point cloud clusters collected by the laser radar, filtering the ordered point cloud data using the outer surface of the spherical filter range obtained in step S310 as a segmentation plane to exclude environmental point clouds, and obtaining a point cloud data set within the spherical range, and then using a progressive morphological filtering method to remove point clouds from the ground in the point cloud set, and the point cloud cluster composed of the remaining point clouds retains the local point cloud information of the target vehicle; S330, using laser radar to collect local point clouds of the target vehicle from multiple directions and perform registration to obtain a point cloud model of the entire vehicle; S400, registering the real-time point cloud with the pre-recorded vehicle point cloud, and after obtaining the vehicle point cloud model obtained in step S300, using the SIFT feature conversion algorithm to extract feature points and feature vectors in the two point cloud clusters and perform feature data enhancement processing; S500, using the FPFH algorithm to describe the local area where the feature point is located as a whole, and quantify it into a feature descriptor, and then extract the point cloud feature vector; S600. After obtaining the eigenvalues ​​of each key point in the measured point cloud and the recorded point cloud, the eigenvalues ​​of the two parts of the point cloud are respectively formed into sets; the elements in the eigenvalue set of the measured point cloud are traversed, and the random sampling consistency algorithm is used to eliminate erroneous feature point matching pairs to complete the rough alignment of the point cloud features, and the ICP is used to perform fine alignment on the matching pairs after the rough alignment to obtain the state transfer matrix, and the state transfer matrix is ​​used as the posture relationship between the charging robot and the target vehicle to realize the autonomous positioning of the charging robot.

2. The method for autonomous positioning of a charging robot based on multi-sensor fusion of binocular camera and laser radar according to claim 1 is characterized in that: The multiple directions are eight directions, and the eight directions are based on the front of the target vehicle as the collection point No. ①, the center of the target vehicle as the origin, and counterclockwise rotation, and selects a point every 45° as a collection point.

3. The method for autonomous positioning of a charging robot based on multi-sensor fusion of binocular camera and laser radar according to claim 2 is characterized in that: In step S400, it specifically includes: S410, scale transform the target point cloud in the three-dimensional space, use the Gaussian filter function to perform fuzzy processing on the point cloud cluster, and obtain the description of the point cloud information in other scale spaces: Among them, L(x, y, z, σ) is the point cloud cluster after scale transformation; G(x, y, z, σ) is the Gaussian filter function; I(x, y, z, σ) is the initial position information of the point cloud cluster; (x, y, z) is the position coordinate of the vehicle center in the base coordinate system; σ is the standard deviation parameter; S420, performing convolution operations on Gaussian filter operators of different scales and pre-recorded point cloud information to obtain transformation results of the target point cloud at the scale; stacking the fuzzification results of different scales in the depth direction to obtain a data tensor composed of point cloud models of different scales, that is, a Gaussian pyramid model; In order to search for local extreme points that meet scale stability in the point cloud cluster, it is necessary to perform differential operations on the point cloud description information in two adjacent layers of transformation space in the Gaussian pyramid model and stack them in the same way to obtain a Gaussian difference pyramid model. Calculate the membrane of the gradient of each point in the pyramid and determine the extreme points as feature points, that is: S430, constructing a feature vector, performing data enhancement on the feature point information obtained by searching in the target point cloud cluster, and obtaining a complete description of the overlapping area of ​​the two point clouds.

4. The method for autonomous positioning of a charging robot based on multi-sensor fusion of binocular camera and laser radar according to claim 3 is characterized in that: In step S500, it specifically includes: Construct the corresponding normal vector angle matrix between two points to characterize the relative position relationship between the neighborhood points and the feature points in the local area, and add the consideration of the influence of the neighborhood points in the local area to the feature descriptor measurement formula of the feature point itself. Perform weighted average processing on the influencing parameters of each neighborhood point: Among them, S(p i ) is the relative position relationship between the local neighborhood point and the feature point; k is the total number of neighborhood points in the local area; ω is the weight coefficient of each neighborhood point, which is inversely proportional to the spatial distance between the neighborhood point and the feature point.

5. The method for autonomous positioning of a charging robot based on multi-sensor fusion of binocular camera and laser radar according to claim 4 is characterized in that: In step S600, it specifically includes: S610, feature matching is performed on the two point clouds based on the feature points in the overlapping area, and the positional relationship between the measured point cloud and the recorded point cloud is modeled using scale invariance. The description of the same position point in the two point cloud clusters satisfies the following relationship: Among them, R ij is the rotation transformation matrix coefficient between the measured point cloud and the recorded point cloud coordinate direction; t x ,t y ,t z It is the translation transformation matrix coefficient between the measured point cloud and the recorded point cloud coordinate origin; S620, selecting multiple groups of feature matching pairs for the position data in step S610 to construct an initial position model, and fitting a pair of rotation matrix and translation matrix models using SVD decomposition, denoted as T; using T to filter all elements in the feature point matching set, and using valid matching pairs to perform secondary fitting on the model, denoted as T'; S630. Based on the point cloud matching pair after T' filtering, the ICP algorithm is used for precise registration. The two points with similar spatial distances in the two point cloud clusters are regarded as the expression of the same point cloud in different spatial coordinate systems, and the overlapping areas of the two point cloud clusters are registered accordingly. The position of the measured point cloud after transformation is regarded as the initial state to be registered, recorded as M. All points in the set are traversed, and the point cloud with a spatial position close to the current query point is searched in the recorded point cloud set. The two form a new matching pair, and the SVD algorithm is used to fit the state transfer matrix, and the measured point cloud position is updated by the new transformation matrix, recorded as M'; the target error function is used to evaluate the transformed position, and the state transfer matrix is ​​updated. The above process is repeated until the target error between the two point cloud clusters is less than the threshold, which proves that the two point clouds are completely overlapped at this time. The state transfer matrix between the measured point cloud and the recorded point cloud can be obtained by matrix operation: Among them, R r ,P r The rotation matrix and translation matrix obtained by rough registration respectively; R i ,P i are the rotation matrix and translation matrix of the i-th iteration respectively; M i+1 is the measured point cloud after i-times position transformation; T F The state transfer matrix result obtained by precise alignment; The obtained state transfer matrix is ​​used as the posture relationship between the charging robot and the target electric vehicle to achieve autonomous positioning of the charging robot.

6. An autonomous positioning system for a charging robot based on multi-sensor fusion of binocular camera and laser radar, characterized in that: The system has a program module corresponding to the steps of any one of claims 1 to 5 above, and executes the steps in the above-mentioned charging robot autonomous positioning method based on multi-sensor fusion of binocular cameras and lidars when running.

Images