Medical robot mobile platform positioning method and fusion map construction method

Abstract

The invention discloses a medical robot mobile platform positioning method, which is fused with a map construction method, and adopts IMU data and wheel type odometer data to estimate a motion model of a mobile platform; updating the predicted value of the state vector by using measured values provided by the IMU and the wheel type odometer; calculating to obtain an updated state vector and a covariance matrix through a prediction stage of an extended Kalman filtering algorithm; obtaining pose updating information provided by a visual odometer; fusing the measurement value of the visual odometer and the prediction state vector by adopting an updating equation of an extended Kalman filtering algorithm; fusing the data of the visual odometer, the IMU and the wheel type odometer by adopting weighted average of an extended Kalman filtering algorithm; and repeatedly executing to obtain the pose estimation of the optimized mobile platform. A multi-sensor fusion positioning method is adopted, and the problem that a medical robot moving platform is not high in positioning precision in a narrow and unstructured room is well solved.

Description

Technical Field

[0001] The present invention belongs to the field of artificial intelligence technology, and specifically relates to a medical robot mobile platform positioning method and a fusion map construction method. Background Art

[0002] During the treatment process, the E-Flash medical robot's working environment is mostly unstructured and narrow operating rooms, which puts higher requirements on the environmental adaptability of the mobile platform. Simultaneous Localization and Mapping (SLAM) technology has become an important research direction in the field of E-Flash medical robots.

[0003] During the SLAM process of the E-Flash medical robot in the unstructured surgical treatment room, there are problems such as inaccurate positioning, poor mapping accuracy, and missing environmental information. Summary of the invention

[0004] The purpose of the present invention is to provide a medical robot mobile platform positioning method and a fusion map construction method to solve the problems of inaccurate positioning and poor mapping accuracy in SLAM of unstructured operating rooms.

[0005] The present invention is achieved through the following technical solutions:

[0006] The medical robot mobile platform positioning method comprises the following steps:

[0007] S01, pre-processing the data of the visual odometer, IMU and wheel odometer, including time synchronization processing and data correction processing;

[0008] S02, establishing an initialization state vector of the mobile platform;

[0009] S03, using IMU data and wheel odometer data to estimate the motion model of the mobile platform; obtaining a prediction equation based on the motion model, and using the measurement values ​​provided by the IMU and wheel odometer to update the predicted value of the state vector; and calculating the updated state vector and covariance matrix through the prediction phase of the extended Kalman filter algorithm;

[0010] S04, obtaining the updated position information provided by the visual odometer; using the update equation of the extended Kalman filter algorithm to fuse the visual odometer measurement value with the predicted state vector; based on the measurement error, using the data information of the visual odometer to update the state vector and covariance matrix, and correct the predicted deviation;

[0011] S05, using the weighted average of the extended Kalman filter algorithm to fuse the data of the visual odometer, IMU and wheel odometer; repeatedly executing steps S03 and S04 to obtain an optimized pose estimate of the mobile platform.

[0012] In some embodiments, the step of smoothing and optimizing the pose estimation in step S05 and outputting the final pose estimation data includes:

[0013] The time update step includes forward calculation of the state vector and forward calculation of the error covariance;

[0014] The measurement update step includes calculating the Kalman gain and updating the estimate and covariance using the observed variables;

[0015] After processing, the final pose estimation data is output.

[0016] In some embodiments, the method further includes the step of pre-processing the data of the wheel odometer, including:

[0017] Perform low-pass filtering and Kalman filtering on the speed and displacement data of the wheel odometer;

[0018] And / or using accelerometer and IMU data to detect the slip of the wheels of the mobile platform through the speed and acceleration changes of the wheel odometer; when slip is detected, compensating for the slip;

[0019] and / or estimate the friction coefficient between the wheel and the ground according to the road surface conditions and compensate for it; use IMU data to adjust the dynamic model;

[0020] and / or when an abnormal change in wheel speed is detected, adjusting the sampling frequency of the wheel odometer;

[0021] and / or presetting boundary conditions of the wheel odometer to limit the wheel speed and displacement to a set range;

[0022] And / or calibrate the error of the wheel odometer based on the data of the IMU and the data odometer.

[0023] On the other hand, the present invention also provides a fusion map construction method based on the medical robot mobile platform positioning method, comprising the following steps:

[0024] Build visual SLAM maps and laser SLAM maps based on RTAB-Map and GTSAM;

[0025] Unify the visual SLAM map and the laser SLAM map into the same format;

[0026] Align the visual SLAM map and the laser SLAM map, and transform the visual and laser coordinate systems into a unified global coordinate system;

[0027] The probability model is constructed using the Bayesian update formula, and the joint probability is calculated using the Bayesian rule. The grid cell state probabilities of the visual SLAM map and the laser SLAM map are jointly updated to obtain the occupancy probability of the fused grid cells.

[0028] According to the occupation probability of the fused grid cells, a global grid map or a point cloud map is generated to construct a fused map.

[0029] In some embodiments, the steps of constructing a visual SLAM map and a laser SLAM map based on RTAB-Map and GTSAM include:

[0030] Filter and remove noise from the point cloud data obtained by RTAB-Map;

[0031] Each new pose in RTAB-Map is used as a node in the factor graph and the factor graph is constructed based on the motion model factor, observation factor, and closed-loop factor;

[0032] GTSAM is used to optimize the factor graph, including: initializing the factor graph in GTSAM, inputting the initial pose estimation and factors into GTSAM; marginalizing, nonlinearly optimizing, and incrementally updating the factor graph;

[0033] The point cloud data is smoothed by a filter, and the point cloud is updated in real time and smoothly using the filtered pose data;

[0034] The pose optimized by GTSAM is used as the real-time pose update of RTAB-Map. The pose optimized by GTSAM is used to update the point cloud to generate visual SLAM map and laser SLAM map.

[0035] In some embodiments, when a closed loop is detected, GTSAM is called in real time to correct the path and update the visual SLAM map and the laser SLAM map.

[0036] In some embodiments, the optimized frequency of GTSAM is dynamically adjusted according to computing resources and map complexity; and / or the filter parameters are adjusted according to environmental characteristics.

[0037] In some embodiments, confidence weighting is used to assign different confidence weights to laser and visual sensor data; and / or in areas where visual and laser information conflict, visual data or laser data is selected by confidence weighting.

[0038] In some embodiments, during the operation of the mobile platform, laser data and visual data are continuously acquired, and the map information is dynamically adjusted using the Bayesian update formula.

[0039] In some embodiments, the method further includes the step of preprocessing the laser radar, IMU, and UWB sensor, including:

[0040] Time synchronization of data from LiDAR, IMU, and UWB sensors;

[0041] And / or filter the IMU data; calculate the displacement and rotation of the IMU through double integration, and obtain the attitude information at each time point in combination with attitude solution;

[0042] And / or use IMU data to calculate the posture change, adjust the posture of each frame of the point cloud in real time, and transform the point cloud into a coordinate system at a unified time; according to the posture information of the IMU, remap each point in the scanned point cloud to correct the displacement and rotation errors caused by movement;

[0043] and / or converting the data from LiDAR, IMU, and UWB sensors into a unified coordinate system;

[0044] and / or voxel filtering or downsampling the point cloud data;

[0045] And / or integrate calibrated IMU, laser point cloud and UWB sensor data into a unified timeline.

[0046] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0047] The present invention adopts a multi-sensor fusion positioning method and realizes the fusion of visual odometer, inertial measurement unit and wheel odometer data in RTAB-Map based on the extended Kalman filter algorithm, which effectively solves the problem of low positioning accuracy of the medical robot mobile platform in a small and unstructured indoor environment.

[0048] The positioning method of the present invention can solve the problems of low positioning accuracy, low mapping accuracy and missing environmental information caused by odometer drift in the SLAM process, and improves positioning accuracy and robustness.

[0049] The present invention utilizes GTSAM and filters to perform real-time optimization of RTAB-Map point cloud, ensures the accuracy and real-time performance of map optimization, and improves the accuracy of generated visual SLAM and laser SLAM maps.

[0050] The present invention utilizes a Bayesian-based map estimation fusion method to fuse vision and laser SLAM maps to obtain a global fusion map with richer information; and through Bayesian probability updating, it fully combines the advantages of laser and vision SLAM to generate a global fusion map with high accuracy and details, so that it has better navigation and positioning performance in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0052] Figure 1 This is a flow chart of the medical robot positioning method in an embodiment of the present invention.

[0053] Figure 2 The figure is a flow chart of the fusion map construction method in an embodiment of the present invention.

[0054] Figure 3 This is a map constructed based on laser SLAM in an embodiment of the present invention.

[0055] Figure 4 It is a fused map constructed using the fused map construction method in an embodiment of the present invention. DETAILED DESCRIPTION

[0056] In order to make the purpose, technical scheme and advantages of the present application clearer, the specific embodiments of the present application are further described in detail below in conjunction with the accompanying drawings. It is understood that the specific embodiments described herein are only used to explain the present application, rather than to limit the present application. It should also be noted that, for the convenience of description, only the part related to the present application but not all the contents are shown in the accompanying drawings. Before discussing the exemplary embodiments in more detail, it should be mentioned that some exemplary embodiments are described as processes or methods depicted as flow charts. Although the flow chart describes each operation (or step) as a sequential process, many of the operations therein can be implemented in parallel, concurrently or simultaneously. In addition, the order of each operation can be rearranged. The process can be terminated when its operation is completed, but it can also have additional steps not included in the accompanying drawings. The process can correspond to a method, a function, a procedure, a subroutine, a subprogram, etc.

[0057] The medical robot includes a mobile platform and a robotic arm. The robotic arm is arranged on the mobile platform, and the movement and positioning of the robotic arm in the operating room are realized through the walking control of the mobile platform.

[0058] Starting from enhancing the positioning and mapping capabilities of the E-Flash medical robot mobile platform in a small and unstructured operating room, the present invention proposes a multi-sensor fusion SLAM method that can be applied to small and unstructured rooms to achieve accurate mapping and navigation in a small operating room environment.

[0059] The E-Flash medical robot mobile platform uses ROS (Robot Operating System) as the software control system. It designs a move base navigation framework and path planning module based on surgical operation requirements, and uses STM32 as the hardware control core to realize the communication between ROS and STM32.

[0060] The multi-sensor fusion positioning method of the present invention is based on the Extended Kalman Filter (EKF) algorithm to achieve the fusion of visual odometer, inertial measurement unit (IMU) and wheel odometer data in RTAB-Map, and solve the problem of low positioning accuracy of the medical robot mobile platform in a small and unstructured room.

[0061] In some embodiments of the present invention, referring to Figure 1 ,The positioning method of the medical robot mobile platform includes the following steps:

[0062] S01. Sensor data preprocessing

[0063] Before data fusion, data from different sensors need to be preprocessed to ensure consistent data formats, synchronized timestamps, and matching resolutions. This includes:

[0064] Time synchronization processing: Since the timestamps of the visual odometry, IMU, and wheel odometry are usually different, time synchronization processing is required first to align the time of each data source.

[0065] Data calibration processing: used to calibrate the coordinate system of each sensor to ensure that it is consistent with the global coordinate system of the mobile platform.

[0066] S02, initialize the state vector

[0067] Establish a state vector containing the medical robot. The state vector usually includes position, velocity and posture information. The state vector can be expressed as:

[0068]

[0069] Among them, x, y, z represent the position, Indicates speed, Indicates the attitude angle.

[0070] S03, prediction stage

[0071] The motion model of the mobile platform is estimated using IMU data (including acceleration and angular velocity) and wheel odometer data.

[0072] The prediction equation is obtained based on the motion model, and the predicted value of the state vector is updated using the measurements provided by the IMU and wheel odometer.

[0073] Through the prediction phase of the EKF algorithm, the updated state vector and covariance matrix are calculated.

[0074] include:

[0075] 1) For the prediction equation X k =f(X k-1 )+Q k Perform Taylor expansion:

[0076]

[0077] make get:

[0078] f(X k-1 )≈AX k-1 +B;

[0079] f(X k-1 )≈AX k-1 +B is brought into the Bayesian filtering formula, and the predicted value is obtained to obey the following normal distribution:

[0080]

[0081] Where Q is the process noise covariance matrix.

[0082] The predicted values ​​follow the following distribution:

[0083]

[0084] 2) For the observation equation y k =h(X k )+R k Perform Taylor expansion:

[0085]

[0086] make get:

[0087] h(X k )≈CX k +D;

[0088] h(X k )≈CX k+D is brought into Bayesian filtering, and we get:

[0089]

[0090] Where R is the observation noise covariance matrix.

[0091] Let Kalman gain Substituting into the above formula, we get:

[0092]

[0093] Among them, X k , X k-1 , Y k , Q k , R k is a random variable. k To predict noise, its probability density function is used Represents; R k is the observation noise, and its probability density function is expressed as f represents the system state function; h represents the sensor measurement function.

[0094] And assume that the observed values ​​y0, y1.....y k , initial value x0 and its probability density function: f0(X). Let X k-1 Obey expectations The variance is The normal distribution of .

[0095] S04, Update Phase

[0096] Get the updated pose information provided by the visual odometer. The update step is expressed as:

[0097]

[0098] The update equation of the EKF algorithm is used to fuse the visual odometer measurement value with the predicted state vector, where the predicted state vector corresponds to the predicted value obtained by using the measurement values ​​of the IMU and the wheel odometer in step S03.

[0099] According to the measurement error, the state vector and covariance matrix are updated using the visual odometry information to correct the predicted deviation.

[0100] S05. Data Fusion

[0101] The weighted average of the EKF algorithm is used to integrate the data of the visual odometry, IMU and wheel odometry.

[0102] The prediction phase and update phase are repeatedly performed to optimize the pose estimation of the mobile platform.

[0103] S06. Data post-processing

[0104] After data fusion is completed, further smoothing and optimization processing can be performed to output the final pose data. Including:

[0105] 1) Time update (forecast)

[0106] The state variables are calculated forward, expressed as:

[0107]

[0108] Among them, u k-1 For control input.

[0109] The forward error covariance is expressed as:

[0110] P k - =AP k-1 A T +Q.

[0111] Among them, A T is the transpose of matrix A, and P k-1 is an initial estimate.

[0112] 2) Measurement update (correction)

[0113] Calculate the Kalman gain, expressed as:

[0114] K k =P k - H T (HP k - H T +R) -1 ;

[0115] Among them, H is the Jacobian matrix of the observation equation, H T is the transpose of matrix H, and R is the observation noise covariance matrix.

[0116] From the observed variable Z k Update the estimate, expressed as:

[0117]

[0118] Update the covariance, expressed as:

[0119] P k =(IK k H)P k - ;

[0120] Where I is the identity matrix.

[0121] The final pose estimation data is released for subsequent map construction or path planning.

[0122] The above positioning method can solve the problems of low positioning accuracy, low mapping accuracy and missing environmental information caused by odometer drift in the SLAM process, thereby improving positioning accuracy and robustness.

[0123] On this basis, the wheeled odometry is further processed before fusion to improve the motion accuracy of the wheeled odometry when it moves in non-structural and narrow operating rooms, and to further optimize the multi-sensor fusion effect.

[0124] The steps for pre-fusion processing of wheel odometers include:

[0125] 1) Noise filtering, including:

[0126] Low-pass filtering: The speed and displacement data of the wheel odometer are processed by low-pass filtering to remove high-frequency noise, especially when a corner appears during the driving process. Low-pass filtering can avoid sudden changes in signal data that affect position estimation.

[0127] Kalman filter processing: Kalman filter is used for smoothing, based on the weighted fusion of the estimated value of the previous state and the current observation value to eliminate the measurement noise of the sensor itself.

[0128] 2) Slip detection and compensation processing

[0129] Slip detection: In unstructured scenarios, such as corners and turns, the wheels of the mobile platform are prone to slipping, resulting in increased errors in the wheel odometer.

[0130] The slip phenomenon is detected by using an accelerometer or IMU, and the difference between the wheel odometer speed change and the acceleration change is used to determine whether slip has occurred.

[0131] Slip compensation: When slip is detected, the impact of slip on position information can be reduced by reducing the weight of the wheel speed signal of the wheel odometer or directly correcting the abnormal signal.

[0132] 3) Dynamic model adjustment

[0133] Wheel odometers usually estimate the position based on the rotation of the wheel. In unstructured and narrow indoor spaces, the wheel contact changes, making the traditional model inapplicable. Here we introduce a dynamic model, including:

[0134] Wheel-to-ground friction coefficient compensation: estimate the friction coefficient and compensate according to the road conditions.

[0135] IMU-assisted adjustment: Use IMU data to implement the adjustment model to make the wheel odometer displacement calculation more in line with actual conditions.

[0136] 4) Real-time status feedback adjustment

[0137] In unstructured, narrow, and curvature-sudden indoor environments, the wheel odometer parameters are adjusted in real time through a feedback mechanism. For example, by detecting abnormal wheel speed changes, the sensor sampling frequency or data fusion weight is automatically increased or decreased.

[0138] 5) Introduce boundary constraints on speed and position

[0139] In data preprocessing, boundary conditions are introduced to constrain wheel speed and displacement within a set range. For example, in unstructured terrain, a speed cap is set to prevent wheel odometer data from generating extreme values ​​due to sudden and severe vibrations.

[0140] 6) Joint calibration and sensor self-calibration processing

[0141] During system initialization or regular operation, sensors are calibrated to correct the deviation of the wheel odometer. At the same time, self-calibration is performed based on the mileage error feedback from the IMU or visual odometer to correct the system offset during long-term operation of the mobile platform.

[0142] Through the above pre-processing steps, the data quality of the wheel odometer in complex indoor environments can be effectively improved, providing more accurate location information for multi-sensor fusion.

[0143] Using the KAIST data set, a comparative experiment was conducted on the RTAB-Map algorithm, the ORB-SLAM3 algorithm and the multi-sensor fusion positioning method of the above-mentioned embodiment of the present invention. The positioning accuracy of the multi-sensor fusion positioning method of the present invention is improved by about 14.1% compared with the RTAB-Map algorithm, and the positioning accuracy is improved by about 13.3% compared with the ORB-SLAM3 algorithm with IMU fusion.

[0144] On the other hand, the present invention also provides a method for constructing a fusion map, referring to Figure 2 , based on the fusion of visual SLAM and laser SLAM algorithms for mapping.

[0145] The visual mapping method RTAB-Map (Real-Time Appearance-Based Mapping) based on global Bayesian closed loop detection is adopted. In view of the real-time performance and optimization effect of the algorithm, the C++ library GTSAM (Georgia Tech Smoothing and Mapping) based on the factor graph is used to optimize the graph, and the RTAB-Map point cloud is optimized in real time by filtering to generate visual SLAM maps and laser SLAM maps. The following steps are included:

[0146] L01. Point cloud data preprocessing

[0147] Point cloud filtering: Filter the point cloud data obtained by RTAB-Map, use voxel grid filtering or downsampling to reduce the density of the point cloud, reduce the amount of data, and improve the efficiency of filtering.

[0148] Remove noise; use statistical filtering and other methods to remove outliers to reduce the impact of errors on graph optimization.

[0149] L02. Create nodes and build factor graphs

[0150] Create nodes; each new pose in the RTAB-Map is used as a node in the graph, containing the pose (position and attitude) of the mobile platform in the environment.

[0151] Construct a factor graph; including:

[0152] 1) Motion model factors: Use wheel odometer, IMU or visual odometer data information to construct constraint factors between postures.

[0153] 2) Observation factor: The point cloud matching result is used as the observation factor to reflect the actual posture information of the mobile platform.

[0154] 3) Closed loop factor: If RTAB-Map detects a closed loop, a closed loop factor can be added to increase constraints and make the optimization results more accurate.

[0155] L03, using GTSAM to optimize the factor graph

[0156] Initialize the factor graph in GTSAM and input the initial pose estimation and factors into GTSAM.

[0157] The optimization process includes:

[0158] Marginalization: For real-time optimization, old nodes can be marginalized to reduce the size of the graph.

[0159] Nonlinear optimization: Use the nonlinear optimization methods provided by GTSAM (such as Levenberg-Marquardt or Gauss-Newton algorithm) to solve the factor graph and obtain the optimal pose estimate.

[0160] Incremental updates: Using GTSAM’s incremental graph optimization, the graph is updated and re-optimized to achieve real-time processing.

[0161] L04, real-time filtering processing

[0162] Use Kalman filtering or particle filtering; use filters to smooth point cloud data and reduce noise in real-time point clouds.

[0163] Based on smooth point cloud update processing; use the filtered pose to update the point cloud in real time and remove redundant old data to improve the real-time performance of the system.

[0164] L05. Result output and visualization

[0165] Posture update processing: Use the posture optimized by GTSAM as the real-time posture update of RTAB-Map.

[0166] Point cloud optimization output; use GTSAM optimized pose to update the point cloud and generate a smooth and consistent map.

[0167] Trigger closed loop detection; when a closed loop is detected, the graph optimization algorithm (GTSAM) is called in real time to correct the path and update the entire map.

[0168] L06, Iterative processing and adaptive adjustment

[0169] According to computing resources and map complexity, the optimization frequency of GTSAM is dynamically adjusted to strike a balance between real-time performance and accuracy.

[0170] Adaptive filter parameter adjustment: adjust filter parameters based on environmental characteristics, such as increasing filter strength or reducing sampling rate to optimize processing speed.

[0171] Through the above steps, GTSAM and filters are used to perform real-time optimization of RTAB-Map point cloud to ensure the accuracy and real-time performance of map optimization and generate high-precision maps.

[0172] In some embodiments of the present invention, the Gmapping algorithm principle and optimization method are combined to optimize the distortion of the laser point cloud using IMU data, and the sensor is preprocessed based on the multi-sensor map fusion principle, including:

[0173] 1) Data time synchronization processing: Multiple sensors (such as lidar, IMU, UWB) usually have different sampling frequencies, and the data of each sensor needs to be time synchronized. Linear interpolation or timestamps can be used to align the time axes of different sensors, and the data enters the processing flow after synchronization.

[0174] 2) IMU data preprocessing; including:

[0175] Remove noise; filter the IMU data (accelerometer and gyroscope data), such as low-pass filtering or Kalman filtering, to reduce the impact of high-frequency noise.

[0176] Integral calculation: The displacement and rotation of the IMU are calculated through double integration, and the attitude information at each time point is obtained by combining the attitude solution.

[0177] 3) Distortion correction processing; including:

[0178] Motion distortion correction processing: Since the laser radar uses line-by-line scanning, the sensor position may change during the entire scanning cycle. Use IMU data to calculate the posture change, adjust the posture of each frame of the point cloud in real time, and transform the point cloud into a coordinate system at a unified time.

[0179] Backtracking correction processing: According to the attitude information of the IMU, each point in the scanned point cloud is remapped to correct the displacement and rotation errors caused by movement.

[0180] 4) Coordinate transformation processing: convert the data of each sensor (such as lidar, IMU, UWB) into a unified coordinate system (such as mobile platform coordinate system or global coordinate system) to ensure data alignment. For example, the transformation can be based on calibration information or the relative position relationship between sensors.

[0181] 5) Filtering: Perform voxel filtering or downsampling on the point cloud data to reduce the computational burden and remove redundant data, retaining the main geometric features.

[0182] 6) Data synchronization output processing: Integrate the calibrated IMU, laser point cloud and other sensor data on a unified timeline to provide more accurate input for point cloud registration and map construction of the SLAM algorithm.

[0183] Through the above sensor preprocessing steps, the Gmapping algorithm can better utilize sensor fusion information and improve the accuracy and robustness of map construction.

[0184] In some embodiments of the present invention, a Bayesian-based map estimation fusion method is used to fuse the visual SLAM map and the laser SLAM map to obtain a more informative global fusion map. Figure 3 and Figure 4. The following steps are included:

[0185] M01. Unified map format

[0186] Usually, laser SLAM maps are usually two-dimensional grid maps or point clouds, while visual SLAM maps are usually sparse or dense three-dimensional point clouds.

[0187] To facilitate fusion, the two maps can be converted into point cloud form, or the visual SLAM point cloud can be projected onto a two-dimensional grid map through dimensionality reduction to match the visual map with the laser map.

[0188] M02, data alignment and coordinate conversion

[0189] Initial alignment: Using the initial estimate of the medical robot’s pose (e.g., obtained through an IMU or odometer), the maps of the visual SLAM and laser SLAM are aligned.

[0190] Coordinate system conversion: Based on the external calibration results, the coordinate systems of vision and laser are converted into a unified global coordinate system, and the space of the visual map and the laser map is aligned.

[0191] M03, Bayesian probability update

[0192] Define the probability model; use the Bayesian update formula to build the probability model. For example, let the grid cell state probability in laser SLAM be P(L|Z laser ), the grid cell state probability in visual SLAM is P(L|Z vision ).

[0193] Update grid probabilities; calculate joint probabilities using Bayesian rule, expressed as:

[0194]

[0195] Here P(L|Z fusion ) represents the occupancy probability of the fused grid unit, L represents the occupancy probability or the occupancy state of a specific grid, P(Z laser , Z vision ) represents the observations of the lidar and visual camera, P(Z laser |L) represents the posterior probability.

[0196] Through joint updating, the observation information of vision and laser SLAM can be integrated to improve the accuracy of the map.

[0197] M04. Conflict Resolution and Confidence Weighting

[0198] Confidence weighting: Since laser SLAM is usually more accurate in distance measurement, while visual SLAM can provide more detailed information on environmental features, different confidence weights can be set for different sensors. For example, a higher weight can be given to the occupancy probability of the laser, while a moderate weight, such as a lower weight, can be given to the visual features.

[0199] Conflict area processing: In areas where visual and laser information conflict (such as areas affected by occlusions or dynamic objects), more reliable data can be selected by weighting with confidence.

[0200] M05, Fusion Map Construction

[0201] According to the occupancy probability P(L|Z fusion ), generate a global grid map or point cloud map, and merge the visual and laser observation information into an information-rich global map.

[0202] In the final map, the spatial structure information provided by laser SLAM can well guarantee the precise geometric shape, and the detailed features provided by visual SLAM can enrich the environmental description and make the map more semantically informative.

[0203] M06, Iterative Update

[0204] During the operation of the mobile platform, laser data and visual data are continuously acquired, and the map information is dynamically adjusted using the Bayesian update formula to adapt to environmental changes and sensor errors.

[0205] In the embodiment of the present invention, the advantages of laser and visual SLAM are fully combined through Bayesian probability updating to generate a global fusion map with high accuracy and details, so that it has better navigation and positioning performance in complex environments.

[0206] In the operating room environment, experiments on positioning and mapping of medical robot mobile platforms in non-structured and narrow corridors, patient positioning and navigation, and radiotherapy experiments were conducted.

[0207] The experimental results show that the fusion map construction method based on multi-sensor fusion in this invention has richer information than the RTAB-Map algorithm in constructing maps. The ATE accuracy in actual scenes is improved by about 34.8%, and the PRE accuracy is improved by about 66.5%. The constructed fusion map has the characteristics of high precision and high robustness. Under the condition of the designed working speed of 1m / s, the comprehensive positioning success rate of the medical robot reaches 95.8%, which can well meet the design requirements.

[0208] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Any simple modification or equivalent change made to the above embodiment based on the technical essence of the present invention shall fall within the protection scope of the present invention.

Claims

1. A method for positioning a mobile platform of a medical robot, characterized in that: The following steps are involved: S01, pre-processing the data of the visual odometer, IMU and wheel odometer, including time synchronization processing and data correction processing; S02, establishing an initialization state vector of the mobile platform; S03, using IMU data and wheel odometer data to estimate the motion model of the mobile platform; obtaining a prediction equation based on the motion model, and using the measurement values ​​provided by the IMU and wheel odometer to update the predicted value of the state vector; By extending the prediction phase of the Kalman filter algorithm, the updated state vector and covariance matrix are calculated; S04, obtaining the updated position information provided by the visual odometer; The update equation of the extended Kalman filter algorithm is used to fuse the visual odometer measurement value with the predicted state vector. According to the measurement error, the state vector and covariance matrix are updated using the data information of the visual odometer to correct the predicted deviation. S05, using the weighted average of the extended Kalman filter algorithm to fuse the data of the visual odometer, IMU and wheel odometer; repeatedly executing steps S03 and S04 to obtain an optimized pose estimate of the mobile platform.

2. The medical robot mobile platform positioning method according to claim 1, characterized in that: The method further includes the step of smoothing and optimizing the pose estimation in step S05 and outputting the final pose estimation data, including: The time update step includes forward calculation of the state vector and forward calculation of the error covariance; The measurement update step includes calculating the Kalman gain and updating the estimate and covariance using the observed variables; After processing, the final pose estimation data is output.

3. The medical robot mobile platform positioning method according to claim 1 or 2, characterized in that: The method also includes the steps of pre-processing the data of the wheel odometer, including: Perform low-pass filtering and Kalman filtering on the speed and displacement data of the wheel odometer; And / or using accelerometer and IMU data to detect the slip of the wheels of the mobile platform through the speed and acceleration changes of the wheel odometer; when slip is detected, compensating for the slip; and / or estimate the friction coefficient between the wheel and the ground according to the road surface conditions and compensate for it; use IMU data to adjust the dynamic model; and / or when an abnormal change in wheel speed is detected, adjusting the sampling frequency of the wheel odometer; and / or presetting boundary conditions of the wheel odometer to limit the wheel speed and displacement to a set range; And / or calibrate the error of the wheel odometer based on the data of the IMU and the data odometer.

4. A fusion map construction method based on the medical robot mobile platform positioning method according to any one of claims 1 to 3, characterized in that: The following steps are involved: Build visual SLAM maps and laser SLAM maps based on RTAB-Map and GTSAM; Unify the visual SLAM map and the laser SLAM map into the same format; Align the visual SLAM map and the laser SLAM map, and transform the visual and laser coordinate systems into a unified global coordinate system; The probability model is constructed using the Bayesian update formula, and the joint probability is calculated using the Bayesian rule. The grid cell state probabilities of the visual SLAM map and the laser SLAM map are jointly updated to obtain the occupancy probability of the fused grid cells. According to the occupation probability of the fused grid cells, a global grid map or a point cloud map is generated to construct a fused map.

5. The method for constructing a fusion map according to claim 4, characterized in that: The steps to build visual SLAM maps and laser SLAM maps based on RTAB-Map and GTSAM include: Filter and remove noise from the point cloud data obtained by RTAB-Map; Each new pose in RTAB-Map is used as a node in the factor graph and the factor graph is constructed based on the motion model factor, observation factor, and closed-loop factor; GTSAM is used to optimize the factor graph, including: initializing the factor graph in GTSAM, inputting the initial pose estimation and factors into GTSAM; marginalizing, nonlinearly optimizing, and incrementally updating the factor graph; The point cloud data is smoothed by a filter, and the point cloud is updated in real time and smoothly using the filtered pose data; The pose optimized by GTSAM is used as the real-time pose update of RTAB-Map. The pose optimized by GTSAM is used to update the point cloud to generate visual SLAM map and laser SLAM map.

6. The method for constructing a fusion map according to claim 5, characterized in that: When a closed loop is detected, GTSAM is called in real time to correct the path and update the visual SLAM map and laser SLAM map.

7. The method for constructing a fusion map according to claim 5, characterized in that: Dynamically adjust the optimized frequency of GTSAM according to computing resources and map complexity; and / or adjust the filter parameters according to environmental characteristics.

8. The method for constructing a fusion map according to claim 4, characterized in that: Confidence weighting is used to assign different confidence weights to laser and visual sensor data; and / or in areas where visual and laser information conflict, visual data or laser data is selected through confidence weighting.

9. The method for constructing a fusion map according to claim 4, characterized in that: During the operation of the mobile platform, laser data and visual data are continuously acquired, and the map information is dynamically adjusted using the Bayesian update formula.

10. The method for constructing a fusion map according to claim 4, characterized in that: It also includes steps for preprocessing the LiDAR, IMU, and UWB sensors, including: Time synchronization of data from LiDAR, IMU, and UWB sensors; And / or filter the IMU data; calculate the displacement and rotation of the IMU through double integration, and obtain the attitude information at each time point in combination with attitude solution; And / or use IMU data to calculate the posture change, adjust the posture of each frame of the point cloud in real time, and transform the point cloud into a coordinate system at a unified time; according to the posture information of the IMU, remap each point in the scanned point cloud to correct the displacement and rotation errors caused by movement; and / or converting the data from LiDAR, IMU, and UWB sensors into a unified coordinate system; and / or voxel filtering or downsampling the point cloud data; And / or integrate calibrated IMU, laser point cloud and UWB sensor data into a unified timeline.

Images