Obstacle position estimation method and apparatus

Abstract

This invention provides an obstacle location estimation method and apparatus, relating to the field of unmanned aerial vehicle (UAV) technology. The method includes: acquiring initial image data collected by an imaging device at at least two initial moments and their corresponding initial UAV position information; the initial image data display includes at least obstacles within the effective field of view of the imaging device; based on the initial image data, determining the skyline pitch plane line-of-sight angle corresponding to the effective field of view at the initial moment; constructing a geometric triangulation relationship based on the skyline pitch plane line-of-sight angle and the observation baseline; and determining the initial obstacle location information corresponding to the obstacle based on the geometric triangulation relationship. This invention achieves stable and reliable obstacle localization without requiring high-precision or high-cost sensors, significantly improving the system's environmental perception robustness under low-cost sensing configurations.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) technology, and in particular to an obstacle position estimation method and apparatus. Background Technology

[0002] With the rapid development of drone technology, obstacle avoidance capability has become one of the key requirements for safe drone flight. In complex and ever-changing flight environments, accurate obstacle distance estimation is crucial for achieving autonomous navigation, path planning, and real-time obstacle avoidance. Traditional obstacle estimation methods mainly include lidar-based ranging technology and binocular vision-based stereo matching methods. These methods achieve relatively accurate distance perception to a certain extent by actively emitting signals or calculating depth information using the parallax principle. However, lidar devices are typically expensive and bulky, making them unsuitable for miniaturized, low-cost consumer-grade drone platforms. While binocular vision systems do not require active emission, they demand high hardware calibration accuracy and are prone to matching failures in low-texture or drastically changing lighting conditions, leading to decreased system robustness. In addition, there are methods that use a monocular camera combined with the PnP (Perspective-n-Point) algorithm for pose estimation to indirectly infer obstacle distances. This method relies on feature points of known size or prior geometric information in the environment, making it difficult to operate stably in application scenarios lacking scale references or structural priors. Therefore, in the existing technology, the above-mentioned obstacle estimation schemes generally suffer from high cost, complex hardware structure, or strong dependence on usage conditions. Summary of the Invention

[0003] In view of this, the purpose of the present invention is to provide an obstacle position estimation method and apparatus that can achieve stable and reliable obstacle localization without the need for high-precision or high-cost sensors, and significantly improve the environmental perception robustness of the system under low-cost sensing configuration.

[0004] In a first aspect, the present invention provides an obstacle location estimation method, which is applied to a drone equipped with an imaging device, and the method includes:

[0005] Acquire initial image data and corresponding initial UAV position information collected by the imaging device at at least two initial moments. The content of the initial image data data includes at least obstacles within the effective field of view of the imaging device.

[0006] Based on the initial image data, determine the skyline pitch plane line of sight angle corresponding to the effective field of view at the initial moment;

[0007] A geometric trigonometric relationship is constructed based on the line-of-sight angle of the skyline pitch plane and the observation baseline; wherein, the observation baseline is constructed based on the difference between the initial UAV position information;

[0008] Based on geometric trigonometric relationships, determine the initial obstacle position information corresponding to the obstacle.

[0009] In one implementation, based on initial image data, the skyline pitch plane line-of-sight angle corresponding to the effective field of view at the initial moment is determined. An observation baseline is constructed using the difference between the initial UAV position information. A geometric triangulation relationship is then constructed by combining the skyline pitch plane line-of-sight angle and the observation baseline, including:

[0010] The effective field of view is divided into multiple sub-fields of view;

[0011] Based on the initial image data, the line-of-sight angles of the skyline pitch plane corresponding to multiple sub-field-of-view ranges at the initial moment are determined. The line-of-sight angle of the skyline pitch plane is: the angle formed in the vertical plane between the line-of-sight direction pointing to the target skyline point within the sub-field-of-sight range, with the location of the UAV as the observation starting point, and the horizontal movement direction of the UAV.

[0012] In one implementation, based on initial image data, determining the skyline pitch plane line-of-sight angles corresponding to multiple sub-field-of-view ranges at the initial moment includes:

[0013] The initial image data is segmented to obtain the initial skyline point set corresponding to the sub-view range. The initial skyline point set includes multiple initial skyline points.

[0014] The initial image data is subjected to attitude correction to obtain the corrected pixel direction vector corresponding to the initial skyline point, and the initial pitch plane line of sight angle corresponding to the initial skyline point is determined based on the corrected pixel direction vector.

[0015] Based on the initial elevation plane line of sight corresponding to the initial skyline point, determine the skyline elevation plane line of sight corresponding to the sub-field of view at the initial moment.

[0016] In one implementation, a geometric triangulation is constructed based on the line-of-sight angle of the skyline pitch plane and the observation baseline, including:

[0017] Determine the target sub-field of view along the horizontal movement direction of the UAV;

[0018] An observation baseline is constructed using the difference between the two initial UAV position information. A geometric triangulation relationship is then established by combining the line-of-sight angles of the skyline pitch plane corresponding to the target sub-field of view at the two initial moments.

[0019] In one implementation, determining the initial obstacle position information corresponding to the obstacle based on geometric triangulation includes:

[0020] In geometric trigonometric relationships, determine the location of the intersection point between the line-of-sight directions corresponding to the line-of-sight angles of two skyline elevation planes;

[0021] The location information of the intersection point is used as the initial obstacle location information corresponding to the obstacle.

[0022] In one implementation, the method further includes:

[0023] The initial obstacle location information is subjected to data degradation testing; the criteria for data degradation testing include one or more of the following: near parallel line of sight criterion, near vertical line of sight criterion, high noise criterion, and short observation baseline criterion;

[0024] If any criterion is met, it is determined that the initial obstacle location information failed the data degradation test;

[0025] Continue acquiring new image data and determining new initial obstacle location information until the new initial obstacle location information passes the data degradation test.

[0026] In one implementation, the method further includes:

[0027] Acquire the current image data collected by the imaging device at the current moment and its corresponding current UAV location information;

[0028] Based on the current image data, determine the skyline pitch plane line of sight angle corresponding to the effective field of view at the current moment;

[0029] Based on the initial obstacle location information, the current UAV location information, and the line-of-sight angle of the skyline pitch plane corresponding to the effective field of view at the current moment, the target obstacle location information at the current moment is estimated.

[0030] In one implementation, based on initial obstacle location information, current UAV location information, and the skyline pitch plane line-of-sight angle corresponding to the effective field of view at the current moment, the target obstacle location information at the current moment is estimated, including:

[0031] Obtain the input data at the current moment. The input data is either the initial obstacle position information or the target obstacle position information estimated at the previous moment.

[0032] Based on the input data and the corresponding horizontal forward displacement of the UAV, determine the initial state vector at the current moment, and determine the error covariance matrix at the current moment;

[0033] Based on the input data and the current drone position information at the current moment, the observation Jacobian matrix and equivalent measurement noise matrix corresponding to the current moment are determined respectively. The current drone position information includes drone altitude information and drone horizontal coordinate information.

[0034] Based on the error covariance matrix, observation Jacobian matrix, and equivalent measurement noise matrix at the current time, determine the Kalman gain matrix at the current time.

[0035] Based on the initial state vector, Kalman gain matrix, and the line-of-sight angle of the skyline pitch plane extracted within the target sub-field of view at the current moment, determine the target state vector at the current moment;

[0036] The target state vector is used as the estimated target obstacle position information at the current moment. The target obstacle position information includes obstacle depth information and obstacle height information.

[0037] In one implementation, the observation Jacobian matrix and equivalent measurement noise matrix corresponding to the current time are determined based on the input data and the current UAV position information at the current time, including:

[0038] Based on the obstacle height and depth information in the input data, as well as the drone's altitude information at the current moment, determine the predicted value of the pitch plane line of sight angle;

[0039] Based on the predicted line-of-sight angle in the pitch plane, the observation Jacobian matrix and equivalent measurement noise matrix corresponding to the current moment are determined respectively.

[0040] In one implementation, the target state vector at the current moment is determined based on the initial state vector, the Kalman gain matrix, and the line-of-sight angle of the skyline pitch plane extracted within the target sub-field of view at the current moment, including:

[0041] Determine the observation residual between the extracted skyline pitch plane line of sight angle within the target sub-field of view at the current moment and the predicted value of the pitch plane line of sight angle;

[0042] The observation residuals are weighted and corrected using the Kalman gain matrix corresponding to the current time.

[0043] The sum of the weighted and corrected observation residuals and the initial state vector at the current time is used as the target state vector at the current time.

[0044] Secondly, the present invention also provides an obstacle position estimation device, which is applied to a drone equipped with an imaging device, and the device includes:

[0045] The acquisition module is used to acquire the initial image data collected by the imaging device at at least two initial moments and the corresponding initial UAV position information. The content of the initial image data display includes at least obstacles within the effective field of view of the imaging device.

[0046] The line-of-sight angle determination module is used to determine the line-of-sight angle of the skyline pitch plane corresponding to the effective field of view at the initial moment, based on the initial image data.

[0047] The triangulation module is used to construct geometric triangulation relationships based on the line-of-sight angle of the skyline pitch plane and the observation baseline; wherein, the observation baseline is constructed using the difference between the initial UAV position information;

[0048] The position determination module is used to determine the initial obstacle position information corresponding to the obstacle based on geometric triangulation relationships.

[0049] This invention provides an obstacle position estimation method and apparatus applied to a drone. The drone is equipped with an imaging device. First, initial image data and corresponding initial drone position information are acquired by the imaging device at at least two initial moments. The initial image data displays at least the obstacles within the effective field of view of the imaging device. Then, based on the initial image data, the horizon pitch plane line-of-sight angle corresponding to the effective field of view at the initial moment is determined. A geometric triangulation is constructed based on the horizon pitch plane line-of-sight angle and the observation baseline. The observation baseline is constructed using the difference between the initial drone position information. Finally, the initial obstacle position information corresponding to the obstacle is determined based on the geometric triangulation. This method utilizes initial image data acquired by the imaging device at different initial moments, combines it with the corresponding drone position information to construct an observation baseline, and establishes a geometric triangulation based on the extracted horizon pitch plane line-of-sight angle. This effectively overcomes the depth blur problem in single-frame observation. Even under unfavorable conditions such as limited sensor accuracy, short baseline, or weak environmental texture, this invention maintains stable positioning performance, significantly reduces dependence on high-cost hardware, and improves the adaptability and practicality of obstacle estimation methods in complex scenarios.

[0050] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention are realized and obtained in accordance with the structures particularly pointed out in the description, claims and drawings.

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

[0052] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0053] Figure 1 A flowchart illustrating an obstacle location estimation method provided in an embodiment of the present invention;

[0054] Figure 2 This is a schematic diagram illustrating the deployment of a fisheye camera according to an embodiment of the present invention;

[0055] Figure 3 A schematic diagram illustrating the relative relationship between a drone and an obstacle, provided as an embodiment of the present invention;

[0056] Figure 4 This is a schematic diagram of the structure of an obstacle position estimation device provided in an embodiment of the present invention;

[0057] Figure 5 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present invention. Detailed Implementation

[0058] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0059] Currently, in existing technologies, the aforementioned obstacle estimation schemes generally suffer from high costs, complex hardware structures, or strong dependence on usage conditions. Based on this, the present invention provides an obstacle position estimation method and apparatus that can achieve stable and reliable obstacle localization without the need for high-precision or high-cost sensors, significantly improving the environmental perception robustness of the system under low-cost sensing configuration.

[0060] To facilitate understanding of this embodiment, a method for estimating obstacle position disclosed in this invention will first be described in detail. This method is applied to a drone equipped with an imaging device, which can be a regular camera or a wide-angle imaging device, preferably a wide-angle imaging device. A wide-angle imaging device refers to an optical imaging device with a field of view not less than a preset threshold. Further, the wide-angle imaging device can be a fisheye camera, which can capture a large area or even a near-hemispherical environmental image at once, covering the sky-horizontal area within the effective field of view of the drone. See [link to relevant documentation]. Figure 1 The diagram shows a flowchart of an obstacle location estimation method, which mainly includes the following steps S102 to S108:

[0061] Step S102: Obtain the initial image data and the corresponding initial UAV position information collected by the imaging device at at least two initial moments.

[0062] The initial moment refers to the time point used to perform obstacle position initialization. The interval is sufficient for the UAV to generate relative motion that can be used for triangulation, and initial image data containing obstacles and initial UAV position information can be obtained at each initial moment.

[0063] The initial image data displays at least the obstacles within the effective field of view of the wide-angle imaging device, which refers to the imaging area that the wide-angle imaging device can actually use to extract skyline features and obstacle information.

[0064] Initial UAV position information refers to the UAV's altitude and horizontal coordinates at the initial moment. The horizontal coordinates are the UAV's x-axis coordinates within a defined coordinate system. This system is defined by the vertical plane of the UAV's horizontal movement direction and vertical altitude; the x-axis represents the UAV's horizontal velocity vector (positive forward), and the y-axis represents the vertical direction (positive vertically upward). Initial obstacle position information is the estimated obstacle position based on the initial image data and the corresponding initial UAV position information. This includes obstacle height and depth information. Obstacle depth refers to the distance between the obstacle's x-axis coordinate and the UAV's x-axis coordinate.

[0065] Optionally, when the wide-angle imaging device is a fisheye camera, the image data is a fisheye image.

[0066] Step S104: Based on the initial image data, determine the skyline pitch plane line of sight angle corresponding to the effective field of view at the initial moment.

[0067] The effective field of view is divided into multiple sub-fields of view. The skyline pitch plane line of sight angle is extracted from the image data. The skyline pitch plane line of sight angle is the angle formed in the vertical plane between the line of sight direction pointing to the target skyline point within the sub-field of view, with the location of the UAV as the observation starting point, and the horizontal movement direction of the UAV.

[0068] Step S106: Construct a geometric triangulation relationship based on the line-of-sight angle of the horizon elevation plane and the observation baseline.

[0069] The observation baseline is constructed using the difference between the initial UAV position information. The geometric triangular relationship refers to the triangular structure established by constructing the observation baseline using the difference between the initial UAV position information and combining it with the line-of-sight angle of the skyline pitch plane extracted within the effective field of view at each initial moment.

[0070] In one implementation, the effective field of view is divided into multiple sub-fields of view, and a target sub-field of view that is consistent with the horizontal movement direction of the UAV is selected. A geometric triangulation relationship is constructed by combining the observation baseline and the line-of-sight angle of the skyline pitch plane of the corresponding target sub-field of view.

[0071] Step S108: Determine the initial obstacle position information based on geometric trigonometric relationships. In one example, the initial position of the obstacle is determined by the intersection of the line of sight direction and the line of sight angle of the horizon pitch plane, and is denoted as the initial obstacle position information.

[0072] The obstacle position estimation method provided in this invention utilizes image data collected by a wide-angle imaging device at different initial moments, combines it with the corresponding UAV position information to construct an observation baseline, and establishes a geometric triangulation relationship based on the extracted skyline pitch plane line-of-sight angle. This effectively overcomes the problem of depth blurring in single-frame observation. Even under unfavorable conditions such as limited sensor accuracy, short baseline, or weak environmental texture, this invention can still maintain stable positioning performance, significantly reducing dependence on high-cost hardware and improving the adaptability and practicality of obstacle estimation methods in complex scenarios.

[0073] In forward obstacle detection scenarios within vertical slices, the pitch angle plane line of sight corresponding to the obstacle can be extracted from fisheye camera images. Combined with the known translation of the UAV between two frames to construct an observation baseline, triangulation based on the angle-side-angle (ASA) geometric relationship can be achieved. However, when the lines of sight in two frames are nearly parallel or there is significant angle measurement noise, triangulation calculations are prone to numerical instability or depth estimation divergence. To address this issue, this invention proposes an integrated processing scheme that combines geometric analysis, robustness criteria, and dynamic smoothing using an extended Kalman filter algorithm. This effectively suppresses error amplification under adverse conditions, thereby achieving stable and reliable obstacle localization without the need for high-precision sensors.

[0074] (a) Input initial image data. See [link / reference] Figure 2 The diagram illustrates the deployment of a fisheye camera. For example, a fisheye camera can be deployed at the nose and / or tail of a drone. Figure 2 The direction of the arrows in the diagram is defined as the nose direction. Fisheye camera #1 is located at the nose, and fisheye camera #2 is located at the tail. The following coordinate system is defined: within the vertical slice plane containing the UAV's horizontal movement direction and vertical height, the x-axis represents the horizontal velocity vector (positive forward), and the y-axis represents the vertical height h (positive upward). Figure 2 Based on this, initial image data acquired by fisheye camera 1 and / or fisheye camera 2 can be obtained for subsequent obstacle estimation.

[0075] (ii) Based on the initial image data, determine the skyline pitch plane line-of-sight angle corresponding to the effective field of view at the initial moment. Construct an observation baseline using the difference between the initial UAV position information. Combine the skyline pitch plane line-of-sight angle and the observation baseline to construct a geometric trigonometric relationship, including:

[0076] (2.1) Divide the effective field of view into multiple sub-field of view and determine the line of sight angle of the skyline pitch plane corresponding to the multiple sub-field of view based on the initial image data. The line of sight angle of the skyline pitch plane is: the angle formed in the vertical plane between the line of sight direction pointing to the target skyline point within the sub-field of view, with the location of the UAV as the observation starting point, and the horizontal movement direction of the UAV.

[0077] For example, in an embodiment of the present invention, the effective field of view can be divided into 18 sectors, each sector being a sub-field of view, and each sector having an angle of 10°.

[0078] The image data is segmented for skyline and its pose corrected to obtain the skyline pitch plane line-of-sight angles corresponding to multiple sub-fields of view. This includes:

[0079] (2.11) Perform skyline segmentation on the image data to obtain the initial skyline point set corresponding to the sub-view range. The initial skyline point set includes multiple initial skyline points. The skyline is the boundary between the sky and the ground.

[0080] The skyline segmentation process is as follows: The skyline is the dividing line between the sky and the ground from the fisheye image perspective. Since sky features are easy to learn, the sky category is identified through an instance segmentation model, outputting a binary mask image (Sky_mask), where the pixel value of the sky region is 1, and the pixel value of the non-sky region is 0. For each column of pixels in the binary mask image (Sky_mask), the system scans from bottom to top, finds the first pixel with a value of 1, and records its coordinates (…). The lowest points of all columns form the initial set of skyline points.

[0081] (2.12) Perform attitude correction on the initial image data to obtain the corrected pixel direction vector corresponding to the initial skyline point, and determine the initial pitch plane line of sight angle corresponding to the initial skyline point based on the corrected pixel direction vector.

[0082] The attitude correction process is as follows: During the movement of the drone, shaking is inevitable, which will cause errors in the skyline detection. Therefore, the roll, pitch, and yaw angles provided by the flight controller / IMU (Inertial Measurement Unit) are used to calculate the gravity vector generated by gravity in the body coordinate system. The unit vector in the direction of gravity is ,calculate arrive rotation matrix Then we have: The rotation matrix is ​​used to correct all pixels of the fisheye camera. Let the direction vector of each pixel on the fisheye camera be... Then the direction vector of the pixel after correction is Then we have: .

[0083] Furthermore, the image data acquired by the fisheye camera can be reprojected, as shown below: equidistant approximation (i.e., angle of incidence) (Projection method where the distance r from the principal point on the image plane is linearly related to the projection method) The angle between the light ray and the optical axis in a fisheye lens (angle of incidence). Distance from principal point on the image plane , Focal length For any pixel coordinate, The main point coordinates are used; then, based on the azimuth angle around the optical axis... Obtain the direction of light :

[0084] ;

[0085] in, for and The coordinate difference between them.

[0086] After skyline detection and pose correction, the effective field of view (FOV) of the fisheye camera is obtained, which is also the effective horizontal FOV of the fisheye camera. The pose correction process yields the corrected orientation vector for each pixel. If the skyline observation point in each sub-field of view is denoted as Then you can obtain the view angle of the pitch plane corresponding to the skyline. Each fisheye camera divides the field of view (FOV) into 18 equal sectors: , For sector index, The starting angle of the effective field of view (FOV) of the current frame after alignment with the body coordinate system.

[0087] (2.13) Based on the initial pitch plane line of sight corresponding to the initial skyline point, determine the skyline pitch plane line of sight corresponding to the sub-field of view.

[0088] After the above steps, the initial pitch plane line-of-sight angles corresponding to multiple initial skyline points contained within multiple sub-fields of view can be obtained. For each sub-field of view, the largest initial pitch plane line-of-sight angle within it can be used as the representative angle of that sub-field of view, denoted as the skyline pitch plane line-of-sight angle. If multiple peaks exist in the initial pitch plane line-of-sight angle contained within a sub-field of view, after non-maximum suppression processing, the initial pitch plane line-of-sight angle with the largest amplitude among the retained peaks is selected as the representative angle of that sub-field of view, denoted as the skyline pitch plane line-of-sight angle. It should be noted that if no valid skyline is detected in a sub-field of view (i.e., the corresponding pixel value in Sky_mask is 0), then that sub-field of view is empty.

[0089] Furthermore, the first-order infinite impulse response (IIR) of the line-of-sight angle of the skyline pitch plane corresponding to multiple sub-fields of view can be filtered to reduce the flickering phenomenon of the line-of-sight angle of the skyline pitch plane.

[0090] Furthermore, a monotonic clock or flight control time is used to ensure that the skyline pitch plane line-of-sight angle, the UAV's horizontal coordinates, and the UAV's altitude are aligned at the same time, and the update step of the image data is controlled within 10ms. The release frequency of the skyline pitch plane line-of-sight angle is consistent with the camera frame rate (20 Hz). The extended Kalman filter algorithm internally performs prediction and correction operations based on the sensor's sampling rate.

[0091] (2.2) Determine the target sub-field of view where the UAV’s horizontal movement direction is located.

[0092] (2.3) Construct an observation baseline based on the difference between the two initial UAV position information, and construct a geometric trigonometric relationship by combining the line-of-sight angle of the horizon pitch plane corresponding to the target sub-field of view at the two initial moments.

[0093] To facilitate understanding, this embodiment of the invention provides a specific implementation of an obstacle position estimation method, where the input is the location information of a UAV. Drone speed FOV angle of obstacles in fisheye images The output is the location information of the target obstacle. Used to describe the x-coordinate of an obstacle in a predefined coordinate system. and ordinate (That is, the height of the obstacle to be determined), such as Figure 3 The diagram shown illustrates the relative relationship between a drone and an obstacle, with the initial moment denoted as... and +1, The essence of this invention is a sine theorem problem of solving triangle information given angles, sides and angles. Figure 3 The initial moment is shown. , +1 drone location information , Initial time , +1 The horizon feature points in the fisheye image below correspond to the horizon pitch plane line of view angle in the direction of the drone's horizontal velocity. , and the maximum FOV angle of the fisheye image. Location information , The distance between them is known, and the quantities to be determined include the depth of the obstacles. and obstacle height .

[0094] (iii) Determine the location information of the intersection point between the line of sight directions corresponding to the line of sight angles of the two horizon elevation planes in the geometric trigonometric relationship, and use the location information of the intersection point as the initial obstacle location information corresponding to the obstacle.

[0095] In this embodiment of the invention, the horizon pitch plane line-of-sight angle corresponding to the target sub-field of view in the horizontal movement direction of the UAV is collected in real time by a fisheye camera. The initial obstacle position information (including obstacle depth information and obstacle height information) is estimated by combining two frames of data, and the initial obstacle position information is used as the initial value of the extended Kalman filter algorithm.

[0096] The process of determining the initial obstacle location information is as follows:

[0097] The basic form of the Law of Sines (ASA) is as follows: For a triangle formed by two initial moments and the target point, we have: , ;in, It is the side length of any triangle. It is the edge The corresponding diagonal, It is the radius of the circumcircle.

[0098] Assume the unknown quantity is the X-axis coordinate of the obstacle in a defined coordinate system (denoted as ). ) and height (denoted as The known quantity is the drone at the initial moment. , +1 (e.g., frame 0, frame 1), the X-axis coordinate, altitude, and horizon pitch plane view angle in the agreed coordinate system, denoted as... ,and .

[0099] Slope of the straight line from frame 0 to the obstacle: ;

[0100] Slope of the straight line from frame 1 to the obstacle: ;

[0101] The location of the intersection point can be determined by solving the two line-of-sight equations simultaneously: +( ) ;

[0102] The simplified X-axis coordinates of the obstacle are: = ;

[0103] Finally, the initial obstacle height can be calculated: = +( ) ;

[0104] The initial obstacle depth is calculated based on the data from the first frame: .

[0105] (iv) Perform data degradation test on the initial obstacle location information; wherein, the criteria for data degradation test include one or more of the following: near parallel line of sight criterion, near vertical line of sight criterion, high noise criterion and short observation baseline criterion; if any criterion is met, it is determined that the initial obstacle location information has not passed the data degradation test, and new image data and new initial obstacle location information are acquired until the new initial obstacle location information passes the data degradation test.

[0106] The near-parallel line-of-sight criterion is that the absolute value of the difference between the line-of-sight angles of the skyline pitch plane at two initial moments is less than the threshold for parallelism determination. For example: if This will cause the denominator to approach 0, making it impossible to find a stable intersection point. The parallelism determination process is as follows: ,like If so, data will be re-collected. The threshold for parallel determination.

[0107] The near-vertical line-of-sight criterion is that the absolute value of the cosine of the line-of-sight angle on the horizon pitch plane at any initial moment is less than the threshold for vertical determination. For example: This will lead to a numerical explosion and trigger data re-collection.

[0108] The high noise criterion is that the noise at the line-of-sight angle of the skyline pitch plane at any initial moment is higher than the noise threshold, for example: This can cause depth errors on the order of cm to dm (centimeter to decimeter), which will trigger data re-acquisition.

[0109] The short observation baseline criterion is that the length of the observation baseline constructed from the difference between two initial UAV position information is less than the baseline length requirement, for example: , For baseline length requirements, the value is generally taken as follows: If the baseline is decisive, then data re-acquisition will be performed.

[0110] (v) Acquire the current image data collected by the imaging device at the current moment and the corresponding current UAV position information; based on the current image data, determine the skyline pitch plane line of sight angle corresponding to the effective field of view at the current moment; based on the initial obstacle position information, the current UAV position information, and the skyline pitch plane line of sight angle corresponding to the effective field of view at the current moment, estimate the target obstacle position information at the current moment.

[0111] The current drone location information includes the drone's altitude and horizontal coordinates at the current moment, while the target obstacle location information includes the obstacle's height and depth.

[0112] In one implementation, during the first recursive estimation process, the target obstacle position information at the current moment is estimated based on the initial obstacle position information, the current UAV position information, and the line-of-sight angle of the skyline pitch plane corresponding to the effective field of view at the current moment; during other recursive estimation processes, the target obstacle position information at the current moment is estimated based on the target obstacle position information estimated at the previous moment, the current UAV position information at the current moment, and the line-of-sight angle of the skyline pitch plane extracted within the effective field of view at the current moment.

[0113] Optionally, a nonlinear state estimation algorithm can be used for recursive estimation, including but not limited to the Extended Kalman Filter (EKF) algorithm, the Unscented Kalman Filter (UKF) algorithm, or the Unscented Transform (PF) algorithm. In this embodiment of the invention, the Kalman Filter algorithm is preferred for recursive estimation of the target obstacle position information.

[0114] This invention provides a specific implementation method for estimating the location of a target obstacle, including:

[0115] (4.1) Define the state vector: ;

[0116] (4.2) Process Model:

[0117] Control is the horizontal forward displacement of the UAV within a vertical slice. : , ;in, , These are the x-axis velocity and acceleration in the agreed coordinate system, respectively.

[0118] Under the assumption that the target is stationary: , ;in, , For the first The height of the obstacle at any given moment. , For the first The obstacle depth at any given moment , These are height process noise and depth process noise, respectively.

[0119] It can be deduced that: , ;in, , For the first The state vector at time t. State transition matrix , To control the input gain matrix , Modeling process noise, This is the process noise matrix, including height observation noise. and depth observation noise .

[0120] (4.3) Observation model and observation Jacobian matrix:

[0121] Using quadrant-bound atan2 as the observation function, the predicted pitch plane line-of-sight angle value obtained by the extended Kalman filter algorithm based on the process model is obtained. :

[0122] ;

[0123] in, For the current moment Obstacle height information, For the current moment The drone's altitude information, For the current moment Obstacle depth information.

[0124] right Observation Jacobian matrix for: ;

[0125] The extended Kalman filter algorithm is used to utilize the line-of-sight angle of the skyline pitch plane. Predicted line-of-sight angle with pitch plane The difference between them is used to perform the optimal estimation of the state using the extended Kalman filter algorithm.

[0126] (4.4) Equivalent Measurement Noise Matrix :

[0127] ;

[0128] ;

[0129] This is used to handle the impact of the drone's own altitude noise on the angle measurement, among which For the observation noise of the line-of-sight angle in the pitch plane, The measured noise is obtained by calculating the noise from the height observation noise transmission and the observation noise from the pitch plane line-of-sight angle. .

[0130] Based on this, the current position information of the target obstacle can be estimated by following these steps:

[0131] A) Obtain the input data at the current moment. The input data is the initial obstacle position information or the target obstacle position information estimated at the previous moment.

[0132] B) Based on the input data and the corresponding horizontal forward displacement of the UAV, determine the initial state vector at the current moment, and determine the error covariance matrix at the current moment:

[0133] ;

[0134] in, For the current moment The initial state vector, Here is the state transition matrix. For the previous moment The target state vector, To control the input gain matrix, For the current moment The horizontal forward displacement corresponding to the unmanned aerial vehicle. For the current moment The initial error covariance matrix, For the previous moment The target error covariance matrix, This is the process noise matrix.

[0135] C) Based on the input data and the current UAV position information at the current moment, determine the observation Jacobian matrix and equivalent measurement noise matrix corresponding to the current moment. This includes: determining the predicted pitch plane line-of-sight angle based on the obstacle height and depth information in the input data, and the UAV altitude information at the current moment; and determining the observation Jacobian matrix and equivalent measurement noise matrix corresponding to the current moment based on the predicted pitch plane line-of-sight angle.

[0136] Specifically, the observation Jacobian matrix corresponding to the current time can be determined according to the aforementioned (4.3) and (4.4). and equivalent measurement noise matrix The embodiments of the present invention will not be described in detail here.

[0137] D) Determine the Kalman gain matrix at the current time based on the error covariance matrix, observation Jacobian matrix, and equivalent measurement noise matrix at the current time: , For the current moment Kalman gain.

[0138] E) Determine the target state vector at the current moment based on the initial state vector, the Kalman gain matrix, and the line-of-sight angle of the skyline pitch plane extracted within the target sub-field of view at the current moment. This includes: determining the observation residual between the line-of-sight angle of the skyline pitch plane extracted within the target sub-field of view at the current moment and the predicted value of the pitch plane line-of-sight angle; applying a weighted correction to the observation residual using the Kalman gain matrix at the current moment; and using the sum of the weighted correction of the observation residual and the initial state vector at the current moment as the target state vector at the current moment.

[0139] Specifically, the target state vector can be determined using the following formula, and the error covariance matrix at the current time step can be updated:

[0140] ;

[0141] in, For the current moment The target state vector, For the current moment The horizon tilt plane line of sight, For the current moment The target error covariance matrix, It is an identity matrix.

[0142] F) The target state vector is used as the estimated target obstacle position information at the current moment. The target obstacle position information includes obstacle depth information and obstacle height information.

[0143] In summary, the embodiments of the present invention have at least the following characteristics:

[0144] (1) Low cost and high versatility: The embodiments of the present invention only require camera angle observation and self-motion (two frames of position / velocity), without the need for lidar, which significantly reduces hardware costs. It is applicable to a variety of camera types, including fisheye cameras, and can be quickly integrated into existing flight control and vision pipelines.

[0145] (2) Analytical solution and filtering double fidelity: The embodiments of the present invention provide a geometric analytical method, which gives a closed solution to ensure the accuracy and efficiency of the calculation. It also uses an extended Kalman filter for noise suppression, stabilizes the output, and effectively suppresses the depth jitter caused by noise.

[0146] (3) Complete numerical stabilization and degradation processing: The embodiments of the present invention include branch processing for near parallel or near perpendicular line of sight, ensuring the stability of the system under extreme conditions, while avoiding numerical instability problems and reducing bursting phenomena.

[0147] (4) Easy to implement and simple to deploy: The interface of the present invention is clear and the input / output is well defined, which makes it easy to quickly integrate into the existing system. It does not require additional hardware support, is compatible with the existing flight control interface, and simplifies the deployment process.

[0148] (5) Explainability and tunability: The condition number, threshold and noise parameters in the embodiments of the present invention are clear and controllable, which facilitates engineers to perform optimization and verification, and makes it easy to optimize and verify performance in practical applications.

[0149] Based on the foregoing embodiments, this invention provides an obstacle location estimation device, which is applied to a drone equipped with an imaging device. (See also...) Figure 4 The diagram shows a structural schematic of an obstacle location estimation device, which mainly includes the following parts:

[0150] The acquisition module 402 is used to acquire the initial image data collected by the imaging device at at least two initial moments and the corresponding initial UAV position information. The content of the initial image data display includes at least obstacles within the effective field of view of the imaging device.

[0151] The line-of-sight angle determination module 404 is used to determine the line-of-sight angle of the skyline pitch plane corresponding to the effective field of view at the initial moment based on the initial image data.

[0152] Triangulation module 406 is used to construct geometric triangulation relationships based on the line-of-sight angle of the skyline pitch plane and the observation baseline; wherein, the observation baseline is constructed using the difference between the initial UAV position information;

[0153] The position determination module 408 is used to determine the initial obstacle position information corresponding to the obstacle based on geometric triangulation.

[0154] The obstacle position estimation device provided in this invention utilizes image data collected by the imaging device at different initial moments, combines it with the corresponding UAV position information to construct an observation baseline, and establishes a geometric triangulation relationship based on the extracted skyline pitch plane line-of-sight angle. This effectively overcomes the problem of depth blurring in single-frame observation. Even under unfavorable conditions such as limited sensor accuracy, short baseline, or weak environmental texture, this invention can still maintain stable positioning performance, significantly reducing dependence on high-cost hardware and improving the adaptability and practicality of obstacle estimation methods in complex scenarios.

[0155] In one embodiment, the line-of-sight angle determination module 404 is specifically used for:

[0156] The effective field of view is divided into multiple sub-fields of view;

[0157] Based on the initial image data, the line-of-sight angles of the skyline pitch plane corresponding to multiple sub-field-of-view ranges at the initial moment are determined. The line-of-sight angle of the skyline pitch plane is: the angle formed in the vertical plane between the line-of-sight direction pointing to the target skyline point within the sub-field-of-sight range, with the location of the UAV as the observation starting point, and the horizontal movement direction of the UAV.

[0158] In one embodiment, the line-of-sight angle determination module 404 is specifically used for:

[0159] The initial image data is segmented to obtain the initial skyline point set corresponding to the sub-view range. The initial skyline point set includes multiple initial skyline points.

[0160] The initial image data is subjected to attitude correction to obtain the corrected pixel direction vector corresponding to the initial skyline point, and the initial pitch plane line of sight angle corresponding to the initial skyline point is determined based on the corrected pixel direction vector.

[0161] Based on the initial elevation plane line of sight corresponding to the initial skyline point, determine the skyline elevation plane line of sight corresponding to the sub-field of view at the initial moment.

[0162] In one implementation, the triangular building block 406 is specifically used for:

[0163] Determine the target sub-field of view along the horizontal movement direction of the UAV;

[0164] An observation baseline is constructed using the difference between the two initial UAV position information. A geometric triangulation relationship is then established by combining the line-of-sight angles of the skyline pitch plane corresponding to the target sub-field of view at the two initial moments.

[0165] In one implementation, the position determination module 408 is specifically used for:

[0166] In geometric trigonometric relationships, determine the location of the intersection point between the line-of-sight directions corresponding to the line-of-sight angles of two skyline elevation planes;

[0167] The location information of the intersection point is used as the initial obstacle location information corresponding to the obstacle.

[0168] In one implementation, a verification module is further included, for:

[0169] The initial obstacle location information is subjected to data degradation testing; the criteria for data degradation testing include one or more of the following: near parallel line of sight criterion, near vertical line of sight criterion, high noise criterion, and short observation baseline criterion;

[0170] If any criterion is met, it is determined that the initial obstacle location information failed the data degradation test;

[0171] Continue acquiring new image data and determining new initial obstacle location information until the new initial obstacle location information passes the data degradation test.

[0172] In one implementation, a real-time estimation module is further included, for:

[0173] Acquire the current image data collected by the imaging device at the current moment and its corresponding current UAV location information;

[0174] Based on the current image data, determine the skyline pitch plane line of sight angle corresponding to the effective field of view at the current moment;

[0175] Based on the initial obstacle location information, the current UAV location information, and the line-of-sight angle of the skyline pitch plane corresponding to the effective field of view at the current moment, the target obstacle location information at the current moment is estimated.

[0176] In one implementation, the real-time estimation module is specifically used for:

[0177] Obtain the input data at the current moment. The input data is either the initial obstacle position information or the target obstacle position information estimated at the previous moment.

[0178] Based on the input data and the corresponding horizontal forward displacement of the UAV, determine the initial state vector at the current moment, and determine the error covariance matrix at the current moment;

[0179] Based on the input data and the current drone position information at the current moment, the observation Jacobian matrix and equivalent measurement noise matrix corresponding to the current moment are determined respectively. The current drone position information includes drone altitude information and drone horizontal coordinate information.

[0180] Based on the error covariance matrix, observation Jacobian matrix, and equivalent measurement noise matrix at the current time, determine the Kalman gain matrix at the current time.

[0181] Based on the initial state vector, Kalman gain matrix, and the line-of-sight angle of the skyline pitch plane extracted within the target sub-field of view at the current moment, determine the target state vector at the current moment;

[0182] The target state vector is used as the estimated target obstacle position information at the current moment. The target obstacle position information includes obstacle depth information and obstacle height information.

[0183] In one implementation, the real-time estimation module is specifically used for:

[0184] Based on the obstacle height and depth information in the input data, as well as the drone's altitude information at the current moment, determine the predicted value of the pitch plane line of sight angle;

[0185] Based on the predicted line-of-sight angle in the pitch plane, the observation Jacobian matrix and equivalent measurement noise matrix corresponding to the current moment are determined respectively.

[0186] In one implementation, the real-time estimation module is specifically used for:

[0187] Determine the observation residual between the extracted skyline pitch plane line of sight angle within the target sub-field of view at the current moment and the predicted value of the pitch plane line of sight angle;

[0188] The observation residuals are weighted and corrected using the Kalman gain matrix corresponding to the current time.

[0189] The sum of the weighted and corrected observation residuals and the initial state vector at the current time is used as the target state vector at the current time.

[0190] The device provided in this embodiment of the invention has the same implementation principle and technical effect as the aforementioned method embodiment. For the sake of brevity, any parts not mentioned in the device embodiment can be referred to the corresponding content in the aforementioned method embodiment.

[0191] This invention provides an electronic device, specifically, the electronic device includes a processor and a memory; the memory stores a computer program, which, when run by the processor, executes the method described in any of the above embodiments.

[0192] Figure 5The present invention provides a schematic diagram of the structure of an electronic device 100, which includes a processor 50, a memory 51, a bus 52 and a communication interface 53. The processor 50, the communication interface 53 and the memory 51 are connected through the bus 52. The processor 50 is used to execute executable modules, such as computer programs, stored in the memory 51.

[0193] The memory 51 may include high-speed random access memory (RAM) or non-volatile memory, such as at least one disk storage device. Communication between this system network element and at least one other network element is achieved through at least one communication interface 53 (which can be wired or wireless), such as the Internet, wide area network, local area network, metropolitan area network, etc.

[0194] Bus 52 can be an ISA bus, PCI bus, or EISA bus, etc. The bus can be divided into address bus, data bus, control bus, etc. For ease of representation, Figure 5 The symbol is represented by a single double-headed arrow, but this does not mean that there is only one bus or one type of bus.

[0195] The memory 51 is used to store programs. After receiving an execution instruction, the processor 50 executes the programs. The method executed by the device for defining the flow process disclosed in any of the foregoing embodiments of the present invention can be applied to the processor 50 or implemented by the processor 50.

[0196] Processor 50 may be an integrated circuit chip with signal processing capabilities. In implementation, each step of the above method can be completed by the integrated logic circuitry in the hardware of processor 50 or by instructions in software form. Processor 50 can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc.; it can also be a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this invention. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the methods disclosed in the embodiments of this invention can be directly implemented by a hardware decoding processor, or implemented by a combination of hardware and software modules in the decoding processor. The software modules can reside in random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, or other mature storage media in the art. The storage medium is located in memory 51. The processor 50 reads the information in memory 51 and, in conjunction with its hardware, completes the steps of the above method.

[0197] The computer program product of the readable storage medium provided in the embodiments of the present invention includes a computer-readable storage medium storing program code. The instructions included in the program code can be used to execute the methods described in the foregoing method embodiments. For specific implementation, please refer to the foregoing method embodiments, which will not be repeated here.

[0198] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0199] Finally, it should be noted that the above-described embodiments are merely specific implementations of the present invention, used to illustrate the technical solutions of the present invention, and not to limit it. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments within the technical scope disclosed in the present invention, or make equivalent substitutions for some of the technical features; and these modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for estimating the location of an obstacle, characterized in that, The method is applied to a drone equipped with an imaging device, and the method includes: Acquire initial image data and corresponding initial UAV position information collected by the imaging device at at least two initial moments, wherein the content of the initial image data data includes at least obstacles within the effective field of view of the imaging device; Based on the initial image data, the skyline pitch plane line of sight angle corresponding to the effective field of view at the initial moment is determined; wherein, the effective field of view is divided into multiple sub-field of view, and the skyline pitch plane line of sight angle is: the angle formed in the vertical plane by the line of sight direction pointing to the target skyline point within the sub-field of view, with the location of the UAV as the observation starting point, and the horizontal movement direction of the UAV. A geometric trigonometric relationship is constructed based on the line-of-sight angle of the skyline pitch plane and the observation baseline; wherein the observation baseline is constructed based on the difference between the initial UAV position information. Based on the geometric triangulation relationship, the initial obstacle position information corresponding to the obstacle is determined.

2. The obstacle location estimation method according to claim 1, characterized in that, Based on the initial image data, determining the skyline pitch plane line of sight angle corresponding to the effective field of view at the initial moment includes: The effective field of view is divided into multiple sub-field of view; Based on the initial image data, determine the skyline pitch plane line-of-sight angles corresponding to the multiple sub-field-of-sight ranges at the initial moment.

3. The obstacle location estimation method according to claim 2, characterized in that, Based on the initial image data, determine the skyline pitch plane line-of-sight angles corresponding to the multiple sub-field-of-view ranges at the initial moment, including: The initial image data is segmented to obtain an initial skyline point set corresponding to the sub-field of view, and the initial skyline point set includes multiple initial skyline points. The initial image data is subjected to attitude correction to obtain the corrected pixel direction vector corresponding to the initial skyline point, and the initial pitch plane line of sight angle corresponding to the initial skyline point is determined based on the corrected pixel direction vector. Based on the initial pitch plane line of sight corresponding to the initial skyline point, determine the skyline pitch plane line of sight corresponding to the sub-field of view at the initial moment.

4. The obstacle location estimation method according to claim 2, characterized in that, Based on the line-of-sight angle of the skyline pitch plane and the observation baseline, a geometric trigonometric relationship is constructed, including: Determine the target sub-field of view where the horizontal movement direction of the UAV is located; An observation baseline is constructed using the difference between the two initial UAV position information, and a geometric triangulation relationship is constructed by combining the line-of-sight angle of the skyline pitch plane corresponding to the target sub-field of view at the two initial times.

5. The obstacle location estimation method according to claim 2, characterized in that, Based on the geometric triangulation relationship, the initial obstacle position information corresponding to the obstacle is determined, including: Determine the location information of the intersection point between the line-of-sight directions corresponding to the two line-of-sight angles of the two skyline pitch planes in the geometric trigonometric relationship; The intersection point location information is used as the initial obstacle location information corresponding to the obstacle.

6. The obstacle location estimation method according to claim 1, characterized in that, The method further includes: The initial obstacle location information is subjected to data degradation testing; wherein, the criteria for the data degradation testing include one or more of the following: near parallel line of sight criterion, near vertical line of sight criterion, high noise criterion, and short observation baseline criterion; If any of the aforementioned criteria are met, it is determined that the initial obstacle location information has failed the data degradation test; Continue acquiring new image data and determining new initial obstacle location information until the new initial obstacle location information passes the data degradation test.

7. The obstacle location estimation method according to claim 1, characterized in that, The method further includes: Obtain the current image data collected by the imaging device at the current moment and its corresponding current UAV location information; Based on the current image data, determine the skyline pitch plane line of sight angle corresponding to the effective field of view at the current moment; Based on the initial obstacle location information, the current UAV location information, and the line-of-sight angle of the skyline pitch plane corresponding to the effective field of view at the current moment, the target obstacle location information at the current moment is estimated.

8. The obstacle location estimation method according to claim 7, characterized in that, Based on the initial obstacle location information, the current UAV location information, and the skyline pitch plane line-of-sight angle corresponding to the effective field of view at the current moment, the target obstacle location information at the current moment is estimated, including: Obtain the input data at the current moment, wherein the input data is the initial obstacle position information or the target obstacle position information estimated at the previous moment; Based on the input data and the horizontal forward displacement of the UAV, determine the initial state vector corresponding to the current moment, and determine the error covariance matrix corresponding to the current moment; Based on the input data and the current drone position information at the current time, the observation Jacobian matrix and the equivalent measurement noise matrix corresponding to the current time are determined respectively. The current drone position information includes drone altitude information and drone horizontal coordinate information. The Kalman gain matrix corresponding to the current time is determined based on the error covariance matrix, the observation Jacobian matrix, and the equivalent measurement noise matrix corresponding to the current time. The target state vector corresponding to the current moment is determined based on the initial state vector, the Kalman gain matrix, and the line-of-sight angle of the skyline pitch plane extracted within the target sub-field of view at the current moment. The target state vector is used as the target obstacle position information estimated at the current time. The target obstacle position information includes obstacle depth information and obstacle height information.

9. The obstacle location estimation method according to claim 8, characterized in that, Based on the input data and the current UAV position information at the current time, the observation Jacobian matrix and equivalent measurement noise matrix corresponding to the current time are determined, including: Based on the obstacle height information, obstacle depth information, and UAV height information at the current moment in the input data, determine the predicted value of the pitch plane line of sight angle; Based on the predicted pitch plane line-of-sight angle, the observation Jacobian matrix and equivalent measurement noise matrix corresponding to the current moment are determined respectively.

10. The obstacle location estimation method according to claim 9, characterized in that, Based on the initial state vector corresponding to the current moment, the Kalman gain matrix, and the skyline pitch plane line-of-sight angle extracted within the target sub-field of view at the current moment, the target state vector corresponding to the current moment is determined, including: Determine the observation residual between the skyline pitch plane line of sight angle extracted within the target sub-field of view at the current moment and the predicted value of the pitch plane line of sight angle; The observation residuals are weighted and corrected using the Kalman gain matrix corresponding to the current time. The sum of the weighted and corrected observation residual and the initial state vector corresponding to the current time is taken as the target state vector corresponding to the current time.

11. An obstacle position estimation device, characterized in that, The device is applied to a drone, the drone being equipped with an imaging device, and the device includes: The acquisition module is used to acquire initial image data and corresponding initial UAV position information collected by the imaging device at at least two initial moments. The content of the initial image data data includes at least obstacles within the effective field of view of the imaging device. The line-of-sight angle determination module is used to determine the skyline pitch plane line-of-sight angle corresponding to the effective field of view at the initial moment based on the initial image data; wherein, the effective field of view is divided into multiple sub-field of view, and the skyline pitch plane line-of-sight angle is: the angle formed in the vertical plane by the line-of-sight direction pointing to the target skyline point within the sub-field of view, with the location of the UAV as the observation starting point, and the horizontal movement direction of the UAV. A triangulation module is used to construct geometric triangulation relationships based on the line-of-sight angle of the skyline pitch plane and the observation baseline; wherein the observation baseline is constructed using the difference between the initial UAV position information; The position determination module is used to determine the initial obstacle position information corresponding to the obstacle based on the geometric triangulation relationship.

Images