Robot motion attitude estimation method and system based on multi-modal sensor fusion

Abstract

The invention relates to the technical field of robots, in particular to a robot motion posture estimation method and system based on multi-modal sensor fusion, and the method comprises the steps: selecting a combined sensor based on a robot task demand to form a multi-modal sensor, and calibrating the multi-modal sensor; the method comprises the following steps: collecting robot motion posture data based on a multi-modal sensor, and preprocessing the posture data; according to the method, heterogeneous data of vision, inertia, touch and the like are integrated, the performance bottleneck of a single sensor is broken through, and unification of high precision, real-time performance and robustness is achieved. The key problems of data synchronization, error calibration, calculation efficiency and the like are solved through a closed-loop process of data acquisition, preprocessing, fusion and optimization, and meanwhile, the reliability in a complex environment is improved through a self-adaptive strategy.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, in particular to a robot motion posture estimation method and system based on multi-modal sensor fusion. BACKGROUND

[0002] The core of robot motion posture estimation is to obtain the position and direction of the robot in three-dimensional space in real time, accurately and robustly.

[0003] At present, the position and direction of the robot in three-dimensional space are obtained by using single-modal sensors, but there are some defects in the single-modal sensor acquisition, and the defects are as follows: a visual sensor (camera, depth camera) can provide rich semantic information (such as object texture, color, geometric structure), support scene understanding and target recognition, but it is more dependent on light conditions (low light, strong light is easy to fail), dynamic occlusion (such as fast moving objects) and texture missing scene (such as pure color wall), which leads to unstable feature matching and tracking, an inertial measurement unit (IMU) collects acceleration and angular velocity at a high frequency (kHz level), which is suitable for short-time dynamic posture tracking (such as rapid turning and jumping), but there is a cumulative drift error (gyroscope zero offset accumulates over time), and the long-term positioning accuracy is insufficient, which needs external calibration or fusion of other sensors for correction, a laser radar (LiDAR) generates a high-precision three-dimensional environment map through point cloud, which is not sensitive to light and occlusion, and is suitable for complex environment modeling, but the data volume is large, the computational complexity is high, and the detection ability for low reflectivity objects (such as black rubber) decreases, a tactile / force sensor can directly perceive physical contact, force feedback and object deformation, which is suitable for fine operations such as grasping and assembly, but the coverage is limited, and it needs to be combined with visual and inertial modalities to obtain global posture information.

[0004] The inherent defects of the above-mentioned single-modal sensors promote the multi-modal fusion to become an inevitable choice for robot motion posture estimation. SUMMARY

[0005] The present application aims to provide a robot motion posture estimation method and system based on multi-modal sensor fusion, which aims to solve the problem of defects in the existing single-modal sensor for obtaining the position and direction of the robot in three-dimensional space.

[0006] To achieve the above-mentioned purpose, in a first aspect, the present application provides a robot motion posture estimation method based on multi-modal sensor fusion, comprising the following steps:

[0007] Selecting a combination of sensors to form a multi-modal sensor based on the task requirements of the robot, and calibrating the multi-modal sensor;

[0008] Collecting robot motion posture data based on the multi-modal sensor, and pre-processing the posture data;

[0009] The multi-modal sensor data fusion framework is constructed to estimate the preprocessed attitude data, and an estimation result is output.

[0010] The specific method of selecting a combination sensor based on the robot task requirement to form a multi-modal sensor and calibrating the multi-modal sensor includes:

[0011] The specific method of selecting a combination sensor based on the robot task requirement to form a multi-modal sensor includes:

[0012] The multi-modal sensor is time-synchronized and sampled frequency-matched.

[0013] The inertial measurement unit, the visual sensor, and the laser radar are calibrated.

[0014] The multi-modal sensor includes one or more of a visual sensor + inertial measurement unit, a visual sensor + tactile / force sensor, a laser radar + visual sensor, and multi-sensor redundancy.

[0015] The specific method of collecting robot motion attitude data based on the multi-modal sensor and preprocessing the attitude data includes:

[0016] The specific method of collecting robot motion attitude data based on the multi-modal sensor includes:

[0017] The attitude data is preprocessed, error is compensated and optimized, and high-frequency noise, image noise, and environmental interference points are removed.

[0018] The specific method of constructing a multi-modal sensor data fusion framework to estimate preprocessed attitude data and output an estimation result includes:

[0019] The robot and the multi-modal sensor coordinate system are defined and converted into a coordinate matrix.

[0020] The fusion filtering method and the optimization method construct a fusion framework, estimate preprocessed attitude data, and output an estimation result.

[0021] The filtering method includes Kalman filtering and particle filtering, and the optimization method includes graph optimization and deep learning methods.

[0022] In a second aspect, the application further provides a multi-modal sensor fusion robot motion attitude estimation system, which is applied to the multi-modal sensor fusion robot motion attitude estimation method as described in the first aspect above, and includes a multi-modal sensor, a synchronization module, a computing processing module, and a communication module, wherein the communication module is connected with the multi-modal sensor, the synchronization module, and the computing processing module respectively.

[0023] The multi-modal sensor is used for collecting robot kinematics and environmental information in real time, and acquiring attitude data.

[0024] The synchronization module is used for synchronizing the attitude data, and generating a data stream with timestamps.

[0025] The computing processing module is used for executing attitude data preprocessing, fusion algorithm and attitude solving, and outputting an estimation result.

[0026] The communication module is used for communication between the multi-modal sensor, the synchronization module and the computing processing module.

[0027] The multi-modal sensor fusion robot motion attitude estimation method provided by the application is based on robot task requirements to select a combination sensor to form a multi-modal sensor, and calibrate the multi-modal sensor; based on the multi-modal sensor to collect robot motion attitude data, and pre-process the attitude data; construct a multi-modal sensor data fusion framework to estimate the pre-processed attitude data, and output an estimation result, which integrates heterogeneous data such as vision, inertia and touch, breaks through the performance bottleneck of a single sensor, realizes the unification of high precision, real-time performance and robustness, solves the key problems of data synchronization, error calibration and computing efficiency through a closed-loop process of 'data acquisition-preprocessing-fusion-optimization', and improves the reliability in a complex environment through an adaptive strategy. BRIEF DESCRIPTION OF DRAWINGS

[0028] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiment or prior art description. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0029] Figure 1 It is a flowchart of the multi-modal sensor fusion robot motion attitude estimation method provided by the present application.

[0030] Figure 2 It is a flowchart of the specific way of selecting a combination sensor to form a multi-modal sensor based on robot task requirements, and calibrating the multi-modal sensor.

[0031] Figure 3 It is a flowchart of the specific way of collecting robot motion attitude data based on the multi-modal sensor, and pre-processing the attitude data.

[0032] Figure 4is a flow chart of a specific way of constructing a multi-modal sensor data fusion framework to estimate pre-processed attitude data and output an estimation result.

[0033] Figure 5 is a connection diagram of a multi-modal sensor fusion robot motion attitude estimation system provided by the application.

[0034] In the figure: 1-multi-modal sensor, 2-synchronization module, 3-computing processing module, 4-communication module. DETAILED DESCRIPTION

[0035] The embodiments of the application are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference signs represent the same or similar elements or elements with the same or similar functions throughout. The embodiments described below by referring to the accompanying drawings are exemplary and are intended to explain the application, and cannot be understood as a limitation of the application.

[0036] Please refer to Figures 1 to 4 , in a first aspect, the application provides a multi-modal sensor 1 fusion robot motion attitude estimation method, comprising the following steps:

[0037] S1 selects a combination sensor to constitute a multi-modal sensor 1 based on the robot task requirement, and calibrates the multi-modal sensor 1;

[0038] Specifically,

[0039] S11 selects a combination sensor to constitute a multi-modal sensor based on the robot task requirement;

[0040] In the embodiments of the application, the multi-modal sensor 1 includes one or more of a vision sensor + inertial measurement unit (IMU), a vision sensor + tactile / force sensor, a laser radar (LiDAR) + vision sensor (camera), and multi-sensor redundancy (deployment of double cameras, double IMUs, etc.).

[0041] S12 performs time synchronization and sampling frequency matching on the multi-modal sensor 1;

[0042] In the embodiments of the application, time synchronization: through hardware triggering (such as a synchronous clock) or software interpolation (such as linear interpolation), the time stamp alignment of multi-sensor data is ensured, and fusion error caused by timing deviation is avoided.

[0043] Sampling frequency matching: the high-frequency IMU data (such as 100Hz) and the low-frequency vision / LiDAR data (such as 30Hz) are processed by downsampling or upsampling to establish a unified time sequence.

[0044] S13 calibrates the inertial measurement unit, the vision sensor and the laser radar.

[0045] In the embodiments of the present application, the inertial measurement unit calibration: through static calibration (such as six face method) to calibrate the gyroscope zero offset, accelerometer scale factor and non-orthogonal error; dynamic calibration (such as rotary table) to compensate temperature drift. The visual sensor calibration: using the chessboard calibration board to calculate the camera internal parameter (focal length, distortion coefficient) and external parameter (conversion relationship with the robot body coordinate system). The laser radar calibration: through the plane / angle point matching to calibrate the external parameter of LiDAR and camera, or using hand-eye calibration to establish the coordinate conversion relationship between sensors.

[0046] S2 collects robot motion posture data based on the multi-modal sensor 1, and pre-processes the posture data;

[0047] Specifically,

[0048] S21 collects robot motion posture data based on the multi-modal sensor 1;

[0049] S22 pre-processes the posture data, compensates for errors and optimizes the posture, removes high-frequency noise, image noise and environmental interference points.

[0050] In the embodiments of the present application, for high-speed motion scene, the image distortion caused by robot displacement during visual / LiDAR sampling is corrected (such as motion compensation in visual SLAM), the repeated scene is identified through visual feature matching or LiDAR point cloud matching, the long-time cumulative error is eliminated by adding loop constraint (such as bag-of-words model in ORB-SLAM). The multi-sensor data and the loop constraint are input into the optimizer (such as g2o / COLMAP) to generate a globally consistent posture trajectory. Among them, the IMU data uses complementary filtering, Kalman filtering or wavelet transform to remove high-frequency noise, and through statistical test (such as 3σ principle) to remove sudden data (such as abnormal values caused by motion impact); the visual data uses median filtering and Gaussian filtering to remove image noise, and uses RANSAC algorithm to remove mis-matching points in feature matching; the LiDAR data uses voxel filtering (Voxel Grid) to downsample, and uses statistical outlier removal to remove environmental interference points (such as dynamic objects).

[0051] S3 constructs a multi-modal sensor 1 data fusion framework to estimate the pre-processed posture data, and outputs the estimation result.

[0052] Specifically,

[0053] S31 defines the robot and the multi-modal sensor 1 coordinate system, and converts it into a coordinate matrix;

[0054] In the embodiment of the present application, the x / y / z axis directions are defined with the robot centroid as the origin (for example, the forward direction is the x axis), the coordinate system of each sensor (for example, the optical center of the vision sensor is the origin, and the optical axis is the z axis), and the sensor data is converted to the body coordinate system through a rotation matrix and a translation vector.

[0055] In S32, a fusion framework is constructed by using a fusion filtering method and an optimization method, and the preprocessed attitude data is estimated, and an estimation result is output.

[0056] In the embodiment of the present application, the filtering method includes Kalman filtering and particle filtering, and the optimization method includes graph optimization and deep learning method.

[0057] Kalman filtering (KF) is suitable for linear Gaussian systems, and the attitude is estimated through state prediction and observation update iteration, such as extended Kalman filtering (EKF) which can process nonlinear problems (such as IMU integration and vision projection). Particle filtering (PF) is based on Monte Carlo sampling and is suitable for nonlinear non-Gaussian scenarios (such as complex dynamic environments), and the real attitude distribution is approximated through weight update and resampling. Graph optimization: the attitude estimation problem is modeled as a graph structure, the nodes are the attitudes at each time, and the edges are sensor observation constraints (such as IMU prediction error, vision loop constraint), and the global optimal attitude is solved through nonlinear optimization (such as Levenberg-Marquardt algorithm). Deep learning method uses CNN / RNN to process vision / LiDAR features, and combines LSTM to fuse time series IMU data, such as directly outputting attitude angles through an end-to-end network (suitable for data-driven scenarios, but requires a large amount of labeled data).

[0058] Multi-source information complementary fusion: using the characteristics of high short-term precision of IMU and high long-term precision of vision / LiDAR, the IMU drift is suppressed through weighted fusion (such as vision loop correction of cumulative error). Redundant fusion: when a certain sensor fails (such as camera obstruction), the attitude estimation is maintained by relying on other sensors (IMU+LiDAR), and the system robustness is improved.

[0059] Please refer to Figure 5 , in the second aspect, the present application also provides a multi-modal sensor 1 fusion robot motion attitude estimation system, applied to the multi-modal sensor 1 fusion robot motion attitude estimation method as described in the first aspect above, comprising a multi-modal sensor 1, a synchronization module 2, a computing processing module 3 and a communication module 4, the communication module 4 is connected with the multi-modal sensor 1, the synchronization module 2 and the computing processing module 3 respectively;

[0060] The multi-modal sensor 1 is used for collecting robot kinematics and environment information in real time, and acquiring attitude data;

[0061] The synchronization module 2 is used for synchronizing the attitude data, and generating a data stream with time stamp;

[0062] The computing processing module 3 is used for executing attitude data preprocessing, fusion algorithm and attitude solution, and outputting an estimation result;

[0063] The communication module 4 is used for realizing communication between the multi-modal sensor 1, the synchronization module 2 and the computing processing module 3.

[0064] In the embodiment of the present application, the multi-modal sensor 1 is used for collecting robot kinematics and environment information in real time, and acquiring attitude data; the synchronization module 2 is used for synchronizing the attitude data, and generating a data stream with time stamp; the computing processing module 3 is used for executing attitude data preprocessing, fusion algorithm and attitude solution, and outputting an estimation result; and the communication module 4 is used for realizing communication between the multi-modal sensor 1, the synchronization module 2 and the computing processing module 3.

[0065] The above only discloses a preferred embodiment of the robot motion attitude estimation method and system of the multi-modal sensor fusion of the present application, and of course cannot limit the scope of the present application, and those skilled in the art can understand that all or part of the above-mentioned embodiments are implemented, and equivalent changes made according to the claims of the present application still belong to the scope of the present application.

Claims

1. A robot motion posture estimation method based on multimodal sensor fusion, characterized in that: The following steps are included: Select a combination of sensors to form a multimodal sensor based on the robot's mission requirements, and calibrate the multimodal sensor; Collecting robot motion posture data based on the multimodal sensor and preprocessing the posture data; Construct a multimodal sensor data fusion framework to estimate the preprocessed posture data and output the estimation results.

2. The robot motion posture estimation method based on multimodal sensor fusion according to claim 1, characterized in that: The specific method of selecting a combination of sensors to form a multimodal sensor based on the robot task requirements and calibrating the multimodal sensor is as follows: Select combined sensors to form multimodal sensing based on robot mission requirements; Performing time synchronization and sampling frequency matching on the multimodal sensor; The inertial measurement unit, the visual sensor, and the lidar are calibrated.

3. The robot motion posture estimation method based on multimodal sensor fusion according to claim 1, characterized in that: The multimodal sensor includes one or more of a visual sensor + an inertial measurement unit, a visual sensor + a tactile / force sensor, a lidar + a visual sensor, and multi-sensor redundancy.

4. The robot motion posture estimation method based on multimodal sensor fusion according to claim 1, It is characterized by: The specific method of collecting robot motion posture data based on the multimodal sensor and preprocessing the posture data is as follows: Collecting robot motion posture data based on the multimodal sensor; The posture data is preprocessed to compensate for errors and optimize the posture, and remove high-frequency noise, image noise and environmental interference points.

5. The robot motion posture estimation method based on multimodal sensor fusion as claimed in claim 1, characterized in that ； The specific method of constructing the multimodal sensor data fusion framework to estimate the preprocessed posture data and output the estimation results is as follows: Defining the coordinate system of the robot and the multimodal sensor and converting it into a coordinate matrix; The fusion filtering method and the optimization method are integrated to build a fusion framework, estimate the preprocessed posture data, and output the estimation results.

6. The robot motion posture estimation method based on multimodal sensor fusion as claimed in claim 5, characterized in that ； The filtering methods include Kalman filtering and particle filtering, and the optimization methods include graph optimization and deep learning methods.

7. A robot motion posture estimation system based on multimodal sensor fusion, applied to the robot motion posture estimation method based on multimodal sensor fusion as claimed in any one of claims 1 to 6, characterized in that ； It includes a multimodal sensor, a synchronization module, a calculation and processing module, and a communication module, wherein the communication module is connected to the multimodal sensor, the synchronization module, and the calculation and processing module respectively; The multimodal sensor is used to collect robot kinematics and environmental information in real time and obtain posture data; The synchronization module is used to synchronize the posture data and generate a data stream with a timestamp; The calculation processing module is used to perform the posture data preprocessing, fusion algorithm and posture solution, and output the estimation result; The communication module is used for communication between the multimodal sensor, the synchronization module and the calculation and processing module.

Images