CN113628275A

CN113628275A - Charging port pose estimation method and system, charger robot and storage medium

Info

Publication number: CN113628275A
Application number: CN202110951068.8A
Authority: CN
Inventors: 张春霞; 宋士佳; 王博; 孙超; 王文伟
Original assignee: Shenzhen Automotive Research Institute of Beijing University of Technology
Current assignee: Shenzhen Automotive Research Institute of Beijing University of Technology
Priority date: 2021-08-18
Filing date: 2021-08-18
Publication date: 2021-11-09
Anticipated expiration: 2041-08-18
Also published as: CN113628275B

Abstract

The invention provides a charging port pose estimation method, a charging port pose estimation system, a charger robot and a storage medium, wherein a standard image of a charging port is obtained; acquiring a charging port image of a vehicle to be charged at one viewing angle; obtaining a plurality of first hole characteristics and a plurality of second hole characteristics which take a charging connecting hole in the charging port as a characteristic according to the standard image and the charging port image respectively; matching the second hole feature to the first hole feature; if the second hole feature is matched with the first hole feature, acquiring a homography matrix; and decomposing the homography matrix to obtain a rotation matrix and a translation matrix, and determining the pose of the charging port according to the rotation matrix and the translation matrix. It can be seen that the non-textured charging port can be matched in a feature matching manner, so that the position of the charging port can be determined. Meanwhile, the number of charging connecting holes in the charging port is small, the number of formed characteristic points is small, and the overall calculation amount is reduced.

Description

Charging port pose estimation method and system, charger robot and storage medium

Technical Field

The invention relates to the technical field of electric vehicle charging, in particular to a charging port pose estimation method and system, a charger robot and a storage medium.

Background

With the development of the automobile industry, the occupancy rate of pure electric vehicles or hybrid electric vehicles (hereinafter referred to as electric vehicles) is increasing, and the charging of electric vehicles is also beginning to be a concern.

The automatic charging technology based on attitude estimation is a technology for realizing automatic charging. The pose of the charging port is estimated by using a pose estimation method, so that the pose information of the charging port is provided for the automatic charging robot.

At present, attitude estimation methods are divided into attitude estimation based on feature matching, attitude estimation based on a template and attitude estimation based on a target, and the three estimation methods have own defects, such as low applicability of the template-based method to a texture-free charging port, large workload at the early stage, insufficient precision and the like of the template-based method or the target-based method.

Disclosure of Invention

The invention mainly solves the technical problem that the charging port without textural features cannot be matched in the existing attitude estimation method based on feature matching.

According to a first aspect, an embodiment provides a charging port pose evaluation method, including:

acquiring a standard image of a charging port;

acquiring a charging port image of a vehicle to be charged at one viewing angle;

respectively obtaining a plurality of first hole characteristics and a plurality of second hole characteristics which take a charging connecting hole in the charging port as a characteristic according to the standard image and the charging port image, wherein the first hole characteristics correspond to the charging connecting hole in the standard image, and the second hole characteristics correspond to the charging connecting hole in the charging port image;

matching the second hole feature to the first hole feature;

if the second hole feature is matched with the first hole feature, acquiring a homography matrix;

and decomposing the homography matrix to obtain a rotation matrix and a translation matrix, and determining the pose of the charging port according to the rotation matrix and the translation matrix.

According to a second aspect, an embodiment provides a charging port pose evaluation system, including:

the image acquisition module is used for acquiring a charging port image of a vehicle to be charged;

the processing module is used for obtaining a plurality of first hole characteristics and a plurality of second hole characteristics which take a charging connecting hole in the charging port as a characteristic according to the standard image and the charging port image respectively, wherein the first hole characteristics correspond to the charging connecting hole in the standard image, and the second hole characteristics correspond to the charging connecting hole in the charging port image; matching the second hole feature to the first hole feature; if the second hole feature is matched with the first hole feature, acquiring a homography matrix; and decomposing the homography matrix to obtain a rotation matrix and a translation matrix, and determining the pose of the charging port according to the rotation matrix and the translation matrix.

According to a third aspect, an embodiment provides a charger robot, including:

the mechanical arm is used for carrying the charging plug to move;

the processing module is used for determining the pose of the charging port according to the charging port image by the charging port pose evaluation method in the technical scheme; according to the position and posture of the charging port, the mechanical arm is controlled to move, so that the charging plug is connected with the charging port.

According to a fourth aspect, an embodiment provides a computer-readable storage medium, on which a program is stored, the program being executable by a processor to implement the charging port pose estimation method according to the above-mentioned technical solution.

According to the charging port pose estimation method, the charging port pose estimation system, the charger robot and the storage medium of the embodiment, the standard image of the charging port is obtained; acquiring a charging port image of a vehicle to be charged at one viewing angle; obtaining a plurality of first hole characteristics and a plurality of second hole characteristics which take a charging connecting hole in the charging port as a characteristic according to the standard image and the charging port image respectively; matching the second hole feature to the first hole feature; if the second hole feature is matched with the first hole feature, acquiring a homography matrix; and decomposing the homography matrix to obtain a rotation matrix and a translation matrix, and determining the pose of the charging port according to the rotation matrix and the translation matrix. It can be seen that the non-textured charging port can be matched in a feature matching manner, so that the position of the charging port can be determined. Meanwhile, the number of charging connecting holes in the charging port is small, the number of formed characteristic points is small, and the overall calculation amount is reduced.

Drawings

Fig. 1 is a schematic flow chart of a charging port pose estimation method according to an embodiment;

FIG. 2 is a schematic flow chart diagram of a charging port pose estimation method according to an embodiment;

fig. 3 is a schematic diagram illustrating a feature point position relationship of a charging port pose estimation method according to an embodiment;

fig. 4 is a schematic structural diagram of a charger robot according to an embodiment;

fig. 5 is a schematic flow chart of a charging port pose estimation method according to another embodiment.

Detailed Description

The present invention will be described in further detail with reference to the following detailed description and accompanying drawings. Wherein like elements in different embodiments are numbered with like associated elements. In the following description, numerous details are set forth in order to provide a better understanding of the present application. However, those skilled in the art will readily recognize that some of the features may be omitted or replaced with other elements, materials, methods in different instances. In some instances, certain operations related to the present application have not been shown or described in detail in order to avoid obscuring the core of the present application from excessive description, and it is not necessary for those skilled in the art to describe these operations in detail, so that they may be fully understood from the description in the specification and the general knowledge in the art.

Furthermore, the features, operations, or characteristics described in the specification may be combined in any suitable manner to form various embodiments. Also, the various steps or actions in the method descriptions may be transposed or transposed in order, as will be apparent to one of ordinary skill in the art. Thus, the various sequences in the specification and drawings are for the purpose of describing certain embodiments only and are not intended to imply a required sequence unless otherwise indicated where such sequence must be followed.

The numbering of the components as such, e.g., "first", "second", etc., is used herein only to distinguish the objects as described, and does not have any sequential or technical meaning. The term "connected" and "coupled" when used in this application, unless otherwise indicated, includes both direct and indirect connections (couplings).

The existing attitude estimation algorithms for objects can be divided into three types: feature matching based pose estimation, template based pose estimation and target detection based pose estimation.

The pose estimation based on the feature matching is suitable for objects with rich textures, and the method directly uses certain feature points, such as SIFT, ORB, FAST, SURF and the like, in a camera image to search the same feature points as images in a database in the camera image. These features are only texture dependent, independent of lighting, scale changes, affine transformations, etc. Generally, objects are rigid bodies which cannot be deformed, and the positions of characteristic points under an object coordinate system are fixed, so that after a plurality of characteristic point pairs are obtained, a homography matrix between the objects in a camera and the objects in a database can be directly solved, a perspective transformation relation between the objects in the camera and the database is found, and the pose of the objects is estimated.

However, there are many scenes in the industry that need to detect non-textured objects, and the pose estimation based on feature matching is no longer applicable, and the template matching algorithm is proposed for such scenes. The pose estimation based on the template can shoot from various pose angles of the object to generate image templates which are stored in a database, and the image of the object to be estimated is matched with the pose image in the database according to certain artificial features, so that the pose is directly estimated. The error of the attitude estimation is large, and only coarse estimation can be carried out. Accurate pose estimation for non-textured objects typically requires reliance on depth information generated by the RGB-D camera. For example, the LineMod algorithm, gradient features of the color image of the object and depth image surface normal vector features are extracted for matching with the templates in the database.

With the development of the target detection algorithm based on deep learning, many people use a network model for target detection to perform a 6D pose estimation task, unlike feature matching-based pose estimation and template matching-based pose estimation, which relies on only a single picture, resulting in a less accurate pose estimation but better handling of occluded objects.

In the field of electric automobiles, automatic charging is realized by adopting an attitude estimation algorithm, wherein charging ports have uniform size standards, and the charging ports in different brands of automobile models have the same size structure, so that the posture estimation of the charging ports can be used for constructing automatic charging robots for charging various automobile models. The charging port has the characteristics of bilateral symmetry and no texture, and mainly consists of a circular hole, so that the existing attitude estimation algorithm based on feature matching cannot be directly applied to pose estimation of the charging port. Both template matching-based and target detection-based pose estimation have two problems: 1. the workload at the early stage is large: for template matching, images of various postures need to be acquired, and a posture library is established to correspond to the images. For the attitude estimation based on target detection, a large amount of complex labeling work needs to be carried out in advance for training the network model. 2. The precision is not high: compared with the attitude estimation based on feature matching, the attitude estimation algorithm based on template matching introduces larger errors when establishing an attitude library; for the attitude estimation based on target detection, errors are introduced during labeling, and the method is limited by the representation form of the pose, so that large errors exist.

Therefore, the embodiment of the invention provides a charging port pose estimation method and system, a charger robot and a storage medium, wherein a charging connection hole in a charging port is used as a feature point to perform feature matching, the position of the charging port is estimated, and the problems that the charging port has no texture features and cannot be matched are solved on the premise of ensuring high precision.

Example one

The embodiment of the invention provides a charging port pose evaluation system which comprises an image acquisition module and a processing module; as shown in fig. 1, a charging port pose evaluation method provided by an embodiment of the present invention includes:

step 1: the processing module acquires a standard image of the charging port. Obtaining a reference image I_{aim_crop}There are many ways in which this method is not limited.

For example, the standard image I can be drawn by a computer or other terminal_{aim_crop}And then transmitted to a processing module; or a color image is shot right in front of a charging port of the vehicle through an image acquisition module (such as an RGB camera) to serve as a standard pose image I_aimThen in the standard pose image I_aimOn hand cutting out standard image I_{aim_crop}. Or standard image I can be imported from the outside_{aim_crop}。

Step 2: the processing module acquires a charging port image of the vehicle to be charged at one visual angle through the image acquisition module.

In one possible implementation, the charging port image I may be acquired in the following manner_{t0_crop}：

Step 201: acquiring a vehicle image I of one side of a vehicle to be charged with a charging port at one viewing angle_t0. For example, after a vehicle to be charged enters a charging parking space and a charging opening cover is opened, the processing module acquires a vehicle image I through the image acquisition module_t0。

Step 202: recognizing vehicle images I_t0The charging port in (1).

Step 203: obtaining a vehicle image I corresponding to the visual angle_t0Charging port image I of corresponding area of middle charging port_{t0_crop}。

For example, it may be that the object detection deep neural network YOLOv4_ tiny is used to detect the vehicle image I_t0The charging port in (1), in particular, the vehicle image I_t0Inputting the image into a YOLOv4_ tiny neural network, regressing to obtain the 2D frame position and size of the charging port, and partially cutting out the frame to obtain a charging port image I_{t0_crop}. Since the charging port usually occupies a small portion in the vehicle image, it is necessary to perform charging port detection before accurate pose estimation, so as to remove a cluttered background, narrow the search range of hole feature points, and reduce interference during hole identification. The YOLOv4_ tiny has the characteristics of small network model structure, high inference speed and high precision, so the YOLOv4_ tiny is selected as a detection model.

And step 3: the processing module obtains a plurality of first hole characteristics and a plurality of second hole characteristics which take a charging connecting hole in the charging port as a characteristic according to the standard image and the charging port image respectively, wherein the first hole characteristics correspond to the charging connecting hole in the standard image, and the second hole characteristics correspond to the charging connecting hole in the charging port image. It should be noted that, the holes or circles mentioned in the embodiments of the present invention refer to charging connection holes in the charging port unless otherwise specified.

In a possible implementation manner, as shown in fig. 2, the step 3 may include the following steps:

step 301: and respectively preprocessing the standard image and the charging port image to strengthen the edge information.

Specifically, a specific preprocessing flow is designed for the characteristics that the standard image corresponding to the charging port and the charging port image have the characteristics that colors are concentrated in a gray area, and the small noise is more due to metal materials. Firstly, using a bilateral filter to carry out edge protection and denoising on two images; and using a Laplace operator to strengthen the edge information; then, graying the two images to convert the RGB image of the 3 channels into a gray image of a single channel; and finally, performing histogram equalization on the two gray level images to disperse the more concentrated gray level distribution, thereby enhancing the internal contrast of the images. Through preprocessing, the contrast of the edge information of the image can be improved, and the detection precision of subsequent edge detection is improved.

Step 302: and carrying out edge detection on the standard image and the charging port image, and identifying the charging connecting hole.

Specifically, edge detection is performed on the two preprocessed images by using a Canny algorithm, circles (namely charging connection holes) in the edges are detected by using Hough transform, and finally the circles are clustered, and the class centers are used as the circles after duplication removal.

Step 303: and carrying out template matching on the standard image and the charging port image to identify the charging connecting hole. Step 303 may be performed synchronously with step 302 or may be performed out of order.

Specifically, since hough transform is a shape-based detection method, the recognition rate of smaller holes or more blurred holes is not high, and in order to solve this problem, a template matching method is added to the method to detect more holes. The method comprises the steps of conducting hole template matching on two preprocessed images by using a normalized covariance template matching method, taking pixel points with normalized covariance values larger than a given threshold value as the circle centers of circles, taking the side length of the template as the diameter of the circle, finally clustering the matched circles, and taking the class centers as the circle after weight removal.

Step 304: and clustering and de-duplicating the charging connecting holes identified by edge detection and template matching to obtain a plurality of first hole characteristics and second hole characteristics.

That is, in the method, by using the characteristic that the charging port has a plurality of charging connection holes with fixed positions, and by using the processing method described in the step 3, the charging connection holes are used as feature points to be matched, and corresponding hole features are formed in two images for subsequent feature matching.

And 4, step 4: the second hole feature is matched to the first hole feature.

In a possible implementation manner, the step 4 may include the following steps:

step 401: obtaining a feature descriptor of the first hole feature and a feature descriptor of the second hole feature according to the first hole features and the second hole features respectively. The feature descriptors of the first hole feature and the second hole feature each include a positional relationship descriptor, a size descriptor, and a color descriptor.

The position relation descriptor of the first hole feature is the position relation of one first hole feature in a plurality of corresponding first hole features; the size descriptor is a radius or diameter of the first hole feature; the color descriptor is the mean and variance of the gray level inside the first hole feature; the position relation descriptor of the second hole feature is the position relation of one second hole feature in a plurality of corresponding second hole features; the size descriptor is a radius or diameter of the second hole feature; the color descriptor is the mean and variance of the gray levels inside the second hole feature.

Because the positional relationship between the charging muzzle inner holes is relatively fixed, the method matches the holes in the two images using the positional relationship between the holes. The shape-context description matrix S is used as a hole location relation descriptor. Since the size of the charging connection holes in the charging port is different, the method uses the radius r of the holes as a size descriptor. Since there is a silver round bolt near the charging port of some vehicles, in order to eliminate the influence of the bolt on the charging port hole recognition, a color descriptor is added. The method proposes to determine the mean mu and the variance sigma of the interior of the pores²As a color descriptor. The method thus defines a hole (corresponding to the first hole feature and the second hole feature) with a feature descriptor of: (positional relationship descriptor S, size descriptor r, color descriptor (. mu.,. sigma.))²))。

As shown in fig. 3, in order to reduce the influence of rotation and scale on the position relation descriptor S and the influence on the matching result, in this step, two points with the largest radius are used as two large holes, the charging port image is rotated so that the included angle between the connecting line of the central points of the two large holes and the positive direction of the x axis is zero, the charging port image and the standard image are set to be a fixed size, and then the feature descriptor of the hole is calculated. According to the method, when the position relation descriptor S is calculated, a circular area near a feature point to be described is divided into 48 sub-areas according to 4 scales and 12 angles, the angle interval is 30 degrees, and the radiuses corresponding to the 4 scales are respectively e¹·minDis、e²·minDis、e³·minDis、e⁴minDis, where minDis is a custom constant. The number of holes identified by each region is taken as an element value of the matrix S, and for the condition that a point is on the boundary line, the method counts the point in two regions at the same time. The shape context description matrix S of the above figure is as follows:

wherein the ith row represents the radius r_iTo r_i-1Between 12 regions in the circle, and the jth column represents 4 regions with angles between 30 ° (j-1) and 30 ° · (j).

Step 402: a cost matrix is generated for the feature descriptors for the second hole features and the feature descriptors for the first hole features.

The method defines a cost function between two characteristic points (holes) L and R to be matched according to the characteristics of charging connecting holes, wherein position relation descriptors of the two holes are respectively set as S_LAnd S_RThe size descriptors are respectively r_LAnd r_RThe color descriptors are respectively (mu)_L，σ_L ²) And (mu)_R，σ_R ²). Defining the cost function cost (L, R) as:

wherein w₁、w₂、w₃、w₄Are weight coefficients.

Assuming that k holes are identified in the charging port image, cost values of the k holes and 9 holes in the standard image are calculated in the step, and a k × 9 dimensional cost matrix C is formed, wherein the value of the ith row and j column is cost (i, j). It should be noted that 9 holes are merely illustrated as an example of one type of charging port, and the specific type of charging port is not limited.

Step 403: the second hole feature is matched to the first hole feature according to the cost matrix.

The problem of matching two sets of feature points can be described as the problem of solving the matching matrix Match:

where a (i, j) is the ith row and j column elements of the matching matrix a, indicating whether the ith hole matches the jth hole in the target picture.

The method is a weighted bipartite matching problem, and the best matching of the bipartite is solved by adopting a classical Kuhn-Munkres algorithm. Common matching algorithms among pose estimation algorithms are a brute force lookup BF algorithm and a fast nearest neighbor FLANN algorithm. The two algorithms have many-to-one repeated matching problem and are not bipartite matching solutions. And Kuhn-Munkres is an algorithm for solving the maximum matching of weighted bipartite graphs, and can enable feature point matching to be more accurate.

The method takes the reciprocal of the cost value between two points as the weight of the connecting line of the two points, and converts the cost matrix C into a weight matrix W.

Specifically, the method can be applied to the Kuhn-Munkres algorithm by referring to the following steps:

41. and filling the extraction points in the point set with the smaller number in P and Q, and setting the connection weight of the extraction points to be 0 so as to ensure that the sizes of the two point sets are equal.

42. Initializing vertex indices for P and Q: get max W_i＝m,jAs a point p_mLet the initial top of all points in Q be 0.

43. The hungarian algorithm is used to find the maximum match that satisfies the matching condition. Matching conditions are as follows: the sum of the superscripts of point P and point Q is equal to the weight between the two points, where point P belongs to the set of points P and point Q belongs to the set of points Q.

44. For the point p which can not find the matching point_iFor the matched left vertex p of the edge with the maximum weight value at the point_jIndex minus d, right vertex q_kWith addition of d to the reference number, and p_iThe point itself is marked with the point minus d.

45. Repeat steps 42 and 33 until all points find a matching pointAnd (4) bundling. Wherein P is a standard image I_{t0_crop}Q is a charging port image I_{aim_crop}W is the weight matrix and d is the step size.

And 5: if the second hole feature matches the first hole feature, a homography matrix is obtained. Wherein obtaining the homography matrix may include: acquiring a second coordinate array of a plurality of second hole features in the charging port image; acquiring a first coordinate array of a plurality of first hole features in a standard image; and obtaining a homography matrix according to the second coordinate array and the first coordinate array.

There are many ways to solve and optimize the corresponding homography matrix H, and the method provides the following steps to locate the feature points in the charging port image I_{t0_crop}And a standard image I_{aim_crop}Is converted into a vehicle image I_t0And standard pose image I_aimAnd (c) coordinates of (c). And solving a homography Matrix (Homograph Matrix) H by using a least square method, eliminating mismatching points by using a RANSAC algorithm, and optimizing the estimation of the homography Matrix H.

The following steps can be referred to for solving the homography matrix specifically by adopting the least square method:

step 5.1: and (5) solving the homography matrix.

Suppose that:

the perspective transformation relation of any feature point of the two images is as follows:

wherein x_i,y_iIs the coordinate, x 'of the ith feature point in the charging port image'_i,y′_iThe coordinates of the corresponding feature points in the standard image. There are 8 unknowns in the homography matrix, so that the homography matrix H can be solved by substituting 4 pairs of feature points. When there are more than 4 feature points, the homography matrix H is solved using the least squares method:

the perspective relationship of pairs of feature points can be expressed as follows:

and (4) carrying out eigenvalue decomposition on the coefficient matrix A on the left side, wherein the eigenvector corresponding to the minimum eigenvalue is the least square solution H of the equation.

Step 5.2: the method defines a noise elimination strategy aiming at small data volume by referring to the idea of random consensus sampling algorithm (RANSAC).

Since the matching result may have the problem of non-hole or mismatching, the least square method for solving the homography matrix is susceptible to the influence of the points, so that it is necessary to eliminate the noise before solving the homography matrix. The problems to be solved in the step are as follows: finding the optimal homography minimizes the sum of the projection errors e for all non-noise points.

e²＝‖p′-Hp‖²

Wherein p represents a charging port image I_t0The coordinates of the characteristic point, p' represents the projection of the point to the standard image I through perspective transformation_aimAnd (c) coordinates of (c).

The process of eliminating mismatching of the homography matrix by adopting the RANSAC algorithm can refer to the following steps:

51. 4 points were sampled randomly.

52. And solving the homography matrix H.

53. Calculating projection errors e of all the points except the sampling points, taking the points with the errors e smaller than a threshold value beta as interior points, and counting the number of the interior points.

54. The first three steps were repeated M times.

55. And (4) recalculating the homography matrix by taking the inner point of the round with the largest number of inner points.

Because the application scene of the method is the attitude estimation of the charging port, the method has at most 9 characteristic points (the number of charging connecting holes corresponding to the charging port is only used for illustration), and the characteristic points are less, the method replaces random sampling with traversal,the global optimal solution is found, and a unique solution is further solved according to the mean square error aiming at the condition of multiple solutions, so that the global optimal solution is found more accurately. The maximum traversal number is

The method is characterized in that the situation that the number of inner points in multiple rounds is the same but the inner points are different can occur in the RANSAC algorithm, aiming at the situation, the method adopts the minimum criterion of the mean square error mse of the inner points, selects the inner points of the round with the minimum mean square error of the inner points in the round with the maximum number of the inner points, and calculates the final homography matrix H by using a least square method. Mean square error mse is:

wherein n is the number of the inner points, and i is the subscript of the inner points. The process of optimizing the homography matrix using the consistent sampling algorithm may refer to the following steps:

56. 4 points are sampled.

57. The homography H is solved using the 4 points of step 56.

58. Calculating projection errors e of all the points except the sampling points, taking the points with the errors e smaller than a threshold value beta as interior points, and counting the number of the interior points.

59. And repeating the first three steps until all 4-point combinations are traversed and then ending.

60. And (4) recalculating the homography matrix by taking the inner points of the round with the largest number of inner points and small mean square error mse.

Step 6: the processing module decomposes the homography matrix to obtain a rotation matrix and a translation matrix, and determines the pose of the charging port according to the rotation matrix and the translation matrix. For example, the homography matrix is decomposed by adopting an SVD singular value algorithm to obtain a rotation matrix and a translation matrix, so that the pose of the charging port is determined according to the rotation matrix and the translation matrix.

And 7: the processing module acquires the poses of a plurality of visual angles, and optimizes and updates the pose of the charging port. Wherein, the beam adjustment method can be adopted to optimize the space coordinates P of 9 holes in the rotating matrix R, the translation matrix T and the charging port frame by frame.

Specifically, after the pose (rotation matrix R, translation matrix T) of the charging port is calculated according to the vehicle image, the mechanical arm starts to move according to the pose. And then, periodically acquiring images of the charging port by the camera at fixed time intervals, calculating the pose of the charging port, and optimizing the current pose of the charging port and the coordinates of the charging port hole in a world coordinate system by using a beam adjustment method frame by frame. The step 7 may include the steps of:

step 701: acquiring a charging port image of a vehicle to be charged at least one other viewing angle;

step 702: acquiring poses corresponding to the charging ports in all the visual angles according to the charging port images corresponding to all the visual angles;

step 703: and (5) the reprojection errors of the poses of all the visual angles are minimized, and the pose of the charging port is updated.

For example, the specific process of frame-by-frame optimization by using the beam adjustment method can refer to the following:

let p be_jRepresenting the spatial three-dimensional point coordinate corresponding to the jth characteristic hole, (R)_i,T_i) Pose estimation for ith frame image, z_ijRepresenting the imaging point (coordinate in the image coordinate system) of the jth hole feature point on the ith frame image,

represents a characteristic point p_jThrough (R)_i,T_i) And projecting the camera internal reference matrix and the distortion parameter to a two-dimensional imaging point on the ith frame image, wherein the projection process is represented by a function h (·):

the vehicle position is static during the detection process, so that the coordinate p of the 9 characteristic holes on the charging port is in a world coordinate system_jIs always unchanged. In the optimization process of the light beam adjustment method, the pose (R) is adjusted_i,T_i) And 9 coordinates p of hole feature points_jAs to be optimizedAmount of will observe the imaging point z_ijAnd projection point

As a cost function:

where t denotes the index of the current frame during motion. Theta_ijIndicating whether the matching feature points of the ith frame contain the jth hole feature point, if so, theta_ij1, otherwise_ij＝0。

The method adopts a Levenberg-Marquardt method to solve the nonlinear optimization problem, and dynamically and adaptively adjusts parameters:

first, the result obtained in step 7 is used as (R)_i,T_i) Estimating the three-dimensional coordinates of the hole feature points according to a triangulation method, and taking the mean value of the three-dimensional coordinates of the estimated feature points in all frames as p_jThe initial value of (c).

Calculating Jacobian matrix J of R, T and p separately_R,J_T,J_pAnd its corresponding information matrix A_R,A_TAnd A_p：

A_Ri(R_i,T_i,p_j)＝J_Ri·J_Ri ^T+λI；

A_Ti(R_i,T_i,p_j)＝J_Ti·J_Ti ^T+λI；

A_pj(R_i,T_i,p_j)＝J_pj·J_pj ^T+λI；

Wherein I is an identity matrix, and λ represents a damping parameter, which is a constant.

Calculating the currentError F (R) of_i,T_i,p_j)；

Δ R was calculated according to the following formula_i，ΔT_i，Δp_j；

A_Ri(R_i,T_i,p_j)·ΔR_i＝-J_Ri(R_i,T_i,p_j)F(R_i,T_i,p_j)；

A_Ti(R_i,T_i,p_j)·ΔT_i＝-J_Ti(R_i,T_i,p_j)F(R_i,T_i,p_j)；

A_pj(R_i,T_i,p_j)·Δp_j＝-J_pj(R_i,T_i,p_j)F(R_i,T_i,p_j)；

Updating R_i，T_iAnd p_j；

R_{i_new}＝R_i+ΔR_i；T_{i_new}＝T_i+ΔT_i；p_{j_new}＝p_j+Δp_j。

When updated error F (R)_i,T_i,p_j) When the error is larger than the error before updating, the R obtained by updating is abandoned_i，T_iAnd p_jWhile increasing the parameter λ, λ ═ 2 λ. Otherwise, the parameter λ is reduced, λ being 0.5 λ. Up to Δ R_i、ΔT_i、Δp_jWhile the update iteration is stopped when less than the threshold epsilon. ε is a very small constant. Therefore, in the motion process of the charging plug carried by the mechanical arm and the RGB camera, new charging port images are acquired at intervals, multiple poses corresponding to multiple frames are obtained, and the poses are updated, so that the estimated poses are more accurate. Namely, the estimation of the three-dimensional coordinates of the current poses R, T and the charging hole is more and more accurate by using all historical information.

In summary, the charging port pose estimation method and system provided by the invention at least have the following technical effects:

1. the invention can estimate the pose of the charging port of different vehicle types, and the pose can be estimated through the pre-stored standard image as long as the size standard of the charging port and the arrangement of the charging connecting holes are unchanged, without installing any mark on the vehicle in advance.

2. Compared with the existing feature matching pose estimation method, the pose estimation method provided by the invention has the advantages that the detected features are hole features, the feature points have upper limits, and the number of the feature points is very small. The existing feature matching pose estimation methods such as orb or sift have hundreds of recognized feature points, which results in large calculation amount. Therefore, the method has faster calculation speed.

3. Compared with the existing feature matching pose estimation method, the method removes a large amount of background interference by using the YOLOv4_ tiny target detection, and redundantly detects holes by adopting two methods of Hough transform and template matching during feature detection, so that the holes are more accurately identified.

The matching is more accurate: the method does not simply adopt ideas like violent matching or FLANN and other algorithms to each point with the minimum matching cost, but classifies the problem as a bipartite graph maximum matching problem, and solves the problem by using a Kuhn-Munkres algorithm so as to enable the matching to be more accurate. After matching is finished, operation of proposing mismatching is added, and mismatching points are searched by using a globally traversed consistency sampling algorithm, so that matching precision is further improved.

Attitude estimation is more accurate: a least square method is used in the homography matrix solving, namely the perspective relation of all matching points is comprehensively considered, so that the obtained homography matrix is more accurate. After each frame of attitude estimation, the light beam adjustment method is used for optimization, namely, the attitude errors of all frames in the automatic charging motion process are comprehensively considered, so that the attitude estimation of each frame is more accurate.

Example two

As shown in fig. 4, the charging port pose estimation method of the present invention can be applied to a charger robot, and the present invention further provides a charger robot, which includes a mechanical arm 30, an image acquisition module 20 (such as an RGB camera), and a processing module 10, wherein the mechanical arm 30 is used for carrying a charging plug 40 to move; the image acquisition module 20 is used for acquiring a charging port image of a vehicle to be charged; the processing module 10 is configured to determine, according to the charging port image, a pose of the charging port by the charging port pose estimation method; according to the pose of the charging port, the mechanical arm 30 is controlled to move so as to realize the connection of the charging plug 40 and the charging port. After the plug 40 to be charged is connected with the charging port, the processing module 10 sends out a corresponding instruction, and the energy supply device charges the vehicle to be charged through the plug 40 and the charging port. After the charging is finished, the processing module 10 receives a corresponding instruction and controls the mechanical arm 30 to move, the charging plug 40 is in contact connection with the charging port, and the vehicle can finish the whole automatic charging process.

For example, as shown in fig. 5, a standard pose image is captured at a fixed position right in front of the charging port in advance and stored in a memory that can be read by a charger robot, after the image is processed to obtain a standard image, the standard image is labeled with a first hole feature, and a feature descriptor corresponding to the first hole feature is generated. It is assumed that the camera is mounted at the end of the robot arm and moves along with the robot arm, and that the vehicle to be charged is parked in a parking space in an unknown posture. In the process of estimating the pose of the charging port, the RGB camera on the mechanical arm acquires two-dimensional colorful vehicle image information of the vehicle charging port side in the parking space. First, a charging port with a standard size is detected by using YOLOv4_ tiny, and a picture of the charging port part is cut out according to a 2D frame to form a charging port image waiting to be matched with the standard image. In step 3 of the embodiment of the present invention, a method for performing edge detection by using a charging connection hole in a charging port as a feature finds a plurality of feature points (a first hole feature and a second hole feature) on a standard image and a charging port image, respectively. In step 4 of the embodiment of the present invention, the feature descriptor calculates the feature of the feature point based on the shape context. And precisely matching the characteristic points of the standard image and the charging port image by adopting a Kuhn-Munkres matching strategy. And eliminating mismatching points by using a consistency sampling algorithm. And (3) calculating a perspective transformation relation between the two groups of characteristic points, namely a homography matrix H by using a least square method, and obtaining a rotation matrix R and a translation matrix T by adopting singular value decomposition SVD. And after the pose of the charging port is preliminarily estimated, guiding the mechanical arm to move, continuously acquiring pictures in the moving process, estimating the pose, and performing global optimization on the pose by using a Bundle Adjustment method. The pose is obtained while the movement is carried out, and the movement track of the mechanical arm is optimized, so that the charging plug can be connected with a charging port.

Further, for example, when a vehicle having a plurality of charging port interface types needs to be automatically charged, a matching relationship of the corresponding charging plug and the standard image may be established. For example, standard images of charging ports of various interface types are stored in advance, a charging port image of any vehicle needing to be charged is acquired, the charging port image is matched with a plurality of standard images after feature matching, a corresponding standard image is determined, and a corresponding charging plug is determined. The arm carries the corresponding charging plug to move to the realization charges the automation of the vehicle of different mouth interface types that charge.

That is to say, the charging port pose estimation method and the charging robot provided by the invention are not limited to pose estimation and automatic charging of one type of charging port, and can be suitable for various types of charging ports.

The invention also provides a charging port pose evaluation system which comprises an image acquisition module and a processing module. The image acquisition module is used for acquiring a charging port image of a vehicle to be charged; the processing module is used for obtaining a plurality of first hole characteristics and a plurality of second hole characteristics which take a charging connecting hole in the charging port as a characteristic according to the standard image and the charging port image respectively, wherein the first hole characteristics correspond to the charging connecting hole in the standard image, and the second hole characteristics correspond to the charging connecting hole in the charging port image; matching the second hole feature to the first hole feature; if the second hole feature is matched with the first hole feature, acquiring a homography matrix; and decomposing the homography matrix to obtain a rotation matrix and a translation matrix, and determining the pose of the charging port according to the rotation matrix and the translation matrix.

The technical effect of the charging port pose evaluation system is the same as that of the charging port pose evaluation method provided in the first embodiment, and details are not repeated here.

Those skilled in the art will appreciate that all or part of the functions of the various methods in the above embodiments may be implemented by hardware, or may be implemented by computer programs. When all or part of the functions of the above embodiments are implemented by a computer program, the program may be stored in a computer-readable storage medium, and the storage medium may include: a read only memory, a random access memory, a magnetic disk, an optical disk, a hard disk, etc., and the program is executed by a computer to realize the above functions. For example, the program may be stored in a memory of the device, and when the program in the memory is executed by the processor, all or part of the functions described above may be implemented. In addition, when all or part of the functions in the above embodiments are implemented by a computer program, the program may be stored in a storage medium such as a server, another computer, a magnetic disk, an optical disk, a flash disk, or a removable hard disk, and may be downloaded or copied to a memory of a local device, or may be version-updated in a system of the local device, and when the program in the memory is executed by a processor, all or part of the functions in the above embodiments may be implemented.

The present invention has been described in terms of specific examples, which are provided to aid understanding of the invention and are not intended to be limiting. For a person skilled in the art to which the invention pertains, several simple deductions, modifications or substitutions may be made according to the idea of the invention.

Claims

1. A charging port pose assessment method is characterized by comprising the following steps:

acquiring a standard image of a charging port;

obtaining a plurality of first hole features and a plurality of second hole features which take a charging connecting hole in the charging port as a feature according to the standard image and the charging port image respectively, wherein the first hole features correspond to the charging connecting hole in the standard image, and the second hole features correspond to the charging connecting hole in the charging port image;

matching the second hole feature with the first hole feature;

if the second hole feature matches the first hole feature, obtaining a homography matrix;

2. The charging port pose assessment method of claim 1, wherein after said determining the pose of the charging port, the method further comprises:

acquiring a charging port image of the vehicle to be charged at least one other viewing angle;

acquiring poses corresponding to the charging ports in all the visual angles according to the charging port images corresponding to all the visual angles;

and (5) minimizing the reprojection errors of the poses of all the visual angles, and updating the poses of the charging port.

3. The charging port pose evaluation method according to claim 1, wherein the obtaining a plurality of first hole features and a plurality of second hole features that are characterized by a charging connection hole in the charging port, from the standard image and the charging port image, respectively, comprises:

respectively preprocessing the standard image and the charging port image to strengthen edge information;

and performing edge detection on the standard image and the charging port image, identifying a charging connection hole, and obtaining a plurality of first hole characteristics and second hole characteristics.

4. The charging port pose assessment method according to claim 1, wherein the matching the second hole feature with the first hole feature comprises:

obtaining a feature descriptor of the first hole feature and a feature descriptor of the second hole feature according to a plurality of the first hole features and a plurality of the second hole features respectively;

matching the second hole feature with the first hole feature according to the feature descriptor of the second hole feature and the feature descriptor of the first hole feature.

5. The charging port pose assessment method according to claim 4, wherein the feature descriptors of the first hole feature and the second hole feature each comprise a position relation descriptor, a size descriptor and a color descriptor;

wherein the positional relationship descriptor of the first hole feature is a positional relationship of one of the first hole features in a corresponding plurality of the first hole features; the size descriptor is a radius or diameter of the first hole feature; the color descriptor is a mean and a variance of the gray levels inside the first hole feature;

the positional relationship descriptor of the second hole feature is a positional relationship of one of the second hole features in a corresponding plurality of the second hole features; the size descriptor is a radius or diameter of the second hole feature; the color descriptor is a mean and variance of the gray levels inside the second hole feature.

6. The charging port pose assessment method according to any one of claims 1-5, wherein the obtaining a homography matrix comprises:

acquiring a second coordinate array of a plurality of second hole features in the charging port image;

acquiring a first coordinate array of a plurality of first hole features in the standard image;

and obtaining the homography matrix according to the second coordinate array and the first coordinate array.

7. The charging port pose estimation method according to any one of claims 1 to 5, wherein the acquiring of the charging port image of the vehicle to be charged in one view comprises:

acquiring a vehicle image of one side of a vehicle to be charged, which is provided with a charging port, at a viewing angle;

identifying a charging port in the vehicle image;

and acquiring a charging port image of a charging port corresponding region in the vehicle image corresponding to the visual angle.

8. A charging port pose assessment system, comprising:

the processing module is used for obtaining a plurality of first hole characteristics and a plurality of second hole characteristics which take a charging connecting hole in the charging port as a characteristic according to a standard image and the charging port image respectively, wherein the first hole characteristics correspond to the charging connecting hole in the standard image, and the second hole characteristics correspond to the charging connecting hole in the charging port image; matching the second hole feature with the first hole feature; if the second hole feature matches the first hole feature, obtaining a homography matrix; and decomposing the homography matrix to obtain a rotation matrix and a translation matrix, and determining the pose of the charging port according to the rotation matrix and the translation matrix.

9. A charger robot, characterized by, includes:

the mechanical arm is used for carrying the charging plug to move;

a processing module for determining the pose of the charging port by the charging port pose estimation method according to any one of claims 1 to 7, based on the charging port image; and controlling the mechanical arm to move according to the pose of the charging port so as to realize that the charging plug is connected with the charging port.

10. A computer-readable storage medium characterized in that the medium has stored thereon a program executable by a processor to implement the charging port pose estimation method according to any one of claims 1 to 7.