Battery replacement robot navigation pose measurement method based on three-dimensional visual features

Abstract

The invention discloses a method for measuring the navigation pose of a battery replacement robot based on three-dimensional visual features. The method comprises the following steps: 1, acquiring a color image and a depth image of a scene by using a 3D visual sensor; 2, positioning a locking and unlocking area in the color image through a convolutional neural network, and projecting depth data of the area into a point cloud by combining internal parameters of a camera; 3, performing statistical filtering and voxel filtering preprocessing on the initial locking and unlocking point cloud; 4, collecting five key points from the target point cloud by using an FPS sampling method, solving the coordinate of each key point in the target point cloud object coordinate system, and taking the coordinate as a directional vector; 5, points in the scene point cloud are converted into an object coordinate system, candidate points of the key points are screened according to the direction and length of the steering vector, and the key points of the scene point cloud are determined from the candidate points; and 6, fitting the pose of the scene and unlocking the point cloud by adopting a least square method according to the corresponding relation between the two groups of key points of the target point cloud and the scene point cloud, and optimizing the pose estimation result by using an ICP algorithm.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent battery replacement for new energy electric vehicles, and specifically is a navigation posture measurement method for a battery replacement robot based on three-dimensional visual features. Background Art

[0002] Against the backdrop of the rapid growth of new energy vehicle ownership in my country, how to effectively solve the problem of rapid energy replenishment for new energy vehicles is becoming the key to alleviating users' mileage anxiety and promoting industrial development. At present, most new energy electric vehicles rely mainly on charging piles for energy replenishment. Although this method is flexible and convenient, it takes a long time to charge and is prone to waste or shortage of charging pile resources. Battery swapping technology, as a solution, enables electric vehicles to complete energy replenishment within a few minutes, greatly shortening the waiting time. However, the key to achieving this process is to provide the battery swapping robot with accurate battery pack unlocking posture information to ensure rapid removal and installation of the battery pack.

[0003] Traditional mechanical positioning uses the accurate dimensions of the vehicle chassis, battery pack, unlocking and unlocking, and the fixed spatial distance after the vehicle stops, and uses a program to calculate the spatial posture of the unlocking and unlocking relative to the battery swapping robot. However, with the diversified development of new energy vehicle brands and battery pack models, mechanical positioning has gradually exposed the problem of insufficient compatibility when dealing with battery packs of different brands and unlocking and unlocking structures. Visual positioning collects rich environmental information through visual sensors, and uses image processing, point cloud processing and advanced posture estimation algorithms to achieve precise positioning of the unlocking and unlocking posture of the battery pack. Compared with mechanical positioning, visual positioning not only has higher compatibility and adaptability, but also reduces dependence on physical infrastructure, reduces system complexity and cost, and achieves a more efficient battery pack replacement process.

[0004] Therefore, there is an urgent need for a vision-based battery pack unlocking pose estimation method to make up for the shortcomings of traditional mechanical positioning. This method will further improve the energy efficiency of new energy vehicles, and will also promote the further development of the new energy vehicle industry and enhance its market competitiveness.

[0005] Technical comparison with patent CN112561998B "A robot positioning and autonomous charging method based on three-dimensional point cloud registration"

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

[0007] Patent CN112561998B first uses the Normal Distributions Transform (NDT) method to roughly align the scene point cloud with the template point cloud. We first use the FPS method to collect a fixed number of key points in the template point cloud, and then use the guide vector to select the key points corresponding to the template point cloud in the scene point cloud. Through the correspondence between the three-dimensional coordinates of the key points, the least squares method is used to fit the spatial pose of the scene point cloud.

[0008] Technical comparison with patent CN115690209A "A three-dimensional positioning and identification method, device, medium, and equipment for distribution transformer terminal"

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

[0010] Technical comparison with patent CN115272655A "Visual positioning method and system device for multi-type battery packs for battery swapping robots"

[0011] Patent CN115272655A uses HSV color space transformation to convert the color of the chassis battery pack point cloud from RGB space to HSV space, and identifies and unlocks the hole based on the color features of the HSV space and the circular feature of the lock hole. We use deep learning-based target detection and instance segmentation technology to locate and unlock, which has stronger robustness and adaptability.

[0012] Patent CN115272655A uses the method of fitting the center and normal vector of the keyhole point cloud to obtain the pose of the keyhole. We first collect a fixed number of key points in the template point cloud using the FPS method, and then use the guide vector to select the key points corresponding to the template point cloud in the scene point cloud. Through the correspondence between the three-dimensional coordinates of the key points, the least squares method is used to fit the spatial pose of the scene point cloud. Affected by the shooting angle, the point cloud of the object will have a certain deformation. The correspondence between the coordinates of the key points can be used without relying on the overall contour of the point cloud, so that the pose estimation result is more accurate.

[0013] Technical comparison with patent CN117635699A "Power battery pose estimation method based on machine vision point cloud segmentation"

[0014] Patent CN117635699A uses the RANSAC method to fit the point cloud of the battery pack plane, and then extracts the lock point cloud based on the distance from each point in the point cloud to the plane. We use RGBD cameras to collect color images and depth images, first use a deep neural network to locate the scene target in the color image, and then project the depth data corresponding to the scene target part to obtain the target point cloud.

[0015] Patent CN117635699A uses the method of obtaining the overall FPFH features of the lock point cloud, and then uses the RANSAC method to roughly align it with the template point cloud, and then uses the ICP precise alignment method to obtain the lock and unlock pose. We first use the FPS method to collect a fixed number of key points in the template point cloud, and then use the guide vector to select the key points corresponding to the template point cloud in the scene point cloud. Through the correspondence between the three-dimensional coordinates of the key points, the least squares method is used to fit the spatial pose of the scene point cloud, and the ICP algorithm is used to optimize the pose estimation results. It can effectively overcome the problem of weak feature expression caused by the point cloud holes that are easy to appear on the side of the object away from the camera. Summary of the invention

[0016] In response to the above problems, the present invention proposes a navigation posture measurement method for a battery swapping robot based on three-dimensional visual features to improve the positioning compatibility of the battery swapping robot in unlocking and unlocking battery packs of different models and different structural sizes.

[0017] To achieve the above object, the technical solution adopted by the present invention is:

[0018] A navigation posture measurement method for a battery-swapping robot based on three-dimensional visual features, the specific steps of the method are as follows:

[0019] (1) Use 3D vision sensors to collect color and depth images of the battery pack;

[0020] (2) Obtain the scene initial encryption point cloud based on the collected color image and depth image;

[0021] (3) Preprocessing the scene initial encryption and unlocking point cloud;

[0022] (4) Obtain the key points of the template point cloud based on the template point cloud and calculate the guide vector;

[0023] (5) Using the required guide vector to obtain the key points of the scene point cloud;

[0024] (6) Utilize the correspondence between the key points of the template point cloud and the key points of the scene point cloud to fit and unlock the estimated pose.

[0025] Furthermore, step (1) uses a 3D vision sensor to collect a color image and a depth image of the battery pack, and the specific process includes the following steps:

[0026] The installation and working method of the 3D vision sensor is: place the vision sensor on the battery swap robot at a distance of 0.4m-0.7m 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 vision sensor takes color and depth images of the car chassis including the battery pack and unlocking.

[0027] Furthermore, step (2) obtains the scene initial encryption and unlocking point cloud based on the collected color image and depth image, and the specific process includes the following steps:

[0028] (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 a membership probability p greater than 0.5 are selected as the segmentation mask of the unlocked target;

[0029] (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 initial point cloud P0 using the intrinsic parameter K of the color camera.

[0030] Furthermore, step (3) preprocesses the scene initial encryption and unlocking point cloud, and the specific process includes the following steps:

[0031] (3-1) First, perform point cloud statistical filtering. For each point p in the point cloud P0 i , calculate the mean μ and variance σ of the Euclidean distance of n points in the area of ​​the point. If p i A point in the neighborhood and p i If the Euclidean distance of is within the range of (μ-kσ,μ+kσ), the point is retained, otherwise it is removed. All points in the point cloud are traversed to obtain the point cloud P1 after statistical filtering of P0;

[0032] (3-2) Create a voxel grid for point cloud P1, calculate the centroid of all point clouds in each voxel grid, and use the centroid as the representative point of the voxel grid to obtain the point cloud P2 after voxel filtering.

[0033] Further, step (4) obtains the key points of the template point cloud according to the template point cloud and calculates the guide vector. The specific process includes the following steps:

[0034] (4-1) Use the FPS sampling method to collect 5 key points from the template point cloud Q, denoted as

[0035]

[0036] (4-2) Calculate the object coordinate system O of the template point cloud Q Q , calculate K Q The key point is O Q The coordinates under the 5 coordinates are taken as the guide vector, recorded as

[0037] Further, step (5) uses the required guide vector to obtain the key points of the scene point cloud, and the specific process includes the following steps:

[0038] (5-1) Establish the object coordinate system O of point cloud P2 P , convert point cloud P2 to O P The point cloud P2′ is obtained in the coordinate system;

[0039] (5-2) Select the point cloud P2′ that matches V Q The points in which the angle error of any vector is less than θ and the length error is less than l are recorded, and the indexes of these points are selected from P2 according to the indexes to form a set S k ={S1,S2,S3,S4,S5};

[0040] (5-3) Calculate S k Each candidate point set S i Center of i , in S i Select and c i The point closest to S is taken as the key point of P2. k The same processing is performed on the five point sets in the figure to obtain the key points of the scene point cloud, which are recorded as

[0041] Furthermore, step (6) uses the correspondence between the key points of the template point cloud and the key points of the scene point cloud to fit the estimated pose of the unlocked object. The specific process includes the following steps:

[0042] (6-1) Based on the template point cloud key point K Q And the scene point cloud key K PThe correspondence between the coordinates is calculated and the estimated pose T1 of the point cloud P2 is fitted using the least squares method.

[0043] (6-2) Apply T1 to point cloud P2 to make it roughly coincide with target point cloud Q. Perform ICP precise registration optimization on the roughly coincident scene point cloud and template point cloud to obtain the precise registration matrix T2. The estimated pose of the final scene unlocking is: T e =T2T1.

[0044] The benefits of this application are:

[0045] The present invention discloses a method for measuring the navigation posture of a battery-swap robot based on three-dimensional visual features. The method first uses a 3D visual sensor to collect a color image and a depth image of a battery pack on a vehicle chassis. The unlocking and unlocking in the color image are located by using target recognition and instance segmentation technology, the depth data of the unlocking and unlocking area is projected to obtain the unlocking and unlocking point cloud, and the point cloud is preprocessed by statistical filtering and voxel filtering. Secondly, the key points of the target point cloud are collected by the FPS sampling method, and the guide vector is obtained according to the collected key points and the template point cloud object coordinate system. Then, the guide vector is used to select the key points corresponding to the key points of the template point cloud from the scene point cloud. Finally, according to the correspondence between the key points of the template point cloud and the scene point cloud, the posture of the unlocking and unlocking is fitted by the least squares method, and the posture result is optimized by the ICP algorithm. This method uses the correspondence between the key points to fit the unlocking and unlocking posture, which can effectively avoid the influence of different angles and distances of the visual sensor on the unlocking and unlocking posture estimation results. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 is a flow chart of the method disclosed in the present invention;

[0047] Figure 2 It is a schematic diagram of the installation and working mode of the 3D visual sensor of the battery swap positioning system disclosed in the present invention;

[0048] Figure 3 It is the pre-processed scene plus unlocked point cloud;

[0049] Figure 4 It is a schematic diagram of the collected template point cloud key points and the template point cloud object coordinate system;

[0050] Figure 5 It is a schematic diagram of the candidate point set of scene point cloud key points;

[0051] Figure 6 It is a schematic diagram of the collected scene point cloud key points and scene point cloud object coordinate system;

[0052] Figure 7 It is a schematic diagram of applying the fitted spatial transformation matrix to the scene point cloud to roughly overlap with the template point cloud. DETAILED DESCRIPTION

[0053] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0054] The present invention discloses a navigation posture measurement method and system for a battery-swapping robot based on three-dimensional visual features. The implementation flow chart of the method is as follows: Figure 1 As shown, the following steps are included:

[0055] Step (1) using a 3D vision sensor to collect a color image and a depth image of a battery pack includes the following steps:

[0056] Place the visual sensor at a fixed position on the battery swap robot 0.4-0.7m away from the car chassis. It is necessary to calibrate the alignment relationship T0 between the color camera and the depth camera of the 3D visual sensor, as shown in the schematic diagram. Figure 2 As shown in the figure, during the movement of the battery swap robot, the visual sensor takes color and depth images of the vehicle chassis including the battery pack and unlocking it.

[0057] Step (2) obtaining the scene initial encryption and unlocking point cloud based on the collected color image and depth image includes the following steps:

[0058] (2-1) Use a convolutional neural network to perform instance segmentation operations on the encryption and unlocking in the color image, and output the encryption and unlocking instance mask. The network uses a multi-layer residual convolutional neural network to extract the multi-level features of the image, and fuses the features of different levels through the feature pyramid structure to obtain the multi-scale features of the image. Then, the segmentation mask prediction layer is used to predict the membership probability p of each pixel between the foreground and the background to generate a pixel-level instance mask. Finally, all pixels with a membership probability p greater than 0.5 are selected as the instance segmentation mask of the encryption and unlocking;

[0059] (2-2) Use the alignment relationship T0 between the color camera and the depth camera to align the color image and the depth image, combine the unlocking instance mask obtained in (2-1), obtain the depth value corresponding to the unlocking area in the color image, and use the intrinsic parameter K of the color camera to project the depth value into the initial point cloud P0 of the unlocking. The projection relationship is as follows:

[0060]

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

[0062] Step (3) preprocessing the scene initial unlocking point cloud includes the following steps:

[0063] (3-1) For a point p in P0i , calculate p i The mean and variance of the Euclidean distance between n points in its neighborhood:

[0064]

[0065]

[0066] where d i For point p i The Euclidean distance to a point in its neighborhood. If d i If the point is within the range of (μ-kσ,μ+kσ), the point is retained, otherwise the point is considered an outlier and removed. All points in P0 are traversed to complete statistical filtering and obtain point cloud P1.

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

[0068]

[0069] The point (x v ,y v ,z v ) as the representative point of the voxel grid. Traverse all the voxel grids in P1 to complete the point cloud voxel filtering and obtain the scene point cloud P2, such as Figure 3 shown.

[0070] Step (4) obtaining the key points of the template point cloud according to the template point cloud and calculating the guide vector includes the following steps:

[0071] (4-1) Use the FPS sampling method to collect 5 key points from the template point cloud Q. A point set S contains m points. Assume that a point p outside the point set i The distance to all points in the point set S is (d1, d2, K, d m ), then p i The distance to the point set S is defined as:

[0072] d pi→s =min{d1,d2,Kd m}

[0073] Randomly select a point from the point cloud Q as the first keypoint Select the distance from the remaining points The farthest point is used as the second key point Will and Considered as a point set K Q , calculate the remaining points in Q and K respectively QThe distance between the two points is the largest, and the one with the largest distance is selected as the third key point According to the selection idea of ​​the third key point, select from the remaining points of Q and Get 5 key points collected using the FPS method Its visualization is as follows Figure 4 shown.

[0074] (4-2) Use the method in step (3-2) to calculate the center of mass of point cloud Q Calculate each point of the point cloud Q and The covariance matrix between :

[0075]

[0076] Solve the eigenvalues ​​of the matrix C and arrange them from large to small such that λ1>λ2>λ3, and find the eigenvectors v1, v2, and v3 corresponding to the three eigenvalues. In order to eliminate the ambiguity in the direction of the eigenvectors, calculate the projections of v1 on the X-axis, v2 on the Y-axis, and v3 on the positive direction of the Z-axis. If the projection is negative, change the direction of the vector. The three eigenvectors after processing are v1′(v 11 ,v 12 ,v 13 ), v′2(v 21 ,v 22 ,v 23 ), v3′(v 31 ,v 32 ,v 33 ), combine v1′, v′2, v3′ by columns to get a three-dimensional matrix as shown below:

[0077]

[0078] by The object coordinate system of point cloud Q is established with v1′, v′2, and v3′ as the origin and coordinate axes. O Q .

[0079] make middle Calculate K Q The key point in the coordinate system O Q The coordinates of the following formula are as follows:

[0080]

[0081] These are the five guide vectors required.

[0082] Step (5) using the required guide vector to obtain the key points of the scene point cloud includes the following steps:

[0083] (5-1) According to the method of (4-2) to establish the object coordinate system of point cloud Q, establish the object coordinate system p of point cloud P2 c -v′ P1 -v′ P2 -v′ P3 , denoted as O P . Where p c is the centroid of point cloud P2, v′ P1 , v′ P2 , v′ P3 The three-dimensional matrix composed of columns is denoted as V P . Convert P2 from world coordinates to coordinate system O P Next, we get the point cloud P2′, the formula is as follows:

[0084] P2′=(P2-p c )·V P

[0085] (5-2) Calculate the midpoint p of P2′ i ′=(x i ′,y i ′,z i ′) and its relationship with V Q Mean vector The angle α between them is as follows:

[0086]

[0087]

[0088] If point p i ′ and V Qi If the length difference is within the distance threshold l and the angle between the two is less than the distance threshold α, then the point is taken as a candidate point for the key point in P2′, as shown in the following formula:

[0089]

[0090] If p i ′ satisfies the above conditions as a key point, and the serial number of this point in P2′ is recorded. After traversing the point cloud P2′, the serial numbers of the points that meet the conditions form an index, and a candidate point set of 5 key points is selected from the scene point cloud P2 according to the index to form a set S k ={S1,S2,S3,S4,S5}, which can be visualized as Figure 5 shown.

[0091] (5-3)S k Contains 5 candidate point sets, including ni The candidate point set S i ={Kp j ,p j+1 ,K}, calculate the point set S i Center of i , and from S i Select and c i The closest point is taken as the key point of P2, as shown in the following formula:

[0092]

[0093]

[0094]

[0095] where p j ∈S i YesS i A point in S k The same processing is performed on the five candidate point sets in the point cloud P2 to obtain the points in the point cloud P2 that are consistent with K Q The corresponding key points Its visualization is as follows Figure 6 shown.

[0096] Step (6) uses the correspondence between the key points of the template point cloud and the key points of the scene point cloud to fit and unlock the estimated pose, including the following steps:

[0097] (6-1) According to the point cloud Q key point And point cloud P2 key points The correspondence between the coordinates is fitted and unlocked using the least squares method. This process can be calculated by singular value decomposition (SVD). The objective function to be optimized is:

[0098]

[0099] In the formula, N = 5 represents K Q , K P The number of key points in and Represent the key points in point cloud Q and point cloud P2 respectively. To use SVD decomposition, first K Q and K P The key points in are centralized, namely:

[0100]

[0101]

[0102] Since the point cloud has been transferred to the centroid, the translation is not considered, and the objective function can be transformed into:

[0103]

[0104] Because K Q , K P The coordinates of the corresponding points are fixed values. After expanding the above formula, removing the terms that do not contain R, the objective function is optimized as follows:

[0105]

[0106] The matrix Perform SVD decomposition on H to obtain matrices U, Σ, and V. Remove the coefficients before the formula, and the objective function can be further optimized as follows:

[0107] F′(R)=tr(RH)=tr(RUΣV T )=tr(ΣV T RU)=tr(ΣM)=σ1m 11 +σ2m 22 +σ3m 33

[0108] From the above formula, we can see that when the matrix M = V T When RU is the unit matrix, the optimal solution can be obtained:

[0109] R 3×3 =VU T

[0110]

[0111] The required rotation matrix R 3×3 and the translation matrix T 3×1 A 4-dimensional space transformation matrix T1 is formed, which is the initial estimated pose of the lock.

[0112] (6-2) Apply T1 to the point cloud P2 so that it roughly overlaps with the target point cloud Q. The visualization is as follows: Figure 7 As shown. The roughly overlapping scene point cloud and template point cloud are optimized by ICP precise registration to obtain the precise registration matrix T2. The estimated pose of the final scene plus unlocking is:

[0113] T e =T2·T1

[0114] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A navigation posture measurement method for a battery-swapping robot based on three-dimensional visual features, characterized in that: The specific steps for use are: (1) Use 3D vision sensors to collect color and depth images of the battery pack; (2) Obtain the scene initial encryption point cloud based on the collected color image and depth image; (3) Preprocessing the scene initial encryption and unlocking point cloud; (4) Obtain the key points of the template point cloud based on the template point cloud and calculate the guide vector; (5) Using the required guide vector to obtain the key points of the scene point cloud; (6) Utilize the correspondence between the key points of the template point cloud and the key points of the scene point cloud to fit and unlock the estimated pose.

2. According to claim 1, a navigation posture measurement method for a battery-swapping robot based on three-dimensional visual features is characterized in that: Step (1) uses a 3D vision sensor to collect a color image and a depth image of the battery pack. The specific process includes the following steps: The installation and working method of the 3D vision sensor is: place the vision sensor on the battery swap robot at a distance of 0.4m-0.7m 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 vision sensor takes color and depth images of the car chassis including the battery pack and unlocking.

3. According to claim 1, a method for measuring the navigation posture of a battery-swapping robot based on three-dimensional visual features is characterized in that: Step (2) obtains the scene initial encryption and unlocking point cloud based on the collected color image and depth image. The specific process 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 a membership probability p greater than 0.5 are selected as the segmentation mask of the unlocked target. (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 initial point cloud P0 using the intrinsic parameter K of the color camera.

4. According to claim 1, a navigation posture measurement method for a battery-swapping robot based on three-dimensional visual features is characterized in that: Step (3) preprocesses the scene initial encryption and unlocking point cloud, and the specific process includes the following steps: (3-1) First, perform point cloud statistical filtering. For each point p in the point cloud P0 i , calculate the mean μ and variance σ of the Euclidean distance of n points in the area of ​​the point. If p i A point in the neighborhood and p i If the Euclidean distance of is within the range of (μ-kσ,μ+kσ), the point is retained, otherwise it is removed. All points in the point cloud are traversed to obtain the point cloud P1 after statistical filtering of P0; (3-2) Create a voxel grid for point cloud P1, calculate the centroid of all point clouds in each voxel grid, and use the centroid as the representative point of the voxel grid to obtain the point cloud P2 after voxel filtering.

5. According to claim 1, a method for measuring the navigation posture of a battery-swapping robot based on three-dimensional visual features is characterized in that: Step (4) obtains the key points of the template point cloud according to the template point cloud and calculates the guide vector. The specific process includes the following steps: (4-1) Use the FPS sampling method to collect 5 key points from the template point cloud Q, denoted as (4-2) Calculate the object coordinate system O of the template point cloud Q Q , calculate K Q The key point is O Q The coordinates under the 5 coordinates are taken as the guide vector, recorded as 6. According to claim 1, a method for measuring the navigation posture of a battery-swapping robot based on three-dimensional visual features is characterized in that: Step (5) uses the required guide vector to obtain the key points of the scene point cloud. The specific process includes the following steps: (5-1) Establish the object coordinate system O of point cloud P2 P , convert point cloud P2 to O P Get point cloud P2′ in the coordinate system; (5-2) Select the point cloud P2′ that matches V Q The points in which the angle error of any vector is less than θ and the length error is less than l are recorded, and the indexes of these points are selected from P2 according to the indexes to form a set S k ={S1,S2,S3,S4,S5}; (5-3) Calculate S k Each candidate point set S i Center of i , in S i Select and c i The closest point is taken as the key point of P2, and the nearest point is taken as the key point of S k The same processing is performed on the five point sets in the figure to obtain the key points of the scene point cloud, which are recorded as 7. According to claim 1, a method for measuring the navigation posture of a battery-swapping robot based on three-dimensional visual features is characterized in that: Step (6) uses the correspondence between the key points of the template point cloud and the key points of the scene point cloud to fit the estimated pose of the unlocked object. The specific process includes the following steps: (6-1) Based on the template point cloud key point K Q And the scene point cloud key K P The correspondence between the coordinates, the estimated pose T1 of the point cloud P2 is fitted using the least squares method; (6-2) Apply T1 to point cloud P2 to make it roughly coincide with target point cloud Q. Perform ICP precise registration optimization on the roughly coincident scene point cloud and template point cloud to obtain the precise registration matrix T2. The estimated pose of the final scene unlocking is: T e =T2T1.

Images