Robot positioning method and device, mobile robot and readable storage medium

Abstract

The application provides a robot positioning method and device, a mobile robot and a readable storage medium, and relates to the technical field of robots. The application determines whether the data of a vision sensor, a laser radar and a GNSS system are valid in the current running environment of the mobile robot, then fuses the actual sensing data of the first target sensing device determined from the vision sensor and the laser radar, and the actual sensing data of the second target sensing device determined from the GNSS system and the wheel odometer, to perform pose estimation, so that the adaptive degrees of various sensors to different working condition environments are determined, the adaptive and effective sensor data are fused according to the changes of the robot running environment, the advantages of various sensors are complemented in the robot pose positioning process, and the robot pose positioning accuracy in a complex outdoor environment is improved.

Description

Technical Field

[0001] This application relates to the field of robotics, and more specifically, to a robot positioning method and apparatus, a mobile robot, and a readable storage medium. Background Technology

[0002] With the continuous development of science and technology, robotics is being applied more and more widely across various industries. Mobile robots inevitably need to perform desired tasks in complex outdoor environments. For mobile robots, the accuracy of pose localization in complex outdoor environments is a crucial factor affecting the accuracy of their motion control. Therefore, how to achieve accurate pose localization of mobile robots in complex outdoor environments with varying working conditions has become an important research direction in robotics technology today. Summary of the Invention

[0003] In view of this, the purpose of this application is to provide a robot localization method and apparatus, a mobile robot and a readable storage medium, which can adaptively select suitable and effective multi-sensor data for fusion pose localization according to the adaptability of multiple sensors to different working environments and as the robot's operating environment changes, so as to realize the complementary advantages of multiple sensors in the robot pose localization process and improve the robot pose localization accuracy in complex outdoor environments.

[0004] To achieve the above objectives, the technical solutions adopted in the embodiments of this application are as follows:

[0005] In a first aspect, this application provides a robot localization method applied to a mobile robot, wherein the mobile robot includes a vision sensor, a lidar, an IMU, a GNSS system, and a wheeled odometer, and the method includes:

[0006] Acquire the actual sensing data collected by the visual sensor, the lidar, the IMU, the GNSS system, and the wheeled odometer at the current moment;

[0007] Based on the actual sensing data of the visual sensor, the lidar, and the GNSS system, it is determined whether the data of the visual sensor, the lidar, and the GNSS system are valid in the current operating environment of the mobile robot.

[0008] A first target sensing device with valid data is identified in the visual sensor and the lidar, and a second target sensing device with valid data is identified in the GNSS system and the wheel odometer;

[0009] Factor graph optimization pose estimation is performed on the actual sensing data of the first target sensing device and the IMU respectively to obtain the corresponding target tightly coupled pose information.

[0010] A preset filter is invoked to perform fusion pose estimation on the actual sensing data of the second target sensing device and the IMU respectively, so as to obtain the corresponding target loosely coupled pose information.

[0011] Factor graph optimization and correction are performed on the tightly coupled pose information and the loosely coupled pose information of the target to obtain the actual predicted pose information of the mobile robot at the current moment.

[0012] In an optional implementation, the step of detecting whether the visual sensor's data is valid in the current operating environment of the mobile robot based on the actual sensing data of the visual sensor includes:

[0013] The actual sensing data of the vision sensor is subjected to grayscale image conversion processing to obtain the corresponding grayscale image to be detected.

[0014] The grayscale image to be detected is subjected to Shi-Tomasi corner detection processing to obtain the number of Shi-Tomasi corners in the grayscale image to be detected;

[0015] The number of Shi-Tomasi corners in the grayscale image to be detected is compared with a preset corner number threshold.

[0016] If the number of Shi-Tomasi corners in the grayscale image to be detected is greater than or equal to the preset corner number threshold, the data from the vision sensor in the current operating environment of the mobile robot is determined to be valid.

[0017] If the number of Shi-Tomasi corners in the grayscale image to be detected is less than the preset corner number threshold, the visual sensor is determined to be faulty in the current operating environment of the mobile robot.

[0018] In an optional implementation, the step of detecting whether the lidar data is valid in the current operating environment of the mobile robot based on the actual sensing data of the lidar includes:

[0019] Based on the actual estimated pose information of the mobile robot at the current moment and the actual sensing data of the lidar, a map estimation is performed to obtain the corresponding local lidar map.

[0020] Extract the local movement map corresponding to the local laser map from the pre-stored robot movement map;

[0021] Calculate the map feature difference value between the local laser map and the local moving map;

[0022] The map feature difference values ​​are compared with a preset feature difference threshold.

[0023] If the map feature difference value is greater than or equal to the preset feature difference threshold, it is determined that the LiDAR data is invalid in the current operating environment of the mobile robot.

[0024] If the map feature difference value is less than the preset feature difference threshold, the data from the lidar is determined to be valid in the current operating environment of the mobile robot.

[0025] In an optional implementation, the step of calculating the map feature difference value between the local laser map and the local moving map includes:

[0026] The local laser map and the local moving map are rasterized according to a preset number of grids;

[0027] For each map grid in the local laser map, calculate the elevation difference between the maximum map elevation value of the map grid and the maximum map elevation value of the target grid corresponding to the location of the map grid in the local moving map;

[0028] The average elevation difference of each of the map grids in the local laser map is calculated to obtain the corresponding average elevation difference.

[0029] The calculated average elevation difference is used as the map feature difference value.

[0030] In an optional implementation, the step of detecting whether the GNSS system is valid in the current operating environment of the mobile robot based on the actual sensing data of the GNSS system includes:

[0031] Based on the historical sensing data of the GNSS system before the current moment and the actual sensing data of the GNSS system, the data continuity of the GNSS system is detected.

[0032] If the detected data continuity is in a continuous state, the GNSS system is deemed to have valid data in the current operating environment of the mobile robot.

[0033] If the detected data continuity is in a state of discontinuity, the GNSS system is deemed to have data failure in the current operating environment of the mobile robot.

[0034] In an optional implementation, the step of performing factor graph-optimized pose estimation on the actual sensing data of the first target sensing device and the IMU respectively to obtain the corresponding target tightly coupled pose information includes:

[0035] The actual sensing data of the IMU is pre-integrated to obtain the corresponding inertial pre-integrated data;

[0036] For each first target sensing device, odometer information prediction processing is performed based on the actual sensing data of the first target sensing device to obtain device odometer information that matches the first target sensing device.

[0037] The inertial pre-integration data and the device odometer information of each of the first target sensing devices are subjected to tight-coupled pose estimation processing using the factor graph optimization algorithm to obtain the target tight-coupled pose information.

[0038] In an optional implementation, the step of fusing pose estimation of the actual sensing data of the second target sensing device and the IMU by calling a preset filter to obtain the corresponding loosely coupled target pose information includes:

[0039] The actual sensing data of the IMU is pre-integrated to obtain the corresponding inertial pre-integrated data;

[0040] For each second target sensing device, odometer information prediction processing is performed based on the actual sensing data of the second target sensing device to obtain device odometer information that matches the second target sensing device.

[0041] The inertial pre-integration data is used as the pose prediction value of the preset ESKF filter, and the device odometer information of all second target sensing devices is used as the pose observation value of the ESKF filter. The ESKF filter is called to perform loosely coupled pose estimation processing to obtain the target loosely coupled pose information.

[0042] Secondly, this application provides a robot positioning device for use in a mobile robot, wherein the mobile robot includes a vision sensor, a lidar, an IMU, a GNSS system, and a wheeled odometer, and the device includes:

[0043] The sensor data acquisition module is used to acquire the actual sensor data collected by the visual sensor, the lidar, the IMU, the GNSS system and the wheeled odometer at the current moment.

[0044] The equipment failure detection module is used to detect whether the data from the visual sensor, the lidar, and the GNSS system are valid in the current operating environment of the mobile robot, based on the actual sensing data of each of the visual sensor, the lidar, and the GNSS system.

[0045] An effective device screening module is used to identify a first target sensing device with valid data among the visual sensor and the lidar, and to identify a second target sensing device with valid data among the GNSS system and the wheel odometer;

[0046] The first pose estimation module is used to perform factor graph optimized pose estimation on the actual sensing data of the first target sensing device and the IMU respectively to obtain the corresponding target tightly coupled pose information.

[0047] The second pose estimation module is used to call a preset filter to perform fusion pose estimation on the actual sensing data of the second target sensing device and the IMU respectively, so as to obtain the corresponding target loosely coupled pose information.

[0048] The pose estimation and correction module is used to perform factor graph optimization and correction on the target tightly coupled pose information and the target loosely coupled pose information to obtain the actual estimated pose information of the mobile robot at the current moment.

[0049] Thirdly, this application provides a mobile robot, including a processor and a memory, wherein the memory stores a computer program that can be executed by the processor, and the processor can execute the computer program to implement the robot positioning method described in any of the foregoing embodiments.

[0050] Fourthly, this application provides a readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the robot localization method described in any of the foregoing embodiments.

[0051] In this case, the beneficial effects of the embodiments of this application may include the following:

[0052] This application detects the validity of data from visual sensors, LiDAR, and GNSS systems in the current operating environment of a mobile robot. Based on the detection results, it identifies a first target sensing device with valid data from the visual sensor and LiDAR, and a second target sensing device with valid data from the GNSS system and wheeled odometer. Then, it performs factor graph optimization pose estimation on the actual sensing data of the first target sensing device and the IMU to obtain the corresponding tightly coupled target pose information. A preset filter is then used to perform fusion pose estimation on the actual sensing data of the second target sensing device and the IMU to obtain the corresponding loosely coupled target pose information. Finally, factor graph optimization correction is performed on both the tightly coupled and loosely coupled target pose information to obtain the actual predicted pose information of the mobile robot at the current moment. This allows for adaptive selection of suitable and valid sensor data for fusion pose localization based on the adaptability of various sensors to different working environments, enabling complementary advantages of multiple sensors during robot pose localization and improving the accuracy of robot pose localization in complex outdoor environments.

[0053] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0054] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0055] Figure 1 A schematic diagram illustrating the composition of the mobile robot provided in an embodiment of this application;

[0056] Figure 2 A flowchart illustrating the robot localization method provided in an embodiment of this application;

[0057] Figure 3 for Figure 2 One of the flowcharts for the sub-steps included in step S220;

[0058] Figure 4 for Figure 2 The second flowchart illustrates the sub-steps included in step S220.

[0059] Figure 5 for Figure 2 The third flowchart illustrates the sub-steps included in step S220.

[0060] Figure 6 for Figure 2 A flowchart illustrating the sub-steps included in step S240;

[0061] Figure 7 for Figure 2 A flowchart illustrating the sub-steps included in step S250;

[0062] Figure 8 This is a schematic diagram illustrating the composition of the robot positioning device provided in the embodiments of this application.

[0063] Icons: 10-Mobile robot; 11-Memory; 12-Processor; 13-Communication unit; 14-Mobile component; 15-Vision sensor; 16-LiDAR; 17-IMU; 18-GNSS system; 19-Wheel odometer; 100-Robot positioning device; 110-Sensor data acquisition module; 120-Equipment failure detection module; 130-Valid equipment screening module; 140-First pose estimation module; 150-Second pose estimation module; 160-Predicted pose correction module. Detailed Implementation

[0064] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0065] Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0066] It should be noted that similar labels and letters in the following figures indicate similar items. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0067] In the description of this application, it should be understood that relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus 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 apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element. Those skilled in the art will understand the specific meaning of the above terms in this application based on the specific circumstances.

[0068] In the description of this application, it should also be noted that, unless otherwise expressly specified and limited, the terms "set up," "install," "connect," and "link" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this application based on the specific circumstances.

[0069] Through diligent research, the applicant discovered that existing solutions for robot pose localization using multi-sensor fusion fall into two categories. One is based on factor graph optimization algorithms to fuse multiple sensors, while the other is based on filtering algorithms. However, it's important to note that the former only applies to any two or three combinations of LiDAR, IMU (Inertial Measurement Unit), and vision sensors. The LiDAR and IMU fusion scheme is unsuitable for rainy and / or open-air scenarios, and the vision sensor and IMU fusion scheme is unsuitable for scenarios with changing lighting. The latter scheme, on the other hand, uses IMU and GNSS (Global Navigation Satellite System) systems. The IMU and GNSS fusion scheme is unsuitable for scenarios with dense occlusion. In other words, existing multi-sensor fusion solutions for robot pose localization cannot effectively adapt to the complex and variable outdoor environment, and cannot guarantee the accuracy of robot pose localization in complex outdoor environments.

[0070] In response, this application provides a robot positioning method and apparatus, a mobile robot, and a readable storage medium. Based on the adaptability of various sensors to different working environments, the method adaptively selects suitable and effective data from multiple sensors for fusion-based pose positioning as the robot's operating environment changes. This achieves complementary advantages of multiple sensors during robot pose positioning and improves the accuracy of robot pose positioning in complex outdoor environments.

[0071] The following detailed description of some embodiments of this application is provided in conjunction with the accompanying drawings. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0072] Please refer to Figure 1 , Figure 1 This is a schematic diagram of the composition of the mobile robot 10 provided in this application embodiment. In this application embodiment, the mobile robot 10 can utilize the adaptability of various sensors to different working environments, and adaptively select suitable and effective multi-sensor data for fusion pose localization as the robot moves and the operating environment changes. This achieves complementary advantages of multiple sensors during robot pose localization, improving the accuracy of robot pose localization in complex outdoor environments. The mobile robot 10 can be a wheeled robot or a tracked robot.

[0073] In this embodiment, the mobile robot 10 may include a memory 11, a processor 12, a communication unit 13, a mobility component 14, a vision sensor 15, a lidar 16, an IMU 17, a GNSS system 18, a wheeled odometer 19, and a robot positioning device 100. The memory 11, processor 12, communication unit 13, mobility component 14, vision sensor 15, lidar 16, IMU 17, GNSS system 18, and wheeled odometer 19 are directly or indirectly electrically connected to each other to achieve data transmission or interaction. For example, these components may be electrically connected via one or more communication buses or signal lines.

[0074] In this embodiment, the memory 11 may be, but is not limited to, Random Access Memory (RAM), Read Only Memory (ROM), Programmable Read-Only Memory (PROM), Erasable Programmable Read-Only Memory (EPROM), Electrically Erasable Programmable Read-Only Memory (EEPROM), etc. The memory 11 is used to store computer programs, and the processor 12 can execute the computer programs accordingly after receiving execution instructions.

[0075] In this embodiment, the processor 12 can be an integrated circuit chip with signal processing capabilities. The processor 12 can be a general-purpose processor, including at least one of a Central Processing Unit (CPU), Graphics Processing Unit (GPU), Network Processor (NP), Digital Signal Processor (DSP), Application-Specific Integrated Circuit (ASIC), Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. The general-purpose processor can be a microprocessor or any conventional processor, capable of implementing or executing the methods, steps, and logic block diagrams disclosed in the embodiments of this application.

[0076] In this embodiment, the communication unit 13 is used to establish a communication connection between the mobile robot 10 and other electronic devices through a wireless communication network, and to send and receive data through the wireless communication network.

[0077] In this embodiment, the moving component 14 is used to achieve the positional movement effect of the mobile robot 10. The moving component 14 may include devices such as tracks, transmission devices, drive motors, and wheels to ensure that the mobile robot 10 can achieve positional movement through the moving component 14.

[0078] In this embodiment, the vision sensor 15 may be a binocular camera, used to realize the image acquisition function of the mobile robot 10, so as to acquire images of the current operating environment of the mobile robot 10.

[0079] In this embodiment, the lidar 16 is used to collect laser point cloud data of the current operating environment of the mobile robot 10, so as to obtain the specific distribution of various objects around the mobile robot 10 in the current operating environment of the mobile robot 10.

[0080] In this embodiment, the IMU17 is used to detect information such as the movement acceleration, movement tilt, robot impact, robot vibration, and robot rotation of the mobile robot 10 during the robot's movement process.

[0081] In this embodiment, the GNSS system 18 is used to detect the actual position of the mobile robot 10.

[0082] In this embodiment, the wheeled odometer 19 is used to detect the actual movement of the mobile robot 10.

[0083] In this embodiment, the robot positioning device 100 may include at least one software function module that can be stored in the memory 11 in the form of software or firmware or embedded in the operating system of the mobile robot 10. The processor 12 can be used to execute the executable modules stored in the memory 11, such as the software function modules and computer programs included in the robot positioning device 100. The mobile robot 10 can utilize the robot positioning device 100 to adaptively select suitable and effective multi-sensor data for fusion pose positioning based on the adaptability of various sensors (including the vision sensor 15, the lidar 16, the IMU 17, the GNSS system 18, and the wheeled odometer 19) to different working environments, thereby achieving complementary advantages of multiple sensors during robot pose positioning and improving the accuracy of robot pose positioning in complex outdoor environments.

[0084] Understandable, Figure 1 The block diagram shown is only a schematic diagram of one composition of the mobile robot 10. The mobile robot 10 may also include components such as... Figure 1 The more or fewer components shown, or having the same Figure 1 The different configurations shown. Figure 1 The components shown can be implemented using hardware, software, or a combination thereof.

[0085] In this application, to ensure that the mobile robot 10 can utilize the adaptability of various sensors to different working environments, and adaptively select suitable and effective multi-sensor data for fusion pose localization as the robot's operating environment changes, so as to achieve complementary advantages of multiple sensors in the robot pose localization process and improve the accuracy of robot pose localization in complex outdoor environments, this application provides a robot localization method to achieve the aforementioned objective. The robot localization method provided in this application will be described in detail below.

[0086] Please refer to Figure 2 , Figure 2 This is a flowchart illustrating the robot localization method provided in an embodiment of this application. In this embodiment, the robot localization method may include steps S210 to S260.

[0087] Step S210: Acquire the actual sensing data collected by the visual sensor, lidar, IMU, GNSS system and wheel odometer at the current moment.

[0088] In this embodiment, the actual sensing data corresponding to the visual sensor 15 may include the environmental image of the current operating environment of the mobile robot 10; the actual sensing data corresponding to the lidar 16 may include the laser point cloud data of the current operating environment of the mobile robot 10; the actual sensing data corresponding to the IMU 17 may include information such as the mobile robot 10's current acceleration, tilt, impact, vibration, and rotation status; the actual sensing data corresponding to the GNSS system 18 may include the actual position of the mobile robot 10 at the current moment; and the actual sensing data corresponding to the wheeled odometer 19 may include the actual movement of the mobile robot 10 at the current moment.

[0089] Step S220: Based on the actual sensing data of the visual sensor, lidar and GNSS system, detect whether the data of the visual sensor, lidar and GNSS system are valid in the current operating environment of the mobile robot.

[0090] In this embodiment, after the mobile robot 10 obtains the actual sensing data of the visual sensor 15, the lidar 16, the IMU 17, the GNSS system 18, and the wheeled odometer 19 at the current moment, it can perform wavelet denoising on the actual sensing data of the IMU 17 to obtain preprocessed actual sensing data of the IMU 17; it can also use a voxel filtering algorithm to downsample, remove noise points, and remove outliers from the actual sensing data of the lidar 16, and then use the actual sensing data of the IMU 17 to optimize the actual sensing data of the lidar 16. The actual sensing data is subjected to motion distortion correction processing to obtain the pre-processed actual sensing data of the lidar 16; the actual sensing data of the vision sensor 15 can be distorted by using the camera intrinsic parameter matrix and distortion coefficients pre-calibrated for the vision sensor 15 to obtain the pre-processed actual sensing data of the vision sensor 15; the actual sensing data of the GNSS system 18 and the wheeled odometer 19 can be denoised and filtered to obtain the pre-processed actual sensing data of the GNSS system 18 and the wheeled odometer 19 respectively.

[0091] Then, the mobile robot 10 will perform sensor failure detection for each of the vision sensor 15, the lidar 16 and the GNSS system 18 to determine whether the data of the vision sensor 15, the lidar 16 and the GNSS system 18 are valid in the current operating environment of the mobile robot 10.

[0092] It is worth noting that, because the IMU17 and the wheeled odometer 19 have extremely high device stability and are very unlikely to malfunction, it is assumed that the IMU17 and the wheeled odometer 19 can operate normally in any robot operating environment, and the actual sensing data of the IMU17 and the wheeled odometer 19 can remain valid in any robot operating environment.

[0093] Alternatively, please refer to Figure 3 , Figure 3 yes Figure 2 One of the flowcharts for step S220 is shown below. In this embodiment of the application, for the vision sensor 15, step S220 may include sub-steps S2211 to S2215 to accurately detect the device effectiveness of the vision sensor 15 in the current operating environment.

[0094] Sub-step S2211: Perform grayscale image conversion processing on the actual sensing data of the vision sensor to obtain the corresponding grayscale image to be detected.

[0095] Sub-step S2212: Perform Shi-Tomasi corner detection processing on the grayscale image to be detected to obtain the number of Shi-Tomasi corners in the grayscale image to be detected.

[0096] Sub-step S2213 compares the number of Shi-Tomasi corners of the grayscale image to be detected with a preset corner number threshold.

[0097] Sub-step S2214: If the number of Shi-Tomasi corners in the grayscale image to be detected is greater than or equal to a preset corner number threshold, the data from the vision sensor in the current operating environment of the mobile robot is determined to be valid.

[0098] Sub-step S2215: If the number of Shi-Tomasi corners in the grayscale image to be detected is less than a preset corner number threshold, it is determined that the visual sensor data is invalid in the current operating environment of the mobile robot.

[0099] Specifically, when the number of Shi-Tomasi corners in the grayscale image to be detected corresponding to the vision sensor 15 is less than a preset corner number threshold, it indicates that there may be a lighting change scenario in the current operating environment, and the data of the vision sensor 15 is invalid in the current operating environment; when the number of Shi-Tomasi corners in the grayscale image to be detected corresponding to the vision sensor 15 is greater than or equal to the preset corner number threshold, it indicates that there is no lighting change scenario in the current operating environment, and the data of the vision sensor 15 is valid in the current operating environment.

[0100] Therefore, this application can accurately detect the device effectiveness of the vision sensor 15 in the current operating environment by executing the above sub-steps S2211 to S2215.

[0101] Alternatively, please refer to Figure 4 , Figure 4 yes Figure 2 The flowchart of step S220 is shown in the second example. In this embodiment of the application, for the lidar 16, step S220 may include sub-steps S2221 to S2216 to accurately detect the device effectiveness of the lidar 16 in the current operating environment.

[0102] Sub-step S2221: Based on the actual estimated pose information of the mobile robot at the current moment and the actual sensing data of the lidar, map estimation is performed to obtain the corresponding local lidar map.

[0103] The mobile robot 10 can perform robot pose estimation processing based on the actual estimated pose information of the previous moment and the actual sensing data of the lidar 16 to obtain preliminary estimated pose information corresponding to the lidar 16. Then, it can use the preliminary estimated pose information and the actual sensing data of the lidar 16 to perform map estimation processing to obtain a local laser map of the mobile robot 10 that matches the lidar 16 at the current moment.

[0104] Sub-step S2222: Extract the local moving map corresponding to the local laser map from the pre-stored robot moving map.

[0105] In this embodiment, after determining the local laser map, the mobile robot 10 can perform map feature matching between the local laser map and the robot's mobile map to extract a local map whose map feature similarity exceeds a preset similarity threshold from the robot's mobile map, and then use the extracted local map as the local mobile map corresponding to the local laser map.

[0106] Sub-step S2223: Calculate the map feature difference value between the local laser map and the local moving map.

[0107] In this embodiment, the mobile robot 10 can determine the map feature difference value between the local laser map and the local moving map by comparing map features. The map feature difference value can be represented by map elevation differences. Therefore, the step of calculating the map feature difference value between the local laser map and the local moving map may include:

[0108] The local laser map and the local moving map are rasterized according to a preset number of grids;

[0109] For each map grid in the local laser map, calculate the elevation difference between the maximum map elevation value of the map grid and the maximum map elevation value of the target grid corresponding to the location of the map grid in the local moving map;

[0110] The average elevation difference of each of the map grids in the local laser map is calculated to obtain the corresponding average elevation difference.

[0111] The calculated average elevation difference is used as the map feature difference value.

[0112] Therefore, this application can determine the specific map feature differences between the local laser map and the local moving map by executing the specific steps of the above sub-step S2223.

[0113] Sub-step S2224 compares the map feature difference values ​​with the preset feature difference threshold.

[0114] Sub-step S2225: If the map feature difference value is greater than or equal to the preset feature difference threshold, determine that the LiDAR data is invalid in the current operating environment of the mobile robot.

[0115] Sub-step S2226: If the map feature difference value is less than the preset feature difference threshold, determine that the LiDAR data is valid in the current operating environment of the mobile robot.

[0116] Specifically, when the map feature difference value between the local laser map and the local moving map corresponding to the lidar 16 is greater than or equal to a preset feature difference threshold, it indicates that the current operating environment may have rainy and / or open-air scenes, and the data of the lidar 16 is invalid in the current operating environment; when the map feature difference value between the local laser map and the local moving map corresponding to the lidar 16 is less than the preset feature difference threshold, it indicates that the current operating environment does not have rainy and / or open-air scenes, and the data of the lidar 16 is valid in the current operating environment.

[0117] Therefore, this application can accurately detect the device effectiveness of the lidar 16 in the current operating environment by executing the above sub-steps S2221 to S2226.

[0118] Alternatively, please refer to Figure 5 , Figure 5 yes Figure 2 The flowchart of step S220 is shown in the third example. In this embodiment of the application, for the GNSS system 18, step S220 may include sub-steps S2231 to S2233 to accurately detect the effective status of the devices in the GNSS system 18 in the current operating environment.

[0119] Sub-step S2231: Based on the historical sensing data of the GNSS system before the current time and the actual sensing data of the GNSS system, the data continuity status of the GNSS system is detected.

[0120] Sub-step S2332: If the detected data continuity is in a continuous state, determine that the GNSS system data is valid in the current operating environment of the mobile robot.

[0121] Sub-step S2333: If the detected data continuity is in a discontinuous state, determine that the GNSS system is in a data failure state in the current operating environment of the mobile robot.

[0122] If the data continuity status of the GNSS system 18 indicates that the sensor data collected by the GNSS system 18 has obvious breakpoints or drastic changes, it indicates that the data continuity status of the GNSS system 18 is in a discontinuous state, the current operating environment may have a dense obstruction scene, and the data of the GNSS system 18 is invalid in the current operating environment; if the data continuity status of the GNSS system 18 indicates that the sensor data collected by the GNSS system 18 does not have obvious breakpoints or drastic changes, it indicates that the data continuity status of the GNSS system 18 is in a continuous state, the current operating environment does not have a dense obstruction scene, and the data of the GNSS system 18 is valid in the current operating environment.

[0123] Therefore, this application can accurately detect the device effectiveness status of the GNSS system 18 in the current operating environment by executing the above sub-steps S2231 to S2233.

[0124] Step S230: Identify a first target sensing device with valid data among the visual sensor and lidar, and identify a second target sensing device with valid data among the GNSS system and wheel odometer.

[0125] In this embodiment, the wheel odometer 19 can be assumed to be a second target sensing device with valid data; if the visual sensor 15 has valid data in the current operating environment, it can be used as a first target sensing device, otherwise it will not be used as a first target sensing device; if the lidar 16 has valid data in the current operating environment, it can be used as a first target sensing device, otherwise it will not be used as a first target sensing device; if the GNSS system 18 has valid data in the current operating environment, it can be used as a second target sensing device, otherwise it will not be used as a second target sensing device.

[0126] Therefore, by performing the above step S230, this application can adaptively select a variety of effective sensors that are compatible with the current operating environment of the mobile robot 10 by utilizing the adaptability of various sensors to different working conditions and environments as the robot's operating environment changes.

[0127] Step S240: Perform factor graph optimization pose estimation on the actual sensing data of the first target sensing device and the IMU respectively to obtain the corresponding target tightly coupled pose information.

[0128] In this embodiment, when the first target sensing device includes the visual sensor 15 and / or the lidar 16, the target tightly coupled pose information may include visual inertial odometry (VIO) information corresponding to the visual sensor 15 and / or laser inertial odometry (LIO) information corresponding to the lidar 16.

[0129] Alternatively, please refer to Figure 6 , Figure 6 yes Figure 2 The flowchart of step S240 includes the sub-steps. In the embodiments of this application, step S240 may include sub-steps S241 to S243 to perform tightly coupled fusion pose localization of multiple effective sensor data adapted to the current operating environment using a factor graph optimization algorithm.

[0130] Sub-step S241 involves pre-integrating the actual sensing data from the IMU to obtain the corresponding inertial pre-integrated data.

[0131] Sub-step S242: For each first target sensing device, perform odometer information prediction processing based on the actual sensing data of the first target sensing device to obtain device odometer information that matches the first target sensing device.

[0132] The device odometry information includes robot pose information and pose covariance matrix determined based on the actual sensing data of the corresponding first target sensing device. If the first target sensing device includes the vision sensor 15, image processing techniques such as feature tracking, feature matching, and consistency detection can be used to process the actual sensing data of the vision sensor 15. In this case, the device odometry information corresponding to the vision sensor 15 is visual odometry (VO) information. If the first target sensing device includes the lidar 16, point cloud processing techniques such as point cloud feature extraction and inter-frame matching can be used to process the actual sensing data of the lidar 16. In this case, the device odometry information corresponding to the lidar 16 is laser odometry (LO) information.

[0133] Sub-step S243: The factor graph optimization algorithm is used to perform tight-coupled pose estimation processing on the inertial pre-integration data and the device odometer information of each of the first target sensing devices to obtain the target tight-coupled pose information.

[0134] In this embodiment, the mobile robot 10 can use the inertial pre-integration data and the device odometer information of each first target sensing device as input factors of a traditional factor graph optimization algorithm, and then use the traditional factor graph optimization algorithm to perform data calculation, thereby obtaining target tightly coupled pose information that matches the pose status of the first target sensing device.

[0135] Therefore, this application can perform tight-coupled fusion pose localization of multiple effective sensor data adapted to the current operating environment by executing the above sub-steps S241 to S243 and using the factor graph optimization algorithm.

[0136] Step S250: Call the preset filter to perform fusion pose estimation on the actual sensing data of the second target sensing device and the IMU respectively, and obtain the corresponding target loosely coupled pose information.

[0137] In this embodiment, after the mobile robot 10 determines the second target sensing device with valid data in the current operating environment, it can use an ESKF (Error State Kalman Filter) filter to perform loosely coupled fusion pose estimation on the actual sensing data of all the second target sensing devices and the IMU 17 to obtain the corresponding target loosely coupled pose information.

[0138] Alternatively, please refer to Figure 7 , Figure 7 yes Figure 2 The flowchart of step S250 is shown below. In this embodiment, step S250 may include sub-steps S251 to S253 to perform loosely coupled fusion pose localization of multiple effective sensor data adapted to the current operating environment using the ESKF filtering algorithm.

[0139] Sub-step S251 involves pre-integrating the actual sensing data from the IMU to obtain the corresponding inertial pre-integrated data.

[0140] Sub-step S252: For each second target sensing device, perform odometer information prediction processing based on the actual sensing data of the second target sensing device to obtain device odometer information that matches the second target sensing device.

[0141] In this embodiment, the second target sensing device includes at least the wheeled odometer 19. If the second target sensing device includes the GNSS system 18 in addition to the wheeled odometer 19, the predicted device odometer information may include robot pose information and pose covariance matrix determined based on the actual sensing data of the corresponding second target sensing device.

[0142] Sub-step S253: The inertial pre-integration data is used as the pose prediction value of the preset ESKF filter, and the device odometer information of all second target sensing devices is used as the pose observation value of the ESKF filter. The ESKF filter is called to perform loosely coupled pose estimation processing to obtain the target loosely coupled pose information.

[0143] Therefore, this application can perform loosely coupled fusion pose localization of multiple effective sensor data adapted to the current operating environment by executing the above sub-steps S251 to S253.

[0144] Step S260: Perform factor graph optimization and correction on the target tightly coupled pose information and the target loosely coupled pose information to obtain the actual predicted pose information of the mobile robot at the current moment.

[0145] In this embodiment, the mobile robot 10 can use the target tightly coupled pose information obtained by the factor graph optimization algorithm and the target loosely coupled pose information obtained by the ESKF filtering algorithm as input factors of the traditional factor graph optimization algorithm. Then, the traditional factor graph optimization algorithm is used to perform data calculation to correct the target tightly coupled pose information through the target loosely coupled pose information. In this way, the factor graph optimization algorithm and the ESKF filtering algorithm are organically combined in the process of robot pose localization by leveraging the complementary advantages of multiple sensors. This ensures that the final estimated pose information can accurately represent the real-time pose status of the mobile robot 10. Furthermore, based on the adaptability of each sensor to different working environments, the robot can adaptively select suitable and effective multi-sensor data for fusion pose localization as the robot's operating environment changes. This achieves the complementary advantages of multiple sensors in the robot pose localization process and improves the accuracy of robot pose localization in complex outdoor environments.

[0146] Furthermore, after calculating the actual estimated pose information, the mobile robot 10 can use the NDT (Normal Distributions Transform)-OMP (Orthogonal Matching Pursuit) fine registration algorithm to verify the obtained actual estimated pose information. If the verification is successful, the actual estimated pose information will be directly output. Otherwise, the mobile robot 10 will jump back to the above steps S240 to S260 and repeat the execution to ensure that the final output actual estimated pose information is true and reliable.

[0147] Therefore, by executing the above steps S210 to S260, this application utilizes the adaptability of various sensors to different working environments to adaptively select suitable and effective data from multiple sensors for fusion pose localization as the robot moves and the operating environment changes. This achieves complementary advantages of multiple sensors during robot pose localization and improves the accuracy of robot pose localization in complex outdoor environments.

[0148] In this application, to ensure that the mobile robot 10 can perform the aforementioned robot positioning method through the robot positioning device 100, this application implements the aforementioned functions by dividing the robot positioning device 100 into functional modules. The specific composition of the robot positioning device 100 provided in this application is described below.

[0149] Please refer to Figure 8 , Figure 8 This is a schematic diagram of the robot positioning device 100 provided in an embodiment of this application. In this embodiment, the robot positioning device 100 may include a sensor data acquisition module 110, a device failure detection module 120, a valid device screening module 130, a first pose estimation module 140, a second pose estimation module 150, and a predicted pose correction module 160.

[0150] The sensor data acquisition module 110 is used to acquire the actual sensor data collected by the visual sensor, lidar, IMU, GNSS system and wheeled odometer at the current moment.

[0151] The equipment failure detection module 120 is used to detect whether the data of the visual sensor, the lidar and the GNSS system are valid in the current operating environment of the mobile robot, based on the actual sensing data of the visual sensor, the lidar and the GNSS system.

[0152] The valid device screening module 130 is used to identify a first target sensing device with valid data among the visual sensor and the lidar, and to identify a second target sensing device with valid data among the GNSS system and the wheel odometer.

[0153] The first pose estimation module 140 is used to perform factor graph optimization pose estimation on the actual sensing data of the first target sensing device and the IMU respectively, so as to obtain the corresponding target tightly coupled pose information.

[0154] The second pose estimation module 150 is used to call a preset filter to perform fusion pose estimation on the actual sensing data of the second target sensing device and the IMU respectively, so as to obtain the corresponding target loosely coupled pose information.

[0155] The pose estimation correction module 160 is used to perform factor graph optimization correction on the target tightly coupled pose information and the target loosely coupled pose information to obtain the actual estimated pose information of the mobile robot at the current moment.

[0156] It should be noted that the robot positioning device 100 provided in this embodiment has the same basic principle and technical effect as the aforementioned robot positioning method. For the sake of brevity, any parts not mentioned in this embodiment can be referred to the above description of the robot positioning method.

[0157] In the embodiments provided in this application, it should be understood that the disclosed apparatus and methods can also be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the flowcharts and block diagrams in the accompanying drawings show the architecture, functionality, and operation of possible implementations of the apparatus, methods, and computer program products according to embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than those marked in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram and / or flowchart, and combinations of blocks in block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions.

[0158] Furthermore, the functional modules in the various embodiments of this application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part. If the function is implemented as a software functional module and sold or used as an independent product, it can be stored in a readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a readable storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned readable storage medium includes: USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, optical disks, and other media capable of storing program code.

[0159] In summary, in the robot localization method and apparatus, mobile robot, and readable storage medium provided in the embodiments of this application, this application detects whether the data from the visual sensor, lidar, and GNSS system are valid in the current operating environment of the mobile robot. Then, based on the detection results, it determines a first target sensing device with valid data from the visual sensor and lidar, and a second target sensing device with valid data from the GNSS system and wheeled odometer. Subsequently, it performs factor graph optimization pose estimation on the actual sensing data of the first target sensing device and the IMU to obtain the corresponding tightly coupled target pose information, and calls a preset filter. The device performs fusion pose estimation on the actual sensing data of the second target sensing device and the IMU to obtain the corresponding loosely coupled target pose information. Then, it performs factor graph optimization and correction on the tightly coupled and loosely coupled target pose information to obtain the actual predicted pose information of the mobile robot at the current moment. Thus, based on the adaptability of various sensors to different working environments, it can adaptively select suitable and effective multi-sensor data for fusion pose localization as the robot's operating environment changes. This enables the complementary advantages of multiple sensors in the robot pose localization process, improving the accuracy of robot pose localization in complex outdoor environments.

[0160] The above descriptions are merely various embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A robot localization method, characterized in that, Applied to mobile robots, wherein the mobile robot includes a vision sensor, lidar, IMU, GNSS system, and wheeled odometer, the method includes: Acquire the actual sensing data collected by the visual sensor, the lidar, the IMU, the GNSS system, and the wheeled odometer at the current moment; Based on the actual sensing data of the visual sensor, the lidar, and the GNSS system, it is determined whether the data of the visual sensor, the lidar, and the GNSS system are valid in the current operating environment of the mobile robot. A first target sensing device with valid data is identified in the visual sensor and the lidar, and a second target sensing device with valid data is identified in the GNSS system and the wheel odometer; Factor graph optimization pose estimation is performed on the actual sensing data of the first target sensing device and the IMU respectively to obtain the corresponding target tightly coupled pose information. A preset filter is invoked to perform fusion pose estimation on the actual sensing data of the second target sensing device and the IMU respectively, so as to obtain the corresponding target loosely coupled pose information. Factor graph optimization and correction are performed on the tightly coupled pose information and the loosely coupled pose information of the target to obtain the actual predicted pose information of the mobile robot at the current moment.

2. The method according to claim 1, characterized in that, For the aforementioned visual sensor, the step of detecting whether the visual sensor's data is valid in the current operating environment of the mobile robot based on the actual sensing data of the visual sensor includes: The actual sensing data of the vision sensor is subjected to grayscale image conversion processing to obtain the corresponding grayscale image to be detected. The grayscale image to be detected is subjected to Shi-Tomasi corner detection processing to obtain the number of Shi-Tomasi corners in the grayscale image to be detected; The number of Shi-Tomasi corners in the grayscale image to be detected is compared with a preset corner number threshold. If the number of Shi-Tomasi corners in the grayscale image to be detected is greater than or equal to the preset corner number threshold, the data from the vision sensor in the current operating environment of the mobile robot is determined to be valid. If the number of Shi-Tomasi corners in the grayscale image to be detected is less than the preset corner number threshold, the visual sensor is determined to be faulty in the current operating environment of the mobile robot.

3. The method according to claim 1, characterized in that, For the aforementioned lidar, the step of detecting whether the lidar data is valid in the current operating environment of the mobile robot based on the actual sensing data of the lidar includes: Based on the actual estimated pose information of the mobile robot at the current moment and the actual sensing data of the lidar, a map estimation is performed to obtain the corresponding local lidar map. Extract the local movement map corresponding to the local laser map from the pre-stored robot movement map; Calculate the map feature difference value between the local laser map and the local moving map; The map feature difference values ​​are compared with a preset feature difference threshold. If the map feature difference value is greater than or equal to the preset feature difference threshold, it is determined that the LiDAR data is invalid in the current operating environment of the mobile robot. If the map feature difference value is less than the preset feature difference threshold, the data from the lidar is determined to be valid in the current operating environment of the mobile robot.

4. The method according to claim 3, characterized in that, The step of calculating the map feature difference value between the local laser map and the local moving map includes: The local laser map and the local moving map are rasterized according to a preset number of grids; For each map grid in the local laser map, calculate the elevation difference between the maximum map elevation value of the map grid and the maximum map elevation value of the target grid corresponding to the location of the map grid in the local moving map; The average elevation difference of each of the map grids in the local laser map is calculated to obtain the corresponding average elevation difference. The calculated average elevation difference is used as the map feature difference value.

5. The method according to claim 1, characterized in that, For the GNSS system, the step of detecting whether the GNSS system is valid in the current operating environment of the mobile robot based on the actual sensing data of the GNSS system includes: Based on the historical sensing data of the GNSS system before the current moment and the actual sensing data of the GNSS system, the data continuity of the GNSS system is detected. If the detected data continuity is in a continuous state, the GNSS system is deemed to have valid data in the current operating environment of the mobile robot. If the detected data continuity is in a state of discontinuity, the GNSS system is deemed to have data failure in the current operating environment of the mobile robot.

6. The method according to any one of claims 1-5, characterized in that, The step of performing factor graph optimization pose estimation on the actual sensing data of the first target sensing device and the IMU respectively to obtain the corresponding target tightly coupled pose information includes: The actual sensing data of the IMU is pre-integrated to obtain the corresponding inertial pre-integrated data; For each first target sensing device, odometer information prediction processing is performed based on the actual sensing data of the first target sensing device to obtain device odometer information that matches the first target sensing device. The inertial pre-integration data and the device odometer information of each of the first target sensing devices are subjected to tight-coupled pose estimation processing using the factor graph optimization algorithm to obtain the target tight-coupled pose information.

7. The method according to any one of claims 1-5, characterized in that, The step of fusing pose estimation of the actual sensing data of the second target sensing device and the IMU by calling a preset filter to obtain the corresponding loosely coupled target pose information includes: The actual sensing data of the IMU is pre-integrated to obtain the corresponding inertial pre-integrated data; For each second target sensing device, odometer information prediction processing is performed based on the actual sensing data of the second target sensing device to obtain device odometer information that matches the second target sensing device. The inertial pre-integration data is used as the pose prediction value of the preset ESKF filter, and the device odometer information of all second target sensing devices is used as the pose observation value of the ESKF filter. The ESKF filter is called to perform loosely coupled pose estimation processing to obtain the target loosely coupled pose information.

8. A robot positioning device, characterized in that, An application in mobile robots, wherein the mobile robot includes a vision sensor, lidar, IMU, GNSS system, and wheeled odometer, the device comprising: The sensor data acquisition module is used to acquire the actual sensor data collected by the visual sensor, the lidar, the IMU, the GNSS system and the wheeled odometer at the current moment. The equipment failure detection module is used to detect whether the data from the visual sensor, the lidar, and the GNSS system are valid in the current operating environment of the mobile robot, based on the actual sensing data of each of the visual sensor, the lidar, and the GNSS system. An effective device screening module is used to identify a first target sensing device with valid data among the visual sensor and the lidar, and to identify a second target sensing device with valid data among the GNSS system and the wheel odometer; The first pose estimation module is used to perform factor graph optimized pose estimation on the actual sensing data of the first target sensing device and the IMU respectively to obtain the corresponding target tightly coupled pose information. The second pose estimation module is used to call a preset filter to perform fusion pose estimation on the actual sensing data of the second target sensing device and the IMU respectively, so as to obtain the corresponding target loosely coupled pose information. The pose estimation and correction module is used to perform factor graph optimization and correction on the target tightly coupled pose information and the target loosely coupled pose information to obtain the actual estimated pose information of the mobile robot at the current moment.

9. A mobile robot, characterized in that, It includes a processor and a memory, the memory storing a computer program that can be executed by the processor to implement the robot localization method according to any one of claims 1-7.

10. A readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the robot localization method according to any one of claims 1-7.

Images