Wheel-legged robot positioning mapping method and system integrating leg encoder and wheel speedometer, and computer program product

Abstract

The invention relates to the field of robot positioning and mapping, and discloses a wheel-legged robot positioning and mapping method and system fusing a leg encoder and a wheel speedometer, and the method comprises the steps: carrying out the calibration of various sensor information, including an RGBD camera, an IMU, the leg encoder and the wheel speedometer, and carrying out the alignment of time and space; reading data from the plurality of sensors and respectively preprocessing the information of the respective sensors; constructing a rear-end positioning system fused with the leg encoder and the wheel speed meter by using the preprocessed sensor information; and receiving speedometer information transmitted by the back-end positioning system and the preprocessed sensor information, and constructing a point cloud semantic hierarchical map. The robot leg odometer is innovatively transplanted to be used in the wheel-legged robot, the tire rolling error is eliminated through wheel speed compensation, high-frequency pose output is achieved, meanwhile, the point cloud semantic hierarchical map is constructed, and favorable conditions are provided for the wheel-legged robot to understand the environment and conduct intelligent operation.

Description

Technical Field

[0001] The present invention belongs to the field of robot positioning and mapping, and more specifically, relates to a wheel-legged robot positioning and mapping method, system and computer program product that integrates leg encoders and wheel speedometers. Background Art

[0002] In the field of robot positioning and mapping, the robustness, real-time performance of positioning and the environmental understanding ability of the map have always been the core challenges restricting the operation ability in complex environments. Although the current mainstream multi-source sensor SLAM solutions (such as VINS-Fusion, FAST-LIO, etc.) have achieved geometric modeling of the environment, there are still three essential defects: First, external sensors such as cameras, lidar, and GNSS all have the risk of failure, and the data acquisition frequency is significantly lower than that of the IMU; Second, these methods have not fully explored the potential efficiency of the wheel-legged robot's own hardware, and the rich and high-frequency leg encoder and wheel speedometer information have not been used; Third, although the existing geometric point cloud maps retain the detailed features of the environment, they still lack semantic dimension understanding and are difficult to support higher-level intelligent decision-making.

[0003] In the field of quadruped robots, researchers introduced leg encoders and deduced the body movement by inverse kinematics of the legs, thus giving birth to the leg odometry (LO) technology. Taking MIT Cheetah3 as an example, it uses Kalman filtering to fuse LO and IMU data to complete state estimation. However, the unique "wheel-leg" composite motion mode of wheel-legged robots breaks the core assumption in the LO technology that "the foot contact point remains stationary in the world coordinate system", resulting in the inapplicability of traditional LO methods. Existing research attempts to compensate by establishing a tire contact point model or separating the wheeled motion component, but still faces problems such as high model complexity and lack of attitude estimation. On the other hand, the breakthrough of deep learning technology has injected environmental semantic parsing ability into the map system, and the task planning intelligence can be significantly improved by constructing a hierarchical semantic map.

[0004] Therefore, there is an urgent need to construct a new positioning and mapping method and system: it is necessary to break through the inherent limitations of the traditional SLAM framework and fully integrate the unique body sensing information of wheel-legged robots; it is also necessary to break through the limitation of single geometric representation and construct a hierarchical environmental model that supports semantic understanding. The realization of this goal will become the core technical breakthrough for improving the operation ability of wheel-legged robots in complex scenarios. Summary of the Invention

[0005] In view of the above deficiencies or improvement requirements of the prior art, the present invention provides a method and system for positioning and mapping of a wheel-legged robot integrating a leg encoder and a wheel speedometer, aiming to break through the inherent limitations of the traditional SLAM framework, fully integrate the unique body sensor information of the wheel-legged robot, and break through the limitation of a single geometric representation to construct a hierarchical environment model supporting semantic understanding.

[0006] To achieve the above object, according to one aspect of the present invention, there is provided a method for positioning and mapping of a wheel-legged robot integrating a leg encoder and a wheel speedometer, including the following steps:

[0007] S1: Calibrate various sensor information including an RGBD camera, an IMU, a leg encoder, and a wheel speedometer, and align them in time and space;

[0008] S2: Read data from the various sensors and perform preprocessing on the sensor information of each sensor respectively, including: extracting feature points from the RGBD camera, obtaining semantic segmentation of a specified object on the RGB image, calculating the forward kinematics and Jacobian matrix of the legs of the wheel-legged robot using the leg encoder data, calculating the wheel speed of a single leg of the wheel-legged robot using the wheel speedometer, and obtaining the ground contact information of each leg of the wheel-legged robot from relevant ground contact detection algorithms;

[0009] S3: Use the preprocessed sensor information to construct a backend positioning system integrating the leg encoder and the wheel speedometer;

[0010] S4: Receive the odometer information passed in by the backend positioning system and the preprocessed sensor information, and construct a point cloud semantic hierarchical map.

[0011] Further, the calibration in S1 includes:

[0012] RGBD camera calibration: Obtain the internal parameter matrix and distortion coefficient of the RGBD camera, and calibrate the depth values in the depth map obtained by the RGBD camera;

[0013] IMU calibration: Obtain the accelerometer noise n a of the IMU, the accelerometer random walk noise n ba , the gyroscope noise n g , and the gyroscope random walk noise n bg ;

[0014] Leg encoder calibration: Obtain its measurement noise n α .

[0015] Further, the alignment in time and space in S1 includes:

[0016] Hardware synchronization triggering of RGBD camera and IMU data, and time alignment of the leg encoder and wheel speedometer data on a single leg of the wheel-legged robot. At the same time, the external parameter matrix of the RGBD camera and IMU needs to be obtained for spatial alignment.

[0017] Furthermore, in S2, to extract feature points from the RGBD camera, the optical flow method based on deep learning is used to track the positions of feature points in each frame of the image. The final output results are the indices of all feature points in the current image frame, as well as the undistorted normalized pixel coordinates and depth values of the feature points in the current image frame.

[0018] Furthermore, in S2, to obtain the semantic segmentation of the specified object on the RGB image, the specific strategy is to use an object detection model to detect the objects of the category specified by the user, and then use a semantic segmentation model to perform high-resolution local segmentation on the local area of the detected objects.

[0019] Furthermore, in S3, the construction of the backend positioning system that fuses the leg encoder and wheel speedometer includes two subsystems, namely the visual-inertial subsystem and the wheel-leg inertial subsystem. The two subsystems can cooperate to output odometer information, or can output odometer information separately when a certain subsystem fails;

[0020] The two subsystems work at different frequencies, and the frequencies are determined by the data input frequencies of the sensors fused in the two subsystems; the two subsystems are fused through a Kalman filter. Among them, the IMU is used for the estimation of system state variables, and the RGBD camera and the leg encoder and wheel speedometer are respectively used for the update of the visual-inertial subsystem and the wheel-leg inertial subsystem.

[0021] Furthermore, the system state variables are divided into:

[0022] IMU-related states, including the pose of the body coordinate system I of the wheel-legged robot in the world coordinate system G speed G v I and position G p I ;

[0023] IMU zero-bias related states, including the gyroscope zero-bias b g and the accelerometer zero-bias b a ;

[0024] IMU and RGBD camera external parameter related states, including the rotation and translation I p C ;

[0025] The state related to the camera for each frame, including the rotation from the camera coordinate system C of the i-th frame camera i to the world coordinate system and translation

[0026] The state related to the legs of the wheel-legged robot, including the position of the center of the tire at the end of each leg in the world coordinate system G d leg，* ;

[0027] The estimation of the system state variables, including:

[0028] Establishing a kinematic model of the system to estimate the system state variables, where the state related to the IMU is estimated using the IMU kinematic model; the state related to the IMU zero bias is assumed to remain unchanged; the state related to the external parameters of the IMU and RGBD camera is assumed to remain unchanged; the state related to the camera for each frame is estimated through the IMU and the external parameters of the camera, and the newly estimated camera state of one frame is added to the system state variables; the state related to the legs of the wheel-legged robot is assumed to be stationary during estimation;

[0029] The estimation of the system state covariance matrix, including:

[0030] Obtaining the system matrix through the established kinematic model, and discretizing the system matrix to estimate the system state covariance matrix;

[0031] The update of the visual-inertial subsystem, including:

[0032] (a) Using the final output results, i.e., the indices of all feature points in the current image frame, the undistorted normalized pixel coordinates of the feature points in the current image frame, and the depth values, to construct a visual residual constraint, and updating the system state variables through the visual residual constraint;

[0033] (b) Maintaining the number of states related to the camera for each frame in the system state variables, constructing an elimination mechanism, and then reconstructing a visual residual constraint for the feature points jointly observed in the camera frame to be eliminated, and repeating step (b) to update the visual-inertial subsystem;

[0034] The update of the wheel-legged inertial subsystem, including:

[0035] Since it is assumed to be stationary when estimating the states related to the legs of the wheel-legged robot, but in reality the tires of the wheel-legged robot will roll, the wheel speed of a single leg of the wheel-legged robot obtained in S2 is used to compensate for the states related to the legs of the wheel-legged robot before the update to make up for the tire movement during this period; after the compensation, the ground contact information of each leg obtained in S2 is used to identify the legs in contact with the ground, and then for the legs that are in contact with the ground at both the previous moment and the current moment, the forward kinematics of the legs of the wheel-legged robot is used to construct the leg odometry constraint, and the system state variables are updated through the leg odometry constraint;

[0036] Maintain the states related to the legs of the wheel-legged robot, including:

[0037] For the legs that were in contact with the ground at the previous moment and are also in contact with the ground at the current moment, the update has been performed in S36; for the legs that were in contact with the ground at the previous moment but are not in contact with the ground at the current moment, they are deleted from the system state variables; for the legs that were not in contact with the ground at the previous moment but are in contact with the ground at the current moment, they are added to the system state variables.

[0038] Furthermore, the construction of the point cloud semantic hierarchical map in S4 includes:

[0039] Implement the spatial registration of the point cloud based on the odometry information; associate and store the object category semantic information obtained after semantic segmentation according to the user-specified category with the corresponding point cloud regions; construct a point cloud semantic hierarchical map including the original point cloud layer and the semantic segmentation object layer; this map can be used for subsequent relocalization and navigation tasks at the same time.

[0040] According to another aspect of the present invention, there is provided a wheel-legged robot positioning and mapping system integrating a leg encoder and a wheel speed meter, including a memory, a processor, and a computer program stored on the memory, and the processor executes the computer program to implement the wheel-legged robot positioning and mapping method as described in any one of the preceding items.

[0041] According to another aspect of the present invention, there is provided a computer program product, including a computer program, and when the computer program is executed by a processor, it implements the wheel-legged robot positioning and mapping method as described above.

[0042] Generally speaking, compared with the prior art, the above technical solutions conceived by the present invention can achieve the following beneficial effects:

[0043] 1. By incorporating the leg-related states of the wheel-legged robot into the system state variables, assuming the leg state is stationary during the state prediction phase, and compensating for the tire rolling displacement in real-time with a wheel speedometer before the state update phase, the present invention effectively solves the problem that traditional leg odometers can only be applied to legged robots. Specifically, through the dynamic recognition and constraint construction of the grounded leg, it is ensured that the body pose can still be corrected using the leg encoder data during wheeled movement. This mechanism not only retains the advantages of high frequency and low latency of the leg odometer but also eliminates the estimation bias introduced by wheeled movement through the wheel speed compensation algorithm, enabling the positioning system to remain stable during complex terrain switching.

[0044] 2. The present invention deeply integrates the unique body sensing information of the wheel-legged robot (including leg encoders and wheel speedometers) into the positioning and mapping system, significantly improving the utilization efficiency of multi-source data. By constructing a wheel-legged inertial subsystem (LegIS) with high-frequency pose output and a visual inertial subsystem (VIS) with low-frequency pose output, a dual-system asynchronous fusion architecture is formed. Among them, LegIS uses the forward kinematic information of the leg encoder to construct constraints for pose correction, and VIS constructs visual residual constraints through the feature points of the RGBD camera. The two systems work together through the Kalman filter framework, ensuring centimeter-level positioning results can still be independently output by the other system when any one subsystem fails while maintaining a high-frequency update of 500 - 1000Hz. This hierarchical fusion strategy breaks through the dependence on external sensors in traditional SLAM, enabling the system to remain stable in complex scenarios such as occlusion and lighting changes.

[0045] 3. The dual-system fusion architecture proposed by the present invention has wide applicability, and the core algorithm design does not depend on the number of robot legs. Through the dynamic recognition and constraint construction mechanism of the grounded leg, whether it is a bipedal, quadrupedal, or hexapod wheel-legged robot, the effective utilization of leg encoder and wheel speedometer data can be achieved. This general design provides a unified solution for the positioning and mapping of different forms of wheel-legged robots, reducing the algorithm adaptation cost for specific models and significantly improving the engineering practicality of the system.

[0046] 4. The present invention adopts a cascaded algorithm of lightweight object detection and local high-resolution segmentation in the semantic mapping module, achieving fine annotation of scene semantics while ensuring real-time performance. By storing the spatial association of semantic labels and point cloud data, a point cloud semantic hierarchical map combining original geometric information and object semantics is constructed. This hierarchical structure not only supports traditional path planning tasks but also can quickly locate specific targets through semantic queries. At the same time, the feature that users can customize the target category labels enables the system to have a task-oriented environment understanding ability, providing semantic-level support for subsequent intelligent decision-making. The combination of this lightweight model and local refinement processing can still achieve efficient operation on embedded platforms with limited computing resources. Brief Description of the Drawings

[0047] Figure 1 It is a schematic diagram of the overall macro process of the localization and mapping method in the embodiment.

[0048] Figure 2 It is a schematic diagram of the structure of the wheel-leg robot introduced in the embodiment.

[0049] Figure 3 It is an architecture diagram of the backend localization system introduced in the embodiment.

[0050] Figure 4 It is a comparison diagram of the quadruped robot and the wheel-leg robot during single-leg movement introduced in the embodiment. It can be seen that the movement of the wheel-leg robot can be decomposed into wheeled movement and legged movement.

[0051] Figure 5 It is a schematic diagram of the single-leg movement of the wheel-leg robot when the tire is locked. At this time, there is only legged movement, and it can be seen that there will still be a small part of displacement at this time.

[0052] Figure 6 It is a schematic diagram of the process of constructing the point cloud semantic hierarchical map and the relationship between the modules in the proposed localization and mapping system introduced in the embodiment. Detailed Description of the Embodiment

[0053] In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0054] The first aspect of the present invention provides a localization and mapping method for a wheel-leg robot that fuses leg encoders and wheel speedometers. Please refer to Figure 1 , and these processes will be further introduced in detail in this embodiment, including formula derivation:

[0055] S1: Calibrate various sensor information including RGBD cameras, IMUs, leg encoders, and wheel speedometers, and align them in time and space.

[0056] The calibration in S1 includes: calibrating the RGBD camera to obtain the internal parameter matrix of the camera, the distortion coefficient, and calibrating the depth values in its depth map; calibrating the IMU to obtain its accelerometer noise n a , accelerometer random walk noise n ba , gyroscope noise n g , gyroscope random walk noise n bg ; calibrating the leg encoder to obtain its measurement noise nα 。

[0057] In a preferred embodiment, the RGBD camera uses an Intel RealSense Depth Camera D435i, which can output RGB images and depth images simultaneously and has an in-built IMU (so that internal hardware triggering is available and there is no need to manually design the hardware triggering of the sensor). Since the official calibration results are provided, the internal camera matrix, distortion coefficients can be obtained, and at the same time, the external parameter matrix between the IMU and the RGB camera can be obtained. In other embodiments, the Kalibr package can also be used for the internal parameter calibration of the RGBD camera and the external parameter calibration between the RGBD camera and the IMU, and imu_utils is used to calibrate the IMU noise. For the calibration of the leg encoders and wheel speedometers, since the positioning and mapping method is updated independently for each leg in the Legged Inertial Subsystem (LegIS) (because the number of legs of a legged wheel robot is uncertain. Although a quadruped legged wheel robot is preferably used for introduction in this embodiment, a legged wheel robot cannot ensure that all legs are in contact with the ground all the time, and the distance between the tires is not fixed either. Therefore, when fusing the leg encoders and wheel speedometers, the model cannot be simplified into a fixed four-wheel motion model or a two-wheel motion model.), it is only necessary to ensure the time alignment of all leg encoders and wheel speedometers on each leg. In a preferred embodiment, after the underlying STM32 development board reads the information of all leg encoders and wheel speedometers, it stamps timestamps and then transmits them to the upper industrial computer. After receiving the data, the industrial computer uses the software synchronization of ROS to synchronize the data on a single leg.

[0058] Preferably, the alignment in time and space in S1 includes the hardware synchronous triggering of the RGBD camera and IMU data, and the time alignment of the leg encoder and wheel speedometer data on a single leg of the legged wheel robot. At the same time, the external parameter matrix of the RGBD camera and the IMU needs to be obtained for spatial alignment.

[0059] S2: Reading data from the multiple sensors and respectively preprocessing the information of each sensor includes extracting feature points from the RGBD camera, obtaining the semantic segmentation of a specified object on the RGB image, calculating the forward kinematics and Jacobian matrix of the legs of the legged wheel robot using the leg encoder data, calculating the wheel speed of a single leg of the legged wheel robot using the wheel speedometer, and obtaining the ground contact information of each leg of the legged wheel robot from relevant ground contact detection algorithms.

[0060] Preferably, in S2, the feature points are extracted from the RGBD camera using a deep learning-based optical flow method to track the positions of the feature points in each frame of the image. This process is equivalent to the front end of visual SLAM, and the final output result is the index of all feature points in the current image frame, the undistorted normalized pixel coordinates of the feature points in the current image frame, and the depth values.

[0061] Preferably, the specific strategy for obtaining the semantic segmentation of the specified object on the RGB image in S2 is to use a lightweight object detection model to detect the objects of the category specified by the user, and then use a semantic segmentation model to perform high-resolution local segmentation on the detected local area of the object.

[0062] Preferably, step S2 further includes:

[0063] S21: The feature points are extracted from the RGBD camera using a deep learning-based optical flow method. In a preferred embodiment, LET-NET is used for optical flow extraction. LET-NET implements a very lightweight feature point extraction and image consistency calculation network, which can process an image of 240×320 on the CPU in about 5 ms. Combined with LK optical flow, the assumption of brightness consistency is broken, and it has good performance on dynamically illuminated and blurred images.

[0064] S22: The semantic segmentation strategy described in it. In a preferred embodiment, YOLOv8 and MobileSAM are used for object detection and semantic segmentation respectively. First, object detection needs to be performed according to the object category specified by the user. After the ROI of the target object is detected, semantic segmentation is only performed on the ROI to reduce the amount of calculation. This strategy is to adapt to the situation where there are fixed small target objects in a large scene and we need to obtain the position of the small objects in the map. For example, in a chemical plant environment, robots often need to read instrument data, so it is necessary to obtain the position of the instrument in advance and mark it on the map to establish semantic associations.

[0065] S23: Please refer to Figure 2 , in this embodiment, since the quadruped wheeled-leg robot used has a foot-end steering joint, it is necessary to calculate the wheel speed in the world coordinate system according to the steering angle of the foot-end steering joint and the speed υ of the wheel speedometer wheel to calculate the wheel speed in the world coordinate system G v leg , and since the steering axis of the steering joint is not always perpendicular to the ground, it is necessary to geometrically transform the measured steering angle of the foot-end steering joint to obtain the actual steering angle where w z is the component of Fk R (α m ) calculated by forward kinematics:

[0066] Fk R (α m ) = [w x w y w z

[0067] Among them, α m represents the measured value of the joint angle, to respectively represent the angle values of the thigh motor, the calf motor, and the steering motor. Then, the wheel speed in the body coordinate system can be obtained as follows:

[0068]

[0069] Furthermore, the wheel speed in the world coordinate system is obtained

[0070] S24: The relevant ground contact detection algorithm in S2 should be able to provide the detection result of whether the wheel-legged robot touches the ground on different terrains. The algorithm should be able to achieve a detection accuracy of more than 95%. In the optimized embodiment, the ground contact detection method deep-contact-estimator in the paper "Legged Robot State Estimation using Invariant Kalman Filtering and Learned Contact Events" published by Tzu-Yuan Lin et al. in the "2021 Conference on Robot Learning" is used. In other embodiments, if the wheel-legged robot body has a built-in ground contact detection module on the tire, the ground contact information can also be directly obtained from the ground contact detection module.

[0071] S3: Receive the preprocessed sensor information, and then construct a backend positioning system that fuses the leg encoder and the wheel speedometer. The backend positioning system is constructed as VIS and LegIS, please refer to Figure 3 , in the preferred embodiment, VIS uses the architecture of S-MSCKF, and the filter framework selects the Invariant Extended Kalman Filter (InEKF) to update VIS and LegIS asynchronously through InEKF. In other embodiments, other filter frameworks such as the Error State Extended Kalman Filter (ESKF), the Extended Kalman Filter (EKF), the Equivariant Filter (EqF), etc. can also be used, and the visual-inertial fusion method is not limited to S-MSCKF.

[0072] Preferably, the backend positioning system that integrates the leg encoder and the wheel speedometer in S3 includes two subsystems, namely the Visual Inertial Subsystem (VIS) and the Leg Inertial Subsystem (LegIS). The two subsystems can output odometer information collaboratively or output odometer information separately when a certain subsystem fails. The two subsystems work at different frequencies, which are determined by the data input frequencies of the sensors integrated in the two subsystems. Generally, the frequency of the Visual Inertial Subsystem is 30 - 60 Hz, and the frequency of the Leg Inertial Subsystem is 500 - 1000 Hz. The two subsystems are fused through a Kalman filter, where the IMU is used for predicting the system state variables, and the camera, the leg encoder, and the wheel speedometer are respectively used for updating the Visual Inertial Subsystem and the Leg Inertial Subsystem.

[0073] Preferably, the IMU-related states include the pose of the body coordinate system I of the wheel-legged robot in the world coordinate system G velocity G V I and position G p I ;

[0074] The IMU bias-related states include the gyroscope bias b g and the accelerometer bias b a ;

[0075] The IMU and RGBD camera extrinsic parameter-related states include the rotation and translation I p C ;

[0076] The state related to each frame of the camera includes the rotation i from the camera coordinate system C where the i-th frame of the camera is located to the world coordinate system

[0077] The state related to the legs of the wheel-legged robot includes the position of the center of the tire at the end of each leg in the world coordinate system G d leg，* ;

[0078] Preferably, the relevant variables involved in S3 are described in detail below:

[0079] S31: The definition of the system state variables is similar to the state definition in S-MSCKF. The state of the system is defined as two parts, namely the state related to the IMU (assuming that the robot body coordinate system and the IMU coordinate system coincide) and the state related to the camera

[0080]

[0081] Among them, G, I, and C represent the world system, the IMU system, and the camera system respectively. Since the project uses an RGBD camera and uses a sliding window to maintain the state of multiple frames of the camera, C i represents the state of the i-th frame of the camera.

[0082] G d leg represents the position of the foot contact point of the wheeled-legged robot in the world system. For simplicity of expression, only one leg is considered here. In fact the states of the four legs of the robot should be included respectively. In addition, to avoid frequent calibration of the external parameters, an external parameter estimation is considered to be added to the system:

[0083]

[0084] represents the transformation matrix from the camera to the IMU. The complete state quantity x of the entire state estimation system is:

[0085]

[0086] It can be seen that the states of n cameras are maintained in the system. To maintain a limited computational complexity, the camera states do not grow infinitely. Instead, like S-MSCKF, a sliding window is used for maintenance, and deletion is performed when the number of cameras reaches the limit.

[0087] S32: Prediction of the system state variables described in, including establishing a system kinematic model to predict the system state variables described in S31, where the IMU-related states are predicted using the IMU kinematic model; the IMU zero-bias-related states are assumed to remain unchanged; the IMU and RGBD camera external parameter-related states are assumed to remain unchanged. The kinematic equations of each prediction term in continuous time are as follows, without considering the relevant noise values on each term at this time:

[0088]

[0089] ω m and a m are the measurement values of the gyroscope and the accelerometer respectively, G g is the gravitational acceleration. During the prediction process, the wheeled motion and the legged motion of the wheeled-legged robot are separated, and it is still considered that the foot end is stationary when it touches the ground. When performing kinematic updates later, the wheeled motion will be compensated. Therefore, here G d leg the derivative with respect to time is 0. Let ω = ω m -b g 、a = a m -b a, discretize the kinematic equation for continuous time:

[0090]

[0091]

[0092] Γ0(ω k Δt) is to substitute ω k Δt as φ into the formula of Γ0(φ) for calculation. (·) k and (·) k+1 are the values at the k-th and (k + 1)-th moments respectively, and Δt is the time interval between adjacent moments. Among them:

[0093]

[0094] Among them, the superscript ^ represents converting a three-dimensional vector into the form of its skew-symmetric matrix.

[0095] When a new camera frame arrives, since the camera and the IMU are fixedly connected, we can easily estimate the state of the new camera frame through the current state of the IMU and add the estimated new camera state to the system state for maintenance:

[0096]

[0097] S33: Estimation of the system state covariance matrix. The system matrix A and the input matrix B are obtained through the kinematic model in S32. The derivation process of the system matrix can be found in the InEKF related theory and will not be elaborated here. The A matrix and the B matrix are expressed as follows:

[0098]

[0099] Construct a noise matrix for the noise of the entire system:

[0100]

[0101] During the pure legged motion of the wheel-legged robot (the tire rolling is locked), assume that the speed of the touchdown point is 0, and n leg is the speed noise of the touchdown point at this time. Next, discretize the A matrix and the B matrix, and at the same time consider the external parameter part to obtain the state transition matrix and the noise covariance matrix

[0102]

[0103] Among them, Φ(t k+1 , τ) represents the state transition from the t k -th moment to the t k+1 -th moment.

[0104] Then, for the convenience of state prediction of the system error covariance matrix, the system error covariance matrix is split into:

[0105]

[0106] Among them, corresponds to the first 3 terms in and corresponds to the last n terms Therefore, the prediction of the error covariance matrix at the (k + 1)-th moment is as follows:

[0107]

[0108] represents the prior estimate value, represents the posterior estimate value. At the same time, since a new camera state is added in S32, it is necessary to augment the camera state for

[0109]

[0110] J I is the partial derivative of the camera state with respect to the IMU-related state variables. The augmented system error covariance matrix

[0111] S34: Update of the visual-inertial subsystem. Using the finally output results, i.e., the indices of all feature points in the current image frame, the undistorted normalized pixel coordinates of the feature points in the current image frame, and the depth values, a visual residual constraint is constructed, and the system state variables are updated through the visual residual constraint. In a preferred embodiment, the method in the paper "Stereo msckf with online extrinsic calibration using invariant extended kalman filter" published by Eugene Auh et al. in the "2021 18th International Conference on Ubiquitous Robots" is used to construct the visual residual constraint to update the system state variables, but the binocular residual constraint in this paper is changed to a monocular residual constraint using an RGBD camera.

[0112] S35: Maintain the number of camera-related states in each frame of the system state variables, construct an elimination mechanism, and then reconstruct the visual residual constraint for the feature points commonly observed in the camera frames to be eliminated, and repeat step S34 to update the visual-inertial subsystem.

[0113] S36: Update of the wheel-leg inertial subsystem. Please refer to​Figure 4 , in a quadruped robot, since the foot tip can be regarded as a particle, the touchdown point of its foot tip G d leg is regarded as stationary during a single touchdown process. Therefore, the following constraint equation can be constructed to update the observation of the leg odometer. n α is the measurement noise of the joint encoder:

[0114]

[0115] The subscript t in the lower right corner indicates that this variable is the true value. However, in a wheel-legged robot, on the one hand, due to the presence of wheels, its foot tip cannot be simply regarded as a particle. On the other hand, the single touchdown process of a wheel-legged robot includes the movements of both the legged and wheeled parts, and it can no longer be assumed that the foot tip is stationary when touching the ground. Fortunately, we can decompose the movement, calculate the movement of the wheels, align the touchdown points between the front and rear frames, and then use this assumption for updating. Please refer to Figure 4 . However, it should still be noted that this alignment is not a strict alignment. Please refer to Figure 5 , even when the wheel is locked, the single-step walking of the wheel still includes a small part of displacement, and this part should be considered in the noise n leg .

[0116] First, calculate the movement of the wheel in a single leg. Since it is assumed to be stationary when estimating the relevant states of the leg of the wheel-legged robot in S32, that is, the prior estimate value of the wheel end at the k + 1 moment is equal to the posterior estimate value of the wheel end at the k moment But in reality, the tire of the wheel-legged robot will roll. Therefore, before updating, the wheel speed of the single leg of the wheel-legged robot obtained in S2 is used to compensate for the relevant states of the leg of the wheel-legged robot to make up for the tire movement during this period. In this embodiment S23, the wheel speed of a single leg in the world coordinate system is obtained G v leg , and then the wheel speed is aligned. The prior value of the wheel end at the k + 1 moment in is corrected to Then the observation equation of InEKF can be constructed:

[0117]

[0118] J p (α m ) is the Jacobian matrix calculated by forward kinematics. The observation error of the leg is constructed as:

[0119]

[0120] At the same time, the Jacobian matrix of the leg observation is:

[0121]

[0122] Therefore, for when only one leg is considered in, the update step of the leg is as follows:

[0123]

[0124] where K leg is the Kalman gain, is the exponential map in InEKF, which is used to map the vector ξ to the manifold. The above completes the update of the system state variables.

[0125] S37: Maintain the state related to the leg of the wheeled-leg robot. For the leg that touched the ground at the previous moment and also touches the ground at the current moment, it has been updated in S36; for the leg that touched the ground at the previous moment but does not touch the ground at the current moment, delete it from the system state variables. Specifically, directly delete the rows and columns corresponding to this leg from and at the same time delete the corresponding rows and columns of the system error covariance matrix P;

[0126] For the leg that did not touch the ground at the previous moment but touches the ground at the current moment, the formula needs to be used to add it to the system state variables and at the same time add the corresponding covariance terms to the system error covariance matrix P. G d leg The corresponding covariance term of G p I is still determined by

[0127] S4: Receive the odometer information passed in by the back-end positioning system in the preferred embodiment S3 and the preprocessed sensor information (mainly the result after semantic segmentation). Please refer to Figure 6 , and then use the odometer information for point cloud registration to construct a point cloud map. In the preferred embodiment, the outlier removal filter and downsampling filter of the PCL library are used to filter the point cloud map, and then it is associated with the result after semantic segmentation to construct a point cloud semantic hierarchical map. The constructed map can be used for subsequent relocalization and navigation tasks at the same time. In the preferred embodiment, we can successfully use this map in the chemical plant inspection task. Specifically, we focus on the reading and detection of instrument parameters. To improve the inspection efficiency, we instruct the target object to be the instrument, and then construct a point cloud semantic hierarchical map. According to this map, we can navigate conveniently and efficiently and plan the optimal inspection path.

[0128] Preferably, the construction of the point cloud semantic hierarchical map in S4 includes: realizing the spatial registration of the point cloud based on the odometer information; associating and storing the object category semantic information obtained after semantic segmentation according to the user-specified category with the corresponding point cloud region; constructing a point cloud semantic hierarchical map including the original point cloud layer and the semantic segmentation object layer. This map can be used for subsequent relocalization and navigation tasks simultaneously.

[0129] The present invention also provides a wheel-legged robot positioning and mapping system integrating a leg encoder and a wheel speedometer. Please refer to Figure 6 (This figure only generally illustrates the relationship between different modules and is not used to represent the specific functions within each module. For specific functions, please refer to the specific introduction of the positioning and mapping method in S1 - S4), including:

[0130] Multi-sensor data preprocessing module: used to perform the calibration, time and space alignment, and feature extraction operations of the above-mentioned multiple sensor data.

[0131] Fusion positioning module: includes a visual inertial subsystem and a wheel-legged inertial subsystem, used to realize the fusion of multiple sensor data and pose solution.

[0132] Semantic mapping module: used to construct a point cloud semantic hierarchical map.

[0133] Motion control interface module: encapsulates the positioning and map information output by the fusion positioning module and the semantic mapping module for the relocalization and navigation tasks of the robot.

[0134] Those skilled in the art can easily understand that the above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A method for positioning and mapping a wheel-leg robot that integrates leg encoders and wheel speedometers, characterized in that: The steps include: S1: Calibrate and align various sensor information including RGBD camera, IMU, leg encoder and wheel speed meter in time and space; S2: Reading data from the multiple sensors and preprocessing the sensor information of each sensor, including: extracting feature points from the RGBD camera, obtaining semantic segmentation of a specified object on the RGB image, calculating the forward kinematics and Jacobian matrix of the wheel-legged robot's legs using the leg encoder data, calculating the wheel speed of a single leg of the wheel-legged robot using the wheel speed meter, and obtaining the ground contact information of each leg of the wheel-legged robot from a related ground contact detection algorithm; S3: Using the pre-processed sensor information, construct a back-end positioning system that integrates the leg encoder and the wheel speed meter; S4: Receive the odometer information and the pre-processed sensor information transmitted by the back-end positioning system, and construct a point cloud semantic layered map.

2. A method for positioning and mapping a wheel-legged robot integrating a leg encoder and a wheel speedometer according to claim 1, characterized in that: The calibration in S1 includes: RGBD camera calibration: obtain the intrinsic parameter matrix and distortion coefficient of the RGBD camera and calibrate the depth value in the depth map obtained by the RGBD camera; IMU Calibration: Obtain the accelerometer noise n of the IMU a , accelerometer random walk noise n ba , gyroscope noise n g , gyroscope random walk noise n bg ; Leg encoder calibration: Obtain its measurement noise n α .

3. The method for positioning and mapping a wheel-leg robot integrating a leg encoder and a wheel speedometer according to claim 1, characterized in that: The temporal and spatial alignment in S1 includes: The hardware synchronization triggering of the RGBD camera and IMU data, as well as the time alignment of the leg encoder and wheel speed meter data on a single leg of the wheel-legged robot, are required. At the same time, the extrinsic parameter matrices of the RGBD camera and IMU must be obtained for spatial alignment.

4. A wheel-legged robot positioning and mapping method integrating a leg encoder and a wheel speedometer according to claim 1, characterized in that In S2, feature points are extracted from the RGBD camera and the optical flow method based on deep learning is used to track the position of feature points in each frame of the image. The final output result is the index of all feature points in the current image frame, as well as the normalized pixel coordinates and depth values of the feature points in the current image frame after dedistortion.

5. A method for wheel-legged robot positioning and mapping that integrates a leg encoder and a wheel speedometer according to claim 1, characterized in that As described in S2, the semantic segmentation of the specified object is obtained on the RGB image. The specific strategy is to use the target detection model to detect objects of the category specified by the user, and then use the semantic segmentation model to perform high-resolution local segmentation on the local area of the detected object.

6. A method for wheel-legged robot positioning and mapping that integrates a leg encoder and a wheel speedometer according to claim 4, characterized in that, The back-end positioning system constructed in S3, which integrates the leg encoder and wheel speedometer, includes two subsystems: a visual inertial subsystem and a wheel-leg inertial subsystem. The two subsystems can output odometer information in a coordinated manner or independently when one subsystem fails. The two subsystems operate at different frequencies, which are determined by the data input frequencies of the sensors fused in the two subsystems. The two subsystems are fused through a Kalman filter, where the IMU is used to estimate the system state variables, and the RGBD camera, the leg encoder, and the wheel speedometer are used to update the visual inertial subsystem and the wheel-leg inertial subsystem, respectively.

7. A method for positioning and mapping a wheel-legged robot integrating a leg encoder and a wheel speedometer according to claim 6, characterized in that: The system state variables are divided into: IMU-related status, including the position and orientation of the wheel-legged robot's body coordinate system I in the world coordinate system G speed G V I and location G p I ; IMU bias related status, including gyroscope bias b g and accelerometer bias b a ; The state related to the extrinsic parameters of the IMU and the RGBD camera, including the rotation from the camera coordinate system C to the IMU coordinate system and translation I p C ; The state related to each camera frame, including the rotation from the camera coordinate system C where the i-th camera is located i to the world coordinate system and translation The leg-related states of the wheel-legged robot, including the positions of the centers of the tires at the end of each leg in the world coordinate system G d leg，* ; The estimation of the system state variables includes: Establish a kinematic model of the system to estimate the system state variables, where the IMU-related states are estimated using the IMU kinematic model; the IMU bias-related states are assumed to remain unchanged; the states related to the external parameters of the IMU and the RGBD camera are assumed to remain unchanged; the state of each frame of the camera is estimated through the external parameters of the IMU and the camera, and the newly estimated state of a frame of the camera is added to the system state variables; the states related to the legs of the wheeled-leg robot are assumed to be stationary during estimation. Estimation of the system state covariance matrix, including: Obtain the system matrix through the established kinematic model, and discretize the system matrix to estimate the system state covariance matrix. Update of the visual-inertial subsystem, including: (a) Use the final output results, i.e., the indices of all feature points in the current image frame, the undistorted normalized pixel coordinates of the feature points in the current image frame, and the depth values, to construct a visual residual constraint, and update the system state variables through the visual residual constraint. (b) Maintain the number of states related to each frame of the camera in the system state variables, construct an elimination mechanism, and then reconstruct a visual residual constraint for the feature points jointly observed in the camera frame to be eliminated, and repeat step (b) to update the visual-inertial subsystem. Update of the wheeled-leg inertial subsystem, including: Since it is assumed that the states related to the legs of the wheeled-leg robot are stationary during estimation, but in reality, the tires of the wheeled-leg robot will roll, so before the update, use the wheel speed of a single leg of the wheeled-leg robot obtained in S2 to compensate for the states related to the legs of the wheeled-leg robot to make up for the tire movement during this period; after the compensation, use the ground contact information of each leg obtained in S2 to identify the legs in contact with the ground, and then use the forward kinematics of the legs of the wheeled-leg robot to construct a leg odometry constraint for the legs that are in contact with the ground at the previous moment and the current moment, and update the system state variables through the leg odometry constraint. Maintain the states related to the legs of the wheeled-leg robot, including: For the legs that were in contact with the ground at the previous moment and are also in contact with the ground at the current moment, they have been updated in S36; for the legs that were in contact with the ground at the previous moment but are not in contact with the ground at the current moment, delete them from the system state variables; for the legs that were not in contact with the ground at the previous moment but are in contact with the ground at the current moment, add them to the system state variables.

8. A wheel-leg robot positioning and mapping method integrating a leg encoder and a wheel speedometer according to claim 1, characterized in that, The construction of the point cloud semantic hierarchical map in S4 includes: Realize the spatial registration of the point cloud based on the odometry information; associate and store the object category semantic information obtained after semantic segmentation according to the user-specified category with the corresponding point cloud regions; construct a point cloud semantic hierarchical map including the original point cloud layer and the semantic segmentation object layer; this map can be used for subsequent relocalization and navigation tasks.

9. A wheel-legged robot positioning and mapping system integrating a leg encoder and a wheel speedometer, comprising a memory, a processor, and a computer program stored on the memory, characterized in that, The processor executes the computer program to implement the wheeled-leg robot localization and mapping method according to any one of claims 1 to 8.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the wheeled-leg robot localization and mapping method according to claim 1.

Images