SLAM Method and System Based on 2D LiDAR and Camera in Medical Environment

Abstract

This invention discloses a SLAM method and system based on 2D LiDAR and camera in a medical environment. The method includes the following specific steps: S1: Constructing a joint calibration algorithm for 2D LiDAR and binocular camera; S2: Constructing an observation information degradation detection algorithm based on 2D LiDAR; S3: Constructing a robot odometry calculation method based on 2D LiDAR and binocular camera; S4: Constructing an optimized grid map containing Z-axis obstacle information; S5: Constructing a loop closure detection algorithm based on 2D LiDAR and binocular camera. This invention solves the problem of robot mapping failure in indoor SLAM in medical environments due to errors in sensor observation information caused by driving bumps or lighting. Based on judging whether the 2D radar point cloud observation information is degraded, visual information is added to ensure the stability of the SLAM process in complex scenes. The traditional grid framework is modified to ensure the real-time performance of loop closure optimization, and a new hardware system is used to achieve robot and obstacle safety in multi-dynamic environments.

Description

Technical Field

[0001] This invention belongs to the field of simultaneous localization and mapping (SLAM) technology for robots, specifically relating to a SLAM method and system based on 2D LiDAR and camera in a medical environment. Background Technology

[0002] The SLAM implementation process can be described as follows: with existing wheel encoder prediction data, the robot uses its own sensor information, such as laser point cloud information, visual images, and depth information, to determine its own pose in real time and build map information for subsequent localization. As the foundation of robot navigation, SLAM has become a hot research topic in the field of robotics in recent years and is considered to be the core link in realizing autonomous robots.

[0003] Since its inception, the indoor positioning accuracy of robots has continuously improved with the increasing frequency of hardware acquiring observation information and the emergence of various positioning algorithms. After meeting the accuracy requirements of warehouse and home robots, the stability of SLAM has become even more important. However, single observation sensors, due to the fixed content of their observation information, present unique challenges when used for SLAM. For example, the current limitations of indoor 2D laser SLAM are mainly the following two points: 1. Limited observation information is acquired, focusing on only one plane, requiring the robot to operate within the same plane; 2. Obstacles such as metal and glass easily lead to observation errors. Furthermore, the limitations of using a single binocular camera for SLAM are mainly: 1. Sensitive to lighting conditions; 2. Unsatisfactory positioning accuracy.

[0004] Indoor medical care robots differ from warehousing and home robots in the following ways: 1. The repetitive environment of hospitals can easily lead to mismatches in laser point cloud data; 2. The numerous gentle slopes and inclines can cause severe abrupt changes in laser observation information; 3. 24-hour fully automated service is more critical, leading to inconsistent visual observation information; 4. The presence of many dynamic obstacles and rapid movement necessitates the design of additional hardware devices to ensure operational safety.

[0005] In summary, using a single observation sensor to construct the grid map required for localization in a medical environment can lead to observation errors or even mismatches in localization, ultimately resulting in the failure of map construction. Summary of the Invention

[0006] To address the technical problem that existing technologies, when using a single observation sensor to construct a grid map for localization in a medical environment, can lead to observation errors or even mismatches in localization, ultimately resulting in mapping failure, this invention provides a SLAM method and system based on 2D LiDAR and a camera in a medical environment. This solves the problem of ensuring the safety of pedestrians and the robot itself during the autonomous SLAM mapping process.

[0007] The technical solution adopted in this invention is:

[0008] A SLAM method based on 2D LiDAR and camera in a medical environment, characterized by the following specific steps:

[0009] S1: Construct a joint calibration algorithm for 2D LiDAR and binocular camera;

[0010] S2: Construct an algorithm for detecting degradation of observation information based on 2D lidar;

[0011] S3: Construct a robot odometry calculation method based on 2D LiDAR and binocular camera;

[0012] S4: Construct an optimized grid map that includes obstacle information along the z-axis;

[0013] S5: Construct a loop closure detection algorithm based on 2D LiDAR and binocular camera.

[0014] Furthermore, in step S1, in order to avoid the accumulation of calibration errors between the lidar and the binocular camera, a camera-liquid joint calibration algorithm based on planar constraints is adopted. First, the coordinate representation of the calibration board plane in the camera coordinate system is calculated by the QR code on the calibration board plane. At the same time, the laser beam falls on the calibration board plane. Using the coordinates of the laser point in the laser coordinate system and the coordinates of the plane equation in the camera coordinate system, the constraints of the point on the plane are constructed to solve the extrinsic parameters, that is, the relative pose of the lidar and the camera.

[0015] Furthermore, in step S2, the degradation is divided into two types: one is the degradation of the observation information caused by the robot performing a rotational action other than the yaw angle, which is called turbulence degradation; the other is the situation where the robot is in a corridor environment or the observation information is a straight line due to the scanning angle being too small and facing the wall, which is called environmental degradation.

[0016] The specific method for detecting turbulence degradation is as follows: First, calculate the camera poses of the previous and next frames obtained by the binocular visual odometry method. Then, determine whether the changes in the robot's pitch angle and roll angle are greater than the set threshold. If they are greater than the set threshold, observe whether the value after correction by the laser odometry of the current frame exceeds the set threshold. If it exceeds the threshold, it is determined that turbulence degradation has occurred at that moment. The degree of turbulence degradation is recorded as τ1 = 1, and the initial value is τ1 = 0.

[0017] The specific method for detecting environmental degradation is as follows: collect point cloud data of the current frame, calculate the normal vector direction of each scan point, and then perform consistency analysis on the normal vector. If the consistency analysis passes, it is determined that environmental degradation has occurred at that moment. The degree of environmental degradation is denoted as τ2, where 0≤τ2≤1.

[0018] Furthermore, in step S3, based on identifying the degradation of observation information, the method for calculating robot odometry based on 2D LiDAR and binocular cameras includes the following steps:

[0019] Step S31: Construct a separate 2D LiDAR odometry matching residual, specifically including:

[0020] Step S311: Implement the sensor data preprocessing part;

[0021] Step S312: Generate a distance grid map;

[0022] Step S313: Obtain the residual amount of point cloud matching in the current frame;

[0023] Step S32: Construct a separate binocular odometry projection error, specifically including:

[0024] Step S321, Feature point matching;

[0025] Step S322, calculate the reprojection error e2;

[0026] Step S33: The binocular camera and 2D LiDAR are tightly coupled and fused. The specific steps of the fusion algorithm are as follows:

[0027] Step S331: Obtain the joint error function based on step S31;

[0028] Step S332: Perform residual optimization.

[0029] Furthermore, in step S4, the process of constructing optimized raster map information specifically includes:

[0030] Based on the robot joint odometry obtained in step S3, the dense point cloud obtained by the binocular camera is converted into an octree map. The conditions that need to be met in the formation process of the octree map are: the voxels in the octree map have planes that overlap or are parallel to the grid map, and the octree coordinate system and the grid map coordinate system are kept to overlap.

[0031] After the octree map is formed, the obstacle content related to the height is recorded for each grid cell in the occupied grid map. Each occupied grid cell will record a vector of its z-axis obstacle situation.

[0032] Furthermore, in step S5, a loop closure detection algorithm based on 2D LiDAR and a binocular camera is constructed, specifically including:

[0033] Step S51: Set loop closure detection conditions;

[0034] The combined odometry obtained in step S3 is used to determine whether loop closure detection should be performed. The determination condition is: the robot's movement distance reaches the horizontal threshold Δ. dOr the cumulative change in robot angle exceeds the angle threshold Δ θ ;

[0035] Step S52: Perform loop closure detection, obtain loop closure residuals, and further obtain the accurate pose by minimizing the error function.

[0036] A system for implementing the above-described method of simultaneous localization and mapping based on 2D LiDAR and camera, characterized in that the system comprises:

[0037] Robot chassis, used to enable the robot's translation, rotation and steering functions;

[0038] The sensor assembly system is used to sense the surrounding environment and acquire external information, providing observation information for steps S1 to S5.

[0039] Robotic arm systems are used to perform actions such as grasping, assembling, and transporting items;

[0040] Human-computer interaction systems are used to enable the transmission of commands, exchange of information, and operation control with robots, allowing humans and robots to work and cooperate together.

[0041] The power and control system provides energy and control signals to the entire system, ensuring the robot's long-term operation and coordinating the work of its various parts.

[0042] Compared with the prior art, the beneficial effects of the present invention are reflected in the following: based on judging whether the 2D radar point cloud observation information has degraded, the present invention adds visual information to ensure the stability of the SLAM process in complex scenes, modifies the traditional grid frame to ensure the real-time performance of loop closure optimization, and cooperates with a brand-new hardware system to achieve the safety of robots and obstacles in multi-dynamic environments. Attached Figure Description

[0043] Figure 1 This is a flowchart illustrating the method of the present invention.

[0044] Figure 2 This is a schematic diagram of laser vision joint calibration for implementing the method of the present invention.

[0045] Figure 3 This is a schematic diagram of the turbulence degradation process in implementing the method of the present invention.

[0046] Figure 4a This is a schematic diagram illustrating environmental degradation during the implementation of the method of the present invention, when a 2D lidar is in a corridor environment with a scanning angle of 360°.

[0047] Figure 4b This is a schematic diagram illustrating environmental degradation in which the method of the present invention is implemented, when a 2D lidar is positioned facing a wall at a scanning angle of 180°.

[0048] Figure 5 It is an occupied grid map for implementing the method of the present invention.

[0049] Figure 6 It is a distance grid map for implementing the method of the present invention.

[0050] Figure 7 This is a schematic diagram of Z-axis obstacle information for implementing the method of the present invention.

[0051] Figure 8 This is a schematic diagram of a medical care robot that implements the method of the present invention.

[0052] Figure 9 This is a schematic diagram of the chassis of a medical care robot that implements the method of the present invention.

[0053] Figure 10 This is a schematic diagram of the robotic arm of a medical care robot that implements the method of the present invention.

[0054] In the diagram, 101-aluminum alloy support chassis, 102-drive wheel, 103-Mecanum driven wheel, 104-brushless motor, 201-first binocular camera, 202-2D LiDAR, 203-force sensor, 204-wheel encoder, 301-robotic arm mounting platform, 302-308-rotary joints, 309-second binocular camera, 310-robotic gripper, 401-human-machine interface, 402-Bluetooth speaker, 403-force control interaction device, 501-industrial computer, 502-power supply. Detailed Implementation

[0055] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the scope of the present invention.

[0056] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other.

[0057] The present invention will now be described in detail with reference to the accompanying drawings and exemplary embodiments.

[0058] Example 1

[0059] Reference Figure 1 The figure shows a workflow diagram of a method for simultaneous localization and mapping based on 2D LiDAR and camera in a medical environment. The workflow is as follows:

[0060] Step S1: Joint calibration of the binocular camera and lidar fusion system as follows Figure 2As shown, the first and second binocular cameras are MyEye D1000-IR-120 cameras, with a binocular baseline of 120mm, a maximum frame rate of 60FPS, and a resolution of 1280*720. The 2D LiDAR is a Pepperl+Fuchs R2000, with a scanning frequency of 40Hz, 1440 scan points per frame, and a ranging range of 0.15-50m. The laser point cloud will fall onto the calibration board ( Figure 2 From the black dots in the image, we can obtain the coordinates of the laser point cloud in the laser coordinate system. Furthermore, the plane equation of the calibration plate in the camera coordinate system can be obtained via the QR code (Apriltag). Therefore, we can construct constraints on the points on the plane to solve for the transformation from the laser radar to the camera, i.e., (R... cl ,t cl ), R cl Let t be the rotation matrix from the camera coordinate system to the radar coordinate system. cl It is a translation vector.

[0061] Finally, to obtain the optimal solution, N samples need to be collected. cal Joint optimization of the data sets was performed, and the optimized residual term was... The nonlinear least squares problem can be solved using tools such as g2o or ceres. This problem can be represented as:

[0062]

[0063] Where, N i Let n be the number of laser beams falling on the calibration plate in the i-th frame. ci d ci Let P be the 3D normal vector of the calibration plate plane corresponding to the laser in the i-th frame in the camera coordinate system and the distance from the origin of the camera coordinate system to the plane, respectively. lim Let be the coordinates of the m-th laser point in the i-th laser frame in the lidar coordinate system.

[0064] Step S2: Based on the calibration of the extrinsic parameters of the 2D lidar and the binocular camera in step S1, construct an observation information degradation detection algorithm based on the 2D lidar. Note that the observation information degradation detection algorithm is only for the planar observation information of the 2D lidar.

[0065] like Figure 3 As shown in Figure 4, the degradation of observational information can be divided into two types: turbulence degradation and environmental degradation.

[0066] Step S21, the cause of the vibration degradation is as follows Figure 3 As shown. This phenomenon can be described as a sudden change in radar sensor observation information caused by the uneven terrain in a medical setting. This sudden change has a significant impact on the radar odometry obtained subsequently through scanning and matching. Sensors for bump degradation detection include: a binocular camera and an IMU.

[0067] It is worth noting that the two types of sensors mentioned above each have their own advantages and disadvantages when detecting turbulence degradation: the visual odometry obtained through binocular camera observations (the acquisition method is described in detail in step S3) calculates the pitch and roll angles in the robot's pose information. The advantage of this approach is that it does not require additional sensors, but its disadvantage is that there is a small delay in the judgment and it consumes computing resources; the approach using an IMU can directly obtain the pitch and roll angles through IMU-driven feedback, but its disadvantage is that it requires the addition of additional sensors.

[0068] The basis for calculating the degree of turbulence degradation τ1, combining the pitch and roll angles obtained above, is as follows:

[0069]

[0070] In the above formula, θ is the threshold angle for setting the turbulence. Since there is no perfectly smooth plane during robot operation, the pitch and roll angles of the robot's current pose will inevitably have slight fluctuations.

[0071] Step S22, the reasons for environmental degradation are as follows Figure 4a , 4b As shown in Figure 4(a), when a 2D lidar is in a corridor environment with a scanning angle of 360°, or in an environment facing a wall with a scanning angle of 180°, as shown in Figure 4(b), its two-dimensional point cloud normal vector direction features are singular, which is called environmental degradation.

[0072] It is worth noting that environmental degradation only applies to 2D lidar observation information, and does not apply to environmental information after the addition of binocular camera observation information.

[0073] Based on the above, the sensor information for determining environmental degradation only needs to come from the observation information of 2D lidar, and the determination steps are as follows:

[0074] Step S221: Calculate the normal vector information of each scanning point of the 2D LiDAR in the current frame.

[0075] In this embodiment, we first assume that the scan point P and its neighborhood point set N are calculated, and then the centroid P of the point set N is calculated. centriod :

[0076]

[0077] Where n is the number of scan points contained in the point set N, and P i Let the i-th point in the point set N be represented, and then the covariance matrix C is constructed. P :

[0078]

[0079] For C P Perform eigenvalue decomposition:

[0080] C P =V P *D P *V P T (5)

[0081] Among them, V P Let D be the eigenvector matrix of the scan point P. P It is the eigenvalue matrix.

[0082] In summary: the eigenvector corresponding to the smallest eigenvalue is taken as the normal vector of the scan point P.

[0083] Step S222: Calculate the consistency of the normal vectors of the current frame scan points using RANSEC (Random Sample Consistency). The steps are as follows:

[0084] a) First, perform random sampling. In the set of normal vectors calculated in step S221, randomly select a set of normal vectors as the sample set for the current iteration.

[0085] b) Set up a local fitting model, where the metric selected for local fitting is the angle direction of the normal vector, and perform local fitting.

[0086] c) Based on this, set a threshold, calculate the distance from all points to the fitted model, and mark the points that are less than the threshold as interior points.

[0087] d) If the number of interior points is greater than the preset consistency threshold, the model is considered to be consistent.

[0088] e) Repeat the iteration, repeating step ad above according to the number of iterations.

[0089] The optimal model is obtained through the above steps. If the model passes the consistency test, it is considered that the robot is currently in a state of environmental degradation. Based on the degree of consistency, the degree of environmental degradation is set to τ2 (0≤τ2≤1).

[0090] Step S3: Based on the identification of observation information degradation in step S2, the specific steps for constructing a robot joint odometry calculation method based on 2D LiDAR and binocular cameras include:

[0091] Step S31, the steps for constructing a separate 2D lidar odometry matching residual calculation method include:

[0092] Step S311, implement the sensor data preprocessing part.

[0093] At time T_1, wheel odometry and IMU data are acquired, and then the non-laser odometry variables at the current time are obtained by EKF (Extended Kalman) filtering and fusion. The robot pose information at the current time is predicted by using the robot pose information corrected by residual optimization at the previous time T_0.

[0094] Step S312: Generate a distance grid map.

[0095] To ensure the real-time performance of positioning optimization and the accuracy of mapping, and considering that adding a visual odometry module would increase the computational load significantly, we decided to replace the computationally intensive beam model with a spatial likelihood domain model to ensure sufficient computing resources.

[0096] Furthermore, to further reduce the computational cost of obtaining residuals during scan matching and graph optimization, based on information from before the raster map update (see...), Figure 5 The Euclidean distance ε(p) between each grid cell and its nearest occupied grid cell is pre-calculated, thus forming a distance grid map.

[0097] The steps involved in creating a distance raster map can be divided into:

[0098] 1. Distance increase step: Assign distance values ​​to the surrounding grid cells of the occupying grid cell in the occupied grid map, and set the maximum expansion distance d;

[0099] 2. Distance descent step: Assign a distance of 0 to the distance grid corresponding to the occupied grid, such as... Figure 6 As shown.

[0100] It is worth noting that, Figure 6 The distance grid map is formed in the form of d=2.

[0101] Step S313: Obtain the residual amount of point cloud matching in the current frame.

[0102] Based on the distance grid map obtained in the above steps, let R be the robot rotation matrix, t be the translation vector, and n l P represents the number of scan points in the current frame's point cloud, where P is the continuous coordinates of a scan point. ε = [x, y], with a matching map of size m. The point cloud matching residual e1 for the current robot pose is obtained using a lookup table. Specifically, e1 is the sum of the Euclidean distances ε(p) of all laser impact points within the dilated region.

[0103]

[0104] In the above formula, П(m,p) represents the location of the nearest Euclidean grid cell to point p in the current map, which can be obtained by looking up a table.

[0105] Furthermore, if computational resources permit, a more refined Euclidean distance residual for each laser impact point can be obtained using bilinear interpolation. Assuming x,y∈[0,1), the value of the Euclidean distance residual ε(x,y) is derived from the four nearest integer coordinates P. 00 ,P 01 ,P 10 ,P 11 The calculation is as follows:

[0106]

[0107] Furthermore, the descent gradient is calculated using numerical differentiation. The nearest coordinate P is taken. 00 ,P 01 ,P 10 ,P 11 The relevant Euclidean distance J ε The Jacobian matrix is ​​calculated as follows:

[0108]

[0109] In the above formula, r is the resolution of the grid.

[0110] Step S32, the specific steps for constructing a separate binocular odometry projection error acquisition step include:

[0111] Step S321, feature point matching.

[0112] The ORB feature point extraction algorithm, which offers good real-time performance, is used to extract feature points from images from both the left and right cameras. The ORB algorithm combines FAST corner detection and the BRIEF descriptor, enabling the extraction of descriptive feature points while maintaining high speed.

[0113] Furthermore, after obtaining ORB feature points, Hamming distance is used for feature point matching and filtering of incorrect matches. If the Hamming distance between two descriptors is less than a threshold, the match is considered successful.

[0114] Step S322, calculate the reprojection error e2.

[0115] e2 can be defined by projecting a 3D point P onto the image plane using the camera pose and calculating the difference between the pixel coordinates of the projected point and the corresponding feature point. The calculation steps are as follows:

[0116] a) Assume there are n c There are n feature points, and the pixel coordinates p of the i-th feature point are given. i For (u i ,v i The corresponding three-dimensional point is P. i .

[0117] b) Assume the camera intrinsic parameter matrix is ​​C, and the reprojection error of the i-th feature point is e. i S i This is scale information.

[0118] c) The reprojection error of the i-th feature point can be simply defined as:

[0119] e i =(u i ,v i )-project(C,R,t,P i (9)

[0120] Where project is the projection function, which projects the three-dimensional point P. i The projection points are obtained by projecting the camera pose onto the image plane. e2 represents all feature points e. i The sum, as detailed in extended formula (9), is defined as:

[0121]

[0122] Step S33, the fusion algorithm steps for tightly coupled binocular camera and 2D LiDAR can be divided into:

[0123] Step S331: Obtain the joint error function based on step S31.

[0124] First, obtain the radar matching error at this moment as e1. Based on step S32, obtain the reprojection error of each feature point matching between two frames of the camera as e2. In step S2, obtain the observation information degradation values ​​τ1 and τ2, and obtain the joint error function e of the two observation information, as shown in formula (11):

[0125] e=(1-τ1)(1-τ2)e1+τ2e2 (11)

[0126] The above formula can be simply explained as follows: When turbulence-induced degradation occurs, the turbulence causes the point cloud and the distance map to mismatch, resulting in a failure to match properly or even a mismatch. Therefore, the confidence level of the radar's matching error component will drop to zero. Furthermore, the degree of environmental degradation is inversely proportional to the confidence level of the radar's matching error component.

[0127] Step S332: Perform residual optimization.

[0128] From the above, the error function is a nonlinear function, and the optimization quantity includes the robot pose R,t. Therefore, the Lie algebra ξ represents the robot pose, and ξ is a six-dimensional vector. The symbol ^ indicates that the six-dimensional vector is converted into a four-dimensional matrix. Therefore, formula (11) is simplified to Lie algebra form by combining formula (6) and formula (10), that is:

[0129]

[0130] Furthermore, Ceres or g2o can be used to obtain optimized robot pose and joint odometry.

[0131] Step S4, the process of constructing and optimizing raster map information, specifically includes:

[0132] Step S41: Generate an octree map based on the radar observation plane.

[0133] Based on the optimized pose obtained in step S3, an octree data structure is used to represent the map built by the stereo camera. During the construction process, the following three conditions must be maintained:

[0134] 1. The voxels of the octree map are aligned with or parallel to the grid map constructed by the lidar (hereinafter referred to as the initial grid map);

[0135] 2. The voxel boundary size of the plane that overlaps with or is parallel to the initial raster map is the same as the raster size of the initial raster map;

[0136] 3. The coordinate system of the octree map overlaps with the coordinate system of the initial raster map;

[0137] Step S42: Construct optimized raster map information.

[0138] Combining the occupancy markers (information on whether a voxel is an obstacle) contained in the octree map, such as... Figure 7 for Figure 5 The occupancy marker in the z-axis direction corresponding to the seventh row of the initial grid map is marked with a height of 7. The black occupancy marker is the obstacle occupancy marker, marked as 1; the white occupancy marker is the free occupancy marker, marked as 0.

[0139] In summary, Figure 5 The initial raster map point P in (5,7) z-axis vector The range is [0,0,1,1,1,0,0]. Distance assignment is performed to ultimately optimize the raster map. Where d i The height of the center of the i-th voxel is the distance from the initial raster map plane.

[0140] It is worth noting that, considering the computational resource constraints in practical applications, the optimized grid map information is not directly involved in the joint odometer part of step S3. The initial grid map and the range grid map are used for the radar's known map information in step S3. The optimized grid map information described in this step is only used in step S5, i.e., the loop closure detection part, because the frequency of loop closure detection is much lower than the frequency of joint odometer calculation.

[0141] Step S5, constructing a loop closure detection algorithm based on 2D LiDAR and binocular cameras specifically includes:

[0142] Step S51: Set the loop closure detection conditions.

[0143] The main function of loop closure detection is to correct accumulated errors, therefore its frequency should not be too high. Loop closure detection is determined using the combined odometry obtained in step S3, with the criterion being: the robot's movement distance reaches a horizontal threshold Δ. d Or the cumulative change in robot angle exceeds the angle threshold Δ θ .

[0144] Step S52: Perform loop closure detection and obtain the loop closure residual.

[0145] Loop closure can be performed using map-to-map or scan-to-map methods. Considering computational resources, scan-to-map detection, which involves comparing the current frame's observation information with the known map, is preferred. To ensure real-time loop closure performance, the following two aspects are considered:

[0146] 1. Only perform z-axis residual calculation on the in-points in step S22, and perform Euclidean distance residual calculation on the remaining laser scanning points. The calculation steps are the same as in step S31.

[0147] 2. Calculate the matching residual e of the current frame. d This includes the cosine distance residuals of the z-axis quantities of all interior points of the current frame 2D LiDAR, as well as the Euclidean distance residuals of all scan points other than interior points to their nearest obstacles.

[0148] The residual is calculated using the cosine distance from the z-axis direction vector, as shown in the following formula:

[0149]

[0150] In the above formula, S m For the optimized raster map information created in step S4, S n This represents the z-axis vector information corresponding to the in-frame points scanned by the 2D LiDAR in the current frame, n. s The number of interior points, Let P be an interior point. j The z-axis vector information corresponding to the optimized raster map. Interior point P obtained from the octree formed by the stereo camera in the current frame j The z-axis vector information.

[0151] Furthermore, the residual e is obtained by summing the Euclidean distances to all scan points except the interior points. d :

[0152]

[0153] It is worth noting that the averaging function was not included in the above formula because the main purpose of the loop closure detection step is to eliminate accumulated errors. Obviously, in the process of SLAM by 2D LiDAR, the situation that is most likely to lead to accumulated errors or even mismatches is the degradation of observation information as described in step S2. Therefore, the purpose of using the sum of the z-axis cosine distances of all interior points is to ensure that the z-axis observation information provided by the stereo camera can have a higher weight when the observation information degrades.

[0154] Furthermore, a scan point proximity normalization factor was added. α is inversely proportional to the average Euclidean distance from the current frame scan point to the scan center (2D LiDAR rotation axis). Simply put, α is added to map the Euclidean distance to the same scale range as the cosine distance.

[0155] In summary, when the robot operates within the loop closure threshold set in step S51, a node is recorded at intervals of the set threshold (the node content includes the robot pose, 2D laser SLAM observation information, and the z-axis vector of the interior point), for a total of K nodes. The residual e of each node can be obtained through the above steps. d The optimization problem for K poses is then solved by constructing a nonlinear least squares problem, and the final closure residual is E:

[0156]

[0157] In the above formula, D is the number of nodes recorded in the two loop closure detections.

[0158] Furthermore, the optimal pose of each node can be obtained using Ceres or g2o.

[0159] Example 2

[0160] Figure 8 This is a system for simultaneous localization and mapping based on 2D LiDAR and camera, provided in Embodiment 2 of the present invention. Accurate mapping and localization are essential for subsequent interactive tasks by the robot. The system includes:

[0161] The robot chassis is used to enable the robot's translation, rotation, and steering functions. Specifically, the robot chassis structure is as follows: Figure 9 As shown, it is used to provide stable support and mobility for the robot. The support chassis 101, which is made of high-strength aluminum alloy, provides stable support for the robot as a whole; the driving capability is achieved by the drive wheel 102 and the brushless motor 104, and the drive wheel 102, together with the Mecanum driven wheel 103, enables the robot to move.

[0162] The sensor assembly system is used to sense the surrounding environment and acquire external information, providing observation information for steps S1 to S5. Specifically, the sensor assembly system mainly provides observation information for the SLAM algorithm in Embodiment 1, including depth map information provided by the first binocular camera 201, used to find feature points for calculating reprojection errors and forming an octree map; lidar 202-2D used to collect laser scanning point data information for calculating point cloud matching residuals; wheel encoder 204 used for odometer data preprocessing or to provide reference values ​​for the combined odometer; and force sensor 203 used to collect contact force data.

[0163] A robotic arm system is used to perform actions such as grasping, assembling, and transporting objects. Specifically, the structure of a robotic arm system is as follows: Figure 10 As shown, the end effector is equipped with a gripper primarily used for holding and manipulating medical equipment, items, or performing specific actions. It includes a robotic arm mounting platform 301 and a main robotic arm structure. The main robotic arm structure consists of several rotary joints 302-308, with a robotic gripper 310 mounted on the end effector's rotary joints and a second binocular camera 309 mounted on the gripper 310. The addition of the robotic arm system and the human-computer interaction system described below are based on the SLAM algorithm in Embodiment 1; therefore, without the separate modules for the robotic arm system and the human-computer interaction system, the SLAM algorithm in Embodiment 1 does not provide any practical help to the patient.

[0164] The human-machine interaction system is used to transmit commands, exchange information, and control operations with the robot, enabling human-robot collaboration. Specifically, the human-machine interaction system is mainly used to facilitate the use of medical care robots by doctors or patients and to provide psychological rehabilitation for patients. It includes a human-machine interface 401 for touchscreen interaction to achieve specific functions, such as delivering items and online diagnosis; a Bluetooth speaker 402 for voice interaction, such as regularly checking on patient medication and reminding them of precautions; and a force control interaction device 403 to ensure the safe operation of the robot and the safety of surrounding objects in a medical environment.

[0165] The power supply and control system provides energy and control signals to the entire system, ensuring the robot's long-term operation and the coordination of its various components. Specifically, the power supply and control system ensures the robot's long-term operation and the coordination of its various components. It includes an industrial control computer 501, which processes sensor data and further implements the SLAM algorithm in Embodiment 1; and a power supply 502, which keeps the robot running for extended periods.

[0166] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A SLAM method based on 2D LiDAR and camera in a medical environment, characterized in that, The method includes the following specific steps: S1: Construct a joint calibration algorithm for 2D LiDAR and binocular camera; S2: Construct an algorithm for detecting degradation of observation information based on 2D lidar; S3: Construct a robot odometry calculation method based on 2D LiDAR and binocular camera; S4: Constructing includes Optimized grid map of axis obstacle information; S5: Construct a loop closure detection algorithm based on 2D LiDAR and binocular camera; In step S2, there are two types of degradation: one is the degradation of observation information caused by the robot performing a rotational action other than yaw angle, which is called turbulence degradation; the other is the situation where the robot is in a corridor environment or the observation information is a straight line due to the robot being too small in scanning angle and facing the wall, which is called environmental degradation. The specific method for detecting turbulence degradation is as follows: First, calculate the camera poses of the preceding and following frames using binocular visual odometry. Then, determine whether the changes in the robot's pitch and roll angles exceed a set threshold. If they do, observe whether the values ​​corrected by the laser odometry in the current frame exceed the set threshold. If they do, it is determined that turbulence degradation has occurred at that moment, and the degree of turbulence degradation is recorded as [value missing]. initial value ; The specific method for detecting environmental degradation is as follows: Collect point cloud data of the current frame, calculate the normal vector direction of each scan point, and then perform a consistency analysis on the normal vectors. If the consistency analysis passes, it is determined that environmental degradation has occurred at that moment, and the degree of environmental degradation is denoted as [value missing]. 0 ; Based on the identification of degraded observation information, the method for calculating robot odometry using 2D LiDAR and binocular cameras includes the following steps: Step S31: Construct a separate 2D LiDAR odometry matching residual, specifically including: Step S311: Implement the sensor data preprocessing part; Step S312: Generate a distance grid map; Step S313: Obtain the residual amount of point cloud matching in the current frame; Step S32: Construct a separate binocular odometry projection error, specifically including: Step S321, Feature point matching; Step S322, reprojection error calculate; Step S33: The binocular camera and 2D LiDAR are tightly coupled and fused. The specific steps of the fusion algorithm are as follows: Step S331: Obtain the joint error function based on step S31; First, the residual value of the radar's current two-dimensional laser point cloud matching is obtained as follows: Based on step S32, the reprojection error of each feature point matching between two camera frames is obtained as follows: In step S2, the degradation value of the observation information is obtained. and The joint error function of the two observation information is obtained. See formula (11): The above formula can be simply explained as follows: When turbulence degradation occurs, the turbulence will cause the point cloud and the distance map to fail to match well or even mismatch, so the confidence of the radar's matching error part will return to zero; in addition, the degree of environmental degradation is inversely proportional to the confidence of the radar's matching error part. Step S332: Perform residual optimization; In step S4, the process of constructing optimized raster map information specifically includes: Based on the robot joint odometry obtained in step S3, the dense point cloud obtained by the binocular camera is converted into an octree map. The conditions that need to be met in the formation process of the octree map are: the voxels in the octree map have planes that overlap or are parallel to the grid map, and the octree coordinate system and the grid map coordinate system are kept to overlap. After the octtree map is generated, the obstacle content related to height is recorded for each grid cell in the occupied grid map. Each occupied grid cell will record its... A vector representing the obstacle situation along the axis; In step S5, a loop closure detection algorithm based on 2D LiDAR and a binocular camera is constructed, specifically including: Step S51: Set loop closure detection conditions; The combined odometry obtained in step S3 is used to determine whether loop closure detection should be performed. The determination condition is: the robot's movement distance reaches a horizontal threshold. Or the cumulative change in robot angle exceeds the angle threshold. ; Step S52: Perform loop closure detection, obtain loop closure residuals, and further obtain the accurate pose by minimizing the error function.

2. The SLAM method based on 2D LiDAR and camera in a medical environment as described in claim 1, characterized in that, In step S1, to avoid the accumulation of calibration errors between the lidar and the binocular camera, a camera-liquid joint calibration algorithm based on planar constraints is adopted. First, the coordinate representation of the calibration board plane in the camera coordinate system is calculated by using the QR code on the calibration board plane. At the same time, the laser beam falls on the calibration board plane. Using the coordinates of the laser point in the laser coordinate system and the coordinates of the plane equation in the camera coordinate system, the constraints of the point on the plane are constructed to solve the extrinsic parameters, i.e., the relative pose of the lidar and the camera.

3. A system for implementing the SLAM method based on 2D LiDAR and camera in a medical environment as described in any one of claims 1 to 2, characterized in that, The system includes: Robot chassis, used to enable the robot's translation, rotation and steering functions; The sensor assembly system is used to sense the surrounding environment and acquire external information, providing observation information for steps S1 to S5. Robotic arm systems are used to perform actions such as grasping, assembling, and transporting items; Human-computer interaction systems are used to enable the transmission of commands, exchange of information, and operation control with robots, allowing humans and robots to work and cooperate together. The power and control system provides energy and control signals to the entire system, ensuring the robot's long-term operation and coordinating the work of its various parts.

Images