CN115755939A - Four-rotor underwater vehicle forward motion state estimation method - Google Patents

Four-rotor underwater vehicle forward motion state estimation method Download PDF

Info

Publication number
CN115755939A
CN115755939A CN202211410245.2A CN202211410245A CN115755939A CN 115755939 A CN115755939 A CN 115755939A CN 202211410245 A CN202211410245 A CN 202211410245A CN 115755939 A CN115755939 A CN 115755939A
Authority
CN
China
Prior art keywords
underwater vehicle
matrix
motion
equation
formula
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
Application number
CN202211410245.2A
Other languages
Chinese (zh)
Inventor
冀大雄
王睿
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Donghai Laboratory
Zhejiang University ZJU
Original Assignee
Donghai Laboratory
Zhejiang University ZJU
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Donghai Laboratory, Zhejiang University ZJU filed Critical Donghai Laboratory
Priority to CN202211410245.2A priority Critical patent/CN115755939A/en
Publication of CN115755939A publication Critical patent/CN115755939A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Abstract

The invention relates to the field of underwater vehicle and robot control, and aims to provide a four-rotor underwater vehicle forward motion state estimation method. The method comprises the following steps: establishing a six-degree-of-freedom dynamics and kinematics model of the four-rotor underwater vehicle by a simulation or experiment method; setting motion state parameters, and simplifying the six-degree-of-freedom model to obtain an advancing motion equation of the underwater vehicle; setting a control law of the underwater vehicle in the forward motion, and determining the corresponding relation between the control quantity and the state quantity; analyzing the stress condition of the underwater vehicle and a dynamic model of uniform-speed forward motion to obtain a functional relation between an inclination angle and speed when the underwater vehicle advances at a uniform speed; establishing a Kalman filtering equation, and setting Kalman filter coefficients corresponding to different motion states; and setting a floating time period, and adjusting parameters by comparing the difference between the actual position and the state estimation position. The method has simple and convenient steps and lower cost; stable and reliable, accurate result and wide application range.

Description

Four-rotor underwater vehicle forward motion state estimation method
Technical Field
The invention relates to the field of underwater vehicle and robot control, in particular to a four-rotor underwater vehicle forward motion state estimation method.
Background
The underwater vehicle is an ocean intelligent device integrating high and new technologies such as microelectronics, computers, control, energy sources, communication and the like. Compared with the traditional torpedo-shaped underwater vehicle, the four-rotor underwater vehicle has the advantages of good hovering performance, low energy consumption, simple structure and the like, and has extremely high application value in the aspects of underwater investigation, detection and the like; but is also more stringent in the design requirements of the control system due to its particular motion pattern. The concrete expression is as follows: 1) An underdrive property. The four-rotor underwater vehicle has four independently controlled input variables and six degrees of freedom of motion, the number of the degrees of freedom is greater than the number of control inputs, and the four-rotor underwater vehicle is a typical under-actuated system. The under-actuated feature may reduce the design and manufacturing difficulty of the system, but may make the control design more complex. 2) And (4) strong coupling. According to the dynamics model and the kinematics model, the control input moment directly acts on the attitude of the underwater vehicle, and when the attitude of the underwater vehicle changes, the conversion matrix changes, so that the position of the underwater vehicle is further changed. 3) Is statically unstable. When the control input is equal to zero, the underwater vehicle cannot remain at the equilibrium point. 4) Non-linear. The underwater vehicle dynamic equation does not meet the superposition, and is a typical nonlinear system.
The observation and estimation of the state quantities is one of the important components of the control system. For a general sensing system, after the sensing system is submerged, position data cannot be obtained through a positioning system such as a GPS and the like, and an underwater special speed sensor such as a Doppler log has the defects of high price, huge volume and weight and the like. Although the acceleration integral obtained by the inertial sensor can be used for obtaining the speed and further estimating the displacement, the data obtained by the inertial sensor with lower precision is only relied on, and the four-rotor underwater vehicle with more complex control design requirements is definitely not accurate enough.
With the emphasis on the ocean development strategy and the development of the underwater vehicle technology, an effective and convenient method for estimating the forward motion state of the four-rotor underwater vehicle is urgently needed at present, so that the design cost is reduced, and the working efficiency of the vehicle is improved.
Disclosure of Invention
The invention aims to solve the technical problem of overcoming the defects in the prior art and provides a method for estimating the forward motion state of a four-rotor underwater vehicle.
In order to solve the technical problem, the technical scheme adopted by the invention is as follows:
the method for estimating the forward motion state of the four-rotor underwater vehicle comprises the following steps:
(1) Establishing a six-degree-of-freedom dynamics and kinematics model of the four-rotor underwater vehicle by a simulation or experiment method;
(2) Setting motion state parameters, and simplifying the six-degree-of-freedom model to obtain an advancing motion equation of the underwater vehicle;
(3) Setting a control law of the underwater vehicle in the forward motion, and determining the corresponding relation between the control quantity and the state quantity;
(4) Analyzing the stress condition of the underwater vehicle and a dynamic model of uniform-speed forward motion to obtain a functional relation between an inclination angle and speed when the underwater vehicle advances at a uniform speed;
(5) Establishing a Kalman filtering equation, and setting Kalman filter coefficients corresponding to different motion states;
(6) And floating the underwater vehicle according to a set time period, and adjusting the parameters by comparing the difference between the actual position and the state estimation position.
Compared with the prior art, the invention has the beneficial effects that:
1. the invention has simple steps and lower cost. The invention provides a state estimator which can obtain speed and displacement only by an angle and an acceleration sensor, aiming at the problems that a GPS (global positioning system) cannot be used underwater and the speed sensor is expensive.
2. The invention is stable and reliable, and the result is accurate. According to the method, the observation values of the angle and the acceleration are simultaneously referred, the Kalman coefficient can be adaptively adjusted according to the motion condition, and a more accurate result can be finally obtained through repeatedly calibrating parameters.
3. The invention has wide application range. The invention can be applied to not only the forward motion of a four-rotor underwater vehicle, but also any rotor type robot which realizes forward or lateral motion by means of attitude transformation, including but not limited to underwater and aerial robots.
Drawings
FIG. 1 is a schematic overall flow diagram of the present invention;
FIG. 2 is a coordinate system diagram of a cruciform quad-rotor underwater vehicle;
FIG. 3 is a force analysis diagram of the underwater vehicle in uniform forward motion;
fig. 4 is a schematic diagram of the state estimation process of the present invention.
Detailed Description
It is first stated that the present invention relates to underwater robotics, hydrodynamic modeling, control, navigation and positioning techniques. The applicant believes that one of ordinary skill in the art will be able to implement the invention with the full benefit of the software programming skills and algorithmic modifications available thereto, as may be appreciated by a person skilled in the art after perusal of the application and an accurate understanding of the principles of implementation and inventive objects of the invention. In the implementation process of the invention, the application of the multi-rotor underwater robot can be involved, and the number of the rotor propellers arranged on the underwater robot can be between 3 and 16; the underwater robot includes but is not limited to: underwater robots, underwater vehicles, submersibles, submergence vehicles, deep submergence vehicles, underwater self-propelled vehicles and the like; the aforementioned filtering equations include, but are not limited to: kalman filtering, extended Kalman filtering, particle filtering, least square filtering, colorless Kalman filtering, unscented Kalman filtering, adaptive Kalman filtering, bayesian filtering, etc., all of which are mentioned in the present application document, the applicant does not enumerate one by one.
The invention provides a method for estimating the forward motion state of a four-rotor underwater vehicle, which comprises the following steps:
(1) Establishing a six-degree-of-freedom dynamics and kinematics model of the four-rotor underwater vehicle by a simulation or experiment method;
the specific implementation of the kinetic and kinematic model is as follows:
(1.1) introducing a body coordinate system and a ground coordinate system in the motion of the underwater vehicle; the underwater vehicle body coordinate system is bound with an underwater vehicle body, the origin is a floating center and moves along with the movement of the underwater vehicle; the geodetic coordinate system is fixedly connected with the inertial system, and the position direction is not changed;
representing the stress and motion conditions of the underwater vehicle in a body coordinate system, sequentially defining linear velocities on x, y and z axes as u, V and w, sequentially defining angular velocities as p, q and r, and defining a velocity matrix as V = [ u, V, w, p, q and r ]] T The superscript T represents the transpose of the matrix, the same applies below; the position and the attitude angle of the device are expressed in a ground coordinate system, the displacement on the x, y and z axes is defined as x, y and z in turn, and the attitude angle is defined as x, y and z in turn
Figure BDA0003932943550000031
Theta, psi, then the pose matrix is defined as
Figure BDA0003932943550000032
(1.2) in the six-degree-of-freedom dynamics and kinematics model, the overall dynamics equation is expressed as:
Figure BDA0003932943550000033
wherein, M T As a matrix of the total mass,
Figure BDA0003932943550000034
is the differential of V, i.e. the point mark represents the differential, the same applies below; c (V) is a total Coriolis force matrix; d L Is a linear resistance coefficient matrix, D NL A nonlinear resistance coefficient matrix is obtained; g (eta) is a restoring force matrix, and F is an input force matrix;
total mass matrix M T From rigid body mass matrix M RB And additional mass matrix M A The composition is calculated by the following formula:
M T ＝M RB +M A
Figure BDA0003932943550000041
Figure BDA0003932943550000042
wherein m is mass; assuming that the x-axis and y-axis coordinates of the center of gravity are both 0 g As z-axis coordinate of center of gravity, I x /I y /I z In turn, the moment of inertia on the xyz coordinate axis,
Figure BDA0003932943550000043
in turn additional mass coefficients in the xyz-axis translational degree of freedom,
Figure BDA0003932943550000044
additional mass coefficients, also called additional moment of inertia coefficients, on the rotation degree of freedom of the xyz axis are sequentially formed;
similarly, the total Coriolis force matrix C (V) is represented by C RB And C A Composition, calculated by the formula:
C(V)＝C RB +C A
Figure BDA0003932943550000045
Figure BDA0003932943550000046
linear resistance coefficient matrix D L And a non-linear resistance coefficient matrix D NL Calculated by the following formula:
D L ＝diag[X u ，Y v ，Z w ，K p ，M q ，N r ]
Figure BDA0003932943550000047
wherein, X u /Y v /Z w In turn, the linear resistance coefficient, K, in the degree of translational freedom of the xyz axis p ，M q ，N r Linear resistance coefficients on the rotation freedom degree of an xyz axis are sequentially formed;
Figure BDA0003932943550000048
in turn the nonlinear drag coefficients in the xyz-axis translational degree of freedom,
Figure BDA0003932943550000049
sequentially is a nonlinear resistance coefficient on the rotation freedom degree of an xyz axis;
a restoring force matrix G (η) calculated by the following equation:
Figure BDA00039329435500000410
wherein G is gravity and B is buoyancy, s = sin, c = cos
An input force matrix F, calculated by:
Figure BDA0003932943550000051
wherein, F z Indicating the force of the rotor in the vertical direction,
Figure BDA0003932943550000052
indicating the rolling moment produced by the rotor, M θ Indicating the pitching moment produced by the rotor, M ψ Representing the pitching moment generated by the rotor;
(1.3) in the six-degree-of-freedom dynamics and kinematics model, the kinematics equation is expressed as:
Figure BDA0003932943550000053
Figure BDA0003932943550000054
Figure BDA0003932943550000055
Figure BDA0003932943550000056
Figure BDA0003932943550000057
wherein, the vector eta 1 、η 2 η is defined as follows:
Figure BDA0003932943550000058
Figure BDA0003932943550000059
wherein t = tan; r 1 Representing a position conversion matrix, R 2 Representing an attitude transformation matrix; 0 3*3 Represent a 3 x 3 zero matrix.
(2) Setting motion state parameters, and simplifying the six-degree-of-freedom model to obtain an advancing motion equation of the underwater vehicle; the method specifically comprises the following steps:
(2.1) setting the advancing direction of the underwater vehicle, and giving motion constraint; taking the example of proceeding along the x-axis, set v =0, p = r =0, y =0,
Figure BDA00039329435500000510
(2.2) unfolding the six-degree-of-freedom model, and reserving a non-zero item;
the kinetic equation is expressed by the following formula:
Figure BDA00039329435500000511
wherein the formula U, the formula Q and the formula W respectively represent the motion conditions of U, Q and W in three degrees of freedom;
the kinematic equation is expressed by the following formula:
Figure BDA0003932943550000061
wherein, the formula X, the formula Z and the formula theta respectively represent the motion conditions of three degrees of freedom of X, y and theta.
(3) Setting a control law of the underwater vehicle in the forward motion, and determining the corresponding relation between the control quantity and the state quantity; the setting content of the control law comprises the following steps:
(3.1) when the underwater vehicle is stably suspended, the four propellers are all in an initial state; the respective thrust forces are the same and the generated thrust force is exactly balanced with the net buoyancy force, which is expressed by the following formula:
F z0 ＝B-G
M θ0 ＝0
let F zt And M θt Respectively represent the time point F z And M θ A value of (A), i.e. in the above formula, F z0 And M θ0 Respectively represent an initial time F z And M θ A value of (d);
(3.2) when the underwater vehicle advances, the kinetic equation is expressed by formula
Figure BDA0003932943550000062
In ensuring
Figure BDA0003932943550000063
And with
Figure BDA0003932943550000064
At the same time of being stable, the utility model,
Figure BDA0003932943550000065
as small as possible.
(4) Analyzing the stress condition of the underwater vehicle and a dynamic model of uniform-speed forward motion to obtain a functional relation between an inclination angle and speed when the underwater vehicle advances at a uniform speed; the method comprises the following specific steps:
(4.1) giving further motion constraint, setting to the underwater vehicle
Figure BDA0003932943550000066
u＞0，w＞0，q＝0；
(4.2) substituting the constraint into a control law expansion equation to obtain a control model in the uniform motion process, wherein the control model is represented by the following formula:
Figure BDA0003932943550000067
Figure BDA0003932943550000068
(4.3) solving the above formula to obtain a formula:
Figure BDA0003932943550000069
the formula is a functional relation between the angle and the speed in the process of uniform motion.
(5) Establishing a Kalman filtering equation, and setting Kalman filter coefficients corresponding to different motion states; the method specifically comprises the following steps:
(5.1)
establishing a prediction equation, and expressing the prediction equation by the following formula:
Figure BDA0003932943550000071
Figure BDA0003932943550000072
wherein the content of the first and second substances,
Figure BDA0003932943550000073
is a prior estimate of the state at time t,
Figure BDA0003932943550000074
is an estimate of the state at time t-1, F is the state transition matrix, u t-1 The acceleration value obtained by the acceleration sensor at the last moment is obtained, and B is an input matrix;
Figure BDA0003932943550000075
is composed of
Figure BDA0003932943550000076
A priori estimate of the covariance matrix, P t-1 Is the last moment
Figure BDA0003932943550000077
The above equation is mainly based on the value u output by the acceleration sensor at the previous moment t-1 Estimating the current time speed state quantity
Figure BDA0003932943550000078
Q is the deviation caused by environmental interference or inaccuracy of the acceleration sensor after the method is used;
(5.2) establishing a state update equation, which is expressed by the following formula:
Figure BDA0003932943550000079
Figure BDA00039329435500000710
Figure BDA00039329435500000711
wherein, K t As a Kalman filter coefficient, P t Is composed of
Figure BDA00039329435500000712
The covariance matrix of (a) is determined,
Figure BDA00039329435500000723
is an estimated value of the state at the moment t, and H is an observation matrix; r is the speed of the current moment obtained through the functional relation between the angle and the speed in the step (4)
Figure BDA00039329435500000713
Then, I is the identity matrix due to the bias caused by model inaccuracy or angle sensor inaccuracy.
(6) Floating the underwater vehicle upwards according to a set time period, and adjusting parameters by comparing the difference between the actual position and the state estimation position; the method specifically comprises the following steps:
(6.1) setting initial values of Q and R;
(6.2) according to the actual motion situation of the underwater vehicle, setting a Kalman filtering coefficient capable of being adjusted in a self-adaptive mode, and estimating the value
Figure BDA00039329435500000714
And
Figure BDA00039329435500000715
and
Figure BDA00039329435500000716
related, and the following rules apply: the larger Q, the more estimated value
Figure BDA00039329435500000717
The larger the occupied proportion is; the larger R is, the more estimated
Figure BDA00039329435500000718
The greater the specific gravity. The specific parameter calibration strategy is therefore: when the acceleration value is larger, the speed value estimated by the acceleration sensor is more accurate,
Figure BDA00039329435500000719
the weight is small and the weight is small,
Figure BDA00039329435500000720
the weight is larger, R should be increased or Q should be decreased appropriately; when the acceleration value is smaller, the underwater vehicle approaches to uniform motion, the velocity value obtained by the angle calculation is more accurate,
Figure BDA00039329435500000721
the weight is greater and the weight is greater,
Figure BDA00039329435500000722
the weight is smaller, R should be reduced or Q should be increased appropriately;
and (6.3) during actual operation, the underwater vehicle floats upwards once at intervals, whether the state estimator is accurate or not is judged through GPS reading, and parameters are properly adjusted.
The invention will be described in further detail below with reference to specific examples in the drawings, in which:
fig. 2 is a coordinate system diagram of a quad-rotor underwater vehicle, and it should be noted that the solution proposed by the present invention is applicable to any vehicle that performs forward/side-shift motions with varying attitude, and a representative object is chosen for illustration purposes only for convenience of description. As shown in the figure, in the specific motion of the underwater vehicle, a body coordinate system and a ground coordinate system are introduced, the body coordinate system is bound with the body of the underwater vehicle, the body coordinate system moves along with the motion of the underwater vehicle, the ground coordinate system is fixedly connected with an inertial system, and the position direction is not changed. Representing the stress and motion conditions of the motion of the underwater vehicle in a body coordinate system, sequentially defining linear velocities on x, y and z axes as u, V and w, sequentially defining angular velocities as p, q and r, and defining a velocity matrix as V = [ u, V, w, p, q and r = [ u, V, w, q and r ]] T The superscript T represents the transpose of the matrix, the same applies below; the position and the attitude angle of the device are expressed in a ground coordinate system, the displacement on the x, y and z axes is defined as x, y and z in turn, and the attitude angle is defined as x, y and z in turn
Figure BDA0003932943550000081
Theta, psi, then the pose matrix is defined as
Figure BDA0003932943550000082
Angular speed of propeller 1/3 is omega 1 /ω 3 Clockwise in the direction of thrust F 1 /F 3 Downward, rotational couple M 1 /M 3 In a clockwise direction; the angular speed of the propeller 1/4 is omega 2 /ω 4 In the counterclockwise direction, thrust F 2 /F 4 Downward, rotational couple M 2 /M 4 In the counterclockwise direction. FIG. 3 is a force analysis diagram of the underwater vehicle in uniform forward motion, where the total thrust is F in the forward direction under the condition of the attitude angle θ, that is, the component on the x axis is F x The component in the y-axis being F y And has F x Is balanced with the resistance D borne by the underwater vehicle, F y Balanced with the net buoyancy.
The forward motion state estimation method of the four-rotor underwater vehicle in the embodiment comprises the following specific steps:
step 1, constructing a motion model of an underwater vehicle, comprising:
establishing a universal dynamic model of the underwater vehicle, and obtaining a formula
Figure BDA0003932943550000083
Wherein, M T Is a matrix of the total mass,
Figure BDA0003932943550000084
is the differential of V, i.e. the point mark represents the differential, the same applies below; c (V) is the total Coriolis force matrix, D L Is a linear resistance coefficient matrix, D NL A nonlinear resistance coefficient matrix is obtained; g (eta) is a restoring force matrix, and F is an input force matrix;
in this embodiment, the matrix or vector is assigned with a total quality matrix of
Figure BDA0003932943550000085
The total coriolis matrix is:
Figure BDA0003932943550000086
the linear resistance and the nonlinear resistance are respectively:
D L ＝diag[1.43，1.43，0.3747，0.0116，0.0116，0.006736]
D NL ＝diag[36.13，36.13，36.35，0.028，0.028，0.008206]
the restoring force matrix is:
Figure BDA0003932943550000091
wherein G and B are gravity and buoyancy respectively, s = sin, c = cos;
taking a cruciform four-rotor underwater vehicle as an example, the input force matrix is represented by the formula:
Figure BDA0003932943550000092
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003932943550000093
thrust generated for each propeller. Since the thrust and the couple generated by the propellers are numerically proportional to the square of the rotation speed, the thrust and the couple generated by each propeller are also proportional. The value of the couple is therefore expressed in terms of the thrust times the scaling factor, and dimensions are ignored.
The following vectors are also defined:
Figure BDA0003932943550000094
Figure BDA0003932943550000095
the kinematic equation can be summarized as
Figure BDA0003932943550000096
Wherein:
Figure BDA0003932943550000097
Figure BDA0003932943550000098
Figure BDA0003932943550000099
Figure BDA00039329435500000910
step 2, constructing an expansion of the forward motion of the underwater vehicle:
because the four-rotor underwater vehicle cannot directly provide power in the forward direction, in the forward motion, the vehicle must generate an inclination angle, and the underwater vehicle is advanced by utilizing the component of thrust in the horizontal direction. If going along the x-axis, the angle of inclination that is active is θ, and if going along the y-axis, the angle of inclination that is active is θ
Figure BDA0003932943550000101
Taking the x-axis progression as an example, during the motion, some of the translational and rotational degrees of freedom are constrained, and in the motion equation, the following equation can be specifically expressed:
v＝0，p＝r＝0，y＝0，
Figure BDA0003932943550000102
neglecting the portion of 0, the kinetic equation can be simplified to expand as:
Figure BDA0003932943550000103
wherein, the formula U, the formula Q and the formula W respectively represent the motion conditions of U, Q and W in three degrees of freedom.
The kinematic equation can be simplified to expand as:
Figure BDA0003932943550000104
wherein, the formula X, the formula Y and the formula theta respectively represent the motion conditions of three degrees of freedom of X, Y and theta.
Step 3, setting the control law by taking the following control method as an example:
assuming a stable suspension, the propellers 1/2/3/4 are initially thrust-identical and the thrust produced by each is exactly balanced with the net buoyancy, i.e.
Figure BDA0003932943550000105
4F 0 ＝B-G
Wherein
Figure BDA0003932943550000106
Representing the thrust generated by the ith propeller at time t, F 0 Indicating the thrust generated by the four propellers at the initial moment.
The control strategy for forward motion is such that the propellers 1/3 are respectively reduced/increased by the same amount Δ 1 After that, propeller 1/2/3/4 is simultaneously increased/decreased by the same amount Δ 2 . Wherein, delta 1 Providing a couple which enables the underwater vehicle to generate an inclination angle, and enabling the attitude of the underwater vehicle to change; delta 2 The depth is stabilized while providing a forward driving force. Concrete brushEquation of formula
Figure BDA0003932943550000107
Figure BDA0003932943550000108
Wherein, delta it Represents time t Δ i The value of (c).
Step 4, the analysis of the uniform motion model and the establishment of the velocity angle function relationship comprise:
when moving at a constant speed, the inclination angle is also fixed, if theta is less than 0, w is more than 0, and
Figure BDA0003932943550000111
u＞0，q＝0
substituting this into the control law expansion yields:
Figure BDA0003932943550000112
Figure BDA0003932943550000113
according to U, in the value range, theta and U have a one-to-one correspondence relationship, namely each theta can uniquely determine one U, and vice versa.
According to Z, each theta can uniquely determine the proportional relation between u and w, namely theta can determine u and w simultaneously. Specifically, when θ =0 °, w =0; when θ =90 °, u =0.
According to Q, after theta, u and w are determined within a value range, delta 1t It can also be determined that each theta uniquely identifies a delta 1t 。
Solving the system of equations to obtain
Figure BDA0003932943550000114
Step 5, establishing a Kalman filtering equation, which comprises the following steps:
in the motion of a quad-rotor underwater robot, the sensors can generally provide acceleration in addition to angle, which enables the design of kalman filters.
The kalman filter general formula is shown below, where the prediction equation passes through the formula:
Figure BDA0003932943550000115
Figure BDA0003932943550000116
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003932943550000117
is a prior estimate of the state at time t,
Figure BDA0003932943550000118
is an estimate of the state at time t-1, F is the state transition matrix, u t-1 The acceleration value obtained by the acceleration sensor at the last moment is B, which is an input matrix:
Figure BDA0003932943550000119
is composed of
Figure BDA00039329435500001110
A priori estimate of the covariance matrix, P t-1 Is the last moment
Figure BDA00039329435500001111
The above equation is mainly based on the value u output by the acceleration sensor at the previous moment t-1 Estimating the current time speed state quantity
Figure BDA00039329435500001112
Q is the deviation caused by environmental interference or inaccuracy of the acceleration sensor after the method is used;
the state update equation is represented by the formula:
Figure BDA0003932943550000121
Figure BDA0003932943550000122
Figure BDA0003932943550000123
wherein, K t As a Kalman filter coefficient, P t Is composed of
Figure BDA0003932943550000124
The covariance matrix of (a) is determined,
Figure BDA0003932943550000125
is an estimated value of the state at the moment t, and H is an observation matrix; r is the speed of the current moment obtained through the functional relation between the angle and the speed
Figure BDA0003932943550000126
Later, I is the identity matrix, where I is the deviation due to model inaccuracy or angle sensor inaccuracy
Figure BDA0003932943550000127
Then H = [0 1 =]；
Let the state quantity be the displacement x of the final prediction t And speed
Figure BDA0003932943550000128
The input being acceleration values obtained by sensors, i.e.
Figure BDA0003932943550000129
Namely, the method comprises the following steps:
Figure BDA00039329435500001210
Figure BDA00039329435500001211
some parameters in the Kalman filtering equation can be obtained as follows
Figure BDA00039329435500001212
The prediction equation is formulated
Figure BDA00039329435500001213
Figure BDA00039329435500001214
Equation of state update by formula
Figure BDA00039329435500001215
Figure BDA00039329435500001216
Figure BDA00039329435500001217
Step 6, setting a self-adaptive and parameter correction rule, comprising:
in the value of each parameter in the above formula, it is preferable
Figure BDA00039329435500001218
P 0 ＝1。
The values of Q and R are determined according to the conditions of the model and the sensor, and the values of H and F are substituted into a Kalman coefficient formula to obtain
Figure BDA00039329435500001219
When the acceleration value is larger, the speed value estimated by the acceleration sensor is more accurate,
Figure BDA00039329435500001220
smaller weight, K t It should be as small as possible, and R may be increased or Q may be decreased as appropriate.
When the acceleration value is smaller, the underwater vehicle approaches to uniform motion, the velocity value obtained by the angle calculation is more accurate,
Figure BDA0003932943550000131
greater weight, K t R should be decreased or Q increased as much as possible as appropriate.
And during actual debugging, floating the underwater vehicle once at intervals, judging whether the state estimator is accurate or not through GPS (global positioning system) reading, and properly adjusting parameters.

Claims (7)

1. A forward motion state estimation method of a four-rotor underwater vehicle is characterized by comprising the following steps:
(1) Establishing a six-degree-of-freedom dynamics and kinematics model of the four-rotor underwater vehicle by a simulation or experiment method;
(2) Setting motion state parameters, and simplifying the six-degree-of-freedom model to obtain an advancing motion equation of the underwater vehicle;
(3) Setting a control law of the underwater vehicle in the forward motion, and determining the corresponding relation between the control quantity and the state quantity;
(4) Analyzing the stress condition of the underwater vehicle and a dynamic model of uniform-speed forward motion to obtain a functional relation between an inclination angle and speed when the underwater vehicle advances at a uniform speed;
(5) Establishing a Kalman filtering equation, and setting Kalman filter coefficients corresponding to different motion states;
(6) And floating the underwater vehicle according to a set time period, and adjusting the parameters by comparing the difference between the actual position and the state estimation position.
2. The method according to claim 1, wherein in the step (1), the dynamics and kinematics model is implemented as follows:
(1.1) introducing a body coordinate system and a ground coordinate system in the motion of an underwater vehicle; the underwater vehicle comprises an underwater vehicle body, a vehicle body coordinate system, an origin point and an underwater vehicle body, wherein the vehicle body coordinate system is bound with the underwater vehicle body, the origin point is a floating center and moves along with the movement of the underwater vehicle; the geodetic coordinate system is fixedly connected with the inertial system, and the position direction is not changed;
representing the stress and motion conditions of the underwater vehicle in a body coordinate system, sequentially defining linear velocities on x, y and z axes as u, V and w, sequentially defining angular velocities as p, q and r, and defining a velocity matrix as V = [ u, V, w, p, q and r ]] T The superscript T represents the transpose of the matrix, the same applies below; the position and the attitude angle of the robot are expressed in a ground coordinate system, the displacements on the x, y and z axes are defined as x, y and z in sequence, and the attitude angle is defined as x, y and z in sequence
Figure FDA0003932943540000011
Theta, psi, then the pose matrix is defined as
Figure FDA0003932943540000012
(1.2) in the six-degree-of-freedom dynamics and kinematics model, the overall dynamics equation is expressed as:
Figure FDA0003932943540000013
wherein, M T As a matrix of the total mass,
Figure FDA0003932943540000014
is the differential of V, i.e. the point mark represents the differential, the same applies below; c (V) is a total Coriolis force matrix; d L Is a linear resistance coefficient matrix, D NL A nonlinear resistance coefficient matrix is obtained; g (eta) is a restoring force matrix, and F is an input force matrix;
total mass matrix M Y From rigid body mass matrix M RB And additional mass matrix M A The composition is calculated by the following formula:
M T ＝M RB +M A
Figure FDA0003932943540000015
Figure FDA0003932943540000021
wherein m is mass; assuming that the x-axis and y-axis coordinates of the center of gravity are both 0 g As z-axis coordinate of center of gravity, I x /I y /I z In turn, the moment of inertia on the xyz coordinate axis,
Figure FDA0003932943540000022
in turn additional mass coefficients in the xyz-axis translational degree of freedom,
Figure FDA0003932943540000023
additional mass coefficients, also called additional inertia coefficients, in the degree of freedom of rotation of the xyz axis are sequentially obtained;
similarly, the total Coriolis force matrix C (V) is represented by C RB And C A Composition, calculated by the formula:
C(V)＝C RB +C A
Figure FDA0003932943540000024
Figure FDA0003932943540000025
linear resistance coefficient matrix D L And a non-linear resistance coefficient matrix D NL Calculated by the following formula:
D L ＝diag[X u ，Y v ，Z w ，K p ，M q ，N r ]
Figure FDA0003932943540000026
wherein X u /Y v /Z w In turn, the linear resistance coefficient, K, in the degree of translational freedom of the xyz axis p ，M q ，N r Linear resistance coefficients on the rotation freedom degree of an xyz axis are sequentially formed;
Figure FDA0003932943540000027
in turn the nonlinear drag coefficients in the xyz-axis translational degree of freedom,
Figure FDA0003932943540000028
sequentially is a nonlinear resistance coefficient on the rotation freedom degree of an xyz axis;
a restoring force matrix G (η) calculated by the following equation:
Figure FDA0003932943540000029
wherein G is gravity, B is buoyancy, s = sin, c = cos;
an input force matrix F, calculated by:
Figure FDA00039329435400000210
wherein, F z Indicating the force of the rotor in the vertical direction,
Figure FDA00039329435400000211
indicating the rolling moment produced by the rotor, M θ Indicating the pitching moment produced by the rotor, M ψ Representing the pitching moment generated by the rotor;
(1.3) in the six-degree-of-freedom dynamics and kinematics model, the kinematics equation is expressed as:
Figure FDA0003932943540000031
Figure FDA0003932943540000032
Figure FDA0003932943540000033
Figure FDA0003932943540000034
Figure FDA0003932943540000035
wherein, the vector eta 1 、η 2 η is defined as follows:
Figure FDA0003932943540000036
Figure FDA0003932943540000037
t＝tan；R 1 representing a position conversion matrix, R 2 Representing an attitude transformation matrix; 0 3*3 Represent a 3 x 3 zero matrix.
3. The method of claim 1, wherein the simplifying process in step (2) comprises:
(2.1) setting the advancing direction of the underwater vehicle, and giving motion constraint; taking the example of proceeding along the x-axis, set v =0, p = r =0, y =0,
Figure FDA0003932943540000038
(2.2) unfolding the six-degree-of-freedom model, and reserving a non-zero item;
the kinetic equation is represented by the following formula:
Figure FDA0003932943540000039
wherein the formula U, the formula Q and the formula W respectively represent the motion conditions of U, Q and W in three degrees of freedom;
the kinematic equation is expressed by the following formula:
Figure FDA00039329435400000310
wherein, the formula X, the formula Z and the formula theta respectively represent the motion conditions of three degrees of freedom of X, y and theta.
4. The method according to claim 1, wherein in the step (3), the setting content of the control law comprises:
(3.1) when the underwater vehicle is stably suspended, the four propellers are all in an initial state; the respective thrust forces are the same and the generated thrust force is exactly balanced with the net buoyancy force, which is expressed by the following formula:
F z0 ＝B-G
M θ0 ＝0
let F zt And M θt Respectively represent t time F z And M θ A value of (A), i.e. in the above formula, F z0 And M θ0 Respectively represent an initial time F z And M θ A value of (d);
(3.2) when the underwater vehicle advances, the kinetic equation passes through a formula
Figure FDA0003932943540000041
In ensuring
Figure FDA0003932943540000042
And
Figure FDA0003932943540000043
at the same time of being stable, the utility model,
Figure FDA0003932943540000044
as small as possible.
5. The method according to claim 1, wherein in the step (4), the specific step of analyzing to obtain the functional relationship comprises:
(4.1) giving further motion constraint and setting to the underwater vehicle
Figure FDA0003932943540000045
u＞0，w＞0，q＝0；
(4.2) substituting the constraint into a control law expansion equation to obtain a control model in the uniform motion process, wherein the control model is expressed by the following formula:
Figure FDA0003932943540000046
Figure FDA0003932943540000047
(4.3) solving the above formula to obtain a formula:
Figure FDA0003932943540000048
the formula is a functional relation between the angle and the speed in the process of uniform motion.
6. The method of claim 1, wherein in step (5), the step of establishing a filter equation comprises:
(5.1) establishing a prediction equation expressed by the following formula:
Figure FDA0003932943540000049
Figure FDA00039329435400000410
wherein the content of the first and second substances,
Figure FDA00039329435400000411
is a prior estimate of the state at time t,
Figure FDA00039329435400000412
is an estimate of the state at time t-1, F is the state transition matrix, u t-1 The acceleration value obtained by the acceleration sensor at the last moment is obtained, and B is an input matrix;
Figure FDA00039329435400000413
is composed of
Figure FDA00039329435400000414
Is estimated prior covariance matrix, P t-1 Is the last moment
Figure FDA00039329435400000415
The above equation is mainly based on the value u output by the acceleration sensor at the previous moment t-1 Estimating the current time speed state quantity
Figure FDA0003932943540000051
Q is the deviation caused by environmental interference or inaccuracy of the acceleration sensor after the method is used;
(5.2) establishing a state update equation, which is expressed by the following formula:
Figure FDA0003932943540000052
Figure FDA0003932943540000053
Figure FDA0003932943540000054
wherein, K t As a Kalman filter coefficient, P t Is composed of
Figure FDA0003932943540000055
The covariance matrix of (a) is determined,
Figure FDA0003932943540000056
is an estimated value of the state at the moment t, and H is an observation matrix; r is the speed of the current moment obtained through the functional relation between the angle and the speed in the step (4)
Figure FDA0003932943540000057
Then, I is an identity matrix due to the deviation caused by model inaccuracy or angle sensor inaccuracy.
7. The method according to claim 1, wherein in the step (6), the adjusting parameters specifically includes:
(6.1) setting initial values of Q and R;
(6.2) according to the actual motion situation of the underwater vehicle, setting a Kalman filtering coefficient capable of being adjusted in an adaptive mode, and estimating the value
Figure FDA0003932943540000058
And
Figure FDA0003932943540000059
and
Figure FDA00039329435400000510
related, and has the following rules: the larger Q, the more estimated value
Figure FDA00039329435400000511
The larger the occupied proportion is; the larger R is, the more estimated
Figure FDA00039329435400000512
The larger the occupied proportion;
therefore, the following parameter calibration strategy is performed: when the acceleration value is larger, the speed value estimated by the acceleration sensor is more accurate,
Figure FDA00039329435400000513
the weight is small and the weight is small,
Figure FDA00039329435400000514
the weight is larger, R should be increased or Q should be decreased appropriately; when the acceleration value is smaller, the underwater vehicle approaches to uniform motion, the velocity value obtained by the angle calculation is more accurate,
Figure FDA00039329435400000515
the weight is greater and the weight is greater,
Figure FDA00039329435400000516
the weight is smaller, R should be reduced or Q should be increased appropriately;
and (6.3) during actual operation, floating the underwater vehicle once at intervals, judging whether the state estimator is accurate or not through GPS (global positioning system) reading, and properly adjusting parameters.
CN202211410245.2A 2022-11-08 2022-11-08 Four-rotor underwater vehicle forward motion state estimation method Pending CN115755939A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211410245.2A CN115755939A (en) 2022-11-08 2022-11-08 Four-rotor underwater vehicle forward motion state estimation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211410245.2A CN115755939A (en) 2022-11-08 2022-11-08 Four-rotor underwater vehicle forward motion state estimation method

Publications (1)

Publication Number Publication Date
CN115755939A true CN115755939A (en) 2023-03-07

Family

ID=85369380

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211410245.2A Pending CN115755939A (en) 2022-11-08 2022-11-08 Four-rotor underwater vehicle forward motion state estimation method

Country Status (1)

Country Link
CN (1) CN115755939A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116466746A (en) * 2023-04-24 2023-07-21 浙江大学 Planning control method and device for four-rotor cluster to pass through dynamic waypoints at high speed

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116466746A (en) * 2023-04-24 2023-07-21 浙江大学 Planning control method and device for four-rotor cluster to pass through dynamic waypoints at high speed
CN116466746B (en) * 2023-04-24 2023-09-29 浙江大学 Planning control method and device for four-rotor cluster to pass through dynamic waypoints at high speed

Similar Documents

Publication Publication Date Title
CN111650948B (en) Quick tracking control method for horizontal plane track of benthonic AUV
CN108267953B (en) Underwater robot position tracking method based on navigator-tracker
CN109460052A (en) A kind of control method for spelling group aircraft
CN111857165B (en) Trajectory tracking control method of underwater vehicle
CN109946976B (en) Wide-navigational-speed AUV motion control method
CN112558477B (en) Unmanned ship state and disturbance observer based on acceleration information
CN113189875B (en) Unmanned aerial vehicle robust landing method on mobile platform based on circular features
CN107491083A (en) A kind of four rotors based on saturation adaptive sliding-mode observer it is autonomous ship's method
CN112015086B (en) Feedback control method for limited-time path tracking output of under-actuated surface ship
CN106840143B (en) Method for judging attitude stability of underwater robot
CN115755939A (en) Four-rotor underwater vehicle forward motion state estimation method
CN114967714A (en) Anti-interference motion control method and system for autonomous underwater robot
CN112947522B (en) Hard air refueling attitude control method based on finite time observer
Guisser et al. A high gain observer and sliding mode controller for an autonomous quadrotor helicopter
CN113467231A (en) Unmanned ship path tracking method based on sideslip compensation ILOS guidance law
CN109901402B (en) Autonomous underwater robot path tracking method based on course smoothing technology
Crocoll et al. Quadrotor inertial navigation aided by a vehicle dynamics model with in-flight parameter estimation
CN111546344A (en) Mechanical arm control method for alignment
Harris et al. Preliminary evaluation of null-space dynamic process model identification with application to cooperative navigation of underwater vehicles
Zhao et al. Model predictive control based integral line-of-sight curved path following for unmanned aerial vehicle
CN115562313A (en) Autonomous underwater vehicle motion control method for pier flaw detection
Abrougui et al. Flight Controller Design Based on Sliding Mode Control for Quadcopter Waypoints Tracking
Yu et al. Attitude adjustment of a multi-joint auv using an improved ladrc
Wasim et al. Estimation of Airship Aerodynamic Forces and Torques Using Extended Kalman Filter
CN115562317A (en) Four-rotor underwater robot water flow disturbance resistance control method based on self-adaptive strategy

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