Vision-based autonomous navigation method and device for quadrotor UAVs

Abstract

This invention provides a vision-based autonomous navigation method and device for quadcopter drones, relating to the field of quadcopter drone technology. The vision-based autonomous navigation method for quadcopter drones includes acquiring visual odometry information and image frame sequences from the quadcopter drone; the image frame sequence includes multiple pairs of RGB image-depth images; determining the presence of a pre-defined circular obstacle based on the RGB and depth images; if a pre-defined circular obstacle exists, determining whether the obstacle is stationary; and employing different trajectory generation strategies depending on whether the obstacle is stationary. The vision-based autonomous navigation method for quadcopter drones provided by the embodiments of this invention accurately locates itself and perceives its surrounding environment based on visual information, while employing corresponding flight trajectory generation strategies for different flight tasks, thereby solving the problems brought about by complex multi-tasking scenarios and making it possible for quadcopter drones to replace manual operations.

Description

Technical Field

[0001] This invention relates to the field of quadcopter drone technology, and more specifically, to a vision-based autonomous navigation method and drone device for quadcopter drones. Background Technology

[0002] With the rapid and high-quality development of drone technology, it has found excellent applications in various industries. However, considering that in some complex scenarios, such as disaster relief and high-risk operations, the terrain is complex, there are many obstacles, dense obstructions, and significant interference, making it unsuitable for large reconnaissance equipment to enter, and manual reconnaissance is inconvenient and even dangerous, the use of quadcopter drones to replace manual labor provides a new solution for these scenarios. However, this inevitably places higher demands on the autonomous navigation performance of quadcopter drones in complex scenarios, namely, the ability of quadcopter drones to achieve stable and effective autonomous navigation for multiple consecutive different flight missions in complex scenarios.

[0003] For quadcopter drones in flight, autonomous navigation primarily consists of environmental perception and intelligent decision-making. The Global Positioning System (GPS) malfunctions in environments with complex terrain, numerous obstacles, and dense obstructions. Furthermore, significant interference in complex multi-tasking scenarios makes it difficult for quadcopter drones to accurately perceive static and dynamic circular obstacles in their surroundings. Therefore, self-positioning and environmental perception are extremely challenging for quadcopter drones in complex multi-tasking scenarios. On the other hand, most existing intelligent decision-making algorithms for quadcopter drones only include a single flight trajectory generation strategy, while complex multi-tasking scenarios involve various types of obstacles. To achieve a smooth and efficient flight process, appropriate flight trajectory generation strategies must be selected for different flight missions, which also presents new challenges for quadcopter drones. Summary of the Invention

[0004] The following provides a brief overview of one or more aspects to offer a basic understanding of them. This overview is not an exhaustive summary of all conceived aspects, nor is it intended to identify key or decisive elements of all aspects, nor to define the scope of any or all aspects. Its sole purpose is to present some concepts of one or more aspects in a simplified form to prepare for the more detailed descriptions that follow.

[0005] The present invention aims to provide a vision-based autonomous navigation method for quadcopter unmanned aerial vehicles (UAVs) that can solve problems arising in complex multi-tasking scenarios.

[0006] The present invention also aims to provide an unmanned aerial vehicle (UAV) device that can adapt to flight requirements in complex multi-tasking scenarios.

[0007] The embodiments of the present invention can be implemented in the following ways:

[0008] A vision-based autonomous navigation method for a quadcopter unmanned aerial vehicle (UAV), comprising:

[0009] The visual odometry information and image frame sequence of a quadcopter drone are acquired; wherein the image frame sequence includes multiple pairs of RGB image-depth image;

[0010] Determine whether a pre-defined circular obstacle exists based on the RGB and depth images;

[0011] If a ring-shaped obstacle of the preset shape exists, determine whether the ring-shaped obstacle is stationary;

[0012] If the ring-shaped obstacle is stationary, the flight yaw angle and flight altitude of the quadcopter UAV are adjusted using an image-based visual servo controller. After adjustment, the flight yaw angle and flight altitude of the quadcopter UAV remain unchanged, and the quadcopter UAV is controlled to fly forward at a constant speed in the body coordinate system to pass through the ring-shaped obstacle.

[0013] If the ring-shaped obstacle is not stationary, then in the body coordinate system, the motion trajectory of the center of the ring-shaped obstacle is predicted by fitting using the nonlinear least squares method. The flight attitude angle and flight altitude of the quadrotor UAV in the local coordinate system are adjusted using a discrete LQR controller. After adjustment, the current flight yaw angle and flight altitude of the quadrotor UAV remain unchanged, and the quadrotor UAV is controlled to fly forward at a constant speed in the body coordinate system to pass through the ring-shaped obstacle.

[0014] Optionally, the step of acquiring the visual odometry information and image frame sequence of the quadcopter drone includes:

[0015] The position information of the quadcopter UAV in the local coordinate system with the takeoff point as the origin was obtained using a visual positioning camera. bx ,p by ,p bz ] T and quaternion attitude information [q bx ,q by ,q bz ,q bw ] T ;

[0016] An RGB image and depth information are acquired using a depth camera, and a depth image is calculated based on the RGB image and the depth information.

[0017] Optionally, the step of determining whether a ring-shaped obstacle of a preset shape exists based on the RGB image and the depth image includes:

[0018] The RGB image is preprocessed to obtain a preprocessed image;

[0019] The preprocessing includes: determining whether the depth value from the real three-dimensional space point corresponding to each pixel in the RGB image to the quadcopter drone is greater than the effective distance threshold based on the depth image; if so, the pixel is masked; if not, it remains unchanged.

[0020] The preprocessed image is mapped from the RGB color space to the HSV color gamut to obtain HSV color gamut features;

[0021] The presence of a ring-shaped obstacle of a preset shape is determined based on the HSV color gamut characteristics.

[0022] Optionally, the step of determining whether a ring-shaped obstacle of a preset shape exists based on the HSV color gamut characteristics includes:

[0023] Extract candidate contours of the ring-shaped obstacle color gamut features that conform to the preset shape from the HSV color gamut features;

[0024] The number of ring-shaped obstacles with a preset shape in the candidate contour is detected based on morphological features;

[0025] Determine whether there is a ring-shaped obstacle with a preset shape in the area in front of the current quadcopter drone; if so, select the one with the largest area as the ring-shaped obstacle to be traversed.

[0026] Optionally, the step of determining whether the ring-shaped obstacle is stationary includes:

[0027] Calculate the three-dimensional coordinates of the center of the ring-shaped obstacle in the local coordinate system at at least two different times;

[0028] Based on the change in the three-dimensional coordinates in the local coordinate system and the judgment threshold, it is determined whether the ring-shaped obstacle is stationary.

[0029] Optionally, the step of calculating the three-dimensional coordinates of the center of the ring-shaped obstacle in the local coordinate system includes:

[0030] Calculate the three-dimensional coordinates [X] of the center in the camera coordinate system. c ,Y c Z c ] T The calculation formula is:

[0031]

[0032] Among them, f x ,f y ,c x ,c y These are the camera's intrinsic parameters; [ucen ,v cen ] T The pixel coordinates of the center in the RGB image; The depth value corresponding to the pixel coordinates.

[0033] Based on the three-dimensional coordinates [X] in the camera coordinate system c ,Y c Z c ] T Calculate the three-dimensional coordinates [X] of the center in the local coordinate system. b ,Y b Z b ] T :

[0034]

[0035] Among them, [p bx ,p by ,p bz ] T The position information of the quadcopter UAV in the local coordinate system; [q bx ,q by ,q bz ,q bw ] T The quaternion attitude information of the quadrotor UAV in the local coordinate system.

[0036] Optionally, the steps of adjusting the flight yaw angle and flight altitude of the quadcopter UAV using an image-based visual servo controller include:

[0037] Adjust the flight yaw angle and flight altitude of the quadcopter drone so that the center of the ring-shaped obstacle to be traversed is located at the center of the RGB image;

[0038] The adjustment process is achieved through the following formula:

[0039]

[0040]

[0041]

[0042]

[0043]

[0044]

[0045]

[0046] in, The pixel coordinates of the center of the ring-shaped obstacle during the i-th program execution cycle. These represent the errors relative to the center of the RGB image; α is a constant velocity; K zp ,K zi ,K zd K represents the coefficients of the PID algorithm. yawp ,K yawd These are the coefficients of the PD algorithm; These are the desired velocity components in the three directions and yaw angle of the body coordinate system during the i-th program execution cycle; Indicates the first i The cumulative total.

[0047] During the adjustment process, the quadcopter drone is controlled to fly forward at a constant speed.

[0048] Optionally, the steps of adjusting the flight attitude angle and flight altitude of the quadrotor UAV in the local coordinate system by fitting and predicting the motion trajectory of the center of the circle using the nonlinear least squares method and employing a discrete LQR controller include:

[0049] The quadcopter drone is controlled to hover, and the current time is taken as time n and the three-dimensional coordinates of the center of the ring obstacle in the body coordinate system are continuously recorded at a fixed frequency until time (n+m) ends.

[0050] The recorded data is substituted into a nonlinear least multiplication formula to fit the motion trajectory of the center in the body coordinate system;

[0051] Predict the three-dimensional coordinates of the center at time (n+h) and transform them to the local coordinate system as the desired target point for the quadcopter UAV to fly in the local coordinate system [p tarx ,p tary ,p tarz ] T ;

[0052] Generate a set of discrete sequences of desired flight trajectories:

[0053]

[0054] At time (n+l), the discrete LQR controller is used to control the quadcopter UAV to adjust its flight attitude angle and flight altitude in the local coordinate system, and the quadcopter UAV reaches the desired target point at time (n+h).

[0055] Where m < l < h, DT = hl, and dt represents the time interval between two adjacent desired flight trajectory points.

[0056] Optionally, the flight attitude angle and flight altitude in the local coordinate system can be adjusted using the following formula:

[0057]

[0058]

[0059] Where K is the feedback matrix calculated by the discrete LQR controller; φ j ,θ j ,ψ j , These represent the j-th roll, pitch, yaw, and altitude control commands, respectively; ψ0 is the yaw angle in hovering mode. Let j be the position on the x-axis and y-axis in the local coordinate system at time j. and Let $j$ be the velocities along the lower x-axis and y-axis of the local coordinate system at time $j$.

[0060] A drone device includes a quadcopter drone, a visual positioning camera, a depth camera, and a control device. The visual positioning camera and the depth camera are both mounted on the quadcopter drone and are electrically connected to the control device. The control device is used to execute the above-described vision-based autonomous navigation method for quadcopter drones and control the quadcopter drone.

[0061] The beneficial effects of the vision-based autonomous navigation method and UAV device provided by the embodiments of the present invention include:

[0062] This invention provides a vision-based autonomous navigation method for a quadcopter drone, comprising acquiring visual odometry information and an image frame sequence of the quadcopter drone; the image frame sequence includes multiple pairs of RGB image-depth images; determining whether a ring-shaped obstacle of a preset shape exists based on the RGB and depth images; if a ring-shaped obstacle of a preset shape exists, determining whether the ring-shaped obstacle is stationary. If the ring-shaped obstacle is stationary, adjusting the quadcopter drone's yaw angle and altitude using an image-based visual servo controller, and then controlling the quadcopter drone to traverse the ring-shaped obstacle; if the ring-shaped obstacle is not stationary, predicting the trajectory of the center of the ring-shaped obstacle, adjusting the quadcopter drone's flight attitude angle and altitude in the local coordinate system using a discrete LQR controller, and then controlling the quadcopter drone to traverse the ring-shaped obstacle. The vision-based autonomous navigation method for quadcopter drones provided by this invention accurately locates itself and perceives its surrounding environment based on visual information, and adopts corresponding flight trajectory generation strategies for different flight tasks, thereby solving the problems brought about by complex multi-task scenarios and making it possible for quadcopter drones to replace manual operations.

[0063] Embodiments of the present invention also provide a drone device, the control device of which is used to execute the above-described vision-based autonomous navigation method for quadcopter drones. Therefore, it also has the beneficial effect of being able to accurately locate itself and perceive the surrounding environment, and adopt corresponding flight trajectory generation strategies for different flight missions, thereby solving the problems brought about by complex multi-mission scenarios and making it possible for quadcopter drones to replace manual operations. Attached Figure Description

[0064] The above-described features and advantages of the present invention will be better understood after reading the following detailed description of embodiments of the present disclosure in conjunction with the accompanying drawings. In the drawings, components are not necessarily drawn to scale, and components having similar related characteristics or features may have the same or similar reference numerals.

[0065] Figure 1 A flowchart of a vision-based autonomous navigation method for a quadrotor unmanned aerial vehicle according to one aspect of the present invention is shown;

[0066] Figure 2 A pinhole imaging model of a pinhole camera is shown in a vision-based autonomous navigation method for a quadcopter unmanned aerial vehicle according to one aspect of the present invention.

[0067] Figure 3 A schematic diagram of coordinate system relationships in a vision-based autonomous navigation method for a quadrotor unmanned aerial vehicle according to one aspect of the present invention is shown;

[0068] Figure 4 The diagram shows the effect of fitting the center motion trajectory in a vision-based autonomous navigation method for a quadrotor unmanned aerial vehicle according to one aspect of the present invention;

[0069] Figure 5 A distribution diagram of position information over time in a vision-based autonomous navigation method for a quadrotor unmanned aerial vehicle (UAV) according to an aspect of the present invention is shown.

[0070] Figure 6 A graph showing the distribution of velocity over time in a vision-based autonomous navigation method for a quadrotor unmanned aerial vehicle according to an aspect of the present invention is provided.

[0071] Figure 7 A distribution diagram of acceleration over time in a vision-based autonomous navigation method for a quadrotor unmanned aerial vehicle (UAV) according to an aspect of the present invention is shown. Detailed Implementation

[0072] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. It should be noted that the aspects described below with reference to the accompanying drawings and specific embodiments are merely exemplary and should not be construed as limiting the scope of protection of the present invention in any way.

[0073] Please refer to Figure 1 and Figure 2 This invention provides a vision-based autonomous navigation method for a quadcopter unmanned aerial vehicle (UAV), and also provides a UAV device (not shown in the figure).

[0074] The unmanned aerial vehicle (UAV) device includes a quadcopter UAV, a visual positioning camera, a depth camera, and a control device. Both the visual positioning camera and the depth camera are mounted on the quadcopter UAV, and both face directly forward of the UAV. Both the visual positioning camera and the depth camera are electrically connected to the control device, and the information acquired by the visual positioning camera and the depth camera can be sent to the control device. The control device is configured to execute a vision-based autonomous navigation method for the quadcopter UAV and control the quadcopter UAV. Optionally, the control device is a controller installed inside the quadcopter UAV. The installation position relationship between the visual positioning camera and the depth camera includes, but is not limited to, installing the visual positioning camera directly above the depth camera. In other embodiments, the visual positioning camera can also be installed directly below the depth camera, or placed side-by-side, ensuring that the visual positioning camera and the depth camera are placed adjacent to each other.

[0075] The vision-based autonomous navigation method for quadcopter unmanned aerial vehicles provided in the embodiments of the present invention specifically includes:

[0076] S01: Acquire visual pathometer information and image frame sequences from the quadcopter UAV.

[0077] The position information of the quadcopter UAV in the local coordinate system with the takeoff point as the origin was obtained using a visual positioning camera. bx ,p by ,p bz ] T and quaternion attitude information [q bx ,q by ,q bz ,q bw ] T Location information [p] bx ,p by ,p bz ] T This includes the quadcopter UAV's horizontal coordinates and altitude information in the local coordinate system. Local coordinate system ox local -y local -z local With the takeoff point as the origin o, and the direction directly in front of the quadcopter drone at takeoff as x localAt takeoff, the drone's right side is y. local At takeoff, the area directly below the drone is z. local The system is then constructed. Optionally, in this embodiment, the visual positioning camera is an InterRealsense T265 camera.

[0078] A depth camera is used to acquire RGB images and depth information. A depth image is then calculated based on the RGB images and depth information, and multiple pairs of RGB image-depth image sequences constitute an image frame sequence. Optionally, an InterRealsense D415 camera is used. The depth camera can be considered a sensor mounted on a quadcopter drone, acquiring RGB images and depth information, and calculating the depth image, which is aligned with the RGB images. After obtaining the depth image, the depth camera transmits the RGB image-depth image sequence to the control device. Simultaneously, the depth camera transmits data in real time, allowing the control device to obtain an image frame sequence composed of RGB image-depth images at different times.

[0079] S02: Determine whether there is a ring-shaped obstacle of a preset shape based on the RGB image and depth image.

[0080] In this embodiment, the preset shape is circular. Therefore, the annular obstacle with the preset shape in this embodiment is a circular obstacle, and correspondingly, the center of the annular obstacle is the center of the circular obstacle. It should be noted that, in the description of this invention, "annular" refers to a shape that is circumferentially closed and has a through hole in the middle.

[0081] Step S02 specifically includes:

[0082] S21: Preprocess the RGB image to obtain a preprocessed image.

[0083] The depth image is used to determine whether the depth value from the real 3D space point corresponding to a pixel in the RGB image to the quadcopter drone is greater than a valid distance threshold. If it is, the pixel is masked; otherwise, it remains unchanged. After processing each pixel in the RGB image, the resulting image is the preprocessed image. Each pixel in the depth image contains a 16-bit integer representing the depth value from the real 3D space point corresponding to the same pixel in the RGB image to the quadcopter drone. Therefore, the depth value corresponding to a pixel in the RGB image can be obtained from the depth image for this determination.

[0084] In this embodiment, the effective distance threshold is 5 meters. During preprocessing, the pixel data of pixels with a depth value greater than 5 meters in the RGB image are set to [0,0,0]. T This eliminates environmental interference and other obstacles that may interfere with the current task in complex multitasking scenarios.

[0085] S22: Map the preprocessed image from the RGB color space to the HSV color gamut to obtain HSV color gamut features.

[0086] Since the HEV color gamut can express the brightness, hue, and vividness of colors very intuitively, thus better reflecting the difference between the circular obstacle and the environment, the preprocessed image is mapped from the RGB color space to the HSV color gamut. By using the HEV color gamut features, candidate contours that conform to the color gamut features of the circular obstacle can be extracted better.

[0087] S23: Determine whether there is a ring-shaped obstacle of a preset shape based on the HEV color gamut characteristics.

[0088] Candidate contours that match the color gamut features of the circular obstacle are extracted from the HEV color gamut features.

[0089] The number of obstacles in a candidate contour is determined based on morphological features. In this embodiment, the morphological features have two constraints, and the number of circular obstacles is determined based on whether the candidate contour satisfies these two constraints.

[0090] First, in an image with a width of 640 and a height of 480, regions enclosed by candidate contours with an area less than 2000 are treated as image noise. Second, regions with a perimeter squared divided by the area of ​​4π regions that is not between 0.8 and 1.2 are treated as non-circular obstacles. It is understood that in other embodiments, the image width and height can be set as needed, and the two constraints can also be set according to the specific shape of the preset shape.

[0091] S24: Determine whether there is a ring-shaped obstacle with a preset shape in the area in front of the current quadcopter drone.

[0092] If the number of circular obstacles in front of the current quadcopter is 0, proceed to step S06; if the number of circular obstacles in front of the current quadcopter is not 0, it means that there are circular obstacles in front of the current quadcopter. In this case, the one with the largest area is taken as the circular obstacle to be traversed, and proceed to step S3.

[0093] Optionally, when using the area with the largest region as the circular obstacle to be traversed, in order to ensure that the quadcopter drone can successfully traverse the circular obstacle, a step can still be set to calculate the radial dimension of the central through-hole of the circular obstacle to be traversed before executing step S3. If the radial dimension of the central through-hole is greater than that of the quadcopter drone and the quadcopter drone can pass through, proceed to step S3. If the radial dimension of the central through-hole is less than or equal to that of the quadcopter drone and the quadcopter drone cannot or has difficulty passing through, the flight route can be planned by avoiding the circular obstacle.

[0094] S03: Determine whether the circular obstacle is stationary.

[0095] By detecting changes in the position of the center of the circular obstacle, it is possible to determine whether the circular obstacle is stationary. This step S03 specifically includes:

[0096] S31: Calculate the three-dimensional coordinates of the center of the circular obstacle in the local coordinate system.

[0097] Figure 2 A diagram illustrating the pinhole imaging model of the pinhole camera in this embodiment is shown. Figure 2 As shown, a camera coordinate system has been established. Specifically, the camera coordinate system cx c -y c -z c With the optical center position c of the InterRealsense D415 camera as the origin of the coordinate system, and the direction perpendicular to the camera plane and forward as z, c The direction to the right parallel to the camera plane is x. c The direction parallel to the camera plane downwards is y. c .

[0098] Establish a coordinate system ouv in the pixel plane.

[0099] Let the coordinates of the center P of the circular obstacle in the camera coordinate system be [X]. c ,Y c Z c ] T The distance from the optical center of the camera to the physical imaging plane is the focal length f.

[0100] The center P of the circle is projected onto the pixel plane through the optical center at point P', and the pixel coordinates of point P' are [u cen ,v cen ] T .

[0101] According to the corresponding relationship, the mapping relationship corresponds to a scaling and translation amount. The coordinates of the center P in the camera coordinate system [X...] c ,Y c Z c ] T It can be derived that:

[0102]

[0103] Among them, f x ,f y ,c x ,c y These are the camera's intrinsic parameters; [u cen ,v cen ] Td represents the pixel coordinates of the center in the RGB image; ucen,vcen The depth value corresponding to the pixel coordinates.

[0104] Based on the three-dimensional coordinates [X] of the center P in the camera coordinate system c ,Y c Z c ] T Calculate its three-dimensional coordinates [X] in the local coordinate system. b ,Y b Z b ] T .

[0105] Since the quadcopter drone is in motion, the coordinates of the center P in the local coordinate system should be obtained from the rotation relationship between the base coordinate system and the camera coordinate system, the horizontal coordinates, altitude information, and quaternion attitude information of the quadcopter drone in the local coordinate system.

[0106] The body coordinate system is established on the quadcopter drone, and the body coordinate system box b-o -y b-o -z b-o With the optical center position c of the InterRealsense D415 camera as the origin O of the coordinate system, and the direction directly in front of the UAV during flight as x, b-o During flight, the drone's right side is y. b-o During flight, the area directly below the drone is z. b-o Furthermore, at the moment of takeoff of the quadcopter drone, the body coordinate system coincides with the local coordinate system.

[0107] The three-dimensional coordinates of the center P in the local coordinate system [X] b ,Y b Z b ] T The derivation process is as follows:

[0108]

[0109] Among them, [p bx ,p by ,p bz ] T This provides the position information of the quadcopter UAV in the local coordinate system; [q bx ,q by ,q bz ,q bw ] T This represents the quaternion attitude information of the quadrotor UAV in the local coordinate system.

[0110] S32: Determine whether the ring-shaped obstacle is stationary based on the change in its three-dimensional coordinates in the local coordinate system and the judgment threshold.

[0111] Step S31 is repeated at least twice to calculate the three-dimensional coordinates of the center of the circle in the local coordinate system at at least two different times. By comparing these three-dimensional coordinates of the center of the circle in the local coordinate system at these at least two different times, the change in the three-dimensional coordinates in the local coordinate system can be obtained. If the change is greater than or equal to a judgment threshold, the circular obstacle is determined to be stationary; if the change is less than the judgment threshold, the circular obstacle is determined to be non-stationary. Optionally, in this embodiment, the judgment threshold is 0.1.

[0112] S04: Adjust the flight yaw angle and flight altitude of the quadcopter UAV using an image-based visual servo controller.

[0113] If the circular obstacle is stationary, then step S04 is executed, and the autonomous navigation process of traversing the static circular obstacle begins according to step S04.

[0114] During the autonomous navigation process of traversing a static circular obstacle, the quadcopter drone is controlled to fly forward at a constant speed in the body coordinate system. It should be noted that "flying forward at a constant speed" here refers to the quadcopter drone moving forward (x...) in the body coordinate system. b-o The velocity component is a constant.

[0115] Simultaneously, an image-based visual servo controller continuously adjusts the quadcopter UAV's yaw angle and altitude, ensuring that the pixel coordinates of the center of the static circular obstacle are accurately determined during flight. It is always located at the center of the RGB image. In this embodiment, the center of the RGB image is [320, 240]. T This ultimately allows the quadcopter drone to point perpendicularly to the center of a static circular obstacle and pass through it. The adjustment process is achieved using the following formula:

[0116]

[0117]

[0118]

[0119]

[0120]

[0121]

[0122]

[0123] in, The pixel coordinates of the center of the ring-shaped obstacle during the i-th program execution cycle. These represent the errors relative to the center of the RGB image; α is a constant velocity; K zp ,K zi ,K zd K represents the coefficients of the PID algorithm. yawp ,K yawd These are the coefficients of the PD algorithm; These are the desired velocity components in the three directions and yaw angle of the body coordinate system during the i-th program execution cycle; Indicates the first i The cumulative total.

[0124] As the quadcopter drone gradually approaches the circular obstacle until it can no longer recognize the complete outline of the circular obstacle, the adjustment phase ends, and the process proceeds to step S06 to complete the autonomous navigation process of traversing the static circular obstacle.

[0125] S05: In the body coordinate system, the motion trajectory of the center of the circle is predicted by fitting the nonlinear least squares method, and the flight attitude angle and flight altitude of the quadcopter UAV in the local coordinate system are adjusted by using a discrete LQR controller.

[0126] Control the quadcopter drone to hover, define the current time as time n, and continuously record the three-dimensional coordinates [X] of the center of the circular obstacle in the drone's coordinate system at a fixed frequency. b_o ,Y b_o Z b_o ] T This continues until time (n+m). Optionally, the fixed frequency is three times per second.

[0127] For a dynamic circular obstacle undergoing non-uniform periodic motion, the trajectory of its center can be decomposed. This embodiment uses a circular obstacle moving only in the YZ plane as an example; therefore, the trajectory of this circular obstacle can be decomposed into independent sub-trajectories on the Y and Z axes. In the body coordinate system, the trigonometric function trajectory of the center on the Y and Z axes, with events as input, can be represented as:

[0128] f1(t)=a1sin(ω1t+b1)+c1

[0129] f2(t)=a2sin(ω2t+b2)+c2

[0130] Where a1 and a2 represent the amplitude values ​​of the sine function; w1 and w2 represent the frequencies of the sine function; b1 and b2 represent the initial phase of the sine function; and c1 and c2 represent the offsets of the sine function.

[0131] By substituting the recorded data (including three-dimensional coordinates and the corresponding recording time t) into the nonlinear least squares formula to fit the motion trajectories of the circles whose centers are on the Y and Z axes of the body coordinate system, the parameters of functions f1(t) and f2(t) can be obtained. This can be achieved through the following formula:

[0132]

[0133]

[0134]

[0135]

[0136] in, The optimal solution is the one that minimizes the values ​​of the cost functions F1(x1) and F2(x2) by iteratively calculating from a given initial value using the nonlinear least squares method. For t i The components of the three-dimensional coordinates along the Y-axis recorded at each moment; For t i The Z-axis component of the three-dimensional coordinates recorded at any given time.

[0137] Predict the three-dimensional coordinates of the circle's center at time (n+h), and transform them to the local coordinate system as the desired target point for the quadcopter UAV's flight in the local coordinate system [p]. tarx ,p tary ,p tarz ] T Specifically, the three-dimensional coordinates of the quadcopter drone in the local coordinate system when it is hovering are recorded [p]. holdx ,p holdy ,p holdz ] T ; Substituting these values ​​into f1(t) and f2(t) respectively, we predict the three-dimensional coordinates of the center of the dynamic circular obstacle at time n+h in the body coordinate system. These three-dimensional coordinates are then transformed into the local coordinate system as the desired target point for the quadcopter UAV's flight in the local coordinate system.

[0138] [p tarx ,p tary ,p tarz ] T .

[0139] To ensure a smooth autonomous navigation process for the quadcopter UAV, a flight mode of uniform acceleration followed by uniform deceleration is adopted. Simultaneously, the quadcopter UAV is set to begin its autonomous navigation process from its hovering three-dimensional coordinates at time (n+1) and reach the desired target point [p] at time (n+1). tarx ,ptary ,p tarz ] T Based on the above requirements for position, time, and smoothness, a set of discrete desired flight trajectory sequences is generated:

[0140]

[0141] in, Let J be the expected coordinates at time j; Let be the velocity in the X and Y axes at time j; dt and dt represent the accelerations in the X and Y axes at time j, respectively; dt represents the total flight time required from the hovering point to the desired target point, i.e., DT = hl; dt represents the time interval between two adjacent desired flight trajectory points; m < l < h.

[0142] Optionally, in this embodiment, m = 5, l = 6, and h = 11. It is understood that in other embodiments, the values ​​of m, l, and h can also be specifically set.

[0143] The discrete desired flight trajectory sequence is generated using the following formula:

[0144] num = DT / dt

[0145] v maxx ,v maxy ,e z =2(p tarx -p holdx ) / DT,2(p tary -p holdy ) / DT,p tarz -p holdz

[0146]

[0147]

[0148]

[0149] To track the generated desired flight trajectory sequence using a discrete LQR controller, the horizontal coordinates and velocity of the quadcopter UAV in the local coordinate system need to be set as state variables x = [p bx ,p by ,v bx ,v by ] T The input quantity is the desired horizontal acceleration in the local coordinate system, command u = [a desx ,a desy ] T The discrete LQR controller can then be described as:

[0150]

[0151]

[0152] Where ST represents the optimization function; J represents the cost function of a problem; and j represents time step.

[0153] The LQR controller solves the Riccati algebraic equation to obtain the S matrix and the feedback matrix K. Input compensation is then applied to the discrete LQR controller to determine the control inputs used for the autonomous navigation process of the quadcopter UAV. This is achieved through the following formula:

[0154]

[0155] Where j represents time; x(j) represents the actual state quantity at time j; x tar (j) represents the position and velocity at time j in the generated expected flight trajectory sequence; a tar (j) represents the acceleration at time j in the generated expected flight trajectory sequence; Let j be the position on the x-axis and y-axis in the local coordinate system at time j. and Let $j$ be the velocities along the lower x-axis and y-axis of the local coordinate system at time $j$.

[0156] Subsequently, after reaching time (n+l), the desired flight trajectory sequence and the visual odometry of the quadcopter UAV in the local coordinate system are substituted into the above formula according to the interval dt. After calculating the control input that changes with time, the flight attitude angle and flight altitude control command of the quadcopter UAV in the local coordinate system are calculated based on the correspondence between the horizontal acceleration and attitude angle of the quadcopter UAV. This ensures that the desired target point is reached at time (n+h), which is exactly at the center of the dynamic circular obstacle. This process is achieved through the following formula:

[0157]

[0158] Where, φ j ,θ j ,ψ j , These represent the j-th roll, pitch, yaw, and altitude control commands, respectively; ψ0 represents the yaw angle in hovering mode.

[0159] At time (n+h), when the quadcopter UAV reaches the center of the dynamic circular obstacle, the discrete LQR controller stops issuing control commands and proceeds to step S06.

[0160] S06: Flying forward at a constant speed.

[0161] Maintaining the current yaw angle and altitude of the quadcopter drone, control it to fly forward at a constant speed in the body coordinate system. Specifically, the formula is:

[0162] v x =2α

[0163] v y =0

[0164] v z =0

[0165]

[0166] The vision-based autonomous navigation method for quadcopter UAVs provided in the embodiments of the present invention eliminates environmental interference and other obstacles that interfere with the current task in complex multi-task scenarios based on depth information. It improves the performance of environmental perception by first extracting the HSV color gamut and then detecting morphological features. At the same time, it selects appropriate flight strategies for different tasks to improve flight robustness.

[0167] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A vision-based autonomous navigation method for a quadcopter unmanned aerial vehicle (UAV), characterized in that, include: The visual odometry information and image frame sequence of a quadcopter drone are acquired; wherein the image frame sequence includes multiple pairs of RGB image-depth image; Determine whether a pre-defined circular obstacle exists based on the RGB and depth images; If a ring-shaped obstacle of the preset shape exists, determine whether the ring-shaped obstacle is stationary; If the ring-shaped obstacle is stationary, the flight yaw angle and flight altitude of the quadcopter UAV are adjusted using an image-based visual servo controller. After adjustment, the flight yaw angle and flight altitude of the quadcopter UAV remain unchanged, and the quadcopter UAV is controlled to fly forward at a constant speed in the body coordinate system to pass through the ring-shaped obstacle. If the ring-shaped obstacle is not stationary, then in the body coordinate system, the motion trajectory of the center of the ring-shaped obstacle is predicted by fitting using the nonlinear least squares method. The flight attitude angle and flight altitude of the quadrotor UAV in the local coordinate system are adjusted using a discrete LQR controller. After adjustment, the current flight yaw angle and flight altitude of the quadrotor UAV remain unchanged, and the quadrotor UAV is controlled to fly forward at a constant speed in the body coordinate system to pass through the ring-shaped obstacle.

2. The vision-based autonomous navigation method for a quadcopter UAV according to claim 1, characterized in that, The steps for acquiring the visual odometry information and image frame sequence of the quadcopter drone include: The position information of the quadcopter UAV in the local coordinate system with the takeoff point as the origin was obtained using a visual positioning camera. bx ,p by ,p bz ] T and quaternion attitude information [q bx ,q by ,q bz ,q bw ] T ; An RGB image and depth information are acquired using a depth camera, and a depth image is calculated based on the RGB image and the depth information.

3. The vision-based autonomous navigation method for a quadcopter UAV according to claim 1, characterized in that, The step of determining whether a ring-shaped obstacle of a preset shape exists based on the RGB image and depth image includes: The RGB image is preprocessed to obtain a preprocessed image; The preprocessing includes: determining whether the depth value from the real three-dimensional space point corresponding to each pixel in the RGB image to the quadcopter drone is greater than the effective distance threshold based on the depth image; if so, the pixel is masked; if not, it remains unchanged. The preprocessed image is mapped from the RGB color space to the HSV color gamut to obtain HSV color gamut features; The presence of a ring-shaped obstacle of a preset shape is determined based on the HSV color gamut characteristics.

4. The vision-based autonomous navigation method for a quadcopter UAV according to claim 3, characterized in that, The step of determining whether a ring-shaped obstacle of a preset shape exists based on the HSV color gamut characteristics includes: Extract candidate contours of the ring-shaped obstacle color gamut features that conform to the preset shape from the HSV color gamut features; The number of ring-shaped obstacles with a preset shape in the candidate contour is detected based on morphological features; Determine whether there is a ring-shaped obstacle with a preset shape in the area in front of the current quadcopter drone; if so, select the one with the largest area as the ring-shaped obstacle to be traversed.

5. The vision-based autonomous navigation method for a quadcopter UAV according to claim 1, characterized in that, The steps for determining whether the ring-shaped obstacle is stationary include: Calculate the three-dimensional coordinates of the center of the ring-shaped obstacle in the local coordinate system at at least two different times; Based on the change in the three-dimensional coordinates in the local coordinate system and the judgment threshold, it is determined whether the ring-shaped obstacle is stationary.

6. The vision-based autonomous navigation method for a quadcopter UAV according to claim 5, characterized in that, The steps for calculating the three-dimensional coordinates of the center of the ring-shaped obstacle in the local coordinate system include: Calculate the three-dimensional coordinates [X] of the center in the camera coordinate system. c ,Y c Z c ] T The calculation formula is: Among them, f x ,f y ,c x ,c y These are the camera's intrinsic parameters; [u cen ,v cen ] T The pixel coordinates of the center in the RGB image; The depth value corresponding to the pixel coordinates; Based on the three-dimensional coordinates [X] in the camera coordinate system c ,Y c Z c ] T Calculate the three-dimensional coordinates [X] of the center in the local coordinate system. b ,Y b Z b ] t : Among them, [P bx ,P by ,P bz ] T The position information of the quadcopter UAV in the local coordinate system; [q bx ,q by ,q bz ,q bw ] T The quaternion attitude information of the quadrotor UAV in the local coordinate system.

7. The vision-based autonomous navigation method for a quadcopter UAV according to claim 1, characterized in that, The steps of adjusting the flight yaw angle and flight altitude of the quadcopter UAV using an image-based visual servo controller include: Adjust the flight yaw angle and flight altitude of the quadcopter drone so that the center of the ring-shaped obstacle to be traversed is located at the center of the RGB image; The adjustment process is achieved through the following formula: in, The pixel coordinates of the center of the ring-shaped obstacle during the i-th program execution cycle. The errors relative to the center of the RGB image are respectively; α is a constant velocity; K zp ,K zi ,K zd K represents the coefficients of the PID algorithm. yawp ,K yawd These are the coefficients of the PD algorithm; These are the desired velocity components in the three directions and yaw angle of the body coordinate system during the i-th program execution cycle; Indicates the first i The cumulative total; During the adjustment process, the quadcopter drone is controlled to fly forward at a constant speed.

8. The vision-based autonomous navigation method for a quadcopter UAV according to claim 1, characterized in that, In the body coordinate system, the steps of fitting and predicting the trajectory of the center of the circle using the nonlinear least squares method, and adjusting the flight attitude angle and flight altitude of the quadrotor UAV in the local coordinate system using a discrete LQR controller include: The quadcopter drone is controlled to hover, and the current time is taken as time n and the three-dimensional coordinates of the center of the ring obstacle in the body coordinate system are continuously recorded at a fixed frequency until time (n+m) ends. The recorded data is substituted into a nonlinear least multiplication formula to fit the motion trajectory of the center in the body coordinate system; Predict the three-dimensional coordinates of the center at time (n+h) and transform them to the local coordinate system as the desired target point for the quadcopter UAV to fly in the local coordinate system [p tarx ,p tary ,p tarz ] T ; Generate a set of discrete sequences of desired flight trajectories: in, Let J be the expected coordinates at time j; Let be the velocity in the X and Y axes at time j; Let be the accelerations in the X and Y directions at time j, respectively; At time (n+l), the discrete LQR controller is used to control the quadcopter UAV to adjust its flight attitude angle and flight altitude in the local coordinate system, and the quadcopter UAV reaches the desired target point at time (n+h). Where m < l < h, DT = hl, and dt represents the time interval between two adjacent desired flight trajectory points.

9. The vision-based autonomous navigation method for a quadcopter UAV according to claim 8, characterized in that, The following formulas can be used to adjust the flight attitude angles and flight altitude in the local coordinate system: Where K is the feedback matrix calculated by the discrete LQR controller; These represent the j-th roll, pitch, yaw, and altitude control commands, respectively; ψ0 is the yaw angle in hovering mode. Let j be the position on the x-axis and y-axis of the local coordinate system at time j. and These are the velocities along the lower x-axis and y-axis of the local coordinate system at time j, respectively. Let be the expected values ​​of the horizontal acceleration along the x and y axes at time j.

10. A drone device, characterized in that, The system includes a quadcopter drone, a visual positioning camera, a depth camera, and a control device. The visual positioning camera and the depth camera are both mounted on the quadcopter drone and are electrically connected to the control device. The control device is used to execute the vision-based autonomous navigation method for a quadcopter drone as described in any one of claims 1-9 and to control the quadcopter drone.

Images