A quadruped robot dog positioning and pose determination method based on point cloud pixel-level mapping

Abstract

The application relates to a kind of four-legged robot dog positioning and pose method based on point cloud pixel level mapping, belong to the field of robot technology, input three-dimensional laser point cloud data collected by laser radar, respectively with global reference coordinate system Z axis, Y axis, X axis as reference for mapping conversion to the three-dimensional data collected, and eliminate hash point and isolated point;Calculate the weighted score of two-dimensional pixel mapping point, set the score threshold and construct model to extract two-dimensional mapping feature points, according to the number of target feature points, dynamically grid division is carried out on two-dimensional mapping surface and the uniform distribution of feature points is realized;Using the matching method of fast global mapping scale feature vector, the pose change of the mapping surface is obtained according to the matching point pair, and the three-dimensional point cloud is corrected with the value;Fusion three mapping plane pose results, so as to quickly obtain the more accurate pose estimation of four-legged robot dog.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology and relates to a method for localization and pose determination of a quadruped robot dog based on pixel-level mapping of point clouds. Background Technology

[0002] With the development of robotics technology, various types of robots are being researched and applied in various fields. Quadruped robot dogs, due to their high flexibility and strong adaptability to extreme environments, can work in unstructured and harsh conditions. By combining external sensors such as cameras, LiDAR, and IMUs, and using multi-view geometry, point cloud filtering, and point cloud alignment techniques, quadruped robot dogs can estimate their pose and build maps in unknown environments. Based on the estimated pose and the constructed map, they can navigate, avoid obstacles, and perform inspections in unknown and complex environments.

[0003] In unknown environments, the success of a quadruped robot dog in completing its task hinges on the accuracy of its pose estimation. Existing laser odometry feature extraction and point cloud matching techniques yield highly accurate pose estimates, but most quadruped robot dog platforms lack powerful computing units and have limited computational resources. Traditional methods for solving the pose between adjacent frames rely on iterating through nearest neighbors; however, this becomes extremely time-consuming in large scenes with numerous points, compromising the system's real-time performance and robustness. Therefore, a robust and real-time inter-frame pose estimation method is needed to fully consider the pose estimation problem of quadruped robot dogs under limited computational resources, ensuring that the quadruped robot dog can effectively complete subsequent localization and mapping functions within a low-power embedded control system. Summary of the Invention

[0004] In view of this, the purpose of this invention is to provide a method for localization and orientation determination of a quadruped robot dog based on point cloud 3D mapping, so as to achieve stable operation of the quadruped robot dog on a low-power embedded platform. This method only uses the data collected by LiDAR to calculate the inter-frame attitude change information of the quadruped robot dog, thereby realizing real-time attitude estimation, saving more computing resources for the quadruped robot dog control system, enhancing the real-time performance of the system, and enabling it to quickly complete localization and mapping tasks.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A method for localization and pose determination of a quadruped robot dog based on pixel-level mapping of point clouds includes the following steps:

[0007] S1: Input the 3D laser point cloud data collected by the lidar. The point cloud pixel-level mapping module maps and transforms the collected 3D data with the global reference coordinate system Z-axis as the reference, and removes scattered points and isolated points.

[0008] S2: Calculate the weighted score of the two-dimensional pixel mapping points, set the score threshold and build a model to extract two-dimensional mapping feature points, and dynamically divide the two-dimensional mapping surface into a grid according to the number of target feature points to achieve uniform distribution of feature points;

[0009] S3: A fast global mapping scale feature vector matching method is adopted. The pose change of the Z-axis mapping surface is obtained by constructing the minimum mapping projection error based on the matching point pairs, and the pose change of the Z-axis mapping surface is used to correct the 3D laser point cloud.

[0010] S4: Map the corrected 3D point cloud to the global reference coordinate system Y-axis, and perform the same steps as S2-S3, correcting the 3D laser point cloud according to the pose change of the Y-axis mapping surface.

[0011] S5: Finally, the corrected 3D laser point cloud is mapped with the global reference coordinate system X-axis as the reference, and the same process as steps S2-S3 is performed to fuse the pose results of the three mapping planes, thereby obtaining the pose estimate of the quadruped robot dog.

[0012] Furthermore, step S1 specifically includes the following steps:

[0013] S11: Establish a three-dimensional coordinate system with the lidar mounted on the quadruped robot dog as the coordinate center, and construct a global reference coordinate system. Let the point cloud P be... i Let be any point cloud in three-dimensional space, and its three-dimensional coordinates be (x, y, z). i ,y i ,z i );

[0014] S12: Construct pixel-level mapping planes for the point cloud. Using the global reference coordinate system as a reference, construct pixel coordinate systems S along the three axes of the mapping plane. xoy S xoz and S yoz As shown in the following formula:

[0015]

[0016] Where x max and x min y max and y min z max and z min These are the maximum and minimum values ​​of the x, y, and z coordinates in the 3D point cloud, respectively.

[0017] S13: For any point cloud P i (x i ,y i ,z iThe 3D point cloud is mapped and transformed to S according to the following formula. xoy Planar pixel coordinate system:

[0018]

[0019]

[0020] Among them, P x and P y Representing S respectively xoy Horizontal and vertical resolution in a planar pixel coordinate system S is the mapping point of the i-th point xoy Planar pixel coordinates; and mapping point P i * The feature vector is set as Where, n i The total number of origin clouds along the Z-axis direction of the mapping point, z i The sum of the total z-values ​​of the origin cloud coordinates along the Z-axis direction of the mapping point, t i To determine the total number of original point cloud intensity values ​​along the Z-axis direction of the coordinate point, a threshold of n is set for the total number of point cloud values ​​at the mapping point. T Hash points and isolated points below the threshold are filtered out.

[0021] Furthermore, step S2 specifically includes the following steps:

[0022] S21: For the i-th mapping point P i * Feature information {n i ,z i ,t i}Weighted calculation is performed to obtain Set the score threshold to I t The formula for calculating its score is as follows:

[0023]

[0024] S22: Extract two-dimensional mapping feature points by constructing a model, denote all feature points in the training data as set P, and construct the information entropy H(P) as follows:

[0025]

[0026] Where c represents the number of feature points in the training data. This represents the number of non-feature points in the training data; for the mapping point P i * The N nearest neighbor mapping points P j Establish two-dimensional mapping feature detection, that is, let P i * Mapping points P of the N nearest neighborsj Compare and obtain state classification As shown in the following formula:

[0027]

[0028] Where s represents a low-probability feature point, n represents a general-probability feature point, and b represents a high-probability feature point, the mapping points within set P are divided into three subsets P based on their states at corresponding positions. s P n and P b And calculate the classification information entropy H according to the following formula. g :

[0029] H g =H(P)-H(P) s )-H(P n )-H(P b )

[0030] Meanwhile, the information entropy is iterated to the minimum within the subset, and the constructed model is applied to the rapid detection of two-dimensional mapped feature points;

[0031] S23: If the number of feature points extracted from the target is T, the two-dimensional mapping surface is divided into I×J two-dimensional mapping sub-blocks according to the following formula:

[0032]

[0033] Where H and W are the height and width of the target two-dimensional mapping surface, respectively; for grid regions without feature points, the next grid region with feature points is dynamically searched in sequence, and the region is divided into multiple sub-regions of the same size; by performing dynamic grid division, the actual number of feature points obtained is kept as close as possible to the target number of extractions; non-maximum suppression is used to replace clustered feature points with a single point, ensuring that feature points at all positions on the plane are evenly distributed.

[0034] Furthermore, step S3 specifically includes the following steps:

[0035] S31: First, a fast global mapping scale feature vector matching algorithm is used to find the nearest matching point pairs. The nearest distance D is the Euclidean distance, defined as follows, and asymptotic consistent sampling method is used to eliminate false matches:

[0036]

[0037] in For the feature point P to be matched i * and The feature vectors; matching is performed by using the feature vectors of two-dimensional mapping points as descriptor information;

[0038] S32: Divide the inter-frame pose into six parts and three rotation angles: pitch angle r p yaw angle r y and roll angle r r Three translational quantities: displacement t in the left and right directions. x vertical displacement t y and displacement t in the forward and backward directions z The feature points in the next frame are projected into the previous frame according to the following formula:

[0039]

[0040] in This represents the coordinates of the i-th feature point in the mapped coordinate system in the k-th frame. R represents the coordinates of the corresponding matching feature point in the (k+1)th frame, where R xoy and t xoy S xoy The rotation and translation of a plane are expressed by the following formula:

[0041]

[0042] S33: Addressing the pose of a quadruped robot dog in S xoy The pose change on the plane is solved by minimizing the mapping projection error S. xoy Planar pose change (R xoy ,t xoy Solving for the optimal (R) yields the solution. xoy ,t xoy Substitute these coordinates into the original 3D point cloud in the (k+1)th frame to obtain the corrected point cloud coordinates. The corresponding formula is as follows:

[0043]

[0044]

[0045] Furthermore, step S4 specifically includes the following steps:

[0046] S41: For any modified point cloud P i (x i ,y i ,z i The point cloud is mapped to S according to the following formula. xoz Planar pixel coordinate system:

[0047]

[0048]

[0049] Among them, Px and P z Representing S respectively xoz Horizontal and vertical resolution in a planar pixel coordinate system S is the mapping point of the i-th point xoz Planar pixel coordinates;

[0050] S42: Map point P i * The feature vector is set as n i The total number of origin clouds along the Y-axis direction of the mapping point, y i t represents the sum of the total y-values ​​of the origin cloud along the Y-axis direction of the mapping point. i To determine the total number of original point cloud intensity values ​​along the Y-axis direction of the coordinate point, a threshold of n is set for the total number of point cloud values ​​at the mapped point. T Hash points and isolated points that are less than the threshold are filtered out.

[0051] S43: For the i-th mapping point P i * Feature information {n i ,y i ,t i}Weighted calculation is performed to obtain A trained model is used to extract two-dimensional mapping feature points. The two-dimensional mapping surface is dynamically divided into grids according to the number of target feature points to achieve uniform distribution of feature points.

[0052] S44: A fast global mapping scale feature vector matching algorithm is used for matching, and the feature points in the next frame are projected onto the previous frame according to the following formula:

[0053]

[0054] In the formula R xoz and t xoz S xoz The rotation and translation of a plane are expressed by the following formula:

[0055]

[0056] S45: Addressing the pose of a quadruped robot dog in S xoz The pose change on the plane is solved by minimizing the mapping projection error S. xoz Planar pose change (R xoz ,t xoz Solving for the optimal (R) yields the solution. xoz ,t xoz Substitute these coordinates into the original 3D point cloud in the (k+1)th frame to obtain the corrected point cloud coordinates. The corresponding formula is as follows:

[0057]

[0058]

[0059] Furthermore, step S5 specifically includes the following steps:

[0060] S51: For any modified point cloud P i (x i ,y i ,z i The three-dimensional point cloud is mapped to S according to the following formula. yoz Planar pixel coordinate system:

[0061]

[0062]

[0063] Among them, P z and P y Representing S respectively yoz Horizontal and vertical resolution in a planar pixel coordinate system S is the mapping point of the i-th point yoz Planar pixel coordinates;

[0064] S52: Map point P i * The feature vector is set as n i x represents the total number of origin clouds along the X-axis direction of the mapping point. i t represents the sum of the total x-coordinates of the origin cloud along the X-axis direction of the mapping point. i To represent the total number of original point cloud intensity values ​​along the X-axis direction of the coordinate point, a threshold of n is set for the total number of point cloud values ​​at the mapping point. T Hash points and isolated points that are less than the threshold are filtered out.

[0065] S53: For the i-th mapping point P i * Feature information {n i ,x i ,t i}Weighted sum to obtain A trained model is used to extract two-dimensional mapping feature points. The two-dimensional mapping surface is dynamically divided into grids according to the number of target feature points to achieve uniform distribution of feature points.

[0066] S54: A fast global mapping scale feature vector matching algorithm is used for matching, and the feature points in the next frame are projected into the previous frame according to the following formula:

[0067]

[0068] In the formula R yoz and t yoz S yoz The rotation and translation of a plane are expressed by the following formula:

[0069]

[0070] S55: For the pose of a quadruped robot dog in S yoz The pose change on the plane is solved by minimizing the mapping error S using the following formula. yoz Planar pose change (R yoz ,t yoz ):

[0071]

[0072] By fusing the pose results from the three mapping planes, the robot dog's pose in the global reference coordinate system is obtained through coordinate transformation, thus ultimately yielding a relatively accurate pose estimate for the quadruped robot dog. y ,r p ,r r ,t x ,t y ,t z ].

[0073] The beneficial effects of this invention are as follows:

[0074] 1. This invention uses only LiDAR data to calculate the pose changes of a quadruped robot dog in complex environments, thus reducing implementation costs associated with installing other inertial navigation systems and external sensors. Compared to existing point cloud preprocessing methods, this invention employs a point cloud 3D mapping method, avoiding the processing of massive amounts of data required when using point set registration algorithms.

[0075] 2. This invention constructs a feature point extraction method for mapped point clouds through model training, enabling faster and more robust calculation of pose changes. This method cleverly avoids directly processing 3D point clouds; this rapid point cloud processing method reduces system memory usage and improves computational efficiency, thus enabling stable operation of the quadruped robot dog on a low-power embedded platform.

[0076] 3. This invention uses the feature vectors of two-dimensional mapping points as descriptor information for matching, which has certain advantages over related traditional methods and effectively improves matching efficiency and results. It also has high robustness in cases with large noise and high feature mismatch rate.

[0077] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0078] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0079] Figure 1 This is the main flowchart of a quadruped robot dog localization and pose determination method based on point cloud pixel-level mapping according to the present invention.

[0080] Figure 2 This is a diagram showing the installation location of the lidar according to an embodiment of the present invention;

[0081] Figure 3 This is an overall structural diagram of a quadruped robot dog localization and pose determination method based on point cloud pixel-level mapping according to the present invention. Detailed Implementation

[0082] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0083] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0084] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0085] Please see Figures 1-3 In this embodiment, a 16-line LiDAR is selected as the data acquisition sensor, and an algorithm is written in the ROS system to realize a quadruped robot dog localization and attitude determination method based on point cloud pixel-level mapping.

[0086] This invention presents a method for localization and pose determination of a quadruped robot dog based on pixel-level mapping of point clouds. The underlying structure is as follows: Figure 1 As shown, the specific steps include:

[0087] S1: Input the 3D laser point cloud data collected by the lidar. The point cloud pixel-level mapping module performs mapping transformation on the collected 3D data using the global reference coordinate system Z-axis as a reference, and removes scattered points and isolated points. In this embodiment, the lidar position is as follows: Figure 2 As shown, a 16-line LiDAR was used to acquire continuous point cloud data at a frequency of 10Hz, a detection angle of 360 degrees, and a resolution of 0.18 degrees. Specific data processing included:

[0088] S11: Establish a three-dimensional coordinate system with the lidar mounted on the quadruped robot dog as the coordinate center, and construct a global reference coordinate system. Let the point cloud P be... i Let be any point cloud in three-dimensional space, and its three-dimensional coordinates be (x, y, z). i ,y i ,z i );

[0089] S12: Construct pixel-level mapping planes for the point cloud. Using the global reference coordinate system as a reference, pixel coordinate systems S along the mapping planes will be constructed respectively along the three axes. xoy S xoz and S yoz As shown in the following formula:

[0090]

[0091] Where x max and x min y max and y minz max and z min These are the maximum and minimum values ​​of the x, y, and z coordinates in the 3D point cloud, respectively.

[0092] S13: For any point cloud P i (x i ,y i ,z i The 3D point cloud is mapped and transformed to S according to the following formula. xoy Planar pixel coordinate system:

[0093]

[0094]

[0095] Among them, P x and P y Representing S respectively xoy Horizontal and vertical resolution in a planar pixel coordinate system S is the mapping point of the i-th point xoy Planar pixel coordinates. And map point P. i * The feature vector is set as Where, n i The total number of origin clouds along the Z-axis direction of the mapping point, z i The sum of the total z-values ​​of the origin cloud coordinates along the Z-axis direction of the mapping point, t i To represent the total number of original point cloud intensity values ​​along the Z-axis direction of the coordinate point, a threshold of n is set for the total number of point cloud values ​​at the mapped point. T Hash points and isolated points below the threshold are filtered out.

[0096] S2: Calculate the weighted score of the two-dimensional pixel mapping points, set the score threshold, construct a model to extract two-dimensional mapping feature points, and dynamically divide the two-dimensional mapping surface into a grid according to the number of target feature points to achieve a uniform distribution of feature points; in this embodiment, it specifically includes:

[0097] S21: For the i-th mapping point P i * Feature information {n i ,z i ,t i}Weighted calculation is performed to obtain Set the score threshold to I t The formula for calculating its score is as follows:

[0098]

[0099] S22: Extract two-dimensional mapping feature points by constructing a model, denote all feature points in the training data as set P, and construct the information entropy H(P) as follows:

[0100]

[0101] Where c represents the number of feature points in the training data. This represents the number of non-feature points in the training data. For the mapping point P... i * The N nearest neighbor mapping points P j Establish two-dimensional mapping feature detection, that is, let P i * Mapping points P of the N nearest neighbors j Compare and obtain state classification As shown in the following formula:

[0102]

[0103] Where s represents a low-probability feature point, n represents a general-probability feature point, and b represents a high-probability feature point, the mapping points within set P are divided into three subsets P based on their states at corresponding positions. s P n and P b And calculate the classification information entropy H according to the following formula. g :

[0104] H g =H(P)-H(P) s )-H(P n )-H(P b (7)

[0105] Meanwhile, the information entropy is iterated to the minimum within the subset, and the constructed model is applied to the rapid detection of two-dimensional mapped feature points.

[0106] S23: If the number of feature points extracted from the target is T, the two-dimensional mapping surface is divided into I×J two-dimensional mapping sub-blocks according to the following formula:

[0107]

[0108] Where H and W represent the height and width of the target 2D mapping surface, respectively. For grid regions without feature points, the next grid region containing feature points is dynamically searched sequentially, and this region is divided into multiple sub-regions of equal size. Dynamic grid division ensures that the actual number of feature points obtained is as close as possible to the target extraction quantity. Non-maximum suppression is used to replace clustered feature points with a single point, ensuring that feature points are evenly distributed across all locations on the plane.

[0109] S3: A fast global mapping scale feature vector matching method is adopted. Based on the matching point pairs, the pose change of the mapping surface is obtained by minimizing the mapping projection error, and the 3D laser point cloud is corrected with this value. In this embodiment, it specifically includes:

[0110] S31: To improve the accuracy of matching, a fast global mapping scale feature vector matching algorithm is first used to find the matching point pairs with the closest distance. The closest distance D adopts the Euclidean distance, defined as follows, and the progressive consistent sampling method is used to eliminate false matches.

[0111]

[0112] in For the feature point P to be matched i * and The feature vectors of the two-dimensional mapping points are used as descriptor information for matching. Compared with related traditional methods, this approach has certain advantages and effectively improves registration efficiency and results.

[0113] S32: Divide the inter-frame pose into six parts and three rotation angles: pitch angle r p yaw angle r y and roll angle r r Three translational quantities: displacement t in the left and right directions. x vertical displacement t y and displacement t in the forward and backward directions z The feature points in the next frame are projected onto the previous frame using the following formula:

[0114]

[0115] in This represents the coordinates of the i-th feature point in the mapped coordinate system in the k-th frame. R represents the coordinates of the corresponding matched feature point in the (k+1)th frame. xoy and t xoy S xoy The rotational and translational transformations of a plane can be expressed by the following formula:

[0116]

[0117] S33: Addressing the pose of a quadruped robot dog in S xoy The pose change on the plane is solved by minimizing the mapping projection error S. xoy Planar pose change (R xoy ,t xoy Solving for the optimal (R) yields the solution. xoy ,t xoySubstitute these coordinates into the original 3D point cloud in the (k+1)th frame to obtain the corrected point cloud coordinates. The corresponding formula is as follows:

[0118]

[0119]

[0120] S4: Map the corrected 3D point cloud to the global reference coordinate system Y-axis, similar to steps S1 to S3, and correct the 3D laser point cloud based on the pose change of the mapped surface; in this embodiment, this specifically includes:

[0121] S41: For any modified point cloud P i (x i ,y i ,z i The point cloud is mapped to S according to the following formula. xoz Planar pixel coordinate system:

[0122]

[0123]

[0124] Among them, P x and P z Representing S respectively xoz Horizontal and vertical resolution in a planar pixel coordinate system S is the mapping point of the i-th point xoz Planar pixel coordinates;

[0125] S42: Map point P i * The feature vector is set as n i The total number of origin clouds along the Y-axis direction of the mapping point, y i t represents the sum of the total y-coordinates of the origin cloud along the Y-axis direction of the mapping point. i To represent the total number of original point cloud intensity values ​​along the Y-axis direction of the coordinate point, a threshold of n is set for the total number of point cloud values ​​at the mapped point. T Hash points and isolated points below the threshold are filtered out.

[0126] S43: For the i-th mapping point P i * Feature information {n i ,y i ,t i}Weighted calculation is performed to obtain A trained model is used to extract two-dimensional mapping feature points. The two-dimensional mapping surface is dynamically divided into grids according to the number of target feature points to achieve uniform distribution of feature points.

[0127] S44: A fast global mapping scale feature vector matching algorithm is used for matching, and the feature points in the next frame are projected onto the previous frame according to the following formula:

[0128]

[0129] In the formula R xoz and t xoz S xoz The rotational and translational transformations of a plane can be expressed by the following formula:

[0130]

[0131] S45: For the pose of a quadruped robot dog in S xoz The pose change on the plane is solved by minimizing the mapping projection error S. xoz Planar pose change (R xoz ,t xoz Solving for the optimal (R) yields the solution. xoz ,t xoz Substitute these coordinates into the original 3D point cloud in the (k+1)th frame to obtain the corrected point cloud coordinates. The corresponding formula is as follows:

[0132]

[0133]

[0134] S5: Finally, the corrected 3D laser point cloud is mapped using the global reference coordinate system X-axis as the reference. Similar to steps S1 to S3, the pose results of the three mapped planes are fused to quickly obtain a relatively accurate pose estimate of the quadruped robot dog. In this embodiment, this specifically includes:

[0135] S51: For any modified point cloud P i (x i ,y i ,z i The three-dimensional point cloud is mapped to S according to the following formula. yoz Planar pixel coordinate system:

[0136]

[0137]

[0138] Among them, P z and P y Representing S respectively yoz Horizontal and vertical resolution in a planar pixel coordinate system S is the mapping point of the i-th pointyoz Planar pixel coordinates;

[0139] S52: Map point P i * The feature vector is set as n i x represents the total number of origin clouds along the X-axis direction of the mapping point. i t represents the sum of the total x-coordinates of the origin cloud along the X-axis direction of the mapping point. i To represent the total number of original point cloud intensity values ​​along the X-axis direction of the coordinate point, a threshold of n is set for the total number of point cloud values ​​at the mapping point. T Hash points and isolated points below the threshold are filtered out.

[0140] S53: For the i-th mapping point P i * Feature information {n i ,x i ,t i}Weighted sum to obtain A trained model is used to extract two-dimensional mapping feature points. The two-dimensional mapping surface is dynamically divided into grids according to the number of target feature points to achieve uniform distribution of feature points.

[0141] S54: A fast global mapping scale feature vector matching algorithm is used for matching, and the feature points in the next frame are projected into the previous frame according to the following formula:

[0142]

[0143] In the formula R yoz and t yoz S yoz The rotational and translational transformations of a plane can be expressed by the following formula:

[0144]

[0145] S55: For the pose of a quadruped robot dog in S yoz The pose change on the plane is solved by minimizing the mapping error S using the following formula. yoz Planar pose change (R yoz ,t yoz ):

[0146]

[0147] By fusing the pose results from the three mapping planes, the robot dog's pose in the global reference coordinate system is obtained through coordinate transformation, thus ultimately yielding a relatively accurate pose estimate for the quadruped robot dog. y ,r p ,r r ,t x ,t y,t z ].

[0148] In this embodiment, a quadruped robot dog localization and pose determination method based on point cloud pixel-level mapping includes a memory and a controller. The memory stores a computer-readable program. When the computer-readable program is called by the controller, it can execute the steps of the quadruped robot dog localization and pose determination method based on point cloud pixel-level mapping as described in this embodiment.

[0149] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for localization and pose determination of a quadruped robot dog based on pixel-level mapping of point clouds, characterized in that: Includes the following steps: S1: Input the 3D laser point cloud data collected by the lidar. The point cloud pixel-level mapping module maps and transforms the collected 3D data with the global reference coordinate system Z-axis as the reference, and removes scattered points and isolated points. S2: Calculate the weighted score of the two-dimensional pixel mapping points, set the score threshold, construct a model to extract two-dimensional mapping feature points, and dynamically divide the two-dimensional mapping surface into a mesh according to the number of target feature points to achieve a uniform distribution of feature points; step S2 specifically includes the following steps: S21: Regarding the first Mapping points Feature information Weighted calculation is performed to obtain Set the score threshold as The formula for calculating its score is as follows: S22: Extract two-dimensional mapping feature points by constructing a model, and denote all feature points in the training data as a set. And construct information entropy As shown in the following formula: in This represents the number of feature points in the training data. Represents the number of non-feature points in the training data; for the mapped points The surrounding neighbors Mapping points Establish two-dimensional mapping feature detection, that is, let With the nearest neighbor Mapping points Compare and obtain state classification As shown in the following formula: in, These are low-probability feature points. These are general probability feature points. For high-probability feature points, the set The mapping points within are divided into three subsets based on their state at their corresponding positions. , and And calculate the classification information entropy according to the following formula. : Meanwhile, the information entropy is iterated to the minimum within the subset, and the constructed model is applied to the rapid detection of two-dimensional mapped feature points; S23: If the target number of feature points extracted is The two-dimensional mapping surface is divided according to the following formula to obtain Two-dimensional mapping sub-blocks: in, These represent the height and width of the target 2D mapping surface, respectively. For grid regions without feature points, the next grid region with feature points is dynamically searched in sequence, and this region is divided into multiple sub-regions of the same size. By performing dynamic grid division, the actual number of feature points obtained is kept as close as possible to the target extraction number. Non-maximum suppression is used to replace clustered feature points with a single point, ensuring that feature points at all positions on the plane are evenly distributed. S3: A fast global mapping scale feature vector matching method is adopted. Based on the matching point pairs, the pose change of the Z-axis mapping surface is obtained by minimizing the mapping projection error, and the pose change of the Z-axis mapping surface is used to correct the 3D laser point cloud; step S3 specifically includes the following steps: S31: First, a fast global mapping scale feature vector matching algorithm is used to find the nearest matching point pair. Using Euclidean distance, defined as follows, and employing asymptotic consistent sampling to eliminate false matches: in , Feature points to be matched and The feature vectors; matching is performed by using the feature vectors of two-dimensional mapping points as descriptor information; S32: Divide the inter-frame pose into six parts and three rotation angles: pitch angle. Yaw angle and roll angle Three translational quantities: displacement in the left and right directions. vertical displacement and displacement in the forward and backward directions The feature points in the next frame are projected onto the previous frame using the following formula: in Representing the The first frame in the mapped coordinate system The coordinates of the feature points Indicates the first The coordinates of the corresponding matching feature points in the frame, where and They are respectively The rotation and translation of a plane are expressed by the following formula: S33: Regarding the pose of a quadruped robot dog The pose change on the plane is solved by minimizing the mapping projection error. Planar pose changes The optimal solution is obtained. Substitute it into the first The original 3D point cloud is corrected by frame correction to obtain the corrected point cloud coordinates. The corresponding formula is as follows: S4: Map the corrected 3D point cloud using the global reference coordinate system Y-axis as a reference, and correct the 3D laser point cloud according to the pose change of the Y-axis mapping surface; step S4 specifically includes the following steps: S41: For any modified point cloud Map the point cloud to the following formula: Planar pixel coordinate system: in, and Represent Horizontal and vertical resolution in a planar pixel coordinate system For the first Mapping points Planar pixel coordinates; S42: Mapping point The feature vector is set as , This represents the total number of origin clouds along the Y-axis direction of the mapping point. The origin cloud coordinates along the Y-axis direction of the mapping point The sum of the total values, To determine the total number of original point cloud intensity values ​​along the Y-axis direction of the coordinate point, a threshold for the total number of point clouds at the mapped point is set. Hash points and isolated points that are less than the threshold are filtered out. S43: Regarding the first Mapping points Feature information Weighted calculation is performed to obtain The training model is used to extract two-dimensional mapping feature points, and the two-dimensional mapping surface is dynamically divided into grids according to the number of target feature points to achieve uniform distribution of feature points; S44: A fast global mapping scale feature vector matching algorithm is used for matching, and the feature points in the next frame are projected onto the previous frame according to the following formula: In the formula and They are respectively The rotation and translation of a plane are expressed by the following formula: S45: Addressing the pose of a quadruped robot dog in... The pose change on the plane is solved by minimizing the mapping projection error. Planar pose changes The optimal solution is obtained. Substitute it into the first The original 3D point cloud is corrected by frame correction to obtain the corrected point cloud coordinates. The corresponding formula is as follows: S5: Finally, the corrected 3D laser point cloud is mapped and corrected with the global reference coordinate system X-axis as the reference, and the pose results of the three mapping planes are fused to obtain the pose estimate of the quadruped robot dog. Step S5 specifically includes the following steps: S51: For any point cloud corrected by S4 The 3D point cloud is mapped to the following formula: Planar pixel coordinate system: in, and Represent Horizontal and vertical resolution in a planar pixel coordinate system For the first Mapping points Planar pixel coordinates; S52: Mapping point The feature vector is set as , This represents the total number of origin clouds along the X-axis direction of the mapping point. The origin cloud coordinates along the X-axis direction of the mapping point The sum of the total values, To determine the total number of original point cloud intensity values ​​along the X-axis direction of the coordinate points, a threshold for the total number of point cloud values ​​at the mapped points is set. Hash points and isolated points that are less than the threshold are filtered out. S53: Regarding the first Mapping points Feature information Weighted average The training model is used to extract two-dimensional mapping feature points, and the two-dimensional mapping surface is dynamically divided into grids according to the number of target feature points to achieve uniform distribution of feature points; S54: A fast global mapping scale feature vector matching algorithm is used for matching, and the feature points in the next frame are projected into the previous frame according to the following formula: In the formula and They are respectively The rotation and translation of a plane are expressed by the following formula: S55: Addressing the pose of quadrupedal robotic dogs The pose change on the plane is solved by minimizing the mapping error using the following formula. Planar pose changes : By fusing the pose results from the three mapping planes, the robot dog's pose in the global reference coordinate system is obtained through coordinate transformation, thus ultimately yielding a relatively accurate pose estimate for the quadruped robot dog. .

2. The method for localization and pose determination of a quadruped robot dog based on point cloud pixel-level mapping according to claim 1, characterized in that: Step S1 specifically includes the following steps: S11: Establish a three-dimensional coordinate system using the lidar mounted on the quadruped robot dog as the coordinate center, and construct a global reference coordinate system, setting the point cloud. Let be any point cloud in three-dimensional space, and its three-dimensional coordinates be... ; S12: Construct pixel-level mapping planes for the point cloud. Using the global reference coordinate system as a reference, construct pixel coordinate systems for the mapping planes along the three axes respectively. , and As shown in the following formula: in and , and , and These are respectively in 3D point clouds , , Maximum and minimum values ​​of coordinates; S13: For any point cloud The 3D point cloud is mapped and transformed according to the following formula. Planar pixel coordinate system: in, and Represent Horizontal and vertical resolution in a planar pixel coordinate system For the first Mapping points Planar pixel coordinates; and mapping points The feature vector is set as ;in, For along the mapping point The total number of origin clouds along the axial direction. For along the mapping point Origin cloud coordinates in the axial direction The sum of the total values, Along the coordinate point The total number of point cloud intensity values ​​along the axis direction, setting a threshold for the total number of point clouds at the mapped points. Hash points and isolated points below the threshold are filtered out.

Images