Microgravity-normal air environment flying robot navigation method and system

Abstract

The application provides a microgravity-normal air environment flying robot navigation method and system, and belongs to the technical field of robot autonomous navigation. In order to solve the problem of collision between the robot and the experimental personnel and the floating object in the environment in the near space. The application comprises the following steps: acquiring navigation positioning information based on the point cloud data of the depth vision camera carried by the robot and the attitude angle sensing data collected by the IMU module; determining the point cloud distribution and the voxelized space size; and constructing an environment map based on the acquired accurate positioning information. The application can realize stable control of position and attitude, autonomous navigation target identification and tracking in a complex environment in the cabin, motion planning and collision avoidance, and further realize high-level and more complex tasks such as efficient interaction with the experimental personnel and collaborative work, realize higher-level autonomous operation capability, and has the technical advantages of high safety and high reliability.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, and more specifically, to a navigation method and system for a flying robot in a microgravity-normal air environment. Background Technology

[0002] Space robots operating in microgravity environments can be used for unmanned supervision of scientific experiments and routine inspections in such environments, helping to reduce reliance on human personnel and alleviate workload in microgravity maintenance. Over the past two decades, various robots designed for microgravity service applications have received widespread attention both domestically and internationally. It is foreseeable that these technologies will play a crucial role in the future exploration and development of applications in microgravity environments.

[0003] Based on differences in robot configuration and motion mechanisms, robots can be mainly divided into two categories: humanoid robots and floating robots. Among them, floating robots, due to their smaller size and more flexible movement, are gradually becoming the mainstream research direction for in-cabin service robots. It is worth noting that while the platform motion control problem in microgravity environments has been largely solved for space-flying robots, there is still no effective solution to the problem of collisions between robots and experimental personnel or floating objects in close proximity. Especially during the process of experimental personnel walking and moving within the cabin, collisions with robots often lead to serious safety accidents. Therefore, how to achieve the identification and modeling of dynamic targets such as experimental personnel and floating objects based on the flying robot platform, and how to plan safe passage paths in real time and accurately and quickly track the desired motion path to avoid collisions between space-flying robots and dynamic targets are key technical problems that urgently need to be overcome. Summary of the Invention

[0004] The technical problem to be solved by this invention is:

[0005] To address the issue of collisions between robots and experimental personnel, as well as floating objects in close proximity to the environment.

[0006] The technical solution adopted by the present invention to solve the above-mentioned technical problems is as follows:

[0007] This invention provides a navigation method for a flying robot in a microgravity-normal air environment, comprising the following steps:

[0008] S100: Based on the robot's 3D position and orientation determined by the visual odometry of the rear navigation camera, the depth point cloud data collected by the front obstacle avoidance camera, and the attitude angle sensing data collected by the IMU module, navigation and positioning information is obtained, resulting in a state equation. Then, maximum likelihood estimation is performed on the state equation, and assuming that the measurement uncertainty is Gaussian distributed, log-likelihood estimation is obtained and transformed into a nonlinear least squares problem. The projection term and IMU term of the front obstacle avoidance camera are optimized. In the projection term, the left and right cameras in the front obstacle avoidance camera are transformed. In the IMU term, covariance propagation is used to pre-integrate the IMU measurements on the manifold.

[0009] S200. The point cloud distribution is determined and the spatial dimensions are voxelized using the TSDF truncation symbolic distance function method.

[0010] S300: Based on the navigation and positioning information obtained in step S100, including establishing a bounding box, dividing it into equal parts and voxels, converting each voxel into a three-dimensional position point; calculating the TSDF value and weight of the current frame, updating the grid weights according to the truncation distance; and finally fusing the current frame with the global data to obtain a fused in-cabin environment obstacle map.

[0011] S400: Based on the robot pose information and environmental map obtained in steps S100 and S300, a hybrid A* algorithm that considers the dynamic model and control constraints of the flying robot is used to plan a global path, and the generated path is used as the reference trajectory for the underlying control to achieve safe collision avoidance and autonomous navigation of the flying robot.

[0012] Further, in step S100, the following are included:

[0013] For the robot's localization within the cabin, the states that need to be estimated include the robot's center 3D position and orientation as determined by the rear navigation camera's visual odometry, the depth or 3D position of the visual landmarks as determined by the front obstacle avoidance camera, and the time-varying acceleration deviation and gyroscope deviation of the IMU module. All states that need to be estimated are represented as follows:

[0014] (1)

[0015] in, Is The basic position state and world-frame attitude state of the system at the sampling time correspond to the robot's position and orientation at different times in the world coordinate system; The state of the forward obstacle avoidance camera, including the depth of each observed feature. , Is The depth of each feature point; This indicates that the robot is in the world coordinate system. The velocity vector at the sampling moment; This indicates that the accelerometer in the IMU module is... Time-varying zero bias error at the sampling time; This indicates the gyroscope in the IMU module. Time-varying zero bias error at the sampling time; The state of the gyroscope;

[0016] State estimation for the robot can be conceived as a maximum likelihood estimation problem, expressed as follows, under the assumption that all measurements are independent:

[0017]

[0018] in, This represents the estimated system state vector; It is a collection of measurements, including measurement data from the forward obstacle avoidance camera and the IMU module; No. Time sensor The measured value; In a given state Below, sensor Measured values The conditional probability;

[0019] By assuming that the uncertainty of the measurement follows a Gaussian distribution, the log-likelihood of the above equation is denoted as:

[0020]

[0021] in, Indicates sensor At any moment The observation model, whose input is a state vector. The output is a predicted value of the sensor measurement; Indicates sensor At any moment The measurement noise covariance matrix; Indicates the total sampling time;

[0022] At this point, the state estimation of the in-cabin robot is transformed into a nonlinear least squares problem. For the in-cabin robot, this optimization includes the projection terms of the forward obstacle avoidance camera and the IMU term.

[0023] Furthermore, in the projection term, the switching relationship between the left and right cameras in the front obstacle avoidance camera is described as follows:

[0024]

[0025] in, Indicates the left camera Each feature point in The observed coordinates at that moment; Indicates the first Each feature point in The position of the feature points of the left camera is predicted at all times based on observations from the right camera and system parameters; Indicates the first Rotation matrix and position for each keyframe; express The rotation matrix and position at each moment; Indicates the first Two-dimensional pixel coordinates of a feature point at time t; This represents the transformation matrix from the machine coordinate system to the current camera coordinate system; Indicates the current time Transformation matrix from world coordinate system to body coordinate system; This indicates that map points are moved from keyframes. The rotation matrix from the camera coordinate system to the world coordinate system; This is a camera projection function used to project 3D points onto a 2D image plane; Indicates the first The depth of each feature point;

[0026] In the IMU term, covariance propagation is used to pre-integrate the IMU measurements on the manifold; between two sampling times t, the pre-integration produces the relative positions. ,speed and rotation matrix The IMU residual is defined as:

[0027]

[0028] in, Indicates from arrive Relative displacement at any given moment; Indicates from arrive The change in relative velocity at any given moment; from arrive The relative rotation matrix at time; express Zero bias of the accelerometer and gyroscope at all times; express Zero bias of the accelerometer and gyroscope at all times; Represents gravitational acceleration; express The rotation matrix and position at each moment; express Time and The speed of time;

[0029] The optimization is solved using the Gauss-Newton method, and the final cost function is expressed as:

[0030]

[0031] in, Residual Jacobian matrix of state variables; State vector The incremental update vector, i.e., the adjustment amount in the current iteration step; Indicates the first In the next iteration, the Jacobian matrix is ​​calculated. Corresponding residual terms When assigning weights, use the covariance estimate obtained at the end of the previous iteration;

[0032] Solving this optimization problem allows us to obtain more accurate navigation and positioning information.

[0033] Further, in step S200, the TSDF truncated symbolic distance function method is selected for mapping. Based on the distribution of the point cloud reconstructed in the environment, the size of the boundary surrounding all point clouds is determined, and the space within the boundary is voxelized according to the set size. Thus, the space is divided into multiple small cubes, and each voxel corresponds to a point in the space.

[0034] Furthermore, each voxel is evaluated by two quantities: the distance of the voxel to the nearest obstacle surface, denoted as . That is, signed distance; the weight during voxel update, denoted as Let the actual depth of the obstacle surface to the front obstacle avoidance camera be... Depth data collected by the front obstacle avoidance camera The symbolic distance value is then expressed as ,when If the voxel is in front of the actual obstacle surface, it indicates that the voxel is in front of the obstacle surface; otherwise, it indicates that the voxel is behind the obstacle surface.

[0035] Further, in step S300, the following are included:

[0036] Construct a rectangular bounding box that completely encloses all possible point clouds from this measurement; then divide the rectangular bounding box into... Divide the data into equal parts; store all voxels in the entire space into the GPU for computation, with each thread processing one voxel. ;for The lattice coordinates, each GPU process scans and processes one Lattice pillars in coordinate system; for each voxel in the constructed solid. , transformation Let q be a three-dimensional position point in the world coordinate system;

[0037] Calculate the TSDF value and weight of the current frame: Iterate through all voxels and obtain the mapping point of point q in the world coordinate system to the camera coordinate system using the camera pose matrix of the depth data. And back-projected by the camera intrinsic parameter matrix Point to obtain the corresponding pixel in the depth image , to pixel The depth value is denoted as TSDF(a), and the distance from point v to the camera coordinate origin is denoted as tsdf(a). The depth distance is calculated using the following formula:

[0038]

[0039] And update the raster weights based on the truncation distance:

[0040]

[0041] Where W(a) represents the raster weight of frame a; W(a+1) represents the raster weight of frame a+1; and w(a) represents the reliability of the current pixel a.

[0042] Finally, the current frame is fused with the global frame to obtain the fused obstacle map of the cabin environment.

[0043] Furthermore, the robot is equipped with a front obstacle avoidance camera, a rear navigation camera, a laser obstacle avoidance sensor, an IMU module, and two symmetrical power modules. The front obstacle avoidance camera is used to collect structured light point cloud data and binocular grayscale image data within the robot's forward field of view for obstacle avoidance and personnel position perception. The rear navigation camera is used to collect stereo vision information behind the robot and generate real-time positioning data for the robot through algorithms in the built-in onboard core processor. The laser obstacle avoidance sensor supplements the obstacle distance information in the blind spots of the front obstacle avoidance camera and the rear navigation camera. The IMU module is used to acquire the robot's instantaneous acceleration and angular velocity information, which is fused with real-time positioning data to improve navigation performance.

[0044] Furthermore, each power module uses a ducted fan as its power source and is equipped with six jet nozzles. The thrust is controlled by controlling the ventilation volume of each jet nozzle. The combination of two power modules ensures that the robot has two jet nozzles in each direction, and the resultant thrust vector formed by the two jet nozzles in each direction passes through the robot's center of mass. The jet nozzles in the same direction realize translational motion control, and the combination of jet nozzles in different directions realizes the robot's posture rotational motion control.

[0045] A navigation system for a flying robot in a microgravity-normal air environment, the system having a program module corresponding to the above steps, and executing the steps in the above-described method for navigating a flying robot in a microgravity-normal air environment during runtime.

[0046] A computer-readable storage medium storing a computer program configured to, when invoked by a processor, implement steps of a navigation method for a flying robot in a microgravity-normal air environment.

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

[0048] Multi-sensor heterogeneous fusion: Innovatively, D435i (depth point cloud obstacle avoidance), T265 (binocular visual positioning) and IMU data are fused through tight coupling optimization (BA+IMU pre-integration) and extended Kalman filtering, which solves the drift and occlusion problems of single sensors in microgravity environment and significantly improves positioning robustness.

[0049] Dynamic TSDF mapping and hybrid A* planning: The GPU-accelerated TSDF voxel map is fused with multi-source depth data in real time, and combined with the hybrid A* algorithm with aircraft dynamic constraints to generate safe paths in complex floating obstacle environments, breaking through the limitations of the traditional SLAM mapping and motion planning separation.

[0050] Sensor collaborative architecture: Through the collaborative design of D435i covering the forward obstacle avoidance blind zone, T265 providing back-end positioning compensation, and IMU high-frequency attitude correction, centimeter-level collision avoidance of dynamic obstacles (such as experimental personnel) inside the cabin is achieved.

[0051] This scheme is the first to realize a navigation system with tight coupling of multiple cameras and IMUs in a microgravity environment, and its technical performance is superior to that of a single sensor or loosely coupled method.

[0052] IMU-Vision Fusion Localization: This method utilizes an IMU (Inertial Measurement Unit) to provide high-frequency acceleration and angular velocity data, while a visual sensor provides spatial information to calculate the robot's position and orientation by detecting environmental feature points. Fusion of IMU and visual data effectively overcomes the drift problem inherent in vision-based localization, improving accuracy and stability.

[0053] Multi-sensor perception: A front obstacle avoidance camera collects structured light point cloud data and binocular grayscale image data within a 12m range in front of the robot for obstacle avoidance and personnel position perception; a rear navigation camera collects stereo vision information from behind the drone and generates real-time positioning data for the flying robot through a built-in processing module; a Time-of-Flight (TOF) laser distance sensor supplements obstacle distance information in the camera's blind spots, enabling blind-spot obstacle detection and avoiding collisions. The fusion of multiple sensors effectively improves positioning and navigation accuracy.

[0054] By calculating the TSDF voxel blocks within the field of view of each depth image under the forward obstacle avoidance camera, and then weighting and averaging the TSDF voxel blocks to obtain the global fusion of all depth information, this sparse representation method will save memory usage and significantly improve the reconstruction efficiency of the environment map.

[0055] In summary, this invention enables robots to build maps and determine their own positions in unknown microgravity environments through real-time localization and navigation using multiple sensors, effectively improving the robot's navigation accuracy. Attached Figure Description

[0056] Figure 1 This is a diagram showing the composition of the robot in an embodiment of the present invention;

[0057] Figure 2 This is an exploded view of the robot in an embodiment of the present invention;

[0058] Figure 3 This is an inner frame diagram of the robot in an embodiment of the present invention;

[0059] Figure 4 This is a flowchart illustrating the real-time positioning process of the method integrating multi-source navigation devices in this embodiment of the invention.

[0060] Figure 5 is a flowchart of the lightweight mapping and path planning method in this embodiment of the invention.

[0061] Figure 6 This is a flowchart of the robot control method in an embodiment of the present invention;

[0062] Figure 7 This is a comparison diagram of the estimated trajectory of the robot in the actual field in an embodiment of the present invention;

[0063] Figure 8 The following is a summary comparison of the relative errors of the robot after testing in the experimental scenario in the embodiments of the present invention. Among them, (a) is the displacement error effect diagram of the robot after testing in the experimental scenario, and (b) is the rotation error effect diagram of the robot after testing in the experimental scenario.

[0064] Figure 9 This is a simulation diagram of the robot's autonomous navigation process in Rviz during actual operation, as shown in the embodiments of the present invention.

[0065] Figure 10 This is a flowchart of a navigation method for a flying robot in a microgravity-normal air environment according to an embodiment of the present invention.

[0066] Explanation of reference numerals in the attached figures:

[0067] 1. Jet nozzle; 2. Rear navigation camera; 3. Front obstacle avoidance camera; 4. Laser obstacle avoidance sensor; 5. IMU module; 6. Onboard core processor. Detailed Implementation

[0068] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0069] In this invention, the term "space flying robot" has the same meaning as "robot" and can refer to various types of rotor-driven flying robot platforms. Figure 2 and Figure 3 As shown, the robot includes a front obstacle avoidance camera 3, a rear navigation camera 2, a laser obstacle avoidance sensor 4, an IMU module 5, and two symmetrical power modules. The front obstacle avoidance camera 3 is used to collect structured light point cloud data and binocular grayscale image data within a 12m range in front of the robot for obstacle avoidance and personnel position perception. The rear navigation camera 2 is used to collect stereo vision information behind the robot and generate real-time positioning data for the robot through an algorithm in the built-in onboard core processor 6. The laser obstacle avoidance sensor 4 supplements the obstacle distance information in the blind spots of the front obstacle avoidance camera 3 and the rear navigation camera 2, with a perception range of 8m for a single point. The IMU module 5 is used to acquire the robot's instantaneous acceleration and angular velocity information, which is fused with real-time positioning data to improve navigation performance. The onboard core processor 6 is used to calculate the robot's position and attitude information and read robot information from the laser obstacle avoidance sensor 4, the front obstacle avoidance camera 3, the rear navigation camera 2, and the IMU module 5.

[0070] The front obstacle avoidance camera 3 (D435i) is responsible for obstacle recognition and personnel detection, and its structured light point cloud data directly participates in TSDF map updates; the rear navigation camera 2 (T265) provides back-end visual odometry, and compensates for the blind spots of the front obstacle avoidance camera 3 through feature matching to ensure positioning continuity; the high-frequency attitude data of the IMU module 5 suppresses the cumulative error of the visual odometry, enabling the system to maintain centimeter-level positioning accuracy even in a microgravity floating environment; finally, through multi-sensor hierarchical fusion and robust motion planning, autonomous obstacle avoidance and collaborative operation in the complex environment inside the cabin are achieved.

[0071] Combination Figure 2As shown, to ensure the momentum balance of the robot (similar to a flying robot) body system, the aerodynamic system of the flying robot is designed with two symmetrical power modules on the left and right sides. The two power modules achieve pressure balance through the internal channels of the robot. Each power module uses a ducted fan as its power source and is equipped with six jet nozzles 1. The thrust is controlled by controlling the ventilation volume of each jet nozzle 1. After the two power modules are combined, the robot body contains two jet nozzles 1 in each direction, and the resultant thrust vector formed by the two jet nozzles 1 in each direction passes through the robot's center of mass. The jet nozzles 1 in the same direction realize translational motion control, and the combination of jet nozzles 1 in different directions realizes the robot's attitude rotational motion control. Through the above jet thrust system layout design, it can be ensured that the aerodynamic system can replace the flywheel to perform attitude control tasks when the flywheel fails. The front obstacle avoidance camera 3 is a D435i depth vision camera with a large depth field of view, used for real-time and accurate measurement of obstacle information. The front obstacle avoidance camera 3 has high resolution. The minimum depth measurement distance at the specified rate is 0.2m, which is less than the ideal measurement distance (0.3~3m). The camera intrinsic parameters are calibrated as follows: focal length x: fx = 642.0, focal length y: fy = 642.0, principal point x: cx = 424.0, principal point y: cy = 240.0, image width: width = 848, image height: height = 480; this meets the obstacle avoidance requirements for robot movement within the cabin. The rear navigation camera 2 is a T265 binocular camera, which can... The built-in VPU processes binocular fisheye visual data to achieve real-time positioning, while binocular images are output in parallel for various tasks; the camera intrinsic parameters are calibrated as follows: focal length x: fx=285.0, focal length y: fy=285.0, principal point x: cx=428.0, principal point y: cy=240.0, image width: width=848, image height: height=480; the fisheye image resolution can reach 848*800, which can meet the requirements of feature recognition and positioning.

[0072] Combination Figure 3As shown, the laser obstacle avoidance sensor 4 is a TOFSense-M, which has a large measurement angle and a small blind spot. Since the minimum depth distance that the front obstacle avoidance camera 3 can measure at high resolution is 0.2m, multiple TOFSense-M laser rangefinders are selected to achieve blind spot obstacle detection and avoid collisions, thus enabling emergency obstacle avoidance. The IMU module 5 is a MEMSMU. The LPMS-IG1, considering the dynamic stability of robot motion, allows the IMU module 5 to correct measurement errors while ensuring the required measurement accuracy and zero bias error. The onboard core processor 6 is a UPboard motherboard used for robot motion control and navigation. The onboard core processor 6 first generates positioning data based on the binocular images acquired by the front obstacle avoidance camera 3, and then fuses this positioning data with the position data of the rear navigation camera 2 and the laser obstacle avoidance data to obtain higher-precision robot position data. Based on this position data, it combines the depth point cloud image acquired by the front obstacle avoidance camera 3 and the binocular fisheye stereo vision image acquired by the visual navigation camera to map the surrounding environment. Finally, the A* algorithm, which considers dynamics, is used to plan the global path in real time in the established map, and the generated path is used as the reference trajectory for the underlying control.

[0073] The autonomous navigation method of this invention can support flying robots to achieve stable position and attitude control in a floating state, autonomous navigation target identification and tracking in complex cabin environments, and motion planning and collision avoidance. It can then achieve higher-level and more complex tasks such as efficient interaction and collaborative operation with experimental personnel, realize a higher level of autonomous operation capability, and has technical advantages such as high safety and high reliability.

[0074] Specific Implementation Plan 1: Combining Figures 1 to 5 and Figure 10 As shown, this invention provides a navigation method for a flying robot in a microgravity-normal air environment, comprising the following steps:

[0075] S100: Navigation and positioning information is obtained by using the three-dimensional position and orientation of the robot center, located by the visual odometry of the rear navigation camera 2, the depth point cloud data collected by the front obstacle avoidance camera 3, and the attitude angle sensing data collected by the IMU module 5.

[0076] The navigation and positioning data of the front obstacle avoidance camera 3 is obtained by a vision-IMU hybrid positioning algorithm. For the robot's positioning inside the cabin, the states that need to be estimated include the three-dimensional position and orientation of the robot center as determined by the visual odometry of the rear navigation camera 2, the depth or three-dimensional position of the visual landmarks as determined by the front obstacle avoidance camera 3, and the time-varying acceleration deviation and gyroscope deviation of the IMU module 5. All states that need to be estimated are represented as follows:

[0077]

[0078] in, Is The basic position state and world-frame attitude state of the system at the sampling time correspond to the robot's position and orientation at different times in the world coordinate system; The state of the forward obstacle avoidance camera 3, including the depth of each observed feature. , Is The depth of each feature point; This indicates that the robot is in the world coordinate system. The velocity vector at the sampling moment; This indicates that the accelerometer in the IMU module is... Time-varying zero bias error at the sampling time; This indicates the gyroscope in the IMU module. Time-varying zero bias error at the sampling time; The state of the gyroscope;

[0079] This represents the robot's velocity vector at different moments in the world coordinate system; This indicates that the accelerometer in IMU module 5 is in Time-varying zero bias error at the sampling time; This indicates that the gyroscope in IMU module 5 is... Time-varying zero bias error at the sampling time; to simplify the notation, the position of IMU module 5 is represented as the center of the robot;

[0080] The robot's state is then estimated, which is constructed as an MLE (Maximum Likelihood Estimation) problem. This MLE problem consists of the possible distributions of the robot's pose after a period of motion; under the assumption that all measurements are independent, it can be expressed as:

[0081]

[0082] in, This represents the estimated system state vector; It is a collection of measurements, including measurement data from the front obstacle avoidance camera 3 and the IMU module 5; This represents the robot state vector to be estimated (including position, orientation, and IMU bias). No. Time sensor Measurements (such as camera feature point depth, IMU angular velocity); In a given state Below, sensor Measured values The conditional probability; Indicates the sampling time;

[0083] By assuming that the measurement uncertainty follows a Gaussian distribution, the log-likelihood of the above equation can be written as:

[0084]

[0085] in, Indicates sensor At any moment The observation model, whose input is a state vector. The output is a predicted value of the sensor measurement; Indicates sensor At any moment The measurement noise covariance matrix; n represents all time points;

[0086] At this point, the state estimation of the robot inside the cabin is transformed into a nonlinear least squares problem, also known as BA optimization. For the robot inside the cabin, this optimization includes the projection terms and IMU terms of the forward obstacle avoidance camera 3, as detailed below:

[0087] Projection Term: Given the intrinsic parameters of the front obstacle avoidance camera 3 and the positional relationship between the two cameras (left and right) in the front obstacle avoidance camera 3, state calibration is performed offline. For each captured image frame, corner feature detection is first performed. These features are tracked by optical flow between consecutive frames, and the tracker also matches features between the left and right images. Based on the correlation of these features, the camera term in the optimization problem is designed. This term is universal for both the left and right cameras of the front obstacle avoidance camera 3 (binocular camera): it can project a feature from the right image to the left image in both time and space, and it can also project a feature from the left image to the right image in space. The transformation relationship between the left and right cameras in the front obstacle avoidance camera 3 can be described as:

[0088]

[0089] in, Indicates the left camera Each feature point in The observation coordinates at that moment Indicates the first Each feature point in The position of the feature points of the left camera is predicted at all times based on observations from the right camera and system parameters. Indicates the first Rotation matrix and position for each keyframe; express The rotation matrix and position at each moment; Indicates the first Two-dimensional pixel coordinates of a feature point at time t; This represents the transformation matrix from the machine coordinate system to the current camera coordinate system; Indicates the current time Transformation matrix from world coordinate system to body coordinate system; This indicates that map points are moved from keyframes. The rotation matrix from the camera coordinate system to the world coordinate system; This is a camera projection function that projects 3D points onto a 2D image plane. Indicates the first The depth of each feature point;

[0090] IMU Terms: The IMU terms are constructed using an IMU pre-integration algorithm, modeling the acceleration and gyroscope bias caused by time-cumulative changes as a random walk process, the derivative of which is Gaussian white noise. Since IMU module 5 acquires data at a higher frequency than other sensors, multiple IMU measurements exist between two frames; therefore, covariance propagation is used to pre-integrate the IMU measurements on the manifold. Between two sampling times t, the pre-integration generates the relative position. Relative velocity and relative rotation matrix Furthermore, pre-integration also propagates the covariance of relative positions; the IMU residual can be defined as:

[0091]

[0092] in, Indicates from arrive Relative displacement at any given moment; Indicates from arrive The change in relative velocity at any given moment; from arrive The relative rotation matrix at time; express Zero bias (time-varying error) of accelerometer and gyroscope at all times; express Zero bias of the accelerometer and gyroscope at all times; Represents gravitational acceleration; express The rotation matrix and position at each moment; express Time and The speed of time;

[0093] The above problem can be solved using the Gauss-Newton method, and the final cost function is expressed as:

[0094]

[0095] in, Residual Jacobian matrix of state variables; State vector The incremental update vector, i.e., the adjustment amount in the current iteration step; Indicates the first In the next iteration, the Jacobian matrix is ​​calculated. Corresponding residual terms When assigning weights, use the previous ( The covariance estimate obtained at the end of the iteration.

[0096] By solving this optimization problem, relatively accurate navigation and positioning information can be obtained, and the relevant parameters in the above-mentioned navigation and positioning invention can be tested and adjusted using a dataset.

[0097] The depth point cloud data collected by the front obstacle avoidance camera 3, the visual odometry positioning data, laser data, and IMU information are fused using an extended Kalman filter algorithm to obtain a more accurate navigation and positioning effect; the trajectory obtained by verifying the relevant parameters in a real-world environment is as follows. Figure 7 As shown; multiple test results indicate that, compared to using only the rear-view positioning camera, the data fused from multiple sensors is closer to the motion capture positioning result; multiple tests were conducted in the above scenario, and the relative error was summarized in... Figure 8 middle;

[0098] S200: Determine the point cloud distribution and voxelize the spatial dimensions.

[0099] The computationally efficient TSDF method was chosen for mapping. Based on the reconstructed point cloud distribution, the size of the boundary enclosing all point clouds was determined, and the space within the boundary was voxelized according to the set size. This space was then divided into multiple small cubes, with each voxel corresponding to a point in the space. The voxel was evaluated using two quantities: first, the distance from the voxel to the nearest surface, denoted as... First, the signed distance; second, the weights during voxel updates, denoted as... Assuming the actual obstacle surface is at a depth of the front obstacle avoidance camera 3, Depth data collected by the front obstacle avoidance camera 3 The symbolic distance value is then expressed as ,when If the voxel is in front of the actual obstacle surface, it indicates that the voxel is in front of the obstacle surface; otherwise, it indicates that the voxel is behind the obstacle surface.

[0100] S300: Based on the acquired precise positioning information, construct an environmental map.

[0101] All voxels in the entire space are stored in the GPU for computation, with each thread processing one voxel. ;for The lattice coordinates, each GPU process scans and processes one Lattice pillars in coordinate system; for each voxel in the constructed solid. , transformation A three-dimensional position point in the world coordinate system ;

[0102] Next, calculate the TSDF value and weight of the current frame: traverse all voxels and obtain the mapping point of point q in the world coordinate system to the camera coordinate system using the camera pose matrix of the depth data. And back-projected by the camera intrinsic parameter matrix Point to obtain the corresponding pixel in the depth image , to pixel The depth value is denoted as TSDF(a), and the distance from point v to the camera coordinate origin is denoted as tsdf(a). The depth distance is calculated using the following formula:

[0103]

[0104] And update the raster weights based on the truncation distance:

[0105]

[0106] Where W(a) represents the raster weight of frame a; W(a+1) represents the raster weight of frame a+1; w(a) represents the reliability of the current pixel a; finally, the current frame is fused with the global image to obtain the fused obstacle map of the cabin environment, such as... Figure 9 As shown;

[0107] S400: Based on the robot pose information and environmental map information obtained in steps S100-S300, a hybrid A* algorithm that considers the dynamics model and control constraints of the flying robot is used to plan the global path. Each node of the hybrid A* algorithm stores continuous position and orientation information, so that the generated path can meet the actual motion constraints. When expanding nodes, the algorithm does not simply move to adjacent grids, but simulates different turning angles and forward and backward operations according to the motion model of the flying robot to generate possible continuous states. The generated path is used as the reference trajectory for the underlying control. The actuators, including the flywheel and the fan, are used to realize the safe collision avoidance and autonomous navigation of the flying robot.

[0108] Specific implementation scheme two: The present invention provides a navigation system for a flight robot in a microgravity-normal air environment. The system has a program module corresponding to the above steps, and executes the steps in the above-mentioned navigation method for a flight robot in a microgravity-normal air environment when it is running.

[0109] The other combinations and connections in this implementation scheme are the same as in Specific Implementation Scheme 1.

[0110] Specific Implementation Scheme 3: The present invention provides a computer-readable storage medium storing a computer program configured to implement, when called by a processor, the steps of a navigation method for a flying robot in a microgravity-normal air environment.

[0111] The other combinations and connections in this implementation scheme are the same as in Specific Implementation Scheme 1.

[0112] While the present invention has been disclosed above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention, and all such changes and modifications will fall within the scope of protection of the present invention.

Claims

1. A navigation method for a flight robot in a microgravity-normal air environment, characterized in that, Includes the following steps: S100. Based on the visual odometry of the rear navigation camera (2) mounted on the robot, the robot's three-dimensional position and orientation are determined, the depth point cloud data collected by the front obstacle avoidance camera (3) and the attitude angle sensing data collected by the IMU module (5) are used to obtain navigation and positioning information and obtain the state equation. Then, the state equation is subjected to maximum likelihood estimation, and the measurement uncertainty is assumed to be Gaussian distributed to obtain log likelihood estimation. It is then converted into a nonlinear least squares problem to optimize the projection term and IMU term of the front obstacle avoidance camera (3). In the projection term, the left and right cameras in the front obstacle avoidance camera (3) are converted. In the IMU term, the IMU measurement values ​​on the manifold are pre-integrated using covariance propagation. S200. The point cloud distribution is determined and the spatial dimensions are voxelized using the TSDF truncation symbolic distance function method. S300. Based on the navigation and positioning information obtained in step S100, including establishing a bounding box, dividing it into equal parts and then voxelizing it, and converting each voxel into a three-dimensional position point. Calculate the TSDF value and weight of the current frame, and update the grid weights based on the truncation distance; finally, fuse the current frame with the global image to obtain the fused cabin environment obstacle map. include, Construct a cuboid bounding box that can completely enclose all point clouds that appear in this measurement; Then the rectangular bounding box is divided into... Divide the data into equal parts; store all voxels in the entire space into the GPU for computation, with each thread processing one voxel. ;for The lattice coordinates, each GPU process scans and processes one Lattice pillars in coordinate system; for each voxel in the constructed solid. , transformation Let q be a three-dimensional position point in the world coordinate system; Calculate the TSDF value and weight of the current frame: Iterate through all voxels and obtain the mapping point of point q in the world coordinate system to the camera coordinate system using the camera pose matrix of the depth data. And back-projected by the camera intrinsic parameter matrix Point to obtain the corresponding pixel in the depth image , to pixel The depth value is denoted as TSDF(a), and the distance from point v to the camera coordinate origin is denoted as tsdf(a). The depth distance is calculated using the following formula: （7） And update the raster weights based on the truncation distance: （8） Where W(a) represents the raster weight of frame a; W(a+1) represents the raster weight of frame a+1; and w(a) represents the reliability of the current pixel a. Finally, the current frame is fused with the global frame to obtain the fused obstacle map of the cabin environment; S400: Based on the robot pose information obtained in steps S100 and S300 and the fused obstacle map of the cabin environment, a hybrid A* algorithm that considers the dynamic model and control constraints of the flying robot is used to plan a global path, and the generated path is used as the reference trajectory for the underlying control to achieve safe collision avoidance and autonomous navigation of the flying robot.

2. The navigation method for a flight robot in a microgravity-normal air environment according to claim 1, characterized in that: In step S100, the following are included: For the robot's positioning within the cabin, the states that need to be estimated include the robot's center three-dimensional position and orientation as determined by the visual odometry of the rear navigation camera (2), the depth or three-dimensional position of the visual landmarks as determined by the front obstacle avoidance camera (3), and the time-varying acceleration deviation and gyroscope deviation of the IMU module (5). All states that need to be estimated are represented as follows: in, Is The basic position state and world-frame attitude state of the system at the sampling time correspond to the robot's position and orientation at different times in the world coordinate system; The state of the forward obstacle avoidance camera (3), including the depth of each observed feature. , Is The depth of each feature point; This indicates that the robot is in the world coordinate system. The velocity vector at the sampling moment; This indicates that the accelerometer in the IMU module (5) is... Time-varying zero bias error at the sampling time; This indicates the gyroscope in the IMU module (5) is in Time-varying zero bias error at the sampling time; The state of the gyroscope; State estimation for the robot can be conceived as a maximum likelihood estimation problem, expressed as follows, under the assumption that all measurements are independent: in, This represents the estimated system state vector; It is a collection of measurements, including measurement data from the front obstacle avoidance camera (3) and the IMU module (5); No. Time sensor The measured value; In a given state Below, sensor Measured values The conditional probability; By assuming that the uncertainty of the measurement follows a Gaussian distribution, the log-likelihood of the above equation is denoted as: in, Indicates sensor At any moment The observation model, whose input is a state vector. The output is a predicted value of the sensor measurement; Indicates sensor At any moment The measurement noise covariance matrix; Indicates the total sampling time; At this point, the state estimation of the in-cabin robot is transformed into a nonlinear least squares problem. On the in-cabin robot, this optimization includes the projection term of the forward obstacle avoidance camera (3) and the IMU term.

3. The navigation method for a flight robot in a microgravity-normal air environment according to claim 2, characterized in that: In the projection term, the switching relationship between the left and right cameras in the front obstacle avoidance camera (3) is described as follows: in, Indicates the left camera Each feature point in The observed coordinates at that moment; Indicates the first Each feature point in The position of the feature points of the left camera is predicted at all times based on observations from the right camera and system parameters; Indicates the first Rotation matrix and position for each keyframe; express The rotation matrix and position at each moment; Indicates the first Two-dimensional pixel coordinates of a feature point at time t; This represents the transformation matrix from the machine coordinate system to the current camera coordinate system; Indicates the current time Transformation matrix from world coordinate system to body coordinate system; This indicates that map points are moved from keyframes. The rotation matrix from the camera coordinate system to the world coordinate system; This is a camera projection function used to project 3D points onto a 2D image plane; Indicates the first The depth of each feature point; In the IMU term, covariance propagation is used to pre-integrate the IMU measurements on the manifold; between two sampling times t, the pre-integration produces the relative positions. ,speed and rotation matrix The IMU residual is defined as: in, Indicates from arrive Relative displacement at any given moment; Indicates from arrive The change in relative velocity at any given moment; from arrive The relative rotation matrix at time; express Zero bias of the accelerometer and gyroscope at all times; express Zero bias of the accelerometer and gyroscope at all times; Represents gravitational acceleration; express The rotation matrix and position at each moment; express Time and The speed of time; The optimization is solved using the Gauss-Newton method, and the final cost function is expressed as: in, Residual Jacobian matrix of state variables; State vector The incremental update vector, i.e., the adjustment amount in the current iteration step; Indicates the first In each iteration, the Jacobian matrix is ​​calculated. Corresponding residual terms When assigning weights, use the covariance estimate obtained at the end of the previous iteration; Solving this optimization problem allows us to obtain more accurate navigation and positioning information.

4. The navigation method for a flight robot in a microgravity-normal air environment according to claim 1, characterized in that: In step S200, the TSDF truncated symbolic distance function method is selected for mapping. Based on the distribution of the point cloud reconstructed in the environment, the size of the boundary surrounding all point clouds is determined, and the space within the boundary is voxelized according to the set size. Thus, the space is divided into multiple small cubes, and each voxel corresponds to a point in the space.

5. A navigation method for a flight robot in a microgravity-normal air environment according to claim 4, characterized in that: Each voxel is evaluated by two quantities: the distance of the voxel to the nearest obstacle surface, denoted as . That is, signed distance; the weight during voxel update, denoted as Let the actual obstacle surface be at a depth from the front obstacle avoidance camera (3). The depth data collected by the front obstacle avoidance camera (3) The symbolic distance value is then expressed as ,when If the voxel is in front of the actual obstacle surface, it indicates that the voxel is in front of the obstacle surface; otherwise, it indicates that the voxel is behind the obstacle surface.

6. The navigation method for a flight robot in a microgravity-normal air environment according to claim 1, characterized in that: The robot is equipped with a front obstacle avoidance camera (3), a rear navigation camera (2), a laser obstacle avoidance sensor (4), an IMU module (5), and two symmetrical power modules on the left and right. The front obstacle avoidance camera (3) is used to collect structured light point cloud data and binocular grayscale image data within the robot's forward field of view for obstacle avoidance and personnel position perception. The rear navigation camera (2) is used to collect stereo vision information behind the robot and generate real-time positioning data of the robot through the algorithm in the built-in onboard core processor (6). The laser obstacle avoidance sensor (4) supplements the obstacle distance information in the blind spots of the front obstacle avoidance camera (3) and the rear navigation camera (2). The IMU module (5) is used to obtain the robot's instantaneous acceleration and angular velocity information and improve navigation performance by fusing it with real-time positioning data.

7. The navigation method for a flight robot in a microgravity-normal air environment according to claim 1, characterized in that: Each power module uses a ducted fan as its power source and is equipped with six jet nozzles (1). The thrust is controlled by controlling the ventilation volume of each jet nozzle (1). The two power modules are combined to ensure that the entire flying robot contains two jet nozzles (1) in each direction. The combined thrust vector formed by the two jet nozzles (1) in each direction passes through the center of mass of the flying robot. The jet nozzles (1) in the same direction realize translational motion control, and the jet nozzles (1) in different directions are combined to realize the attitude rotation motion control of the flying robot.

8. A navigation system for a flight robot in a microgravity-normal air environment, characterized in that: The system has a program module corresponding to the steps of the navigation method for a flight robot in a microgravity-normal air environment as described in any one of claims 1-7, and executes the steps of the above-described navigation method for a flight robot in a microgravity-normal air environment when it is run.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program configured to, when invoked by a processor, implement the steps of the navigation method for a flying robot in a microgravity-normal air environment as described in any one of claims 1-7.

Images