A battery replacing robot pose estimation method based on point cloud component segmentation and registration

Abstract

A battery changing robot pose estimation method based on point cloud component segmentation and registration, comprising the steps of: 1, placing a 3D vision sensor under the chassis of an electric vehicle to shoot a battery pack to be changed, obtaining an RGB color image and a depth image containing a battery pack unlocking device in the scene; 2, using a convolutional neural network to obtain the unlocking instance segmentation mask of the collected color image, combining the collected depth image and the camera intrinsic parameter to project the unlocking instance into an unlocking point cloud; 3, performing statistical filtering and voxel filtering on the unlocking point cloud to remove point cloud noise and perform downsampling, thereby improving the quality of the point cloud data; 4, segmenting the unlocking lock head point cloud through a PointNet point cloud segmentation network; 5, using a principal component analysis method to obtain the important part of the unlocking lock head point cloud, calculating the FPFH feature of the important part, finding the corresponding relationship between the scene lock head point cloud and the template lock head point cloud through the RANSAC method to complete coarse registration, and finally using ICP to optimize the registration result to obtain the estimated pose of the unlocking.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of intelligent battery replacement of electric vehicles, and particularly relates to a battery replacement robot pose estimation method based on point cloud component segmentation and registration. BACKGROUND

[0002] Under the situation of increasing new energy vehicle ownership in China, solving the problem of new energy vehicle rapid energy replenishment plays an important role in relieving user range anxiety and accelerating the cultivation and development of the new energy vehicle industry. At present, most electric vehicles use fixed charging piles for charging to replenish energy. This method is flexible and convenient, but requires a long charging time, and is prone to two extreme situations of zombie piles and insufficient charging piles. Compared with the charging method, directly replacing the battery pack can complete the rapid energy replenishment of new energy vehicles.

[0003] In the battery replacement process of new energy vehicles, providing accurate battery pack locking and unlocking space pose information for the battery replacement robot is a key step to realize the disassembly and installation of the battery pack. Mechanical positioning accurately calculates the locking and unlocking space pose through a flexible parking platform, fixed vehicles, battery packs, and locking and unlocking sizes. With the increase of new energy vehicle brands, the problem of weak compatibility of mechanical positioning for different brands of battery packs and locking and unlocking gradually emerges.

[0009] Visual positioning collects environmental information through a visual sensor, and realizes locking and unlocking pose estimation through image processing, point cloud processing, and corresponding pose estimation algorithms, which has strong adaptability to the environment, battery pack types, and locking and unlocking structures. Therefore, there is an urgent need for a visual-based battery locking and unlocking positioning method to compensate for the defects of mechanical positioning and improve the compatibility and accuracy of battery pack locking and unlocking positioning.

[0005] Comparison with patent CN112561998B "Robot positioning and autonomous charging method based on three-dimensional point cloud registration"

[0006] In patent CN112561998B, scene point clouds obtained by a laser radar are used to obtain point cloud data effectively containing a charging room by using a method of constraining point cloud coordinates. We use an RGBD camera to collect a color image and a depth image, first locate a scene target in the color image using a deep neural network, and then project the depth data corresponding to the scene target part to obtain a target point cloud. Since the distance between the robot and the target is constantly changing, the point cloud cutting method based on image target recognition and instance segmentation is more adaptive and flexible than the method of cutting target point clouds in a fixed range.

[0007] In patent CN112561998B, the normal distribution transform (NDT) method is used for rough registration with the template point cloud. However, we use the random sample consensus (RANSAC) method for rough registration with the template point cloud, which is more suitable for scenes that do not require high alignment between the scene point cloud and the template point cloud and need to be processed quickly.

[0008] Comparison with the technology of patent CN115690209A "Three-dimensional positioning and recognition method, device, medium and equipment for wiring terminal of power distribution transformer"

[0009] In patent CN115690209A, a 3D scanner is used to obtain the scene point cloud, and then a straight-through filtering method is used to extract the target point cloud from the scene point cloud. However, we use an RGBD camera to collect color images and depth maps, first use a deep neural network to locate the scene target in the color image, and then project the corresponding depth data of the scene target part to obtain the target point cloud. Since straight-through filtering requires specifying a coordinate range with three fixed directions of the point cloud, this limits the extraction of target point clouds at different distances and angles. We rely on deep learning methods, and the extraction process of target point clouds is more intelligent and flexible.

[0010] Comparison with the technology of patent CN115272655A "Multi-type battery pack visual positioning method and system device for battery changing robot"

[0011] In patent CN115272655A, the HSV color space transformation is used to convert the color of the vehicle chassis battery pack point cloud from the RGB space to the HSV space, and the lock hole is identified based on the color features of the HSV space and the circular shape of the lock hole. However, we use a deep learning-based target detection and instance segmentation technology to locate the lock and unlock, which has stronger robustness and adaptability.

[0012] In patent CN115272655A, the method of fitting the center and normal vector of the lock hole point cloud is used to obtain the pose of the lock hole. However, we use a method based on point cloud feature description and registration. Due to the influence of the shooting angle, the point cloud of the object will have certain deformation. The feature description method can not rely on the overall contour of the point cloud, so the result of pose estimation is more accurate.

[0013] Comparison with the technology of patent CN117635699A "Power battery pose estimation method based on machine vision point cloud segmentation"

[0014] The patent CN117635699A adopts the RANSAC method to fit the battery pack plane part point cloud, and extracts the lock head point cloud according to the distance of each point in the point cloud to the plane. And we use the point cloud component segmentation method based on deep learning to directly segment the lock head point cloud from the unlocking point cloud, which effectively overcomes the problem of inaccurate segmentation results caused by selecting point clouds at a fixed distance.

[0015] The patent CN117635699A adopts the method of calculating the overall FPFH feature of the lock head point cloud, then using the RANSAC method for rough registration with the template point cloud, and then using the ICP (Iterative Closest Point, ICP) for accurate registration to obtain the unlocking pose. And we use the principal component analysis method to extract the main part of the lock head point cloud, then extract the point cloud feature of this part for RANSAC rough registration, and then use the original overall point cloud for ICP fine registration to obtain the unlocking pose, which can effectively overcome the problem of weak feature expression caused by the hole in the point cloud on the side of the object away from the camera. SUMMARY

[0016] To solve the above technical problems, the application provides a battery changing robot pose estimation method based on point cloud component segmentation and registration, which improves the compatibility of the battery changing robot for different vehicle models and different structural size battery packs.

[0017] To achieve the above purpose, the technical scheme adopted by the application is:

[0018] A battery changing robot pose estimation method based on point cloud component segmentation and registration, the specific steps of which are:

[0019] (1) 3D vision sensor installation and working mode;

[0020] Use the 3D vision sensor to collect the color image and depth image of the battery pack of the vehicle chassis in the intelligent battery changing station;

[0021] (2) Obtain the unlocking original point cloud;

[0022] Apply the image instance segmentation technology to the collected color image, output the unlocking instance segmentation mask, locate the region of interest of the unlocking in the image, and project the interested unlocking region to obtain the scene point cloud P0 in combination with the instance segmentation mask and the collected depth image;

[0023] (3) Unlocking point cloud preprocessing;

[0024] Use point cloud statistical filtering to filter the point cloud P0, remove the outliers and noise points in P0, obtain the point cloud P1, use voxel filtering to downsample the point cloud P1, reduce the number of point clouds and retain the fine geometric structure of the point cloud, obtain the point cloud P2;

[0025] (4) unlocking and locking point cloud segmentation;

[0026] Train the PointNet point cloud segmentation network, input the point cloud P2 into the trained PointNet network for component segmentation, and obtain the lock point cloud P3;

[0027] (5) Enabling and unlocking the lock point cloud registration;

[0028] The principal component analysis method is used to calculate the important part P of the lock point cloud P3 m , and calculate P m FPFH local feature F(P m ), use the same method to obtain the important part Q of the template point cloud Q m and important part features F(Q m ), and use the RANSAC method to find F(P m ) and F(Q m ) completes the corresponding relationship P m and Q m Then use ICP to complete the precise registration of P3 and Q to obtain the unlocked estimated pose T e .

[0029] Furthermore, the 3D vision sensor in step (1) is installed and works as follows:

[0030] The visual sensor is placed at a fixed position on the battery swap robot at a distance of 0.4 to 0.7 meters from the car chassis. The alignment relationship T0 of the 3D vision sensor's color camera and depth camera needs to be calibrated. During the movement of the battery swap robot, the visual sensor takes color and depth images of the car chassis including the battery pack unlocked.

[0031] Furthermore, step (2) of obtaining the encrypted and unlocked original point cloud includes the following steps:

[0032] (2-1) Use a convolutional neural network to segment the unlocked instances in the image and output the unlocked instance mask. The output mask is the probability p of each image pixel belonging to the foreground and background. All pixels with a membership probability p greater than 0.5 are selected as the segmentation mask of the unlocked target.

[0033] (2-2) Using the alignment relationship T0 between the color camera and the depth camera, the depth value corresponding to the unlocked area in the color image is obtained, and the depth value is projected into the unlocked original point cloud P0 using the intrinsic parameter K of the depth camera.

[0034] Furthermore, step (3) of preprocessing the unlocked point cloud includes the following steps:

[0035] (3-1) First, perform point cloud statistical filtering. For each point p in the point cloud,i , calculate the mean μ and variance σ of the Euclidean distance of n points in the field of the point, if p i The Euclidean distance between a point in the neighborhood and p i is within the range of (μ-kσ, μ+kσ), the point is retained, otherwise it is rejected, and all points in the point cloud are traversed to obtain the P0 statistical filtered point cloud P1.

[0036] (3-2) A voxel grid is established for the point cloud P1, the centroid of the point cloud in each voxel grid is calculated and used to replace the voxel grid point, and all voxel grids are traversed to obtain the voxel filtered point cloud P2.

[0037] Further, step (4) of adding and unlocking the lock head point cloud segmentation includes the following steps:

[0038] (4-1) The collected point cloud data set is expanded, and the point cloud data set is manually labeled using the CloudCompare software, wherein the lock head part is labeled as 0, and the other parts except the lock head are labeled as 1;

[0039] (4-2) The labeled point cloud data set is used to train the PoinNet network;

[0040] (4-3) The point cloud P2 is input into the trained PoinNet network to obtain the confidence score containing the label of each point in the point cloud belonging to 0 and 1, and the points belonging to label 0 are segmented from the point cloud according to the confidence result to obtain the added and unlocked lock head point cloud P3.

[0041] Further, step (5) of adding and unlocking the lock head point cloud registration includes the following steps:

[0042] (5-1) The principal component analysis method is used to extract the important parts P m and Q m of the lock head point cloud P3 and the template point cloud Q;

[0043] (5-2) The FPFH features F(P m ) and F(Q m ) of P m and Q m are calculated;

[0044] (5-3) The RANSAC method is used to find the correspondence of F(P m ) and F(Q m ) to complete the coarse registration of P3 and Q, and finally the ICP is used to complete the accurate registration of P3 and Q to obtain the estimated pose T e of the added and unlocked lock head.

[0045] The benefits brought by the present application are:

[0046] The application discloses a battery replacing robot pose estimation method and system based on point cloud component segmentation and registration. The method uses a visual sensor to obtain a color image and a depth image containing battery unlocking and locking in a scene, obtains a lock head point cloud through image instance segmentation and PointNet point cloud component segmentation, uses a principal component analysis method to obtain an important part of the lock head point cloud, extracts an FPFH feature of the part, uses a RANSAC algorithm to preliminarily register the scene lock head point cloud and the target lock head point cloud on the basis, and finally obtains the pose of the battery unlocking and locking through an ICP point cloud fine registration algorithm. The pose estimation method uses a powerful nonlinear data feature learning capability of a neural network, and can effectively avoid the influence of different angles and distances of the visual sensor on the battery unlocking and locking pose estimation result by extracting important components of the point cloud. BRIEF DESCRIPTION OF DRAWINGS

[0047] Figure 1 is a flowchart of the method disclosed by the application;

[0048] Figure 2 is the installation and working mode of the 3D visual sensor of the battery replacing positioning system disclosed by the application;

[0049] Figure 3 is a structural schematic diagram of the PointNet point cloud segmentation model. DETAILED DESCRIPTION

[0050] The application will be further described in detail in combination with the drawings and the specific embodiments:

[0051] The application discloses a battery replacing robot pose estimation method and system based on point cloud component segmentation and registration, and a flowchart of the method is as shown in Figure 1 The method comprises the following steps:

[0052] The installation and working mode of the 3D visual sensor in step (1) is as shown in Figure 2 .

[0053] The visual sensor is placed on the battery replacing robot at a fixed position 0.4-0.7 m away from the automobile chassis. It is necessary to calibrate the alignment relationship T0 of the 3D visual sensor color camera and the depth camera. During the movement of the battery replacing robot, the visual sensor shoots color and depth images containing battery unlocking and locking of the automobile chassis.

[0054] Step (2) of obtaining the original point cloud of unlocking and locking comprises the following steps:

[0055] (2-1) Using a convolutional neural network to segment the image unlocking instance, output the unlocking instance mask. The network uses a multi-layer residual convolutional neural network to extract multi-level features of the image, and fuses features of different scales through a feature pyramid structure to obtain multi-scale features of the image. Then, use the segmentation mask prediction layer to predict the membership probability p of each pixel between foreground and background, and generate a pixel-level instance mask. Finally, select all pixels with membership probability p greater than 0.5 as the instance segmentation mask of the unlocking target;

[0056] (2-2) Using the alignment relationship T0 of the color camera and the depth camera, the depth value corresponding to the unlocking area in the color picture is obtained, and the depth value is projected into the original point cloud P0 of the unlocking by using the internal parameter K of the depth camera. The projection relationship is as follows:

[0057]

[0058] Where u, v are pixel coordinates, f x , f y , c x , c y are the internal parameters of the color camera, and z is the depth value.

[0059] Step (3) unlocking point cloud preprocessing includes the following steps:

[0060] (3-1) For a point p i in the point cloud P0, calculate the mean and variance of the Euclidean distance between p i and its n neighbors:

[0061]

[0062] Where d i is the Euclidean distance between p i and its neighbor. If the Euclidean distance between a point in the neighborhood of p v and p v is within the range (μ-kσ, μ+kσ), the point is retained, otherwise the point is considered as an outlier and is removed. Traverse all points in P0 to complete statistical filtering and obtain point cloud P1.

[0063] (3-2) Establish a voxel grid for point cloud P1, and set the voxel grid to 1mm. For a voxel grid containing m points, calculate the centroid of all points in the grid:

[0064]

[0065] Use point (x v , y v , z v ) as the filtered point of the voxel grid. Traverse all voxel grids in P1 to complete point cloud voxel filtering and obtain point cloud P2.

[0066] Step (4) Enabling and unlocking the lock point cloud segmentation includes the following steps:

[0067] (4-1) Collect 200 locked and unlocked point clouds from different angles and distances. Perform a small-distance spatial pose transformation on each point cloud to increase the angle of the collected point cloud. Then add Gaussian noise to simulate the random jitter during the point cloud collection process to complete the expansion of the point cloud data. Use CloudCompare software to manually label the expanded point cloud dataset, where the locked part is labeled as 0 and the rest of the part is labeled as 1.

[0068] (4-2) Store the xyz coordinate information of n points in the point cloud into a tensor Input, whose shape is (n,3). The main module of the PointNet network is the multi-layer perceptron mlp (c1,c2) , the maximum pooling layer max d , feature concatenation layer cat d [F1, F2]. MLP consists of a convolutional layer Conv, a normalization layer Bn, and an activation layer ReLU. c1 and c2 represent the number of input and output channels of MLP, respectively. d represents the dimension for maximum pooling and feature concatenation. F1 and F2 represent the two features to be concatenated.

[0069] The forward propagation process of the PointNet model is as follows: the shape of the two-dimensional tensor composed of the xyz coordinates of n points in the point cloud is (n, 3), which is input into the mlp (3,64) Get the local feature f1, f1 input mlp (64,128) and mlp (128,1024) After the two mlp layers are connected in series, f2 is obtained. f2 is input into the max1 layer to obtain a global feature f3 with a shape of (1,1024). f3 and f1 are input into cat2(f3,f1) to output a feature f with both local and global information. m , f m Enter mlp (1088,512) 、mlp (512,256) and mlp (256,128) The structure of three mlp layers in series is f4, which is input into mlp (128,2) mlp(128,2) gets f5, and finally f5 is input into the Log-softmax layer to output a tensor Output with a shape of (n,2). The structural diagram of the model is as follows Figure 3 shown.

[0070] (4-3) Each point cloud is sent into the trained PointNet to output an Output tensor with shape (n, 2), containing n points corresponding to the confidence scores of label 0 and label 1. Points with label 0 scores greater than label 1 scores are segmented from P2 to obtain the locking / unlocking lock head point cloud P3.

[0071] Step (5) locking / unlocking lock head point cloud registration includes the following steps:

[0072] (5-1) Extract the important components of the point cloud using principal component analysis. First, project the points p i (x i ,y i ,z i ) of the point cloud P3 and the centroid p c (x c ,y c ,z c ) to the xoy plane, and calculate the following covariance matrix:

[0073]

[0074] Solve the eigenvalues of matrix C, and arrange them in ascending order λ1< λ2< λ3. Solve the eigenvectors corresponding to the three eigenvalues {v1, v2, v3}, and establish the following principal component coordinate system:

[0075]

[0076] Move the coordinate system X p Y p Z p to the centroid p c of the point cloud, calculate the projection of each point in P3 in the X p direction of the coordinate system, and divide the point cloud into two parts according to the positive and negative of the projection. Select the part with more points and lower resolution as the important part P m of the point cloud. Use the same method to obtain the important part Q m of the template point cloud Q.

[0077] (5-2) The FPFH feature calculation formula of a point p s in the point cloud is:

[0078]

[0079] where p j is a point in the neighborhood of p s , ω j is the Euclidean distance between p j and p s , and SPFH(p s ) is the simplified feature histogram, which is calculated as follows:

[0080] 1. Note the point p s The normal vector is n s , establish p s and p j The local coordinate system (u,v,w):

[0081]

[0082] 2. According to the local coordinate system (u, v, w) and p j Normal vector calculation p s Local features of:

[0083]

[0084] 3. Divide the value space of the characteristic quantities α, φ and θ into 11 equal parts, and count the p s Each point pair in the neighborhood (p s ,p j ) features (α, φ, θ), and then concatenate these three histograms into a 33-dimensional vector representing p s SPFH characteristics.

[0085] Point cloud P m and Point Cloud Q m All points in the calculation of the FPFH feature to obtain F(P m ) and F(Q m ).

[0086] (5-3) The process of using the RANSAC method to complete the point cloud coarse registration is described as follows:

[0087] 1. In point cloud P m Three sampling points are randomly selected in the point cloud Q. To ensure the difference of FPFH features of the sampling points, the distance between the two sampling points should be greater than the predetermined distance threshold d. m Find and P m The three corresponding points with the most similar FPFH features among the sampling points.

[0088] 2. Based on the three pairs of matching points selected, the rigid body transformation matrix T between the corresponding points is calculated by SVD singular value decomposition SAC , according to T SAC Find point cloud P m and Q m The transformation error is:

[0089]

[0090] Where p i is a point in the scene point cloud, q i Point cloud Q m Middle distance pi The latest point, h(·) is Huber loss.

[0091] 3. Repeat steps (1) and (2) until the error T SAC is less than a threshold or the number of iterations reaches an upper limit, obtaining the final transformation matrix T SAC . Apply T SAC to P3 to complete the coarse registration of P3 and Q.

[0092] After the preliminary registration of P3 and Q, their spatial positions are roughly coincident. In order to make the result of pose estimation more accurate, the ICP algorithm is used to complete the accurate registration of point clouds P3 and Q, obtaining the pose transformation matrix T ICP The final output of the unlocking pose information is T e = T ICP · T SAC .

[0093] The above description is only a preferred embodiment of the present application, and is not intended to limit the present application in any other form, and any modification or equivalent change made according to the technical essence of the present application still falls within the scope of the present application.

Claims

1. A method for estimating the pose of a battery-swapping robot based on point cloud component segmentation and registration, characterized by: The specific steps for use are: (1) 3D vision sensor installation and working method; Use 3D vision sensors to collect color and depth images of vehicle chassis battery packs in smart battery swap stations; (2) Obtain the encrypted and unlocked original point cloud; Apply the image instance segmentation technology to the collected color image, output the unlocked instance segmentation mask, locate the unlocked region of interest in the image, combine the instance segmentation mask and the collected depth map, and project the unlocked region of interest to obtain the scene point cloud ; (3) Encryption and unlocking point cloud preprocessing; Point cloud statistical filtering Filter and remove Outliers and noise points in the point cloud are obtained , use voxel filtering to filter the point cloud Downsampling is performed to reduce the number of point clouds and retain the fine geometric structure of the point cloud to obtain the point cloud ; (4) Enabling and unlocking the lock point cloud segmentation; Train the PointNet point cloud segmentation network and transform the point cloud Input the trained PointNet network for component segmentation to obtain the unlocking lock point cloud ; (5) Enabling and unlocking the lock point cloud registration; Calculate the lock point cloud using principal component analysis The important part , and calculate FPFH local features , use the same method to obtain the template point cloud The important part and important features , and use the RANSAC method to find and The corresponding relationship is completed and Coarse step registration, and then complete with ICP and Accurate registration to obtain the estimated pose of the unlocked .

2. The method for estimating the pose of a battery-swap robot based on point cloud component segmentation and registration according to claim 1, characterized in that: The 3D vision sensor installation and working method in step (1) is as follows: Place the visual sensor at a fixed position on the battery swap robot at a distance of 0.4~0.7m from the vehicle chassis. Calibrate the alignment relationship between the 3D visual sensor's color camera and depth camera. During the movement of the battery swap robot, the visual sensor takes color and depth pictures of the car chassis including the battery pack and unlocking it.

3. The method for estimating the pose of a battery-swap robot based on point cloud component segmentation and registration according to claim 1, characterized in that: Step (2) to obtain the encrypted and unlocked original point cloud includes the following steps: (2-1) Use a convolutional neural network to segment the unlocked instances in the image and output the unlocked instance mask. The output mask is the membership probability p of each image pixel to the foreground and background. All pixels with membership probability p greater than 0.5 are selected as the segmentation mask of the unlocked target. (2-2) Utilizing the alignment relationship between the color camera and the depth camera , obtain the depth value corresponding to the unlocked area in the color image, and use the intrinsic parameter of the depth camera Project the depth value into the unlocked raw point cloud .

4. The method for estimating the pose of a battery-swap robot based on point cloud component segmentation and registration according to claim 1, characterized in that: Step (3) Encryption and unlocking point cloud preprocessing includes the following steps: (3-1) First, perform point cloud statistical filtering. For each point in the point cloud , calculate the area within the point The mean of the Euclidean distances of the points and variance ,if A point in the neighborhood The Euclidean distance is If the point is within the range, it will be retained, otherwise it will be eliminated. After traversing all the points in the point cloud, we can get Point cloud after statistical filtering ; (3-2) Point Cloud Establish a voxel grid, calculate the centroid of the point cloud in each voxel grid and use it to replace the voxel grid point, traverse all voxel grids, and obtain the voxel filtered point cloud .

5. The method for estimating the pose of a battery-swap robot based on point cloud component segmentation and registration according to claim 1, characterized in that: Step (4) Enabling and unlocking the lock point cloud segmentation includes the following steps: (4-1) Expand the collected point cloud dataset and manually label the point cloud dataset using CloudCompare software, where the lock part is labeled as 0 and the rest of the part is labeled as 1; (4-2) Train the PoinNet network using the labeled point cloud dataset; (4-3) Point cloud Input the trained PoinNet network to obtain the confidence score of each point in the point cloud belonging to two labels: 0 and 1. According to the confidence result, the points belonging to label 0 are segmented from the point cloud to obtain the unlocking point cloud. .

6. The method for estimating the pose of a battery-swap robot based on point cloud component segmentation and registration according to claim 1, characterized in that: Step (5) Enabling and unlocking the key point cloud registration includes the following steps: (5-1) Extracting the lock point cloud using principal component analysis and template point cloud The important part and ; (5-2) Calculation and FPFH characteristics and ; (5-3) Use RANSAC method to find and The corresponding relationship is completed and Coarse step registration, and finally completed with ICP and Accurate registration to obtain the estimated pose of the unlocked .

Images