A UAV indoor SLAM positioning method

Abstract

The present invention discloses a method for indoor SLAM positioning of unmanned aerial vehicles. The method combines a single-line laser radar, an inertial measurement unit and a single-point ranging module, expands the original two-dimensional laser SLAM for solving a single plane to the three-dimensional positioning of unmanned aerial vehicles, introduces a height dimension into the strategy of SLAM map construction and global optimization, and is used to solve the possible inconsistency problem of horizontal maps at different heights; in addition, a matching positioning based on a two-level dynamic search space is proposed, which can better deal with the matching positioning divergence problem that is easy to occur when the unmanned aerial vehicle suddenly turns or changes speed under the premise of saving computing resources. The method of the present invention provides better autonomous positioning and navigation capabilities for micro-unmanned aerial vehicles in GNSS denial scenarios.

Description

Technical Field

[0001] The present invention relates to the field of unmanned system positioning and navigation, and in particular to a high-precision simultaneous localization and mapping (SLAM) method for a micro-unmanned aerial vehicle. Background Art

[0002] In recent years, with the continuous breakthroughs in various technologies such as positioning and navigation, robot control, and artificial intelligence, the application of unmanned systems has also developed rapidly. In the field of drones, applications such as spraying pesticides, aerial inspections, express delivery, and video shooting are becoming increasingly common. One of the keys to achieving these tasks is that drones have the ability to autonomously locate and navigate. At present, most drone applications are in open outdoor environments, which are inseparable from the high-precision positioning information provided by the Global Navigation Satellite System Positioning System (GNSS) and its augmentation system. However, in GNSS-denied environments such as buildings, underground spaces, and forests, traditional indoor positioning methods have great limitations due to the complex spatial structure, high mobility, and little prior information of drone missions. For example, positioning based on wireless LANs must be accurately deployed with auxiliary base stations in advance, while positioning based on inertial measurement units (IMUs) cannot work alone for too long due to the error accumulation effect.

[0003] The introduction of real-time positioning and mapping technology has made up for the inability of unmanned systems to locate themselves in unknown and unfamiliar environments, and has become the mainstream research direction in the field of unmanned system positioning and navigation in recent years. From the perspective of sensor sources, SLAM technology is divided into laser SLAM and visual SLAM; from the perspective of application scenarios, SLAM technology is divided into two-dimensional SLAM and three-dimensional SLAM. For indoor application scenarios of drones, they are usually micro-drones, which have the characteristics of limited load weight, limited computing resources, and strong maneuverability. Traditional two-dimensional laser SLAM does not consider height changes and is mostly used for ground unmanned vehicles. When used with odometers, it can better solve the problem of single horizontal plane positioning; three-dimensional laser SLAM simultaneously collects laser scanning data at multiple heights, extracts features such as lines and surfaces for matching, and obtains three-dimensional positioning results, but multi-line laser radars have the disadvantages of heavy equipment, high power consumption, and high cost; visual SLAM is based on the image information collected by the camera to extract features such as corner points for matching and positioning. It has the advantages of rich visual information and low hardware cost, but problems such as sensitive illumination changes and high dynamic focus blur are also its main disadvantages. Summary of the invention

[0004] The invention discloses an indoor SLAM positioning method for a UAV, which combines a single-line laser radar, an IMU module and a single-point ranging module to provide autonomous positioning and navigation capabilities for a micro UAV in an indoor scene.

[0005] The embodiment of the present invention provides a method for indoor SLAM positioning of a drone, comprising:

[0006] The drone periodically collects laser scanning data, IMU data, and ground ranging data;

[0007] Estimate the latest attitude angle and altitude coordinates of the drone based on IMU data and ground ranging data;

[0008] According to the laser scanning data, a two-level dynamic search space is used to perform correlation matching with the latest local grid map to determine the latest horizontal coordinates and heading angle of the UAV;

[0009] Update the local grid map list based on the latest UAV position and laser scanning data;

[0010] According to the latest posture and laser scanning data of the UAV, the local grid maps with a height difference less than the first threshold are selected to join the global optimization, and the latest posture of the UAV and the global coordinates of the local grid maps are updated.

[0011] Optionally, the method of using the two-level dynamic search space to perform correlation matching with the latest local grid map to determine the latest horizontal coordinates of the drone includes:

[0012] Determine the latest estimated position based on the historical position of the drone;

[0013] Taking the latest estimated posture as the center, establishing a first search space for correlation matching, and selecting the one with the highest correlation score as the first candidate posture;

[0014] If the height difference between the first candidate pose and the latest local grid map is less than a first threshold and the correlation score of the first candidate pose is greater than a second threshold, taking the first candidate pose as the initial pose, and adopting interpolation optimization based on minimum matching residual square to obtain the best matching pose;

[0015] Otherwise, the latest estimated pose is taken as the center, the second search space is expanded to re-match the correlation, the second candidate pose with the highest correlation score in this round is selected as the initial pose, and the interpolation optimization based on the minimum matching residual square is adopted to obtain the best matching pose.

[0016] Optionally, a method for determining the second search space includes:

[0017] If the UAV adopts the "headless" flight mode, the search ranges in the horizontal X and Y directions of the first search space are expanded by equal proportional factors as the search ranges in the horizontal X and Y directions of the second search space, and the heading angle search range of the second search space is kept consistent with the heading angle search range of the first search space;

[0018] If the UAV adopts the "headed" flight mode, the search range is expanded by the proportional factors in the horizontal X and Y directions of the first search space projected along the direction of the nose as the horizontal X and Y direction search range of the second search space, and it is determined whether the heading angle change between two consecutive laser scanning moments exceeds the third threshold. If so, the heading angle search range of the first search space is expanded according to the proportional factor as the heading angle search range of the second search space, otherwise, the heading angle search range of the second search space is kept consistent with the heading angle search range of the first search space.

[0019] Optionally, the updating of the local grid map list includes:

[0020] If the local grid map list is empty, or the height difference between the latest posture of the UAV and the latest member of the local grid map list is less than the first threshold, or the number of posture points of the latest member of the local grid map list reaches the fourth threshold, a new local grid map is created and added to the local grid map list;

[0021] Sorting by joining time, confirming that the other members of the local grid map list except the last two members are set to the completed state, and their grid occupancy probabilities are no longer updated;

[0022] Traversing the members in the local grid map list that are not set to the completed state, if the height difference between the latest posture of the drone and the member that is not set to the completed state is less than the first threshold, or the member that is not set to the completed state is the latest member, adding the latest posture point of the drone to the member that is not set to the completed state, and updating its grid occupancy probability;

[0023] If the number of pose points of the next newest member of the local grid map list reaches twice the fourth threshold, or the height difference with the latest pose of the drone is greater than or equal to the first threshold, the next newest member of the local grid map list is set to a completed state and its grid occupancy probability is no longer updated.

[0024] Optionally, the screening of local grid maps with height differences less than a first threshold value to add to the global optimization includes:

[0025] Traversing the members in the local grid map list that are set to a completed state, selecting a local grid map whose height difference with the latest posture of the drone is less than the first threshold, and calculating a correlation score between the laser scanning data and the local grid map;

[0026] If the correlation score exceeds the fifth threshold, the latest posture of the UAV and the global coordinates of the local grid map are used as nodes for graph optimization, the relative posture of the latest posture of the UAV and the local grid map are calculated as constraint edges for graph optimization, and a loss function based on the minimum square difference is constructed for iterative calculation.

[0027] The above technical solution of the present invention has at least the following beneficial effects:

[0028] 1) By combining a single-line laser radar, an IMU module, and a single-point ranging module, the original two-dimensional laser SLAM that solves a single plane is expanded to the three-dimensional positioning of drones. Compared with pure three-dimensional laser SLAM based on multi-line laser radar, it has obvious advantages in load weight, equipment cost, and computing resources; 2) The proposed two-level dynamic search space can better deal with the matching positioning divergence problem that is prone to occur when the drone suddenly turns or changes speed while saving computing resources; 3) The height dimension is introduced in the strategy of establishing local grid maps and global optimization in SLAM to solve the possible inconsistency of horizontal maps at different heights. Therefore, the embodiment of the present invention is suitable for autonomous positioning and navigation of micro drones in GNSS-denied scenarios such as inside buildings and underground spaces. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a flow chart of a method for indoor SLAM positioning of a drone provided in the first embodiment of the present invention;

[0030] Figure 2 is a schematic diagram of determining the second search space under different flight modes of the UAV provided in the second embodiment of the present invention;

[0031] Figure 3 is a schematic diagram of a process for updating a local grid map list provided in Embodiment 3 of the present invention;

[0032] Figure 4 This is a schematic diagram of member changes in a local grid map list provided in the third embodiment of the present invention. DETAILED DESCRIPTION

[0033] In order to make the technical problems, technical solutions and advantages to be solved by the present invention more clear, a detailed description will be given below with reference to the accompanying drawings and specific embodiments.

[0034] Figure 1 A schematic diagram of a flow chart of a method for indoor SLAM positioning of a drone provided in the first embodiment of the present invention is shown in FIG. Figure 1 As shown, the following steps are included:

[0035] S101: The drone periodically collects laser scanning data, IMU data, and ground ranging data.

[0036] S102: Estimate the latest attitude angle and altitude coordinates of the UAV based on the IMU data and the ground ranging data.

[0037] S103: According to the laser scanning data, a two-level dynamic search space is used to perform correlation matching with the latest local grid map to determine the latest horizontal coordinates and heading angle of the UAV.

[0038] Specifically, the two-level dynamic search space is used to perform correlation matching with the latest local grid map, including:

[0039] (1) Determine the latest estimated position based on the historical position of the UAV;

[0040] (2) Taking the latest estimated posture as the center, establish a first search space for correlation matching, and select the one with the highest correlation score as the first candidate posture;

[0041] (3) If the height difference between the first candidate pose and the latest local grid map is less than a first threshold and the correlation score of the first candidate pose is greater than a second threshold, the first candidate pose is used as the initial pose, and an interpolation optimization based on the minimum matching residual square is constructed to obtain the best matching pose;

[0042] (4) Otherwise, with the latest estimated pose as the center, expand to the second search space and re-perform correlation matching, select the second candidate pose with the highest correlation score in this round as the initial pose, construct an iterative optimization based on the minimum matching residual square, and obtain the best matching pose.

[0043] S104: Update the local grid map list according to the latest posture and laser scanning data of the UAV.

[0044] Specifically, the updating of the local grid map list includes:

[0045] (1) If the local grid map list is empty, or the height difference between the latest posture of the UAV and the latest member of the local grid map list is greater than or equal to the first threshold, or the number of posture points of the latest member of the local grid map list reaches a fourth threshold, a new local grid map is created and added to the local grid map list;

[0046] (2) Sorting by joining time, confirming that the other members of the local grid map list except the last two members are set to the completed state, and their grid occupancy probabilities are no longer updated;

[0047] (3) traversing the members in the local grid map list that are not set to the completed state, if the height difference between the latest posture of the UAV and the member that is not set to the completed state is less than the first threshold, or the member that is not set to the completed state is the latest member, adding the latest posture point of the UAV to the member that is not set to the completed state, and updating its grid occupancy probability;

[0048] (5) If the number of pose points of the next newest member of the local grid map list reaches twice the fourth threshold, or the height difference with the latest pose of the drone is greater than or equal to the first threshold, the next newest member of the local grid map list is set to a completed state, and its grid occupancy probability is no longer updated.

[0049] S105: According to the latest posture and laser scanning data of the UAV, a local grid map with a height difference less than a first threshold is selected to be added to the global optimization, and the latest posture of the UAV and the global coordinates of the local grid map are updated.

[0050] Specifically, the screening of local grid maps with a height difference less than a first threshold value to add to the global optimization includes:

[0051] (1) traversing the members in the local grid map list that are set to a completed state, selecting a local grid map whose height difference with the latest posture of the drone is less than the first threshold, and calculating a correlation score between the laser scanning data and the local grid map;

[0052] (2) If the correlation score exceeds the fifth threshold, the latest posture of the UAV and the global coordinates of the local grid map are used as nodes for graph optimization, and the relative posture of the latest posture of the UAV and the local grid map are calculated as constraint edges for graph optimization. A loss function based on the minimum square difference is constructed for iterative calculation.

[0053] In step S101 of this embodiment, the hardware modules that the drone needs to have include: an IMU module, whose fixed direction is consistent with the coordinate system direction of the drone carrier, used to measure the three-axis acceleration and three-axis angular velocity of the drone; a single-line laser radar, whose fixed scanning plane is consistent with the XY plane of the drone carrier, used to measure the distance from the drone horizontal plane to the surrounding obstacles; a ground ranging module, whose fixed ranging direction is consistent with the Z-axis direction of the drone carrier, used to measure the distance of the drone from the ground; and a computing unit, used to receive the above and execute the above steps. It should be noted that the implementation method of the ground ranging module can be single-point laser, ultrasonic wave, millimeter wave radar, etc., which is not limited here.

[0054] In step S102 of this embodiment, the IMU data includes at least three-axis acceleration and three-axis angular velocity, and preferably, may also include three-axis magnetic intensity. The method for estimating the latest posture of the drone from the IMU data can be based on the recursive solution of Euler angles, cosine matrix or quaternion, which is not limited here. Then, the latest altitude coordinate H of the drone in the navigation coordinate system is calculated by the following formula:

[0055] H=D·|cos(θ)·cos(γ)|

[0056] Wherein, D is the ground distance measurement value, θ and γ are the roll angle and pitch angle respectively.

[0057] In step S103 of this embodiment, the local grid map refers to dividing a local plane space of a certain size into a number of small blocks (i.e., grids) at equal intervals (i.e., resolution) along the horizontal X and Y directions, and each grid has an occupancy probability p, which represents the probability that the grid is occupied by an obstacle. The first search space refers to a set of translation offsets discretely sampled with a certain step size in the horizontal X and Y directions within a set range, and a set of rotation offsets discretely sampled with a certain step size in the heading angle dimension within a set range; the latest estimated posture plus each member of the set constitutes a set of alternative postures. The correlation matching between the laser data and the local grid map refers to finding the grid position projected on the local grid map according to the scanning direction and ranging value of all laser scanning data at the alternative posture. Assume that the i-th alternative posture is The scanning angle of the jth laser scanning beam is α j , the distance to the obstacle is L j , the global pose of the kth local grid map is , the grid resolution is Δd, and the coordinates of the end point of the laser scanning beam on the local grid map are estimated

[0058]

[0059]

[0060] The occupancy probabilities of all laser scanning beam endpoints at the grid positions are accumulated as the relevant scores of the candidate positions:

[0061]

[0062] In step 103 of this embodiment, the interpolation optimization based on the minimum matching residual square refers to obtaining candidate translation offsets and rotation offsets smaller than the grid resolution by interpolation near the initial pose, and selecting a certain increment to iteratively approximate the optimal solution according to the minimization of the matching error. The interpolation method can be bilinear interpolation, bicubic interpolation, inverse distance weighted interpolation, spline interpolation and other methods, and the iteration can be secant iteration method, steepest descent method, conjugate gradient method, Newton method, etc., which are not limited here.

[0063] In step 104 of this embodiment, the newly created local grid map refers to a local grid map constructed with a preset size and resolution using the horizontal coordinates of the latest posture of the drone as the global coordinates, and the initial occupancy probability p of all grids is 1 / 2. In addition, it can be seen that there will be no more than two members in the local grid map list that are not marked as completed, the number of coordinate points added to each local grid map will not exceed twice the fourth threshold at most, and the height difference between each local grid map and the coordinate points added to it will not exceed the first threshold.

[0064] Figure 2 is a schematic diagram of determining the second search space of a drone in different flight modes provided by the second embodiment of the present invention, such as Figure 2 As shown, the gray triangle represents the estimated posture at the corresponding moment, the arrow on the triangle represents the alternative rotation offset of the heading angle, the gray oblique grid represents the horizontal alternative translation offset of the first search space, and the white grid represents the horizontal alternative translation offset of the second search space. The second search space is an extension of the first search space, and the determination method specifically includes:

[0065] (1) If the UAV adopts the "headless" flight mode, the search ranges in the horizontal X and Y directions of the first search space are expanded by equal proportional factors as the search ranges in the horizontal X and Y directions of the second search space, and the heading angle search range of the second search space is kept consistent with the heading angle search range of the first search space;

[0066] (2) If the UAV adopts the "head-on" flight mode, the search range is expanded by the proportional factors in the horizontal X and Y directions of the first search space projected along the direction of the head as the search range in the horizontal X and Y directions of the second search space, and it is determined whether the heading angle change between two consecutive laser scanning moments exceeds a third threshold. If so, the heading angle search range of the first search space is expanded by the proportional factor as the heading angle search range of the second search space; otherwise, the heading angle search range of the second search space is kept consistent with the heading angle search range of the first search space.

[0067] Assuming that the first candidate position that satisfies the iterative optimization is obtained in the first search space in the i-1th laser scan, and the first candidate position that does not meet the conditions is obtained in the first search space in the i-th laser scan, in the "headless" flight mode, the movement of the drone is not constrained by the heading, and the probability of the drone moving in each dimension is the same, so all dimensions of the second search space are expanded by equal proportional factors. In the "headed" flight mode, the speed direction of the drone is consistent with the heading. If there is a mismatch, it is mainly due to two possibilities: 1) the heading direction changes, so the horizontal dimension of the second search space is also expanded in the heading direction; 2) a change of direction occurs, and the IMU sensor and motion model fail to Whether the heading angle search range of the second search space is expanded depends on whether the heading angle change between the latest estimated position and the position at the time of the last laser scan is large enough.

[0068] Figure 3 FIG. 1 is a schematic diagram of a process for updating a local grid map list provided in Embodiment 3 of the present invention. Figure 3 As shown in the figure, after each round of laser scanning, a new pose point is generated and added to the corresponding local grid map, including the following steps:

[0069] S301: Determine whether the local grid map list is empty, if not, proceed to S302, if yes, proceed to S304;

[0070] S302: Determine whether the height difference between the latest position of the UAV and the latest member of the local grid map list is greater than or equal to the first threshold Thr1, if not, proceed to S303, if yes, proceed to S304;

[0071] S303: Determine whether the number of pose points of the latest member of the local grid map list reaches a fourth threshold Thr4, if not, proceed to S305, if yes, proceed to S304;

[0072] S304: Create a new local grid map and add it to the local grid map list, and then enter S305;

[0073] S305: sorting by joining time, confirming that the other members of the local grid map list except the last two members are set to the completed state, and no longer updating their grid occupancy probabilities, and entering S306;

[0074] S306: traverse the members in the local grid map list that are not set to the completed state, and enter S307;

[0075] S307: Determine whether the height difference between the latest posture of the drone and the member that is not set to the completed state is less than the first threshold, if not, proceed to S308, if yes, proceed to S309;

[0076] S308: Determine whether the member that is not set to the completed state is the latest member, if so, proceed to S309, if not, proceed to S311;

[0077] S309: adding the latest posture point of the drone to the member that is not set to the completed state, updating its grid occupancy probability, and entering S310;

[0078] S310: Determine whether it is the newest member of the local grid map list, and whether the number of posture points reaches twice the fourth threshold Thr4. If so, proceed to S311, if not, end this round;

[0079] S311: setting the next newest member of the local grid map list to a completed state, and no longer updating its grid occupancy probability, thus ending this round.

[0080] To better understand the update process of the local grid map list described in this embodiment, Figure 4 Give a specific indication of the changes in list members during the update process, such as Figure 4 As shown, including:

[0081] (1) When the first coordinate point is generated, the local grid map list is empty, so a new local grid map Submap0 is added to the list; from the first coordinate point to the Nth coordinate point, the height difference with Submap0 is less than the first threshold, so they are all added to Submap0 and their grid occupancy probability is updated; where N represents the fourth threshold.

[0082] (2) When the N+1th coordinate point is generated, the number of coordinates of Submap0 reaches N, so a new local grid map Submap1 is added to the list; from the N+1th coordinate point to the N*2th coordinate point, the height difference with Submap0 and Submap1 is less than the first threshold, so they are all added to Submap0 and Submap1, and their grid occupancy probabilities are updated.

[0083] (3) When the N*2+1th coordinate point is generated, the number of coordinates of Submap0 reaches N*2, so it is set to the completed state; the number of coordinates of Submap1 reaches the fourth threshold N, so a new local grid map Submap2 is created and added to the list; from the N*2+1th coordinate point to the N*2+Mth coordinate point, the height difference with Submap1 and Submap2 is less than the first threshold, so they are all added to Submap1 and Submap2, and their grid occupancy probabilities are updated.

[0084] (4) When the N*2+M+1th coordinate point is generated, the height difference with Submap2 exceeds the first threshold, so Submap1 and Submap2 are both set to the completed state, and a new Submap3 is added to the list; from the N*2+M+1th coordinate point to the subsequent coordinate points, the height difference with Submap3 is less than the first threshold, so they are all added to Submap3.

[0085] In this embodiment, the specific method for updating the grid occupancy probability is: according to step S103 of the first embodiment, the latest three-dimensional coordinate point of the drone and the grid coordinates of all laser scanning beams in the local grid map are obtained; all laser scanning beams are traversed, and the occupancy probability p of the grid coordinate point corresponding to the laser scanning beam is increased, and the occupancy probability p of the grid on the line connecting the grid coordinate corresponding to the laser scanning beam and the grid coordinate corresponding to the latest three-dimensional coordinate point of the drone is reduced.

[0086] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A UAV SLAM positioning method, characterized in that: include: The drone periodically collects laser scanning data, IMU data, and ground ranging data; Estimate the latest attitude angle and altitude coordinates of the drone based on IMU data and ground ranging data; According to the laser scanning data, a two-level dynamic search space is used to perform correlation matching with the latest local grid map to determine the latest horizontal coordinates and heading angle of the drone, including: determining the latest estimated posture according to the historical posture of the drone; taking the latest estimated posture as the center, establishing a first search space for correlation matching, and selecting the one with the highest correlation score as the first candidate posture; if the height difference between the first candidate posture and the latest local grid map is less than a first threshold and the correlation score of the first candidate posture is greater than a second threshold, then the first candidate posture is used as the initial posture, and the interpolation optimization based on the minimum matching residual square is used to obtain the best matching posture; otherwise, taking the latest estimated posture as the center, the second search space is expanded to perform correlation matching again, and the second candidate posture with the highest correlation score in this round is selected as the initial posture, and the interpolation optimization based on the minimum matching residual square is used to obtain the best matching posture; According to the latest posture and laser scanning data of the drone, the local grid map list is updated, including: if the local grid map list is empty, or the height difference between the latest posture of the drone and the latest member of the local grid map list is less than the first threshold, or the number of posture points of the latest member of the local grid map list reaches the fourth threshold, a new local grid map is added to the local grid map list; sorting by joining time, confirming that the other members of the local grid map list except the last two members are set to the completed state, and no longer updating their grid occupancy probability; traversing the members in the local grid map list that are not set to the completed state, if the height difference between the latest posture of the drone and the member that is not set to the completed state is less than the first threshold, or the member that is not set to the completed state is the latest member, adding the latest posture point of the drone to the member that is not set to the completed state, and updating its grid occupancy probability; if the number of posture points of the second newest member of the local grid map list reaches twice the fourth threshold, or the height difference with the latest height coordinate of the drone is greater than or equal to the first threshold, setting the second newest member of the local grid map list to the completed state, and no longer updating its grid occupancy probability; According to the latest posture and laser scanning data of the UAV, local grid maps with a height difference less than a first threshold are screened and added to the global optimization, including: traversing the members in the local grid map list set to the completed state, selecting the local grid map with a height difference with the latest posture of the UAV less than the first threshold, and calculating the correlation score between the laser scanning data and the local grid map; if the correlation score exceeds the fifth threshold, the latest posture of the UAV and the global coordinates of the local grid map are used as nodes for graph optimization, the relative posture of the latest posture of the UAV and the local grid map are calculated as constraint edges for graph optimization, a loss function based on the minimum square difference is constructed for iterative calculation, and the latest posture of the UAV and the global coordinates of the local grid map are updated.

2. The method according to claim 1, characterized in that The method for determining the second search space includes: If the UAV adopts the "headless" flight mode, the search ranges in the horizontal X and Y directions of the first search space are expanded by equal proportional factors as the search ranges in the horizontal X and Y directions of the second search space, and the heading angle search range of the second search space is kept consistent with the heading angle search range of the first search space; If the UAV adopts the "headed" flight mode, the search range is expanded by the proportional factors in the horizontal X and Y directions of the first search space projected along the direction of the nose as the horizontal X and Y direction search range of the second search space, and it is determined whether the heading angle change between two consecutive laser scanning moments exceeds the third threshold. If so, the heading angle search range of the first search space is expanded according to the proportional factor as the heading angle search range of the second search space, otherwise, the heading angle search range of the second search space is kept consistent with the heading angle search range of the first search space.

Images