Unmanned aerial vehicle navigation 3D printing method based on indoor scene positioning

Abstract

The invention discloses an unmanned aerial vehicle navigation 3D printing method based on indoor scene positioning. The method comprises the steps that 1, an infrared camera arranged strategically is used for collecting all-dimensional dynamic information of a flight area of an unmanned aerial vehicle; 2, inputting the acquired data into a pre-prepared computing system, processing the data, and computing the accurate position of the camera in the space; 3, the attitude and position of the unmanned aerial vehicle are dynamically adjusted through triangulation and beam adjustment, and it is ensured that the unmanned aerial vehicle can achieve accurate and stable fixed-point flight in a complex environment, especially under the disturbance condition. And 4, after accurate positioning is completed, the system conducts 3D printing modeling according to the target task, the printing task is accurately executed through the unmanned aerial vehicle, printing materials are deposited layer by layer, and a needed three-dimensional structure is completed. According to the system, the operation precision and safety of the unmanned aerial vehicle are improved, and a low-cost and high-precision unmanned aerial vehicle positioning and 3D printing solution is provided for the fields of indoor buildings, model manufacturing and the like.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle navigation and 3D printing, and specifically relates to a unmanned aerial vehicle navigation 3D printing method based on indoor scene positioning. Background Art

[0002] During the flight of drones, GPS (Global Positioning System), as a common positioning technology, has the problem of non-autonomy due to its reliance on external satellite signals. In the case of unfavorable environmental factors (such as building obstructions, indoor environments or bad weather), GPS signals may be lost or weakened, which in turn affects positioning accuracy. In addition, the accuracy of GPS is usually only a few meters, and even in some cases it can reach tens of meters, which is not accurate enough for precise navigation and is difficult to meet actual needs.

[0003] On the other hand, traditional 3D printing requires a lot of space. Traditional 3D printers usually require a fixed printing platform, and the size and space of the printing platform are usually fixed. For larger printed objects, traditional 3D printers require a very large space, which limits their application in certain scenarios; and traditional 3D printing is limited to flat printing: traditional 3D printers can generally only print on horizontal surfaces, and sufficient ground space needs to be provided for each object. If you need to print vertical or inaccessible areas (such as building facades), traditional 3D printers are not competent. Summary of the invention

[0004] In view of the problems of high price and low precision in traditional UAV navigation technology, the present invention proposes a UAV navigation 3D printing method based on indoor scene positioning.

[0005] To achieve the above object, the technical solution adopted by the present invention is:

[0006] A method for drone navigation 3D printing based on indoor scene positioning includes the following steps:

[0007] (1) Task definition and geometric modeling:

[0008] Before the task starts, the printing target is first defined and its geometric model is established. According to the geometric model of the task target, the path planning system divides the target task into multiple levels and designs the printing path of each layer layer by layer. The height of each layer is set to h. The printing task realizes the layered printing of the task by modeling the geometric shape of each layer.

[0009] (2) Path planning and printing trajectory design:

[0010] According to the task definition and geometric model, the system generates the printing path and flight path. The printing path is usually a circular trajectory. The printing trajectory of each layer is set according to the target radius and the speed of the nozzle. The nozzle moves evenly along the circular trajectory to ensure the uniformity of material deposition.

[0011] (3) Flight control and real-time path optimization:

[0012] During the printing process, due to external interference, the system needs to optimize the flight path and nozzle trajectory in real time. Through real-time feedback and position tracking, the system adjusts the flight speed and nozzle angle in real time according to the error between the actual position of the drone and the target trajectory to ensure that the nozzle moves accurately along the designed trajectory.

[0013] As a further improvement of the present invention, in step (1), the task definition and geometric modeling of 3D printing includes: achieving layered printing of the task by modeling the geometric shape of each layer.

[0014] As a further improvement of the present invention, in the step (2), the path planning and printing trajectory design include nozzle trajectory design, the nozzle deposits material along the path trajectory to ensure that the material of each layer is evenly distributed to avoid printing errors; height control and inter-layer adjustment, the system accurately tracks the position of the drone through an infrared camera positioning system to ensure that its vertical height is accurate to the micron level of the layer height, and adjusts the flight altitude through the path planning system to ensure the accurate docking of the printing position of each layer.

[0015] As a further improvement of the present invention, in step (2),

[0016] (1) Constraints:

[0017] During the printing process, due to possible external interference, the path needs to be optimized in real time. At this time, it is assumed that the target object is on the circular trajectory of the i-th layer with a radius of R i , then the three-dimensional coordinates M(x,y,z) of the target object are expressed as:

[0018] x=R i ·cosθ

[0019] y=R i sinθ

[0020] z=h i .

[0021] (2) Optimization objectives:

[0022] The pose T of all cameras is obtained by optimization i =(R i ,t i ) and the positions of all three-dimensional points X j, so that the error between the projection results of all cameras and the actual observed image feature points is minimized, that is, the following objective function is minimized:

[0023]

[0024] The significance of the objective function is to minimize the distance between the feature points observed by each camera and the projection points calculated by the currently estimated camera parameters and 3D point positions;

[0025] Where N cams Indicates the number of cameras; N points represents the number of three-dimensional points; p ij represents the actual observation of the jth feature point captured by the i-th camera; Indicates that according to the current camera pose T i and the 3D point position X j The calculated projection points; ρ(·) is a robust kernel function used to reduce the impact of outliers. First, assume that ρ(x) = x;

[0026] (3) Error function:

[0027] The residual of each observation point is defined as:

[0028] r ij =p ij -p ij (T i ,X j )

[0029] (4) The goal is to minimize the following error function:

[0030]

[0031] (5) Gauss-Newton method iterative optimization;

[0032] S1. Linearized residual function;

[0033] Using Taylor expansion, the error function r ij In the current parameter {T i ,X j Linearization around}:

[0034]

[0035] in:

[0036] ●J ij is the Jacobian matrix of the residual function with respect to the optimization parameters, which is divided into two parts:

[0037] Jacobian for the camera pose;

[0038] Jacobian for 3D point positions;

[0039] S2. Construct a linearized objective function;

[0040] After linearization, the objective function of the sum of squared errors becomes:

[0041]

[0042] After expansion, it can be written as:

[0043]

[0044] in:

[0045] ●J is the global Jacobian matrix of all observation points

[0046] S3. Hessian matrix and gradient;

[0047] H=J T J,g=J T r

[0048] S4. Update amount Δθ;

[0049]

[0050] Where: Δθ includes ΔT i and ΔX j

[0051] S5. Parameter update;

[0052] After solving Δθ, new parameters are obtained through the update rule:

[0053] T i,new =T i,old +ΔT i

[0054] X j,new =X j,old +ΔX j .

[0055] The advantages of the present invention are:

[0056] Compared with the prior art, the significant advantage of the present invention is that it improves the algorithm of drone positioning and reduces positioning errors, so that it can quickly and accurately respond to control instructions. The drone 3D printing system usually uses a lightweight drone platform and realizes the printing function through a simple print head and nozzle design. This low-cost hardware combined with efficient software control greatly reduces the construction cost of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 It is a flow chart of the method for 3D printing of unmanned aerial vehicle navigation based on indoor scene positioning of the present invention;

[0058] Figure 2 It is a flowchart of the steps of positioning the drone in the present invention;

[0059] Figure 3 It is a software interface diagram of the drone of the present invention being recognized by the camera in real time;

[0060] Figure 4 is a schematic diagram of the shape of the drone in the present invention;

[0061] Figure 5 It is a trajectory diagram of the UAV positioning in the present invention. DETAILED DESCRIPTION

[0062] Assume that there is a scene captured by multiple cameras, and the goal is to optimize the position of 3D points and the camera pose based on the observations of these cameras, and to print a cylinder with a height H of 2 meters and a radius of 0.5 meters. Please refer to the flowchart as follows Figure 1 For the schematic diagram of the drone, please refer to Figure 4 shown.

[0063] We use the most common RGB camera, such as the PS3 Eye camera. On this basis, we remove the infrared filter in the camera, add a red transparent plastic sheet, and then place three red LED lights on the drone in a triangular pattern. Through the feature point detection method, the image taken by each camera will project a set of two-dimensional feature points.

[0064] Now let's define the mission and perform geometric modeling on the drone. First, we need to define the mission goal, which is to print a cylinder. The geometric model of the cylinder includes the height H and the radius R. Based on this, the path planning system divides the mission into multiple levels and designs the path based on the geometry of each layer. The height of the cylinder is 2 meters and the radius is 0.5 meters. The printing will be divided into 400 layers, and the height of each layer is 5 mm.

[0065] Path planning mainly involves the flight trajectory of the drone and the movement trajectory of the printing nozzle. In order to ensure printing accuracy and stability, we use circular trajectory planning and print layer by layer. During the printing process of each layer, the nozzle deposits material along the predetermined circular trajectory to ensure that each layer is printed evenly.

[0066] Height adjustment between layers: After each layer of printing is completed, the drone needs to fly up to a certain height to complete the printing task of the next layer. Assuming that the height of each layer is 5mm, the flight path passes through InfraredThe camera's positioning system tracks the drone's altitude changes in real time, ensuring that the flight altitude accurately matches the height of each printed layer, avoiding misalignment of printing caused by deviations during flight.

[0067] Nozzle trajectory design: The printing path of each layer is designed in advance as a circular path through the host computer system. The nozzle needs to be deposited along this circular trajectory to ensure uniform printing layers. The trajectory of the nozzle is determined by the target radius R and the printing speed. The movement speed of the nozzle on the circular trajectory remains consistent to ensure the deposition quality of each layer. Based on this path design, we combine positioning algorithms such as triangulation and beam adjustment to achieve precise adjustment of the real-time position of the drone and the nozzle trajectory. Specifically, triangulation and beam adjustment can effectively correct the flight path and nozzle position of the drone to ensure that the nozzle accurately deposits materials along the circular trajectory.

[0068] The specific path design is as follows:

[0069]

[0070] in, is the radius of the current layer, θ is the angle, ranging from 0 to 2π;

[0071] During the printing process, due to possible external interference (such as air flow, drone control accuracy error, etc.), the path needs to be optimized in real time. At this time, it is assumed that the target object (nozzle) is on the circular trajectory of the i-th layer with a radius of R i , then the theoretical three-dimensional coordinates M(x, y, z) of the target object can be expressed as:

[0072]

[0073]

[0074] z=h i

[0075] Then use the triangulation method to restore the three-dimensional position of the target object. There are two cameras C1 and C2, which observe the same target object. Each camera captures the two-dimensional coordinates of the target object p1 = (u1, v1) and p2 (u2, v2), as well as the camera's internal and external parameter information (e.g., focal length, rotation matrix, translation matrix, etc.). Our goal is to calculate the position of the target object in three-dimensional space X = (X, Y, Z) through the projection coordinates of these two-dimensional points.

[0076] (1) Camera projection model

[0077] The camera projection model can be represented by the projection matrix. Assume that the camera's intrinsic parameter matrix is ​​K and the extrinsic parameter matrix is ​​[R i |ti ], the projection process of the camera can be expressed as:

[0078] p i =K[R i t i ]X

[0079] in:

[0080] ●p i =(u i ,v i ,1) is the two-dimensional coordinate captured by the i-th camera

[0081] ●X=(X,Y,Z,1) is the three-dimensional coordinates of the target object

[0082] ●K is the camera's intrinsic parameter matrix (including focal length, principal point, etc.)

[0083] ●[R i |t i ] is the external parameter of the i-th camera, indicating the position and attitude of the camera, R i is the rotation matrix, indicating the direction of the camera, t i is the translation matrix, representing the position of the camera.

[0084] (1) Problem Analysis

[0085] Using the two-dimensional coordinates obtained from multiple viewpoints, we can use the least squares method to solve the three-dimensional coordinates of the target object.

[0086] At this point we have two-dimensional projections of two perspectives, p1 = (u1, v1) corresponds to camera C1, p2 = (u2, v2) corresponds to camera C2, and the camera's intrinsic parameter matrix is

[0087] in:

[0088] ●f x and f y is the horizontal and vertical focal length of the camera

[0089] ●c x and c y are the coordinates of the principal point of the camera image (usually the center of the image)

[0090] (2) Basic matrix F

[0091] The basic matrix F is a 3×3 matrix that describes the geometric relationship between two cameras. It is calculated using the internal and external parameters of the camera. The basic matrix relationship is:

[0092]

[0093] in:

[0094] ●p1 and p2 are the 2D coordinates of corresponding points in the two perspectives.

[0095] ●F is the basic matrix, which contains the relative position and attitude information of the camera (the camera's rotation matrix R and translation vector t)

[0096] (3) Epipolar constraint

[0097] We can get the epipolar line of the target point in the second perspective through the basic matrix F. Given the point p1 in the first camera, the epipolar line l2 in the second perspective can be calculated by the following formula:

[0098] l2=Fp1

[0099] Using this epipolar line, you can determine the possible locations of the target points in the second view. Epipolar geometry provides us with matching constraints between the two images, helping us reduce the search space for matching.

[0100] (4) Calculate the depth Z

[0101] The parallax d is the offset of an object between two cameras. Calculating the parallax is one of the key steps in triangulation. Assuming that the horizontal parallax between camera C1 and camera C2 is d = u1-u2, the depth Z can be calculated by the following formula:

[0102]

[0103] in:

[0104] f is the focal length of the camera (which can be obtained from the camera's intrinsic parameter matrix K)

[0105] ●b is the baseline distance of the cameras (the horizontal distance between the two cameras)

[0106] ●d=u1-u2 is the disparity, that is, the difference in the horizontal coordinates of the target object in the two images.

[0107] (5) Restore the three-dimensional coordinates of the target object

[0108] According to the camera's projection model, the 3D coordinates X = (X, Y, Z) of the target object can be recovered from the 2D coordinates (u, v) and depth Z by the following formula:

[0109]

[0110] Z=Z

[0111] in:

[0112] ●u and v are the 2D coordinates of the target object in the image;

[0113] ●c x and c y are the principal point coordinates of the camera;

[0114] ●f x and f y is the focal length of the camera in the horizontal and vertical directions;

[0115] So far, the coordinates X of the drone have been preliminarily calculated.

[0116] The next goal is to optimize the camera's position and the position of each feature point in 3D space so that the error between these projected points and the actual observed feature points is minimized. For the optimization results, please refer to Figure 3 As shown.

[0117] Optimization goal: We hope to obtain the pose T of all cameras through optimization. i =(R i ,t i ) and the positions of all three-dimensional points X j , so that the error between the projection results of all cameras and the actual observed image feature points is minimized. That is, the following objective function (cost function) is minimized:

[0118]

[0119] The significance of the objective function is to minimize the distance between the feature points observed by each camera and the projection points calculated by the current estimated camera parameters and 3D point positions.

[0120] N cams Indicates the number of cameras; N points represents the number of three-dimensional points; p ij represents the actual observation of the jth feature point captured by the i-th camera; Indicates that according to the current camera pose T i and the 3D point position X j The calculated projection points; ρ(·) is a robust kernel function used to reduce the impact of outliers (it can be assumed that ρ(x) = x).

[0121] Error function: The residual of each observation point is defined as:

[0122]

[0123] The goal is to minimize the following error function (cost function):

[0124]

[0125] Gauss-Newton iterative optimization

[0126] (1) Linearized residual function

[0127] Using Taylor expansion, the error function r ij In the current parameter {T i ,X j Linearization around}:

[0128]

[0129] in:

[0130] ●J ij is the Jacobian matrix of the residual function with respect to the optimization parameters, which is divided into two parts:

[0131] Jacobian for the camera pose

[0132] Jacobian for the position of a 3D point

[0133] (2) Constructing a linearized objective function

[0134] After linearization, the objective function of the sum of squared errors becomes:

[0135]

[0136] After expansion, it can be written as:

[0137]

[0138] in:

[0139] ●J is the global Jacobian matrix of all observation points

[0140] (3) Hessian Matrix and Gradient

[0141] H=J T J,g=J T r

[0142] (4) Update amount Δθ

[0143] HΔθ=-g

[0144] Where: Δθ includes ΔT i and ΔX j

[0145] (5) Parameter update

[0146] After solving Δθ, new parameters are obtained through the update rule:

[0147] T i,new =T i,old +ΔT i

[0148] Xj,new =X j,old +ΔX j

[0149] By repeating steps (1) to (5), the complexity of the algorithm is reduced by using epipolar geometry, and the joint optimization of camera pose and 3D point position is achieved by using bundle adjustment, which gradually reduces the projection error and improves the accuracy of UAV navigation and positioning.

[0150] Communicate through the esp32 chip and use double-layer nested PID to continuously adjust the position of the drone. For the positioning process, please refer to Figure 2 shown.

[0151] By minimizing this error function, we not only optimize the position of the target object (nozzle), but also ensure that the drone and nozzle can accurately print along the predetermined circular trajectory. For specific trajectories, please refer to Figure 5 shown.

[0152] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A drone navigation 3D printing method based on indoor scene positioning, characterized in that: The following steps are involved: (1) Task definition and geometric modeling: Before the task starts, the printing target is first defined and its geometric model is established. According to the geometric model of the task target, the path planning system divides the target task into multiple levels and designs the printing path of each layer layer by layer. The height of each layer is set to h. The printing task realizes the layered printing of the task by modeling the geometric shape of each layer. (2) Path planning and printing trajectory design: According to the task definition and geometric model, the system generates the printing path and flight path. The printing path is usually a circular trajectory. The printing trajectory of each layer is set according to the target radius and the speed of the nozzle. The nozzle moves evenly along the circular trajectory to ensure the uniformity of material deposition. (3) Flight control and real-time path optimization: During the printing process, due to external interference, the system needs to optimize the flight path and nozzle trajectory in real time. Through real-time feedback and position tracking, the system adjusts the flight speed and nozzle angle in real time according to the error between the actual position of the drone and the target trajectory to ensure that the nozzle moves accurately along the designed trajectory.

2. The method for 3D printing based on indoor scene positioning of unmanned aerial vehicle navigation according to claim 1, characterized in that: In the step (1), the task definition and geometric modeling of 3D printing include: achieving layered printing of the task by modeling the geometric shape of each layer.

3. The method for 3D printing based on indoor scene positioning and navigation of a drone according to claim 1, characterized in that: In the step (2), path planning and printing trajectory design include nozzle trajectory design, and the nozzle deposits material along the path trajectory to ensure that the material of each layer is evenly distributed; the system accurately tracks the position of the drone through an infrared camera positioning system to ensure the accurate docking of the printing position of each layer.

4. The method for 3D printing based on indoor scene positioning and navigation of a drone according to claim 1, characterized in that: In the step (3), (1) Constraints: During the printing process, due to possible external interference, the path needs to be optimized in real time. At this time, it is assumed that the drone is on a circular trajectory on the i-th layer with a radius of R i , then the three-dimensional coordinates M(x,y,z) of the target object are expressed as: x=R i ·cosθ y=R i ·sinθ z=h i 。 (2) Optimization objectives: The pose T of all cameras is obtained by optimization i =(R i ,t i ) and the positions of all three-dimensional points X j , so that the error between the projection results of all cameras and the actual observed image feature points is minimized, that is, the following objective function is minimized: The significance of the objective function is to minimize the distance between the feature points observed by each camera and the projection points calculated by the currently estimated camera parameters and 3D point positions; Where N cams Indicates the number of cameras; N points represents the number of three-dimensional points; p ij represents the actual observation of the jth feature point captured by the i-th camera; Indicates that according to the current camera pose T i and the 3D point position X j The calculated projection points; ρ(·) is a robust kernel function used to reduce the impact of outliers; (3) Error function: The residual of each observation point is defined as: r ij =p ij -p ij (T i ,X j ) (4) The goal is to minimize the following error function: (5) Gauss-Newton method iterative optimization; S1. Linearized residual function; Using Taylor expansion, the error function r ij In the current parameter {T i ,X j Linearization is performed around}: in: ·J ij is the Jacobian matrix of the residual function with respect to the optimization parameters, which is divided into two parts: · Jacobian for the camera pose; · Jacobian for 3D point positions; S2. Construct a linearized objective function; After linearization, the objective function of the sum of squared errors becomes: After expansion, it can be written as: in: J is the global Jacobian matrix of all observation points S3. Hessian matrix and gradient; H=J T J,g=J T r S4. Update amount Δθ; HΔθ=-g Where: Δθ includes ΔT i and ΔX j S5. Parameter update; After solving Δθ, new parameters are obtained through the update rule: T i,new =T i,old +ΔT i X j,new =X j,old +ΔX j 。

Images