A mapping and navigation method for legged robots based on binocular visual odometry

Abstract

An embodiment of the present invention discloses a mapping and navigation method for a legged robot based on binocular visual odometry, which relates to the technical field of multi-legged mobile robots and is capable of autonomous navigation of the legged robot even after leaving the effective signal range of the GPS. The present invention includes: capturing image frames using a binocular camera; extracting feature points of the image frames at each time stamp and establishing a camera coordinate system, and then using the extracted feature points and the camera coordinate system to obtain odometry information; obtaining key frames during the movement of the legged robot; further establishing a sub-map, creating a map based on the sub-map, planning a route in the map, and navigating according to the planned route. The designed image processing method prevents the image effects captured by the legged robot during its fluctuating state from affecting the calculated odometry.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-legged mobile robots, and in particular to a mapping and navigation method for a legged robot based on binocular visual odometry. Background Art

[0002] Since the 21st century, both domestic and international attention has been focused on the development of robotics, which is considered one of the high technologies crucial to the development of future emerging industries. Mobile robots, in particular, have a wide range of applications, encompassing ground, air, underwater, and even outer space. Wheeled robots utilize technologies such as dead reckoning, computer vision, landmark recognition, wireless positioning, and SLAM for positioning, enabling map-based path planning and motion control. Legged mobile robots, on the other hand, are designed to mimic the locomotion of mammals through research in system design, gait planning, and stability. Compared to wheeled robots, legged robots offer greater adaptability and a more complex range of applications. They can easily navigate various obstacles, possessing excellent freedom of movement, flexibility, ease, and stability.

[0003] Legged robots have long lacked effective methods for pose estimation, aside from GPS positioning and navigation. The main challenge is that while wheeled robots can estimate their travel paths through methods like wheeled odometry, most existing odometry solutions are suitable for wheeled and tracked robots, relying on axle sensors on the drive wheels to calculate mileage. However, these solutions are difficult to effectively apply to legged robots. Therefore, how to achieve autonomous navigation for legged robots outside the effective GPS signal range has become a research challenge. Summary of the Invention

[0004] An embodiment of the present invention provides a mapping and navigation method for a legged robot based on a binocular visual odometry, which can achieve autonomous navigation of the legged robot even after leaving the effective signal range of the GPS.

[0005] To achieve the above objectives, the embodiments of the present invention adopt the following technical solutions:

[0006] S1, collect image frames through binocular camera;

[0007] S2. Extract feature points of the image frames at each time stamp and establish a camera coordinate system, and then obtain odometer information using the extracted feature points and the camera coordinate system;

[0008] S3. Acquire key frames during the movement of the legged robot;

[0009] S4, filtering and estimating the odometer information through an extended Kalman filter to obtain odometer correction data;

[0010] S5. Acquire environmental perception data through a laser radar, wherein the environmental perception data includes: distance data between the legged robot and obstacles;

[0011] S6. Using the odometer correction data obtained in S4, the environmental perception data obtained in S5, and the data collected by the inertial measurement unit, a submap for underlying motion control is established, wherein the data collected by the inertial measurement unit includes acceleration measurement data and angular velocity measurement data;

[0012] S7. Optimize and update the sub-map to obtain a map.

[0013] The legged robot mapping and navigation method based on binocular visual odometry provided by the embodiment of the present invention can realize mapping and navigation of the legged robot, as well as autonomous movement and path planning in the range without GPS signals. Through the designed image processing method, the image effect captured by the fluctuating state during the movement of the legged robot is prevented from affecting the calculation of the odometry. The processing method of binocular visual odometry data is optimized, thereby improving the accuracy of the odometry data and optimizing the mapping effect. The radar data is filtered to prevent the influence of noise on the mapping. Without the need for additional sensors, the intelligence of the legged robot is greatly improved while reducing costs and improving production efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0015] Figure 1 A flow chart for designing a binocular visual odometry based on an optimization algorithm provided by an embodiment of the present invention;

[0016] Figure 2 A flow chart for designing a binocular visual odometer based on a filtering algorithm provided by an embodiment of the present invention;

[0017] Figure 3 A block diagram of a visual odometry system with weighted calculation provided by an embodiment of the present invention;

[0018] Figure 4 A block diagram of a legged robot mapping and navigation system based on visual odometry provided in an embodiment of the present invention;

[0019] Figure 5 A schematic diagram of a method flow chart provided in an embodiment of the present invention;

[0020] Figure 6 A schematic diagram of the interaction mode of the ROS platform provided in an embodiment of the present invention;

[0021] Figure 7 A schematic diagram of a submap processing method provided in an embodiment of the present invention;

[0022] Figure 8 A schematic diagram of an example of a possible submap provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0023] To enable those skilled in the art to better understand the technical solutions of the present invention, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments. The embodiments of the present invention will be described in detail below, with examples of the embodiments illustrated in the accompanying drawings. Throughout, identical or similar reference numerals represent identical or similar elements or elements having identical or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and intended only to explain the present invention and are not to be construed as limiting the present invention. Those skilled in the art will appreciate that, unless otherwise stated, the singular forms "a," "an," "said," and "the" used herein may also include the plural forms. It should be further understood that the term "comprising" as used in the description of the present invention refers to the presence of the stated features, integers, steps, operations, elements, and / or components, but does not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. It should be understood that when an element is referred to as being "connected" or "coupled" to another element, it may be directly connected or coupled to the other element, or intervening elements may be present. Furthermore, "connected" or "coupled" as used herein may include wireless connections or couplings. The term "and / or" as used herein includes any and all combinations of one or more associated listed items. It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by those skilled in the art in the art to which the present invention belongs. It should also be understood that terms such as those defined in general dictionaries should be understood to have meanings consistent with their meanings in the context of the prior art, and will not be interpreted in an idealized or overly formal sense unless defined as such herein.

[0024] The advantage of legged robots lies in their superior terrain adaptability, enabling them to navigate more complex terrains than wheeled and tracked robots. The hardware required to calculate odometry data for wheeled and tracked robots is already quite mature, with relatively high accuracy. However, for legged robots, there has been a lack of robust hardware to support odometry calculations. Therefore, this paper investigates methods for visual odometry and applies them to mapping and navigation algorithms.

[0025] The embodiment of the present invention provides a mapping and navigation method for a legged robot based on binocular visual odometry, such as Figure 5 Shown, including:

[0026] S1. Collect image frames through binocular camera.

[0027] Among them, such as Figure 4 As shown, the legged robot is equipped with a visual odometry module, a low-level motion module, and an intelligent control module. The visual odometry module includes binocular cameras mounted on the legged robot, each consisting of a left camera and a right camera. The low-level motion module includes an inertial measurement unit (IMU), which is used to acquire acceleration and angular velocity data of the legged robot. The legged robot is also equipped with a laser radar (LIDAR) for acquiring distance data between the legged robot and obstacles in its surroundings. The captured image frames include left-eye grayscale images captured by the left camera and right-eye grayscale images captured by the right camera. The legged robot platform is equipped with a forward-looking binocular camera that captures data at a rate of 20 frames per second. The IMU measures acceleration and angular acceleration at a rate of 200 Hz. The LIDAR collects data on surrounding obstacles. In the absence of GPS signal coverage, the robot can use the SLAM mapping method to build a global map using odometry data obtained from machine vision and perform navigation.

[0028] S2. Extract feature points of the image frames at each time stamp and establish a camera coordinate system. Then, use the extracted feature points and the camera coordinate system to obtain odometer information.

[0029] The camera coordinate system can be understood as a spatial coordinate system established with the camera body as the origin.

[0030] S3. Acquire key frames during the movement of the legged robot.

[0031] S4. Filter and estimate the odometer information through an extended Kalman filter to obtain odometer correction data.

[0032] The extended Kalman filter (EKF) is an algorithm deployed in the robot's embedded controller. The general process of filtering estimation involves recalculating and eliminating accumulated errors. Specifically, filtering algorithms are used to reduce the accumulated errors caused by the quadruped robot's motion.

[0033] The purpose of data processing using extended Kalman filtering in this embodiment is to estimate the next state based on the current state, for example: Figure 1As shown, this embodiment uses two state vectors, the IMU state vector and the camera state vector, to construct an extended Kalman filter model. The input is the IMU state vector and camera state vector at different times (the vector includes the position p and the attitude quaternion q). The feature points form geometric constraints between multiple camera states, and then use the geometric constraints to construct an observation model to update the Kalman filter model. The latest camera state obtained is the predicted position and pose of the camera, that is, the odometry information. Its characteristic is that the number of camera states always remains unchanged, and the elimination of historical states can greatly reduce the amount of calculation and avoid the consequences of gradient explosion.

[0034] Considering that the visual odometry based on filtering has a fast processing speed but low accuracy, while the optimization method based on nonlinear optimization has a slow processing speed but high accuracy, the real data of the odometry has been processed and expressed using a weighted method in the past period of time. Where Odom represents the final odometer data; Odom nol are odometer data based on Kalman filtering and odometer data based on nonlinear optimization respectively; n represents the number of odometer data obtained by the Kalman filtering method per unit time.

[0035] For example, the design steps of the binocular visual odometer based on the filtering algorithm used in this embodiment are as follows: Figure 2 As shown: 1. First, initialize the state vector and covariance; 2. Then perform IMU integration, and the state vector and covariance change; 3. Add the new camera state to the state vector and expand the covariance matrix; 4. Perform observation update, and all states and covariances after the second observation change; 5. When the number of camera states reaches the threshold, remove the relatively original data; 6. Repeat 2 to 5.

[0036] Afterwards, if Figure 1 As shown in the figure, the arithmetic mean filtering method can be used to smooth the data. The specific method is to take N consecutive data for arithmetic averaging operation, where N should not be too large, which will affect the normal data collection, nor too small, which will not achieve the filtering effect. After processing, the radar data will reduce the interference of noise and greatly reduce the amount of data, which has a good optimization effect on the algorithm.

[0037] S5. Acquire environmental perception data through a laser radar, where the environmental perception data includes: distance data between the legged robot and obstacles.

[0038] S6. Using the odometer correction data obtained in S4, the environmental perception data obtained in S5, and the data collected by the inertial measurement unit, a submap for underlying motion control is established.

[0039] The data collected by the inertial measurement unit includes acceleration measurement data and angular velocity measurement data. Figure 7 As shown, processing is based on the principle of submaps. Each time a laser scan (i.e., distance data) is obtained, it is matched with the most recently created submap, so that the laser scan data for that frame is inserted into the optimal position on the submap. (Here, the Gauss-Newton least squares problem is used.) As new data frames are continuously inserted, the submap is updated. A certain amount of data is combined into a submap. When no new scans are inserted into the submap, the submap is considered complete, and the next submap is created. All submaps form a global map with inaccurate scale information. This "global map with inaccurate scale information" serves as the submap established in S6 for low-level motion control. The inaccurate scale information needs to be addressed through subsequent updates and iterations, namely S7.

[0040] The submap mentioned in this embodiment is created by several consecutive lidar data (scans), such as Figure 8 As shown, the probability grid of 5cm*5cm size Constructed, when the submap is created, the grid probability is less than p min Indicates that there is no obstacle at this point. min With p max Between means unknown, greater than p max Indicates that there is an obstacle at that point. Multiple radar data will fill the location area and update the known area.

[0041] In this embodiment, the odometer correction data information can be sent through the ROS platform, specifically to the final control unit for use in subsequent map building steps. Specifically, the ROS platform is deployed on the robot controller to receive and send messages from each part. All sensor data can be sent to the final processing control unit through the ROS platform. For example Figure 6 The figure shows a partial message node flow chart of the ROS platform in this embodiment. First, the binocular camera obtains two grayscale images of the left and right frames at the same time. After pose estimation, approximate odometry data is obtained. Then, after loop closure detection and error reduction, more accurate odometry data is obtained. Then, the cartographer algorithm is used to construct a map in combination with the lidar information.

[0042] S7. Optimize and update the sub-map to obtain a map.

[0043] Among them, optimizing and updating the sub-map can be understood as: the sub-map is continuously iterated, and the spatial points passed through twice are continuously determined, and finally a usable "map" is iterated. The final map can be used to plan the route and navigate according to the planned route. The optimizing and updating of the sub-map and obtaining the map include: the multi-legged robot starts to move using the sub-map, and during the movement, the multi-legged robot passes by the same spatial point in the map for the second time in real time and records it as a revision point; in each update cycle, at least one revision point is obtained and recorded in the sub-map; when the revision points in the sub-map stop increasing, the update is stopped and the sub-map with the last update is used as the map.

[0044] In practical applications, the determination that the multi-legged robot passes by the same spatial point in the map for the second time includes: if the image frame currently captured by the binocular camera and the key frame previously captured have at least 10 identical feature points, then they are determined to be the same spatial point.

[0045] The sub-map is converted from the ROS platform's inherent node (cartographer_occupancy_grid_node) to the ROS GridMap grid map format for subsequent navigation. The target point here is not the same as the target point mentioned above. It refers to the navigation point selected by the user after the map is built. Specifically, the SLAM mapping method can be used to restore the three-dimensional environment features, build a global map, and then plan the route in real time based on the built map.

[0046] Specifically, in S2, extracting feature points of the image frames at each time stamp includes: performing feature matching on the extracted feature points.

[0047] Among them, if the descriptors of two feature points are close in distance in vector space, they are merged into the same feature point. The information of the feature points of an image frame includes key points and descriptors. The key points represent the position, direction and scale information of the feature points in the image, and the descriptors represent the information of the pixels around the feature points. Specifically, in order to better perform image matching, it is necessary to select representative areas in the image, that is, feature points. The feature point information in an image is composed of key points and descriptors. The key points represent the position, direction and scale information of the feature points in the image, and the descriptors represent the information of the pixels around the feature points. When performing feature matching, if the descriptors of two feature points are close in distance in vector space, they are considered to be the same feature point. Furthermore, in S2, the odometry information is obtained by using the extracted feature points of the image frame and the obtained three-dimensional coordinate points, including: obtaining the three-dimensional coordinate points of the feature points at different times in the camera coordinate system, and establishing a rotation and translation transformation matrix (RT).

[0048] The binocular camera's motion trajectory in space is obtained through the rotation-translation transformation matrix. The odometry information includes the parameters of the motion trajectory, including the rotation direction and translation distance. In practical applications, the obtained rotation-translation transformation matrix also indicates the camera's motion trajectory in space, including the rotation direction and translation distance, and thus obtains the camera's odometry information since the start. Since the relative positions of the camera and the quadruped robot's moving body remain unchanged, the quadruped robot's odometry information can be obtained. Considering the computational complexity of embedded devices, it is necessary to select appropriate key frames as the calculation of the transformation matrix between adjacent frames.

[0049] Specifically, in S3, obtaining key frames during the movement of the legged robot includes:

[0050] During the movement of the legged robot, image frames captured during various time periods are acquired and stored as image frame sets corresponding to each time period. Within each image frame set, descriptor information of feature points in each image frame is extracted, and key frames for each image frame set are selected based on the distance between the descriptor information in a spatial vector.

[0051] The feature matching of the extracted feature points includes: extracting feature points of the left eye grayscale image and the right eye grayscale image at the same time stamp, and performing feature matching. Figure 1As shown in the figure, after feature matching, triangulation and the Perspective-n-Point (PnP) algorithm are used to obtain the three-dimensional coordinates of the objects captured within the binocular camera's field of view in the camera coordinate system. For the same feature point, the RT rotation and translation transformation matrices of the three-dimensional coordinates at different times in the camera coordinate system are calculated. Triangulation calculates the depth information of the same feature point in the left and right cameras. PnP (Perspective-n-Point) is a method for solving 3D to 2D point pair motion.

[0052] Specifically, in the process of obtaining the three-dimensional coordinate points of various objects captured in the field of view of the binocular camera in the camera coordinate system, it includes: estimating the posture T of the current image frame relative to the previous image frame each time cr , then the coordinates of the current frame relative to the world coordinate system are: T cw =T cr T rw , where the coordinate of the previous frame relative to the world coordinate system is T rw .

[0053] Wherein, the PNP algorithm includes:

[0054] The homogeneous coordinates of the spatial point P are P = (X, Y, Z, 1), where X, Y, and Z are the coordinates of the xyz axes respectively. The feature point projected into image 1 is x1 = (u1, v1, 1). T , where u1 represents the result parameter after normalization of the spatial coordinate x-axis, and v1 represents the result parameter after normalization of the spatial coordinate y-axis. The normalized coordinate under camera 1 is x1=(u1,v1,1), and the normalized coordinate can be converted from the pixel coordinate p1: x1=K-1 p1;.

[0055] Among them, the position of the binocular camera is [R|t], R represents the rotation matrix, t represents the translation vector, and we can express the projection relationship in an expanded form, where s is the Z-axis value of the three-dimensional coordinate of point P in the camera coordinate system, that is, the depth of point P:

[0056]

[0057] Eliminate s using the last line and obtain the constraint:

[0058]

[0059] The row vectors are defined as:

[0060] t1=(t1,t2,t3,t4) T ,t2=(t5,t6,t7,t8) T ,t3=(t9,t 10 ,t11 ,t 12 ) T

[0061] So we have:

[0062]

[0063] Assuming there are n points, we can construct a linear equation system as follows:

[0064]

[0065] Since t has 12 dimensions, a total of 6 pairs of matching points are required to achieve the linear solution of the above equation. Alternatively, methods such as SVD (Singular Value Decomposition) can be used to obtain the least squares solution.

[0066] In this embodiment, a method based on two frames is used to estimate the pose, that is, each time the pose T of the current frame relative to the previous frame is estimated. cr , we know that the coordinate of the previous frame relative to the world coordinate system is T rw , then the coordinates of the current frame relative to the world coordinate system are:

[0067] T cw =T cr T rw

[0068] Furthermore, in the process of navigating according to the planned route, the following steps are further included:

[0069] Detect whether the image frame currently captured by the binocular camera overlaps with the key frame captured previously. If so, determine that the multi-legged robot passes by the same spatial point in the map for the second time, and reposition the multi-legged robot.

[0070] The current pose of the multi-legged robot is calculated using the TransForm algorithm. For example, if the multi-legged robot passes the same spatial point and the frames captured by the binocular camera overlap with the keyframes at the previous time, the robot will be repositioned and the current pose of the robot will be calculated using the TransForm algorithm.

[0071] In this embodiment, the bag-of-words model can be used to perform loop closure detection on the image to relocate the robot. The specific steps are as follows:

[0072] During the robot's motion, if it reaches a path it has previously traveled, that is, if the image captured by the camera matches a frame in the stored image library, the established odometer data will be re-evaluated to construct a loop detection. Specifically, it includes:

[0073] Build a visual dictionary tree and establish a sequential index and reverse index from the initial moment to the current image. First, it is necessary to extract features from historical images and cluster the extracted feature descriptions (similar features are placed in the same path). The role of the dictionary tree is to enrich the content of the dictionary and query whether the feature information of the current image already exists.

[0074] The method for establishing the visual dictionary tree in this embodiment includes: extracting the feature points of the current frame, calculating the corresponding feature descriptors, and obtaining a bag-of-words vector describing the frame image; using the reverse index on the visual dictionary tree to find a series of images with the same words as the current image as candidate images for loop detection; then calculating the similarity between the current image and the candidate image (the previous image), and taking the one with the highest similarity as the loop pair; finally, verifying the obtained loop pair to check whether it is a correct loop. In this process, the odometry data can be estimated independently of the loop detection results, that is, the global estimation of loop detection will be affected by VIO, but VIO is not affected by the global estimation.

[0075] For example, Figure 1 As shown, a binocular camera is installed on the robot. The binocular camera includes a left camera and a right camera. The left and right grayscale images obtained by the left and right cameras are used to generate RGBD images to directly obtain the depth information of the target point. Specifically, an IMU (inertial measurement unit) can be used to obtain acceleration and angular velocity data. A lidar is used to obtain distance data of surrounding obstacles.

[0076] In practical applications, such as Figure 1 As shown in the figure, the image output by the binocular camera needs to be smoothed to prevent the ups and downs of the legged robot during movement from affecting the subsequent data processing results. The method is to limit the output image. For example, if the input image size is 1024*768, the image height is reduced by the upper and lower limits according to the size of the legged robot's steps, and finally modified to 1024*720;

[0077] The method of obtaining the feature points of the image captured by the binocular camera can be understood as follows:

[0078] The ORB algorithm is used to extract feature points from an image. It can quickly identify and find all feature points in an image and add descriptors to them (using a binary representation method to describe the uniqueness of the feature points). Its characteristics are that it uses region segmentation and the NMS (Non-Maximum Suppression) method to limit the number of feature points in each region of the image to prevent the clustering of feature points. At the same time, all feature points are sorted according to the corresponding values of the corner points (i.e., the feature prominence), and the top N corner points are selected as the feature point set of a single image. The feature points selected by this method are also scale-invariant and rotation-invariant, which greatly improves the uniqueness of the feature points.

[0079] Select the frame with more feature points than the set value as the key frame. The number of feature points depends on the captured image. If the image is pure color, there will be no feature points. The key frame with the largest number of feature points is the reference frame. If there are several reference frames in the same time period, the key frame closest to the previous frame on the time axis is selected from the reference frames as the reference frame.

[0080] Initialize the binocular camera and IMU. The operation process first triangulates the current frame and the first frame to solve the pose in the spatial coordinate system, and then uses the 3d-2d: Pnp (Perspective-n-Point) method to solve the pose of the feature points in each frame. Then, initialize the angular velocity zero bias of the IMU, use the camera rotation and IMU integral rotation between two adjacent frames, and then use common technical means to construct the least squares problem and solve it.

[0081] During the robot's movement, the binocular camera can directly obtain depth images due to its own structure, so it can directly obtain the three-dimensional coordinates of the feature points. Then, at least three pairs of three-dimensional coordinates of feature points and two-dimensional coordinates on the image are obtained, and the three-dimensional coordinates of each feature point in the camera coordinate system are finally calculated using the triangle correspondence relationship. Finally, the rotation and translation matrix during the movement process is solved using the given two paired 3D points. According to the principle of relative motion, the motion trajectory of the camera (which can also be directly understood as the legged robot body) can be obtained.

[0082] This embodiment discloses a method for autonomous mapping and navigation of a legged robot based on binocular visual odometry, relating to the field of intelligent walking device technology. The method can achieve robot mapping and navigation functions while reducing hardware costs. The method incorporates a forward-looking binocular camera, a lidar radar, a high-precision inertial measurement unit (IMU), and two embedded control boards on the legged robot platform. The legged robot's mapping uses a lidar-based SLAM (Simultaneous Localization and Mapping) method to construct an indoor planar map. Unlike wheeled robots, legged robots cannot directly access odometry information, so visual odometry information is used as an alternative. SLAM mapping is achieved using visual odometry data, IMU data, and lidar data. Subsequently, sensors such as the binocular camera, IMU, and lidar are used to navigate within the previously constructed map. This method enables the legged robot, in the absence of a wheeled odometry system, to use odometry data obtained by processing binocular camera image data as one of its inputs, in conjunction with IMU data and lidar data, to achieve real-time pose estimation and environmental perception in non-GPS environments, significantly improving the legged robot's intelligence.

[0083] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment. The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited to this. Any changes or replacements that can be easily thought of by any technician familiar with this technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A mapping and navigation method for a legged robot based on binocular visual odometry, characterized in that: include: S1, collect image frames through binocular camera; S2. Extract feature points of the image frames at each time stamp and establish a camera coordinate system, and then obtain odometer information using the extracted feature points and the camera coordinate system; S3. Acquire key frames during the movement of the legged robot; S4, filtering and estimating the odometer information through an extended Kalman filter to obtain odometer correction data; S5. Acquire environmental perception data through a laser radar, wherein the environmental perception data includes: distance data between the legged robot and obstacles; S6. Using the odometer correction data obtained in S4, the environmental perception data obtained in S5, and the data collected by the inertial measurement unit (IMU), a submap for underlying motion control is established, wherein the data collected by the inertial measurement unit includes acceleration measurement data and angular velocity measurement data; S7. Optimizing and updating the sub-map to obtain a map; wherein the resulting map is used to plan a route and navigate according to the planned route; In the process of filtering and estimating the odometry information through the extended Kalman filter, two state vectors, including the IMU state vector and the camera state vector, are selected to construct the extended Kalman filter model. The input is the IMU state vector and the camera state vector at different times. Among them, the feature points form geometric constraints between multiple camera states, and then the observation model is constructed using the geometric constraints to update the Kalman filter model. The latest camera state obtained is the predicted camera pose as the odometry information. The real data processing method of the odometry is: ,in Represents the final odometer data; , are the odometer data based on Kalman filtering and the odometer data based on nonlinear optimization respectively; n represents the number of odometer data obtained by the Kalman filtering method per unit time; In the process of processing using the principle of submap, every time distance data is obtained, it is matched with the most recently established submap, so that the distance data of this frame is inserted into the optimal position on the submap. The submap is updated while new data frames are continuously inserted. When no new distance data is inserted into the submap, it is considered that the submap has been created. A global map with inaccurate scale information is constructed using all submaps, and the global map with inaccurate scale information is used as the submap for underlying motion control established in S6, which includes a probability grid of 5cm*5cm size. , the grid probability is less than the minimum value Indicates that there is no obstacle at this point. With the maximum value Between means unknown, greater than Indicates that there is an obstacle at that point; Then, the global map with inaccurate scale information is updated iteratively, wherein the submap is optimized and updated, including: The legged robot starts to move using the sub-map, and during the movement, detects in real time that the legged robot passes by the same spatial point in the map for the second time and records the point as a revision point; In each update cycle, obtaining at least one revision point and recording it in the sub-map; When the revision points in the sub-map stop increasing, the updating is stopped and the sub-map updated last is used as the map.

2. The method according to claim 1, characterized in that A visual odometer module, a bottom motion module and an intelligent control module are installed on the legged robot; The visual odometry module includes a binocular camera installed on the legged robot, and the binocular camera includes a left camera and a right camera; The bottom motion module includes an inertial measurement unit, which is used to obtain acceleration data and angular velocity data of the legged robot; the legged robot is also equipped with a laser radar, which is used to obtain distance data between the legged robot and obstacles; The captured image frames include: image frames of left-eye grayscale images captured by the left-eye camera and image frames of right-eye grayscale images captured by the right-eye camera.

3. The method according to claim 1 or 2, characterized in that In S2, obtaining odometer information by using the extracted feature points of the image frame and the acquired three-dimensional coordinate points includes: Obtain the three-dimensional coordinates of the feature points at different times in the camera coordinate system and establish a rotation and translation transformation matrix; The motion trajectory of the binocular camera in space is obtained through the rotation and translation transformation matrix, and the odometer information includes parameters of the motion trajectory, and the parameters of the motion trajectory include rotation direction and translation distance.

4. The method according to claim 1, wherein In S2, extracting feature points of image frames at respective timestamps includes: Feature matching is performed on the extracted feature points. If the descriptors of two feature points are close to each other in the vector space, they are merged into the same feature point. The information of the feature points of an image frame includes key points and descriptors. The key points represent the position, direction and scale information of the feature points in the image, and the descriptors represent the information of the pixels around the feature points.

5. The method according to claim 4, characterized in that In S3, during the movement of the legged robot, obtaining key frames includes: During the movement of the legged robot, image frames captured in various time periods are acquired and stored as a set of image frames corresponding to each time period; In each image frame set, descriptor information of feature points in each image frame is extracted, and key frames of each image frame set are filtered according to the distance of the descriptor information in the space vector.

6. The method according to claim 4, characterized in that The feature matching of the extracted feature points includes: extracting feature points of the left-eye grayscale image and the right-eye grayscale image at the same time stamp, and performing feature matching; After feature matching, the three-dimensional coordinate points of the objects captured in the field of view of the binocular camera in the camera coordinate system are obtained through triangulation and PNP algorithm.

7. The method according to claim 1 or 6, characterized in that The process of obtaining the three-dimensional coordinate points of various objects captured within the field of view of the binocular camera in the camera coordinate system includes: Each time the pose of the current image frame relative to the previous image frame is estimated 𝑐𝑟 , then the coordinates of the current frame relative to the world coordinate system are: , where the coordinate of the previous frame relative to the world coordinate system is 𝑇 𝑟𝑤 .

8. The method according to claim 1, characterized in that The process of navigating according to the planned route also includes: detecting whether an image frame currently captured by the binocular camera overlaps with a key frame previously captured, and if so, determining that the legged robot passes by the same spatial point in the map for the second time, and repositioning the legged robot; The position and posture of the current legged robot are calculated using the TF algorithm.

9. The method according to claim 8, characterized in that The determining that the legged robot passes by the same spatial point in the map for the second time includes: If the image frame currently captured by the binocular camera and the key frame previously captured have at least 10 identical feature points, they are determined to be the same spatial point.

Images