CN117742326A - Track tracking control method and device - Google Patents
Track tracking control method and device Download PDFInfo
- Publication number
- CN117742326A CN117742326A CN202311743607.4A CN202311743607A CN117742326A CN 117742326 A CN117742326 A CN 117742326A CN 202311743607 A CN202311743607 A CN 202311743607A CN 117742326 A CN117742326 A CN 117742326A
- Authority
- CN
- China
- Prior art keywords
- speed
- target
- track
- track point
- smooth
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 69
- 238000013507 mapping Methods 0.000 claims abstract description 7
- 238000009499 grossing Methods 0.000 claims description 58
- 230000001133 acceleration Effects 0.000 claims description 47
- 230000008569 process Effects 0.000 abstract description 14
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 abstract description 5
- 238000004364 calculation method Methods 0.000 description 16
- 230000004044 response Effects 0.000 description 8
- 230000001105 regulatory effect Effects 0.000 description 7
- 238000004891 communication Methods 0.000 description 5
- 230000001276 controlling effect Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000001914 filtration Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000003993 interaction Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000004069 differentiation Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
The invention belongs to the technical field of flight control, and discloses a track tracking control method and device. The method comprises the following steps: acquiring a collision-free track point set, and determining target track points in the collision-free track point set according to the current flight speed; mapping the current flight position to a collision-free track point set to obtain a projected track point; determining track curvature according to the projection track points and the target track points; when the track curvature is larger than a preset curvature threshold, dynamically adjusting the speed of the target track point to obtain a target speed; generating a smooth speed instruction based on the projected track points and the smoothed target speed; the aircraft is controlled to track based on the smooth speed command. Through the mode, proper target points are selected in the planned and output track points in a concentrated mode, the speed of the selected points is dynamically adjusted and smoothed, the accurate track tracking of the aircraft is ensured, the command can be smoothed in real time, and stable gesture in the flight process is ensured.
Description
Technical Field
The present invention relates to the field of flight control technologies, and in particular, to a method and an apparatus for tracking and controlling a trajectory.
Background
In the field of multi-rotor aircraft, the influence of system response delay on control performance is large, the track tracking control algorithm commonly used at present is proportional-integral-derivative control (Proportion Integration Differentiation, PID) and model predictive control (Model Predictive Control, MPC), the PID control calculates control quantity by using the proportion, integral and derivative of system errors, but the control takes track points at the current moment as target information, and ignores the response time delay of the system, so that the adjustment lag is large, the track tracking error is large, the MPC obtains an optimal control sequence by solving an optimization problem, but the MPC needs more calculation resources for solving the optimization problem based on a model, and the method is not suitable for a system with high control frequency and real-time requirements.
The foregoing is provided merely for the purpose of facilitating understanding of the technical solutions of the present invention and is not intended to represent an admission that the foregoing is prior art.
Disclosure of Invention
The invention mainly aims to provide a track tracking control method and a track tracking control device, and aims to solve the technical problems that in the prior art, track tracking errors are large by using a proportional-integral-derivative control mode, and real-time requirements are difficult to achieve by using a model prediction control mode.
In order to achieve the above object, the present invention provides a trajectory tracking control method, the method comprising the steps of:
acquiring a collision-free track point set, and determining a target track point in the collision-free track point set according to the current flight speed;
mapping the current flight position to the collision-free track point set to obtain a projected track point;
determining track curvature according to the projection track points and the target track points;
when the track curvature is larger than a preset curvature threshold, dynamically adjusting the speed of the target track point to obtain a target speed;
generating a smooth speed instruction based on the projected track points and the smoothed target speed;
and controlling the aircraft to track based on the smooth speed instruction.
Optionally, the determining the target track point in the collision-free track point set according to the current flight speed includes:
acquiring a first corresponding relation among a preset speed step length, a preset index step length, a flying speed and an index value;
calculating a current index value corresponding to the current flying speed according to a preset speed step length, a preset index step length and the first corresponding relation;
And taking the track point corresponding to the current index value in the collision-free track point set as the target track point.
Optionally, the determining the track curvature according to the projected track point and the target track point includes:
determining a horizontal component of a connecting line between the projection track point and the target track point according to the position information of the projection track point and the position information of the target track point;
acquiring a speed horizontal component of the target track point, and calculating an included angle cosine value according to the connecting line horizontal component and the speed horizontal component of the target track point;
and determining the track curvature according to the cosine value of the included angle.
Optionally, when the curvature of the track is greater than a preset curvature threshold, dynamically adjusting the speed of the target track point to obtain a target speed, including:
projecting the horizontal velocity component of the target track point to the horizontal line component between the projected track point and the target track point to obtain a vertical velocity vector;
determining a horizontal adjustment amount according to the speed vector and the adjustment weight;
determining a horizontal component of a target speed of the target track point according to the horizontal adjustment amount and the horizontal component of the speed of the target track point;
Taking the vertical component of the speed of the target track point as the vertical component of the target speed of the target track point;
and obtaining the target speed based on the horizontal component of the target speed and the vertical component of the target speed.
Optionally, the generating a smooth speed instruction based on the projected track point and the smoothed target speed includes:
smoothing the target speed to obtain a smooth speed;
generating a speed instruction according to the projected track point, the current flight position and the smooth speed;
and carrying out smoothing processing on the speed command to obtain the smooth speed command.
Optionally, the smoothing processing on the target speed to obtain a smooth speed includes:
acquiring a second corresponding relation among the initial smooth speed, the adjusting factor, the target speed and the speed error;
calculating a speed error corresponding to the target speed according to the initial smooth speed, the adjusting factor and the second corresponding relation;
limiting the speed error according to a preset speed error threshold value to obtain a limiting speed error;
acquiring a third corresponding relation among the amplitude limiting speed error, the initial smoothing speed and the smoothing speed;
And determining the smoothing speed according to the initial smoothing speed, the amplitude limiting speed error and the third corresponding relation.
Optionally, the generating a speed command according to the projected track point, the current flight position and the smooth speed includes:
determining a current position error according to the current flight position and the position information of the projection track point;
acquiring a fourth corresponding relation among proportional gain, differential gain, control running period, speed feedforward gain, current position error, smooth speed and speed instruction;
determining an initial speed command according to a proportional gain, a differential gain, a control running period, a speed feedforward gain, the current position error, the smooth speed and the fourth corresponding relation;
when the initial speed command is greater than or equal to a speed threshold, taking the speed threshold as the speed command;
and when the initial speed command is smaller than a speed threshold value, taking the initial speed command as the speed command.
Optionally, the smoothing processing on the speed command to obtain the smoothed speed command includes:
acquiring a fifth corresponding relation among a control running period, a speed instruction, an initial smooth speed instruction and acceleration;
Determining acceleration according to a control running period, an initial smooth speed instruction, the speed instruction and the fifth corresponding relation;
acquiring a sixth corresponding relation among acceleration, an initial smooth speed instruction, an acceleration threshold value and the smooth speed instruction;
and determining the smooth speed instruction according to the acceleration, the acceleration threshold, the initial smooth speed instruction and the sixth corresponding relation.
Optionally, the controlling the aircraft to track based on the smooth speed command includes:
and generating an actuator instruction according to the smooth speed instruction, so that the aircraft responds to the actuator instruction and performs track tracking while stably flying.
In addition, in order to achieve the above object, the present invention also proposes a trajectory tracking control device including:
the target point selection module is used for acquiring a collision-free track point set, and determining target track points in the collision-free track point set according to the current flight speed;
the speed adjusting module is used for mapping the current flight position to the collision-free track point set to obtain a projection track point;
the speed adjusting module is further used for determining track curvature according to the projection track point and the target track point;
The speed adjusting module is further used for dynamically adjusting the speed of the target track point when the track curvature is larger than a preset curvature threshold value to obtain a target speed;
the instruction smoothing module is used for generating a smooth speed instruction based on the projection track points and the smoothed target speed;
and the track tracking module is used for controlling the aircraft to track based on the smooth speed instruction.
In addition, in order to achieve the above object, the present invention also proposes a trajectory tracking control device including: a memory, a processor, and a trajectory tracking control program stored on the memory and operable on the processor, the trajectory tracking control program configured to implement the steps of the trajectory tracking control method as described above.
In addition, in order to achieve the above object, the present invention also proposes a storage medium having stored thereon a trajectory tracking control program which, when executed by a processor, implements the steps of the trajectory tracking control method as described above.
According to the method, a collision-free track point set is obtained, a target track point is determined in the collision-free track point set according to the current flight speed, the current flight position is mapped to the collision-free track point set to obtain a projection track point, track curvature is determined according to the projection track point and the target track point, when the track curvature is larger than a preset curvature threshold value, the speed of the target track point is dynamically regulated to obtain the target speed, a smooth speed instruction is generated based on the projection track point and the smoothed target speed, and the aircraft is controlled to track based on the smooth speed instruction. Compared with a proportional-integral-derivative control mode, the method has the advantages that the response time delay is ignored, so that the track tracking error is larger, the method can intensively select proper target points at the track points planned to be output, dynamically adjust and smooth the speed of the selected points, consider the system time delay and the flight stability, ensure that the aircraft accurately tracks, improve the tracking precision, and compared with a model prediction control mode which is difficult to meet the real-time requirement, the method can run in real time on a multi-rotor aircraft with limited computing resources and high-speed flight, smooth the control instructions, and ensure the stable gesture in the flight process.
Drawings
FIG. 1 is a schematic diagram of a trace tracking control device of a hardware running environment according to an embodiment of the present invention;
FIG. 2 is a flowchart of a first embodiment of a track following control method according to the present invention;
FIG. 3 is a schematic diagram of interaction logic of an embodiment of a track following control method according to the present invention;
FIG. 4 is a flowchart of a track following control method according to a second embodiment of the present invention;
FIG. 5 is a schematic overall flow chart of an embodiment of a track following control method according to the present invention;
fig. 6 is a block diagram of a first embodiment of the track following control device of the present invention.
The achievement of the objects, functional features and advantages of the present invention will be further described with reference to the accompanying drawings, in conjunction with the embodiments.
Detailed Description
It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
Referring to fig. 1, fig. 1 is a schematic structural diagram of a track following control device of a hardware running environment according to an embodiment of the present invention.
As shown in fig. 1, the trajectory tracking control device may include: a processor 1001, such as a central processing unit (Central Processing Unit, CPU), a communication bus 1002, a user interface 1003, a network interface 1004, a memory 1005. Wherein the communication bus 1002 is used to enable connected communication between these components. The user interface 1003 may include a Display, an input unit such as a Keyboard (Keyboard), and the optional user interface 1003 may further include a standard wired interface, a wireless interface. The network interface 1004 may optionally include a standard wired interface, a Wireless interface (e.g., a Wireless-Fidelity (Wi-Fi) interface). The Memory 1005 may be a high-speed random access Memory (Random Access Memory, RAM) Memory or a stable nonvolatile Memory (NVM), such as a disk Memory. The memory 1005 may also optionally be a storage device separate from the processor 1001 described above.
It will be appreciated by those skilled in the art that the configuration shown in fig. 1 does not constitute a limitation of the trajectory tracking control device, and may include more or fewer components than shown, or may combine certain components, or may have a different arrangement of components.
As shown in fig. 1, an operating system, a network communication module, a user interface module, and a trajectory tracking control program may be included in the memory 1005 as one type of storage medium.
In the trajectory tracking control device shown in fig. 1, the network interface 1004 is mainly used for data communication with a network server; the user interface 1003 is mainly used for data interaction with a user; the processor 1001 and the memory 1005 in the track following control apparatus of the present invention may be provided in the track following control apparatus, which invokes the track following control program stored in the memory 1005 through the processor 1001 and executes the track following control method provided by the embodiment of the present invention.
An embodiment of the present invention provides a track tracking control method, referring to fig. 2, fig. 2 is a schematic flow chart of a first embodiment of a track tracking control method of the present invention.
In this embodiment, the track following control method includes the following steps:
Step S10: and acquiring a collision-free track point set, and determining target track points in the collision-free track point set according to the current flight speed.
It should be noted that, the execution body of the embodiment may be a track tracking control device, in which a track tracking control program is set, and by running the track tracking control program, accurate tracking of the track by the aircraft may be implemented, or may be other terminals with computing capabilities, which is not limited in this embodiment, and the track tracking control device is described as an example.
It can be understood that the aircraft in this embodiment may be a multi-rotor aircraft, for the multi-rotor aircraft, the system response delay has a relatively large influence on the control performance, and the common track tracking control algorithm is PID and MPC, but the PID usually ignores the response delay of the system, resulting in adjustment lag, so that the track tracking error is relatively large, and the MPC is complex in calculation and difficult to meet the real-time requirement.
It should be understood that the collision-free track point set refers to a set of three-dimensional track points that are generated after the acquisition of the flight mission and connect the start position and the end position, and have no collision, where the track points generally include information such as position, speed, acceleration, time, and the like, and the collision-free track point set may be generated by the track planning module. Generally, the position, velocity, and acceleration in this embodiment are all three-dimensional.
In the specific implementation, after a flight task is acquired, a three-dimensional track point set which is connected with a starting point position and an end point position and has no collision is generated according to a certain frequency, and meanwhile, the position and the speed of an aircraft at the current moment are acquired in real time.
The target track point is selected from the collision-free track point set.
Further, determining a target track point in the collision-free track point set according to the current flight speed comprises: acquiring a first corresponding relation among a preset speed step length, a preset index step length, a flying speed and an index value; calculating a current index value corresponding to the current flying speed according to a preset speed step length, a preset index step length and the first corresponding relation; and taking the track point corresponding to the current index value in the collision-free track point set as the target track point.
It can be understood that the preset speed step and the preset index step are preset speed step and index step, and specific values can be determined according to practical situations, which is not limited in this embodiment. The first correspondence between the preset speed step, the preset index step, the flying speed and the index value refers to a calculation relational expression designed in the embodiment and used for determining the index value of the track point, as follows:
wherein index represents the index value of the track point, ceil () is an upward rounding function, v c Is the module length of the current flying speed, v 0 For a preset speed step, k 0 And calculating the modular length of the current flying speed for the preset index step length, substituting the modular length of the current flying speed, the preset speed step length and the preset index step length to obtain a current index value, and finding out the track points in the current index value in the collision-free track point set, namely the target track points to be selected.
In specific implementation, a prespecified point strategy is utilized to select a proper target track point in a collision-free track point set planned and output, firstly, a segmented track point index value is designed according to a preset speed step length and an index step length, then, an index value at the current moment is calculated according to the modular length of the current flight speed, and then, the track point at the index value is selected as the target track point in the collision-free track point set.
Step S20: and mapping the current flight position to the collision-free track point set to obtain a projected track point.
It should be understood that the current flight position, i.e. the position at the current moment, is the projected trajectory point, i.e. the projected locus point, and the projected trajectory point is obtained by mapping the position at the current moment to the collision-free trajectory point set.
Step S30: and determining the track curvature according to the projection track point and the target track point.
Further, the step S30 includes: determining a horizontal component of a connecting line between the projection track point and the target track point according to the position information of the projection track point and the position information of the target track point; acquiring a speed horizontal component of the target track point, and calculating an included angle cosine value according to the connecting line horizontal component and the speed horizontal component of the target track point; and determining the track curvature according to the cosine value of the included angle.
The position information of the projected trajectory point is the three-dimensional position of the projected trajectory point, and the position information of the target trajectory point is the three-dimensional position of the target trajectory point. The horizontal component of the line between the projected track point and the target track point refers to the component of the line between the projected track point and the target track point corresponding to the horizontal direction/horizontal plane. The horizontal component of the speed of the target track point refers to a component corresponding to the speed of the target track point in the horizontal direction/horizontal plane, and correspondingly, the vertical component of the speed of the target track point refers to a component corresponding to the speed of the target track point in the vertical direction (Z axis), and the speed of the target track point refers to the planned speed, and further adjustment is needed.
It can be understood that the cosine value of the included angle refers to the cosine value of the included angle between the horizontal component of the connecting line and the horizontal component of the speed of the target track point, and the track curvature refers to the curvature of the current flight track. The calculation relation of the track curvature is as follows:
where E is the track curvature, v obj,xy Is the velocity horizontal component of the target track point,v op,xy for the connection horizontal component of the target track point and the projection track point, in order to obtain the connection horizontal component, the connection vector of the target track point and the projection track point is usually determined, and the relation is calculated as follows:
v op ＝p obj -p proh
wherein v is op Is the connection line vector of the target track point and the projection track point, p obj For the three-dimensional position of the target track point, p proj And (3) projecting the three-dimensional position of the track point to obtain a connecting line horizontal component, and further calculating an included angle cosine value, namely the track curvature to be determined, according to the connecting line horizontal component and the speed horizontal component of the target track point.
In the specific implementation, a cosine value of an included angle between a horizontal component of a connecting line of the target track point and the projection track point and a horizontal component of the speed of the target track point is calculated and is used as the curvature of the current flight track.
Step S40: and when the track curvature is larger than a preset curvature threshold, dynamically adjusting the speed of the target track point to obtain a target speed.
It should be understood that the preset curvature threshold, that is, the preset curvature threshold, may be a specific value determined according to the actual situation, which is not limited in this embodiment. If the curvature (track curvature) of the current flight track is larger than a set threshold (preset curvature threshold), dynamic adjustment of the speed direction and the speed of the target track point is needed, so that distribution of output horizontal components is adjusted and controlled, and tracking errors caused by long response delay of the system are reduced.
Further, the step S40 includes: projecting the horizontal velocity component of the target track point to the horizontal line component between the projected track point and the target track point to obtain a vertical velocity vector; determining a horizontal adjustment amount according to the speed vector and the adjustment weight; determining a horizontal component of a target speed of the target track point according to the horizontal adjustment amount and the horizontal component of the speed of the target track point; taking the vertical component of the speed of the target track point as the vertical component of the target speed of the target track point; and obtaining the target speed based on the horizontal component of the target speed and the vertical component of the target speed.
The velocity vector refers to a vector of a horizontal component of the velocity of the target track point projected onto a horizontal component of a line connecting the target track point and the projected track point, and the calculation relation is as follows:
in the formula, v lat,xy V is the velocity vector obj,xy V, the velocity horizontal component of the target track point op,xy Is the horizontal component of the line between the target track point and the projection track point. The target speed is the speed of the adjusted target track point, the horizontal component of the target speed is the component corresponding to the target speed in the horizontal direction/horizontal plane, namely the horizontal component of the speed of the adjusted target track point, the vertical component of the target speed is the component corresponding to the target speed in the vertical direction (Z axis), namely the vertical component of the speed of the adjusted target track point, and the target speed can be determined according to the obtained horizontal component and vertical component of the target speed.
It is understood that the horizontal adjustment amount refers to an amount by which the speed of the target track point needs to be adjusted in the horizontal direction/horizontal plane, that is, an amount by which the horizontal component of the speed of the target track point needs to be adjusted, and the adjustment weight is a weight coefficient when the adjustment is set, and the specific value can be determined according to the actual situation, which is not limited in this embodiment. Multiplying the adjusting weight by the velocity vertical vector to obtain a horizontal adjusting quantity, adding the horizontal adjusting quantity to the velocity horizontal component of the target track point to obtain the velocity horizontal component of the adjusted target track point, namely the velocity horizontal component of the target velocity, and calculating the following relation:
v′ obj,xy ＝v obj,xy +βv lat,xy
In the formula, v ′ obj,xy Is the horizontal component of the target speed, beta isAdjusting the weight, v lat,xy V is the velocity vector obj,xy Is the velocity horizontal component of the target track point.
It should be understood that the velocity vertical component of the target track point in this embodiment is not adjusted, that is, the vertical component of the target velocity is equal to the velocity vertical component of the target track point, and the calculation relation is as follows:
v ′ obj,z ＝v obj,z
in the formula, v ′ obj,z V is the vertical component of the target velocity obj,z Is the vertical component of the velocity of the target track point, thereby determining the target velocity based on the horizontal component of the target velocity and the vertical component of the target velocity.
In the specific implementation, the current flight speed is utilized to select a target track point in a collision-free track point set planned and output, the selected point speed is dynamically regulated and smoothed, the system time delay and the flight stability are considered, the track can be accurately tracked, and the precision is improved by 1.5 times compared with that of PID.
Step S50: and generating a smooth speed instruction based on the projection track points and the smoothed target speed.
After the target speed is obtained, the process from zero acceleration to the target speed, the change of the speed of the navigation section and the deceleration to zero are required to be subjected to smoothing treatment so as to ensure the smoothness of the acceleration, and further the amplitude and the frequency of the change of the attitude angle of the aircraft are smaller, so that the stability of the flight can be improved.
It can be understood that the smoothed speed command refers to a finally obtained smoothed speed command, and in this embodiment, the position of the projected track point, the position of the current time and the smoothed target speed are used as input information, and the proportional-differential and speed feedforward controller is used to generate the speed command, and perform smoothing processing on the speed command, and meet the constraint of maximum speed and acceleration, so as to obtain the smoothed speed command.
It should be understood that, for the smoothing process of the target speed and the smoothing process of the speed command, any smoothing method such as a first-order filtering smoothing method, a slope clipping smoothing method, a kalman filtering smoothing method, etc. may be used, which is not limited in this embodiment, and may be selected according to actual requirements.
Step S60: and controlling the aircraft to track based on the smooth speed instruction.
Further, the step S60 includes: and generating an actuator instruction according to the smooth speed instruction, so that the aircraft responds to the actuator instruction and performs track tracking while stably flying.
In the specific implementation, the smoothed speed instruction is transmitted into the flight control system to generate an actuator instruction, and the aircraft responds and tracks the instruction, so that an accurate tracking track is realized, and the aircraft can fly stably.
After the flight task is acquired, the track planning module generates a collision-free three-dimensional track point set connecting the starting point position and the end point position, selects a proper track point through a pre-aiming point strategy according to a series of track points and real-time speed, then gives a smooth speed instruction by utilizing a proportional-differential and speed feedforward controller according to the real-time speed and the position and combining the maximum speed and acceleration constraint of the aircraft, and then responds and tracks the speed instruction by a flight control system, and solves the instruction of the actuator to the multi-rotor aircraft, thereby achieving the purposes of flying according to the instruction speed and accurately tracking the position.
In this embodiment, a collision-free track point set is obtained, a target track point is determined in the collision-free track point set according to a current flight speed, a current flight position is mapped to the collision-free track point set to obtain a projected track point, track curvature is determined according to the projected track point and the target track point, when the track curvature is greater than a preset curvature threshold value, the speed of the target track point is dynamically regulated to obtain a target speed, a smooth speed instruction is generated based on the projected track point and the smoothed target speed, and an aircraft is controlled to track based on the smooth speed instruction. According to the method, the device and the system, the proper target point can be selected in the set of planned and output track points, the speed of the selected point is dynamically adjusted and smoothed, the system time delay and the flight stability are considered, the accurate track tracking of the aircraft is ensured, the tracking precision is improved, meanwhile, the method and the system can run on a multi-rotor aircraft with limited computing resources and high-speed flight in real time, the control instruction is smoothed, and the stable attitude in the flight process is ensured.
Referring to fig. 4, fig. 4 is a flowchart of a track following control method according to a second embodiment of the present invention.
Based on the above embodiment, the step S50 includes:
step S501: and carrying out smoothing treatment on the target speed to obtain a smooth speed.
Further, the step S501 includes: acquiring a second corresponding relation among the initial smooth speed, the adjusting factor, the target speed and the speed error; calculating a speed error corresponding to the target speed according to the initial smooth speed, the adjusting factor and the second corresponding relation; limiting the speed error according to a preset speed error threshold value to obtain a limiting speed error; acquiring a third corresponding relation among the amplitude limiting speed error, the initial smoothing speed and the smoothing speed; and determining the smoothing speed according to the initial smoothing speed, the amplitude limiting speed error and the third corresponding relation.
The second correspondence between the initial smooth velocity, the adjustment factor, the target velocity and the velocity error refers to a calculation relation of the velocity error, as follows:
v err ＝α(v smooth，k -v′ obj )
in the formula, v smooth，k For the smoothing speed at the last moment (i.e. initial smoothing speed), alpha is the regulating factor, v err As a speed error, v' obj And substituting the corresponding initial smooth speed and the adjusting factor into a calculation relation of the speed error for the target speed, so that the speed error at the current moment can be calculated.
It can be understood that the preset speed error threshold is a preset speed error threshold, and the specific value can be determined according to the actual situation, which is not limited in this embodiment. In this embodiment, the speed error is limited by using a preset speed error threshold, so as to obtain a limited speed error, that is, a limited speed error, and the limiting may be performed by using the following calculation relation:
wherein min () is a minimum function, e thr Is a preset speed error threshold value, v' err Limiting speed error, v err Substituting a preset speed error threshold value and the speed error at the current moment into a limiting calculation relation for the speed error, and calculating to obtain the limiting speed error at the current moment.
It should be understood that the third correspondence between clipping speed error, initial smoothing speed and smoothing speed refers to the calculation relation of smoothing speed, as follows:
v smooth，k+1 ＝v smooth，k +v′ err
in the formula, v smooth，k+1 For smooth speed v smooth，k For initial smoothing speed, v' err Substituting the limiting speed error at the current moment and the smooth speed at the last moment into a calculating relation of the smooth speed to obtain the smooth speed at the current moment.
In a specific implementation, a first-order filtering smoothing mode is utilized to carry out smoothing treatment on the target speed, and the smoothing treatment is carried out according to the acceleration set in the acceleration and deceleration process (from zero acceleration to the target speed, the change of the speed of a leg and the deceleration to zero) so as to ensure the smoothness of the acceleration, so that the amplitude and the frequency of the change of the attitude angle of the aircraft are smaller, and the flying stability is further improved.
Step S502: and generating a speed instruction according to the projected track point, the current flight position and the smooth speed.
Further, the step S502 includes: determining a current position error according to the current flight position and the position information of the projection track point; acquiring a fourth corresponding relation among proportional gain, differential gain, control running period, speed feedforward gain, current position error, smooth speed and speed instruction; determining an initial speed command according to a proportional gain, a differential gain, a control running period, a speed feedforward gain, the current position error, the smooth speed and the fourth corresponding relation; when the initial speed command is greater than or equal to a speed threshold, taking the speed threshold as the speed command; and when the initial speed command is smaller than a speed threshold value, taking the initial speed command as the speed command.
The current position error refers to a position error at the current time, and is determined according to the position at the current time and the three-dimensional position of the projection track point, and the calculation relational expression is as follows:
e p，k+1 ＝p proj -p cur
wherein p is cur E is the current flight position p，k+1 For the current position error, p proj Substituting the position at the current moment and the three-dimensional position of the projection track point into the three-dimensional position of the projection track point to obtain the position error at the current moment.
It will be appreciated that the fourth correspondence between proportional gain, derivative gain, control run period, speed feed forward gain, current position error, smoothed speed and speed command refers to the calculated relationship for the speed command as follows:
in the formula e p，k+1 E is the current position error p，k Mu, the position error at the previous moment p Is proportional gain, mu d Is the differential gain, sigma t To control the run period ρ v For speed feed forward gain, v cmd V is a speed command smooth，k+1 Is a smooth speed. The proportional gain, the differential gain, the control running period and the speed feedforward gain are all preset parameters, and specific numerical values can be determined according to actual conditionsThe embodiment is not limited thereto. The method can calculate an initial speed command, namely the initial speed command, by substituting the related data into a calculation relation of the speed command, and also needs to satisfy the constraint of the maximum speed, namely the speed command cannot exceed the maximum speed (speed threshold), so that when the initial speed command is larger than or equal to the speed threshold, the speed threshold is taken as a final speed command, and when the initial speed command is smaller than the speed threshold, the initial speed command is the final speed command, and the method can be expressed by the following calculation relation:
v cmd ＝min{v cmd ，v max }
In the formula, v cmd V is a speed command max Is a speed threshold.
In a specific implementation, the speed command is generated using a proportional-derivative and speed feedforward controller and the maximum speed constraint is satisfied.
Step S503: and carrying out smoothing processing on the speed command to obtain the smooth speed command.
Further, the step S503 includes: acquiring a fifth corresponding relation among a control running period, a speed instruction, an initial smooth speed instruction and acceleration; determining acceleration according to a control running period, an initial smooth speed instruction, the speed instruction and the fifth corresponding relation; acquiring a sixth corresponding relation among acceleration, an initial smooth speed instruction, an acceleration threshold value and the smooth speed instruction; and determining the smooth speed instruction according to the acceleration, the acceleration threshold, the initial smooth speed instruction and the sixth corresponding relation.
It should be appreciated that the fifth correspondence between the control run period, the speed command, the initial smooth speed command, and the acceleration refers to a calculated relationship of acceleration, as follows:
in the formula, v cmd，s，k Is a speed instruction after the previous time is smoothedI.e., initial smooth velocity command), a is acceleration, v cmd For speed command, sigma t In order to control the running period, the relevant data are substituted, and the corresponding acceleration can be calculated.
The sixth correspondence between the acceleration, the initial smooth speed command, the acceleration threshold value, and the smooth speed command refers to a calculation relation of the smooth speed command, as follows:
v cmd，s，k+1 ＝v cmd，s，k +sign(a)min{||a||，a max }σ t
in the formula, v cmd，s，k+1 For the smooth speed command at the current moment, sign () is a sign function, a max Is the maximum acceleration (acceleration threshold), v cmd，s，k For initial smooth speed command, a is acceleration, v cmd For speed command, sigma t In order to control the running period, the relevant data are substituted, and the corresponding smooth speed instruction can be obtained through calculation.
In a specific implementation, a slope limiting method is adopted to carry out smooth processing on the speed instruction, and the maximum acceleration constraint is met.
As shown in the overall flow chart of fig. 5, position and speed information of an aircraft are acquired in real time, a collision-free track point set is generated, an appropriate pre-aiming point is selected in the collision-free track point set by utilizing actual speed and a set speed step length, the actual position at the current moment is mapped to the track point set to obtain a projection point, the speed of the pre-aiming point is dynamically regulated by utilizing a designed curvature decision method, the regulated speed of the pre-aiming point is subjected to smooth processing according to acceleration set in the acceleration and deceleration process, the projection point and the smoothed speed are used as input information, a smooth speed instruction is generated by utilizing a proportional-differential and speed feedforward controller, the smoothed speed instruction is transmitted into a flight control system to generate an actuator instruction, and the multi-rotor aircraft is subjected to instruction response and tracking to realize accurate track tracking and smooth flight.
In this embodiment, the smoothing speed is obtained by smoothing the target speed, and the speed command is generated according to the projected trajectory point, the current flight position, and the smoothing speed command is obtained by smoothing the speed command. According to the method, the projection track point position, the current flight position and the smooth speed are used as input information, a speed instruction is generated by using a proportional-differential and speed feedforward controller, the speed instruction is smoothed in a slope limiting mode, maximum speed and acceleration constraint are met, the smooth speed instruction is obtained, the aircraft is further controlled according to the smooth speed instruction, the aircraft can run on a multi-rotor aircraft with limited computing resources and high-speed flight in real time, meanwhile, the input and output instructions are smoothed, and stable gesture is ensured in the flight process.
In addition, the embodiment of the invention also provides a storage medium, wherein the storage medium is stored with a track tracking control program, and the track tracking control program realizes the steps of the track tracking control method when being executed by a processor.
Referring to fig. 6, fig. 6 is a block diagram showing the structure of a first embodiment of the track following control device of the present invention.
As shown in fig. 6, the track following control device provided in the embodiment of the present invention includes:
the target point selection module 10 is configured to obtain a set of collision-free track points, and determine a target track point in the set of collision-free track points according to the current flight speed.
The speed adjusting module 20 is configured to map the current flight position to the collision-free track point set, so as to obtain a projected track point.
The speed adjusting module 20 is further configured to determine a track curvature according to the projected track point and the target track point.
The speed adjusting module 20 is further configured to dynamically adjust the speed of the target track point when the track curvature is greater than a preset curvature threshold value, so as to obtain a target speed.
The instruction smoothing module 30 is configured to generate a smooth speed instruction based on the projected track point and the smoothed target speed.
The track tracking module 40 is configured to control the aircraft to track a track based on the smooth speed command.
In this embodiment, a collision-free track point set is obtained, a target track point is determined in the collision-free track point set according to a current flight speed, a current flight position is mapped to the collision-free track point set to obtain a projected track point, track curvature is determined according to the projected track point and the target track point, when the track curvature is greater than a preset curvature threshold value, the speed of the target track point is dynamically regulated to obtain a target speed, a smooth speed instruction is generated based on the projected track point and the smoothed target speed, and an aircraft is controlled to track based on the smooth speed instruction. According to the method, the device and the system, the proper target point can be selected in the set of planned and output track points, the speed of the selected point is dynamically adjusted and smoothed, the system time delay and the flight stability are considered, the accurate track tracking of the aircraft is ensured, the tracking precision is improved, meanwhile, the method and the system can run on a multi-rotor aircraft with limited computing resources and high-speed flight in real time, the control instruction is smoothed, and the stable attitude in the flight process is ensured.
In an embodiment, the target point selection module 10 is further configured to obtain a first correspondence between a preset speed step, a preset index step, a flight speed and an index value;
calculating a current index value corresponding to the current flying speed according to a preset speed step length, a preset index step length and the first corresponding relation;
and taking the track point corresponding to the current index value in the collision-free track point set as the target track point.
In an embodiment, the speed adjusting module 20 is further configured to determine a horizontal component of a connection line between the projected track point and the target track point according to the position information of the projected track point and the position information of the target track point;
acquiring a speed horizontal component of the target track point, and calculating an included angle cosine value according to the connecting line horizontal component and the speed horizontal component of the target track point;
and determining the track curvature according to the cosine value of the included angle.
In an embodiment, the speed adjustment module 20 is further configured to project a speed horizontal component of the target track point to a link horizontal component between the projected track point and the target track point, so as to obtain a speed vertical vector;
Determining a horizontal adjustment amount according to the speed vector and the adjustment weight;
determining a horizontal component of a target speed of the target track point according to the horizontal adjustment amount and the horizontal component of the speed of the target track point;
taking the vertical component of the speed of the target track point as the vertical component of the target speed of the target track point;
and obtaining the target speed based on the horizontal component of the target speed and the vertical component of the target speed.
In an embodiment, the instruction smoothing module 30 is further configured to smooth the target speed to obtain a smoothed speed;
generating a speed instruction according to the projected track point, the current flight position and the smooth speed;
and carrying out smoothing processing on the speed command to obtain the smooth speed command.
In an embodiment, the instruction smoothing module 30 is further configured to obtain a second correspondence between the initial smoothing speed, the adjustment factor, the target speed and the speed error;
calculating a speed error corresponding to the target speed according to the initial smooth speed, the adjusting factor and the second corresponding relation;
limiting the speed error according to a preset speed error threshold value to obtain a limiting speed error;
Acquiring a third corresponding relation among the amplitude limiting speed error, the initial smoothing speed and the smoothing speed;
and determining the smoothing speed according to the initial smoothing speed, the amplitude limiting speed error and the third corresponding relation.
In an embodiment, the instruction smoothing module 30 is further configured to determine a current position error according to the current flight position and the position information of the projected track point;
acquiring a fourth corresponding relation among proportional gain, differential gain, control running period, speed feedforward gain, current position error, smooth speed and speed instruction;
determining an initial speed command according to a proportional gain, a differential gain, a control running period, a speed feedforward gain, the current position error, the smooth speed and the fourth corresponding relation;
when the initial speed command is greater than or equal to a speed threshold, taking the speed threshold as the speed command;
and when the initial speed command is smaller than a speed threshold value, taking the initial speed command as the speed command.
In an embodiment, the instruction smoothing module 30 is further configured to obtain a fifth correspondence between the control running period, the speed instruction, the initial smoothed speed instruction, and the acceleration;
Determining acceleration according to a control running period, an initial smooth speed instruction, the speed instruction and the fifth corresponding relation;
acquiring a sixth corresponding relation among acceleration, an initial smooth speed instruction, an acceleration threshold value and the smooth speed instruction;
and determining the smooth speed instruction according to the acceleration, the acceleration threshold, the initial smooth speed instruction and the sixth corresponding relation.
In one embodiment, the trajectory tracking module 40 is further configured to generate an actuator command according to the smooth speed command, so that the aircraft performs trajectory tracking while flying smoothly in response to the actuator command.
It should be understood that the foregoing is illustrative only and is not limiting, and that in specific applications, those skilled in the art may set the invention as desired, and the invention is not limited thereto.
It should be noted that the above-described working procedure is merely illustrative, and does not limit the scope of the present invention, and in practical application, a person skilled in the art may select part or all of them according to actual needs to achieve the purpose of the embodiment, which is not limited herein.
In addition, technical details not described in detail in the present embodiment may refer to the track tracking control method provided in any embodiment of the present invention, which is not described herein.
Furthermore, it should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or system. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or system that comprises the element.
The foregoing embodiment numbers of the present invention are merely for the purpose of description, and do not represent the advantages or disadvantages of the embodiments.
From the above description of the embodiments, it will be clear to those skilled in the art that the above-described embodiment method may be implemented by means of software plus a necessary general hardware platform, but of course may also be implemented by means of hardware, but in many cases the former is a preferred embodiment. Based on such understanding, the technical solution of the present invention may be embodied essentially or in a part contributing to the prior art in the form of a software product stored in a storage medium (e.g. Read Only Memory)/RAM, magnetic disk, optical disk) and including several instructions for causing a terminal device (which may be a mobile phone, a computer, a server, or a network device, etc.) to perform the method according to the embodiments of the present invention.
The foregoing description is only of the preferred embodiments of the present invention, and is not intended to limit the scope of the invention, but rather is intended to cover any equivalents of the structures or equivalent processes disclosed herein or in the alternative, which may be employed directly or indirectly in other related arts.
Claims (10)
1. A track following control method, characterized in that the track following control method comprises:
acquiring a collision-free track point set, and determining a target track point in the collision-free track point set according to the current flight speed;
mapping the current flight position to the collision-free track point set to obtain a projected track point;
determining track curvature according to the projection track points and the target track points;
when the track curvature is larger than a preset curvature threshold, dynamically adjusting the speed of the target track point to obtain a target speed;
generating a smooth speed instruction based on the projected track points and the smoothed target speed;
and controlling the aircraft to track based on the smooth speed instruction.
2. The method of claim 1, wherein said determining a target trajectory point from said set of collision-free trajectory points as a function of a current flight speed comprises:
Acquiring a first corresponding relation among a preset speed step length, a preset index step length, a flying speed and an index value;
calculating a current index value corresponding to the current flying speed according to a preset speed step length, a preset index step length and the first corresponding relation;
and taking the track point corresponding to the current index value in the collision-free track point set as the target track point.
3. The method of claim 1, wherein said determining track curvature from said projected track point and said target track point comprises:
determining a horizontal component of a connecting line between the projection track point and the target track point according to the position information of the projection track point and the position information of the target track point;
acquiring a speed horizontal component of the target track point, and calculating an included angle cosine value according to the connecting line horizontal component and the speed horizontal component of the target track point;
and determining the track curvature according to the cosine value of the included angle.
4. The method of claim 3, wherein dynamically adjusting the speed of the target trajectory point to obtain a target speed when the trajectory curvature is greater than a preset curvature threshold comprises:
Projecting the horizontal velocity component of the target track point to the horizontal line component between the projected track point and the target track point to obtain a vertical velocity vector;
determining a horizontal adjustment amount according to the speed vector and the adjustment weight;
determining a horizontal component of a target speed of the target track point according to the horizontal adjustment amount and the horizontal component of the speed of the target track point;
taking the vertical component of the speed of the target track point as the vertical component of the target speed of the target track point;
and obtaining the target speed based on the horizontal component of the target speed and the vertical component of the target speed.
5. The method of claim 1, wherein generating a smoothing speed instruction based on the projected trajectory point and the smoothed target speed comprises:
smoothing the target speed to obtain a smooth speed;
generating a speed instruction according to the projected track point, the current flight position and the smooth speed;
and carrying out smoothing processing on the speed command to obtain the smooth speed command.
6. The method of claim 5, wherein smoothing the target speed to obtain a smoothed speed comprises:
Acquiring a second corresponding relation among the initial smooth speed, the adjusting factor, the target speed and the speed error;
calculating a speed error corresponding to the target speed according to the initial smooth speed, the adjusting factor and the second corresponding relation;
limiting the speed error according to a preset speed error threshold value to obtain a limiting speed error;
acquiring a third corresponding relation among the amplitude limiting speed error, the initial smoothing speed and the smoothing speed;
and determining the smoothing speed according to the initial smoothing speed, the amplitude limiting speed error and the third corresponding relation.
7. The method of claim 5, wherein generating a speed command based on the projected trajectory point, the current flight location, and the smoothing speed comprises:
determining a current position error according to the current flight position and the position information of the projection track point;
acquiring a fourth corresponding relation among proportional gain, differential gain, control running period, speed feedforward gain, current position error, smooth speed and speed instruction;
determining an initial speed command according to a proportional gain, a differential gain, a control running period, a speed feedforward gain, the current position error, the smooth speed and the fourth corresponding relation;
When the initial speed command is greater than or equal to a speed threshold, taking the speed threshold as the speed command;
and when the initial speed command is smaller than a speed threshold value, taking the initial speed command as the speed command.
8. The method of claim 5, wherein smoothing the speed command to obtain the smoothed speed command comprises:
acquiring a fifth corresponding relation among a control running period, a speed instruction, an initial smooth speed instruction and acceleration;
determining acceleration according to a control running period, an initial smooth speed instruction, the speed instruction and the fifth corresponding relation;
acquiring a sixth corresponding relation among acceleration, an initial smooth speed instruction, an acceleration threshold value and the smooth speed instruction;
and determining the smooth speed instruction according to the acceleration, the acceleration threshold, the initial smooth speed instruction and the sixth corresponding relation.
9. The method of any one of claims 1 to 8, wherein controlling the aircraft for trajectory tracking based on the smooth speed command comprises:
and generating an actuator instruction according to the smooth speed instruction, so that the aircraft responds to the actuator instruction and performs track tracking while stably flying.
10. A trajectory tracking control device, characterized by comprising:
the target point selection module is used for acquiring a collision-free track point set, and determining target track points in the collision-free track point set according to the current flight speed;
the speed adjusting module is used for mapping the current flight position to the collision-free track point set to obtain a projection track point;
the speed adjusting module is further used for determining track curvature according to the projection track point and the target track point;
the speed adjusting module is further used for dynamically adjusting the speed of the target track point when the track curvature is larger than a preset curvature threshold value to obtain a target speed;
the instruction smoothing module is used for generating a smooth speed instruction based on the projection track points and the smoothed target speed;
and the track tracking module is used for controlling the aircraft to track based on the smooth speed instruction.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311743607.4A CN117742326A (en) | 2023-12-15 | 2023-12-15 | Track tracking control method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311743607.4A CN117742326A (en) | 2023-12-15 | 2023-12-15 | Track tracking control method and device |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117742326A true CN117742326A (en) | 2024-03-22 |
Family
ID=90280788
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311743607.4A Pending CN117742326A (en) | 2023-12-15 | 2023-12-15 | Track tracking control method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117742326A (en) |
-
2023
- 2023-12-15 CN CN202311743607.4A patent/CN117742326A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Han et al. | State-constrained agile missile control with adaptive-critic-based neural networks | |
| KR20030060776A (en) | Method, apparatus and design procedure for controlling multi-input, multi-output(mimo) parameter dependent systems using feedback lti'zation | |
| CN110989626A (en) | Unmanned aerial vehicle path planning method based on control parameterization | |
| Zeng et al. | High-speed trajectory tracking based on model predictive control for omni-directional mobile robots | |
| CN112498744A (en) | Longitudinal and transverse loose coupling online track planning method and electronic equipment | |
| CN115712308A (en) | Multi-machine formation anti-collision method based on distributed model prediction control and formation control | |
| Suresh et al. | An on-line learning neural controller for helicopters performing highly nonlinear maneuvers | |
| Lee et al. | Automatic gain tuning method of a quad-rotor geometric attitude controller using A3C | |
| Lin et al. | Dynamic programming for model predictive control of adaptive cruise control systems | |
| Basson | Control allocation as part of a fault-tolerant control architecture for UAVs | |
| CN113110527B (en) | Cascading control method for finite time path tracking of autonomous underwater vehicle | |
| CN112597612B (en) | Robot optimization method, device, terminal equipment and computer readable storage medium | |
| CN116301058B (en) | Unmanned flight feedback nonlinear yaw control method, system and equipment | |
| Almeida | Tight formation flight with feasible model predictive control | |
| CN117742326A (en) | Track tracking control method and device | |
| Vanek et al. | Practical approach to real-time trajectory tracking of UAV formations | |
| Abdulla et al. | Roll control system design using auto tuning LQR technique | |
| CN115488881A (en) | Man-machine sharing autonomous teleoperation method and system based on multi-motor skill prior | |
| Ramos | Optimal control for time and energy minimization in the trajectory generation of a mobile robot | |
| CN110510155B (en) | Online iterative landing guidance method, system comprising same and storage | |
| Tunik et al. | Hierarchical and Heterogeneous Leader-follower Formation for Quadrotor's Outdoor Application | |
| CN112965370A (en) | Underwater vehicle track tracking control method based on improved firefly algorithm optimization | |
| Duan et al. | Flight-path tracking control of a transportation aircraft: Comparison of two nonlinear design approaches | |
| JPH06274220A (en) | Nc device | |
| CN114509936B (en) | Exercise planning method, device and medium for online learning of exercise capacity |
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 |