A robot localization and navigation system and method based on multimodal information fusion

Abstract

This invention discloses a robot positioning and navigation system and method based on multimodal information fusion, belonging to the field of multi-source information fusion navigation technology. The robot positioning and navigation system based on multimodal information fusion of this invention includes: a visual sensing module, an inertial measurement unit (IMU), a BeiDou navigation receiver, a control processor, and an electronically controlled driver. This invention addresses the problem that existing technologies rely on a single guidance method for robots, resulting in low positioning and navigation accuracy and reliability, and difficulty in adapting to complex environments. By fusing positioning information from different guidance methods, such as visual, IMU, and BeiDou navigation information, this invention improves the positioning accuracy and reliability of the robot in complex environments, and can better adapt to low-texture scenes such as changes in lighting, large-angle curves, and rapid movements.

Description

Technical Field

[0001] This invention relates to the field of multi-source information fusion navigation technology, specifically to a robot positioning and navigation system and method based on multimodal information fusion. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) technology is a fundamental requirement for robots operating in unknown environments. Existing single-sensor SLAM technologies are relatively mature, such as LOAM based on LiDAR and ORB-SLAM based on cameras. However, they still cannot handle degradation and errors caused by the limitations of the sensors themselves. For example, LiDAR cannot handle glass surfaces or scenes lacking structured information, and cameras cannot extract effective information in the dark. Inertial Measurement Units (IMUs) can solve the degradation problem of LiDAR and camera information in a short time, but cannot avoid drift problems during long-term operation. Different sensors have their own advantages and disadvantages, and can compensate for each other; therefore, fusing information from multiple sensors is essential in practical applications.

[0003] Autonomous landing of robots on ships requires extremely high positioning accuracy. Precise positioning is a core technology, its main function being to provide accurate and highly reliable input information to the automatic control system, namely the relative positional relationship between the robot's current position and the ideal landing point. Common guidance methods include radar, satellite navigation, inertial navigation, and electro-optical guidance. However, using any one of these guidance methods alone is insufficient to achieve ideal results in terms of key technical performance aspects such as guidance accuracy and reliability.

[0004] Chinese patent CN118149854A discloses an odometry method based on multi-sensor fusion, comprising: acquiring an event stream output by an event camera and extracting corner points; using the LK optical flow method to track and triangulate the extracted corner points to obtain the trajectory in the VIO local coordinate system as the pose estimation result of the VIO; initializing the GNSS to obtain the coordinate trajectory in ECEF and aligning it with the trajectory in the VIO local coordinate system to obtain the pose estimation result of the GNSS; constructing a joint optimization objective function based on nonlinear joint optimization and using the original measurement data obtained from the event camera, IMU and GNSS; using the Ceres optimization library to solve the objective function to optimize the above two pose estimation results to obtain the optimized pose estimation result.

[0005] This invention overcomes the motion blur problem in high-speed motion scenes by fusing data from event cameras, IMUs, and GNSS, and improves the accuracy of pose estimation in dynamic environments. However, in robot localization and navigation, in addition to high-speed motion, the effects of low-texture environments and changes in lighting must also be considered to ensure navigation accuracy. Summary of the Invention

[0006] The purpose of this invention is to provide a robot positioning and navigation system and method based on multimodal information fusion, which integrates positioning information from different guidance methods to improve the reliability of input information and solves the problems mentioned in the background art.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] A robot positioning and navigation system based on multimodal information fusion includes: a visual sensing module for acquiring image information from the environment; an inertial measurement unit (IMU) for acquiring motion information of the robot; a Beidou navigation receiver for receiving Beidou navigation signals; a control processor for performing fusion processing on the information provided by the visual sensing module, the inertial measurement unit, and the Beidou navigation receiver to obtain the robot's precise position and attitude information; and an electronically controlled actuator for adjusting the speed of the drive motor according to the speed control commands provided by the control processor, enabling the robot to move along a preset path.

[0009] Preferably, the control processor includes a speed estimation module and a PID controller. The speed estimation module is used to estimate the robot's motion speed in real time and uses a motion state optimization estimation model to achieve optimized estimation of its motion state. The PID controller is used to adjust the control parameters of the electronic control driver according to the optimization variable information provided by the speed estimation module to ensure that the robot's actual speed is consistent with the target speed.

[0010] Preferably, the PID controller employs an autocoupled PID collaborative control strategy to improve the accuracy and response speed of speed control.

[0011] Preferably, the autocoupled PID collaborative control strategy includes: calculating the speed control error of the left and right drive motors; introducing a speed factor Zc to tightly couple the proportional, integral, and derivative components to improve speed control accuracy; and compensating for the speed optimization changes in the sliding window to control the speed of the drive wheel motor.

[0012] Preferably, the visual sensing module includes an RGB-D depth camera for providing three-dimensional environmental information.

[0013] Preferably, the motion state optimization estimation model obtains visual motion constraints, BeiDou navigation motion constraints, and IMU motion constraints input values ​​through an RGB-D depth camera, a BeiDou navigation receiver, and an inertial measurement unit, respectively, and obtains the optimization variable fit degree, key frames, and visual inertial state variables through a motion observation residual nonlinear optimization function.

[0014] Preferably, the control processor further includes: a map building module for building an environmental map based on the robot's pose information; and a path planning module for planning the robot's travel path based on the environmental map.

[0015] Preferably, the inertial measurement unit includes an accelerometer and a gyroscope for measuring and reporting the robot's acceleration, angular velocity, and / or orientation to determine the device's attitude and motion state.

[0016] Preferably, the robot includes a suspension system, a body structure, a chassis structure, and an electronically controlled drive module to achieve autonomous movement.

[0017] An implementation method for a robot localization and navigation system based on multimodal information fusion, comprising the following steps:

[0018] Step 1: Data Acquisition: The all-terrain mobile robot collects visual and self-operation information from the environment by carrying a calibrated RGB-D depth camera, accelerometer, and gyroscope and transmits the data to a web server. It also obtains positioning information through a Beidou navigation receiver.

[0019] Step 2, Data Processing: The all-terrain mobile robot creates camera nodes through the ROS robot operating system, receives image information in real time through the RGB-D depth camera, and uses high- and low-dimensional feature nodes to aggregate feature information around the points of interest in the image to calculate the matching descriptor fit. It adopts a tightly coupled approach to fuse visual and IMU information, uses a forward and reverse indexing strategy to select key frames, and combines BeiDou navigation information to correct pose perception information.

[0020] Step 3: Map Building: The current running speed is calculated and a map is built by calculating the pose perception information of the all-terrain mobile robot;

[0021] Step 4, Speed ​​Control: When the all-terrain mobile robot detects passive deceleration conditions such as large-angle curves and obstacles, and its running speed does not meet the set speed, the control parameters can be calculated based on the deviation between the set value and the current value. The control parameters are then fed back to the electronic control driver to change the motor input voltage, thereby adjusting the motor speed. An autotransformer PID collaborative control strategy is used to restore the current speed of the all-terrain mobile robot to the set value.

[0022] Compared with the prior art, the beneficial effects of the present invention are:

[0023] 1. This invention improves the positioning accuracy and reliability of robots in complex environments by integrating positioning information from different guidance methods, such as vision, IMU and Beidou navigation information.

[0024] 2. This invention can adapt well to low-texture scenes such as changes in lighting, large-angle curves, and fast movements. Compared with traditional SLAM technology, it exhibits stronger adaptability and robustness.

[0025] 3. This invention utilizes an autocoupled PID collaborative control strategy to optimize and estimate variables in the model based on real-time updated motion states, thereby achieving real-time estimation and precise control of the motion speed of an all-terrain mobile robot.

[0026] 4. This invention effectively reduces the trajectory error of all-terrain mobile robots and has good mapping capabilities. Attached Figure Description

[0027] Figure 1 This is a block diagram of a vision-based all-terrain mobile robot prototype according to the present invention;

[0028] Figure 2 This is a framework diagram of the all-terrain mobile robot speed control system of the present invention;

[0029] Figure 3 This is a schematic diagram of the speed control process of the all-terrain mobile robot of the present invention;

[0030] Figure 4 This is a comparative analysis of the trajectory plots of the present invention with ORB-SLAM2 and GCNv2-SLAM on the KITTI and TUM datasets;

[0031] Figure 5 This is a comparative analysis diagram of the indoor and outdoor real-world trajectories of the all-terrain mobile robot of the present invention;

[0032] Figure 6 This is a comparison diagram of the outdoor dynamic trajectory tracking of the all-terrain mobile robot of the present invention; Detailed Implementation

[0033] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0034] To address the problem that existing technologies for guiding robots are relatively simple, resulting in low positioning and navigation accuracy and reliability, and difficulty in adapting to complex environments, this embodiment provides the following technical solution:

[0035] Please see Figure 1 The all-terrain mobile robot in this embodiment mainly consists of a suspension system, a vehicle body structure, a chassis structure, and an electronically controlled drive module.

[0036] The chassis frame is an important component of all-terrain mobile robots. It forms the overall shape of the all-terrain mobile robot and is powered by electronic control. It not only ensures normal driving, but also serves as the load base for its own weight and the weight of the transported objects.

[0037] The vehicle body structure is installed on the chassis frame of the all-terrain mobile robot, connecting the various assemblies and components of the mobile robot. It can withstand various loads from inside and outside the vehicle, and its performance directly affects the control performance and safety of the all-terrain mobile robot.

[0038] The suspension system is a crucial component for all-terrain mobile robots to cope with various complex road surfaces. It is an elastic connection between the wheels and the vehicle body, which can reduce the impact caused by uneven road surfaces, keep the vehicle in a stable posture during driving, and effectively transfer loads, reduce impacts, and dampen vibrations, thus ensuring the safety and stability of the all-terrain mobile robot.

[0039] Electric drive is a crucial component that provides power for the autonomous movement of all-terrain mobile robots. It can control the speed of the left and right drive motors through algorithms according to the tasks set by the user, coordinate the power distribution and output, and enable the all-terrain mobile robot to move in different scenarios.

[0040] A robot localization and navigation system based on multimodal information fusion includes:

[0041] The visual sensing module is used to acquire image information from the environment and provide 3D environmental information, employing an RGB-D depth camera, the Inter D435.

[0042] An inertial measurement unit (IMU) is used to acquire motion information of the robot, including an accelerometer and a gyroscope UM7. It can be used to measure and report the robot's acceleration, angular velocity and / or orientation to determine the device's attitude and motion state.

[0043] The Beidou navigation receiver is used to receive Beidou navigation signals and adopts the K823W Beidou navigation terminal.

[0044] The control processor is used to fuse information from the vision sensing module, inertial measurement unit, and BeiDou navigation receiver to obtain the robot's precise position and attitude information. The control processor includes a velocity estimation module and a PID controller. The velocity estimation module estimates the robot's motion velocity in real time, using a motion state optimization estimation model to optimize its motion state. The PID controller adjusts the control parameters of the electronic drive based on the optimization variable information provided by the velocity estimation module to ensure that the robot's actual speed matches the target speed. The PID controller employs an autocoupling PID cooperative control strategy to improve the accuracy and response speed of velocity control.

[0045] The control processor also includes: a map building module for building an environment map based on the robot's pose information; and a path planning module for planning the robot's travel path based on the environment map.

[0046] An electronically controlled actuator is used to adjust the speed of the drive motor according to the speed control instructions provided by the control processor, so that the robot can move along a preset path.

[0047] Please see Figure 2 The all-terrain mobile robot achieves speed control through an electronically controlled actuator to ensure stable movement at a set speed. To ensure the accuracy of the mobile robot's speed control, visual information (acquired by an Inter D435 depth camera), IMU information (acquired by an accelerometer and UM7 gyroscope), and BeiDou information (acquired by a K823W BeiDou terminal) are fused together, and a motion state optimization estimation model is used to optimize the estimation of its motion state. Based on this, a drive motor speed control system is constructed.

[0048] The drive motor speed control system of the all-terrain mobile robot realizes the real-time estimation of the robot's motion speed based on the real-time optimization variable fit degree, key frames, and visual inertial state variables in the motion state optimization estimation model, and completes the real-time control of its drive motor by combining an autocoupling PID cooperative control strategy.

[0049] Please see Figure 3-6 An implementation method for a robot localization and navigation system based on multimodal information fusion, comprising the following steps:

[0050] Step 1: Data Acquisition: The all-terrain mobile robot collects visual and self-operation information from the environment by carrying a calibrated RGB-D depth camera, accelerometer, and gyroscope and transmits the data to a web server. It also obtains positioning information through a Beidou navigation receiver.

[0051] Step 2, Data Processing: The all-terrain mobile robot creates camera nodes through the ROS robot operating system, receives image information in real time through the RGB-D depth camera, and uses high- and low-dimensional feature nodes to aggregate feature information around the points of interest in the image to calculate the matching descriptor fit. Visual and IMU information are fused using a tightly coupled approach, and keyframes (kf) are selected using a forward and backward indexing strategy. j And combine BeiDou navigation information to correct the pose perception information;

[0052] Step 3: Map Building: The current running speed is calculated and a map is built by calculating the pose perception information of the all-terrain mobile robot;

[0053] Step 4, Speed ​​Control: When the all-terrain mobile robot detects passive deceleration conditions such as large-angle curves and obstacles, and its running speed does not meet the set speed, the control parameters can be calculated based on the deviation between the set value and the current value. The control parameters are then fed back to the electronic control driver to change the motor input voltage, thereby adjusting the motor speed. An autotransformer PID collaborative control strategy is used to restore the current speed of the all-terrain mobile robot to the set value.

[0054] The all-terrain mobile robot employs an autocoupled PID cooperative control strategy to regulate its movement speed, ensuring safe and stable movement. The steps are as follows:

[0055] Step 1: When the all-terrain mobile robot detects large-angle curves and passive deceleration due to obstacles, it determines the speed based on the distance d between the left and right drive wheels of the all-terrain mobile robot and the longitudinal component v of the current speed v. ex With tangential component v ey And the expected speed v of the all-terrain mobile robot t With the desired angular velocity ω t The speed control error e of its left and right drive motors is calculated. l and e r They are respectively:

[0056] (1)

[0057] Step 2: To improve the speed control accuracy, a speed factor Zc is introduced. Zc tightly couples the proportional, integral, and derivative components to form an autocoupling PID collaborative control, which overcomes the shortcomings of the proportional, integral, and derivative components being independent of each other and lacking coordination in the control process.

[0058] (2)

[0059] Where α=α1 / T t 0 < α1 < 100, β = 1 / T t T t This represents the transition time from dynamic to steady state. e1, e2, and e3 are the tracking error, integral error, and derivative error, respectively, and b0 is the control channel gain adjustment parameter.

[0060] u is the cooperative output of the autotransformer PID control:

[0061] (3)

[0062] Step 3: Optimize the change in velocity Δv within the sliding window. s Compensation is used to control the speed of the drive wheel motors of the all-terrain mobile robot. The speed control strategy for the all-terrain mobile robot is as follows:

[0063] (4)

[0064] Among them, P h P h-1 are the speed control outputs at the current speed control moment and the previous speed control moment (h-1), respectively. is the control error between the left and right drive motor moments h and the previous moment (h-1) in equation (4).

[0065] Working principle: First, image information from the environment and robot motion information are collected by sensors such as RGB-D depth cameras and inertial measurement units. Positioning information is obtained using a Beidou navigation terminal, and a camera node is created through the ROS robot operating system to receive depth camera image information in real time. Feature information around points of interest in the image is aggregated, and visual and IMU information are fused in a tightly coupled manner. Next, key frames are selected through a forward and reverse indexing strategy, and pose perception information is corrected by combining Beidou navigation information. Then, the variables in the motion state optimization estimation model are optimized based on the real-time updated motion state, including visual and inertial state variables, key frame information, etc., to achieve real-time estimation of the motion state of the all-terrain mobile robot. Finally, an autocoupled PID cooperative control strategy is adopted. Based on the distance between the left and right drive wheels of the all-terrain mobile robot, the longitudinal and tangential components of the current speed, and the desired speed and angular velocity, the speed control error of the left and right drive motors is calculated. A speed factor Zc is introduced to tightly couple the proportional, integral, and derivative components to form an autocoupled PID cooperative control, so as to achieve real-time estimation and precise control of the current speed of the all-terrain mobile robot.

[0066] It should be noted that, in this document, relational terms such as "first" and "second" are used only 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 process, method, article, or apparatus.

[0067] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention.

Claims

1. A robot positioning and navigation system based on multimodal information fusion, characterized in that: include: The visual sensing module is used to acquire image information from the environment; An inertial measurement unit (IMU) is used to collect motion information of the robot. Beidou navigation receiver, used to receive Beidou navigation signals; The control processor is used to perform fusion processing based on information provided by the vision sensing module, inertial measurement unit and Beidou navigation receiver to obtain the robot's precise position and attitude information; An electronically controlled actuator is used to adjust the speed of the drive motor according to the speed control instructions provided by the control processor, so that the robot can move along a preset path; The control processor includes a speed estimation module and a PID controller. The speed estimation module is used to estimate the robot's motion speed in real time and uses a motion state optimization estimation model to achieve optimized estimation of its motion state. The PID controller is used to adjust the control parameters of the electronic control driver according to the optimization variable information provided by the speed estimation module to ensure that the robot's actual speed is consistent with the target speed. The PID controller employs an autocoupled PID collaborative control strategy to improve the accuracy and response speed of speed control. The autocoupled PID collaborative control strategy includes: calculating the speed control error of the left and right drive motors; introducing a speed factor Zc to tightly couple the proportional, integral and derivative components to improve speed control accuracy; and compensating for the speed optimization changes in the sliding window to control the speed of the drive wheel motor.

2. The robot positioning and navigation system based on multimodal information fusion according to claim 1, characterized in that: The visual sensing module includes an RGB-D depth camera for providing three-dimensional environmental information.

3. The robot positioning and navigation system based on multimodal information fusion according to claim 2, characterized in that: The motion state optimization estimation model obtains visual motion constraints, BeiDou navigation motion constraints, and IMU motion constraints input values ​​through an RGB-D depth camera, a BeiDou navigation receiver, and an inertial measurement unit, respectively. The optimization variable, the fit degree, the key frame, and the visual-inertial state variables are obtained through a motion observation residual nonlinear optimization function.

4. A robot positioning and navigation system based on multimodal information fusion according to claim 3, characterized in that: The control processor further includes: a map building module for building an environmental map based on the robot's pose information; and a path planning module for planning the robot's travel path based on the environmental map.

5. A robot positioning and navigation system based on multimodal information fusion according to claim 4, characterized in that: The inertial measurement unit includes an accelerometer and a gyroscope, used to measure and report the robot's acceleration, angular velocity, and / or orientation to determine the device's attitude and motion state.

6. A robot positioning and navigation system based on multimodal information fusion according to claim 5, characterized in that: The robot includes a suspension system, a body structure, a chassis structure, and an electronically controlled drive module to achieve autonomous movement.

7. An implementation method for a robot localization and navigation system based on multimodal information fusion, implemented based on the robot localization and navigation system based on multimodal information fusion as described in any one of claims 1-6, characterized in that: Includes the following steps: Step 1: Data Acquisition: The all-terrain mobile robot collects visual and self-operation information from the environment by carrying a calibrated RGB-D depth camera, accelerometer, and gyroscope and transmits the data to a web server. It also obtains positioning information through a Beidou navigation receiver. Step 2, Data Processing: The all-terrain mobile robot creates camera nodes through the ROS robot operating system, receives image information in real time through the RGB-D depth camera, and uses high- and low-dimensional feature nodes to aggregate feature information around the points of interest in the image to calculate the matching descriptor fit. It adopts a tightly coupled approach to fuse visual and IMU information, uses a forward and reverse indexing strategy to select key frames, and combines BeiDou navigation information to correct pose perception information. Step 3: Map Building: The current running speed is calculated and a map is built by calculating the pose perception information of the all-terrain mobile robot; Step 4, Speed ​​Control: When the all-terrain mobile robot detects large-angle curves and passive deceleration due to obstacles, and its running speed does not meet the set speed, the control parameters are calculated based on the deviation between the set value and the current value. The control parameters are then fed back to the electronic control driver to change the motor input voltage, thereby adjusting the motor speed. An autotransformer PID collaborative control strategy is used to restore the current speed of the all-terrain mobile robot to the set value.

Images