A Terrain Perception Method and System for Legged Robots Based on Multi-Sensor Fusion

Abstract

The present invention belongs to the field of robot motion control, and provides a terrain perception method and system for a legged robot based on multi-sensor fusion, including: collecting lidar data and inertial sensor data of the legged robot and fusing them through a manifold Kalman filter, and deriving and estimating states based on the fused data; using the estimated states as odometer data; constructing a topographic map of the legged robot by using the depth camera point cloud data of the legged robot and the odometer data. Compared with the prior art, the present invention can provide a terrain perception method for the legged robot with strong robustness and low cost.

Description

Technical Field

[0001] The present invention belongs to the field of robot motion control, and particularly relates to a terrain perception method and system for a legged robot based on multi-sensor fusion. Background Art

[0002] With the continuous progress of society and the rapid development of technology, the application fields of mobile robots have gradually expanded. Traditional wheeled and tracked robots are widely used due to advantages such as simple control and mature technology, but their adaptability in complex terrains is poor, which limits the working environment. To overcome this limitation, legged robots have emerged. Due to the ability to walk on unstructured terrains, legged robots can perform tasks that wheeled or tracked robots cannot complete. However, due to the unique motion mode of legged robots, perceiving the surrounding environment is crucial for them. General 2D maps for wheeled and tracked robots cannot meet their needs, so it is necessary to construct a topographic map with richer information including height information to provide a basis for the footfall motion planning of legged robots. Legged robots can use this information to navigate in unknown environments or perform perceptual motion control on rough terrains.

[0003] Currently, there are mainly two methods for constructing a topographic map for legged robots: One is based on local perception, using the information of the robot body to calculate the odometer. Although it can continuously reconstruct the topographic map around the legged robot, in the case of inaccurate robot state estimation, the results of this method are often not accurate enough. The other method completely relies on depth sensors. By arranging multiple sensors around the legged robot, it tries to cover the entire space as much as possible and perform real-time processing to generate a topographic map. However, these existing technologies still face challenges when dealing with complex environments and fail to effectively solve the environmental perception requirements of legged robots under dynamic and uncertain conditions. Summary of the Invention

[0004] The present invention aims to overcome the defect of insufficient robustness in complex situations of the above-mentioned existing technologies, and proposes a terrain perception method and system for a legged robot based on multi-sensor fusion.

[0005] To achieve the above technical effects, the technical solution of the present invention is as follows:

[0006] A terrain perception method for a legged robot based on multi-sensor fusion includes the following steps:

[0007] Collect the lidar data and inertial sensor data of the legged robot and fuse them through a manifold Kalman filter, and deduce the estimated state based on the fused data;

[0008] Use the estimated state as odometry data; construct a topographic map of the legged robot using the depth camera point cloud data of the legged robot and the odometry data.

[0009] The present invention also proposes a terrain perception system for a legged robot based on multi-sensor fusion, the system comprising:

[0010] Data acquisition and processing module: used to acquire lidar data and inertial sensor data and input them into the manifold Kalman filter;

[0011] Kalman filtering module: fuse the input lidar data and inertial sensor data,

[0012] Derive an estimated state based on the fused data;

[0013] Odometry update module: based on using the estimated state as odometry data;

[0014] Topographic map construction module: construct a topographic map of the legged robot using the point cloud data of the depth camera and the updated odometry.

[0015] Compared with the prior art, the beneficial effects of the present invention:

[0016] By fusing lidar data and inertial sensor data, the present invention reduces the noise and error effects brought by a single sensor, and then uses the manifold Kalman filter to estimate the state in a non-linear system, improving the adaptability to environmental changes; based on this state estimation, the depth camera is coupled through a spatial transformation; the point cloud data output by the depth camera is mapped into space to generate a topographic map used by the legged robot, enabling the topographic map to have height information and three-dimensional environmental characteristics, and further constructing a more comprehensive and detailed topographic map.

[0017] Through the multi-sensor data fusion method, the present invention can still maintain good performance when facing external interference, significantly improving the reliability of the topographic map. Description of the Drawings

[0018] Figure 1 Flowchart of the terrain perception method for a legged robot based on multi-sensor fusion in Embodiment 1.

[0019] Figure 2 Topographic map generated by using the terrain perception method for a legged robot based on multi-sensor fusion.

[0020] Figure 3 Physical diagram of the legged robot and the sensors carried on it.

[0021] Figure 4 System architecture diagram of the terrain perception system for a legged robot based on multi-sensor fusion in Embodiment 2. Detailed implementation manners

[0022] The attached drawings are only for illustrative purposes and should not be construed as limitations on the present invention;

[0023] For those skilled in the art, it is understandable that some well-known descriptions in the drawings may be omitted.

[0024] The technical solutions of the present invention will be further described below with reference to the attached drawings and embodiments.

[0025] Embodiment 1

[0026] This embodiment proposes a terrain perception method for a legged robot based on multi-sensor fusion. As Figure 1 shown, it is the flow chart of the terrain perception method for a legged robot based on multi-sensor fusion in this embodiment.

[0027] The terrain perception method for a legged robot based on multi-sensor fusion proposed in this embodiment includes the following steps:

[0028] Collect the lidar data and inertial sensor data of the legged robot and fuse them through a manifold Kalman filter, and deduce and estimate the state based on the fused data;

[0029] Take the estimated state as odometry data; use the depth camera point cloud data of the legged robot and the odometry data to construct a topographic map of the legged robot.

[0030] In this embodiment, the lidar data and inertial sensor data are fused, and then the manifold Kalman filter is used to estimate the state in a non-linear system, so as to achieve highly robust odometry positioning; furthermore, through the coupling of the depth camera and the odometry, a high-precision terrain perception map can be established based on the relatively dense point cloud of the depth camera. By using the data fusion method of multi-sensors, this method can still maintain good performance when facing external interference, and significantly improve the reliability of the topographic map. As Figure 2 shown, it is the topographic map generated by the terrain perception method for a legged robot based on multi-sensor fusion. As Figure 3 shown, it is the physical diagram of the robot and the sensors carried on it.

[0031] In an optional embodiment, the step of fusing based on the manifold Kalman filter includes:

[0032] In the manifold Kalman filter, based on the respective sampling times of the lidar data and the inertial sensor data, the inertial sensor data is used to perform point-by-point matching on the lidar data and merge them.

[0033] In this embodiment, the sampling frequency of the inertial sensor is much higher than that of the lidar; both the lidar data and the inertial sensor data are used as measurement values and input into the manifold Kalman filter to update the estimated state. By merging the lidar data and the inertial sensor data on the time axis, the time synchronization problem caused by the different sampling times of the two is effectively solved; among them, the lidar provides high-precision spatial information, while the inertial sensor provides fast dynamic information, and the combination of the two greatly improves the accuracy and robustness of terrain perception. Since both the lidar data and the inertial sensor data are used as measurements for subsequent state updates, based on the inertial sensor data with high-frequency sampling, the state will be updated at a very high frequency in the Kalman filter.

[0034] In an optional embodiment, the step of deriving the estimated state based on the fused data includes:

[0035] Simulate the inertial sensor data based on a stochastic process and extend it to kinematics to construct a state transition model; construct a measurement model based on the inertial sensor data and the lidar data;

[0036] Use the state transition model to predict the state and covariance at the next moment;

[0037] Obtain the measurement data at the current moment through the measurement model, calculate the residual based on the state and covariance at the next moment, and update the state and covariance at the next moment through the residual to obtain the estimated state.

[0038] In this embodiment, a state transition model and a measurement model are established, and data fusion is performed at the sampling time of each lidar point or inertial measurement unit to achieve real-time and strongly robust pose estimation. By using the state transition model to predict the state and covariance at the next moment, the system can quickly adapt to environmental changes and update the state estimate in real time; secondly, obtain the measurement data at the current moment through the measurement model, calculate the residual based on the state and covariance at the next moment, identify and correct the estimation error in a timely manner, improve the accuracy of the state estimate, and enhance the robustness of the overall system.

[0039] In an optional embodiment, the step of simulating the inertial sensor data based on a stochastic process and extending it to kinematics to construct a state transition model includes the following steps:

[0040] The state transition model takes the first frame of data in the inertial sensor data as the origin of the world coordinate system, models the angular velocity and acceleration using the random deviation and Gaussian noise of the inertial sensor, and discretizes the model to obtain the state transition model

[0041] The expression of the state transition model is as follows:

[0042]

[0043] Among them, and respectively represent the attitude, position and velocity of the odometer in the world coordinate system, represents the gravity vector in the world coordinate system; and respectively represent the angular velocity and acceleration in the odometer coordinate system, represents the skew-symmetric matrix constructed by the angular velocity vector; Gaussian noise is the random deviation of the inertial measurement unit; is Gaussian noise ; is the change rate of the angular velocity and acceleration, which conforms to the Gaussian distribution; k represents the time instant.

[0044] More specifically, since there are singularities and discontinuities in using Euler angles or quaternions for the attitude in the odometer, in order to describe the operation of the system state space model in discrete space in continuous time, two operation symbols and are defined. In positioning, only the manifolds SO(3) and are involved:

[0045] ;

[0046] ;

[0047] ;

[0048] where is the exponential map on SO(3), and log is the inverse map. For a composite manifold such as , there is:

[0049] = , =

[0050] Taking the inertial measurement unit coordinate system as the odometer coordinate system, and the first frame of data of the inertial measurement unit as the origin G of the world coordinate system, the following continuous motion model can be obtained:

[0051]

[0052] Discretize formula (2), represents the measurement time interval. Assuming that the input remains unchanged in the interval , we get:

[0053]

[0054] Among them, the manifold , dim( ) = 24, , is the covariance matrix of the process noise .

[0055] Based on Equation (2) and Equation (3), the state transition model of this embodiment can be obtained.

[0056] In this embodiment, by taking the first frame of data in the inertial sensor data as the origin of the world coordinate system and considering random deviations and Gaussian noise, the state transition model can effectively capture and describe the dynamic characteristics of the legged robot during movement, improving the accuracy of motion prediction. Secondly, by using the manifold SO(3) to process attitude representation, the singularities and discontinuities in attitude description of Euler angles and quaternions are avoided. The introduced Gaussian noise can enhance the robustness of the system to noise and deviations.

[0057] In an optional embodiment, the step of constructing a measurement model based on inertial sensor data and lidar data includes:

[0058] Based on the known mechanical structure, obtain the static transformation relationship between the inertial sensor and the lidar, and project the lidar points into the world coordinate system; the expression of the measurement model is as follows:

[0059]

[0060]

[0061] Among them, , are Gaussian noises; is the state vector; is the position of the lidar point in the inertial measurement unit coordinate system; and are the angular velocity and acceleration in the inertial sensor.

[0062] Specifically, let the transformation matrix between the lidar coordinate system L and the inertial measurement unit coordinate system be , and the measurement point is affected by the Gaussian noise , then the position of a lidar point measurement value in the inertial measurement unit coordinate system

[0063] = +

[0064] Utilize the attitude of a real-time inertial measurement unit Project the lidar points onto the world coordinate system. This measurement point should theoretically be exactly located on a local small plane of the map. Assume is the normal vector of this local small plane, is an arbitrary point on this plane, and there is:

[0065]

[0066] The inertial measurement unit data includes angular velocity measurement ( ) and acceleration measurement ( ):

[0067]

[0068] where , , let be the measurement noise of the inertial measurement unit. Since is included in the state vector , Equation (5) represents an implicit measurement model of . Rearrange Equation (5) and Equation (6) to obtain the measurement model of this embodiment.

[0069] More specifically, define the following symbols: is the state at time k ; is the true value of the state; is the propagated value and updated value of the state ; is the error between the true value and the estimated value

[0070] Receive the measurement value at time k. The updated value at this time is , and its covariance matrix is . Let . The state propagation from time k to time k + 1 follows Equation (3), and there are state transition model and covariance propagation as follows:

[0071]

[0072]

[0073] where is the process noise covariance, matrix , can be calculated by the following formula:

[0074]

[0075]

[0076]

[0077]

[0078] s

[0079] In this embodiment, by projecting the lidar points into the world coordinate system, the measurement model can achieve accurate conversion between different coordinate systems, ensuring that the lidar data and the inertial sensor data are processed within the same reference framework, thereby improving the accuracy of the overall data fusion. Secondly, the concept of Gaussian noise is introduced, enabling the measurement model to effectively reflect the measurement errors that may occur in the actual operation of the sensor. The construction of this measurement model provides a basis for the measurement of lidar and inertial sensor data and subsequent state updates.

[0080] In an optional embodiment, the state at the next moment includes the lidar prediction value and the inertial measurement prediction value; the step of calculating the residual includes:

[0081] For the lidar prediction value, based on the lidar feature map, this data and the adjacent points are fitted into a local small plane, and it is judged whether the distance between the adjacent points and the local small plane is less than the first threshold. If so, the residual is calculated for the lidar prediction value; otherwise, the residual is not calculated.

[0082] For the inertial measurement prediction value, first judge whether the difference between this measurement value and the saturation value is greater than the preset threshold. If so, the residual is calculated for the inertial measurement prediction value; otherwise, the residual is not calculated.

[0083] The residual is calculated as follows:

[0084] Where is the Jacobian matrix, representing the partial derivative of the lidar data or the inertial measurement data with respect to the state variable ; represents the partial derivative of the lidar data or the inertial measurement data with respect to the measurement noise .

[0085] Specifically, the lidar measurement part uses the predicted attitude to map the lidar points into the world coordinate system , and searches for the nearest 5 points within the defined range in the lidar feature map and fits them into a local small plane, whose normal vector is , and the centroid is If the nearest 5 points are not located on the fitting plane, that is, the distance from the plane is greater than the first threshold, the point is fused into the lidar feature map, and the residual and state update are not calculated. Otherwise, the residual is calculated according to the measurement model as follows:

[0086] -

[0087] where , is the true state value at time k + 1. At the same time:

[0088]

[0089]

[0090]

[0091] For the inertial measurement unit, first detect whether its measurement value is saturated. If the difference from the saturation value is less than the second threshold, the measurement is discarded. Otherwise, for non-saturated measurements, there is:

[0092] -

[0093]

[0094] where , is the true state value at time k + 1. At the same time: )]

[0095]

[0096]

[0097] Combining Equation (11) and Equation (13) gives the residual calculation expression of this embodiment.

[0098] As an exemplary illustration, the value of the first threshold is 0.1 meter.

[0099] In this embodiment, by determining whether lidar data can be fitted into a local small plane based on the lidar feature map, it is ensured that the residual calculation is only performed when the environmental features are obvious and the data is reliable. The inertial measurement data is judged to determine whether it is a saturated measurement, and the residual is calculated only when it is a non-saturated measurement. The residual calculation is only performed when meeting specific criteria through conditional judgment, reducing unnecessary computational overhead and avoiding error propagation caused by noisy data. The Jacobian matrix is used in the residual calculation to accurately describe the influence degree of lidar data and inertial measurement data on the state variables, enabling the state estimation to flexibly adjust itself under complex conditions and improving the adaptability of the system to complex environments.

[0100] In an optional embodiment, the step of obtaining the estimated state by updating the state and covariance at the next moment through the residual includes:

[0101] Using the Kalman gain for the calculated residual to determine the weight of the residual for state correction;

[0102] Updating the state and covariance matrix at the next moment based on the residual with updated weight and performing state propagation.

[0103] Specifically, according to Equation (7) of state propagation and covariance , it can be known that the prior Gaussian distribution is as follows:

[0104]

[0105] From the residual calculation formula, it can be known that:

[0106]

[0107] Integrating Equation (16) and Equation (17) can obtain the posterior distribution of the state :

[0108]

[0109] Where , Equation (18) is a standard quadratic programming problem, and the optimal solution can be calculated:

[0110]

[0111]

[0112] +

[0113]

[0114] Then The updated value of is:

[0115]

[0116] The updated state is used for propagation at the next moment, so it is necessary to calculate the state estimate and the true state the covariance of the error between them

[0117]

[0118]

[0119] where is the projection matrix:

[0120]

[0121]

[0122] According to Equation (21), it can be known that:

[0123]

[0124] The updated state and the covariance matrix are jointly used in the propagation process at the next moment, and the odometer positioning is obtained by continuously iterating and updating the state.

[0125] In this embodiment, by using the Kalman gain to determine the weight of the residual for state correction, the influence of different measurement data on state update is dynamically adjusted. When updating the state and covariance matrix, the uncertainty change of the state can be effectively reflected. By timely adjusting the covariance matrix, the credibility of the future state can be better evaluated, and the stability and robustness of this method in a dynamic environment can be improved.

[0126] In an optional embodiment, the steps of constructing a topographic map of a legged robot by using the point cloud data of a depth camera and the updated odometer include:

[0127] Project the point cloud data collected by the depth camera onto the world coordinate system to obtain the height measurement value (h, ) and calculate the Jacobian matrix of the displacement and rotation of the depth camera relative to the odometer and ;

[0128] Calculate the variance of the height measurement value using the obtained Jacobian matrix , and pass the height measurement value (h, ) through a one-dimensional Kalman filter and the topographic map height estimate Fusion is performed to obtain a new height estimate and the variance of the new height estimate , and its expression is as follows:

[0129]

[0130]

[0131] where, represents the measured value of height, represents the variance of the measured value of height, represents the estimated value of the height of the next-state topographic map, represents the variance of the estimated value of the height of the next-state topographic map.

[0132] Specifically, since the point cloud obtained by the lidar is relatively sparse, it is not conducive to building a high-precision map. Therefore, it is necessary to fuse the denser point cloud generated by the depth camera. Since the depth camera and the lidar are rigidly connected, let the transformation matrix between the depth camera coordinate system C and the lidar coordinate system (the main body is the inertial measurement unit) be . Then the position of the depth camera measurement value in the inertial measurement unit coordinate system is:

[0133] =

[0134] At this time, the odometry positioning obtained by the lidar can be used to project the depth camera measurement value into the world coordinate system

[0135] =

[0136] =

[0137] Update the topographic map using the point cloud

[0138] Map the measurement value from the depth camera onto the topographic map in the world coordinate system. At the (x, y) cell on the topographic map, a height measurement will be obtained. Assuming the external environment is static, the Jacobian matrix of the position and attitude of the depth camera is known as:

[0139]

[0140]

[0141] We get:

[0142]

[0143] In the formula is the covariance matrix of the depth camera. The height measurement is fused with the topographic map height estimation through a Kalman filter to obtain the expression for fusing the calculated Jacobian matrix with the odometer data through the Kalman filter in this embodiment.

[0144] In this embodiment, by fusing the dense point cloud data of the depth camera with the data of the lidar, the accuracy of the topographic map is significantly improved, filling the deficiency of the sparse data of the lidar, making the details of the topographic map richer and more accurate. By using the Kalman filter to fuse the Jacobian matrix with the odometer data, the uncertainty of the measurement can be better evaluated, improving the stability and accuracy of the system during map updating and ensuring the reliability of the map.

[0145] In an optional embodiment, the step of constructing the topographic map of the legged robot further includes:

[0146] Performing ray projection on any point in the topographic map based on the sensor, and finding the points on the ray path where the ray height is lower than the estimated height of the topographic map as unoccluded points, and removing the height values of the unoccluded points.

[0147] This embodiment also reduces the problem that the position of the sensor relative to the map often drifts by calculating the height error between the current depth camera point cloud measurement value and the data of a relatively flat part of the topographic map, and accordingly adjusting the topographic map.

[0148] In this embodiment, through the method of ray projection, the height values of unoccluded points can be effectively identified and removed, thereby reducing the measurement error caused by dynamic obstacles after they move away, ensuring that the information retained in the topographic map is more accurate and reflecting the true terrain features.

[0149] Embodiment 2

[0150] This embodiment proposes a terrain perception system for a legged robot based on multi-sensor fusion, applying the terrain perception method for a legged robot based on multi-sensor fusion proposed in Embodiment 1. As Figure 4 shown, it is the architecture diagram of the terrain perception system for a legged robot based on multi-sensor fusion in this embodiment.

[0151] A terrain perception system for a legged robot based on multi-sensor fusion, the system includes:

[0152] Data acquisition and processing module: used to acquire lidar data and inertial sensor data and input them into the manifold Kalman filter;

[0153] Kalman Filter Module: It fuses the input lidar data and inertial sensor data,

[0154] and derives the estimated state based on the fused data;

[0155] Odometer Update Module: It uses the estimated state as odometer data;

[0156] Topographic Map Construction Module: It constructs the topographic map of the legged robot by using the point cloud data of the depth camera and the updated odometer.

[0157] It can be understood that the system in this embodiment corresponds to the method in Embodiment 1 above. The optional items in Embodiment 1 above are also applicable to this embodiment, so they will not be described repeatedly here.

[0158] The terms in the drawings are only for illustrative purposes and should not be construed as limitations of this patent;

[0159] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, rather than limitations on the implementation manners of the present invention. For those of ordinary skill in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to enumerate all the implementation manners here. 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 claims of the present invention.

Claims

1. A terrain perception method for a legged robot based on multi-sensor fusion, characterized in that: include: Collect the lidar data and inertial sensor data of the legged robot and fuse them through the manifold Kalman filter, and derive the estimated state based on the fused data; using the estimated state as odometer data; constructing a terrain map of the legged robot using the depth camera point cloud data of the legged robot and the odometry data; The step of fusing by the manifold Kalman filter comprises: In the manifold Kalman filter, based on the respective sampling times of the lidar data and the inertial sensor data, the lidar data is matched point by point using the inertial sensor data and merged; The step of deriving the estimated state based on the fused data includes: Simulate inertial sensor data based on random processes and extend it to kinematics to build a state transition model; build a measurement model based on inertial sensor data and lidar data; Predicting the state and covariance at the next moment using the state transition model; The measurement data at the current moment is obtained through the measurement model, and the residual is calculated based on the state and covariance at the next moment, and the state and covariance at the next moment are updated through the residual to obtain the estimated state.

2. The method for terrain perception of a legged robot based on multi-sensor fusion according to claim 1, characterized in that: The method of simulating inertial sensor data based on random processes and extending it to kinematics to construct a state transition model includes the following steps: The first frame of inertial sensor data is used as the origin of the world coordinate system. The angular velocity and acceleration are modeled using the random deviation and Gaussian noise of the inertial sensor. The model is discretized to obtain a state transition model. The expression of the state transition model is as follows: in, 、 Represent the attitude, position and speed of the odometer in the world coordinate system respectively, Represents the gravity vector in the world coordinate system; 、 Respectively represent the angular velocity and acceleration in the odometer coordinate system, Represents the antisymmetric matrix constructed from the angular velocity vector; Gaussian noise is the random deviation of the inertial measurement unit; Gaussian noise ; is the rate of change of angular velocity and acceleration, which conforms to Gaussian distribution; k represents the time.

3. The method for terrain perception of a legged robot based on multi-sensor fusion according to claim 1, characterized in that: The step of constructing a measurement model based on inertial sensor data and lidar data includes: Based on the known mechanical structure, the static transformation relationship between the inertial sensor and the lidar is obtained, and the lidar points are projected into the world coordinate system. The expression of the measurement model is as follows: in, 、 is Gaussian noise; is the state vector; is the position of the lidar point in the inertial measurement unit coordinate system; and are the angular velocity and acceleration in the inertial sensor.

4. The method for terrain perception of a legged robot based on multi-sensor fusion according to claim 1, characterized in that: The predicted state at the next moment includes a laser radar prediction value and an inertial measurement prediction value; and the step of calculating the residual includes: For the laser radar prediction value, the data and the adjacent points are fitted into a local facet based on the laser radar feature map, and it is determined whether the distance between the adjacent points and the local facet is less than a first threshold. If so, a residual is calculated for the laser radar prediction value, otherwise no residual is calculated; For the inertial measurement prediction value, first determine whether the difference between the inertial measurement prediction value and the saturation value is greater than a preset threshold. If so, calculate the residual of the inertial measurement prediction value, otherwise do not calculate the residual; The residual The calculation formula is as follows: in, is the Jacobian matrix, which represents the relationship between the lidar data or inertial measurement data and the state variables The partial derivative of Represents the measurement noise of lidar data or inertial measurement data The partial derivative of .

5. The method for terrain perception of a legged robot based on multi-sensor fusion according to claim 1, characterized in that: The step of performing state updating on the state and covariance at the next moment by using the residual to obtain the estimated state includes: The Kalman gain is used to determine the weight of the residual on the state correction. Based on the residual after updating the weights, the state and covariance matrix of the next moment are updated and the state is propagated.

6. The method for terrain perception of a legged robot based on multi-sensor fusion according to any one of claims 1 to 5, characterized in that: The step of constructing a terrain map of the legged robot using the point cloud data of the depth camera and the updated odometer includes: Project the point cloud data collected by the depth camera into the world coordinate system to obtain the height measurement value (h, ) and calculate the Jacobian matrix of the depth camera's displacement and rotation relative to the odometer and ; The obtained Jacobian matrix is used to calculate the variance of the height measurement value , the height measurement value (h, ) is estimated by using a one-dimensional Kalman filter and terrain map height Fusion, get a new height estimate and the variance of the new height estimate , which is expressed as follows: in, A measurement of height, represents the variance of the height measurements, represents the estimated value of the next state terrain map height, Indicates the variance of the next-state terrain height estimate.

7. The method for terrain perception of a legged robot based on multi-sensor fusion according to claim 6, characterized in that: The step of constructing a terrain map of the legged robot further comprises: Perform ray projection based on the sensor for any point in the terrain map, and find the point on the ray path where the ray height is lower than the estimated height of the terrain map as the unobstructed point, and remove the height value of the unobstructed point.

8. A multi-sensor fusion-based terrain perception system for a legged robot, applied to the multi-sensor fusion-based terrain perception method for a legged robot according to any one of claims 1 to 7, characterized in that: The system comprises: Data acquisition and processing module: used to collect lidar data and inertial sensor data and input them into the manifold Kalman filter; Kalman filter module: fuses the input lidar data and inertial sensor data, Deriving an estimated state based on the fused data; Odometer update module; uses the estimated state as odometer data; Terrain map construction module: Uses the point cloud data from the depth camera and the updated odometry to build a terrain map for the legged robot.

Images