A method for controlling the flight trajectory of a moving composite vehicle near obstacles

Abstract

This invention discloses a method for controlling the flight trajectory around obstacles near a moving composite object, belonging to the field of motion control technology for intelligent agents / UAVs / spacecraft swarms. The method includes acquiring relative flight trajectory points at various time points; drawing a target flight trajectory based on these points; designing a flight trajectory formation control law based on the target trajectory; drawing the actual flight trajectory based on the control law; and controlling the intelligent agent to fly around the obstacle in the moving composite object according to the actual flight trajectory. From a control perspective, this invention ensures that the intelligent agent can quickly and accurately avoid obstacles while operating on a trajectory near an overall moving obstacle.

Description

Technical Field

[0001] This invention relates to the field of motion control technology for intelligent agents / UAVs / spacecraft swarms, and specifically to a method for controlling the flight trajectory of a moving composite body around obstacles. Background Technology

[0002] In research on real-world swarm formation control / trajectory tracking control of drones, spacecraft, and other similar applications, forming a specified motion trajectory in obstacle-prone environments is a crucial research focus. While existing research has considered obstacle trajectory formation control for relatively simple obstacle shapes (such as circles or ellipses), there is a lack of corresponding studies addressing obstacle attitude motion.

[0003] For obstacles with complex shapes or those undergoing motion, existing research generally employs planning rather than control methods to study trajectory formation in their vicinity due to the complexity of potential function construction. While the effectiveness of these algorithms can be verified through numerous simulations, their efficiency is limited. In scenarios similar to the relative motion of spacecraft, spacecraft often have relatively regular geometric shapes, and individual spacecraft typically move as a whole. Therefore, for trajectory formation near obstacles in simple composite structures undergoing overall motion, it is necessary to simplify the process to control the trajectory formation around the obstacle by superimposing several potential functions adapted to its geometric units and using appropriate coordinate transformation relationships. This avoids the need to solve complex planning problems. Summary of the Invention

[0004] To address the problem that existing technologies cannot quickly and efficiently achieve obstacle avoidance requirements when obstacles are complex in shape or moving, this invention provides a method for controlling the trajectory of a moving composite object around obstacles. From a control perspective, this method ensures that the intelligent agent can quickly and accurately avoid obstacles while traversing its trajectory near moving obstacles.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A method for controlling the flight trajectory of a moving composite vehicle near an obstacle, comprising:

[0007] Obtain the relative orbital trajectory points at each time point;

[0008] Draw the target's orbital trajectory based on the relative orbital trajectory points obtained at each time point;

[0009] Design a control law for the formation of the target bypass trajectory;

[0010] The actual flight path is drawn based on the control law formed by the flight path, and the control agent flies around the obstacle of the moving combination body according to the actual flight path.

[0011] As a further improvement of the present invention, the acquisition of the relative orbital trajectory points at each time point is specifically represented as follows:

[0012] Relative orbital trajectory point c B (t k ;p) represents

[0013] c B (t k ;p)=R(I→B)(t k )c(t k ;p)

[0014] Among them, t k Time is t; p is the parameter of the orbital trajectory curve; R(I→B)(t) k R(I→B)(t) is the coordinate transformation matrix from the reference coordinate system to the obstacle's own coordinate system, and R(I→B)(t) is the coordinate transformation matrix from the reference coordinate system to the obstacle's own coordinate system. k The periodic condition R(I→B)(t) is satisfied. k )=R(I→B)(t k +T c );c(t k p) is the original orbital trajectory point, and c(t) k ;p) satisfies the periodic condition c(t) k ;p)=c(t k +T c ;p);T c >0 indicates the period of change in the orbital trajectory.

[0015] As a further improvement of the present invention, the step of drawing the orbital trajectory based on the relative orbital trajectory points at each time moment involves using the relative orbital trajectory points c at each time moment... B (t k ;p) Connect the lines to obtain the target's orbital trajectory. Then, the shape of the target's orbital trajectory within the obstacle system needs to meet the set state.

[0016] If the shape of the target's trajectory within the obstacle system meets the set conditions, no action is taken.

[0017] If the shape of the target's orbital trajectory within the obstacle system does not meet the set parameters, then adjust the relative orbital trajectory point c. B (t k The parameters p in the flight trajectory curve are adjusted until the shape of the target flight trajectory within the obstacle system satisfies the set state.

[0018] As a further improvement of the present invention, the step of drawing the orbital trajectory based on the relative orbital trajectory points at each time point, and then drawing the relative orbital trajectory point c B (t k ;p) distance to the obstacle Follow t k (k = 1, 2, ..., n) t The curve of the change is used to approximate the minimum distance from the target's flight path to the obstacle. And determine the minimum distance from the target's flight path to the obstacle. Is it greater than or equal to the obstacle avoidance detection distance D? O ;

[0019] If the minimum distance from the target's flight trajectory to the obstacle Less than obstacle avoidance detection distance D O Then increase the relative orbital trajectory point c. B (t k The parameters related to the magnitude of the target's orbital trajectory curve are then used to re-observe the distance from the relative orbital trajectory point to the obstacle at each time step, and plot their variation curves until the minimum distance from the target's orbital trajectory to the obstacle is reached. Greater than or equal to obstacle avoidance detection distance D O The final parameters of the orbital trajectory curve are denoted as p. * .

[0020] As a further improvement of the present invention, a control law for the formation of the flight trajectory is designed based on the target flight trajectory. According to the dynamic equation of the flight agent in the obstacle system, the error dynamic system of the flight agent is constructed, and the control law for the formation of the flight trajectory is designed.

[0021] As a further improvement of the present invention, the error dynamics system of the flying intelligent agent is specifically represented as follows:

[0022]

[0023] in, Let be the position error of the agent in the obstacle system at time t. Let be the velocity error of the agent in the obstacle system at time t. Let be the acceleration error of the agent in the obstacle system at time t. Let t be the control quantity of the agent error dynamics system in the obstacle body system at time t.

[0024] As a further improvement of the present invention, the control law for the flight trajectory formation is specifically expressed as follows:

[0025] The control variables of the intelligent agent within the obstacle system:

[0026]

[0027] Control variables of the agent in the reference frame:

[0028] uI (t)=R(B→I)(t)u B (t)

[0029] Where: c″ B (t;p * ) represents c B (t;p * The second derivative with respect to time; Let be the obstacle repulsion potential function; R(I→B)(t) be the coordinate transformation matrix from the reference coordinate system to the obstacle's own system; u B (t) represents the control quantity of the agent in the obstacle system at time t; k p k is the position feedback coefficient. v f is the position and velocity feedback coefficient; B (ξ B (t), ζ B (t) is the open-loop dynamics model of the agent in this system.

[0030] As a further improvement of the present invention, the specific representation of the obstacle repulsive potential function is as follows:

[0031]

[0032] in, For the obstacle Basic geometric units The repulsive potential function.

[0033] As a further improvement of the present invention, the step of drawing the actual flight trajectory based on the control law of the flight trajectory formation means drawing the trajectory of the actual position of the intelligent agent in the obstacle system. If the trajectory of the actual position of the intelligent agent meets the set state, the intelligent agent is controlled to fly around the obstacle of the moving combination body according to the actual flight trajectory.

[0034] As a further improvement of the present invention, the step of drawing the actual flight trajectory according to the flight trajectory formation control law refers to drawing the trajectory of the actual position of the intelligent agent in the obstacle system. If the trajectory of the actual position of the intelligent agent does not meet the set state, the position feedback coefficient and velocity feedback coefficient in the flight trajectory formation control law are modified to form a new flight trajectory formation control law. The actual flight trajectory is redrawn according to the new flight trajectory formation control law until the trajectory of the actual position of the intelligent agent redrawn in the obstacle system meets the set state.

[0035] Compared with the prior art, the present invention has the following beneficial effects.

[0036] This invention provides a method for controlling the trajectory of a moving composite system around obstacles. In the trajectory planning section, by knowing the obstacle's attitude and motion laws, the target trajectory is transformed into a stationary system relative to the obstacle. This facilitates observation of the target trajectory's positional relationship with the obstacle, allowing for a direct assessment of whether the trajectory shape meets the set requirements and obstacle avoidance needs. It also facilitates the use of the distance formula from a point to the obstacle to calculate the distance from the target trajectory's position to the obstacle at various times. In the trajectory formation control section, the problem of controlling the trajectory formation of an obstacle with its attitude and motion is simplified to controlling the trajectory formation near a fixed obstacle. This facilitates the design of a suitable obstacle avoidance motion control law based on the classical artificial potential field method. Therefore, from a control perspective, this invention quickly identifies the trajectory near an obstacle that meets the obstacle avoidance requirements, enabling the agent to quickly and accurately avoid obstacles based on the planned trajectory, protecting the agent from obstacle threats. Attached Figure Description

[0037] Figure 1 This is a flowchart illustrating a method for controlling the trajectory of a moving assembly around an obstacle according to the present invention.

[0038] Figure 2 This is a schematic flowchart illustrating the specific process of a method for controlling the trajectory of a moving composite vehicle around an obstacle according to the present invention.

[0039] Figure 3 This is a schematic diagram of the three azimuth angles of the obstacle body coordinate system relative to the reference coordinate system in this invention;

[0040] Figure 4 This is an image of the orbital trajectory curve model in the reference coordinate system in this embodiment of the invention;

[0041] Figure 5 This is a graph showing the change of the spin angle of the obstacle body coordinate system relative to the reference coordinate system over time in an embodiment of the present invention.

[0042] Figure 6 An image of the target's flight trajectory designed in this embodiment of the invention within the obstacle system;

[0043] Figure 7 This is a curve showing the change in distance from the relative flight trajectory point to the obstacle over time in an embodiment of the present invention;

[0044] Figure 8 Images showing the trajectory of the agent's position at different times in the simulation results;

[0045] Figure 9 This is a graph showing the change of the agent's position deviation magnitude over time in the simulation results;

[0046] Figure 10 The graph shows the change of the agent's control magnitude over time in the simulation results.

[0047] Figure 11 This is a graph showing the change in distance between the agent and obstacles over time in the simulation results. Detailed Implementation

[0048] The present invention will now be described in detail with reference to the accompanying drawings and embodiments. It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other.

[0049] The following detailed description is exemplary and intended to provide further detailed explanation of the invention. Unless otherwise specified, all technical terms used in this invention have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains. The terminology used in this invention is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention.

[0050] To address the problem that existing technologies cannot quickly and efficiently achieve obstacle avoidance requirements when encountering obstacles with complex shapes or those that are moving, this invention provides a method for controlling the flight trajectory of a moving composite vehicle near obstacles, such as... Figure 1 As shown, the method includes:

[0051] Obtain the relative orbital trajectory points at each time point;

[0052] Draw the target's orbital trajectory based on the relative orbital trajectory points obtained at each time point;

[0053] Design a control law for the formation of the target bypass trajectory;

[0054] The actual flight path is drawn based on the control law formed by the flight path, and the control agent flies around the obstacle of the moving combination body according to the actual flight path.

[0055] The invention will be further explained below with reference to the specific accompanying drawings:

[0056] like Figure 2 As shown, a method for controlling the flight trajectory of a moving composite vehicle near an obstacle includes:

[0057] S1: Given the obstacle model and obstacle avoidance detection distance D O The model of the flight trajectory curve c(t; p) with time as the independent variable, the law of change of the azimuth angle of the obstacle system relative to the reference coordinate system, and the dynamic equation of the flight agent.

[0058] The azimuth angles of the obstacle system relative to the reference coordinate system include the spin angle γ(t), the nutation angle β(t), and the precession angle α(t). The variation patterns of these three azimuth angles can be explicitly expressed as second-order differentiable functions of time t, or given as a system of second-order differential equations with respect to time t. Both of these variation patterns satisfy periodicity, and their period T is constant. a With the orbital period T c The following compatibility conditions must be met

[0059]

[0060] Where n a n c It is a positive integer constant.

[0061] Among them, obstacles Depend on It consists of several basic geometric units, and any point ξ in the obstacle body coordinate system (i.e., the coordinate system in which the obstacle remains stationary, hereinafter referred to as this system, denoted as B) B To the obstacle Basic geometric units distance All can be analytically represented, and ξ can be expressed analytically. B Find the first derivative.

[0062] Any point ξ in this system B To the obstacle The distance can be expressed in the following form

[0063]

[0064] The orbital trajectory curve has time t as the independent variable and p as the parameter, where p contains several parameters related to the magnitude of the orbital trajectory. The orbital trajectory curve satisfies the following periodic condition.

[0065] c(t+T c ;p)=c(t;p) (3)

[0066] Where T c >0 indicates the period of change in the orbital trajectory.

[0067] The three azimuth angles (hereinafter referred to as azimuth angles) of the obstacle's coordinate system relative to the reference coordinate system (hereinafter referred to as the reference system, denoted as I) are as follows: Figure 3 The figures shown represent the spin angle γ(t), nutation angle β(t), and precession angle α(t), respectively. These are the coordinates ξ in this system. B It can be transformed into the reference system according to the following relationship.

[0068] ξ I =R(B→I)ξB (4)

[0069] Where R(B→I)(t) is the coordinate transformation matrix from the local system to the reference system at time t, it can be expressed in the following form

[0070] R(B→I)(t)=R z (α(t))R x (β(t))R z (γ(t)) (5)

[0071] Accordingly, there is a coordinate transformation matrix from the reference system to this system of the following form.

[0072] R(I→B)(t)=(R(B→I)(t)) -1 =R z (-γ(t))R x (-β(t))R z (-α(t)) (6)

[0073] Where R z (.) and R x (·) are the rotation matrices about the current z-axis and the current x-axis, respectively, and can be represented as follows:

[0074]

[0075] The variation patterns of the three azimuth angles mentioned above can be explicitly expressed as a second-differentiable function of time t.

[0076]

[0077] Alternatively, it can be given in the form of a system of second-order differential equations with respect to time t.

[0078]

[0079] And given γ(0), β(0), α(0), v γ (0), v β (0), v α (0).

[0080] Both of the above-mentioned patterns of change satisfy the following periodic conditions.

[0081]

[0082] Where T a >0 represents the azimuth change period, which is related to the flight trajectory period T. c The following compatibility conditions must be met

[0083]

[0084] Where na n c It is a positive integer constant.

[0085] The dynamic equations of the flying intelligent agent can be expressed in the reference frame as follows:

[0086]

[0087] Where ξ I (t), ζ I (t), and u I (t) represents the agent's position, velocity, acceleration, and control variables in the reference frame at time t, f I (ξ I (t), ζ I (t) is the open-loop dynamics model of the agent in the reference frame.

[0088] Correspondingly, the dynamic equations of the circling intelligent agent in this system are as follows:

[0089]

[0090] Where ξ B (t), ζ B (t), and u B (t) represents the agent's position, velocity, acceleration, and control variables in the system at time t, f B (ξ B (t), ζ B (t) is the open-loop dynamics model of the agent in this system.

[0091] The following formula can be obtained from the coordinate transformation relationship.

[0092] ξ B (t)=R(I→B)(t)ξ I (t) (13)

[0093]

[0094]

[0095]

[0096] u B (t)=R(I→B)(t)u I (t) (17) where and The first and second derivatives of R(B→I) with respect to time are respectively,

[0097]

[0098]

[0099] R′ z (.) and R′ x (·) represent R respectively z (·) and R x (·) The first derivative matrix with respect to the independent variable can be expressed as follows:

[0100]

[0101] R″ z (·) and R″ x (·) represent R respectively z (·) and R x (·) The second derivative matrix with respect to the independent variable can be expressed as follows:

[0102]

[0103] In particular, if only the spin angle among the three azimuth angles changes with time, i.e., β(t) = β and α(t) = α, then formulas (18) and (19) can be rewritten as follows:

[0104]

[0105]

[0106] S2: Calculate the azimuth angle and coordinate transformation matrix at a series of time points.

[0107] Specifically, for azimuth angles, the following two models are used for calculation based on different azimuth angle variation patterns:

[0108] If the azimuth angle is explicitly expressed as a function of time t, then in [0, k T n ac Select n uniformly within the interval t At time t k (k = 1, 2, ..., n) t n ac For n in formula (10) a and n c (the least common multiple), and calculate t at each time point respectively. k The spin angle γ(t) below k ), nutation angle β(t) k ) and precession angle α(t) k ).

[0109] If the azimuth angle is given by the system of differential equations (8) in terms of time t, then for this system of differential equations in [0, k T nac Numerical integration is performed over a time interval to obtain a series of time points t. k (k = 1, 2, ..., n) t Spin angle γ(t) under ) k ), nutation angle β(t) k ) and precession angle α(t) k ).

[0110] Specifically, for t k The coordinate transformation matrix at time t can be obtained by replacing t with t k Based on this, the coordinate transformation matrix R(B→I)(t) from the local system to the reference system is calculated according to formula (5). k The coordinate transformation matrix from the reference system to the local system can be obtained by formula (6).

[0111] S3: Calculate the relative flight trajectory points at a series of time points, and the distance from each relative flight trajectory point to the obstacle.

[0112] Specifically, in t k At time c, relative to the orbital trajectory point B (t k ;p) can be represented as

[0113] c B (t k ;p)=R(I→B)(t k )c(t k ;p) (22)

[0114] Among them, t k Time is t; p is the parameter of the orbital trajectory curve; R(I→B)(t) k R(I→B)(t) is the coordinate transformation matrix from the reference coordinate system to the obstacle's own coordinate system, and R(I→B)(t) is the coordinate transformation matrix from the reference coordinate system to the obstacle's own coordinate system. k The periodic condition R(I→B)(t) is satisfied. k )=R(I→B)(t k +T c );c(t k p) is the original orbital trajectory point, and c(t) k ;p) satisfies the periodic condition c(t) k ;p)=c(t k +T c ;p);T c >0 indicates the period of change in the orbital trajectory.

[0115] Relative orbital trajectory point c B (t k Distance to the obstacle The distance from the point to the obstacle is calculated using the formula given in S1.

[0116] S4: Draw the target's orbital trajectory in this system and determine whether it meets the user's requirements.

[0117] Specifically, the target orbital trajectory refers to the relative orbital trajectory point c at each moment. B (t k The line connecting p). If its shape in this system meets the user's requirements, no processing is required; otherwise, the user's requirements are met by adjusting the parameters p of the flight trajectory curve.

[0118] S5: Plot the curve of the distance from the target's flight path to the obstacle over time, and determine whether the obstacle avoidance constraint is met.

[0119] Specifically, draw the relative orbital trajectory point c. B (t k ;p) distance to the obstacle Follow t k (k = 1, 2, ..., n) t The curve of change of ) in which By using ξ in formula (2) B Replace with c B (t k The minimum distance from the flight trajectory to the obstacle is then approximated by the image.

[0120]

[0121] Determine the following constraints.

[0122]

[0123] Is it true? If not, appropriately increase the magnitude-related parameters in the orbital trajectory curve parameter p, and then return to S3 to continue execution until constraint condition (24) is satisfied. The final orbital trajectory curve parameter is denoted as p. * .

[0124] S6: Design the flight trajectory to form the control law.

[0125] Specifically, referring to formula (12), a flight-around intelligent agent error dynamics system is constructed in this system.

[0126]

[0127] in Let c′ represent the position error, velocity error, and acceleration error of the agent in the system at time t, respectively. B (t;p * ) and c″ B (t;p *) represent c respectively B (t;p * The first and second derivatives with respect to time are calculated using the following formulas.

[0128]

[0129]

[0130] Let uB(t) be the error system control quantity at time t, and it has the following transformation relationship with uB(t).

[0131]

[0132] Based on proportional-derivative control and the artificial potential field method, the control quantity of the error system can be designed as follows:

[0133]

[0134] The corresponding control variable u of the intelligent agent in this system B (t) and the control quantity u in the reference system I (t) are respectively

[0135]

[0136] u I (t)=R(B→I)(t)u B (t) (31)

[0137] Where k p and k v These are the position feedback coefficient and the velocity feedback coefficient, respectively. Let the obstacle repulsion potential function be of the following form.

[0138]

[0139] For the obstacle Basic geometric units The repulsive potential function in the obstacle system can be expressed as follows:

[0140]

[0141] c O This is referred to here as the obstacle avoidance coefficient. According to stability theory, when k... p k v c O When all values ​​are greater than 0, the stability of the closed-loop system can be guaranteed.

[0142] S7: Perform motion simulation of the flight trajectory formation and determine whether the simulation results meet the user's requirements.

[0143] Specifically, the dynamic equation (12) of the flying agent substituted into the control law (30) (or the dynamic equation (11) of the flying agent substituted into the control law (31)) is integrated with formula (7) or (8), and then the actual position ξ of the agent obtained is plotted in this system. B The trajectory of (t). If the simulation results meet the user's requirements, the process ends; otherwise, by adjusting the parameters in S6, S7 is re-executed until the user's requirements are met.

[0144] The present invention will be further explained below with reference to specific embodiments.

[0145] Example

[0146] S1: Given the obstacle model and obstacle avoidance detection distance D O The model of the flight trajectory curve c(t; p) with time as the independent variable, the law of change of the azimuth angle of the obstacle system relative to the reference coordinate system, and the dynamic equation of the flight agent.

[0147] Specifically, the obstacle model is a simplified satellite. The model consists of two cuboids, the centers of which coincide with the obstacle's main system. The length, width, and height are aligned with the x, y, and z axes of the obstacle's main system, respectively, and are 2.2m × 2.22m × 1.72m and 2.2m × 18.096m × 0.05m.

[0148] Specifically, obstacle avoidance detection distance D O The value is set to 8m.

[0149] Specifically, the variation law of the azimuth angle of the obstacle system relative to the reference coordinate system is represented by the following system of differential equations.

[0150]

[0151] Given γ(0) = 0, β(0) = 41.581°, α(0) = 0, v γ (0) = 0, v β (0) = 0, v α (0) = 0, e = 8.0531 × 10 -4 , a=6767.587km, μ=398600km 3 / s 2 According to orbital mechanics, the azimuth angle change period...

[0152] Specifically, the orbital trajectory curve model is designed as a spatial ellipse equation of the following form.

[0153] c(t;p)=ξ C +a·cos(ωt)+b·sin(ωt) (35)whereξ C Let ξ be the coordinates of the center of the orbital trajectory, and let a and b be the major and minor axis vectors, respectively (related to the size and shape of the orbital trajectory). These three vectors are represented by ξ in the reference system. C = (0, 0, 20m) T a = (20m, 0, 0) T b = (0, 20m, 0) T . For the angular velocity of the orbital trajectory, T c =T a The period of change of the orbital trajectory. The shape of the orbital trajectory curve model in the reference coordinate system is as follows: Figure 4 As shown (the obstacle is in its initial position).

[0154] The parameter p of the orbital trajectory curve is taken as follows:

[0155]

[0156] Specifically, based on formula (11), the dynamic equations of the orbital trajectory are designed according to the two-body dynamics model, and the open-loop dynamics model is further refined as follows:

[0157]

[0158] in

[0159]

[0160] The reference spacecraft's position vector in the geocentric inertial frame.

[0161] S2: Calculate the azimuth angle and coordinate transformation matrix at a series of time points.

[0162] Specifically, since the azimuth variation law cannot be explicitly expressed in the form of time as the independent variable, we use the ode45 integrator to express the differential equation system (34) of the azimuth variation law in [0, T]. a Integrate over the time interval to obtain n. t =Azimuth angles at 81 time points, including t k Time (k = 1, 2, ..., n) t Spin angle γ(t) under ) k ), nutation angle β(t) k ) and precession angle α(t) k The coordinate transformation matrix R(B→I)(t) from the local system to the reference system at each corresponding time point. k The coordinate transformation matrix R(I→B)(t) from the reference system to the local system at each time step can be obtained according to formula (5).k The spin angle can be calculated using formula (6). The curve showing the change of spin angle over time is as follows: Figure 5 As shown (since the nutation angle and precession angle do not change with time, they are not shown).

[0163] S3: Calculate the relative flight trajectory points at a series of time points, and the distance from each relative flight trajectory point to the obstacle.

[0164] Specifically, based on the azimuth angles obtained in the previous step, t is calculated using formula (22). k The relative orbital trajectory point c at time t B (t k Then, based on the formula for the distance from the point to the obstacle given in S1, the relative orbital trajectory point c is calculated. B (t k Distance to the obstacle

[0165] S4: Draw the target's orbital trajectory in this system and determine whether it meets the user's requirements.

[0166] Specifically, to visually demonstrate the distribution of the target's orbital trajectory relative to the obstacle, we simultaneously plotted the obstacle and the point c relative to the orbital trajectory at each time point within the obstacle system. B (t k The line connecting p) is as follows: Figure 6 As shown. We believe the target's orbital trajectory meets the requirements, therefore we will not adjust the orbital trajectory curve parameter p for now. Otherwise, we could adjust p to meet the requirements.

[0167] S5: Plot the curve of the distance from the target's flight path to the obstacle over time, and determine whether the obstacle avoidance constraint is met.

[0168] Specifically, draw the relative orbital trajectory point c. B (t k ;p) distance to the obstacle Follow t k (k = 1, 2, ..., n) t The curve of change, such as Figure 7 As shown in the figure, the minimum distance from the target's flight path to the obstacle is... Therefore, the obstacle avoidance constraint is satisfied. Otherwise, the constraint can be satisfied by increasing the parameters a and b, which are related to the size of the flight path.

[0169] S6: Design the flight trajectory to form the control law.

[0170] Specifically, the control quantity u of the intelligent agent in this system B (t) and the control quantity u in the reference system I(t) Design according to formula (30) and formula (31) respectively, and try to take k p =10 -4 s -2 k v =10 -2 s -1 c O =1m 4 / s 2 .

[0171] S7: Perform motion simulation of the flight trajectory formation and determine whether the simulation results meet the user's requirements.

[0172] Specifically, the dynamic equation (12) of the flying agent substituted into the control law (30) is integrated with the formula (34), or the dynamic equation (11) of the flying agent substituted into the control law (31) is integrated with the formula (34), and then the obtained agent position ξ is plotted in this system. B The trajectory of (t) at different times is shown in the graph. Figure 8 The blue curve in the figure is shown. The corresponding curves of position deviation magnitude, control amplitude, and distance between the agent and the obstacle over time are shown in the figure. Figure 9 , Figure 10 and Figure 11 As shown, the agent can converge to the designed target trajectory with high accuracy and maintain a sufficient distance from obstacles. Therefore, the simulation results are considered to meet user requirements, and the simulation ends. Alternatively, by adjusting the parameters in S6 and re-executing S7, user requirements can be better met.

[0173] The second objective of this invention is to provide a flight trajectory control system for a moving assembly near an obstacle, comprising:

[0174] Acquisition module: used to acquire the relative orbital trajectory points at various time points;

[0175] Drawing module: used to draw the target's orbital trajectory based on the relative orbital trajectory points at various times;

[0176] Design module: Used to design the control law for the formation of the flight trajectory based on the target flight trajectory;

[0177] Control module: Used to draw the actual flight path based on the control law formed by the flight path, and control the intelligent agent to fly around the obstacle of the moving combination body according to the actual flight path.

[0178] A third objective of this invention is to provide an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method for controlling the trajectory of a moving assembly around an obstacle.

[0179] The method for controlling the flight trajectory of a moving composite vehicle near an obstacle includes the following steps:

[0180] Obtain the relative orbital trajectory points at each time point;

[0181] Draw the target's orbital trajectory based on the relative orbital trajectory points obtained at each time point;

[0182] Design a control law for the formation of the target bypass trajectory;

[0183] The actual flight path is drawn based on the control law formed by the flight path, and the control agent flies around the obstacle of the moving combination body according to the actual flight path.

[0184] A fourth objective of this invention is to provide a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the method for controlling the trajectory of a moving assembly around an obstacle.

[0185] The method for controlling the flight trajectory of a moving composite vehicle near an obstacle includes the following steps:

[0186] Obtain the relative orbital trajectory points at each time point;

[0187] Draw the target's orbital trajectory based on the relative orbital trajectory points obtained at each time point;

[0188] Design a control law for the formation of the target bypass trajectory;

[0189] The actual flight path is drawn based on the control law formed by the flight path, and the control agent flies around the obstacle of the moving combination body according to the actual flight path.

[0190] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0191] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0192] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0193] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0194] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A method for controlling the flight trajectory of a moving composite vehicle near an obstacle, characterized in that, include: Obtain the relative orbital trajectory points at each time point; Draw the target's orbital trajectory based on the relative orbital trajectory points obtained at each time point; Design a control law for the formation of the target bypass trajectory; The actual flight path is drawn based on the control law formed by the flight path, and the control agent flies around the obstacle of the moving combination body according to the actual flight path; The acquisition of the relative orbital trajectory points at each time point is specifically represented as follows: Relative flight trajectory points Represented as in, For a specific moment; These are the parameters for the orbital trajectory curve; Let be the coordinate transformation matrix from the reference coordinate system to the obstacle's own coordinate system, and Satisfying the periodic condition ; The original flight trajectory point, and Satisfying the periodic condition = ; The period of change of the orbital trajectory; The process involves drawing the target's orbital trajectory based on the relative orbital trajectory points acquired at each time point, followed by drawing the relative orbital trajectory points. Distance to the obstacle Follow , The change curve is then used to approximate the minimum distance from the target's flight trajectory to the obstacle using the image. And determine the minimum distance from the target's flight path to the obstacle. Is it greater than or equal to the obstacle avoidance detection distance? ; If the minimum distance between the target's flight trajectory and the obstacle Less than obstacle avoidance detection distance This increases the relative orbital point. Mid-flight trajectory curve parameters The parameters related to the target's size are then used to re-observe the distance from the relative flight trajectory point to the obstacle at each time step, and the change curves are plotted until the minimum distance from the target's flight trajectory to the obstacle is reached. Greater than or equal to obstacle avoidance detection distance The final parameters of the orbital trajectory curve are denoted as ; The control law for the flight trajectory is specifically expressed as follows: The control variables of the intelligent agent in the obstacle system: Control variables of the agent in the reference frame: in: express The second derivative with respect to time; Let be the repulsive potential function of the obstacle; This is the coordinate transformation matrix from the reference coordinate system to the obstacle's own coordinate system; for The agent controls the amount of data within the obstacle system at any given moment; This is the position feedback coefficient; This refers to the position and velocity feedback coefficient. This is an open-loop dynamic model of the agent in this system; for The positional error of the agent within the obstacle system at any given moment. for The speed error of the intelligent agent in the obstacle system at any given moment; The specific representation of the obstacle repulsive potential function is as follows: in, For the obstacle Basic geometric units The repulsive potential function, where, .

2. The method for controlling the flight trajectory of a moving composite near an obstacle according to claim 1, characterized in that, The step of drawing the target's orbital trajectory based on the relative orbital trajectory points at each time point involves using the relative orbital trajectory points at each time point... The target's orbital trajectory is obtained by connecting the lines. Then, the shape of the target's orbital trajectory within the obstacle system needs to meet the set state. If the shape of the target's trajectory within the obstacle system meets the set conditions, no action is taken. If the shape of the target's orbital trajectory within the obstacle system does not meet the set parameters, then adjust the relative orbital trajectory points. Mid-flight trajectory curve parameters The target's trajectory continues until the shape of its orbital path within the obstacle system satisfies the set parameters.

3. The method for controlling the flight trajectory of a moving composite vehicle near an obstacle according to claim 1, characterized in that, Based on the target bypass trajectory, a bypass trajectory formation control law is designed. According to the dynamic equation of the bypass agent in the obstacle system, the error dynamic system of the bypass agent is constructed, and the bypass trajectory formation control law is designed.

4. The method for controlling the flight trajectory of a moving composite vehicle near an obstacle according to claim 1, characterized in that, The error dynamics system of the flying agent is specifically represented as follows: in, for The positional error of the agent within the obstacle system at any given moment. for The speed error of the agent within the obstacle system at any given moment. for The acceleration error of the agent within the obstacle system at any given moment. for The control quantity of the agent error dynamics system at a given time in the obstacle body system.

5. The method for controlling the flight trajectory of a moving composite vehicle near an obstacle according to claim 1, characterized in that, The process of drawing the actual flight trajectory based on the control law formed by the flight trajectory refers to drawing the trajectory of the actual position of the intelligent agent in the obstacle system. If the trajectory of the actual position of the intelligent agent meets the set state, the intelligent agent is controlled to fly around the obstacle of the moving assembly according to the actual flight trajectory.

6. The method for controlling the flight trajectory around obstacles of a moving composite body according to claim 1, characterized in that... The process of drawing the actual flight trajectory based on the flight trajectory formation control law refers to drawing the trajectory of the actual position of the intelligent agent within the obstacle system. If the trajectory of the actual position of the intelligent agent does not meet the set state, the position feedback coefficient and velocity feedback coefficient in the flight trajectory formation control law are modified to form a new flight trajectory formation control law. The actual flight trajectory is then redrawn based on the new flight trajectory formation control law until the trajectory of the actual position of the intelligent agent redrawn within the obstacle system meets the set state.

Images