Positioning mapping method, system and device based on legged robot and storage medium

Abstract

The application belongs to the technical field of inspection robots, and specifically discloses a positioning mapping method, system and device based on a foot-type robot and a storage medium, the method comprising the following steps: collecting inertial measurement data, motion data, point cloud data and image data through an IMU, a motor encoder, a laser radar and a camera of the foot-type robot; performing filtering processing on the inertial measurement data through a Kalman filtering algorithm; acquiring a first odometer, a second odometer and a third odometer based on the inertial measurement data, the motion data and the image data; acquiring a fourth odometer in a laser radar coordinate system based on the point cloud data by eliminating moving point cloud blocks and performing point cloud registration; acquiring a fifth odometer as positioning information through multi-odometer joint optimization based on the above odometers; and reorganizing key frame point clouds to acquire a point cloud map without the influence of dynamic objects. Through the application, the positioning accuracy and the mapping quality can be improved simultaneously.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of inspection robots, and more particularly to a positioning and mapping method, system, device and storage medium based on a foot-type robot. BACKGROUND

[0002] When a foot-type robot performs inspection work in an unknown scene, it needs to obtain accurate self-positioning and an environment map. Simultaneous localization and mapping (SLAM) based on a laser radar and an inertial measurement unit (IMU) is a common solution. In this method, the positioning and mapping accuracy depends on the data reliability of the laser radar and the IMU. However, when an IMU equipped with a laser radar is used, the IMU only contains six-axis information. If the laser radar is not installed horizontally or the laser radar is not placed close to the robot's center of mass, the data of the IMU is unreliable, resulting in low positioning accuracy. At the same time, dynamic objects are prone to appear in the mapping process, causing "ghosting" phenomenon in the final map, which adversely affects the display effect of the map and the subsequent development of navigation functions. How to improve the positioning accuracy and mapping effect of the foot-type robot is a technical problem to be solved in the field. SUMMARY

[0003] In view of the defects of the prior art, the purpose of the present application is to improve the positioning accuracy and mapping effect of the foot-type robot.

[0004] To achieve the above purpose, in a first aspect, the present application provides a positioning and mapping method based on a foot-type robot, comprising:

[0005] Inertial measurement data, motion data, point cloud data and image data are respectively collected by the IMU, motor encoder, laser radar and camera of the foot-type robot;

[0006] The inertial measurement data is filtered by a Kalman filtering algorithm;

[0007] Based on the motion data, image data and filtered inertial measurement data, the foot-type robot pose is estimated to obtain a first odometry in the IMU system, a second odometry in the base coordinate system and a third odometry in the camera coordinate system;

[0008] Based on the point cloud data, the fourth odometry in the laser radar coordinate system is obtained by removing the moving point cloud block and performing point cloud registration;

[0009] Based on the first odometry, the second odometry, the third odometry and the fourth odometry, the fifth odometry is obtained as positioning information by multi-odometry joint optimization;

[0010] Based on the fifth odometry, the key frame point cloud in the point cloud data after removing the moving point cloud block is reorganized to obtain a point cloud map without the influence of dynamic objects.

[0011] In a possible implementation, the observation noise parameter of the Kalman filtering algorithm is determined by the following steps:

[0012] determining whether a foot switches from a swing phase to a support phase at the current time;

[0013] if yes, determining a target vibration direction at the current time, and using a first observation noise parameter for the inertial measurement data in the target vibration direction, and if no, using a second observation noise parameter for the inertial measurement data in each direction at the current time, the first observation noise parameter being greater than the second observation noise parameter.

[0014] In a possible implementation, based on the motion data, the image data and the filtered inertial measurement data, pose estimation of the legged robot is performed to obtain a first odometry in an IMU system, a second odometry in a base coordinate system and a third odometry in a camera coordinate system, including:

[0015] based on the filtered inertial measurement data, performing pose estimation of the legged robot by integration to obtain the first odometry in the IMU system;

[0016] based on the motion data, performing pose estimation of the legged robot by integration to obtain the second odometry in the base coordinate system;

[0017] based on the image data, obtaining the third odometry in the camera coordinate system and a dynamic object candidate list by extracting image feature points and analyzing by an optical flow method, the dynamic object candidate list being used to indicate candidate dynamic objects and coordinates of the candidate dynamic objects in the camera coordinate system.

[0018] In a possible implementation, based on the point cloud data, the fourth odometry in a laser radar coordinate system is obtained by removing moving point cloud blocks and performing point cloud registration, including:

[0019] based on the filtered inertial measurement data, removing distortion from the point cloud data;

[0020] based on the dynamic object candidate list and the extrinsic calibration of the camera and the laser radar, searching for point cloud blocks corresponding to each candidate dynamic object in the point cloud data after distortion removal;

[0021] based on the speed of each candidate dynamic object corresponding point cloud block and the speed of the overall point cloud, searching for a moving point cloud block in the point cloud blocks corresponding to all candidate dynamic objects, the difference between the speed of the moving point cloud block and the speed of the overall point cloud being greater than a preset speed difference threshold;

[0022] removing the moving point cloud block from the point cloud data after distortion removal to obtain point cloud data after removing the moving point cloud block;

[0023] Based on the first odometer and the second odometer, a first odometer increment is obtained by weighted average;

[0024] Based on the first odometer increment and the point cloud data after removing the motion point cloud block, a fourth odometer in the laser radar coordinate system is obtained by a nearest neighbor matching method.

[0025] In a possible implementation, the multi-odometer joint optimization includes:

[0026] Based on the first odometer, the second odometer, the third odometer and the fourth odometer, a factor graph is constructed, and the factor graph is optimized and solved to minimize the negative logarithm of the joint probability of all factors in the factor graph, so as to obtain an optimal estimation of the robot pose as a fifth odometer;

[0027] Wherein, the robot pose at all key frame moments is taken as a vertex in the factor graph, the fourth odometer is taken as an initial estimation value and a prior factor, the first odometer, the second odometer and the third odometer between two adjacent key frame moments are taken as interpolation factors by using weighted average, and the key frame is obtained by frame screening on the point cloud data after removing the motion point cloud block.

[0028] In a possible implementation, based on the fifth odometer, the key frame point cloud in the point cloud data after removing the motion point cloud block is reorganized to obtain a point cloud map without dynamic object influence, including:

[0029] Based on the pose provided by the fifth odometer, the aligned key frame point cloud is obtained by key frame point cloud rigid transformation processing;

[0030] Based on the aligned key frame point cloud, the point cloud map without dynamic object influence is obtained by global splicing and deduplication.

[0031] In a possible implementation, a damping device is arranged at the bottom of the mounting plane of the laser radar, and the damping device is constructed based on a spring and a damper.

[0032] In a second aspect, the application provides a positioning and mapping system based on a legged robot, including:

[0033] A data acquisition module is configured to acquire inertial measurement data, motion data, point cloud data and image data through the IMU, motor encoder, laser radar and camera of the legged robot.

[0034] A Kalman filtering module is configured to filter the inertial measurement data by using a Kalman filtering algorithm.

[0035] The odometry acquisition module is used to perform pose estimation of a legged robot based on motion data, image data, and filtered inertial measurement data, and to acquire the first odometry in the IMU system, the second odometry in the base coordinate system, and the third odometry in the camera coordinate system.

[0036] The odometer acquisition module is used to acquire a fourth odometer in the lidar coordinate system based on point cloud data by removing moving point cloud blocks and performing point cloud registration.

[0037] The joint optimization module is used to obtain the fifth odometer as positioning information by jointly optimizing the first, second, third, and fourth odometers.

[0038] The mapping module is used to reconstruct keyframe point clouds from point cloud data after removing moving point cloud blocks, based on the fifth odometer, to obtain a point cloud map without the influence of dynamic objects.

[0039] Thirdly, this application provides an electronic device, including: a memory and one or more processors; the memory is coupled to one or more processors, the memory is used to store computer program code, the computer program code including computer instructions; one or more processors invoke the computer instructions to cause the electronic device to perform the method described in the first aspect or any possible implementation of the first aspect.

[0040] Fourthly, this application provides a computer-readable storage medium including instructions that, when executed on an electronic device, cause the electronic device to perform the method described in the first aspect or any possible implementation thereof.

[0041] Overall, the technical solutions conceived in this application have the following beneficial effects compared with the prior art:

[0042] First, inertial, motion, point cloud, and image data are acquired using an IMU, motor encoder, LiDAR, and camera, respectively. Kalman filtering is used to reduce noise in the inertial measurement data (raw IMU data), improving its reliability. Second, based on motion data, image data, and the filtered inertial measurement data, the first to third odometry values ​​are calculated in the IMU, base, and camera coordinate systems. Simultaneously, motion interference is removed and registered using point cloud data (effectively filtering out dynamic object interference), resulting in a fourth odometry value in the LiDAR system, forming redundant positioning information across multiple coordinate systems. Finally, a multi-odometry joint optimization algorithm (such as factor graph optimization) is used to impose global consistency constraints on the four types of odometry, eliminating the accumulation of errors from a single sensor and outputting a high-precision fifth odometry value as the positioning result. Based on this optimized pose, the keyframe point cloud is reconstructed, ultimately building a high-precision, highly consistent static environment point cloud map. This method achieves a synergistic improvement in positioning accuracy and mapping quality by filtering the raw IMU data, removing dynamic object interference from the point cloud data, and performing multi-odometry joint optimization. Attached Figure Description

[0043] Figure 1 This is a schematic flowchart of the localization and mapping method based on a legged robot provided in an embodiment of this application;

[0044] Figure 2 This is a schematic diagram of the localization and mapping system based on a legged robot provided in an embodiment of this application;

[0045] Figure 3 This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0046] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0047] The terms "first" and "second," etc., used in the specification and claims of this application are used to distinguish different objects, not to describe a specific order of objects. For example, "first odometer" and "second odometer," etc., are used to distinguish different odometers, not to describe a specific order of odometers.

[0048] In the embodiments of this application, the terms "exemplary" or "for example" are used to indicate that something is an example, illustration, or description. Any embodiment or design that is described as "exemplary" or "for example" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or design. Specifically, the use of the terms "exemplary" or "for example" is intended to present the relevant concepts in a specific manner.

[0049] In the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more, for example, multiple processing units means two or more processing units, multiple elements means two or more elements, etc.

[0050] The embodiments of this application are described below with reference to the accompanying drawings.

[0051] Figure 1 This is a flowchart illustrating the localization and mapping method based on a legged robot provided in an embodiment of this application, as shown below. Figure 1 As shown, the method includes the following steps S101 to S106.

[0052] Step S101: Inertial measurement data, motion data, point cloud data, and image data are collected by the legged robot's IMU, motor encoder, lidar, and camera, respectively.

[0053] Step S102: The inertial measurement data (raw IMU data) is filtered using a Kalman filter algorithm; specifically, the Kalman filter algorithm can be an Iterated Extended Kalman Filter (IEKF).

[0054] Step S103: Based on motion data, image data and filtered inertial measurement data, perform pose estimation of the legged robot to obtain the first odometry in the IMU system, the second odometry in the base coordinate system (BASE system) and the third odometry in the camera coordinate system (CAMERA system).

[0055] Step S104: Based on point cloud data, by removing moving point cloud blocks and performing point cloud registration, a fourth odometry in the LiDAR coordinate system is obtained.

[0056] Step S105: Based on the first odometer, second odometer, third odometer, and fourth odometer, the fifth odometer is obtained as positioning information through multi-odometer joint optimization;

[0057] Step S106: Based on the fifth odometry, the keyframe point cloud in the point cloud data after removing moving point cloud blocks is reconstructed to obtain a point cloud map without the influence of dynamic objects.

[0058] Specifically, firstly, inertial, motion, point cloud, and image data are collected using an IMU, motor encoder, LiDAR, and camera, respectively. Kalman filtering is used to denoise the inertial measurement data (raw IMU data), improving its reliability. Secondly, based on motion data, image data, and filtered inertial measurement data, the first to third odometry values ​​in the IMU, base, and camera coordinate systems are calculated. Simultaneously, motion interference is removed from the point cloud data and registered (effectively filtering out dynamic object interference) to obtain the fourth odometry value in the LiDAR system, forming redundant positioning information across multiple coordinate systems. Finally, a multi-odometry joint optimization algorithm (such as factor graph optimization) is used to impose global consistency constraints on the four types of odometry, eliminating the accumulation of errors from a single sensor and outputting a high-precision fifth odometry value as the positioning result. Based on this optimized pose, the keyframe point cloud is reconstructed, ultimately building a high-precision, highly consistent static environment point cloud map. This method achieves a synergistic improvement in positioning accuracy and mapping quality by filtering the raw IMU data, removing dynamic object interference from the point cloud data, and performing multi-odometry joint optimization.

[0059] In one possible implementation, the observation noise parameters of the Kalman filter algorithm are determined through the following steps:

[0060] Determine whether a foot has switched from the swing phase to the support phase at the current moment;

[0061] If so, the target vibration direction (i.e., the main vibration direction) is determined at the current moment, and the first observation noise parameter is applied to the inertial measurement data (IMU raw data) in the target vibration direction. Otherwise, the second observation noise parameter is applied to the inertial measurement data in each direction at the current moment, and the first observation noise parameter is greater than the second observation noise parameter.

[0062] Specifically, the system determines in real time whether the foot is transitioning from the swing phase to the support phase. When foot contact is detected, the system identifies the target vibration direction (i.e., the primary vibration direction, which can be experimentally determined) and applies a larger first observation noise parameter in that direction to tolerate the instantaneous high-frequency vibration caused by foot impact. In the absence of foot contact or in non-target vibration directions, a smaller second observation noise parameter is used to ensure effective filtering of conventional measurement noise. This adaptive noise configuration mechanism based on foot state allows Kalman filtering to dynamically balance response sensitivity and noise suppression strength during robot movement, ultimately outputting optimized inertial measurement data and improving the reliability of the inertial measurement data.

[0063] Here, we will explain the optional implementation of the above-mentioned determination of whether a foot has switched from the swing phase (the foot is in a swinging state) to the support phase (the foot is in a support state) at the current moment. The posture of each joint can be determined based on the motion data of the motor encoder, and then the current gait of the legged robot can be determined based on the posture of each joint. Based on the current gait, it can be determined whether a foot of the legged robot has switched from the swing phase to the support phase at the current moment.

[0064] In one possible implementation, the above-mentioned legged robot pose estimation based on motion data, image data, and filtered inertial measurement data is performed to obtain a first odometry in the IMU frame, a second odometry in the base coordinate system (BASE frame), and a third odometry in the camera coordinate system (CAMERA frame), including:

[0065] Based on the filtered inertial measurement data, the pose of the legged robot is estimated by integration, and the first odometry in the IMU system is obtained.

[0066] Based on motion data, the pose of the legged robot is estimated by integration to obtain a second odometry in the base coordinate system (BASE system);

[0067] Based on image data, by extracting image feature points and performing optical flow analysis, a third odometry and a candidate list of moving objects are obtained in the camera coordinate system (CAMERA system). The candidate list of moving objects is used to indicate the candidate moving objects and their coordinates in the camera coordinate system (CAMERA system).

[0068] The first odometry is obtained by deriving pose estimation from integrated inertial measurement data in the IMU coordinate system: the IMU data (containing angular velocity and acceleration information) after Kalman filtering is integrated over time to calculate the robot's attitude (such as quaternions or Euler angles) and displacement changes; then, based on the inherent coordinate system characteristics of the IMU, the relative pose estimation in the IMU system is output to form the first odometry.

[0069] The second odometry is obtained by deriving pose from integrated motion data in the base coordinate system (BASE system): based on the joint angle and velocity data collected by the motor encoder, the motion state of the robot base is calculated through a kinematic model (such as forward kinematics or foot contact state estimation); then, by integrating the base motion velocity or displacement over time, the pose change in the BASE system is obtained, thus forming the second odometry.

[0070] The third odometry is obtained in the camera coordinate system (CAMERA system) through image feature point extraction and optical flow analysis: feature points (such as corner points or descriptors) are extracted from consecutive image frames, and the motion trajectory of feature points between adjacent frames is tracked using optical flow; then, the rotation and translation motion of the camera itself are inferred based on the pixel displacement of the feature points to generate a relative pose estimate in the CAMERA system, forming the third odometry. Simultaneously, by analyzing the motion characteristics of the feature points, a dynamic object candidate list is generated for dynamic interference removal in subsequent point cloud map construction.

[0071] In one possible implementation, the above-mentioned acquisition of a fourth odometry in the LiDAR coordinate system, based on point cloud data, involves removing moving point cloud blocks and performing point cloud registration, including:

[0072] Based on the filtered inertial measurement data, distortion correction is performed on the point cloud data;

[0073] Based on the candidate list of dynamic objects and the extrinsic parameter calibration of the camera and LiDAR, the point cloud blocks corresponding to each candidate dynamic object are searched in the distortion-free point cloud data.

[0074] Based on the velocity of the point cloud block corresponding to each candidate dynamic object and the velocity of the overall point cloud, a moving point cloud block is searched among the point cloud blocks corresponding to all candidate dynamic objects. The difference between the velocity of the moving point cloud block and the velocity of the overall point cloud is greater than a preset velocity difference threshold.

[0075] Remove the moving point cloud blocks from the distortion-free point cloud data to obtain the point cloud data after removing the moving point cloud blocks;

[0076] Based on the first and second odometers, the increment of the first odometer is obtained by weighted averaging.

[0077] Based on the first odometry increment and the point cloud data after removing moving point cloud blocks, point cloud registration is performed using the nearest neighbor matching method to obtain the fourth odometry in the LiDAR coordinate system.

[0078] Specifically, by combining the candidate list of dynamic objects provided by the camera and the extrinsic parameter calibration of the camera and LiDAR, the point cloud blocks corresponding to each candidate dynamic object are located in the distortion-free point cloud. By analyzing the velocity difference between these point cloud blocks and the overall point cloud, moving point cloud blocks with velocity differences exceeding a preset threshold are identified and removed, thereby eliminating interference from dynamic objects. Then, based on the weighted average results of the first and second odometry, an initial pose increment is generated as prior information for point cloud registration. The nearest neighbor matching method (such as the ICP algorithm) is used to register the static point cloud after removing the dynamic point cloud, and the relative pose change in the LiDAR system is calculated to form the fourth odometry.

[0079] In one possible implementation, the above-mentioned multi-odometer joint optimization includes:

[0080] Based on the first, second, third, and fourth odometry, a factor graph is constructed, and the factor graph is optimized to minimize the negative logarithm of the joint probability of all factors in the factor graph, so as to obtain the optimal estimate of the robot pose as the fifth odometry.

[0081] In this context, the robot pose at all key frame moments is used as the vertex in the factor graph, the fourth odometry is used as the initial estimate and prior factor, and the first, second and third odometry between two adjacent key frame moments are used as the weighted average as the interpolation factor. The key frames are obtained by filtering the point cloud data after removing motion point cloud blocks frame by frame.

[0082] In one possible implementation, the above-mentioned method, based on fifth-order odometry, reconstructs keyframe point clouds from point cloud data after removing moving point cloud blocks to obtain a point cloud map unaffected by dynamic objects, including:

[0083] Based on the pose provided by the fifth odometry, the aligned keyframe point cloud is obtained through rigid transformation of the keyframe point cloud.

[0084] Based on aligned keyframe point clouds, a point cloud map without the influence of dynamic objects is obtained through global stitching and deduplication.

[0085] In one possible implementation, the bottom of the lidar mounting plane is equipped with a shock-absorbing device, which is constructed based on springs and dampers.

[0086] Specifically, the vibration frequency of the legged robot during walking is measured, and the spring stiffness and damping coefficient are selected based on the vibration frequency to ensure the stability of the IMU data.

[0087] The following comprehensive example illustrates the localization and mapping method based on a legged robot provided in this application.

[0088] This comprehensive example includes both hardware and software optimizations.

[0089] Hardware optimization: A shock absorption device is added to the bottom of the lidar mounting plane. The shock absorption device includes a spring and a damper. The vibration frequency of the legged robot is measured when it walks. The spring stiffness and damping coefficient are selected according to the vibration frequency to ensure the stability of the IMU data.

[0090] Software optimization includes: (1) data acquisition, (2) data preprocessing, (3) individual odometer calculation, (4) multi-odometer joint optimization, and (5) construction of point cloud map.

[0091] (1) Data acquisition;

[0092] The system utilizes an IMU to collect inertial measurement data of the legged robot, a motor encoder to collect motion data of the legged robot, a lidar to collect point cloud data, and a camera to collect image data.

[0093] (2) Data preprocessing;

[0094] The acquired IMU data is smoothed using the Kalman filter algorithm, and dynamic noise parameters (mainly involving the observation noise (R) parameter) are set according to the gait of the legged robot. When the swing phase switches to the support phase, and when the swing phase is in different postures, a larger noise parameter (specifically the observation noise (R) parameter of the Kalman filter algorithm) is used to smooth the raw IMU data in the main vibration direction.

[0095] (3) Calculation of separate odometer

[0096] Pre-integrate the filtered IMU data to obtain the first odometry estimate of the robot pose (position and attitude) in the IMU system.

[0097] The obtained motion data from the motor encoder is pre-integrated to obtain a second odometry for robot pose estimation in the BASE system (base coordinate system).

[0098] Deep learning is used to extract image feature points, and the third odometry for robot pose estimation in the CAMERA system (camera coordinate system) is obtained based on optical flow method, and a preliminary list of dynamic object candidates is identified.

[0099] Optionally, the deep learning model used above is a lightweight convolutional neural network, which can ensure the robustness of the system in scenarios with insufficient features while also taking into account the real-time performance of the system.

[0100] The filtered IMU data is used to remove motion distortion from the obtained point cloud data. Based on the initially identified candidate list of dynamic objects, point cloud blocks corresponding to the candidate list are obtained according to the extrinsic parameter calibration of the camera and LiDAR. The velocity of the point cloud blocks is estimated. If the velocity of the point cloud block differs significantly from that of the overall point cloud, the point cloud block is removed. Based on the first and second odometry, the first odometry increment is obtained by weighted averaging. Based on the first odometry increment and the point cloud data after the above removal process, point cloud registration is performed by ICP (nearest neighbor matching) to obtain the fourth odometry for robot pose estimation in the LiDAR coordinate system.

[0101] Based on the distance, direction, and time information of each frame of point cloud data after the removal process, it is determined whether each frame is a key frame. From the start position to the end, they are defined as the first key frame, the second key frame, the third key frame, etc., and the corresponding time is defined as the first key frame time, the second key frame time, the third key frame time, etc.

[0102] (4) Joint optimization of multiple odometers;

[0103] A factor graph is constructed based on the first, second, third, and fourth odometry measurements. The Levenberg-Marquardt (LM) method is used to optimize the constructed factor graph, aiming to minimize the negative logarithm of the joint probability of all factors in the factor graph, thus obtaining the optimal estimate of the robot pose (fifth odometry). The fifth odometry serves as accurate positioning information.

[0104] The robot pose at all keyframe moments is used as a vertex in the factor graph. The fourth odometry is used as the initial estimate (Value) and prior factor. The weighted average of the first, second, and third odometry values ​​between two keyframe moments is used as the interpolation factor (BetWeenFactor). The weighting parameters of each odometry are set according to the gait state (the odometry weighting parameters used under different gait states can be determined experimentally).

[0105] (5) Construct a point cloud map;

[0106] The keyframe point clouds are jointly reconstructed based on the fifth odometry, ultimately yielding accurate positioning information and a point cloud map (or environment map) free from the influence of dynamic objects. Specifically, based on the pose provided by the fifth odometry, aligned keyframe point clouds are obtained through rigid transformation of the keyframe point clouds; based on the aligned keyframe point clouds, the environment map is obtained through global stitching and deduplication.

[0107] Understandably, the above data preprocessing (processing the collected IMU data using the Kalman filter algorithm) reduces the impact of IMU data fluctuations during the legged robot's stepping; by adding dynamic object detection and removal operations, problems such as "ghosting" in the final mapping can be solved, providing a reliable guarantee for the development of subsequent functions such as inspection; by using multi-sensor data fusion, the positioning accuracy of the system in different scenarios can be effectively improved.

[0108] The localization and mapping system based on a legged robot provided in this application is described below. The localization and mapping system based on a legged robot described below can be referred to in correspondence with the localization and mapping method based on a legged robot described above.

[0109] Figure 2This is a schematic diagram of the localization and mapping system based on a legged robot provided in an embodiment of this application, as shown below. Figure 2 As shown, the system includes: a data acquisition module 10, a Kalman filter module 20, an odometer acquisition module 30, a joint optimization module 40, and a mapping module 50. Among them:

[0110] The data acquisition module 10 is used to acquire inertial measurement data, motion data, point cloud data and image data respectively through the IMU, motor encoder, lidar and camera of the legged robot;

[0111] Kalman filter module 20 is used to filter inertial measurement data using the Kalman filter algorithm;

[0112] The odometry acquisition module 30 is used to perform pose estimation of the legged robot based on motion data, image data and filtered inertial measurement data, and to acquire the first odometry in the IMU system, the second odometry in the base coordinate system and the third odometry in the camera coordinate system.

[0113] The odometer acquisition module 30 is used to acquire a fourth odometer in the lidar coordinate system based on point cloud data by removing moving point cloud blocks and performing point cloud registration.

[0114] The joint optimization module 40 is used to obtain the fifth odometer as positioning information by jointly optimizing the first, second, third, and fourth odometers.

[0115] The mapping module 50 is used to reconstruct keyframe point clouds in point cloud data after removing moving point cloud blocks based on the fifth odometer, so as to obtain a point cloud map without the influence of dynamic objects.

[0116] It is understood that the detailed functional implementation of each of the above units / modules can be found in the description in the aforementioned method embodiments, and will not be repeated here.

[0117] It should be understood that the above system is used to execute the methods in the above embodiments. The corresponding program modules in the system are similar in implementation principle and technical effect to those described in the above methods. The working process of the system can be referred to the corresponding process in the above methods, and will not be repeated here.

[0118] Based on the methods in the above embodiments, this application provides an electronic device. Figure 3 This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application, such as... Figure 3As shown, the electronic device may include a processor 810, a communications interface 820, a memory 830, and a communication bus 840, wherein the processor 810, the communications interface 820, and the memory 830 communicate with each other through the communication bus 840. The processor 810 can call logical instructions in the memory 830 to execute the methods in the above embodiments.

[0119] Furthermore, the logical instructions in the aforementioned memory 830 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application.

[0120] Based on the methods in the above embodiments, this application provides a computer-readable storage medium storing a computer program that, when run on a processor, causes the processor to execute the methods in the above embodiments.

[0121] Based on the methods in the above embodiments, this application provides a computer program product that, when run on a processor, causes the processor to execute the methods in the above embodiments.

[0122] It is understood that the processor in the embodiments of this application can be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. A general-purpose processor can be a microprocessor or any conventional processor.

[0123] The method steps in this application embodiment can be implemented in hardware or by a processor executing software instructions. The software instructions can consist of corresponding software modules, which can be stored in random access memory (RAM), flash memory, read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), registers, hard disks, portable hard disks, CD-ROMs, or any other form of storage medium known in the art. An exemplary storage medium is coupled to the processor, enabling the processor to read information from and write information to the storage medium. Of course, the storage medium can also be a component of the processor. The processor and the storage medium can reside in an ASIC.

[0124] In the above embodiments, implementation can be achieved entirely or partially through software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented entirely or partially as a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of this application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted through the computer-readable storage medium. The computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, fiber optic, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid-state disk (SSD)).

[0125] It is understood that the various numerical designations used in the embodiments of this application are merely for the convenience of description and are not intended to limit the scope of the embodiments of this application.

[0126] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. A localization and mapping method based on a legged robot, characterized in that, include: The legged robot uses its IMU, motor encoder, lidar, and camera to collect inertial measurement data, motion data, point cloud data, and image data, respectively. The inertial measurement data is filtered using the Kalman filter algorithm. Based on motion data, image data, and filtered inertial measurement data, pose estimation of a legged robot is performed to obtain the first odometry in the IMU system, the second odometry in the base coordinate system, and the third odometry in the camera coordinate system. Based on point cloud data, a fourth odometry measurement in the lidar coordinate system is obtained by removing moving point cloud blocks and performing point cloud registration. Based on the first, second, third, and fourth odometers, the fifth odometer is obtained as positioning information through multi-odometer joint optimization; Based on the fifth odometry, the keyframe point cloud in the point cloud data after removing moving point cloud blocks is reconstructed to obtain a point cloud map without the influence of dynamic objects. The observation noise parameters of the Kalman filter algorithm are determined through the following steps: Determine whether a foot has switched from the swing phase to the support phase at the current moment; If so, the target vibration direction is determined at the current moment, and the first observation noise parameter is applied to the inertial measurement data in the target vibration direction. Otherwise, the second observation noise parameter is applied to the inertial measurement data in each direction at the current moment, and the first observation noise parameter is greater than the second observation noise parameter.

2. The localization and mapping method based on a legged robot according to claim 1, characterized in that, The process of estimating the pose of a legged robot based on motion data, image data, and filtered inertial measurement data, and acquiring a first odometry in the IMU system, a second odometry in the base coordinate system, and a third odometry in the camera coordinate system, includes: Based on the filtered inertial measurement data, the pose of the legged robot is estimated by integration, and the first odometry in the IMU system is obtained. Based on motion data, the pose of the legged robot is estimated by integration to obtain a second odometry in the base coordinate system. Based on image data, by extracting image feature points and performing optical flow analysis, a third odometry and a candidate list of dynamic objects in the camera coordinate system are obtained. The candidate list of dynamic objects is used to indicate the candidate dynamic objects and their coordinates in the camera coordinate system.

3. The localization and mapping method based on a legged robot according to claim 2, characterized in that, The process of obtaining a fourth odometry measurement in the lidar coordinate system based on point cloud data, by removing moving point cloud blocks and performing point cloud registration, includes: Based on the filtered inertial measurement data, distortion correction is performed on the point cloud data; Based on the candidate list of dynamic objects and the extrinsic parameter calibration of the camera and LiDAR, the point cloud blocks corresponding to each candidate dynamic object are searched in the distortion-free point cloud data. Based on the velocity of the point cloud block corresponding to each candidate dynamic object and the velocity of the overall point cloud, a moving point cloud block is searched among the point cloud blocks corresponding to all candidate dynamic objects. The difference between the velocity of the moving point cloud block and the velocity of the overall point cloud is greater than a preset velocity difference threshold. Remove the moving point cloud blocks from the distortion-free point cloud data to obtain the point cloud data after removing the moving point cloud blocks; Based on the first and second odometers, the increment of the first odometer is obtained by weighted averaging. Based on the first odometry increment and the point cloud data after removing moving point cloud blocks, the point cloud is registered using the nearest neighbor matching method to obtain the fourth odometry in the lidar coordinate system.

4. The localization and mapping method based on a legged robot according to claim 1, characterized in that, The multi-odometer joint optimization includes: Based on the first, second, third, and fourth odometry, a factor graph is constructed, and the factor graph is optimized to minimize the negative logarithm of the joint probability of all factors in the factor graph, so as to obtain the optimal estimate of the robot pose as the fifth odometry. In this context, the robot pose at all key frame moments is used as the vertex in the factor graph, the fourth odometry is used as the initial estimate and prior factor, and the first, second and third odometry between two adjacent key frame moments are used as the weighted average as the interpolation factor. The key frames are obtained by filtering the point cloud data after removing motion point cloud blocks frame by frame.

5. The localization and mapping method based on a legged robot according to claim 1, characterized in that, The process of reconstructing keyframe point clouds from point cloud data after removing moving point cloud blocks, based on the fifth odometry, to obtain a point cloud map unaffected by dynamic objects includes: Based on the pose provided by the fifth odometry, the aligned keyframe point cloud is obtained through rigid transformation of the keyframe point cloud. Based on aligned keyframe point clouds, a point cloud map without the influence of dynamic objects is obtained through global stitching and deduplication.

6. The localization and mapping method based on a legged robot according to any one of claims 1-5, characterized in that, The bottom of the lidar mounting surface is equipped with a shock-absorbing device, which is based on springs and dampers.

7. A localization and mapping system based on a legged robot, characterized in that, include: The data acquisition module is used to collect inertial measurement data, motion data, point cloud data, and image data through the IMU, motor encoder, lidar, and camera of the legged robot, respectively. The Kalman filter module is used to filter inertial measurement data using the Kalman filter algorithm. The odometry acquisition module is used to perform pose estimation of a legged robot based on motion data, image data, and filtered inertial measurement data, and to acquire the first odometry in the IMU system, the second odometry in the base coordinate system, and the third odometry in the camera coordinate system. The odometer acquisition module is used to acquire a fourth odometer in the lidar coordinate system based on point cloud data by removing moving point cloud blocks and performing point cloud registration. The joint optimization module is used to obtain the fifth odometer as positioning information by jointly optimizing the first, second, third, and fourth odometers. The mapping module is used to reconstruct keyframe point clouds in point cloud data after removing moving point cloud blocks based on the fifth odometry, so as to obtain a point cloud map without the influence of dynamic objects. The observation noise parameters of the Kalman filter algorithm are determined through the following steps: Determine whether a foot has switched from the swing phase to the support phase at the current moment; If so, the target vibration direction is determined at the current moment, and the first observation noise parameter is applied to the inertial measurement data in the target vibration direction. Otherwise, the second observation noise parameter is applied to the inertial measurement data in each direction at the current moment, and the first observation noise parameter is greater than the second observation noise parameter.

8. An electronic device, characterized in that, include: Memory and one or more processors; The memory is coupled to the one or more processors, and the memory is used to store computer program code, the computer program code including computer instructions; The one or more processors invoke the computer instructions to cause the electronic device to perform the method as described in any one of claims 1-6.

9. A computer-readable storage medium comprising instructions, characterized in that: When the instructions are executed on an electronic device, the electronic device causes the electronic device to perform the method as described in any one of claims 1-6.

Images