Mobile robot positioning and navigation methods, systems, terminal devices and storage media

Abstract

This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified, and constructs a more accurate map to improve obstacle avoidance accuracy.

Description

Technical Field

[0001] This invention relates to the field of mobile robot technology, and in particular to a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. Background Technology

[0002] Existing visual navigation robotic vacuum cleaners are generally based on navigation systems using a single visual sensor, pure laser navigation systems, or laser navigation systems combined with a single TOF (Time of Flight) camera.

[0003] However, when a single visual sensor builds a map, there is a certain blind spot in the sensor's field of view. Only taller objects can be detected, which reduces the positioning accuracy of objects within the blind spot, making it impossible to obtain a high-accuracy map and achieve high-accuracy obstacle avoidance. Although pure laser navigation systems or navigation systems with laser navigation plus a single TOF camera have high mapping accuracy, they cannot effectively identify the shape of objects. Therefore, they sometimes cannot obtain accurate positioning of the objects to be identified, cannot obtain a high-accuracy map, and cannot achieve high-accuracy obstacle avoidance.

[0004] Therefore, it is necessary to propose a localization and navigation method for mobile robots to obtain accurate positioning of the object to be identified. Summary of the Invention

[0005] The main objective of this invention is to provide a positioning and navigation method, system, terminal device, and storage medium for a mobile robot, aiming to solve the problem that existing navigation systems cannot obtain accurate positioning of objects to be identified, and to build a more accurate map to improve obstacle avoidance accuracy.

[0006] To achieve the above objectives, embodiments of the present invention provide a positioning and navigation method for a mobile robot, wherein the traditional positioning and navigation method for a mobile robot includes:

[0007] The motion state of the mobile robot is detected, and three-dimensional point cloud data of the current scene is acquired through multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot.

[0008] Scene processing is performed based on the 3D point cloud data and the motion state of the mobile robot to obtain scene processing results;

[0009] Based on the scene processing results, the current two-dimensional pose of the mobile robot is inferred, and a working scene map of the mobile robot is constructed based on the two-dimensional pose.

[0010] Optionally, the mobile robot includes a main control board, and the plurality of TOF sensors include: a first TOF sensor and a second TOF sensor. Before the step of performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain the scene processing result, the following steps are included:

[0011] The three-dimensional point cloud data is subjected to distortion processing;

[0012] The step of performing distortion processing on the three-dimensional point cloud data includes:

[0013] The sequence times of the main control board and the TOF sensor are obtained and synchronized to obtain the timestamp of the TOF sensor in the time coordinate system of the main control board;

[0014] Angle compensation is performed on the TOF sensor based on the timestamp.

[0015] Optionally, the step of obtaining and synchronizing the sequence times of the main control board and the TOF sensor to obtain the timestamp of the TOF sensor in the time coordinate system of the main control board includes:

[0016] Obtain the sequence times of the main control board and the TOF sensor, and calculate the sequence time difference between the main control board and the TOF sensor;

[0017] The time sequence of the TOF sensor is converted to the time coordinate axis of the main control board according to the time difference, so as to obtain the timestamp of the TOF sensor in the time coordinate system of the main control board.

[0018] Optionally, the mobile robot further includes an inertial unit, and the step of performing angle compensation on the TOF sensor based on the timestamp includes:

[0019] The attitude of the inertial unit that is closest to the timestamps of the second TOF sensor and the first TOF sensor is obtained, and the compensated attitude of the inertial unit is calculated.

[0020] The angle of the three-dimensional point cloud data is compensated by the compensation posture.

[0021] Optionally, the step of obtaining the attitude of the inertial unit that is closest to the timestamps of the second TOF sensor and the first TOF sensor, and calculating the compensated attitude of the inertial unit, includes:

[0022] Obtain the launch attitude of the inertial unit whose launch time is closest to the timestamp of the first TOF sensor;

[0023] Obtain the return attitude of the inertial unit whose return time is closest to the timestamp of the second TOF sensor;

[0024] The compensated attitude of the inertial unit is calculated based on the launch attitude and the return attitude of the inertial unit.

[0025] Optionally, the step of performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain the scene processing result includes the following before:

[0026] Convert the three-dimensional point cloud data into two-dimensional point cloud data;

[0027] The scene is processed by combining the two-dimensional point cloud data and the motion state of the mobile robot to obtain the scene processing result.

[0028] Optionally, the step of inferring the current two-dimensional pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the two-dimensional pose, includes:

[0029] A low-resolution map is constructed using the scene processing results, and the optimal solution for the low-resolution map is calculated.

[0030] A high-resolution map is constructed using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and the optimal solution of the high-resolution map is calculated.

[0031] The optimal solution of the high-resolution map is optimized using a preset optimization algorithm to obtain the two-dimensional pose;

[0032] Based on the two-dimensional pose, the two-dimensional point cloud data is updated to the working scene map of the mobile robot.

[0033] Optionally, the mobile robot further includes an encoder, and the step of constructing a low-resolution map using the scene processing results and calculating the optimal solution for the low-resolution map includes:

[0034] The encoder processes the data from the inertial unit to obtain a pose estimate.

[0035] The pose estimates are selected to obtain candidate solutions for low-resolution maps, and a queue of candidate solutions for low-resolution maps is constructed.

[0036] The low-resolution map is constructed using the candidate solutions of the low-resolution map and the two-dimensional point cloud data.

[0037] Using the two-dimensional point cloud data, the candidate solutions of the low-resolution map are matched with the low-resolution map within a preset time to obtain the highest matching degree of multiple candidate solutions in the candidate solution queue of the low-resolution map.

[0038] The highest matching degree of multiple candidate solutions in the low-resolution map candidate solution queue is sorted to obtain the first matching degree;

[0039] Based on the first matching degree, the pose estimation value of the candidate solution queue of the low-resolution map is selected, and the selection result is taken as the optimal solution of the low-resolution map.

[0040] Optionally, the step of constructing a high-resolution map using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and calculating the optimal solution of the high-resolution map, includes:

[0041] The optimal solution of the low-resolution map is selected to obtain the candidate solution of the high-resolution map, and a queue of candidate solutions of the high-resolution map is constructed.

[0042] The high-resolution map is constructed using the candidate solutions for the high-resolution map and the two-dimensional point cloud data.

[0043] Using the two-dimensional point cloud data, the high-resolution map candidate solution is matched with the high-resolution map within a preset time period to obtain the highest matching degree of multiple candidate solutions in the high-resolution map candidate solution queue.

[0044] The highest matching degree of multiple candidate solutions in the high-resolution map candidate solution queue is sorted to obtain the second matching degree;

[0045] The pose estimate of the candidate solution queue of the high-resolution map is selected based on the second matching degree, and the selection result is taken as the optimal solution of the high-resolution map.

[0046] Optionally, the step of selecting the pose estimate of the candidate solution queue of the low-resolution map based on the first matching degree and using the selection result as the optimal solution of the low-resolution map includes:

[0047] The candidate solutions in the low-resolution map candidate solution queue are sorted from highest to lowest according to their highest matching degree to obtain the sorting result;

[0048] Calculate the difference between the two highest matching degrees that rank last in the sorting results;

[0049] If the difference in the highest matching degree exceeds a preset difference range, then delete the two candidate solutions with the lower highest matching degree that are ranked lower in the sorting results.

[0050] Optionally, the step of constructing a high-resolution map using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and calculating the optimal solution of the high-resolution map, includes the following:

[0051] The pose estimate of the low-resolution map is replaced with the optimal solution of the high-resolution map.

[0052] Optionally, the mobile robot includes an odometer, and the step of optimizing the optimal solution of the high-resolution map using a preset optimization algorithm to obtain the two-dimensional pose includes:

[0053] If the mobile robot is in a preset scenario, the travel distance measured by the odometer is calculated, and the calculation result is used as the positioning coordinate result.

[0054] Based on the positioning coordinates, the two-dimensional point cloud data is updated to the working scene map of the mobile robot;

[0055] If the mobile robot is not in the preset scene, then the following step is performed: update the two-dimensional point cloud data to the working scene map of the mobile robot according to the two-dimensional pose.

[0056] Optionally, the step of combining the two-dimensional point cloud data and the motion state of the mobile robot to perform scene processing and obtain the scene processing result includes:

[0057] If the mobile robot is detected to be in a pitch state and the pitch exceeds a preset range, the two-dimensional point cloud data outside the preset range will be cropped.

[0058] If the mobile robot is detected to be in a state of no pitch but slipping, then the change in the odometer reading is removed;

[0059] If the mobile robot is detected to be in a state of pitch and slippage, the data change of the odometer is removed, and the heading angle information of the inertial unit is retained.

[0060] Optionally, the step of detecting the motion state of the mobile robot includes:

[0061] The two-dimensional point cloud data is matched using a preset data registration algorithm to obtain the matching difference;

[0062] Calculate the difference between the matching difference and the change in the odometer data to obtain the change in the difference;

[0063] If the change in the difference exceeds a preset threshold, it is determined that the mobile robot is slipping.

[0064] Furthermore, to achieve the above objectives, the present invention also provides a data processing system for industrial instruments, the system comprising:

[0065] The sensor module is used to detect the motion state of the mobile robot and acquire three-dimensional point cloud data of the current scene through multiple TOF sensors, wherein the multiple TOF sensors are installed at different positions of the mobile robot.

[0066] The mapping business control module is used to perform scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain the scene processing result.

[0067] The mapping module is used to infer the current two-dimensional pose of the mobile robot based on the scene processing results, and to construct a working scene map of the mobile robot based on the two-dimensional pose.

[0068] In addition, to achieve the above objectives, the present invention also provides a terminal device, the terminal device comprising: a memory, a processor, and a mobile robot positioning and navigation method stored in the memory and executable on the processor, wherein when the mobile robot positioning and navigation program is executed by the processor, the steps of the mobile robot positioning and navigation method as described above are implemented.

[0069] Furthermore, to achieve the above objectives, the present invention also provides a computer-readable storage medium storing a program for the positioning and navigation of a mobile robot, wherein when the program for the positioning and navigation of the mobile robot is executed by a processor, the program implements the steps of the positioning and navigation method for the mobile robot as described above.

[0070] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. Acquiring 3D point cloud data of the current scene using multiple TOF sensors, installed at different positions on the mobile robot, provides more comprehensive 3D point cloud data of the current scene, avoiding blind spots due to a single sensor's field of view, improving positioning accuracy, and making mapping more accurate; combining the 3D point cloud data and the motion state of the mobile robot for scene processing removes errors in data collected under special scenarios; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose, thereby constructing a more accurate map to improve obstacle avoidance accuracy. Attached Figure Description

[0071] Figure 1 This is a schematic diagram of the functional modules of the terminal device to which the positioning and navigation device of the mobile robot of the present invention belongs;

[0072] Figure 2 This is a flowchart illustrating the first embodiment of the positioning and navigation method for a mobile robot according to the present invention.

[0073] Figure 3 This is a flowchart illustrating the second embodiment of the positioning and navigation method for a mobile robot according to the present invention.

[0074] Figure 4 This is a flowchart illustrating the third embodiment of the positioning and navigation method for a mobile robot according to the present invention.

[0075] Figure 5 This is a schematic diagram of the first refinement process for distortion processing of three-dimensional point cloud data in an embodiment of the positioning and navigation method for mobile robots of the present invention.

[0076] Figure 6 This is a schematic diagram of the second refinement process for distorting three-dimensional point cloud data in an embodiment of the positioning and navigation method for mobile robots of the present invention;

[0077] Figure 7 This is a schematic diagram of the third refinement process for distorting 3D point cloud data in an embodiment of the positioning and navigation method for mobile robots of the present invention;

[0078] Figure 8 This is a schematic diagram of the first detailed process for constructing a working scene map of a mobile robot in an embodiment of the positioning and navigation method for a mobile robot of the present invention;

[0079] Figure 9 This is a schematic diagram of a second detailed process for constructing a working scene map of a mobile robot in an embodiment of the positioning and navigation method for a mobile robot of the present invention;

[0080] Figure 10 This is a schematic diagram of the third refinement process for constructing a work scene map of a mobile robot in an embodiment of the positioning and navigation method for a mobile robot of the present invention;

[0081] Figure 11 This is a schematic diagram of the fourth refinement process for constructing a work scene map of a mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention;

[0082] Figure 12 This is a schematic diagram of the fifth detailed process for constructing a work scene map of a mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention.

[0083] Figure 13 This is a schematic diagram of the sixth detailed process for constructing a work scene map of a mobile robot in an embodiment of the positioning and navigation method for a mobile robot of the present invention;

[0084] Figure 14This is a detailed flowchart illustrating the first scenario-specific processing of the mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention.

[0085] Figure 15 This is a schematic diagram of a second detailed process for handling special scenarios of a mobile robot in an embodiment of the positioning and navigation method for a mobile robot of the present invention;

[0086] Figure 16 This is a flowchart illustrating the fourth embodiment of the positioning and navigation method for a mobile robot according to the present invention.

[0087] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0088] It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0089] The main solution of this invention is as follows: Detect the motion state of the mobile robot and acquire 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different locations on the mobile robot; perform scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain scene processing results; infer the current 2D pose of the mobile robot based on the scene processing results, and construct a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate the object to be identified, and constructs a more accurate map to improve obstacle avoidance accuracy.

[0090] Technical terms involved in the embodiments of this invention:

[0091] TOF (Time of Flight) measures the distance between a sensor and an object based on the time difference between the emission of a signal and its return to the sensor after being reflected by the object.

[0092] ICP (Iterative Closest Point) is the most classic data registration algorithm. It finds corresponding point pairs between the source and target point clouds, constructs rotation and translation matrices based on these pairs, and uses these matrices to transform the source point cloud into the target point cloud's coordinate system. It estimates the error function between the transformed source and target point clouds. If the error function value is greater than a threshold, the above calculation is iterated until a given error requirement is met.

[0093] IMU (Inertial Measurement Unit) is a device that measures the three-axis attitude angles (or angular rates) and acceleration of an object.

[0094] Ceres is an open-source C++ library for modeling and solving large, complex optimization problems. It can be used to solve nonlinear least-squares problems with boundary constraints and general unconstrained optimization problems. It is a mature, feature-rich, and high-performance library.

[0095] Point cloud data refers to a collection of vectors in a three-dimensional coordinate system. Besides geometric location, point cloud data sometimes also contains color information. Color information is typically obtained by acquiring color images from a camera and then assigning the color information (RGB) of the corresponding pixels to the corresponding points in the point cloud. Intensity information is obtained from the echo intensity collected by a laser scanner receiver. This intensity information is related to the target's surface material, roughness, incident angle, and the instrument's emission energy and laser wavelength.

[0096] 3D (three-dimensional): A dimension refers to a spatial system formed by adding a direction vector to a two-dimensional planar system. Three dimensions are the three axes of a coordinate system: the x-axis, y-axis, and z-axis, where x represents left-right space, y represents front-back space, and z represents top-bottom space.

[0097] 2D (two-dimensional): Two-dimensional refers to the two axes of a coordinate system, namely the x-axis and the y-axis, where x represents the left-right space and y represents the front-back space.

[0098] Depth image: Set up two TOF cameras and acquire the field of view of each TOF camera; select multiple specific points in the overlapping area of ​​the field of view of the two TOF cameras to acquire the depth image of the two TOF cameras.

[0099] Slam (Simultaneous Localization and Mapping), also known as CML (Concurrent Mapping and Localization), allows a robot to move from an unknown location in an unknown environment. During the movement, the robot performs its own localization based on its location and the map, and simultaneously builds an incremental map based on its localization, thus achieving autonomous localization and navigation.

[0100] Cartographer: Generally refers to a mapmaker. Cartography is the visualization and representation of data on maps.

[0101] With the development of TOF camera module technology, more and more commercially available machines are using TOF cameras.

[0102] Most existing visual navigation robot vacuums are based on a single visual sensor, which is installed directly above the robot. Although it can also obtain the location of the object being measured, the mapping of a single visual sensor is limited by the sensor's field of view, resulting in a certain blind spot. Only taller objects can be detected, thus reducing the positioning accuracy in the blind spot.

[0103] Pure laser navigation systems or laser navigation plus a single TOF camera can achieve obstacle avoidance. Although laser navigation mapping has high accuracy, single-line radar sensors cannot effectively identify the shape of objects, so they cannot achieve high-accuracy obstacle avoidance.

[0104] This invention provides a solution to address the problems of existing navigation systems, such as inaccurate positioning of objects to be identified, inaccurate mapping when the field of view is obstructed, and large errors of single Time-of-Flight (TOF) sensors. It acquires 3D point cloud data of the current scene using multiple TOF sensors installed at different locations on a mobile robot, thus obtaining more comprehensive 3D point cloud data. This 3D point cloud data is then converted into 2D point cloud data, allowing for better projection of the 3D image of the real space onto a planar map. Scene processing is performed by combining the 2D point cloud data with the mobile robot's motion state to eliminate errors in data collected under specific scenarios. Based on this data, pose analysis is then conducted to construct a more accurate map, thereby improving obstacle avoidance accuracy.

[0105] Specifically, refer to Figure 1 , Figure 1 This is a functional module diagram of the terminal device to which the positioning and navigation device of the mobile robot of the present invention belongs. The positioning and navigation device of the mobile robot can be a device independent of the terminal device, capable of image processing and network model training, and can be carried on the terminal device in hardware or software form. The terminal device can be a mobile robot, a mobile phone, a tablet computer, or other intelligent mobile terminal with data processing capabilities, or it can be a fixed terminal device or server with data processing capabilities.

[0106] In this embodiment, the terminal device to which the positioning and navigation device of the mobile robot belongs includes at least an output module 110, a processor 120, a memory 130, and a communication module 140.

[0107] The memory 130 stores the operation method and the positioning and navigation program of the mobile robot. The positioning and navigation device of the mobile robot can acquire image data of the current working scene, convert the acquired image data into two-dimensional point cloud data, calculate the pose data of the mobile robot, and store the two-dimensional point cloud data and the pose data of the mobile robot in the memory 130. The output module 110 can be a display screen, etc. The communication module 140 can include a WIFI module, a mobile communication module, and a Bluetooth module, etc., and communicates with external devices or servers through the communication module 140.

[0108] When the mobile robot's positioning and navigation program in memory 130 is executed by the processor, the following steps are performed:

[0109] The motion state of the mobile robot is detected, and three-dimensional point cloud data of the current scene is acquired through multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot.

[0110] Scene processing is performed based on the 3D point cloud data and the motion state of the mobile robot to obtain scene processing results;

[0111] Based on the scene processing results, the current two-dimensional pose of the mobile robot is inferred, and a working scene map of the mobile robot is constructed based on the two-dimensional pose.

[0112] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0113] The three-dimensional point cloud data is subjected to distortion processing;

[0114] The step of performing distortion processing on the three-dimensional point cloud data includes:

[0115] The sequence times of the main control board and the TOF sensor are obtained and synchronized to obtain the timestamp of the TOF sensor in the time coordinate system of the main control board;

[0116] Angle compensation is performed on the TOF sensor based on the timestamp.

[0117] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0118] Obtain the sequence times of the main control board and the TOF sensor, and calculate the sequence time difference between the main control board and the TOF sensor;

[0119] The time sequence of the TOF sensor is converted to the time coordinate axis of the main control board according to the time difference, so as to obtain the timestamp of the TOF sensor in the time coordinate system of the main control board.

[0120] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0121] The attitude of the inertial unit that is closest to the timestamps of the second TOF sensor and the first TOF sensor is obtained, and the compensated attitude of the inertial unit is calculated.

[0122] The angle of the three-dimensional point cloud data is compensated by the compensation posture.

[0123] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0124] Obtain the launch attitude of the inertial unit whose launch time is closest to the timestamp of the first TOF sensor;

[0125] Obtain the return attitude of the inertial unit whose return time is closest to the timestamp of the second TOF sensor;

[0126] The compensated attitude of the inertial unit is calculated based on the launch attitude and the return attitude of the inertial unit.

[0127] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0128] Convert the three-dimensional point cloud data into two-dimensional point cloud data;

[0129] The scene is processed by combining the two-dimensional point cloud data and the motion state of the mobile robot to obtain the scene processing result.

[0130] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0131] A low-resolution map is constructed using the scene processing results, and the optimal solution for the low-resolution map is calculated.

[0132] A high-resolution map is constructed using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and the optimal solution of the high-resolution map is calculated.

[0133] The optimal solution of the high-resolution map is optimized using a preset optimization algorithm to obtain the two-dimensional pose;

[0134] Based on the two-dimensional pose, the two-dimensional point cloud data is updated to the working scene map of the mobile robot.

[0135] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0136] The encoder processes the data from the inertial unit to obtain a pose estimate.

[0137] The pose estimates are selected to obtain candidate solutions for low-resolution maps, and a queue of candidate solutions for low-resolution maps is constructed.

[0138] The low-resolution map is constructed using the candidate solutions of the low-resolution map and the two-dimensional point cloud data.

[0139] Using the two-dimensional point cloud data, the candidate solutions of the low-resolution map are matched with the low-resolution map within a preset time to obtain the highest matching degree of multiple candidate solutions in the candidate solution queue of the low-resolution map.

[0140] The highest matching degree of multiple candidate solutions in the low-resolution map candidate solution queue is sorted to obtain the first matching degree;

[0141] Based on the first matching degree, the pose estimation value of the candidate solution queue of the low-resolution map is selected, and the selection result is taken as the optimal solution of the low-resolution map.

[0142] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0143] The optimal solution of the low-resolution map is selected to obtain the candidate solution of the high-resolution map, and a queue of candidate solutions of the high-resolution map is constructed.

[0144] The high-resolution map is constructed using the candidate solutions for the high-resolution map and the two-dimensional point cloud data.

[0145] Using the two-dimensional point cloud data, the high-resolution map candidate solution is matched with the high-resolution map within a preset time period to obtain the highest matching degree of multiple candidate solutions in the high-resolution map candidate solution queue.

[0146] The highest matching degree of multiple candidate solutions in the high-resolution map candidate solution queue is sorted to obtain the second matching degree;

[0147] The pose estimate of the candidate solution queue of the high-resolution map is selected based on the second matching degree, and the selection result is taken as the optimal solution of the high-resolution map.

[0148] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0149] The candidate solutions in the low-resolution map candidate solution queue are sorted from highest to lowest according to their highest matching degree to obtain the sorting result;

[0150] Calculate the difference between the two highest matching degrees that rank last in the sorting results;

[0151] If the difference in the highest matching degree exceeds a preset difference range, then delete the two candidate solutions with the lower highest matching degree that are ranked lower in the sorting results.

[0152] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0153] The pose estimate of the low-resolution map is replaced with the optimal solution of the high-resolution map.

[0154] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0155] If the mobile robot is in a preset scenario, the travel distance measured by the odometer is calculated, and the calculation result is used as the positioning coordinate result.

[0156] Based on the positioning coordinates, the two-dimensional point cloud data is updated to the working scene map of the mobile robot;

[0157] If the mobile robot is not in the preset scene, then the following step is performed: update the two-dimensional point cloud data to the working scene map of the mobile robot according to the two-dimensional pose.

[0158] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0159] If the mobile robot is detected to be in a pitch state and the pitch exceeds a preset range, the two-dimensional point cloud data outside the preset range will be cropped.

[0160] If the mobile robot is detected to be in a state of no pitch but slipping, then the change in the odometer reading is removed;

[0161] If the mobile robot is detected to be in a state of pitch and slippage, the data change of the odometer is removed, and the heading angle information of the inertial unit is retained.

[0162] Furthermore, when the mobile robot's localization and navigation program in memory 130 is executed by the processor, the following steps are also performed:

[0163] The two-dimensional point cloud data is matched using a preset data registration algorithm to obtain the matching difference;

[0164] Calculate the difference between the matching difference and the change in the odometer data to obtain the change in the difference;

[0165] If the change in the difference exceeds a preset threshold, it is determined that the mobile robot is slipping.

[0166] This embodiment, through the above-described scheme, specifically detects the motion state of the mobile robot and acquires 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different locations on the mobile robot; scene processing is performed based on the 3D point cloud data and the motion state of the mobile robot to obtain scene processing results; based on the scene processing results, the current 2D pose of the mobile robot is inferred, and a working scene map of the mobile robot is constructed based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate the object to be identified, and constructs a more accurate map to improve obstacle avoidance accuracy.

[0167] Based on, but not limited to, the terminal device architecture described above, embodiments of the method of the present invention are proposed.

[0168] Reference Figure 2 , Figure 2 This is a flowchart illustrating the first embodiment of the positioning and navigation method for a mobile robot according to the present invention. The positioning and navigation method for the mobile robot includes:

[0169] Step S101: Detect the motion state of the mobile robot and acquire 3D point cloud data of the current scene through multiple TOF sensors, wherein the multiple TOF sensors are installed at different positions of the mobile robot.

[0170] The subject executing the method in this embodiment can be a positioning and navigation device or a mobile robot. The positioning and navigation device can also be deployed on the mobile robot. This embodiment uses a mobile robot as an example.

[0171] This embodiment addresses the problem of not being able to obtain accurate positioning of the object to be identified by designing a positioning and navigation method for a mobile robot, constructing a more accurate map, and improving the accuracy of obstacle avoidance.

[0172] The system architecture involved in the mobile robot localization and navigation method of this embodiment may include: a sensor module, a sensor processing module, a mapping service control module, and a mapping module; wherein, the sensor module may be a TOF sensor module, the mapping service control module may be a SLAM mapping service control module, and the mapping module may be a SLAM mapping module.

[0173] The system includes a sensor module for detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors installed at different locations on the mobile robot; a sensor processing module for converting the 3D point cloud data into 2D point cloud data; a mapping control module for performing scene processing based on the 2D point cloud data and the motion state of the mobile robot to obtain a scene processing result; and a mapping module for inferring the current 2D pose of the mobile robot based on the scene processing result and constructing a map of the mobile robot's working scene based on the 2D pose.

[0174] First, the TOF sensor module acquires image data of the current scene and converts it into 3D point cloud data. Then, the sensor processing module converts the 3D point cloud data provided by the TOF sensor module into 2D point cloud format data and sends it to the SLAM mapping business control module for filtering. Finally, the SLAM mapping business control module sends the filtered data to the SLAM mapping module for 2D mapping.

[0175] As one implementation method, in this embodiment, firstly, the motion state of the mobile robot is detected, data of the current scene is acquired through multiple TOF sensors, and the data of the current scene is converted into three-dimensional point cloud data.

[0176] The mobile robot has multiple Time-of-Flight (TOF) sensors located at different positions within the robot. In some examples, the robot has two TOF sensors: a first TOF sensor and a second TOF sensor. The first TOF sensor can be a front-mounted sensor, and the second TOF sensor can be a right- or left-mounted sensor. Alternatively, the first TOF sensor can be either a right- or left-mounted sensor, while the second TOF sensor is a front-mounted sensor. When the TOF sensor module acquires image data of the current scene, it first converts the image data into depth data, and then converts the depth data into 3D point cloud data.

[0177] If flying point noise, multipath interference or other issues occur, flying point filtering, inter-frame filtering, voxel filtering and other filtering processes are performed. The processing is implemented by the TOF sensor module provided by a third-party supplier. This process is integrated on the 1806 chip and independently processes the data acquired by the TOF sensor.

[0178] As another implementation, in this embodiment, the mobile robot has a front-mounted TOF sensor module and a right-mounted TOF sensor module. When the TOF sensor module acquires the image data of the current scene, it first converts the image data of the current scene into depth data, and then converts the depth data into 3D point cloud data.

[0179] If flying point noise, multipath interference or other issues occur, flying point filtering, inter-frame filtering, voxel filtering and other filtering processes are performed. The processing is implemented by the TOF sensor module provided by a third-party supplier. This process is integrated on the 1806 chip and independently processes the data acquired by the TOF sensor.

[0180] As another implementation, in this embodiment, the mobile robot has a left-side TOF sensor module and a right-side TOF sensor module. When the TOF sensor module acquires the image data of the current scene, it first converts the image data of the current scene into depth data, and then converts the depth data into 3D point cloud data.

[0181] If flying point noise, multipath interference or other issues occur, flying point filtering, inter-frame filtering, voxel filtering and other filtering processes are performed. The processing is implemented by the TOF sensor module provided by a third-party supplier. This process is integrated on the 1806 chip and independently processes the data acquired by the TOF sensor.

[0182] As another implementation method, in this embodiment, the mobile robot has a left-side TOF sensor module and a front-side TOF sensor module. When the TOF sensor module acquires the image data of the current scene, it first converts the image data of the current scene into depth data, and then converts the depth data into 3D point cloud data.

[0183] If flying point noise, multipath interference or other issues occur, flying point filtering, inter-frame filtering, voxel filtering and other filtering processes are performed. The processing is implemented by the TOF sensor module provided by a third-party supplier. This process is integrated on the 1806 chip and independently processes the data acquired by the TOF sensor.

[0184] Therefore, by acquiring 3D point cloud data of the current scene through multiple TOF sensors, more comprehensive image data of the current scene can be obtained, thereby constructing a more accurate map to improve obstacle avoidance accuracy.

[0185] Step S102: Based on the 3D point cloud data and the motion state of the mobile robot, scene processing is performed to obtain the scene processing result.

[0186] In this embodiment, the three-dimensional point cloud data is converted into two-dimensional point cloud data, and the scene is processed by combining the two-dimensional point cloud data and the motion state of the mobile robot to obtain the scene processing result.

[0187] As one implementation method, the sensor processing module acquires 3D point cloud data provided by two TOF sensor modules and processes the acquired 3D point cloud data as follows:

[0188] First, the distance and angle between each point and the machine center are calculated, and then projected onto a 360-degree simulated laser data set. When projected points with the same angle appear, the point with the shorter distance is retained. Finally, the sensor processing module merges the data from the two TOF sensors into a single 2D point cloud dataset.

[0189] Therefore, by converting the three-dimensional point cloud data into two-dimensional point cloud data through the sensor processing module, the acquired 3D information can be better converted into 2D information, thereby constructing a more accurate map to improve the accuracy of obstacle avoidance.

[0190] Furthermore, scene processing is performed by combining 2D point cloud data with relevant parameters of the mobile robot's motion state.

[0191] In this embodiment, the SLAM mapping service control module receives 2D point cloud data from the sensor processing module and performs scene processing by combining relevant parameters of the mobile robot's motion state. Scene processing refers to the process of processing the 3D point cloud data acquired by the mobile robot based on its motion state to obtain 3D point cloud data that conforms to the real-world scene. For example, when the mobile robot pitches, point clouds hitting the ground and point clouds at excessively high altitudes are deleted.

[0192] Furthermore, when the mobile robot slips, its relevant parameters can be adjusted to restore it to a normal working state.

[0193] Therefore, by processing the 2D point cloud data and relevant parameters of the mobile robot's motion state, the accuracy of the map can be improved.

[0194] Step S103: Based on the scene processing results, infer the current two-dimensional pose of the mobile robot, and construct a working scene map of the mobile robot based on the two-dimensional pose.

[0195] In this embodiment, more accurate information is obtained based on the scene processing results, thereby inferring a more accurate 2D pose of the mobile robot; then, the 2D point cloud data is synchronized to the mobile robot's working scene map according to the mobile robot's current 2D pose.

[0196] As one implementation method, in this embodiment, the SLAM mapping module receives TOF sensor data processed by the mapping business control module, infers the robot's current 2D pose, and updates the map.

[0197] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified. By acquiring 3D point cloud data of the current scene using multiple TOF sensors, more comprehensive image data of the current scene can be obtained, thereby constructing a more accurate map to improve obstacle avoidance accuracy.

[0198] Reference Figure 3 , Figure 3 This is a flowchart illustrating the second embodiment of the positioning and navigation method for the mobile robot of the present invention.

[0199] In this embodiment, the system architecture involved in the positioning and navigation method of the mobile robot includes a TOF sensor module, a sensor processing module, a SLAM mapping business control module, a SLAM mapping module, an inertial unit, and an encoder.

[0200] The TOF sensor module includes a front-facing TOF sensor and a left-facing TOF sensor, replacing the 2D radar. First, the TOF sensor module acquires image data of the current scene and converts it into 3D point cloud data. Then, the sensor processing module converts the 3D point cloud data provided by the TOF sensor module into 2D point cloud format data and sends it to the SLAM mapping business control module for filtering. Finally, the SLAM mapping business control module sends the filtered data to the SLAM mapping module for 2D mapping.

[0201] Among them, the synchronous positioning and mapping service layer belongs to the SLAM mapping service control module, and the synchronous positioning and mapping algorithm layer belongs to the SLAM mapping module.

[0202] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified. By acquiring 3D point cloud data of the current scene using multiple TOF sensors, more comprehensive image data of the current scene can be obtained, thereby constructing a more accurate map to improve obstacle avoidance accuracy.

[0203] Reference Figure 4 , Figure 4 A flowchart illustrating the third embodiment of the positioning and navigation method for a mobile robot according to the present invention. Based on the above... Figure 2 In the embodiment shown, the mobile robot includes a main control board and multiple TOF sensors, including a front TOF sensor and a left TOF sensor. Step S102: Before obtaining the scene processing result, scene processing is performed based on the 3D point cloud data and the motion state of the mobile robot, the following steps are also included:

[0204] Step S104: Perform distortion processing on the three-dimensional point cloud data.

[0205] To synchronize the 3D point cloud data of the current scene acquired by multiple TOF sensors, it is necessary to synchronize the 3D point cloud data of the current scene acquired by multiple TOF sensors.

[0206] As one implementation method, in this embodiment, the images acquired by multiple TOF sensors can be unified by unifying the parameters of the images acquired by multiple TOF sensors.

[0207] Specifically, step S104 includes:

[0208] Step S1041: Obtain and synchronize the sequence time of the main control board and the TOF sensor to obtain the timestamp of the TOF sensor in the time coordinate system of the main control board.

[0209] Since multiple Time-of-Flight (TOF) sensors are located at different positions on the mobile robot, there is a problem of asynchronous image capture times. To solve this problem, the specific solution is as follows:

[0210] In this embodiment, the mobile robot includes a main control board. First, the sequence times of the mobile robot's main control board and the TOF sensor are obtained; then, the sequence times of the mobile robot's main control board and the TOF sensor are unified according to the same coordinate axis to obtain the timestamps of each TOF sensor in the main control board's time coordinate system.

[0211] The sequence time of the mobile robot main control board and the TOF sensor is obtained according to a preset standard, which includes 5 frames / s, 10 frames / s, etc. The preset standard can be set according to the actual situation, and this embodiment does not make a specific limitation on it.

[0212] More specifically, firstly, the sequence times of the mobile robot's main control board and the TOF sensor are obtained; then, the sequence times of the mobile robot's main control board and the TOF sensor are unified according to the same coordinate axis to obtain the timestamp of the main control board in the time coordinate system of each TOF sensor.

[0213] Therefore, by unifying the sequence time of the mobile robot's main control board and the TOF sensor according to the same coordinate axis, the problem of different timing accuracy between the TOF sensor chip and the main control board chip as time progresses can be solved, thus achieving the goal of time synchronization.

[0214] Step S1042: Perform angle compensation on the TOF sensor according to the timestamp.

[0215] As one implementation method, in this embodiment, angle compensation of the TOF sensor based on the timestamp can unify the image data of the current scene acquired by each TOF sensor, thereby constructing a more accurate map to improve the accuracy of obstacle avoidance.

[0216] Therefore, by distorting the 3D point cloud data, the 3D point cloud data of the current scene acquired by multiple TOF sensors can be synchronized, thereby constructing a more accurate map to improve the accuracy of obstacle avoidance.

[0217] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified. By distorting the 3D point cloud data, the 3D point cloud data of the current scene acquired by multiple TOF sensors can be synchronized, constructing a more accurate map to improve obstacle avoidance accuracy.

[0218] Reference Figure 5 , Figure 5 This is a schematic diagram of the first refinement process for distortion processing of 3D point cloud data in an embodiment of the positioning and navigation method for a mobile robot of the present invention. Based on the above... Figure 4 In the embodiment shown, step S1041: acquiring and synchronizing the sequence time of the main control board and the TOF sensor, and obtaining the timestamp of the TOF sensor in the time coordinate system of the main control board includes:

[0219] Step S10411: Obtain the sequence time of the main control board and the TOF sensor, and calculate the sequence time difference between the main control board and the TOF sensor.

[0220] To address the issue of differing timing start times due to variations in the startup times of the main control board and the TOF sensor, the specific solution is as follows:

[0221] As one implementation method, in this embodiment, firstly, the sequence time Tmain after the main control board of the mobile robot starts up and the sequence time Tmodule after the TOF sensor starts up are obtained; then, the time difference Delta-T between the sequence time after the main control board of the mobile robot starts up and the sequence time after the TOF sensor starts up is calculated.

[0222] The sequence time of the mobile robot main control board and the TOF sensor is obtained according to a preset standard, which includes 5 frames / s, 10 frames / s, etc. The preset standard can be set according to the actual situation, and this embodiment does not make a specific limitation on it.

[0223] Step S10412: Convert the sequence time of the TOF sensor to the time coordinate axis of the main control board according to the time difference to obtain the timestamp of the TOF sensor in the time coordinate system of the main control board.

[0224] As one implementation method, in this embodiment, the time of the TOF sensor is converted to the time coordinate of the main control board to obtain the timestamp of the TOF sensor in the time coordinate system of the main control board.

[0225] Where T module = T main controller + Delta - T.

[0226] Therefore, by converting the time of the TOF sensor to the time coordinate of the main control board, the time of the main control board and the TOF sensor module can be synchronized, thereby building a more accurate map and improving the accuracy of obstacle avoidance.

[0227] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified. By converting the time of the TOF sensors to the time coordinates of the main control board, the time of the main control board and the TOF sensor module can be synchronized, thereby constructing a more accurate map to improve obstacle avoidance accuracy.

[0228] Reference Figure 6 , Figure 6 This is a schematic diagram illustrating the second refinement process for distortion processing of 3D point cloud data in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 4 In the embodiment shown, the mobile robot further includes an inertial unit. Step S1042: Performing angle compensation on the TOF sensor based on the timestamp includes:

[0229] Step S10421: Obtain the attitude of the inertial unit that is closest to the timestamp of the left TOF sensor and the front TOF sensor, and calculate the compensated attitude of the inertial unit.

[0230] Step S10422: Angle compensation is performed on the three-dimensional point cloud data using the compensation posture.

[0231] In this embodiment, the mobile robot also includes an inertial unit, which is a device for measuring the three-axis attitude angles (or angular rates) and acceleration of an object. Generally, an inertial unit includes three single-axis accelerometers and three single-axis gyroscopes. The accelerometers detect the acceleration signals of the object along the three independent axes of the carrier coordinate system, while the gyroscopes detect the angular velocity signals of the carrier relative to the navigation coordinate system. By measuring the angular velocity and acceleration of the object in three-dimensional space, the object's attitude is calculated. In this embodiment, the attitude of the mobile robot detected by the inertial unit is taken as the attitude of the inertial unit.

[0232] The mobile robot's Time-of-Flight (TOF) sensors include a front-mounted TOF sensor and a left-mounted TOF sensor. Because the two TOF sensors take images at different times, motion distortion processing is required. The specific solution is as follows:

[0233] First, based on the timestamps of the left and front TOF sensors in the main control board's time coordinate system, the attitude of the inertial unit (IU) closest to the timestamps of the left and front TOF sensors is obtained. Then, based on the attitude of the IU closest to the timestamps of the left and front TOF sensors, the compensated attitude of the IU is calculated. Finally, based on the compensated attitude of the IU, the 3D point cloud data acquired by the left and front TOF sensors is processed to synchronize the 3D point cloud data. The compensated attitude of the IU is the attitude that can synchronize the image capture times of the two TOF sensors.

[0234] Therefore, based on the compensated attitude of the inertial unit, the 3D point cloud data acquired by the left TOF sensor and the front TOF sensor are processed to improve the accuracy of the map and the accuracy of obstacle avoidance.

[0235] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified. By processing the 3D point cloud data acquired by the left-side TOF sensor and the front-side TOF sensor according to the compensated attitude of the inertial unit, the accuracy of the map and the accuracy of obstacle avoidance are improved.

[0236] Reference Figure 7 , Figure 7This is a schematic diagram illustrating the third refinement process for distortion processing of 3D point cloud data in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 6 In the embodiment shown, step S10421: obtaining the attitude of the inertial unit that is closest to the timestamps of the left-side TOF sensor and the front-side TOF sensor, and calculating the compensated attitude of the inertial unit includes:

[0237] Step S104211: Obtain the launch attitude of the inertial unit whose launch time is closest to the launch time of the front TOF sensor;

[0238] Step S104212: Obtain the return attitude of the inertial unit that is closest to the return time of the left-side TOF sensor timestamp.

[0239] As one implementation method, in this embodiment, the launch attitude of the inertial unit whose launch time is closest to the timestamp of the front TOF sensor is obtained; the return attitude of the inertial unit whose return time is closest to the timestamp of the left TOF sensor is obtained.

[0240] As another implementation, in this embodiment, the launch attitude of the inertial unit whose launch time is closest to the timestamp of the left TOF sensor is obtained; the return attitude of the inertial unit whose return time is closest to the timestamp of the front TOF sensor is obtained.

[0241] Step S104213: Calculate the compensation attitude of the inertial unit based on the launch attitude and return attitude of the inertial unit.

[0242] As one implementation method, in this embodiment, the attitude data that needs to be mutually compensated is calculated based on the launch attitude and return attitude of the inertial unit, that is: compensation attitude = launch attitude of the inertial unit - return attitude of the inertial unit.

[0243] Therefore, by compensating the attitude of the inertial unit, angle compensation can be performed between TOF sensors, thereby constructing a more accurate map to improve obstacle avoidance accuracy.

[0244] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified. By using the compensating attitude of the inertial unit, angle compensation can be performed between the TOF sensors, thereby constructing a more accurate map to improve obstacle avoidance accuracy.

[0245] Reference Figure 8 , Figure 8 This is a schematic diagram illustrating the first detailed process for constructing a work scene map of the mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 2 In the embodiment shown, step S103: based on the scene processing result, inferring the current two-dimensional pose of the mobile robot, and constructing a working scene map of the mobile robot based on the two-dimensional pose includes:

[0246] Step S1031: Construct a low-resolution map using the scene processing results, and calculate the optimal solution for the low-resolution map.

[0247] In this embodiment, the combined field of view of the data collected by the two TOF sensors is approximately 200 degrees, which is smaller than the 360-degree field of view of the LiDAR. This results in insufficient feature points when the robot enters narrow areas, making matching prone to failure. To solve this problem, the specific solution is as follows:

[0248] First, relevant data with a high degree of matching with the real scene are selected from the scene processing results, and a low-resolution map is constructed based on these data. Then, the low-resolution map is continuously matched with 2D point cloud data to select the optimal solution of the low-resolution map. The optimal solution of the low-resolution map is the one that makes the high-resolution map most consistent with the pose of the real scene.

[0249] Step S1032: Construct a high-resolution map using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and calculate the optimal solution of the high-resolution map.

[0250] As one implementation method, in this embodiment, firstly, a high-resolution map is constructed using the optimal solution of the low-resolution map and 2D point cloud data; then, the high-resolution map is continuously matched with the 2D point cloud data to select the optimal solution of the high-resolution map, wherein the optimal solution of the high-resolution map is the one that makes the generated working scene map most consistent with the pose of the real scene.

[0251] Step S1033: Optimize the optimal solution of the high-resolution map using a preset optimization algorithm to obtain the two-dimensional pose.

[0252] Step S1034: Update the two-dimensional point cloud data to the working scene map of the mobile robot according to the two-dimensional pose.

[0253] As one implementation method, in this embodiment, firstly, the optimal solution of the high-resolution map is optimized using a preset optimization algorithm to obtain a two-dimensional pose. This preset optimization algorithm includes algorithms from the Ceres library, among others. Then, the optimization result of the preset optimization algorithm, i.e., the two-dimensional pose, is used as the final positioning coordinate result of the SLAM mapping module. The two-dimensional pose represents the planar coordinates of the mobile robot on a two-dimensional plane. Finally, based on the final positioning coordinate result, the 2D point cloud data is updated to the mobile robot's work scene map. For example, if the final positioning coordinate result of the mobile robot is (3,5), then the image acquired by the mobile robot at this time is updated to the position (3,5) on the work scene map.

[0254] Therefore, even if the total field of view of the TOF sensor is obstructed, it can still map and locate with high accuracy.

[0255] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problems of existing navigation systems being unable to accurately locate the object to be identified, and the problem of matching failures due to insufficient feature points when the mobile robot enters a narrow area. Even if the total field of view of the TOF sensor is obstructed, high-accuracy mapping and positioning are still possible.

[0256] Reference Figure 9 , Figure 9 This is a schematic diagram illustrating a second detailed process for constructing a work scene map for a mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 8 In the embodiment shown, the mobile robot further includes an encoder. Step S1031: Constructing a low-resolution map using the scene processing results and calculating the optimal solution for the low-resolution map includes:

[0257] Step S10311: The encoder processes the data of the inertial unit to obtain the pose estimation value.

[0258] In this embodiment, the mobile robot also includes an encoder, which is a device that encodes signals (such as bit streams) or data and converts them into a signal form that can be used for communication, transmission and storage.

[0259] In one implementation method, in this embodiment, the inertial unit data is processed by an encoder to obtain a pose estimate, wherein the inertial unit data includes the pose.

[0260] Step S10312: Select the pose estimation value to obtain low-resolution map candidate solutions and construct a queue of low-resolution map candidate solutions.

[0261] Because the combined field of view of the two TOF data points is approximately 200 degrees, which is smaller than the 360-degree field of view of the LiDAR, the robot is prone to matching failures due to insufficient feature points when entering narrow areas. Therefore, this embodiment adds 9 historical candidate solutions, bringing the total number of candidate solutions to 10, including those from the position estimator. These 10 candidate solutions are matched against the sampled map to select the preliminary optimal candidate solution. The specific scheme is as follows:

[0262] In this embodiment, candidate solutions for low-resolution maps are selected and a queue of candidate solutions for low resolution is constructed.

[0263] The pose estimate is added to the candidate solution queue of the low-resolution map when one of the following conditions is met:

[0264] a) If the candidate solution queue has less than 10 options, add a new pose estimate.

[0265] b) If the candidate solution queue has reached 10, remove the oldest head candidate solution and add the new pose estimate.

[0266] c) If the difference between the best matching degree of the last two candidates in the candidate solution queue and the best matching degree of the low-resolution map is less than the preset threshold for the difference of the best matching degree, then the last candidate solution in the candidate solution queue is removed and a new pose estimate is added. The preset threshold for the difference of the best matching degree is set according to the actual situation.

[0267] Therefore, by using the above method, we can obtain 10 candidate solutions for low-resolution maps and a queue of candidate solutions for low-resolution maps.

[0268] Step S10313: Construct the low-resolution map using the candidate solutions of the low-resolution map and the two-dimensional point cloud data;

[0269] Step S10314: Using the two-dimensional point cloud data, the candidate solutions of the low-resolution map are matched with the low-resolution map within a preset time period to obtain the highest matching degree of multiple candidate solutions in the candidate solution queue of the low-resolution map.

[0270] As one implementation method, in this embodiment, firstly, a low-resolution map is constructed using candidate solutions of the low-resolution map and 2D point cloud data, and the matching degree between the candidate solutions and the low-resolution map is calculated; then, the 10 candidate solutions in the candidate solution queue of the low-resolution map are matched with the map in the low-resolution map in turn using 2D scan data, and the highest matching degree of each candidate solution in the low-resolution map is obtained. The low-resolution map is constantly updated and changed, and the preset time is set according to the actual situation.

[0271] Step S10315: Sort the highest matching degree of multiple candidate solutions in the low-resolution map candidate solution queue to obtain the first matching degree;

[0272] Step S10316: Select the pose estimation value of the candidate solution queue of the low-resolution map according to the first matching degree, and take the selection result as the optimal solution of the low-resolution map.

[0273] As one implementation method, in this embodiment, the highest matching degree of the candidate solutions in the candidate solution queue of the low-resolution map is sorted, and the candidate solution with the highest matching degree, i.e., the first matching degree, is taken as the optimal solution of the low-resolution map.

[0274] Therefore, by using the above method, the pose estimation value that best matches the current scenario is selected, thereby constructing a more accurate map to improve obstacle avoidance accuracy.

[0275] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified by selecting the pose estimation value that best matches the current scene and constructing a more accurate map to improve obstacle avoidance accuracy.

[0276] Reference Figure 10 , Figure 10 This is a schematic diagram illustrating the third refinement process for constructing a work scene map of the mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 8 In the embodiment shown, step S1032: constructing a high-resolution map using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and calculating the optimal solution of the high-resolution map includes:

[0277] Step S10321: Select the optimal solution of the low-resolution map to obtain the candidate solution of the high-resolution map, and construct a queue of candidate solutions for the high-resolution map.

[0278] In this embodiment, candidate solutions for high-resolution maps are selected and a queue of high-resolution candidate solutions is constructed.

[0279] The optimal solution of the low-resolution map is added to the candidate solution queue of the high-resolution map when one of the following conditions is met:

[0280] a) If the candidate solution queue is less than 10, then add the optimal solution from the new low-resolution map.

[0281] b) If the candidate solution queue has reached 10, remove the oldest head candidate solution and add the optimal solution of the new low-resolution map.

[0282] c) If the difference between the best matching degree of the last two candidates in the candidate solution queue and the best matching degree of the high-resolution map is less than the preset threshold for the difference of the best matching degree, then the last candidate solution in the candidate solution queue is removed and a new optimal solution of the low-resolution map is added. The preset threshold for the difference of the best matching degree is set according to the actual situation.

[0283] Therefore, by using the above method, 10 candidate solutions for high-resolution maps and a queue of candidate solutions for high-resolution maps can be obtained.

[0284] Step S10322: Construct the high-resolution map using the candidate solutions of the high-resolution map and the two-dimensional point cloud data;

[0285] Step S10323: Using the two-dimensional point cloud data, the high-resolution map candidate solution is matched with the high-resolution map within a preset time period to obtain the highest matching degree of multiple candidate solutions in the high-resolution map candidate solution queue.

[0286] As one implementation method, in this embodiment, firstly, a high-resolution map is constructed using candidate solutions of a high-resolution map and 2D point cloud data, and the matching degree between the candidate solutions of the high-resolution map and the high-resolution map is calculated; then, the candidate solutions in the candidate solution queue of the high-resolution map are matched with the map in the high-resolution map in turn using 2D scan data, and the highest matching degree of each candidate solution in the high-resolution map is obtained. The high-resolution map is constantly updated and changed, and the preset time is set according to the actual situation.

[0287] Step S10324: Sort the highest matching degree of multiple candidate solutions in the high-resolution map candidate solution queue to obtain the second matching degree;

[0288] Step S10325: Select the pose estimate of the candidate solution queue of the high-resolution map according to the second matching degree, and take the selection result as the optimal solution of the high-resolution map.

[0289] As one implementation method, in this embodiment, the highest matching degree of the candidate solutions in the candidate solution queue of the high-resolution map is sorted, and the candidate solution with the highest matching degree, i.e. the second matching degree, is taken as the optimal solution of the low-resolution map.

[0290] Therefore, by using the above method, the pose estimation value that best matches the current scenario is selected, thereby constructing a more accurate map to improve obstacle avoidance accuracy.

[0291] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified. By selecting the pose estimation value that best matches the current scene, a more accurate map is constructed to improve obstacle avoidance accuracy.

[0292] Reference Figure 11 , Figure 11 This is a schematic diagram illustrating the fourth refinement process for constructing a work scene map of the mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 9In the embodiment shown, step S10316, which involves selecting the pose estimation value of the candidate solution queue of the low-resolution map based on the first matching degree and using the selection result as the optimal solution of the low-resolution map, includes:

[0293] Step S103161: Sort the highest matching degree of multiple candidate solutions in the candidate solution queue of the low-resolution map from high to low to obtain the sorting result;

[0294] Step S103162: Calculate the difference between the two highest matching degrees ranked last in the sorting result.

[0295] As one implementation method, in this embodiment, firstly, the highest matching degree of multiple candidate solutions in the candidate solution queue of the low-resolution map is sorted from high to low; then, the difference between the best matching degree of the low-resolution map corresponding to the last two digits of the candidate solution queue is calculated.

[0296] As another implementation method, in this embodiment, firstly, the highest matching degree of multiple candidate solutions in the candidate solution queue of the low-resolution map is sorted from high to low; then, the difference between the optimal matching degree of the low-resolution map corresponding to the last three digits of the candidate solution queue is calculated to obtain 3 differences.

[0297] Step S103163: If the difference in the highest matching degree exceeds a preset difference range, then delete the two candidate solutions with the lower highest matching degree among the two ranked lower results in the sorting results.

[0298] As one implementation method, in this embodiment, if the difference in the highest matching degree exceeds a preset difference range, the two candidate solutions with the lower highest matching degree among the two ranked results are deleted.

[0299] As another implementation method, in this embodiment, if the difference in the highest matching degree exceeds a preset difference range, the two candidate solutions with the lower highest matching degree among the two ranked results are deleted in turn.

[0300] Therefore, by using the above methods, the pose error can be reduced, and a more accurate map can be constructed to improve the accuracy of obstacle avoidance.

[0301] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified, reduces pose errors, and constructs a more accurate map to improve obstacle avoidance accuracy.

[0302] Reference Figure 12 , Figure 12 This is a schematic diagram of the fifth detailed process for constructing a work scene map of the mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 9 In the embodiment shown, step S1032, after constructing a high-resolution map using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and calculating the optimal solution of the high-resolution map, includes:

[0303] Step S1035: Replace the pose estimate of the low-resolution map with the optimal solution of the high-resolution map.

[0304] As one implementation method, in this embodiment, the optimal solution of the high-resolution map is used to replace the last candidate solution in the candidate solution queue of the low-resolution map, thereby improving the accuracy of the pose estimation.

[0305] The above method will not affect the accuracy of mapping even when used for robot localization and mapping where the total field of view is obstructed, such as robots with built-in radar and laser and where the local field of view is blocked.

[0306] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate the object to be identified. Even when used for robot positioning and mapping where the total field of view is obstructed, such as robots with built-in radar or laser sensors where the local field of view is blocked, the accuracy of the mapping is not affected.

[0307] Reference Figure 13 , Figure 13 This is a schematic diagram of the sixth detailed process for constructing a work scene map of the mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 8 In the embodiment shown, the mobile robot includes an odometer. Step S1033: After optimizing the optimal solution of the high-resolution map using a preset optimization algorithm to obtain the two-dimensional pose, the following steps are included:

[0308] Step S1036: If the mobile robot is in a preset scene, the travel distance of the odometer is calculated and the calculation result is used as the positioning coordinate result.

[0309] Step S1037: Update the two-dimensional point cloud data to the working scene map of the mobile robot according to the positioning coordinate results;

[0310] If the mobile robot is not in the preset scene, then step S1034 is executed: update the two-dimensional point cloud data to the working scene map of the mobile robot according to the two-dimensional pose.

[0311] In this embodiment, the mobile robot includes distance sensor devices such as an odometer. In actual working environments, external distance sensors can be replaced with internal ones, thereby reducing the thickness of the robot.

[0312] As one implementation method, in this embodiment, if the mobile robot is in a preset scene, the travel distance of the odometer is calculated to obtain the calculation result as the positioning coordinate result; then, based on the positioning coordinate result, the 2D point cloud data is updated to the mobile robot's working scene map.

[0313] The preset scenes include corridors and other similar scenarios.

[0314] Therefore, by adjusting the pose acquisition method according to the working environment of the mobile robot, a more accurate map can be constructed to improve the accuracy of obstacle avoidance.

[0315] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified, adjusts the pose acquisition method according to the working environment of the mobile robot, and constructs a more accurate map to improve obstacle avoidance accuracy.

[0316] Reference Figure 14 , Figure 14 This is a detailed flowchart illustrating the first scenario-specific processing of the mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 2 In the embodiment shown, step S102: Based on the 3D point cloud data and the motion state of the mobile robot, scene processing is performed to obtain the scene processing result, including:

[0317] Step S1021: If it is detected that the mobile robot is in a pitch state and the pitch exceeds a preset range, then the two-dimensional point cloud data outside the preset range is cropped.

[0318] As one implementation method, in this embodiment, if the mobile robot's motion state is in a pitch state and the pitch exceeds a preset range, then the two-dimensional point cloud data outside the preset range is cropped, and the cropped two-dimensional point cloud data is used as the above-mentioned scene processing result.

[0319] The mobile robot may pitch when it climbs over obstacles such as thresholds. When the mobile robot pitches beyond the preset range, the 3D point cloud hitting the ground and / or the 3D point cloud that is too high will be clipped. The preset range is set according to the actual situation.

[0320] Step S1022: If the mobile robot is detected to be in a state of no pitch but slipping, then the change in the number of odometer readings is removed.

[0321] As one implementation method, in this embodiment, if the mobile robot is detected to be in a state of slippage without pitch, the change in the odometer reading is removed, the mobile robot is restored to its normal working position, and the normal workflow is performed. The data collected when the mobile robot is performing the normal workflow is used as the above scenario processing result.

[0322] Step S1023: If the mobile robot is detected to be in a state of pitch and slippage, the data change of the odometer is removed, and the heading angle information of the inertial unit is retained.

[0323] As one implementation method, in this embodiment, if the mobile robot is detected to be in a state of pitch and slippage, the data change of the odometer is removed, the heading angle information of the inertial unit is retained, and the two-dimensional point cloud data obtained based on the heading angle information is used as the above-mentioned scene processing result.

[0324] By removing changes in the odometer data, the mobile robot is restored to its normal working position and can perform its normal workflow. The heading angle information of the inertial unit is retained so that only the horizontal plane information is preserved, and the error recorded due to pitch slip is not retained, thereby building a more accurate map to improve the accuracy of obstacle avoidance.

[0325] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified. Through special scene processing, the collected information is standardized, unnecessary information is removed, and a more accurate map is constructed to improve obstacle avoidance accuracy.

[0326] Reference Figure 15 , Figure 15 This is a detailed flowchart illustrating a second approach to handling special scenarios for the mobile robot in an embodiment of the mobile robot positioning and navigation method of the present invention. Based on the above... Figure 14 In the embodiment shown, step S1022: detecting the motion state of the mobile robot includes:

[0327] Step S10221: Match the two-dimensional point cloud data using a preset data registration algorithm to obtain the matching difference;

[0328] Step S10222: Calculate the difference between the matching difference and the change in the odometer data to obtain the change in the difference;

[0329] Step S10223: If the change in the difference exceeds a preset threshold, it is determined that the mobile robot is slipping.

[0330] As one implementation method, in this embodiment, firstly, 2D point cloud data is matched using a preset data registration algorithm to obtain a matching difference, wherein the preset data matching algorithm includes algorithms such as ICP; then, the difference between the matching difference and the data change amount of the odometer is calculated to obtain the difference change amount; finally, it is determined whether the difference change amount exceeds a preset threshold. If the difference change amount exceeds the preset threshold, it is determined that the mobile robot is slipping, wherein the preset threshold is set according to the actual situation.

[0331] Therefore, by determining whether the mobile robot is slipping, we can better process the scene for the mobile robot and build a more accurate map to improve the accuracy of obstacle avoidance.

[0332] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified. By determining whether the mobile robot is slipping, it better processes the scene for the mobile robot and constructs a more accurate map to improve obstacle avoidance accuracy.

[0333] Reference Figure 16 , Figure 16 This is a flowchart illustrating the fourth embodiment of the positioning and navigation method for a mobile robot according to the present invention.

[0334] As one implementation method, in this embodiment, the positioning and navigation method of the mobile robot is applied to the positioning and navigation system of the mobile robot, which includes: a front TOF sensor, a left TOF sensor, an odometer, an inertial unit, a sensor processing module, a mapping business control module, and a mapping module.

[0335] First, the scene image data of the current mobile robot is stimulated by the front TOF sensor and the left TOF sensor and converted into 3D point cloud data; relevant data of the mobile robot during operation are obtained by the odometry and inertial unit; then the above data is input into the sensor processing module.

[0336] Secondly, the time series of the front TOF sensor, the left TOF sensor, and the main control board are obtained through the sensor processing module; then, the time series of the front TOF sensor and the left TOF sensor are converted to the time coordinate axis of the main control board to obtain the timestamp of each TOF sensor in the motherboard time coordinate system.

[0337] Furthermore, the attitude of the inertial unit is obtained based on the timestamp, and angle compensation is performed between the front TOF sensor and the left TOF sensor.

[0338] Furthermore, the 3D point cloud data collected by the front TOF sensor and the left TOF sensor are combined, and the 3D point cloud data is converted into 2D point cloud data.

[0339] Secondly, the mapping business control module performs special scene processing on the 2D point cloud data.

[0340] Furthermore, when the mobile robot pitches above a preset threshold, it clips the point cloud that hits the ground and the point cloud that is too high.

[0341] When the mobile robot slips horizontally, the change in odometer data is removed.

[0342] When the mobile robot pitches and slips, the changes in odometry data are removed, and only the heading angle information of the inertial unit is retained.

[0343] The pose estimation value is output from the mapping business control module to the mapping module.

[0344] Finally, the mapping module filters the pose estimates and adds them to the candidate solution queue. It then matches them with the low-resolution solution to obtain the optimal low-resolution solution. Next, it matches the optimal low-resolution solution with the high-resolution map to obtain the optimal high-resolution map solution, and replaces the last candidate solution in the low-resolution candidate solution queue.

[0345] Furthermore, the optimal solution of the high-resolution map is optimized using a preset algorithm to obtain the two-dimensional pose.

[0346] Furthermore, it is determined whether the mobile robot is currently in a preset scenario such as a corridor. If the mobile robot is in a preset scenario such as a corridor, the driving data of the odometer is calculated and the calculation result is used to replace the two-dimensional pose.

[0347] Furthermore, output the two-dimensional pose.

[0348] This invention discloses a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. The method includes: detecting the motion state of the mobile robot and acquiring 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different positions on the mobile robot; performing scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain a scene processing result; inferring the current 2D pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified, and constructs a more accurate map to improve obstacle avoidance accuracy.

[0349] Furthermore, this invention also proposes a positioning and navigation system for a mobile robot, the positioning and navigation system for the mobile robot comprising:

[0350] The sensor module is used to detect the motion state of the mobile robot and acquire three-dimensional point cloud data of the current scene through multiple TOF sensors, wherein the multiple TOF sensors are installed at different positions of the mobile robot.

[0351] The mapping business control module is used to perform scene processing based on the 3D point cloud data and the motion state of the mobile robot to obtain the scene processing result.

[0352] The mapping module is used to infer the current two-dimensional pose of the mobile robot based on the scene processing results, and to construct a working scene map of the mobile robot based on the two-dimensional pose.

[0353] The principle and implementation process of data processing for industrial instruments in this embodiment are described in the above embodiments and will not be repeated here.

[0354] Furthermore, this embodiment of the invention also proposes a terminal device, which includes a memory, a processor, and a positioning and navigation program for a mobile robot stored in the memory and executable on the processor. When the positioning and navigation program for the mobile robot is executed by the processor, it implements the steps of the positioning and navigation method for the mobile robot as described above.

[0355] Since the positioning and navigation program of this mobile robot adopts all the technical solutions of all the aforementioned embodiments when it is executed by the processor, it has at least all the beneficial effects brought about by all the technical solutions of all the aforementioned embodiments, which will not be repeated here.

[0356] Furthermore, this embodiment of the invention also proposes a computer-readable storage medium storing a positioning and navigation program for a mobile robot. When the positioning and navigation program for the mobile robot is executed by a processor, it implements the steps of the positioning and navigation method for the mobile robot as described above.

[0357] Since the positioning and navigation program of this mobile robot adopts all the technical solutions of all the aforementioned embodiments when it is executed by the processor, it has at least all the beneficial effects brought about by all the technical solutions of all the aforementioned embodiments, which will not be repeated here.

[0358] Compared to existing technologies, this invention provides a positioning and navigation method, system, terminal device, and storage medium for a mobile robot. It detects the motion state of the mobile robot and acquires 3D point cloud data of the current scene using multiple Time-of-Flight (TOF) sensors, wherein the multiple TOF sensors are installed at different locations on the mobile robot. Based on the 3D point cloud data and the motion state of the mobile robot, scene processing is performed to obtain a scene processing result. Based on the scene processing result, the current 2D pose of the mobile robot is inferred, and a working scene map of the mobile robot is constructed based on the 2D pose. This invention solves the problem that existing navigation systems cannot accurately locate objects to be identified, and constructs a more accurate map to improve obstacle avoidance accuracy.

[0359] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or approach that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or approach. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or approach that includes that element.

[0360] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0361] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes several instructions to cause a terminal device (which may be a mobile phone, computer, server, controlled terminal, or network device, etc.) to execute the methods of each embodiment of the present invention.

[0362] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A positioning navigation method of a mobile robot, characterized by, The mobile robot includes an odometer, a front-mounted TOF sensor, a left-mounted TOF sensor, or a right-mounted TOF sensor. The method includes the following steps: The motion state of the mobile robot is detected, and point cloud data of the current scene is obtained through the front TOF sensor and the left TOF sensor, or point cloud data of the current scene is obtained through the front TOF sensor and the right TOF sensor; the motion state includes pitch state, no pitch but slipping state, and pitch and slipping state. Scene processing is performed based on the point cloud data and the motion state of the mobile robot to obtain scene processing results; If the mobile robot is in a preset scenario, the travel distance measured by the odometer is calculated, and the calculation result is used as the positioning coordinate result. Based on the positioning coordinates, the point cloud data is updated to the working scene map of the mobile robot; If the mobile robot is not in the preset scene, the current two-dimensional pose of the mobile robot is inferred based on the scene processing result, and a working scene map of the mobile robot is constructed based on the two-dimensional pose, wherein the preset scene includes a corridor; The step of detecting the motion state of the mobile robot includes: Calculate the difference between the matching difference obtained from the point cloud data matching and the data change of the odometer to obtain the difference change; If the change in the difference exceeds a preset threshold, it is determined that the mobile robot is slipping.

2. The positioning and navigation method for a mobile robot according to claim 1, characterized in that, The mobile robot is also equipped with a main control board. Before the step of performing scene processing based on the point cloud data and the motion state of the mobile robot to obtain the scene processing result, the following steps are included: The sequence times of the main control board and the multiple TOF sensors are obtained and synchronized to obtain the timestamps of the multiple TOF sensors in the time coordinate system of the main control board; Angle compensation is performed on multiple TOF sensors based on the timestamp.

3. The positioning and navigation method for a mobile robot according to claim 2, characterized in that, The step of acquiring and synchronizing the sequence times of the main control board and the multiple TOF sensors to obtain the timestamps of the multiple TOF sensors in the time coordinate system of the main control board includes: Obtain the sequence times of the main control board and the multiple TOF sensors, and calculate the sequence time difference between the main control board and the TOF sensors; The time sequences of the multiple TOF sensors are converted to the time coordinate axis of the main control board according to the time difference, so as to obtain the timestamp of the TOF sensor in the time coordinate system of the main control board.

4. The positioning and navigation method for a mobile robot according to claim 2, characterized in that, The point cloud data includes 3D point cloud data and 2D point cloud data. The mobile robot also includes an inertial unit. The mobile robot includes a first TOF sensor and a second TOF sensor. The step of performing angle compensation on the TOF sensor based on the timestamp includes: The attitude of the inertial unit that is closest to the timestamps of the second TOF sensor and the first TOF sensor is obtained, and the compensated attitude of the inertial unit is calculated. The angle of the three-dimensional point cloud data is compensated by the compensation posture.

5. The positioning and navigation method for a mobile robot according to claim 4, characterized in that, The step of obtaining the attitude of the inertial unit that is closest to the timestamps of the second TOF sensor and the first TOF sensor, and calculating the compensated attitude of the inertial unit, includes: Obtain the launch attitude of the inertial unit whose launch time is closest to the timestamp of the first TOF sensor; Obtain the return attitude of the inertial unit whose return time is closest to that of the second TOF sensor timestamp; The compensated attitude of the inertial unit is calculated based on the launch attitude and the return attitude of the inertial unit.

6. The positioning and navigation method for a mobile robot according to claim 5, characterized in that, The step of inferring the current two-dimensional pose of the mobile robot based on the scene processing result, and constructing a working scene map of the mobile robot based on the two-dimensional pose, includes: A low-resolution map is constructed using the scene processing results, and the optimal solution for the low-resolution map is calculated. A high-resolution map is constructed using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and the optimal solution of the high-resolution map is calculated. The optimal solution of the high-resolution map is optimized using a preset optimization algorithm to obtain the two-dimensional pose; Based on the two-dimensional pose, the two-dimensional point cloud data is updated to the working scene map of the mobile robot.

7. The positioning and navigation method for a mobile robot according to claim 6, characterized in that, The mobile robot also includes an encoder, and the step of constructing a low-resolution map based on the scene processing results and calculating the optimal solution for the low-resolution map includes: The encoder processes the data from the inertial unit to obtain a pose estimate. The pose estimates are selected to obtain candidate solutions for low-resolution maps, and a queue of candidate solutions for low-resolution maps is constructed. The low-resolution map is constructed using the candidate solutions of the low-resolution map and the two-dimensional point cloud data. Using the two-dimensional point cloud data, the candidate solutions of the low-resolution map are matched with the low-resolution map within a preset time to obtain the highest matching degree of multiple candidate solutions in the candidate solution queue of the low-resolution map. The highest matching degree of multiple candidate solutions in the low-resolution map candidate solution queue is sorted to obtain the first matching degree; Based on the first matching degree, the pose estimation value of the candidate solution queue of the low-resolution map is selected, and the selection result is taken as the optimal solution of the low-resolution map.

8. The positioning and navigation method for a mobile robot according to claim 6, characterized in that, The step of constructing a high-resolution map using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and calculating the optimal solution of the high-resolution map, includes: The optimal solution of the low-resolution map is selected to obtain the candidate solution of the high-resolution map, and a queue of candidate solutions of the high-resolution map is constructed. The high-resolution map is constructed using the candidate solutions for the high-resolution map and the two-dimensional point cloud data. Using the two-dimensional point cloud data, the high-resolution map candidate solution is matched with the high-resolution map within a preset time period to obtain the highest matching degree of multiple candidate solutions in the high-resolution map candidate solution queue. The highest matching degree of multiple candidate solutions in the high-resolution map candidate solution queue is sorted to obtain the second matching degree; The pose estimate of the candidate solution queue of the high-resolution map is selected based on the second matching degree, and the selection result is taken as the optimal solution of the high-resolution map.

9. The positioning and navigation method for a mobile robot according to claim 7, characterized in that, The step of selecting the pose estimation value of the candidate solution queue of the low-resolution map based on the first matching degree and taking the selection result as the optimal solution of the low-resolution map includes: The candidate solutions in the low-resolution map candidate solution queue are sorted from highest to lowest according to their highest matching degree to obtain the sorting result; Calculate the difference between the two highest matching degrees that rank last in the sorting results; If the difference in the highest matching degree exceeds a preset difference range, then delete the two candidate solutions with the lower highest matching degree that are ranked lower in the sorting results.

10. The positioning and navigation method for a mobile robot according to claim 6, characterized in that, The step of constructing a high-resolution map using the optimal solution of the low-resolution map and the two-dimensional point cloud data, and calculating the optimal solution of the high-resolution map, includes the following: The pose estimate of the low-resolution map is replaced with the optimal solution of the high-resolution map.

11. The positioning and navigation method for a mobile robot according to claim 1, characterized in that, The steps for performing scene processing based on the point cloud data and the motion state of the mobile robot to obtain the scene processing result include: If the mobile robot is detected to be in a pitch state and the pitch exceeds a preset range, then the two-dimensional point cloud data outside the preset range is cropped. If the mobile robot is detected to be in a state of no pitch but slipping, then the change in the odometer reading is removed; If the mobile robot is detected to be in a state of pitch and slippage, the data change of the odometer is removed, and the heading angle information of the inertial unit is retained.

12. The positioning and navigation method for a mobile robot according to claim 11, characterized in that, The step of detecting the motion state of the mobile robot includes: The two-dimensional point cloud data is matched using a preset data registration algorithm to obtain the matching difference; Calculate the difference between the matching difference and the change in the odometer data to obtain the change in the difference; If the change in the difference exceeds a preset threshold, it is determined that the mobile robot is slipping.

13. A positioning and navigation system for a mobile robot, characterized in that, include: The sensor module is used to detect the motion state of the mobile robot and acquire point cloud data of the current scene through the front TOF sensor and the left TOF sensor, or acquire point cloud data of the current scene through the front TOF sensor and the right TOF sensor; the motion state includes pitch state, no pitch but slipping state, and pitch and slipping state. The mapping business control module is used to perform scene processing based on the point cloud data and the motion state of the mobile robot to obtain the scene processing result. If the mobile robot is in a preset scenario, the travel distance measured by the odometer is calculated, and the calculation result is used as the positioning coordinate result. Based on the positioning coordinates, the point cloud data is updated to the working scene map of the mobile robot, wherein the preset scene includes a corridor; The mapping module is used to infer the current two-dimensional pose of the mobile robot based on the scene processing results, and to construct a working scene map of the mobile robot based on the two-dimensional pose.

14. A terminal device, characterized in that, The terminal device includes a memory, a processor, and a mobile robot positioning and navigation method stored in the memory and executable on the processor. When the mobile robot positioning and navigation program is executed by the processor, it implements the steps of the mobile robot positioning and navigation method as described in any one of claims 1-12.

15. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a program for the positioning and navigation of a mobile robot, which, when executed by a processor, implements the steps of the positioning and navigation method for a mobile robot as described in any one of claims 1-12.

16. A positioning and navigation method for a mobile robot, characterized in that, The mobile robot includes a front-mounted TOF sensor, a left-mounted TOF sensor or a right-mounted TOF sensor, an odometer, and an inertial unit. The method includes the following steps: Merge the point cloud data collected by the front TOF sensor and the left TOF sensor, or merge the point cloud data collected by the front TOF sensor and the right TOF sensor. If the mobile robot is detected to be in a pitch state and the pitch exceeds a preset range, then the point cloud data outside the preset range is cropped. If the mobile robot is detected to be in a state of no pitch but slipping, then the change in the odometer reading is removed; If the mobile robot is detected to be in a state of pitch and slippage, the data change of the odometer is removed, and the heading angle information of the inertial unit is retained.

17. A positioning and navigation method for a mobile robot, characterized in that, The mobile robot includes a front-mounted TOF sensor, a left-mounted TOF sensor, or a right-mounted TOF sensor, and the method includes the following steps: Merge the point cloud data collected by the front TOF sensor and the left TOF sensor, or merge the point cloud data collected by the front TOF sensor and the right TOF sensor. The scene is processed by combining point cloud data and the motion state of the mobile robot to obtain the scene processing result; A low-resolution map is constructed using the scene processing results, and the optimal solution for the low-resolution map is calculated. The optimal solution for the low-resolution map is the pose that makes the high-resolution map most consistent with the real scene. The high-resolution map is constructed using the optimal solution of the low-resolution map and the point cloud data, and the optimal solution of the high-resolution map is calculated. The optimal solution of the high-resolution map is the pose that makes the generated work scene map most consistent with the real scene. The optimal solution of the high-resolution map is optimized using a preset optimization algorithm to obtain the two-dimensional pose; Based on the two-dimensional pose, the point cloud data is updated to the working scene map of the mobile robot.

Images