Unmanned vehicle track estimation method and system, computer equipment and storage medium

Abstract

The invention provides an unmanned vehicle track estimation method and system, computer equipment and a storage medium, and belongs to the field of unmanned vehicle system simulation and estimation, and the method comprises the steps: initializing the related parameters of an unmanned vehicle, and enabling the parameters to meet the ellipsoid constraint conditions; and setting the maximum simulation time length, and determining an estimator gain matrix at the kth moment by utilizing an optimization problem based on related parameters. And constructing a dynamic model of the unmanned vehicle system at the (k + 1) th moment, and obtaining a non-fragile ellipsoid set membership estimator at the (k + 1) th moment together with the gain matrix. And related parameters at the (k + 1) th moment are obtained through non-fragile ellipsoid set membership estimation at the (k + 1) th moment, and the non-fragile ellipsoid set membership estimator is iteratively updated according to a judgment condition, so that the track estimation accuracy of the unmanned vehicle is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned vehicle system simulation and estimation, and specifically relates to an unmanned vehicle trajectory estimation method, system, computer equipment and storage medium. Background Art

[0002] With the rapid development of information technology, unmanned vehicles have been widely used in logistics and transportation fields such as docks, airports, and warehouses. The trajectory estimation problem of unmanned vehicles is the core issue of unmanned vehicles.

[0003] Since there is a certain time lag when the actuator responds to the control command, the steering oscillation of the unmanned vehicle is more likely to occur. Due to the limitation of network bandwidth, the measurement information of the unmanned vehicle needs to be quantized before being transmitted through the network. In addition, when the unmanned vehicle transmits measurement information, the implementation of the estimator may be affected by parameter fluctuations caused by digital rounding errors, component aging, and digital-to-analog conversion accuracy. The estimator is sensitive to small changes or uncertainties in the parameters. At this time, the estimator is non-fragile, resulting in reduced estimation performance accuracy. Summary of the invention

[0004] In order to reduce the impact of quantization error and estimator gain perturbation on the boundedness of unmanned vehicle trajectory estimation, and to tolerate the occurrence of process noise, measurement noise, and time lag, the present invention provides an unmanned vehicle trajectory estimation method, system, computer device, and storage medium.

[0005] In order to achieve the above object, the present invention provides the following technical solutions:

[0006] A method for estimating a trajectory of an unmanned vehicle comprises the following steps:

[0007] Obtaining the position parameters of the unmanned vehicle at the kth moment and their estimated values ​​so that they satisfy the ellipsoid constraint condition, and determining the estimator gain matrix at the kth moment based on the position parameters of the unmanned vehicle at the kth moment and their estimated values ​​by using the optimization problem;

[0008] According to the estimator gain matrix at the kth moment, considering the influence of process noise and measurement noise at the moment, constructing a dynamic model of the unmanned vehicle system at the k+1th moment;

[0009] According to the estimator gain matrix at the kth time and the dynamic model of the unmanned vehicle system at the k+1th time, a non-fragile ellipsoid set membership estimator at the k+1th time is constructed;

[0010] Determining an estimated value of the unmanned vehicle at the k+1th time according to the non-fragile ellipsoid set membership estimator at the k+1th time;

[0011] The displacement trajectory of the unmanned vehicle is determined according to the estimated value of the non-fragile ellipsoid set membership estimator.

[0012] Preferably, the ellipsoid constraint condition is specifically:

[0013]

[0014] In the formula, is the position state of the unmanned vehicle at the kth moment; is η k The estimated value of x k is the horizontal displacement of the unmanned vehicle at the kth moment; y k is the vertical displacement of the unmanned vehicle at the kth moment; is the yaw angle of the unmanned vehicle at the kth moment; s k is the sideslip angle of the unmanned vehicle at the kth moment; γ k is the yaw rate of the unmanned vehicle at the kth moment; For x k An estimated value of for y k An estimated value of for An estimated value of For k An estimated value of For γ k The estimated value of ; d>0 is a constant time lag; P k is the positive definite matrix at the kth moment; P k The inverse matrix of .

[0015] Preferably, the optimization problem is specifically:

[0016]

[0017] So that:

[0018]

[0019] In the formula, min{} is the minimum value function; G k The transpose of For L k The transpose of G k and L k is the estimator gain matrix at the kth moment; ρ x and ρ y are the quantization densities in the horizontal and vertical directions respectively; u x0 and u y0 are the maximum quantization values ​​of the quantization levels in the horizontal and vertical directions respectively; τ i (i=1,2,…,8) are all positive real numbers; P k+1is the positive definite matrix at the k+1th moment; trace{P k+1} is P k+1 traces; E k for The factor matrix of For E k The transpose of E k-d for The factor matrix, P k-d is a positive definite symmetric matrix at the kdth moment, For E k-d The transpose of is the estimated value of the position state of the unmanned vehicle at the kdth moment The transpose of is the control input u at the kth moment k =[vδ] T The transpose of , v is the speed of the unmanned vehicle, δ is the front wheel steering angle of the unmanned vehicle; M1, M2, N1 and N2 are all real matrices; N1 T is the transpose of N1; N2 T is the transpose of N2; Q k Inverse matrix; For R k+1 The inverse matrix of k and R k+1 are the positive definite matrices at the kth moment and the k+1th moment respectively; I is the identity matrix; is the discretized system matrix The transpose of is the delay matrix A d The transpose of is the discretized control input matrix The transpose of is the measurement matrix C p The transpose of is the process noise distribution matrix D p The transpose of is the measurement noise distribution matrix F p The transpose of s is the sampling time, e=2.718281828459045 is a natural constant, and the system matrix A and control input matrix B are:

[0020]

[0021] Where m is the mass of the unmanned vehicle; I z is the inertia matrix of the unmanned vehicle; C f and C r They represent the turning stiffness of the front and rear tires of the unmanned vehicle respectively; l1 and l2 represent the distance from the center of gravity of the unmanned vehicle to the front and rear respectively.

[0022] Preferably, the dynamic model of the unmanned vehicle system at the k+1th moment is specifically:

[0023] η k+1 =A p η k +A d η k-d +B p u k +D p w k

[0024] ξ k+1 =C p η k+1 +F p μ k+1 +Δ k+1

[0025]

[0026]

[0027] Where η k+1 is the position state of the unmanned vehicle at the k+1th moment; is the measurement noise of the unmanned vehicle at the kth moment, w x and w y is the process noise of the unmanned vehicle in the horizontal and vertical directions; is the process noise corresponding to the yaw angle of the unmanned vehicle, w s is the process noise corresponding to the sideslip angle of the unmanned vehicle, w γ is the process noise corresponding to the yaw rate of the unmanned vehicle; ξ k+1 is the logarithmic quantization value of the measurement output of the unmanned vehicle at the k+1th moment; μ k+1 =[μ x μ y ] T is the measurement noise of the unmanned vehicle at the k+1th moment, μ x and μ y are the measurement noise of the unmanned vehicle in the horizontal and vertical directions respectively; is the quantization parameter matrix, and are the quantization parameters in the horizontal and vertical directions respectively; μ k is the measurement noise of the unmanned vehicle at the kth moment; μ k The transpose of w k The transpose of .

[0028] Preferably, the constructing of a non-fragile ellipsoid set membership estimator at the k+1th moment is specifically as follows:

[0029]

[0030] In the formula, is η k+1 The estimated value of G k and L k are the estimator gain matrices at the kth moment; ΔG k =G k M1F 1,k N1 and ΔL k =L k M2F 2,k N2 are the estimator gain perturbations at the kth moment, F 1,k and F 2,k are all time-varying matrices and satisfy and

[0031] Preferably, determining the estimated value of the position parameter of the unmanned vehicle at the k+1th time according to the non-fragile ellipsoid set membership estimator at the k+1th time specifically includes:

[0032] According to the estimated value of the non-fragile ellipsoid set at the k+1th moment, the estimated value of the position state of the unmanned vehicle system at the kth moment, and the estimator gain matrix G at the kth moment k and L k , the perturbation of the estimator gain at the kth moment, the discretized control input matrix B, and the logarithmic quantization of the measured output of the unmanned vehicle at the k+1th moment ξ k+1 , determine the estimated value of the horizontal displacement, the estimated value of the vertical displacement, the estimated value of the yaw angle, the estimated value of the sideslip angle, and the estimated value of the yaw rate of the unmanned vehicle at the k+1th moment.

[0033] The present invention also provides an unmanned vehicle trajectory estimation system, comprising:

[0034] The estimator gain matrix determination module is used to obtain the position parameters of the unmanned vehicle at the kth moment and their estimated values ​​so that they meet the ellipsoid constraint conditions, and based on the position parameters of the unmanned vehicle at the kth moment and their estimated values, determine the estimator gain matrix at the kth moment using the optimization problem.

[0035] A dynamic model building module is used to build a dynamic model of the unmanned vehicle system at the k+1th moment based on the estimator gain matrix at the kth moment, taking into account the influence of process noise and measurement noise at the moment;

[0036] The non-fragile ellipsoid set membership estimator determination module is used to construct a non-fragile ellipsoid set membership estimator at the k+1th moment according to the estimator gain matrix at the kth moment and the dynamic model of the unmanned vehicle system at the k+1th moment.

[0037] An estimated value determination module is used to determine the estimated value of the position parameter of the unmanned vehicle at the k+1th moment according to the non-fragile ellipsoid set membership estimator at the k+1th moment.

[0038] The trajectory acquisition module is used to determine the displacement trajectory of the unmanned vehicle according to the estimated value of the position parameter of the unmanned vehicle by the non-fragile ellipsoid set membership estimator.

[0039] The present invention also provides a computer device, comprising a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps described in the unmanned vehicle trajectory estimation method.

[0040] The present invention also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is loaded by a processor, the steps described in the unmanned vehicle trajectory estimation method can be executed.

[0041] The unmanned vehicle trajectory estimation method, system, computer device and storage medium provided by the present invention have the following beneficial effects:

[0042] The present invention obtains the estimator gain matrix of the unmanned vehicle by obtaining the position parameters of the unmanned vehicle and their estimated values, including the displacement of the unmanned vehicle in the horizontal direction and the displacement in the vertical direction, yaw angle, sideslip angle and yaw rate. Considering the influence of process noise, measurement noise and time lag on the estimation performance of the unmanned vehicle, a dynamic model of the unmanned vehicle system is established. The position estimate of the unmanned vehicle at the next moment is determined according to the estimator gain matrix and the dynamic model. After obtaining the estimated value within a period of time, the displacement trajectory of the unmanned vehicle is judged. Compared with other unmanned vehicle trajectory estimation methods, the present invention achieves the purpose of anti-disturbance, and has the advantages of being easy to solve and implement, thereby improving the accuracy of unmanned vehicle trajectory estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] In order to more clearly illustrate the embodiment of the present invention and its design scheme, the following briefly introduces the drawings required for this embodiment. The drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0044] Figure 1 The present invention is a flow chart of a method for estimating the trajectory of an unmanned vehicle.

[0045] Figure 2 is the displacement of the unmanned vehicle and the phase plane diagram of the ellipsoid set.

[0046] Figure 3 It is the trajectory diagram of the unmanned vehicle’s horizontal displacement, estimated value and its upper and lower bounds.

[0047] Figure 4 It is the trajectory diagram of the unmanned vehicle’s vertical displacement, estimated value and its upper and lower bounds. DETAILED DESCRIPTION

[0048] In order to enable those skilled in the art to better understand the technical solution of the present invention and implement it, the present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the scope of protection of the present invention.

[0049] Example

[0050] The present invention provides a method for estimating the trajectory of an unmanned vehicle, specifically Figure 1 As shown, the following steps are included:

[0051] S1. Obtain the horizontal displacement and its estimated value, the vertical displacement and its estimated value, the yaw angle and its estimated value, the sideslip angle and its estimated value, and the yaw rate and its estimated value of the unmanned vehicle at the kth moment, so that the position parameters and their estimated values ​​of the unmanned vehicle at the kth moment satisfy the following specific ellipsoid constraint conditions:

[0052]

[0053] in, is the position state of the unmanned vehicle at the kth moment, is η k The estimated value of x k is the horizontal displacement of the unmanned vehicle at the kth moment; y k is the vertical displacement of the unmanned vehicle at the kth moment; is the yaw angle of the unmanned vehicle at the kth moment; s k is the sideslip angle of the unmanned vehicle at the kth moment; γ k is the yaw rate of the unmanned vehicle at the kth moment; For x k An estimated value of for y k An estimated value of for An estimated value of For k An estimated value of For γ k The estimated value of ; d>0 is a constant time lag; P k is the positive definite matrix at the kth moment; P kThe inverse matrix of .

[0054] And set k = 1 and its maximum simulation time k max .

[0055] S2. Based on the position parameters of the unmanned vehicle at the kth moment, the estimator gain matrix at the kth moment is determined by using an optimization problem. The specific optimization problem is:

[0056]

[0057] So that:

[0058]

[0059] Among them, min{} is the minimum value function; G k The transpose of For L k The transpose of G k and L k is the estimator gain matrix at the kth moment; ρ x and ρ y are the quantization densities in the horizontal and vertical directions respectively; u x0 and u y0 are the maximum quantization values ​​of the quantization levels in the horizontal and vertical directions respectively; τ i (i=1,2,…,8) are all positive real numbers; P k+1 is the positive definite matrix at the k+1th moment; trace{P k+1} is P k+1 traces; E k for The factor matrix of For E k The transpose of E k-d for The factor matrix, P k-d is a positive definite symmetric matrix at the kdth moment, For E k-d The transpose of is the estimated value of the position state of the unmanned vehicle at the kdth moment The transpose of is the control input u at the kth moment k =[vδ] T The transpose of , v is the speed of the unmanned vehicle, δ is the front wheel steering angle of the unmanned vehicle; M1, M2, N1 and N2 are all real matrices; N1 T is the transpose of N1; N2 T is the transpose of N2; Q k Inverse matrix; For R k+1The inverse matrix of k and R k+1 are the positive definite matrices at the kth moment and the k+1th moment respectively; I is the identity matrix; is the discretized system matrix The transpose of is the delay matrix A d The transpose of is the discretized control input matrix The transpose of is the measurement matrix C p The transpose of is the process noise distribution matrix D p The transpose of is the measurement noise distribution matrix F p The transpose of s is the sampling time, e=2.718281828459045 is a natural constant; the system matrix A and the control input matrix B are as follows:

[0060]

[0061] Where m is the mass of the unmanned vehicle; I z is the inertia matrix of the unmanned vehicle; C f and C r They represent the turning stiffness of the front and rear tires of the unmanned vehicle respectively; l1 and l2 represent the distance from the center of gravity of the unmanned vehicle to the front and rear respectively.

[0062] S3. Based on the estimator gain matrix at the kth moment, considering the influence of process noise and measurement noise at this moment, the dynamic model of the unmanned vehicle system at the k+1th moment is constructed, specifically:

[0063] η k+1 =A p η k +A d η k-d +B p u k +D p w k

[0064] ξ k+1 =C p η k+1 +F p μ k+1 +Δ k+1

[0065]

[0066]

[0067] Among them, ηk+1 is the position state of the unmanned vehicle at the k+1th moment; is the measurement noise of the unmanned vehicle at the kth moment; w x and w y is the process noise of the unmanned vehicle in the horizontal and vertical directions; is the process noise corresponding to the yaw angle of the unmanned vehicle; w s is the process noise corresponding to the sideslip angle of the unmanned vehicle; w γ is the process noise corresponding to the yaw rate of the unmanned vehicle; ξ k+1 is the logarithmic quantization value of the measurement output of the unmanned vehicle at the k+1th moment; μ k+1 =[μ x μ y ] T is the measurement noise of the unmanned vehicle at the k+1th moment, μ x and μ y are the measurement noise of the unmanned vehicle in the horizontal and vertical directions respectively; is the quantization parameter matrix, and are the quantization parameters in the horizontal and vertical directions respectively; μ k is the measurement noise of the unmanned vehicle at the kth moment; μ k The transpose of is the transpose of wk.

[0068] S4. According to the estimator gain matrix at the kth moment and the dynamic model of the unmanned vehicle system at the k+1th moment, a non-fragile ellipsoid set membership estimator at the k+1th moment is constructed, specifically:

[0069]

[0070] in, is η k+1 The estimated value of G k and L k are the estimator gain matrices at the kth moment; ΔG k =G k M1F 1,k N1 and ΔL k =L k M2F 2,k N2 are the estimator gain perturbations at the kth moment, F 1,k and F 2,k are all time-varying matrices and satisfy and

[0071] S5. Determine the estimated value of the horizontal displacement, the estimated value of the vertical displacement, the estimated value of the yaw angle, the estimated value of the sideslip angle and the estimated value of the yaw rate of the unmanned vehicle at the k+1th moment according to the non-fragile ellipsoid set membership estimator at the k+1th moment.

[0072] S6. When k+1<k max After the conditions are updated, the estimated values ​​of the position parameters of the unmanned vehicle at the k+1th moment are updated. The estimated values ​​of the position parameters of the unmanned vehicle at the next moment are calculated repeatedly until k+1=k max , estimate the displacement trajectory of the unmanned vehicle.

[0073] Set system parameters for simulation:

[0074] I z =3411.52kg·m 2 , C f =-87594N / rad, C r =-87594N / rad,

[0075] l2=1.35m,v=1m / s,T s =0.1s,u x =7.838,u y =7.272.

[0076]

[0077]

[0078] N1=[0.010.010.010.010.01],N2=[0.010.01],

[0079]

[0080] like Figure 2 , Figure 3 and Figure 4 As shown, the set system parameters are substituted into the estimator to obtain the estimated values ​​of the position parameters of the unmanned vehicle at the corresponding time. It is found that the estimated values ​​of the position parameters of the unmanned vehicle are all within the upper and lower boundaries of the ellipsoid set. Therefore, the trajectory of the unmanned vehicle can be effectively estimated based on the estimated information of the position parameters of the unmanned vehicle at the corresponding time.

[0081] Technical effects of the present invention:

[0082] (1) The present invention simultaneously processes process noise, measurement noise and time lag, and obtains a trajectory estimation method for an unmanned vehicle under logarithmic quantization based on recursive linear matrix inequality. This solves the problem that the existing state estimation method cannot simultaneously process an unmanned vehicle system with process noise, measurement noise and time lag, and at the same time solves the problem that the position state accuracy of the unmanned vehicle system is reduced due to logarithmic quantization. The purpose of anti-disturbance is achieved, and it has the advantages of being easy to solve and implement.

[0083] (2) The present invention utilizes mathematical induction and recursive linear matrix inequality processing tools to design an estimator gain matrix to ensure that the estimated value of the position state of the unmanned vehicle system can be constrained within the ellipsoid set, thereby minimizing the estimation error and improving the accuracy of the unmanned vehicle system state.

[0084] (3) The present invention utilizes a logarithmic quantizer to solve the problem of reduced accuracy of the position status of the unmanned vehicle system due to network bandwidth limitations, eliminates the negative impact of the quantization effect on the estimator performance, and improves network transmission efficiency.

[0085] The present invention also provides an unmanned vehicle trajectory estimation system, comprising:

[0086] The estimator gain matrix determination module is used to obtain the position parameters of the unmanned vehicle at the kth moment and their estimated values ​​so that they meet the ellipsoid constraint conditions, and based on the position parameters of the unmanned vehicle at the kth moment and their estimated values, determine the estimator gain matrix at the kth moment using the optimization problem.

[0087] The dynamic model building module is used to build the dynamic model of the unmanned vehicle system at the k+1th moment based on the estimator gain matrix at the kth moment, taking into account the influence of process noise and measurement noise at that moment.

[0088] The non-fragile ellipsoid set membership estimator determination module is used to construct a non-fragile ellipsoid set membership estimator at the k+1th moment according to the estimator gain matrix at the kth moment and the dynamic model of the unmanned vehicle system at the k+1th moment.

[0089] The estimated value determination module is used to determine the estimated value of the position parameter of the unmanned vehicle at the k+1th moment according to the non-fragile ellipsoid set membership estimator at the k+1th moment.

[0090] The trajectory acquisition module is used to determine the displacement trajectory of the unmanned vehicle according to the estimated value of the position parameter of the unmanned vehicle by the non-fragile ellipsoid set membership estimator.

[0091] Each module in the above-mentioned unmanned vehicle trajectory estimation system can be implemented in whole or in part by software, hardware, or a combination thereof. Each of the above-mentioned modules can be embedded in or independent of a processor in a computer device in the form of hardware, or can be stored in a memory in a computer device in the form of software, so that the processor can call and execute the operations corresponding to each of the above modules.

[0092] The present invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps in an embodiment of an unmanned vehicle trajectory estimation method. The specific implementation method can be found in the method embodiment, which will not be repeated here.

[0093] Furthermore, the present invention also provides a non-temporary computer-readable storage medium containing instructions, and a computer program is stored on the storage medium. For example, a memory containing instructions, the above instructions can be executed by a processor of a computer device to complete the above method. For example, a non-temporary computer-readable storage medium can be a ROM, a random access memory (RAM), a CD-ROM, a tape, a floppy disk, and an optical data storage device. When the computer program is executed by the processor, the steps in an embodiment of a method for estimating the trajectory of an unmanned vehicle can be implemented. The specific implementation method can be found in the method embodiment, which will not be repeated here.

[0094] It will be appreciated by those skilled in the art that embodiments of the present invention may provide methods, systems or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented 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.

[0095] The present invention is described with reference to flowcharts and / or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as a combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0096] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0097] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0098] It should be pointed out that the specific implementation methods described above can enable those skilled in the art to understand the invention more comprehensively, but do not limit the invention in any way. Therefore, although the invention has been described in detail in this specification and embodiments, those skilled in the art should understand that the invention can still be modified or replaced by equivalents; and all technical solutions and improvements that do not deviate from the spirit and scope of the invention are included in the protection scope of the patent for the invention. Any figure mark in the claims should not be regarded as limiting the claims involved. Any simple change or equivalent replacement of the technical solution that can be obviously obtained by any technician familiar with the field within the technical scope disclosed in the present invention belongs to the protection scope of the present invention.

Claims

1. A method for estimating the trajectory of an unmanned vehicle, characterized in that: The following steps are involved: Obtaining the position parameters and estimated values ​​of the position parameters of the unmanned vehicle at the kth moment so that they satisfy the ellipsoid constraint condition, and determining the estimator gain matrix at the kth moment based on the position parameters and estimated values ​​of the unmanned vehicle at the kth moment by using an optimization problem; wherein the position parameters include the displacement of the unmanned vehicle in the horizontal direction and the displacement in the vertical direction, the yaw angle, the sideslip angle, and the yaw rate; According to the estimator gain matrix at the kth moment, considering the influence of process noise and measurement noise at the moment, constructing a dynamic model of the unmanned vehicle system at the k+1th moment; According to the estimator gain matrix at the kth time and the dynamic model of the unmanned vehicle system at the k+1th time, a non-fragile ellipsoid set membership estimator at the k+1th time is constructed; Determining an estimated value of the position parameter of the unmanned vehicle at the k+1th moment according to the non-fragile ellipsoid set membership estimator at the k+1th moment; The displacement trajectory of the unmanned vehicle is determined according to the estimated value of the position parameter of the unmanned vehicle at the k+1th moment.

2. The unmanned vehicle trajectory estimation method according to claim 1, characterized in that: The ellipsoid constraint conditions are specifically: In the formula, is the position state of the unmanned vehicle at the kth moment; is η k The estimated value of x k is the horizontal displacement of the unmanned vehicle at the kth moment; y k is the vertical displacement of the unmanned vehicle at the kth moment; is the yaw angle of the unmanned vehicle at the kth moment; s k is the sideslip angle of the unmanned vehicle at the kth moment; γ k is the yaw rate of the unmanned vehicle at the kth moment; For x k An estimated value of for y k An estimated value of for An estimated value of For k An estimated value of For γ k The estimated value of ; d>0 is a constant time lag; P k is the positive definite matrix at the kth moment; P k The inverse matrix of .

3. The unmanned vehicle trajectory estimation method according to claim 2, characterized in that: The optimization problem is specifically: So that: In the formula, min{} is the minimum value function; G k The transpose of For L k The transpose of G k and L k is the estimator gain matrix at the kth moment; ρ x and ρ y are the quantization densities in the horizontal and vertical directions respectively; u x0 and u y0 are the maximum quantization values ​​of the quantization levels in the horizontal and vertical directions respectively; τ i (i=1,2,…,8) are all positive real numbers; P k+1 is the positive definite matrix at the k+1th moment; trace{P k+1 } is P k+1 traces; E k for The factor matrix of For E k The transpose of E k-d for The factor matrix, P k-d is a positive definite symmetric matrix at the kdth moment, For E k-d The transpose of is the estimated value of the position state of the unmanned vehicle at the kdth moment The transpose of is the control input u at the kth moment k =[v δ] T The transpose of , v is the speed of the unmanned vehicle, δ is the front wheel steering angle of the unmanned vehicle; M1, M2, N1 and N2 are all real matrices; N1 T is the transpose of N1; N2 T is the transpose of N2; Q k Inverse matrix; For R k+1 The inverse matrix of k and R k+1 are the positive definite matrices at the kth moment and the k+1th moment respectively; I is the identity matrix; is the discretized system matrix The transpose of is the delay matrix A d The transpose of is the discretized control input matrix The transpose of is the measurement matrix C p The transpose of is the process noise distribution matrix D p The transpose of is the measurement noise distribution matrix F p The transpose of s is the sampling time; e=2.718281828459045 is a natural constant, and the system matrix A and the control input matrix B are: Where m is the mass of the unmanned vehicle; I z is the inertia matrix of the unmanned vehicle; C f and C r They represent the turning stiffness of the front and rear tires of the unmanned vehicle respectively; l1 and l2 represent the distance from the center of gravity of the unmanned vehicle to the front and rear respectively.

4. The unmanned vehicle trajectory estimation method according to claim 3, characterized in that: The dynamic model of the unmanned vehicle system at the k+1th moment is specifically: or k+1 =A p or k +A d or k-d +B p you k +D p w k x k+1 =C p or k+1 +F p m k+1 +D k+1 Where η k+1 is the position state of the unmanned vehicle at the k+1th moment; is the measurement noise of the unmanned vehicle at the kth moment, w x and w y is the process noise of the unmanned vehicle in the horizontal and vertical directions; is the process noise corresponding to the yaw angle of the unmanned vehicle, w s is the process noise corresponding to the sideslip angle of the unmanned vehicle, w γ is the process noise corresponding to the yaw rate of the unmanned vehicle; ξ k+1 is the logarithmic quantization value of the measurement output of the unmanned vehicle at the k+1th moment; μ k+1 =[μ x μ y ] T is the measurement noise of the unmanned vehicle at the k+1th moment, μ x and μ y are the measurement noise of the unmanned vehicle in the horizontal and vertical directions respectively; is the quantization parameter matrix, and are the quantization parameters in the horizontal and vertical directions respectively; μk is the measurement noise of the unmanned vehicle at the kth moment; is the transpose of μk, w k The transpose of .

5. The unmanned vehicle trajectory estimation method according to claim 4, characterized in that: The non-fragile ellipsoid set membership estimator at the k+1th moment is specifically: In the formula, is η k+1 The estimated value of G k and L k are the estimator gain matrices at the kth moment; ΔG k =G k M1F 1,k N1 and ΔL k =L k M2F 2,k N2 are the estimator gain perturbations at the kth moment, F 1,k and F 2,k are all time-varying matrices and satisfy and 6. The unmanned vehicle trajectory estimation method according to claim 5, characterized in that: Determining the estimated value of the position parameter of the unmanned vehicle at the k+1th time according to the non-fragile ellipsoid set membership estimator at the k+1th time specifically includes: According to the estimated value of the non-fragile ellipsoid set at the k+1th moment, the estimated value of the position state of the unmanned vehicle system at the kth moment, and the estimator gain matrix G at the kth moment k and L k , the perturbation of the estimator gain at the kth moment, the discretized control input matrix B, and the logarithmic quantization of the measured output of the unmanned vehicle at the k+1th moment ξ k+1 , determine the estimated value of the horizontal displacement, the estimated value of the vertical displacement, the estimated value of the yaw angle, the estimated value of the sideslip angle, and the estimated value of the yaw rate of the unmanned vehicle at the k+1th moment.

7. An unmanned vehicle trajectory estimation system, characterized in that: include: an estimator gain matrix determination module, used to obtain the position parameters of the unmanned vehicle at the kth moment and the estimated values ​​thereof so as to satisfy the ellipsoid constraint condition, and determine the estimator gain matrix at the kth moment based on the position parameters of the unmanned vehicle at the kth moment and the estimated values ​​thereof by using an optimization problem; A dynamic model building module is used to build a dynamic model of the unmanned vehicle system at the k+1th moment based on the estimator gain matrix at the kth moment, taking into account the influence of process noise and measurement noise at the moment; A non-fragile ellipsoid set membership estimator determination module is used to construct a non-fragile ellipsoid set membership estimator at the k+1th moment according to the estimator gain matrix at the kth moment and the dynamic model of the unmanned vehicle system at the k+1th moment; An estimated value determination module, used to determine an estimated value of the position parameter of the unmanned vehicle at the k+1th moment according to the non-fragile ellipsoid set membership estimator at the k+1th moment; The trajectory acquisition module is used to determine the displacement trajectory of the unmanned vehicle according to the estimated value of the unmanned vehicle position parameter of the non-fragile ellipsoid set membership estimator.

8. A computer device comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is loaded into a processor, it can execute the steps of the method according to any one of claims 1 to 6.

Images