An unmanned aerial vehicle based on SLAM positioning navigation of laser radar

Abstract

The application relates to the technical field of positioning, in particular to a kind of unmanned aerial vehicle based on laser radar SLAM positioning navigation, comprising: unmanned aerial vehicle body, INS positioning module, GNSS positioning module, SLAM positioning module, processor module and map information acquisition module;Under normal circumstances, the unmanned aerial vehicle is positioned by INS, when the unmanned aerial vehicle reaches node, GNSS positioning data is acquired, when the difference between GNSS positioning data and INS positioning data exceeds the set threshold, then it is judged that satellite signal is weak, at this time, the real-time position of unmanned aerial vehicle in the preset map is judged by the SLAM positioning module built-in unmanned aerial vehicle, and the position information cache of INS positioning module is updated;In this way, on the one hand, the positioning accuracy of the unmanned aerial vehicle can be guaranteed, and on the other hand, the working time of the SLAM positioning module can be reduced.

Description

Technical Field

[0001] This application relates to the field of positioning technology, and in particular to a drone based on lidar SLAM positioning and navigation. Background Technology

[0002] Existing UAV dynamic positioning systems are based on GNSS systems. They acquire GNSS positioning data to achieve dynamic positioning of the UAV. However, this method has significant positioning errors when navigation satellite signals are weak. In particular, when the UAV needs to traverse narrow terrain, relying solely on GNSS positioning poses a safety hazard. Summary of the Invention

[0003] To address the problems existing in the prior art, this application provides a UAV based on lidar SLAM positioning and navigation.

[0004] A UAV based on LiDAR SLAM positioning and navigation includes: a UAV body, an INS positioning module, a GNSS positioning module, a SLAM positioning module, a processor module, and a map information acquisition module; wherein, the INS positioning module, GNSS positioning module, and SLAM positioning module are used to acquire positioning data of the UAV body; the map information acquisition module is used to acquire map information; the processor module is used to plan the flight path of the UAV based on the positioning data and map information; the processor module is also used to run a program to enable and disable the INS positioning module, GNSS positioning module, and SLAM positioning module;

[0005] When the UAV is running, the INS positioning module remains on; the GNSS positioning module acquires the UAV's positioning data from the GNSS system at set time intervals; the SLAM positioning module acquires the UAV's positioning data based on preset path nodes; when the GNSS and SLAM positioning modules acquire positioning data, they correct the positioning data of the INS positioning module.

[0006] The processor module is also used to compare the positioning data obtained by the INS positioning module, GNSS positioning module and SLAM positioning module, and analyze the reliability of the positioning data.

[0007] Furthermore, when the map information acquisition module acquires map information, the map information includes global map information and local map information constructed by SLAM. The global map information includes coordinate system, scale, starting coordinates, ending coordinates, precise coordinates (X, Y, Z) of all known obstacles and their physical size / features; the starting coordinates are obtained from GNSS, and the ending coordinates are obtained from the user.

[0008] Furthermore, the processor module sets the following condition parameters when planning the flight path:

[0009] Set a safe distance threshold Set minimum safe distance Calculate the distance from each point on the path to the nearest obstacle. Set path planning rules Set safe path conditions Setting dangerous path conditions .

[0010] Furthermore, the preset path nodes are configured with the following condition parameters:

[0011] In the safe path, with Set path nodes for spacing. For fixed values; in dangerous paths, with Set path nodes for spacing; The following Change with change, setting Follow The coefficient that changes with the change ;

[0012] .

[0013] Furthermore, when the processor module determines the reliability of the positioning data, the set condition parameters include:

[0014] Acquire GNSS positioning data Obtain INS location data Set positioning error threshold Set data difference ;

[0015] Data difference calculation ;

[0016] when GNSS is reliable when GNSS is active; when GNSS is active, GNSS is reliable when GNSS is active. At that time, GNSS was unreliable.

[0017] Furthermore, when executing the program, the processor module is also used to: correct the INS positioning data when GNSS is reliable. When GNSS is unreliable, the SLAM positioning module is activated to correct the INS positioning data based on the SLAM positioning data.

[0018] Furthermore, the positioning error threshold is based on the time since the last positioning verification. Changes with the changes: ;

[0019] in: This is the static error threshold. This represents the INS error growth rate.

[0020] Furthermore, when the processor module corrects the INS positioning data based on the SLAM positioning data, the set condition parameters include: selecting multiple reference objects from the global map information; calculating the distance between the UAV and the multiple reference objects; and calculating the real-time position of the UAV based on the multiple distances.

[0021] Furthermore, when the processor module corrects the INS positioning data based on the SLAM positioning data, the set condition parameters also include:

[0022] point Each stage involves corrections. As a preset value, ;

[0023] No. In the first stage, with the first The coordinate data for each stage are the basic coordinates, denoted as... ,by Choose reference points along the X-axis, Y-axis, and Z-axis of the map coordinate system, centered on the reference point. Centered on, select in each direction There is a reference point on each side, that is... , , , , , The actual distances between the drone and each reference object are respectively , , , , Based on the actual distance between the drone and each reference object, After correction, we get:

[0024] ; ; ; ;

[0025] in, for X-axis coordinates for X-axis coordinates for Y-axis coordinate, for Y-axis coordinate, for Z-axis coordinate, for The Z-axis coordinate.

[0026] Furthermore, when the processor module executes the program, it also includes the following steps: when the SLAM positioning module acquires local map information, the processor module compares the local map information with the global map information to determine whether there are new obstacles; when there are new obstacles, the scanning frequency of the SLAM positioning module is increased according to the distance between the new obstacle and the UAV body; the closer the new obstacle is to the UAV body, the higher the scanning frequency of the SLAM positioning module; and a local route is planned based on the local map information acquired by SLAM.

[0027] The technical effects and advantages of this application are as follows:

[0028] In addition to GNSS, this application also uses INS and SLAM positioning technologies to locate the UAV. Under normal circumstances, the UAV is located via INS. When the UAV reaches a node, GNSS positioning data is acquired and compared with INS positioning data. If the difference between the GNSS positioning data and INS positioning data exceeds a set threshold, it is determined that the satellite signal is weak. At this time, the distance between the UAV and several reference objects is obtained through the UAV's built-in SLAM positioning module, thereby determining the UAV's real-time position in a preset map and updating the position information cache of the INS positioning module. In this way, the accuracy of UAV positioning can be guaranteed, and the working time of the SLAM positioning module can be reduced.

[0029] Other features and advantages of this application will be set forth in the following description and will be apparent in part from the description or may be learned by practicing the application. The objectives and other advantages of this application may be realized and obtained by means of the structures pointed out in the description and the accompanying drawings. Attached Figure Description

[0030] Figure 1 This is a schematic diagram of the structure of a drone based on lidar SLAM positioning and navigation;

[0031] Figure 2 It is a flowchart of the positioning method implemented by the processor when executing the program. Detailed Implementation

[0032] 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. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0033] Furthermore, in the application, the terms "first," "second," and other similar words are not intended to imply any order, quantity, or importance, but are merely used to distinguish different elements, and the terms "upper," "lower," "left," "right," and other similar words are merely positional relationships in the accompanying drawings.

[0034] In this application, a flight path is planned using obstacle point information, starting point location information, and ending point location information from the acquired global map information. The flight path consists of several safe paths and several dangerous paths. Theoretically, in the absence of external interference, the farther the UAV is from obstacles, the safer it is. Therefore, a safe distance threshold can be set. On the map, when the distance between a flight path point and an obstacle is greater than the threshold, it is considered a safe path; when the distance is less than the threshold, it is considered a dangerous path. Accordingly, evenly spaced path nodes are set in the safe paths, and denser path nodes are set in the dangerous paths. When the UAV reaches a node, it is located in real time via GNSS. When the UAV moves to an area with weak satellite signals, the GNSS positioning of the UAV has a large error. Therefore, it is necessary to determine whether the GNSS positioning of the UAV is accurate, and when the positioning error exceeds the threshold, a new positioning method needs to be used to locate the UAV.

[0035] In this application, in addition to GNSS, INS positioning technology is also used to locate the UAV. Under normal circumstances, the UAV is located by INS. When the UAV arrives at the node, GNSS positioning data is acquired, and the GNSS positioning data and INS positioning data are compared to determine the reliability of the GNSS positioning data and to determine whether the SLAM positioning module needs to be enabled.

[0036] The SLAM localization module uses onboard sensors (cameras, lidar, millimeter-wave radar) to perceive the surrounding environment features in real time. Through feature matching and motion estimation, it calculates the UAV's localization data and builds a real-time local map.

[0037] Specifically:

[0038] This application provides a UAV based on LiDAR SLAM positioning and navigation, including: a UAV body, an INS positioning module, a GNSS positioning module, a SLAM positioning module, a processor module, and a map information acquisition module; wherein, the INS positioning module, GNSS positioning module, and SLAM positioning module are used to acquire positioning data of the UAV body; the map information acquisition module is used to acquire map information; the processor module is used to plan the flight path of the UAV based on the positioning data and map information; the processor module is also used to run a program to enable and disable the INS positioning module, GNSS positioning module, and SLAM positioning module;

[0039] When the UAV is running, the INS positioning module remains on; the GNSS positioning module acquires the UAV's positioning data from the GNSS system at set time intervals; the SLAM positioning module acquires the UAV's positioning data and local map information based on preset path nodes; when the GNSS and SLAM positioning modules acquire positioning data, they correct the positioning data of the INS positioning module.

[0040] The processor module is also used to compare the positioning data obtained by the INS positioning module, GNSS positioning module and SLAM positioning module, and analyze the reliability of the positioning data.

[0041] Specific implementation:

[0042] When acquiring map information, the map information acquisition module obtains global map information and local map information constructed by SLAM. The global map information includes coordinate system, scale, starting coordinates, ending coordinates, precise coordinates (X, Y, Z) of all known obstacles and their physical size / features; the starting coordinates are obtained from GNSS, and the ending coordinates are obtained from the user.

[0043] According to the above embodiments, during the operation of the UAV, the INS positioning module remains always on, while the GNSS module periodically acquires positioning data from the GNSS system. The SLAM positioning module acquires the UAV's positioning data and local map information based on preset path nodes. To avoid collisions between the UAV and obstacles during the GNSS system's operating time intervals or in the middle of path nodes, path nodes are set based on the minimum distance between the flight path and obstacles. Specifically:

[0044] When planning a flight path, the processor module sets the following parameters:

[0045] Set a safe distance threshold Set minimum safe distance Calculate the distance from each point on the path to the nearest obstacle. Set path planning rules Set safe path conditions Setting dangerous path conditions .

[0046] When setting up location verification nodes, set the spacing between location verification nodes in the security path. (Unit: m), with Set the spacing for location verification nodes on safe paths; set the spacing for location verification nodes on hazardous paths. (Unit: m) Follow Change with change The smaller, The smaller it is, therefore, it can be set Follow The coefficient that changes with the change ,but The calculation formula is: .

[0047] It should be noted that GNSS consumes more power than INS, and INS has better anti-interference capabilities than GNSS. However, the positioning accuracy of INS decreases over time. Therefore, in this embodiment, INS is used as the normal positioning module, and GNSS is used as an auxiliary positioning module to periodically correct the INS positioning data. Furthermore, the higher the frequency of GNSS correction of INS positioning data, the more accurate the real-time positioning of the UAV. Conversely, the closer the UAV is to obstacles, the higher the risk factor during flight. Therefore, [the following is a more detailed explanation of the specific requirements for GNSS and INS positioning]. Follow Adapting to change can further improve the flight safety of drones.

[0048] It should be noted that before correcting the INS positioning data based on the GNSS positioning data, it is necessary to determine whether the UAV has reached the positioning verification node. When the difference between the coordinate data of the INS data and the coordinate data of the positioning verification node is less than the preset value, it is determined that the UAV has reached the preset positioning verification node. Otherwise, the UAV has not reached the positioning verification node. When the UAV has reached the preset positioning verification node, the INS positioning data is corrected using the acquired GNSS positioning data.

[0049] On the other hand, under normal circumstances, the positioning data of the UAV is provided by INS. Since the positioning data provided by INS has errors, and the error value increases with the increase of flight time, positioning verification nodes are set in the flight path to perform precise single-point positioning of the UAV through GNSS. On the other hand, when the flight attitude remains unchanged, the positioning error of INS has a certain threshold within a unit of time. Thus, the reliability of GNSS positioning data can be judged based on this threshold.

[0050] Therefore, in one embodiment of this application, when the processor executes the program, it further includes determining the reliability of the GNSS positioning data based on the INS positioning error threshold.

[0051] Specifically: Obtain GNSS positioning data Obtain INS location data Set positioning error threshold Set position difference ;

[0052] Calculation of position difference: ,

[0053] when GNSS is reliable when it is active; when it is not active. At that time, GNSS was unreliable;

[0054] When GNSS is reliable, correct INS positioning data. ;

[0055] When GNSS is unreliable, the SLAM positioning module is activated to acquire SLAM positioning data, and the INS positioning data is corrected based on the SLAM positioning data.

[0056] In one embodiment of this application, the positioning error threshold It is floating; the positioning error threshold depends on the time since the last positioning verification. Changes with the changes: ;

[0057] in: This is the static error threshold. This represents the INS error growth rate.

[0058] Therefore, considering the accumulation of INS errors over time, we can avoid misjudging the reliability of GNSS when there is no calibration for a long time.

[0059] It should be noted that when using a SLAM positioning module to locate drones, the closer the SLAM positioning module is to the reference object, the more accurate the positioning. The SLAM positioning module calculates the distance between the drone and multiple reference objects, and calculates the drone's real-time position based on these multiple distances.

[0060] Furthermore, when selecting a reference point, the division Each stage As a preset value, .

[0061] Phase 1: Using INS real-time positioning coordinates as the base coordinates, denoted as... ,by Choose reference points along the X-axis, Y-axis, and Z-axis of the map coordinate system, centered on the reference point. Centered on, select in each direction There is a reference point on each side, that is... , , , , , The actual distances between the drone and each reference object are respectively , , , , , Based on the actual distance between the drone and each reference object, After correction, we get:

[0062] , , , ,

[0063] in, for X-axis coordinates for X-axis coordinates for Y-axis coordinate, for Y-axis coordinate, for Z-axis coordinate, for The Z-axis coordinate.

[0064] The second stage, with Based on the coordinates, Choose reference points along the X-axis, Y-axis, and Z-axis of the map coordinate system, centered on the reference point. Centered on, select in each direction There is a reference point on each side, that is... , , , , , The actual distances between the drone and each reference object are respectively , , , , , Based on the actual distance between the drone and each reference object, After correction, we get: , , , ,

[0065] in, for X-axis coordinates for X-axis coordinates for Y-axis coordinate, for Y-axis coordinate, for Z-axis coordinate, for The Z-axis coordinate.

[0066] Thus, according to the above rules, after the [number]th [period]... After a certain stage, we obtain .

[0067] When the above corrections are made, the error value decreases as the number of corrections increases, and the function of the error value with respect to the number of corrections converges to 0.

[0068] In this way, the actual X-axis coordinate of the drone can be calculated using the coordinates of the reference object in the X-axis direction and the distance between the drone and the reference object; the actual Y-axis coordinate of the drone can be calculated using the coordinates of the reference object in the Y-axis direction and the distance between the drone and the reference object; and the actual Z-axis coordinate of the drone can be calculated using the coordinates of the reference object in the Z-axis direction and the distance between the drone and the reference object.

[0069] It should be noted that selecting two reference objects with opposite directions on the same coordinate axis aims to reduce the ranging error through multiple calculations.

[0070] Based on the above, the SLAM positioning module, as another auxiliary positioning system, is only activated when GNSS data is unreliable. However, due to the lag in the positioning data acquired by the SLAM positioning module, the UAV may be unable to avoid obstacles after the SLAM positioning module provides the positioning data. Therefore, in one embodiment of this application, the program executed by the processor module further includes: a SLAM positioning strategy, specifically:

[0071] A positioning verification node is set at a predetermined distance before entering the dangerous path. When the drone flies to this node, the SLAM positioning module is activated and settings are configured. when At that time, the frequency of the probe waves emitted by the SLAM module is increased, and the rate of increase of the frequency increases with... and The difference increases as the difference increases.

[0072] In one embodiment of this application, when the processor module executes the program, it further includes: when the SLAM positioning module acquires local map information, the processor module compares the local map information with the global map information to determine whether there are new obstacles; when there are new obstacles, the scanning frequency of the SLAM positioning module is increased according to the distance between the new obstacle and the UAV body, and the closer the distance between the new obstacle and the UAV body, the higher the scanning frequency of the SLAM positioning module; and a local route is planned based on the local map information acquired by SLAM.

[0073] Accordingly, Figure 2 As shown, this application discloses a UAV based on LiDAR SLAM positioning and navigation, whose processor module, when executing a program, implements positioning methods including but not limited to the following:

[0074] Get map information;

[0075] Planned flight path

[0076] Configure the activation conditions for the GNSS and SLAM modules;

[0077] Enabling conditions for periodic scanning GNSS and SLAM modules;

[0078] Enable the INS module;

[0079] When the conditions for enabling the GNSS module are met, the GNSS module is enabled and GNSS positioning data is acquired.

[0080] Update INS positioning data when GNSS positioning data is reliable;

[0081] When GNSS positioning data is unreliable, or when the conditions for enabling SLAM are met, the SLAM module is activated and SLAM positioning data is acquired.

[0082] Update INS location data based on SLAM location data.

[0083] Finally, it should be noted that the above description is only a preferred embodiment of this application and is not intended to limit this application. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A UAV based on SLAM positioning navigation with laser radar, characterized in that, The unmanned aerial vehicle comprises an unmanned aerial vehicle body, an INS positioning module, a GNSS positioning module, a SLAM positioning module, a processor module, and a map information acquisition module; the INS positioning module, the GNSS positioning module, and the SLAM positioning module are used to acquire positioning data of the unmanned aerial vehicle body; the map information acquisition module is used to acquire map information; the processor module is used to plan a flight path of the unmanned aerial vehicle according to the positioning data and the map information; the processor module is also used to run a program to enable and disable the INS positioning module, the GNSS positioning module, and the SLAM positioning module; When the unmanned aerial vehicle body is running, the INS positioning module is always in an enabled state; the GNSS positioning module acquires positioning data of the unmanned aerial vehicle from a GNSS system according to a set time interval; the SLAM positioning module acquires positioning data of the unmanned aerial vehicle and acquires local map information according to a preset path node; when the GNSS positioning module and the SLAM positioning module acquire the positioning data, the positioning data of the INS positioning module is corrected; The processor module is also used to compare the positioning data acquired by the INS positioning module, the GNSS positioning module, and the SLAM positioning module, and analyze the reliability of the positioning data; When the processor module judges the reliability of the positioning data, the set condition parameters comprise: When the map information acquisition module acquires the map information, the map information comprises global map information and local map information constructed by the SLAM, wherein the global map information comprises a coordinate system, a scale, a starting point coordinate, an ending point coordinate, accurate coordinates (X, Y, Z) of all known obstacles and their physical sizes / features; the starting point coordinate is acquired from the GNSS, and the ending point coordinate is acquired from a user. Acquiring GNSS positioning data , acquiring INS positioning data , setting a positioning error threshold , setting a data difference ; data difference calculation, ; When GNSS is reliable; when GNSS is not reliable; The processor module, when executing the program, is further configured to implement: when the GNSS is reliable, correcting the INS positioning data ; when the GNSS is unreliable, enabling a SLAM positioning module, and correcting the INS positioning data according to SLAM positioning data. The positioning error threshold varies according to the change in time since the last positioning check: where: is a static error threshold, is an INS error growth rate.​ 2. The unmanned aerial vehicle based on SLAM positioning and navigation by laser radar according to claim 1, characterized in that, When the processor module plans the flight path, the set condition parameters comprise: 3.The UAV based on SLAM positioning and navigation with laser radar according to claim 1, wherein, The preset path node, the set condition parameters comprise: Setting a safety distance threshold ; setting a minimum safety distance ; calculating the distance of each point on the path to the nearest obstacle ; setting path planning rules ; setting safety path conditions ; setting dangerous path conditions .

4. The unmanned aerial vehicle based on SLAM positioning and navigation by laser radar according to claim 3, characterized in that, When the processor module corrects the INS positioning data according to the SLAM positioning data, the set condition parameters comprise: selecting multiple reference objects from the global map information; calculating distances between the radar and the multiple reference objects; and calculating a real-time position of the radar according to the multiple distances. In the safe path, the path nodes are set at intervals of , in the dangerous path, the path nodes are set at intervals of ; in the dangerous path, the path nodes are set at intervals of ; , which varies with , is set to , which varies with , is set to ; .

5. The unmanned aerial vehicle based on SLAM positioning and navigation with laser radar according to claim 1, characterized in that, When the processor module corrects the INS positioning data according to the SLAM positioning data, the set condition parameters further comprise:

6. The unmanned aerial vehicle based on SLAM positioning and navigation with laser radar according to claim 5, characterized in that, When the processor module executes the program, the SLAM positioning module acquires local map information, the processor module compares the local map information with the global map information, judges whether there is a new obstacle, and increases a scanning frequency of the SLAM positioning module according to a distance between the new obstacle and the unmanned aerial vehicle body; the closer the distance between the new obstacle and the unmanned aerial vehicle body, the higher the scanning frequency of the SLAM positioning module; and a local route is planned based on the local map information acquired by the SLAM. phases are executed, correction, preset value, ; No. In the first stage, with the first The coordinate data for each stage are the basic coordinates, denoted as... ,by Choose reference points along the X-axis, Y-axis, and Z-axis of the map coordinate system, centered on the reference point. Centered on, select in each direction There is a reference point on each side, that is... , , , , , The actual distances between the radar and each reference point are respectively , , , , Based on the actual distance between the radar and each reference object... After correction, we get: ， ， ， ， in, for X-axis coordinates for X-axis coordinates for Y-axis coordinate, for Y-axis coordinate, for Z-axis coordinate, for The Z-axis coordinate.

7. The unmanned aerial vehicle based on SLAM positioning and navigation with laser radar according to claim 1, characterized in that, ​

Images