Online multi-source sensor extrinsic parameter calibration method and system for legged robot

Abstract

The application belongs to the technical field of leg-foot robots, and provides an online multi-source sensor external parameter calibration method and system for a leg-foot robot. The method is applied to the movement of the leg-foot robot, and comprises camera image acquisition, three-dimensional reconstruction according to the pose and image flow feature points of each frame of image, and obtaining three-dimensional reconstruction constraints of the camera; pre-integration is performed by using the motion information of the IMU to obtain IMU constraints; the external parameters between the camera and the IMU are calculated according to the three-dimensional reconstruction constraints of the camera and the IMU constraints; the visual feature points after three-dimensional reconstruction are fitted to obtain the geometric feature properties of the visual plane; the laser point cloud is projected to the camera image by the laser radar, the visual feature points of the visual plane are clustered by the laser point cloud, and the geometric feature properties of the point cloud plane after clustering are calculated; and the external parameters between the camera and the laser radar are calculated according to the geometric feature properties of the visual plane and the geometric feature properties of the point cloud plane.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of leg-foot robot, and particularly relates to an online multi-source sensor extrinsic parameter calibration method and system for a leg-foot robot. BACKGROUND

[0002] The statements in this section merely provide background information related to the present application and do not necessarily constitute prior art.

[0003] When a leg-foot robot works in a specific scene, it needs to use the Simultaneous Localization and Mapping (SLAM) technology to realize the acquisition of environmental information and the estimation of its own state. After the robot acquires the positions of surrounding obstacles, its own motion state, and body positioning information, it performs path planning, navigation movement, and local obstacle avoidance, etc. The implementation of the above technologies relies on the accurate description of environmental information by multiple sensors (such as multi-line laser radar, binocular depth camera, Inertial Measurement Unit (IMU), etc.), and the complementary solution to the deficiencies and defects of single sensor devices in representing environmental information through data fusion processing. For example, laser radar combined with camera increases color information to dye point cloud, camera realizes scale estimation and motion parameter acquisition through IMU, and laser radar combined with IMU data realizes data motion distortion correction. Before performing sensor data fusion, the installation extrinsic parameters between multiple devices need to be accurately measured.

[0004] However, the existing multi-source sensor extrinsic parameter calibration method often needs to use a specific sensor calibration board and large-scale offline optimization calculation, which is time-consuming and cumbersome, and only gives the extrinsic parameters between two sensors (such as camera and laser radar, camera and inertial measurement unit) during calibration. The disturbance of the sensor data caused by the interaction between the foot end and the ground during the movement of the leg-foot robot will lead to inaccurate extrinsic parameter calibration results, and further affect the positioning and state estimation information acquisition of the leg-foot robot. SUMMARY

[0005] In order to solve the above problems, the present application proposes an online multi-source sensor extrinsic parameter calibration method and system for a leg-foot robot. The present application does not depend on specific calibration board references and offline large-scale calculations, and can obtain the calibration results of three sensors (laser radar, camera, and inertial measurement unit), which has the advantages of online calibration calculation and resistance to the disturbance of foot-ground interaction impact of the leg-foot robot.

[0006] According to some embodiments, the present application adopts the following technical solutions:

[0007] The first aspect of the present application provides an online multi-source sensor extrinsic parameter calibration method for a leg-foot robot.

[0008] The online multi-source sensor extrinsic parameter calibration method for a leg-foot robot is applied in the moving process of the leg-foot robot, and comprises the following steps:

[0009] The camera field of view is directed towards a reference surface and rotates to collect images, three-dimensional reconstruction is performed according to the pose and image flow feature points of each frame of image, and three-dimensional reconstruction constraints of the camera are obtained; pre-integration is performed by using the motion information of the IMU, and IMU constraints are obtained; and the extrinsic parameters between the camera and the IMU are calculated according to the three-dimensional reconstruction constraints of the camera and the IMU constraints.

[0010] The visual feature points after three-dimensional reconstruction are fitted to obtain the geometric feature attributes of the visual plane; the laser radar field of view is directed towards the reference surface and the laser point cloud is projected to the camera image, the visual feature points of the visual plane are clustered by laser point cloud, and the geometric feature attributes of the point cloud plane after clustering are calculated; and the extrinsic parameters between the camera and the laser radar are calculated according to the geometric feature attributes of the visual plane and the geometric feature attributes of the point cloud plane.

[0011] Further, the extrinsic parameters between the IMU and the laser radar are calculated according to the extrinsic parameters between the camera and the IMU and the extrinsic parameters between the camera and the laser radar.

[0012] Further, the geometric feature attributes of the visual plane include the center point and the normal vector of the visual plane, and the geometric feature attributes of the point cloud plane include the center point and the normal vector of the point cloud plane.

[0013] Further, the extrinsic parameters between the camera and the laser radar are calculated by the following formula:

[0014]

[0015] wherein, the normal vector of the visual plane is represented by, the extrinsic parameters between the camera and the laser radar are represented by, the center point of the point cloud plane is represented by s, and s represents the scale of the camera (referring to the observation scale of the camera in the real world), the normal vector of the point cloud plane is represented by.

[0016] Further, the extrinsic parameters between the camera and the laser radar include the rotation extrinsic parameters and the translation extrinsic parameters between the camera and the radar.

[0017] Further, the rotation extrinsic parameters between the camera and the radar are the ratio of the normal vector of the point cloud plane to the normal vector of the visual plane.

[0018] Further, after obtaining the geometric feature attribute of the visual plane, the visual feature points of a frame of image on the visual plane are projected to the current frame of image, and the bundle adjustment method is used for optimization, the moving pose of the camera and the distribution of the visual feature points are calculated, and the laser point cloud clustering is performed on the distribution of the visual feature points.

[0019] Further, the extrinsic parameters between the camera and the IMU include the rotational extrinsic parameters between the camera and the IMU.

[0020] Further, the camera, the laser radar and the IMU are time-stamped and synchronized during the movement of the leg-foot robot.

[0021] In a second aspect, the present application provides an online multi-source sensor extrinsic parameter calibration system for a leg-foot robot.

[0022] The online multi-source sensor extrinsic parameter calibration system for the leg-foot robot is applied to the movement of the leg-foot robot, and includes a camera, a laser radar, an IMU and a calibration calculation processing module arranged on a carrier of the leg-foot robot, wherein the camera, the laser radar and the IMU are connected with the calibration calculation processing module.

[0023] The camera is configured to rotate to collect an image when a field of view of the camera faces a reference surface.

[0024] The laser radar is configured to collect laser point clouds when a field of view of the laser radar faces the reference surface, and project the laser point clouds to the camera image.

[0025] The IMU is configured to obtain current motion information of the leg-foot robot.

[0026] The calibration calculation processing module is configured to perform three-dimensional reconstruction according to a pose and image flow feature points of each frame of image, to obtain three-dimensional reconstruction constraints of the camera; perform pre-integration according to the motion information of the IMU, to obtain IMU constraints; calculate extrinsic parameters between the camera and the IMU according to the three-dimensional reconstruction constraints of the camera and the IMU constraints; fit the visual feature points after the three-dimensional reconstruction, to obtain geometric feature attributes of a visual plane; perform laser point cloud clustering on the visual feature points of the visual plane, to calculate geometric feature attributes of a point cloud plane of the clustered laser point clouds; and calculate extrinsic parameters between the camera and the laser radar according to the geometric feature attributes of the visual plane and the geometric feature attributes of the point cloud plane.

[0027] Compared with the prior art, the present application has the following beneficial effects:

[0028] The present application performs data fusion processing on the multi-line laser radar, the camera and the inertial measurement unit, and online calibrates the extrinsic parameters between the multi-sensors, thereby avoiding the dependence on specific calibration parameter objects such as calibration plates and calibration codes.

[0029] The present application is for sparse feature extraction processing, which is independent of the calibration offline large-scale algorithm optimization, and realizes the online operation of the calibration solving process.

[0030] Due to the planar constraint of the ground, the present application can effectively describe the visual and laser features when facing the foot-ground impact disturbance of the leg-foot robot, and ensure the convergence of the calibration result. BRIEF DESCRIPTION OF DRAWINGS

[0031] The drawings accompanying the specification of the present application serve to provide further understanding of the present application, and the illustrative embodiments of the present application and the description thereof serve to explain the present application, and do not constitute an improper limitation on the present application.

[0032] Figure 1 is the flow chart of the online multi-source sensor extrinsic parameter calibration method for the leg-foot robot shown by the present application;

[0033] Figure 2 is the sensor field of view angle schematic diagram of the online multi-source sensor extrinsic parameter calibration system shown by the present application;

[0034] Figure 3 is the online multi-source sensor extrinsic parameter calibration system and the leg-foot robot carrier appearance schematic diagram of the present application. DETAILED DESCRIPTION

[0035] The present application will be further described below in combination with the drawings and embodiments.

[0036] It should be pointed out that the following detailed description is exemplary, and is intended to provide further description of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as generally understood by those skilled in the art to which the present application belongs.

[0037] It should be noted that the terms used herein are only for the purpose of describing the specific embodiments, and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form, and in addition, it should be understood that when the terms "comprise" and / or "include" are used in the specification, they indicate the presence of the features, steps, operations, devices, components and / or their combinations.

[0038] In the present application, the terms such as "connected", "connected" and the like should be understood broadly, which means that it can be fixedly connected, integrally connected or detachably connected; it can be directly connected or indirectly connected through an intermediate medium. For relevant scientific research or technical personnel in the art, the specific meaning of the above terms in the present application can be determined according to the specific circumstances, and it should not be understood as a limitation on the present application.

[0039] Term explanation:

[0040] The RANSAC algorithm is an algorithm that calculates the mathematical model parameters of a sample dataset containing outliers to obtain valid sample data.

[0041] Singular value decomposition (SVD) is an orthogonal matrix decomposition method.

[0042] Example 1

[0043] Figure 1 The main principle of the online multi-source sensor extrinsic parameter calibration method applicable to legged robots of this invention is as follows:

[0044] Step 1: Position the camera's field of view at a 45° angle towards the reference plane, here choosing either the ground or a horizontal plane (e.g., ...). Figure 2 As shown), rotational motion is performed, with the camera operating frequency set to 30Hz and the camera resolution set to 640*480. Optical flow feature points are extracted from the pixels in each frame using the KLT optical flow algorithm, ensuring that no less than 50 feature points are extracted from each frame. The pose and global coordinate representation of the optical flow feature points of each frame are calculated using SFM 3D reconstruction. Here, the first frame is used as the global base coordinate system. At the same time, the motion information of the IMU is used for pre-integration to solve the rotation constraint term between every two frames, as shown in formula (1):

[0045]

[0046] in Indicates the amount of rotation. This indicates the acceleration measured by the IMU. This represents the rotation constraint term, and k and k+1 represent the interval between two consecutive IMU frames.

[0047] Based on the IMU constraints and the 3D reconstruction constraints, as shown in formula (2):

[0048]

[0049] The system of equations consists of at least ten sets, among which This represents the fixed rotational extrinsic parameter between the camera and the IMU, where This represents the pre-integration constraint terms between IMUs, through Accumulation over a fixed interval yields the following result: The 3D reconstruction constraints between cameras are represented, and the rotational extrinsic parameters between the camera and the IMU are obtained using matrix decomposition.

[0050] Step 2: RANSAC plane fitting of the visual feature points in the world frame recovered in the previous step, to recover the geometric properties of the visual ground plane, including the center point and normal vector Project the 3D feature points in the previous frame onto the current frame and optimize using bundle adjustment, to solve the camera pose and the distribution of the visual feature points.

[0051] Step 3: Project the laser point cloud onto the camera image (as shown in Figure 2 ), and cluster the laser points according to the 3D visual feature points obtained in the previous step. Each visual 3D feature point has at least 10 laser points nearby, ensuring that the number of data in the selected laser point cloud is at least 500. Perform RANSAC algorithm fitting on the clustered point cloud to solve the point cloud fitting plane, and simultaneously solve the center point and normal vector

[0052] Steps 1-3 above are performed simultaneously during the mobile phase of the leg-foot robot. At the same time, time synchronization devices are used to synchronize the timestamps of the above sensors.

[0053] Step 4: Collect at least five sets of visual image and laser point cloud data by moving the leg-foot robot. Considering the uniformity of the geometric properties of the ground, according to formula (3):

[0054]

[0055] where, represents the rotation external parameter between the radar and the camera. This formula indicates that the plane fitted by the visual feature points and the plane fitted by the laser point cloud are parallel. Similarly, using the geometric correspondence between the center points, as shown in formula (4), the external parameter transformation relationship between the camera and the laser radar can be established.

[0056]

[0057] where, Extrinsic parameters between camera and lidar, including rotation and translation, s represents the scale of the camera, the points in the plane fitted by the radar point cloud are expressed in the plane fitted by the camera feature points after matrix transformation, at this time, the vector formed by the normal vector of the camera feature point plane and the points of the two constitutes a perpendicular relationship, according to formulas (3), (4), more than five groups of data are obtained by moving the robot, and an extrinsic parameter solving equation set is constructed. The final calibration extrinsic parameter is solved by SVD matrix decomposition. During the movement of the leg-foot robot, due to the planar geometric constraint of the ground, the extracted visual sparse features and point cloud sparse features are not affected by the foot landing impact disturbance, ensuring the accuracy of the final calibration result.

[0058] At the same time, based on the constraint of the geometric properties of the ground, the calibration extrinsic parameter is solved, which effectively avoids the solving deviation caused by the disturbance of the impact force of the foot-ground interaction of the leg-foot robot during movement.

[0059] Embodiment two

[0060] As shown in Figure 3 , the online multi-source sensor extrinsic parameter calibration system suitable for leg-foot robots of the embodiment comprises:

[0061] (1) a laser radar, which is used to collect laser point clouds, and extract the geometric properties of the ground through the description of the ground by the laser point clouds.

[0062] (2) an inertial measurement unit, which is used to obtain the current angular velocity and acceleration information of the leg-foot robot, and perform pre-integration and camera three-dimensional reconstruction on the information to form a pose transformation constraint.

[0063] (3) a camera, which is used to obtain image information to extract visual feature points, and extract optical flow feature points through a KLT algorithm.

[0064] (4) a calibration calculation processing module, which is used for data fusion processing, pre-integration item calculation of the motion information collected by the IMU, feature point extraction and three-dimensional reconstruction of the collected visual image, recovery of 3D feature points under the world coordinate system and fitting of the geometric properties of the ground, and then light beam adjustment optimization based on plane constraint, and clustering and fitting of the geometric properties of the laser point cloud, and then construction of more than five groups of camera and radar geometric property correlation equations for solving the extrinsic parameters between the two.

[0065] (5) a leg-foot robot carrier, which is used to carry the multi-sensor online calibration module and perform moving operation, so that the multi-sensor collects multiple sets of calibration data during movement.

[0066] The above merely provides the preferred embodiments of the present application, and is not used to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modifications, equivalent replacements, improvements, etc. made within the principles and technical scope of the present application shall fall into the scope of the present application.

Claims

1. An online multi-source sensor extrinsic parameter calibration method for a legged robot, characterized in that, The application is applied to the movement of a leg-foot robot, and comprises the following steps: The camera field of view is directed to a reference surface and rotates to collect images, three-dimensional reconstruction is performed according to the pose of each frame of image and image optical flow feature points, and three-dimensional reconstruction constraints of the camera are obtained; pre-integration is performed by using the motion information of the IMU to obtain IMU constraints; and the extrinsic parameters between the camera and the IMU are calculated according to the three-dimensional reconstruction constraints of the camera and the IMU constraints; The visual feature points after three-dimensional reconstruction are fitted to obtain the geometric feature attributes of the visual plane; the laser radar field of view is directed to the reference surface, and laser point clouds are projected to the camera image; the visual feature points of the visual plane are clustered by using laser point clouds to calculate the geometric feature attributes of the point cloud plane after clustering; and the extrinsic parameters between the camera and the laser radar are calculated according to the geometric feature attributes of the visual plane and the geometric feature attributes of the point cloud plane.

2. The online multi-source sensor extrinsic parameter calibration method for a legged robot according to claim 1, wherein, The extrinsic parameters between the camera and the IMU and the extrinsic parameters between the camera and the laser radar are used to calculate the extrinsic parameters between the IMU and the laser radar. 3.The online multi-source sensor extrinsic calibration method for legged robots of claim 1, wherein, The geometric feature attributes of the visual plane comprise a center point and a normal vector of the visual plane, and the geometric feature attributes of the point cloud plane comprise a center point and a normal vector of the point cloud plane.

4. The online multi-source sensor extrinsic parameter calibration method for a legged robot according to claim 3, wherein, The extrinsic parameters between the camera and the laser radar are calculated by the following formula: wherein, denotes a normal vector of the visual plane, denotes extrinsic parameters between the camera and the lidar, denotes a center point of the point cloud plane, s denotes a camera scale, denotes a normal vector of the point cloud plane.

5. The online multi-source sensor extrinsic parameter calibration method for a legged robot according to claim 1 or 4, characterized in that, The extrinsic parameters between the camera and the laser radar comprise rotational extrinsic parameters and translational extrinsic parameters between the camera and the laser radar.

6. The online multi-source sensor extrinsic parameter calibration method for a legged robot according to claim 5, wherein, The rotational extrinsic parameters between the camera and the laser radar are the ratio of the normal vector of the point cloud plane to the normal vector of the visual plane.

7. The online multi-source sensor extrinsic parameter calibration method for a legged robot according to claim 1, wherein, After obtaining the geometric feature attributes of the visual plane, the visual feature points of a frame of image on the visual plane are projected to a current frame of image, and the moving pose of the camera and the distribution of the visual feature points are calculated by using a bundle adjustment method for optimization, and the distribution of the visual feature points is clustered by using laser point clouds. 8.The online multi-source sensor extrinsic calibration method for legged robots of claim 1, wherein, The extrinsic parameters between the camera and the IMU comprise rotational extrinsic parameters between the camera and the IMU. 9.The online multi-source sensor extrinsic calibration method for legged robots according to claim 1, wherein, During the movement of the leg-foot robot, the camera, the laser radar and the IMU are time-stamped synchronously.

10. An online multi-source sensor extrinsic parameter calibration system for legged robots, characterized in that, The application is applied to the movement of a leg-foot robot, and comprises a camera, a laser radar, an IMU and a calibration calculation processing module arranged on a carrier of the leg-foot robot, wherein the camera, the laser radar and the IMU are connected with the calibration calculation processing module; The camera is used to collect images by rotating when the camera field of view is directed to a reference surface; The laser radar is used to collect laser point clouds when the laser radar field of view is directed to the reference surface, and the laser point clouds are projected to the camera image; The IMU is used to acquire the current motion information of the leg-foot robot; The calibration calculation processing module is used to perform three-dimensional reconstruction according to the pose of each frame of image and image optical flow feature points to obtain three-dimensional reconstruction constraints of the camera; pre-integration is performed by using the motion information of the IMU to obtain IMU constraints; and the extrinsic parameters between the camera and the IMU are calculated according to the three-dimensional reconstruction constraints of the camera and the IMU constraints; and the visual feature points after three-dimensional reconstruction are fitted to obtain the geometric feature attributes of the visual plane; The visual feature points of the visual plane are clustered by using laser point clouds to calculate the geometric feature attributes of the point cloud plane after clustering; and According to the geometric feature attribute of the visual plane and the geometric feature attribute of the point cloud plane, the extrinsic parameters between the camera and the laser radar are calculated.

Images