CN112445242B - Method, device, equipment and storage medium for tracking route - Google Patents
Method, device, equipment and storage medium for tracking route Download PDFInfo
- Publication number
- CN112445242B CN112445242B CN201910797686.4A CN201910797686A CN112445242B CN 112445242 B CN112445242 B CN 112445242B CN 201910797686 A CN201910797686 A CN 201910797686A CN 112445242 B CN112445242 B CN 112445242B
- Authority
- CN
- China
- Prior art keywords
- route
- point
- real
- grid
- aerial vehicle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 230000001133 acceleration Effects 0.000 claims abstract description 19
- 238000010845 search algorithm Methods 0.000 claims abstract description 19
- 230000006870 function Effects 0.000 claims description 73
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000010276 construction Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 7
- 230000004807 localization Effects 0.000 claims description 6
- 230000001419 dependent effect Effects 0.000 claims description 5
- 238000004422 calculation algorithm Methods 0.000 claims description 4
- 230000003287 optical effect Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 239000013307 optical fiber Substances 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/12—Target-seeking control
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The embodiment of the invention discloses a method, a device, equipment and a storage medium for tracking a route. Wherein the method comprises the following steps: acquiring a real-time positioning point of the unmanned aerial vehicle; constructing a grid table of termination points and duration time, wherein the termination points are positioned on the airlines; calculating tracking curves respectively corresponding to all grid cells in the termination point and duration grid table according to the motion state parameters matched with the real-time positioning points by adopting a grid search algorithm; acquiring a target curve with the minimum function value of the cost function; and calculating the motion state parameter of the target curve at the next moment, and determining the tracking control quantity for controlling the unmanned aerial vehicle to track the route by using the motion state parameter. The embodiment of the invention solves the problems of overlarge control quantity or abrupt change caused by the interference of the position, the speed and the acceleration of the machine body due to various reasons in the prior art, and can lead the unmanned aerial vehicle to track the upper route and fly by attaching the route in a smooth and very small shaking way under any position state.
Description
Technical Field
The embodiment of the invention relates to unmanned aerial vehicle technology, in particular to a method, a device, equipment and a storage medium for tracking a route.
Background
The existing course tracking control method is related to the representation form of the course. If the route is a straight route, the general route speed is distributed into trapezoid change speed, the forward control quantity can be calculated according to the current position of the machine body in the route, and the lateral control quantity can be calculated according to the vertical distance between the machine body and the route. If the route is a curve route, the position, speed and acceleration required at the moment can be calculated according to the independent variables such as time, and the machine body at the moment can be directly controlled according to the calculated position, speed and acceleration at the moment.
In the prior art, the tracking control mode aiming at the linear route is too simple and cannot be suitable for curve scenes; in practical application, the position, speed and acceleration of the machine body at a certain moment and the position, speed and acceleration calculated according to the curve are overlarge, so that the control quantity is overlarge or suddenly changed, and dangerous working conditions are caused.
Disclosure of Invention
The embodiment of the invention provides a method, a device, equipment and a storage medium for tracking a route, which are used for tracking an upper route in a smooth and very small-shake mode under any position state of an unmanned aerial vehicle and enabling the unmanned aerial vehicle to fly in a mode of attaching to the route.
In a first aspect, an embodiment of the present invention provides a method for tracking an air route, including:
acquiring a real-time positioning point of the unmanned aerial vehicle in the process of flying the unmanned aerial vehicle along a set route;
according to the position relation between the real-time positioning points and the airlines, constructing a grid table of termination points and duration time according to the set position interval and the set time interval, wherein the termination points are positioned on the airlines;
calculating tracking curves respectively corresponding to all grid cells in the termination point and duration grid table according to the motion state parameters matched with the real-time positioning points by adopting a grid search algorithm;
acquiring a target curve with the minimum function value of the cost function from tracking curves corresponding to the grid cells respectively;
and calculating the motion state parameter of the target curve at the next moment, and determining the tracking control quantity for controlling the unmanned aerial vehicle to track the route by using the motion state parameter.
In a second aspect, an embodiment of the present invention further provides a tracking apparatus for an air line, including:
The positioning point acquisition module is used for acquiring real-time positioning points of the unmanned aerial vehicle in the process of flying the unmanned aerial vehicle along a set route;
the grid table construction module is used for constructing a grid table of termination points and duration time according to the position relation between the real-time positioning points and the airlines and the set position interval and the set time interval, wherein the termination points are positioned on the airlines;
the curve calculation module is used for calculating tracking curves respectively corresponding to the termination points and each grid unit in the continuous time grid table according to the motion state parameters matched with the real-time positioning points by adopting a grid search algorithm;
the curve determining module is used for acquiring a target curve with the minimum function value of the cost function from tracking curves corresponding to the grid units respectively;
the control quantity determining module is used for calculating the motion state parameter of the target curve at the next moment and determining the tracking control quantity for controlling the unmanned aerial vehicle to track the route by using the motion state parameter.
In a third aspect, an embodiment of the present invention further provides a computer device, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor, where the processor executes the program to implement a method for tracking a route according to the embodiment of the present invention.
In a fourth aspect, embodiments of the present invention further provide a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements a method for tracking a route according to embodiments of the present invention.
According to the technical scheme, the grid table of the termination point and the duration is constructed according to the position relation between the real-time positioning points and the airlines and the set position interval and the set time interval, then a grid search algorithm is adopted, tracking curves respectively corresponding to all grid units in the grid table of the termination point and the duration are calculated according to at least one motion state parameter matched with the real-time positioning points, and in the tracking curves respectively corresponding to all grid units, a target curve with the minimum function value of a cost function is obtained, the motion state parameter of the target curve at the next moment is calculated, the motion state parameter is used for determining the tracking control quantity for controlling the unmanned aerial vehicle to track the airlines, the problem that the control quantity is overlarge or suddenly changes due to interference of the position, the speed and the acceleration of the unmanned aerial vehicle in the prior art is solved, the target line point matched with the real-time positioning points and the airlines can be determined according to the position relation between the real-time positioning points by adopting the grid search algorithm, and the target curve connected with the target line points and the positioning points is determined, and the unmanned aerial vehicle can track the unmanned aerial vehicle in a smooth and very small flight line.
Drawings
FIG. 1 is a flowchart of a method for tracking a route according to an embodiment of the present invention;
FIG. 2 is a flowchart of a method for tracking a route according to a second embodiment of the present invention;
fig. 3 is a schematic structural diagram of a tracking device for an air route according to a third embodiment of the present invention;
fig. 4 is a schematic structural diagram of a computer device according to a fourth embodiment of the present invention.
Detailed Description
The invention is described in further detail below with reference to the drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the invention and are not limiting thereof.
It should be further noted that, for convenience of description, only some, but not all of the matters related to the present invention are shown in the accompanying drawings. Before discussing exemplary embodiments in more detail, it should be mentioned that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although a flowchart depicts operations (or steps) as a sequential process, many of the operations can be performed in parallel, concurrently, or at the same time. Furthermore, the order of the operations may be rearranged. The process may be terminated when its operations are completed, but may have additional steps not included in the figures. The processes may correspond to methods, functions, procedures, subroutines, and the like.
Example 1
Fig. 1 is a flowchart of a method for tracking an air line according to an embodiment of the present invention, where the method may be performed by an apparatus for tracking an air line according to an embodiment of the present invention, and the apparatus may be implemented in software and/or hardware, and may be generally integrated in an unmanned aerial vehicle. As shown in fig. 1, the method in this embodiment specifically includes:
step 101, acquiring a real-time positioning point of the unmanned aerial vehicle in the process of flying the unmanned aerial vehicle along a set route.
Wherein, the route is a preset unmanned aerial vehicle route. The route may be a curved route or a straight route. The real-time locating point is the current waypoint position of the unmanned aerial vehicle. Optionally, the real-time location point of the unmanned aerial vehicle is obtained by a global positioning system (Global Positioning System, GPS) on the unmanned aerial vehicle.
And 102, constructing a grid table of termination points and duration time according to the position relation between the real-time positioning points and the airlines and the set position interval and the set time interval, wherein the termination points are positioned on the airlines.
In a specific example, before the unmanned aerial vehicle obtains the real-time positioning point of the unmanned aerial vehicle in the flight process along the set route, the method further comprises: constructing a comparison table of route independent variables and waypoint positions according to the route; the route is a route function taking the route independent variable as the independent variable and the waypoint position as the dependent variable. According to the position relation between the real-time positioning points and the navigation lines and according to the set position interval and the set time interval, constructing an ending point and duration grid table can comprise: searching at least one waypoint position matched with the real-time locating point in a comparison table of the route independent variable and the waypoint position, and determining a route independent variable starting point according to the searched waypoint position; and constructing an end point and duration grid table according to the route independent variable starting point, the set position interval and the set time interval.
And 103, calculating tracking curves respectively corresponding to the termination points and each grid unit in the duration grid table according to the motion state parameters matched with the real-time positioning points by adopting a grid search algorithm.
Optionally, the at least one motion state parameter matched with the real-time localization point may include: position information matching the real-time anchor point, velocity information matching the real-time anchor point, and acceleration information matching the real-time anchor point.
In a specific example, a grid search algorithm is adopted, and according to at least one motion state parameter matched with a real-time positioning point, a tracking curve corresponding to each grid cell in the grid table of the termination point and the duration time is calculated, which may include: and calculating tracking curves respectively corresponding to the termination point and each grid cell in the continuous time grid table according to the set calculation formula, the position information matched with the real-time positioning point, the speed information matched with the real-time positioning point, the acceleration information matched with the real-time positioning point, the values of the route function and each grid cell, the values of the first derivative of the route function and each grid cell, and the values of the second derivative of the route function and each grid cell.
And 104, acquiring a target curve with the minimum function value of the cost function from tracking curves corresponding to the grid cells.
And respectively calculating the function values of the cost functions corresponding to the tracking curves, and selecting the tracking curve with the smallest function value as the target curve.
And 105, calculating a motion state parameter of the target curve at the next moment, and determining a tracking control quantity for controlling the unmanned aerial vehicle tracking route by using the motion state parameter.
Optionally, position information, speed information, and acceleration information of the target curve at the next time are calculated. And taking the position information, the speed information and the acceleration information as input information of the unmanned aerial vehicle controller at the next moment, and inputting the input information into the unmanned aerial vehicle controller to obtain the tracking control quantity which is output by the unmanned aerial vehicle controller at the next moment and is used for controlling the unmanned aerial vehicle to track the route. Alternatively, the unmanned controller uses a hybrid control method of acceleration feedforward control plus proportional-integral-derivative (Proportion Integral Differential, PID) control.
Optionally, after calculating at least one motion state parameter of the target curve at the next moment and determining a tracking control amount for controlling the unmanned aerial vehicle to track the free route using the motion state parameter, the method may further include: after the waiting time is set at intervals, the operation of acquiring the real-time positioning point of the unmanned aerial vehicle in the process that the unmanned aerial vehicle flies along the set free route is returned until the unmanned aerial vehicle flies to the tail end of the free route.
The waiting time can be set according to service requirements. Setting a waiting time at intervals, and repeatedly executing the steps 101-105 until the unmanned aerial vehicle flies to the tail end of the route.
The embodiment of the invention provides a tracking method of a route, which comprises the steps of constructing an ending point and duration grid table according to a set position interval and a set time interval according to the position relation between real-time positioning points and the route, then adopting a grid search algorithm, calculating tracking curves respectively corresponding to grid units in the ending point and duration grid table according to at least one motion state parameter matched with the real-time positioning points, acquiring a target curve with the minimum function value of a cost function in the tracking curves respectively corresponding to the grid units, calculating the motion state parameter of the target curve at the next moment, determining the tracking control quantity for controlling an unmanned aerial vehicle to track the route by using the motion state parameter, solving the problems that the position, the speed and the acceleration of the unmanned aerial vehicle are disturbed and the control quantity is overlarge or suddenly changed due to various reasons in the prior art, constructing the ending point and the duration grid table according to the position relation between the real-time positioning points and the route, adopting the grid search algorithm, and determining the target route point matched with the real-time positioning points and the target curve for connecting the target route point and the real-time positioning points, thereby enabling the unmanned aerial vehicle to track in a smooth and small-jitter flight mode.
Example two
Fig. 2 is a flowchart of a tracking method of an air route according to a second embodiment of the present invention. This embodiment may be combined with each of the alternatives in one or more embodiments, and in this embodiment, before the acquiring the real-time positioning point of the unmanned aerial vehicle during the flight of the unmanned aerial vehicle along the set route, the method may further include: constructing a comparison table of route independent variables and waypoint positions according to the route; the route is a route function taking the route independent variable as the independent variable and the waypoint position as the dependent variable.
And constructing an ending point and duration grid table according to the position relation between the real-time positioning points and the navigation lines and the set position interval and the set time interval, and the method can comprise the following steps: searching at least one waypoint position matched with the real-time locating point in a comparison table of the route independent variable and the waypoint position, and determining a route independent variable starting point according to the searched waypoint position; and constructing an end point and duration grid table according to the route independent variable starting point, the set position interval and the set time interval.
As shown in fig. 2, the method in this embodiment specifically includes:
step 201, constructing a comparison table of route independent variables and waypoint positions according to a route; the route is a route function taking the route independent variable as the independent variable and the waypoint position as the dependent variable.
Optionally, constructing a comparison table of route arguments and waypoint positions according to the route may include: substituting the current value of the route independent variable into a route function matched with the route, and calculating to obtain the position of the waypoint matched with the current value, wherein the route independent variable has a set initial value; adding the current value and the waypoint position matched with the current value into a comparison table of the route independent variable and the waypoint position; and updating the current value of the route independent variable by using the set increment value of the route independent variable, returning to execute the operation of substituting the current value of the route independent variable into a route function matched with the route, and calculating to obtain the position of the waypoint matched with the current value until the preset ending processing condition is met.
Alternatively, the route arguments may include: arc length or time. The course function is a function for calculating the waypoint location based on the course argument. Optionally, the waypoint position may be a position of the unmanned aerial vehicle in a set space rectangular coordinate system.
Step 202, acquiring a real-time positioning point of the unmanned aerial vehicle in the process of flying the unmanned aerial vehicle along a set route.
And 203, searching at least one waypoint position matched with the real-time locating point in a comparison table of the route independent variable and the waypoint position, and determining a route independent variable starting point according to the searched waypoint position.
Optionally, searching at least one waypoint position matched with the real-time locating point in the comparison table of the waypoint position and the waypoint independent variable, and determining the waypoint independent variable starting point according to the searched waypoint position, may include: searching a first navigation point position and a second navigation point position which are closest to the real-time positioning point in a comparison table of the navigation line independent variable and the navigation point position. And determining a route independent variable starting point by adopting a dichotomy algorithm according to the first route independent variable corresponding to the first route position and the second route independent variable corresponding to the second route point.
Specifically, determining, by a dichotomy algorithm, a starting point of the route argument according to a first route argument corresponding to the first route location and a second route argument corresponding to the second route location may include: and calculating an average value of the first route independent variable and the second route independent variable, marking the average value as a third route independent variable, substituting the third route independent variable into a route function matched with the route, and calculating to obtain a third navigation point position. And judging whether the third navigation point position is consistent with the real-time positioning point. And if the third waypoint position is consistent with the real-time locating point, determining the third route independent variable as a route independent variable starting point. If the third waypoint location is inconsistent with the real-time locating point, searching two closest waypoint locations from the first waypoint location, the second waypoint location and the third waypoint location. And calculating an average value of route independent variables corresponding to the two waypoint positions, marking the average value as a fourth route independent variable, substituting the fourth route independent variable into a route function matched with the route, and calculating to obtain a fourth waypoint position. And judging whether the fourth navigation point position is consistent with the real-time positioning point. And if the fourth waypoint position is consistent with the real-time locating point, determining the fourth route independent variable as a route independent variable starting point. If the fourth waypoint position is inconsistent with the real-time locating point, continuing to calculate according to the calculation mode according to the two closest waypoint positions and the fourth waypoint position until the waypoint position consistent with the real-time locating point is finally found, and determining the route independent variable corresponding to the waypoint position as a route independent variable starting point.
And 204, constructing an ending point and duration grid table according to the route argument starting point, the set position interval and the set time interval.
Optionally, constructing the end point and duration grid table according to the route argument start point, the set position interval and the set time interval may include: to be used for<Δs，ΔT>For intervals, generate a plurality of<s k ，T k >The end points and duration grid table is formed:
<s 0 ，T 0 >…<s k ，T 0 >…<s n ，T 0 >
<s 0 ，T 1 >…<s k ，T 1 >…<s n ，T 1 >
<s 0 ，T k >…<s k ，T k >…<s n ，T k >
…
<s 0 ，T n >…<s k ，T n >…<s n ，T n >
wherein ,sk ＝s 0 +Δs*k，T k ＝T 0 +ΔT.k, where k ε [0, n]，s 0 T is determined according to the position of the starting point of the route independent variable 0 For unmanned aerial vehicle moves to AND s from real-time locating point k The corresponding waypoint location's time interval set point.
Alternatively, s 0 S is determined according to the position of the starting point of the route independent variable 0 Is greater than the route argument starting point. T (T) 0 Is a preset time interval.
Step 205, calculating tracking curves corresponding to the termination points and each grid unit in the duration grid table respectively according to the motion state parameters matched with the real-time positioning points by adopting a grid search algorithm.
Optionally, a grid search algorithm is adopted, and according to a motion state parameter matched with a real-time positioning point, a tracking curve corresponding to each grid cell in the grid table of the termination point and the duration time is calculated, which may include:
Calculating each grid cell in the endpoint and duration grid table according to the following formula<s k ，T k >Tracking curves respectively corresponding to
wherein ,
wherein ,p0 For matching position information, v, with real-time location points 0 For speed information, a, matching real-time localization points 0 Acceleration information p for matching with real-time anchor point f At s for the course function k Lower value v f The first derivative of the course function is s k Lower value, a f The second derivative of the course function is s k The following values.
And 206, acquiring a target curve with the minimum function value of the cost function from tracking curves corresponding to the grid cells.
Optionally, in the tracking curves corresponding to the grid cells respectively, obtaining the target curve with the smallest function value of the cost function may include: the formula is calculated through a cost function:computing and grid cells<s k ，T k >The function value J of the cost function corresponding to the tracking curves of the model (C); wherein k is E [0, n]Alpha, beta and gamma are respectively connected with grid cells<s k ，T k >And (5) association.
And respectively calculating the function value of the cost function corresponding to each tracking curve, and selecting the tracking curve with the smallest function value as the target curve.
Step 207, calculating a motion state parameter of the target curve at the next moment, and determining a tracking control quantity for controlling the unmanned aerial vehicle tracking route by using the motion state parameter.
Optionally, position information, speed information, and acceleration information of the target curve at the next time are calculated. And taking the position information, the speed information and the acceleration information as input information of the unmanned aerial vehicle controller at the next moment, and inputting the input information into the unmanned aerial vehicle controller to obtain the tracking control quantity which is output by the unmanned aerial vehicle controller at the next moment and is used for controlling the unmanned aerial vehicle to track the route. For example, the next time is Δt, Δt is brought into the target curve, and position information, speed information, and acceleration information (p (Δt) v (Δt) a (Δt)) of the target curve at the next time are calculated. And (p (delta t) v (delta t) a (delta t)) is used as input information of the unmanned aerial vehicle controller at the next moment and is input into the unmanned aerial vehicle controller, so that tracking control quantity for controlling the unmanned aerial vehicle to track the route at the next moment, which is output by the unmanned aerial vehicle controller, is obtained.
And step 208, after the waiting time is set at intervals, returning to execute the operation of acquiring the real-time positioning point of the unmanned aerial vehicle in the process of flying the unmanned aerial vehicle along the set route until the unmanned aerial vehicle flies to the tail end of the route.
The embodiment of the invention provides a tracking method of a route, which comprises the steps of constructing a comparison table of route independent variables and route point positions according to the route, searching at least one route point position matched with a real-time positioning point in the comparison table of the route independent variables and the route point positions, determining a route independent variable starting point according to the searched route point positions, constructing a termination point and duration grid table according to the route independent variable starting point, the set position interval and the set time interval, constructing the termination point and duration grid table according to the comparison table of the route independent variables and the route point positions, determining a target route point matched with the real-time positioning point and a target curve connecting the target route point and the real-time positioning point by adopting a grid search algorithm, determining a tracking control quantity for controlling an unmanned aerial vehicle to track the route by using a motion state parameter of the target curve at the next moment, and enabling the unmanned aerial vehicle to track an upper route in a smooth and very small shaking mode under any position state, and attaching the route to fly.
Example III
Fig. 3 is a schematic structural diagram of a tracking device for an air route according to a third embodiment of the present invention, as shown in fig. 3, where the device includes: a setpoint acquisition module 301, a grid table construction module 302, a curve calculation module 303, a curve determination module 304, and a control amount determination module 305.
The positioning point obtaining module 301 is configured to obtain a real-time positioning point of the unmanned aerial vehicle during the flight of the unmanned aerial vehicle along a set route; the grid table construction module 302 is configured to construct a grid table of termination points and duration time according to the position relationship between the real-time positioning points and the airlines and the set position interval and the set time interval, where the termination points are located on the airlines; the curve calculation module 303 is configured to calculate tracking curves corresponding to the termination point and each grid unit in the duration grid table respectively according to the motion state parameters matched with the real-time positioning points by using a grid search algorithm; the curve determining module 304 is configured to obtain a target curve with a minimum function value of the cost function in tracking curves corresponding to the grid cells respectively; the control amount determining module 305 is configured to calculate a motion state parameter of the target curve at a next moment, and determine a tracking control amount for controlling the unmanned aerial vehicle to track the route using the motion state parameter.
The embodiment of the invention provides a track device for a route, which constructs an ending point and duration grid table according to a position relation between a real-time positioning point and the route and a set position interval and a set time interval, then adopts a grid search algorithm, calculates track curves respectively corresponding to grid units in the ending point and duration grid table according to at least one motion state parameter matched with the real-time positioning point, obtains a target curve with the minimum function value of a cost function in the track curves respectively corresponding to the grid units, calculates a motion state parameter of the target curve at the next moment, and determines a track control quantity for controlling an unmanned aerial vehicle to track the route by using the motion state parameter, thereby solving the problems that the position, the speed and the acceleration of the machine body are disturbed and the control quantity is overlarge or suddenly changed due to various reasons in the prior art.
On the basis of the above embodiments, the method may further include: the comparison table construction module is used for constructing a comparison table of the route independent variable and the navigation point position according to the route; the route is a route function taking a route independent variable as an independent variable and taking a waypoint position as a dependent variable; the grid table construction sub-module may include: the starting point determining unit is used for searching at least one waypoint position matched with the real-time locating point in a comparison table of the route independent variable and the waypoint position, and determining the route independent variable starting point according to the searched waypoint position; and the grid table construction unit is used for constructing an end point and duration grid table according to the route independent variable starting point, the set position interval and the set time interval.
Based on the above embodiments, the look-up table construction module may include: the position calculation sub-module is used for substituting the current value of the route independent variable into a route function matched with the route to calculate and obtain the navigation point position matched with the current value, and the route independent variable has a set initial value; the position adding sub-module is used for adding the current value and the waypoint position matched with the current value into a comparison table of the route independent variable and the waypoint position; and the current value updating sub-module is used for updating the current value of the route independent variable by using the set increment value of the route independent variable, returning to execute the operation of substituting the current value of the route independent variable into a route function matched with the route, and calculating to obtain the position of the waypoint matched with the current value until the preset ending processing condition is met.
Based on the above embodiments, the route arguments may include: arc length or time.
On the basis of the above embodiments, the start point determination unit may include: the position searching subunit is used for searching a first navigation point position and a second navigation point position which are closest to the real-time positioning point in a comparison table of the route independent variable and the navigation point position; and the starting point determining subunit is used for determining the starting point of the route independent variable by adopting a dichotomy algorithm according to the first route independent variable corresponding to the first route position and the second route independent variable corresponding to the second route point position.
On the basis of the above embodiments, the grid table construction unit may include: grid table construction subunit for<Δs，ΔT>For intervals, generate a plurality of<s k ，T k >The end points and duration grid table is formed:
<s 0 ，T 0 >…<s k ，T 0 >…<s n ，T 0 >
<s 0 ，T 1 >…<s k ，T 1 >…<s n ，T 1 >
…
<s 0 ，T k >…<s k ，T k >…<s n ，T k >
…
<s 0 ，T n >…<s k ，T n >…＜s n ，T n >
wherein ,sk ＝s 0 +Δs*k，T k ＝T 0 +ΔT.k, where k ε [0, n]，s 0 T is determined according to the position of the starting point of the route independent variable 0 For unmanned aerial vehicle moves to AND s from real-time locating point k The corresponding waypoint location's time interval set point.
On the basis of the above embodiments, the curve calculation sub-module may include: a tracking curve calculation unit for calculating the end point and the grid cells < s in the duration grid table according to the following formula k ，T k >Tracking curves respectively corresponding to
wherein ,
wherein ,p0 For matching position information, v, with real-time location points 0 For speed information, a, matching real-time localization points 0 Acceleration information p for matching with real-time anchor point f At s for the course function k Lower value v f The first derivative of the course function is s k Lower value, a f The second derivative of the course function is s k The following values.
Based on the above embodiments, the curve determining module 304 may include: a numerical calculation subunit, configured to calculate a formula by using a cost function:computing and grid cells<s k ，T k >The function value J of the cost function corresponding to the tracking curves of the model (C); wherein k is E [0, n]Alpha, beta and gamma are respectively connected with grid cells<s k ，T k >And (5) association.
On the basis of the above embodiments, the method may further include: and the return execution module is used for returning to execute the operation of acquiring the real-time positioning point of the unmanned aerial vehicle in the process of flying the unmanned aerial vehicle along the set route after the waiting time is set at intervals until the unmanned aerial vehicle flies to the tail end of the route.
The route tracking device can execute the route tracking method provided by any embodiment of the invention, and has the corresponding functional modules and beneficial effects of executing the route tracking method.
Example IV
Fig. 4 is a schematic structural diagram of a computer device according to a fourth embodiment of the present invention. Fig. 4 illustrates a block diagram of an exemplary computer device 512 suitable for use in implementing embodiments of the present invention. The computer device 512 shown in fig. 4 is merely an example, and should not be construed as limiting the functionality and scope of use of embodiments of the present invention.
As shown in FIG. 4, computer device 512 is in the form of a general purpose computing device. Components of computer device 512 may include, but are not limited to: one or more processors 516, a memory 528, a bus 518 that connects the different system components (including the memory 528 and the processor 516).
Bus 518 represents one or more of several types of bus structures, including a memory bus or memory controller, a peripheral bus, an accelerated graphics port, a processor, or a local bus using any of a variety of bus architectures. By way of example, and not limitation, such architectures include Industry Standard Architecture (ISA) bus, micro channel architecture (MAC) bus, enhanced ISA bus, video Electronics Standards Association (VESA) local bus, and Peripheral Component Interconnect (PCI) bus.
Computer device 512 typically includes a variety of computer system readable media. Such media can be any available media that is accessible by computer device 512 and includes both volatile and nonvolatile media, removable and non-removable media.
Memory 528 may include computer system readable media in the form of volatile memory, such as Random Access Memory (RAM) 530 and/or cache memory 532. The computer device 512 may further include other removable/non-removable, volatile/nonvolatile computer system storage media. By way of example only, storage system 534 may be used to read from or write to a non-removable, nonvolatile magnetic medium (not shown in FIG. 4, commonly referred to as a "hard disk drive"). Although not shown in fig. 4, a magnetic disk drive for reading from and writing to a removable non-volatile magnetic disk (e.g., a "floppy disk"), and an optical disk drive for reading from or writing to a removable non-volatile optical disk (e.g., a CD-ROM, DVD-ROM, or other optical media) may be provided. In such cases, each drive may be coupled to bus 518 through one or more data media interfaces. Memory 528 may include at least one program product having a set (e.g., at least one) of program modules configured to carry out the functions of embodiments of the invention.
A program/utility 540 having a set (at least one) of program modules 542 may be stored in, for example, memory 528, such program modules 542 including, but not limited to, an operating system, one or more application programs, other program modules, and program data, each or some combination of which may include an implementation of a network environment. Program modules 542 generally perform the functions and/or methods in the described embodiments of the invention.
The computer device 512 may also communicate with one or more external devices 514 (e.g., keyboard, pointing device, display 524, etc.), one or more devices that enable a user to interact with the computer device 512, and/or any devices (e.g., network card, modem, etc.) that enable the computer device 512 to communicate with one or more other computing devices. Such communication may occur through an input/output (I/O) interface 522. Also, the computer device 512 may communicate with one or more networks such as a Local Area Network (LAN), a Wide Area Network (WAN) and/or a public network, such as the Internet, through a network adapter 520. As shown, network adapter 520 communicates with other modules of computer device 512 via bus 518. It should be appreciated that although not shown in fig. 4, other hardware and/or software modules may be used in connection with computer device 512, including, but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, data backup storage systems, and the like.
Processor 516 executes programs stored in memory 528 to perform various functional applications and data processing, such as implementing the tracking methods of the airlines provided by embodiments of the present invention. That is, in the process that the unmanned aerial vehicle flies along a set route, acquiring a real-time positioning point of the unmanned aerial vehicle; according to the position relation between the real-time positioning points and the airlines, constructing a grid table of termination points and duration time according to the set position interval and the set time interval, wherein the termination points are positioned on the airlines; calculating tracking curves respectively corresponding to all grid cells in the termination point and duration grid table according to the motion state parameters matched with the real-time positioning points by adopting a grid search algorithm; acquiring a target curve with the minimum function value of the cost function from tracking curves corresponding to the grid cells respectively; and calculating the motion state parameter of the target curve at the next moment, and determining the tracking control quantity for controlling the unmanned aerial vehicle to track the route by using the motion state parameter.
Example five
A fifth embodiment of the present application provides a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements a method for tracking a route as provided by all embodiments of the present application. That is, in the process that the unmanned aerial vehicle flies along a set route, acquiring a real-time positioning point of the unmanned aerial vehicle; according to the position relation between the real-time positioning points and the airlines, constructing a grid table of termination points and duration time according to the set position interval and the set time interval, wherein the termination points are positioned on the airlines; calculating tracking curves respectively corresponding to all grid cells in the termination point and duration grid table according to the motion state parameters matched with the real-time positioning points by adopting a grid search algorithm; acquiring a target curve with the minimum function value of the cost function from tracking curves corresponding to the grid cells respectively; and calculating the motion state parameter of the target curve at the next moment, and determining the tracking control quantity for controlling the unmanned aerial vehicle to track the route by using the motion state parameter.
Any combination of one or more computer readable media may be employed. The computer readable medium may be a computer readable signal medium or a computer readable storage medium. The computer readable storage medium can be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples (a non-exhaustive list) of the computer-readable storage medium would include the following: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In this document, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
The computer readable signal medium may include a propagated data signal with computer readable program code embodied therein, either in baseband or as part of a carrier wave. Such a propagated data signal may take any of a variety of forms, including, but not limited to, electro-magnetic, optical, or any suitable combination of the foregoing. A computer readable signal medium may also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device.
Program code embodied on a computer readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.
Computer program code for carrying out operations of the present invention may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, smalltalk, C ++ and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computer (for example, through the Internet using an Internet service provider).
Note that the above is only a preferred embodiment of the present invention and the technical principle applied. It will be understood by those skilled in the art that the present invention is not limited to the particular embodiments described herein, but is capable of various obvious changes, rearrangements and substitutions as will now become apparent to those skilled in the art without departing from the scope of the invention. Therefore, while the invention has been described in connection with the above embodiments, the invention is not limited to the embodiments, but may be embodied in many other equivalent forms without departing from the spirit or scope of the invention, which is set forth in the following claims.
Claims (12)
1. A method of tracking an airline, comprising:
acquiring a real-time positioning point of the unmanned aerial vehicle in the process of flying the unmanned aerial vehicle along a set route;
according to the position relation between the real-time positioning points and the airlines, constructing a termination point and duration grid table according to the set position interval and the set time interval, wherein the termination point is positioned on the airlines;
calculating tracking curves respectively corresponding to all grid cells in the termination point and duration grid table according to the motion state parameters matched with the real-time positioning points by adopting a grid search algorithm;
Acquiring a target curve with the minimum function value of the cost function from tracking curves respectively corresponding to the grid cells;
and calculating a motion state parameter of the target curve at the next moment, and determining a tracking control quantity for controlling the unmanned aerial vehicle to track the route by using the motion state parameter.
2. The method of claim 1, wherein before acquiring the real-time localization point of the unmanned aerial vehicle during the flight of the unmanned aerial vehicle along the set-up route, further comprising:
constructing a comparison table of route independent variables and waypoint positions according to the route; wherein, the route is a route function taking the route independent variable as the independent variable and the waypoint position as the dependent variable;
according to the position relation between the real-time positioning point and the route, constructing an ending point and duration grid table according to the set position interval and the set time interval, wherein the method comprises the following steps:
searching at least one waypoint position matched with the real-time locating point in a comparison table of the route independent variable and the waypoint position, and determining a route independent variable starting point according to the searched waypoint position;
and constructing an ending point and duration grid table according to the route independent variable starting point, the set position interval and the set time interval.
3. The method of claim 2, wherein constructing a look-up table of course arguments versus waypoint locations based on the course comprises:
substituting the current value of the route independent variable into a route function matched with the route to calculate and obtain a navigation point position matched with the current value, wherein the route independent variable has a set initial value;
adding the current value and the waypoint position matched with the current value into a comparison table of the route independent variable and the waypoint position;
and updating the current value of the route independent variable by using the set increment value of the route independent variable, and returning to execute the operation of substituting the current value of the route independent variable into a route function matched with the route to calculate the position of the waypoint matched with the current value until the preset ending processing condition is met.
4. A method according to claim 3, wherein the route arguments comprise: arc length or time.
5. The method of claim 2, wherein searching for at least one waypoint location having a waypoint location matching the real-time anchor point in a lookup table of the waypoint location and the waypoint location, and determining a waypoint starting point based on the searched waypoint location, comprises:
Searching a first navigation point position and a second navigation point position which are closest to the real-time positioning point in a comparison table of the route independent variable and the navigation point position;
and determining a route independent variable starting point by adopting a dichotomy algorithm according to the first route independent variable corresponding to the first navigation point position and the second route independent variable corresponding to the second navigation point position.
6. The method of claim 2, wherein constructing an ending point and duration grid table from the route argument starting point, and the set position interval and time interval, comprises:
generating a plurality of < s with < deltas, deltaT > as intervals k ，T k > said termination points and duration grid table of composition:
＜s 0 ，T 0 ＞…s k ，T 0 ＞…s n ，T 0 ＞
＜s 0 ，T 1 >…s k ，T 1 >…s n ，T 1 >
…
＜s 0 ，T k ＞…s k ，T k ＞…s n ，T k ＞
…
＜s 0 ，T n ＞…s k ，T n ＞…s n ，T n ＞
wherein ,sk ＝s 0 +Δs*k，T k ＝T 0 +ΔT.k, where k ε [0, n]，s 0 Determining T according to the position of the starting point of the route independent variable 0 For the unmanned aerial vehicle to move to and s from the real-time positioning point k A time interval set point of the corresponding waypoint position;
where Δs is the position interval and Δt is the time interval.
7. The method of claim 6, wherein computing tracking curves corresponding to each grid cell in the ending point and duration grid table, respectively, using a grid search algorithm based on at least one motion state parameter matched to the real-time anchor point, comprises:
Calculating each grid cell < s in the grid table of termination points and durations according to the following formula k ，T k Tracking curves corresponding to each other
wherein ,
wherein ,p0 For the position information, v, matching the real-time localization point 0 For speed information, a, matching the real-time localization point 0 Acceleration information p for matching with the real-time positioning point f At s for the course function k Lower value v f At s, the first derivative of the course function k Lower value, a f At s is a second derivative of the course function k The following values.
8. The method of claim 7, wherein obtaining the target curve with the smallest function value of the cost function in the tracking curves corresponding to the grid cells, respectively, comprises:
the formula is calculated through a cost function:calculating and each grid unit < s k ，T k >The function value J of the cost function corresponding to the tracking curves of the model (C);
wherein k is E [0, n]Alpha, beta and gamma are respectively with the grid cells<s k ，T k Correlation.
9. The method according to claim 1, further comprising, after calculating a motion state parameter of the target curve at a next time and using the motion state parameter, determining a tracking control amount for controlling the unmanned aerial vehicle to track the course:
After the waiting time is set at intervals, the operation of acquiring the real-time positioning point of the unmanned aerial vehicle in the process that the unmanned aerial vehicle flies along the set route is returned until the unmanned aerial vehicle flies to the tail end of the route.
10. A tracking device for an airline, comprising:
the positioning point acquisition module is used for acquiring real-time positioning points of the unmanned aerial vehicle in the process of flying the unmanned aerial vehicle along a set route;
the grid table construction module is used for constructing a grid table of termination points and duration time according to the position relation between the real-time positioning points and the airlines and the set position interval and the set time interval, wherein the termination points are positioned on the airlines;
the curve calculation module is used for calculating tracking curves respectively corresponding to the termination points and each grid unit in the duration grid table according to the motion state parameters matched with the real-time positioning points by adopting a grid search algorithm;
the curve determining module is used for acquiring a target curve with the minimum function value of the cost function from tracking curves respectively corresponding to the grid cells;
and the control quantity determining module is used for calculating the motion state parameter of the target curve at the next moment and determining the tracking control quantity for controlling the unmanned aerial vehicle to track the route by using the motion state parameter.
11. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the method of tracking an airline according to any of claims 1-9 when executing the program.
12. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements a method of tracking an airlines as claimed in any of the claims 1-9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910797686.4A CN112445242B (en) | 2019-08-27 | 2019-08-27 | Method, device, equipment and storage medium for tracking route |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910797686.4A CN112445242B (en) | 2019-08-27 | 2019-08-27 | Method, device, equipment and storage medium for tracking route |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112445242A CN112445242A (en) | 2021-03-05 |
| CN112445242B true CN112445242B (en) | 2023-09-01 |
Family
ID=74741614
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910797686.4A Active CN112445242B (en) | 2019-08-27 | 2019-08-27 | Method, device, equipment and storage medium for tracking route |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112445242B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115858549B (en) * | 2023-02-27 | 2023-05-05 | 西安索格亚航空科技有限公司 | Navigation database of aviation navigation equipment and route updating method |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105487554A (en) * | 2016-01-12 | 2016-04-13 | 武汉顶翔智控科技有限公司 | Multi-rotor unmanned aerial vehicle automatic homeward flight path planning algorithm |
| WO2018145291A1 (en) * | 2017-02-10 | 2018-08-16 | SZ DJI Technology Co., Ltd. | System and method for real-time location tracking of drone |
| CN109655063A (en) * | 2018-11-12 | 2019-04-19 | 中航通飞研究院有限公司 | Large-scale amphibious aircraft naval searching Route planner |
| CN110118557A (en) * | 2019-04-24 | 2019-08-13 | 深圳市道通智能航空技术有限公司 | A kind of course line joining method, system and user terminal |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11530921B2 (en) * | 2018-09-28 | 2022-12-20 | Intel Corporation | Method of generating a collision free path of travel and computing system |
-
2019
- 2019-08-27 CN CN201910797686.4A patent/CN112445242B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105487554A (en) * | 2016-01-12 | 2016-04-13 | 武汉顶翔智控科技有限公司 | Multi-rotor unmanned aerial vehicle automatic homeward flight path planning algorithm |
| WO2018145291A1 (en) * | 2017-02-10 | 2018-08-16 | SZ DJI Technology Co., Ltd. | System and method for real-time location tracking of drone |
| CN109655063A (en) * | 2018-11-12 | 2019-04-19 | 中航通飞研究院有限公司 | Large-scale amphibious aircraft naval searching Route planner |
| CN110118557A (en) * | 2019-04-24 | 2019-08-13 | 深圳市道通智能航空技术有限公司 | A kind of course line joining method, system and user terminal |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112445242A (en) | 2021-03-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11002840B2 (en) | Multi-sensor calibration method, multi-sensor calibration device, computer device, medium and vehicle | |
| US20180102058A1 (en) | High-precision autonomous obstacle-avoidance flying method for unmanned aerial vehicle | |
| WO2023142794A1 (en) | Vehicle control method and apparatus, and device and storage medium | |
| CN110825111A (en) | Unmanned aerial vehicle control method suitable for overhead warehouse goods inventory, goods inventory method, device, server and storage medium | |
| CN112148033B (en) | Unmanned aerial vehicle route determining method, device, equipment and storage medium | |
| JP2022538275A (en) | Parameter arrangement method and device, electronic device and storage medium | |
| WO2023151548A1 (en) | Navigation method and apparatus, and program and computer-readable storage medium | |
| CN107831496B (en) | Method and system for intelligent automobile to automatically acquire front speed | |
| CN112445242B (en) | Method, device, equipment and storage medium for tracking route | |
| CN115016435A (en) | Automatic driving vehicle test method, device, system, equipment and medium | |
| JP2022081613A (en) | Method, apparatus, equipment, medium and computer program for identifying characteristic of automatic operation | |
| CN116301058B (en) | Unmanned flight feedback nonlinear yaw control method, system and equipment | |
| CN116974291A (en) | Control error determining method and device for master-slave cooperative navigation agricultural machinery | |
| CN111942188B (en) | Unmanned aerial vehicle aerial charging system, charging method, device, equipment and medium | |
| CN110853098B (en) | Robot positioning method, device, equipment and storage medium | |
| CN113306570A (en) | Method and device for controlling an autonomous vehicle and autonomous dispensing vehicle | |
| CN115168787A (en) | Flight trajectory associated tracking method based on speculative calculation | |
| CN112393731B (en) | Method, device, electronic equipment and storage medium for tracking path | |
| CN113741529A (en) | Remote guidance method and remote guidance device for spacecraft and intersection part | |
| CN114919661A (en) | Parking control method, device, equipment and storage medium | |
| CN115061499A (en) | Unmanned aerial vehicle control method and unmanned aerial vehicle control device | |
| CN113483770A (en) | Path planning method and device in closed scene, electronic equipment and storage medium | |
| AbdElHamid et al. | A novel software simulator model based on active hybrid architecture | |
| WO2020021954A1 (en) | Information processing device, information processing method, and program | |
| CN113759887A (en) | Path planning method and device, storage medium and electronic equipment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: 510000 Block C, 115 Gaopu Road, Tianhe District, Guangzhou City, Guangdong Province Applicant after: XAG Co., Ltd. Address before: 510000 Block C, 115 Gaopu Road, Tianhe District, Guangzhou City, Guangdong Province Applicant before: Guangzhou Xaircraft Technology Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |