Autonomous door frame control method and system for unmanned aerial vehicle based on binocular vision positioning

Abstract

The present invention discloses a control method and system for autonomously crossing a door frame of a drone based on binocular vision positioning. First, a binocular camera is used to shoot an RGB image of the door frame, and a histogram equalization process is performed. Then, a binary image of the door frame range is obtained by screening using an HSV channel threshold, and a morphological process is performed. Then, the center point of the corner point of the door frame is detected. By establishing a tf conversion in a ROS system, the relative coordinates of the camera and the door frame are converted into the absolute coordinates between the door frame and the world coordinate system, and the coordinates of the crossing key points on the normal line of the door frame plane are calculated. After the drone flies to a target point, the stability of its own hovering is determined, and the flight mission is continued after reaching the standard, so that the drone can cross the center of the door frame. The present invention avoids the problem of inaccurate depth information acquisition of the corner point as an edge point by detecting the center point of the corner point of the door frame, and has good recognition and crossing capabilities for quadrilateral door frames with consistent colors and good lighting conditions.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle applications, and in particular relates to a control method and system for an unmanned aerial vehicle to autonomously pass through a door frame based on binocular vision positioning. Background Art

[0002] Drones are currently widely used in industry, agriculture, military and other fields. When performing tasks, drones often need to pass through door frames with limited area and pass through indoor and outdoor areas. Due to the existence of the edge of the door frame, collisions are prone to occur. Therefore, drones need to have the function of automatically identifying and locating door frames. Binocular vision is based on the principle of parallax. It uses imaging equipment to obtain two images of the object to be measured from different positions, and obtains the three-dimensional geometric information of the object by calculating the position deviation between the corresponding points of the images. By setting a binocular camera on the drone, the first image and the second image of the door frame are obtained by the binocular camera. Then, according to the parallax principle and the generated stereoscopic vision information, the three-dimensional spatial coordinates of the door frame can be directly restored, and then the depth value of the door frame can be obtained. Based on the obtained depth value, the distance between the drone and the door frame is obtained, and then the drone is allowed to avoid the door frame. The currently commonly used door frame corner point recognition algorithm is easy to misidentify the depth of the corner point in the camera image as the background depth near the corner point due to the slight deviation of the pixel coordinates of the corner point in the process of obtaining the corner point by the binocular camera, which causes a large error in the acquisition of the depth information of the corner point, affecting subsequent calculations. Summary of the invention

[0003] The present invention provides a control method and system for a drone to identify and locate a door frame using a binocular camera when the drone passes through indoors or outdoors, and to realize automatic crossing. The method removes the background and locates the depth of the door frame image captured by the binocular camera, determines the position of the door frame relative to the drone, and autonomously calculates the key points of crossing to complete the autonomous flight of the drone passing through the door frame.

[0004] In order to achieve the above object, the present invention provides a method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning, comprising the following steps:

[0005] Step 1: Use a binocular camera to capture an RGB scene image containing a door frame and perform histogram equalization processing;

[0006] Step 2, the HSV channel threshold is used to obtain a binary image of the door frame range, and morphological processing is performed;

[0007] Step 3, detect the center point of the corner point of the door frame and calculate the key point of crossing;

[0008] Step 4, convert the camera coordinates of the key points to the world coordinate system;

[0009] Step 5: Plan a path to cross the door frame obstacle.

[0010] Moreover, in the step 2, the RGB image of the door frame taken by the camera is first converted into the HSV color space, and then the HSV image is thresholded for the three channels of H, S, and V, that is, the threshold range is set for the three channels of H, S, and V respectively, and the pixels that meet the threshold range are retained and marked as white, and the pixels that are not in the threshold range are marked as black. The six threshold values ​​of the three target HSV channels are manually adjusted so that the shape of the door frame can be screened out from the background. After the HSV threshold screening, the image becomes a binary image in black and white that only shows the door frame range. The binary image after HSV screening is morphologically processed, that is, an open operation is first performed and then a closed operation is performed. The open operation is first corrosion and then expansion, and the closed operation is first expansion and then corrosion. The corrosion operation can eliminate noise and erase part of the boundary contour of the image, so that the overall area of ​​the image is reduced relative to before. The expansion operation can increase the target image range and fill the holes, so that the overall area of ​​the image is expanded relative to before.

[0011] Moreover, in step 3, the binary image after morphological processing is subjected to contour detection to obtain the inner and outer contours of the door frame, and the eight corner pixel positions of the inner and outer contours of the door frame are obtained at the same time. The pixel coordinate positions of the corner points are compared with the pixel coordinate positions of the center point of the door frame, and the four groups of eight corner points, namely, the upper left, lower left, upper right, and lower right, are distinguished. The average value of the pixel coordinates of each group of corner points is calculated to obtain the pixel coordinates of the midpoint of the line connecting each group of corner points at the corresponding position, that is, the pixel coordinates of the center point of the corner point. The origin of the pixel coordinate system is located in the upper left corner of the image, the x-axis points to the horizontal direction of the image, increasing from left to right, and the y-axis points to the vertical direction of the image, increasing from top to bottom. The origin position of the camera coordinate system is the same as the center of mass position of the drone, and moves with the movement of the drone. The x-axis of the coordinate system points to the right of the drone head, the y-axis points downward, and the z-axis points in the direction of the drone head. Through the coordinate conversion between the pixel coordinate system and the camera coordinate system, the pixel coordinates of the center point of the corner point are converted to the camera coordinate system. The coordinate conversion formula is as follows:

[0012]

[0013]

[0014]

[0015] In the formula, x c ,y c 、z c They represent the coordinates of the center of the corner point in the camera coordinate system after coordinate transformation; d represents the depth value of the center of the corner point measured by the camera in the pixel coordinate system; f is a constant, representing the conversion of the depth map in millimeters to the actual distance in meters; x p ,y pRepresent the pixel coordinates of the center point of the corner point; c x 、c y They represent the offset of the camera optical axis in the image in pixels; f x 、f y Represents the focal length of the camera in the x and y directions respectively.

[0016] According to the coordinates of the center points of the two lower corner points in the camera coordinate system, the angle between the door frame and the x-axis of the camera coordinate system in the top-down perspective is calculated, which is denoted as α, that is:

[0017]

[0018] In the formula, z br Indicates the z coordinate of the center point of the lower right corner of the door frame in the camera coordinate system, z bl Indicates the z coordinate of the center point of the lower left corner of the door frame in the camera coordinate system, x br Indicates the x coordinate of the center point of the lower right corner of the door frame in the camera coordinate system, x bl Indicates the x-coordinate of the center point of the lower left corner of the door frame in the camera coordinate system.

[0019] The coordinates of the center point of the door frame can be obtained by averaging the coordinates of the center points of the four corner points, and the distance d from the center point of the door frame to the normal line of the door frame plane passing through the center point of the door frame can be calculated. w The two coordinates of the length are used as the hovering points before and after the drone passes through the door frame, recorded as crossing key point 1 and crossing key point 2. The normal equation is calculated as follows:

[0020] z=z m (5)

[0021] y-ym=tanα(x-xm)(6)

[0022] Where α is the angle between the door frame and the x-axis of the camera coordinate system in the top-down perspective, x, y, and z represent any point on the normal line of the door frame plane passing through the center point of the door frame, and x m ,y m 、z m Represents the coordinate value of the center point of the door frame in the camera coordinate system. The position of the crossing key point is calculated by the coordinate value of the center point of the door frame and the angle α, that is:

[0023] x1=x m -d w sinα(7)

[0024] y1=y m (8)

[0025] z1=z m +d w cosα(9)

[0026] x2=x m +d w sinα(10)

[0027] y2=y m (11)

[0028] z2=z m -d w cosα(12)

[0029] Where x1, y1, and z1 represent the coordinate values ​​of the crossing key point 1 in the camera coordinate system; x2, y2, and z2 represent the coordinate values ​​of the crossing key point 2 in the camera coordinate system; m ,y m 、z m Indicates the coordinate value of the center point of the door frame in the camera coordinate system; d w Indicates the distance from the key point of crossing to the center point of the door frame. The value is set manually according to actual needs.

[0030] Check whether the calculation of crossing key points 1 and 2 is correct. Suppose vector A is the vector from the center of the door frame to crossing key point 1, and vector B is the vector from the center of the door frame to the origin of the camera coordinate system. After doing the dot product of vectors A and B, if the result is greater than 0, it means that the angle between the two vectors is less than 90°, and the values ​​of crossing key point 1 and crossing key point 2 remain unchanged; if the result is less than 0, it means that the angle between the two vectors is greater than 90°, and the coordinate values ​​of crossing key point 1 and crossing key point 2 are exchanged.

[0031] Moreover, in step 4, the tf conversion tool in the ROS system is used to establish the conversion relationship between the body coordinate system and the world coordinate system. The origin of the world coordinate system is the position of the drone slam when the map is built. The x-axis of the coordinate system points to the right of the drone head, the y-axis points to the direction of the head, and the z-axis points to the top of the drone. Since the camera is fixed on the drone, the deviation between the origin position of the camera coordinate system and the origin position of the body coordinate system is very small, so the camera coordinate system is regarded as the body coordinate system. In the ROS system, the real-time tf conversion relationship between the body coordinate system and the world coordinate system is published and subscribed, and the tf conversion tool is used to convert the coordinates of the crossing key point 1 and the crossing key point 2 in the camera coordinate system into the coordinate values ​​in the world coordinate system.

[0032] Moreover, the path planning in step 5 adopts a control method of directly flying to the flight point released by the drone. After obtaining the coordinates of the crossing key points 1 and 2, the first target point is set to be above the coordinates of the current drone, and the second and third flight target points are the converted crossing key points 1 and 2 coordinates. When the drone hovers stably at the first target point, it flies to the second flight target point. After arriving at the second flight target point, it continues to wait for the drone to hover stably, and then flies to the third flight target point. In this process, the drone completes the crossing of the door frame.

[0033] The hovering stability of the drone is determined by the distance and time between the current position coordinates of the drone and the target position coordinates. The current position coordinates of the drone are obtained in real time through the topic representing the drone's posture information in the ROS system. Whenever the drone receives the command of the flight target point, the ROS system starts to calculate the distance between the current position of the drone and the target position. When the distance between the current position of the drone and the target position is less than the threshold d e After that, the timer starts to accumulate time. Whenever the distance between the current position of the drone and the target position is greater than this threshold, the timer time is reset to zero. When the timer time accumulates to the time threshold t, it is determined that the drone's hovering stability meets the standard, and the drone receives and executes the command to fly to the next target point.

[0034] The present invention also provides a control system for an unmanned aerial vehicle autonomously passing through a door frame based on binocular vision positioning, which is used to implement a control method for an unmanned aerial vehicle autonomously passing through a door frame based on binocular vision positioning as described above.

[0035] Moreover, it includes a processor and a memory, the memory is used to store program instructions, and the processor is used to call the stored instructions in the memory to execute the above-mentioned method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning.

[0036] Alternatively, it includes a readable storage medium having a computer program stored thereon, and when the computer program is executed, the above-mentioned method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning is implemented.

[0037] Compared with the prior art, the present invention has the following advantages:

[0038] 1) The present invention simultaneously identifies the coordinates of the inner and outer contour corner points of the door frame, calculates the average value to obtain the corner point average value, so that the position of the center point of the corner point is located in the central part of the door frame body, thereby avoiding the erroneous identification of the depth of the corner point; 2) The present invention takes into account the collision risk that may be caused by the initial posture of the door frame, and sets the crossing key point at a certain distance in front of the door frame by calculating the attitude angle of the door frame relative to the world coordinate system, thereby avoiding the problem of collision between the drone body and the door frame when crossing the sideways door frame. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0040] Figure 1The present invention is a flow chart of a method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning.

[0041] Figure 2 It is the HSV three-channel screening threshold set in the embodiment of the present invention.

[0042] Figure 3 It is a binary image of a door frame obstacle and corner point center positioning according to an embodiment of the present invention.

[0043] Figure 4 It is the flight trajectory of an aircraft when the unmanned aerial vehicle performs a GPS-free autonomous door frame flight test in an embodiment of the present invention.

[0044] Figure 5(a) is the flight speed of the aircraft in the x direction when the drone is tested for autonomously crossing a door frame without GPS according to an embodiment of the present invention. Figure 5(b) is the flight speed of the aircraft in the y direction when the drone is tested for autonomously crossing a door frame without GPS according to an embodiment of the present invention. Figure 5(c) is the flight speed of the aircraft in the z direction when the drone is tested for autonomously crossing a door frame without GPS according to an embodiment of the present invention. DETAILED DESCRIPTION

[0045] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in combination with the drawings and embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0046] Example 1

[0047] like Figure 1 As shown, an embodiment of the present invention provides a method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning, comprising the following steps:

[0048] Step 1: Get the world coordinate system map of the current scene.

[0049] Start the SLAM mapping program of the drone's airborne laser radar to obtain the world coordinate system map of the current scene. The origin of the world coordinate system is the current location of the drone, the x-axis of the coordinate system points to the right of the drone's head, the y-axis points to the direction of the drone's head, and the z-axis points to the top of the drone.

[0050] Step 2: Get the internal parameters of the binocular camera.

[0051] Use the rostopic echo command in the ROS system (Robot Operating System) to find the intrinsic parameter k matrix of the binocular camera, and obtain the focal length of the camera in the x and y directions and the offset of the camera's optical axis in the image in pixels.

[0052] Step 3: Use a binocular camera to collect an RGB scene image containing a door frame, and perform histogram equalization, HSV channel threshold screening, and morphological processing on the scene image.

[0053] Use a binocular camera to collect RGB scene images containing door frames. First, convert the image format collected by the camera into a Mat image format that can be processed by OpenCV, and then perform histogram equalization on the image so that the image brightness can be more evenly distributed on the histogram, enhancing the local contrast of the image brightness without affecting the overall contrast of the image. Convert the image in the RGB color space collected by the camera into the HSV color space, and then perform threshold screening on the H, S, and V channels of the image, that is, set the threshold range for the H, S, and V channels respectively, retain the pixels that meet the threshold range and mark them as white, and mark the pixels that are not in the threshold range as black. This threshold screening algorithm can be applied to rectangular door frames of different colors. Manually adjust the six threshold values ​​of the target HSV three channels so that the shape of the door frame can be screened out from the background. After HSV threshold screening, the image becomes a binary image in black and white that only shows the door frame range. The center points of the four corner points of the door frame are marked with dots, and the center point of the door frame is marked with a cross. Figure 2 As shown, in this embodiment, the H channel is set to 45-75 (range 0-179), the S channel is set to 50-100 (range 0-255), and the V channel is set to 1-38 (range 0-255) for a green door frame under indoor lighting conditions, and the binary image and corner center positioning of the door frame obstacle are obtained as shown in FIG. Figure 3 As shown. Figure 3 It can be seen that the HSV three-channel threshold screening algorithm proposed in the present invention also has good recognition performance for quadrilateral door frames with uneven edges.

[0054] Morphological processing is performed on the image for the noise and holes that may appear in the door frame part of the binary image after HSV screening. The erosion operation can eliminate noise and erase the boundary contour of part of the image, so that the overall area of ​​the image is smaller than before. The dilation operation can increase the range of the target image and fill the holes, so that the overall area of ​​the image is larger than before. The process of first erosion and then dilation is called opening operation. The opening operation is used on the binary image to eliminate the noise signal outside the object and separate the object at the thin connection, so that the image boundary becomes smoother without significantly expanding or reducing the image area, and the overall position and shape of the image will not change. The closing operation is a process of first dilation and then erosion. The closing operation is used on the binary image to fill the small holes in the image and make the boundary smoother without significantly expanding or reducing the image area. The present invention utilizes the opening operation function cv2.morphologyEx(src,cv2.MORPH_OPEN,kernel) and the closing operation function cv2.morphologyEx(src,cv2.MORPH_CLOSE,kernel) provided in the OpenCV library to perform an opening-first-and-closing operation on the binary image after HSV screening. After the binary image is subjected to the image morphological operation, the internal holes and the noise signals outside the image are removed, the overall boundary of the door frame image is smoothed, and some connected domains are connected, so that the shape of the binary image is more regular, and the area of ​​the door frame image relative to the binary image before the morphological processing is not significantly changed.

[0055] Step 4: Detect the center points of the corners of the door frame and calculate the key points of crossing.

[0056] This embodiment uses the contour detection function cv::findContours() provided by OpenCV to perform contour detection on the binary image after morphological processing, obtain the inner and outer contours of the door frame, and simultaneously obtain the eight corner pixel positions of the inner and outer contours of the door frame. The pixel coordinate positions of the corner points are compared with the pixel coordinate positions of the center point of the door frame, and the four groups of eight corner points, namely the upper left, lower left, upper right, and lower right, are distinguished. The average value of the pixel coordinates of each group of corner points is calculated, and the pixel coordinates of the midpoint of the line connecting each group of corner points at the corresponding position are obtained, that is, the pixel coordinates of the center point of the corner point. The origin of the pixel coordinate system is located in the upper left corner of the image, the x-axis points to the horizontal direction of the image, increasing from left to right, and the y-axis points to the vertical direction of the image, increasing from top to bottom.

[0057] The origin of the camera coordinate system is the same as the center of mass of the drone, and moves with the drone. The x-axis of the coordinate system points to the right of the drone head, the y-axis points downward, and the z-axis points to the direction of the drone head. Through the coordinate conversion between the pixel coordinate system and the camera coordinate system, the pixel coordinates of the center point of the corner point are converted to the camera coordinate system. The coordinate conversion formula is as follows:

[0058]

[0059]

[0060]

[0061] In the formula, x c ,y c 、z c They represent the coordinates of the center of the corner point in the camera coordinate system after coordinate conversion; d represents the depth value of the center of the corner point measured by the camera in the pixel coordinate system; f is a constant, representing the conversion of the depth map in millimeters to the actual distance in meters, and its value is 1000; x p ,y p Represent the pixel coordinates of the center point of the corner point; c x 、c y They represent the offset of the camera optical axis in the image in pixels; f x 、f y Represents the focal length of the camera in the x and y directions respectively.

[0062] According to the coordinates of the center points of the two lower corner points in the camera coordinate system, the angle between the door frame and the x-axis of the camera coordinate system in the top-down perspective is calculated, which is denoted as α, that is:

[0063]

[0064] In the formula, z br Indicates the z coordinate of the center point of the lower right corner of the door frame in the camera coordinate system, z bl Indicates the z coordinate of the center point of the lower left corner of the door frame in the camera coordinate system, x br Indicates the x coordinate of the center point of the lower right corner of the door frame in the camera coordinate system, x bl Indicates the x-coordinate of the center point of the lower left corner of the door frame in the camera coordinate system.

[0065] The coordinates of the center point of the door frame can be obtained by averaging the coordinates of the center points of the four corner points. Calculate the distance d from the center point of the door frame on the normal line of the door frame plane passing through the center point of the door frame. w The two coordinates of the length are used as the hovering points before and after the drone passes through the door frame, and are recorded as crossing key point 1 and crossing key point 2. The normal equation is calculated as follows:

[0066] z=z m (5)

[0067] y-ym=tanα(x-xm)(6)

[0068] Where α is the angle between the door frame and the x-axis of the camera coordinate system in the top-down perspective, x, y, and z represent any point on the normal line of the door frame plane passing through the center point of the door frame, and xm ,y m 、z m Represents the coordinate value of the center point of the door frame in the camera coordinate system. The position of the crossing key point is calculated by the coordinate value of the center point of the door frame and the angle α, that is:

[0069] x1=x m -d w sinα(7)

[0070] y1=y m (8)

[0071] z1=z m +d w cosα(9)

[0072] x2=x m +d w sinα(10)

[0073] y2=y m (11)

[0074] z2=z m -d w cosα(12)

[0075] Where x1, y1, and z1 represent the coordinate values ​​of the crossing key point 1 in the camera coordinate system; x2, y2, and z2 represent the coordinate values ​​of the crossing key point 2 in the camera coordinate system; m ,y m 、z m Indicates the coordinate value of the center point of the door frame in the camera coordinate system; d w It represents the distance from the key point of crossing to the center point of the door frame, which is 0.6m in this embodiment.

[0076] Check whether the calculation of crossing key points 1 and 2 is correct. Let vector A be the vector from the center of the door frame to crossing key point 1, and vector B be the vector from the center of the door frame to the origin of the camera coordinate system. After doing the dot product of vectors A and B, if the result is greater than 0, it means that the angle between the two vectors is less than 90°, and the values ​​of crossing key points 1 and 2 are kept unchanged; if the result is less than 0, it means that the angle between the two vectors is greater than 90°, and the coordinate values ​​of crossing key points 1 and 2 are swapped. The camera coordinate system coordinate values ​​of crossing key points 1 and 2 are output in the ROS terminal. The coordinate values ​​of crossing key points 1 and 2 are published in the ROS topic, and the published topic names are / point / camera / first and / point / camera / second respectively.

[0077] Step 5: Convert the camera coordinates of the key points to the world coordinate system.

[0078] After obtaining the camera coordinates of the door frame crossing key points 1 and 2, since the flight instructions of the drone are all referenced to the world coordinate system, it is necessary to complete the conversion between the camera coordinate system of the crossing key points, the body coordinate system, and the world coordinate system. Since the camera is fixed on the drone, the deviation between the origin of the camera coordinate system and the origin of the body coordinate system is very small, so the camera coordinate system is regarded as the body coordinate system to simplify the calculation and code writing process.

[0079] The relative position and attitude of the camera coordinate system and the world coordinate system will change in real time during the flight of the drone, so the conversion relationship between the two needs to take into account the real-time position of the drone. The rostopic published by the drone provides the real-time position and attitude information of the drone relative to the origin coordinates of the world coordinate system, including the offset values ​​in the x, y, and z directions and the quaternion of the attitude, which can be used as the conversion relationship between the world coordinate system and the body coordinate system, and further as the conversion relationship between the world coordinate system and the camera coordinate system. Use the tf conversion tool in the ROS system to establish the conversion relationship from the body coordinate system to the world coordinate system. Publish and subscribe to the real-time tf conversion relationship between the two coordinate systems in the ROS system, and use the tf conversion tool to convert the coordinates of the crossing key point 1 and the crossing key point 2 in the camera coordinate system to the coordinate values ​​in the world coordinate system.

[0080] Step 6: Plan a path to cross the door frame obstacle.

[0081] Limited by the computing power and transmission bandwidth of the onboard processor, the camera's refresh frame rate is low. Under the current equipment and running code, the real-time performance of the drone's post-flight and camera data processing is poor. Therefore, the crossing program is run in a stable state before the drone flies to calculate the world coordinate system coordinates of crossing key points 1 and 2.

[0082] The path planning adopts the control method of directly flying to the flight point released by the drone. After obtaining the coordinates of the crossing key points 1 and 2, the first target point is set to be above the coordinates of the current drone, and the second and third flight target points are the converted crossing key points 1 and 2 coordinates. When the drone hovers stably at the first target point, it flies to the second flight target point. After reaching the second flight target point, it continues to wait for the drone to hover stably, and then flies to the third flight target point. In this process, the drone completes the crossing of the door frame. The judgment of drone hovering stability is jointly represented by the distance and time between the current position coordinates of the drone and the target position coordinates. The topic representing the drone's posture information in the ROS system can be used to obtain the coordinates of the current position of the drone in real time. Whenever the drone receives an instruction to the flight target point, the ROS system begins to calculate the distance between the current position of the drone and the target position. When the distance between the current position of the drone and the target position is less than the threshold d eAfter that, the timer starts to accumulate time; every time the distance between the current position of the drone and the target position is greater than this threshold, the timer time is reset to zero. When the timer time accumulates to the time threshold t, it is determined that the drone's hovering stability meets the standard, and the drone receives and executes the command to fly to the next target point.

[0083] The drone was tested for autonomous flight through a door frame without GPS. The flight trajectory of the drone is as follows: Figure 4 shown. Figure 4 It is the flight trajectory of the drone crossing the door frame twice from a bird's-eye view. S is the starting point of the drone's flight. It flies counterclockwise. After the first crossing, the drone circles from one side and returns to the crossing key point 1, and then makes the second crossing. Figure 5(a)-5(c) It shows the flight speed of the drone in the x, y, and z directions during this flight. The horizontal axis represents the timestamp and the vertical axis represents the flight speed (unit: m / s), reflecting the flight and hovering process of the aircraft. Figure 4 and Figure 5(a)-5(c) It can be seen that the intelligent control method for autonomously crossing a door frame by a drone proposed in the present invention can enable the drone to fly and hover according to a preset trajectory, preventing the drone from crossing the door frame when it is in an unstable state, effectively avoiding the collision between the drone and the door frame, and completing the door frame crossing task.

[0084] Example 2

[0085] Based on the same inventive concept, the present invention also provides a control system for an unmanned aerial vehicle autonomously crossing a door frame based on binocular vision positioning, comprising a processor and a memory, the memory being used to store program instructions, and the processor being used to call the program instructions in the memory to execute a control method for an unmanned aerial vehicle autonomously crossing a door frame based on binocular vision positioning as described above.

[0086] Example 3

[0087] Based on the same inventive concept, the present invention also provides a control system for an unmanned aerial vehicle autonomously crossing a door frame based on binocular vision positioning, including a readable storage medium, on which a computer program is stored. When the computer program is executed, a control method for an unmanned aerial vehicle autonomously crossing a door frame based on binocular vision positioning as described above is implemented.

[0088] In specific implementation, the method proposed in the technical solution of the present invention can be implemented by technical personnel in this field using computer software technology to realize automatic operation process. System devices for implementing the method, such as computer-readable storage media storing the corresponding computer program of the technical solution of the present invention and computer equipment running the corresponding computer program, should also be within the scope of protection of the present invention.

[0089] The specific embodiments described herein are merely examples of the spirit of the present invention. Those skilled in the art may make various modifications or additions to the specific implementation cases described, or replace them with similar methods, without departing from the spirit of the present invention or exceeding the scope defined by the appended claims.

Claims

1. A method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning, characterized in that: The following steps are involved: Step 1: Use a binocular camera to capture an RGB scene image containing a door frame and perform histogram equalization processing; Step 2, the HSV channel threshold is used to obtain a binary image of the door frame range, and morphological processing is performed; Step 3, detect the center point of the corner point of the door frame and calculate the key point of crossing; Perform contour detection on the binary image after morphological processing to obtain the inner and outer contours of the door frame, and at the same time obtain the pixel positions of the eight corner points of the inner and outer contours of the door frame, respectively compare the pixel coordinate positions of the corner points with the pixel coordinate positions of the center point of the door frame, distinguish four groups of eight corner points, namely, the upper left, lower left, upper right, and lower right, respectively, calculate the average value of the pixel coordinates of each group of corner points, and obtain the pixel coordinates of the midpoint of the line connecting each group of corner points at the corresponding position, that is, the pixel coordinates of the center point of the corner point; the origin of the pixel coordinate system is located in the upper left corner of the image, the x-axis points to the horizontal direction of the image, increasing from left to right, and the y-axis points to the vertical direction of the image, increasing from top to bottom; The origin of the camera coordinate system is the same as the center of mass of the drone, and moves with the drone. The x-axis of the coordinate system points to the right of the drone head, the y-axis points downward, and the z-axis points to the direction of the drone head. Through the coordinate conversion between the pixel coordinate system and the camera coordinate system, the pixel coordinates of the center point of the corner point are converted to the camera coordinate system. The coordinate conversion formula is as follows: In the formula, x c ,y c 、z c They represent the coordinates of the center of the corner point in the camera coordinate system after coordinate transformation; d represents the depth value of the center of the corner point measured by the camera in the pixel coordinate system; f is a constant, representing the conversion of the depth map in millimeters to the actual distance in meters; x p ,y p Represent the pixel coordinates of the center point of the corner point; c x 、c y They represent the offset of the camera optical axis in the image in pixels; f x 、f y Represents the focal length of the camera in the x and y directions respectively; According to the coordinates of the center points of the two lower corner points in the camera coordinate system, the angle between the door frame and the x-axis of the camera coordinate system in the top-down perspective is calculated, which is denoted as α, that is: In the formula, z br Indicates the z coordinate of the center point of the lower right corner of the door frame in the camera coordinate system, z bl Indicates the z coordinate of the center point of the lower left corner of the door frame in the camera coordinate system, x br Indicates the x coordinate of the center point of the lower right corner of the door frame in the camera coordinate system, x bl Indicates the x-coordinate of the center point of the lower left corner of the door frame in the camera coordinate system; Use the average coordinates of the center points of the four corner points to get the coordinates of the center point of the door frame, and calculate the distance d from the center point of the door frame on the normal line of the door frame plane passing through the center point of the door frame w The two coordinates of the length are used as the hovering points before and after the drone passes through the door frame, recorded as crossing key point 1 and crossing key point 2. The normal equation is calculated as follows: z=z m (5) y-ym=tanα(x-xm) (6) Where α is the angle between the door frame and the x-axis of the camera coordinate system in the top-down perspective, x, y, and z represent any point on the normal line of the door frame plane passing through the center point of the door frame, and x m ,y m 、z m Represents the coordinate value of the center point of the door frame in the camera coordinate system. The position of the crossing key point is calculated by the coordinate value of the center point of the door frame and the angle α, that is: x1=x m -d w sinα (7) y1=y m (8) z1=z m +d w cosα (9) x2=x m +d w sinα (10) y2=y m (11) z2=z m -d w cosα (12) Where x1, y1, and z1 represent the coordinate values ​​of the crossing key point 1 in the camera coordinate system; x2, y2, and z2 represent the coordinate values ​​of the crossing key point 2 in the camera coordinate system; m ,y m 、z m Indicates the coordinate value of the center point of the door frame in the camera coordinate system; d w Indicates the distance between the key point of crossing and the center point of the door frame. The value can be set manually according to actual needs. Check whether the calculation of crossing key points 1 and 2 is correct. Let vector A be the vector from the center of the door frame to crossing key point 1, and vector B be the vector from the center of the door frame to the origin of the camera coordinate system. After doing the scalar product of vectors A and B, if the result is greater than 0, it means that the angle between the two vectors is less than 90°, and the values ​​of crossing key points 1 and 2 are kept unchanged; if the result is less than 0, it means that the angle between the two vectors is greater than 90°, and the coordinate values ​​of crossing key points 1 and 2 are exchanged. Step 4, convert the camera coordinates of the key points to the world coordinate system; Step 5: Plan a path to cross the door frame obstacle.

2. The method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning as claimed in claim 1, characterized in that: In step 2, the RGB image of the door frame taken by the camera is first converted to the HSV color space, and then the HSV image is thresholded for the H, S, and V channels, that is, the threshold range is set for the H, S, and V channels respectively, the pixels that meet the threshold range are retained and marked as white, and the pixels that are not in the threshold range are marked as black; the six threshold values ​​of the target HSV three channels are manually adjusted so that the shape of the door frame can be screened out from the background; after the HSV threshold screening, the image becomes a binary image in black and white that only shows the door frame range.

3. The method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning as claimed in claim 2, characterized in that: In step 2, morphological processing is performed on the binary image after HSV screening, that is, an opening operation is performed first and then a closing operation is performed. The opening operation is to erode first and then expand, and the closing operation is to expand first and then erode. The erosion operation can eliminate noise and erase part of the boundary contour of the image, so that the overall area of ​​the image is smaller than before. The expansion operation can increase the range of the target image and fill the holes, so that the overall area of ​​the image is expanded relative to before.

4. The method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning as claimed in claim 1, characterized in that: In step 4, the tf conversion tool in the ROS system is used to establish the conversion relationship between the body coordinate system and the world coordinate system. The origin of the world coordinate system is the position of the drone slam when building the map. The x-axis of the coordinate system points to the right of the drone head, the y-axis points to the direction of the head, and the z-axis points to the top of the drone. Since the camera is fixed on the drone, the distance deviation between the origin of the camera coordinate system and the origin of the body coordinate system is very small, so the camera coordinate system is regarded as the body coordinate system. In the ROS system, the real-time tf conversion relationship between the body coordinate system and the world coordinate system is published and subscribed, and the tf conversion tool is used to convert the coordinates of the crossing key point 1 and the crossing key point 2 in the camera coordinate system into the coordinate values ​​in the world coordinate system.

5. The method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning as claimed in claim 1, characterized in that: In step 5, the path planning adopts the control method of directly flying to the flight point released by the drone. After obtaining the coordinates of the crossing key points 1 and 2, the first target point is set to be above the coordinates of the current drone, and the second and third flight target points are the converted crossing key points 1 and 2 coordinates. When the drone hovers stably at the first target point, it flies to the second flight target point. After reaching the second flight target point, it continues to wait for the drone to hover stably, and then flies to the third flight target point. In this process, the drone completes the crossing of the door frame.

6. The method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning as claimed in claim 5, characterized in that: In step 5, the stability of the drone hovering is determined by the distance and time between the current position coordinates of the drone and the target position coordinates. The current position coordinates of the drone are obtained in real time through the topic representing the drone's posture information in the ROS system. Whenever the drone receives the command of the flight target point, the ROS system starts to calculate the distance between the current position of the drone and the target position. When the distance between the current position of the drone and the target position is less than the threshold d e After that, the timer starts to accumulate time. Whenever the distance between the current position of the drone and the target position is greater than this threshold, the timer time is reset to zero. When the timer time accumulates to the time threshold t, it is determined that the drone's hovering stability meets the standard, and the drone receives and executes the command to fly to the next target point.

7. A control system for autonomously crossing door frames of unmanned aerial vehicles based on binocular vision positioning, characterized in that: It includes a processor and a memory, the memory is used to store program instructions, and the processor is used to call the program instructions in the memory to execute a method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning as described in any one of claims 1 to 6.

8. A control system for autonomously passing through door frames of unmanned aerial vehicles based on binocular vision positioning, characterized in that: It includes a readable storage medium, on which a computer program is stored. When the computer program is executed, a method for controlling a drone to autonomously pass through a door frame based on binocular vision positioning as described in any one of claims 1 to 6 is implemented.

Images