CN113138608B - A Visual Servo Control Method for Quadrotor UAV Using Disturbance Observer and Nonlinear Velocity Observer - Google Patents

Abstract

The invention discloses a four-rotor unmanned aerial vehicle vision servo control method using a disturbance observer and a nonlinear speed observer, which comprises the following steps: establishing a corresponding dynamic model according to the space motion condition of the four-rotor unmanned aerial vehicle; selecting an image moment as a characteristic of visual servo, fusing a virtual characteristic plane, and establishing image characteristic dynamics under the virtual camera plane; the dynamic visual servo controller based on the image is designed, firstly, according to the acceptance condition of the four-rotor unmanned aerial vehicle, a disturbance observer is analyzed and designed, then, a speed observer in a virtual camera plane is designed, and finally, a sliding mode controller based on index approach is designed by using a backstepping method. Therefore, the four-rotor unmanned aerial vehicle can improve the robustness to external disturbance, relatively accurate estimation is carried out on the unmeasurable image characteristic linear velocity in the virtual image plane, and the final sliding mode controller based on the exponential approach can further improve the robustness of the system.

Description

A Quadrotor UAV Vision Using Disturbance Observer and Nonlinear Velocity Observer Perceptual Servo Control Method

technical field

The invention belongs to the technical field of visual servo control, and in particular relates to a visual servo control method for a quadrotor unmanned aerial vehicle using a disturbance observer and a nonlinear speed observer.

Background technique

In recent years, unmanned aerial vehicles (UAVs) have attracted much attention in military and civilian fields because of their many advantages and huge application potential. At present, multi-rotor drones, especially quad-rotor drones, have been widely used, such as aerial photography and rescue. A distinctive feature of multi-rotor UAVs is vertical take-off and landing, and quad-rotor UAVs are a typical example.

Quadrotor drones are generally composed of inertial navigation systems, flight controllers, power motors, and other auxiliary systems. In addition, most quadrotor drones will carry a low-cost monocular camera, so the automation level of quadrotor drones can be further improved with the help of machine vision technology. When the quadrotor drone is in some specific environments, such as indoor environments, low altitude areas, and some complex urban environments, the quadrotor drone will not be able to obtain the location information of the drone itself through the GPS , once the automatic flight of the quadrotor UAV cannot obtain more accurate position information, it may increase the risk of crashing. In order to solve this problem, the commonly used technology is to combine image-based visual servoing technology (Image Based Visual Servoing, IBVS) with UAV.

Since the quadrotor UAV has the characteristics of full drive and nonlinearity, there are many difficulties in the combination of IBVS and UAV, such as the selection of image features and the design of the controller. In addition, a practical problem is that in the real environment, the impact of disturbance on the UAV is inevitable, which will bring more uncertainties to the UAV.

At present, most of the quadrotor UAV systems on the market are composed of the above-mentioned components. In the actual development process, the speed information of the quadrotor UAV is often not directly available, so the speed information of the quadrotor UAV also needs to be considered as a factor. In view of the many problems existing in the visual servo system of the current quadrotor UAV, we propose a visual servo system of the quadrotor UAV using a disturbance observer and a nonlinear velocity observer, which can simultaneously solve the influence of external disturbance and The problem with speed measurement.

Invention content:

The purpose of the present invention is to provide a visual servo control method for a quadrotor UAV using a disturbance observer and a nonlinear velocity observer, which can solve the problems of external disturbance and velocity observation in the prior art. The specific plan is as follows:

A visual servo control method for a quadrotor UAV using a disturbance observer and a nonlinear velocity observer, comprising:

According to the space motion of the quadrotor UAV, the corresponding dynamic model is established;

Select the image moment as the feature of visual servoing, and integrate the virtual feature plane to establish the image feature dynamics under the virtual camera plane;

To design a dynamic image-based visual servo controller, first analyze and design the disturbance observer according to the force of the quadrotor UAV, then design the velocity observer in the virtual camera plane, and finally use the backstepping method to design close sliding mode controller.

Preferably, in the above-mentioned four-rotor UAV visual servo control method using a disturbance observer and a nonlinear speed observer provided in the implementation of the present invention, according to the spatial motion of the four-rotor UAV, the establishment of a corresponding dynamic model includes :

F＝-U ₁ E ₃ +mgR ^T e ₃

Among them: ζ=(x, y, z) ^T is the representation of the origin of the body coordinate system in the inertial coordinate system, R: B→I is the rotation matrix between the body coordinate system and the inertial coordinate system, R ^T is the rotation matrix the transpose of R, is the linear velocity of the quadrotor UAV in the body coordinate system, ω=(ω ₁ ,ω ₂ ,ω ₃ ) ^T is the angular velocity of the quadrotor UAV in the body coordinate system, sk(ω) represents the skew symmetric matrix , m and J respectively represent the mass and inertia of the quadrotor UAV, F is the external force of the quadrotor UAV, d is the external disturbance, τ is the moment acting on the quadrotor UAV, U ₁ is the quadrotor UAV is the total thrust of the machine body, g is the gravitational acceleration, E ₃ =e ₃ =(0,0,1) ^T is a unit vector.

Preferably, in the above-mentioned four-rotor UAV visual servo control method using a disturbance observer and a nonlinear velocity observer provided in the implementation of the present invention, the image moment is selected as the feature of the visual servo, and the virtual feature plane is fused to establish a virtual Image feature dynamics below the camera plane include:

For a fixed point ^IP =[ ^I x, ^I y, ^I z] ^T in the inertial coordinate system, assuming that the camera coordinate system C coincides with the body coordinate system B, and the camera has a downward field of view, the fixed point The expression of a point in the camera coordinate system is ^C P=[ ^C x, ^C y, ^C z] ^T . At the same time, it is assumed that there is a virtual camera plane whose pitch angle and roll angle are always 0, and the yaw angle has nothing to do with the quadrotor. Man-machine synchronization, the origin coincides with the camera coordinate system, then the expression of this point in the virtual camera coordinate system is ^ν P = [ ^ν x, ^ν y, ^ν z] ^T , the point is in the inertial coordinate system and the virtual camera coordinate system The relationship is:

in: is the rotation matrix around the Z axis, and ψ represents the yaw angle. The following derivative relationship can be obtained as:

in: is the velocity vector, />

According to the perspective projection relationship, the point P is expressed in the virtual camera plane as:

Where: λ is the focal length of the camera, ( ^ν u, ^ν n) is the point coordinates in the virtual camera plane. According to the above relationship, the dynamics of the projected points can be obtained as follows:

At this time, it is assumed that there are N fixed points in the inertial coordinate system and in the same plane, and their image moment features are:

in: ^ν u _k and ^ν n _k are the kth point, a = ^ν μ ₀₂ + ^ν μ ₂₀ is the moment value of the image, /> a ^* is the expected value of a. Using the above relationship, the dynamics of the image moment feature can be obtained as:

Among them: q＝[q _x ,q _y ,q _z ] ^T , z ^* is the expected height value.

To describe the yaw motion of a quadrotor UAV, the following feature description is used:

Preferably, in the above-mentioned quadrotor UAV visual servo control method using a disturbance observer and a nonlinear velocity observer provided in the implementation of the present invention, a dynamic image-based visual servo controller is designed, first according to the quadrotor UAV According to the acceptance situation, analyze and design the disturbance observer, then design the velocity observer in the virtual camera plane, and finally use the backstepping method to design the sliding mode controller based on exponential approach, including:

Using the translational dynamic equation of quadrotor UAV The expression of the external disturbance can be obtained directly /> Then design the nonlinear disturbance observer as follows:

in: is the estimated value of external disturbance, and K>0 is the gain constant. Introducing auxiliary vectors /> Then its derivative with respect to time is:

Finally, the disturbance observer of the system is obtained as:

Now design the nonlinear velocity observer of the image moment feature of the system in the virtual camera coordinate system, first take a set of desired image moment features and /> At this time, the image moment-translation feature error is expressed as q ₁ =q-(0,0,1) ^T , and then using this relationship and image moment feature dynamics, the image feature error dynamic equation is written as:

Where: f=((-R _φθ U ₁ E ₃ )/m)+ge ₃ , R _φθ ＝R _θ R _φ . The nonlinear velocity observer in the virtual image plane is designed as follows:

in: and /> are the estimated values of q ₁ and v, respectively, /> and /> is the corresponding estimation error, both k ₁ and k ₂ are positive parameters.

After completing the design of the disturbance observer and the nonlinear velocity observer, the visual servo controller based on exponential approach is then designed, and the following formula is used as the controller:

in: and /> are the second-order error term and the third-order error term respectively, s=q ₃ is the sliding mode surface, /> is the yaw error term, φ and θ are the roll angle and pitch angle of the quadrotor UAV, respectively, sgn( ) represents a signed function, k ₃ , k ₄ , c ₁ , and η are all positive parameters, and the control parameters should satisfy the following relationship:

It can be seen from the above technical solution that a visual servo control method for a quadrotor UAV using a disturbance observer and a nonlinear velocity observer provided by the present invention includes: establishing a corresponding The dynamic model of the dynamics model; select the image moment as the feature of the visual servo, and integrate the virtual feature plane to establish the dynamics of the image feature under the virtual camera plane; design a dynamic image-based visual servo controller, firstly according to the acceptance of the quadrotor UAV In this case, the disturbance observer is analyzed and designed, and then the velocity observer in the virtual camera plane is designed. Finally, the sliding mode controller based on exponential approach is designed by using the backstepping method.

On the basis of the dynamics of the four-rotor UAV, the present invention analyzes and establishes the image characteristic dynamics in the virtual image plane, and establishes the image moment characteristic dynamics, based on the established image moment characteristic dynamics based on the virtual image plane, The disturbance observer of the quadrotor UAV and the nonlinear velocity observer of the characteristic line velocity of the image moment in the virtual image plane were analyzed and designed. The above two designs improved the robustness of the system, and finally designed a The sliding mode controller further improves the robustness of the system to external disturbances.

Description of drawings:

In order to more clearly illustrate the technical solutions in the implementation of the present invention and related technologies, the following will briefly introduce the accompanying drawings that need to be used in the description of related technologies at the implementation level.

Fig. 1 is the schematic diagram of the body coordinate system and the inertial coordinate system provided in the implementation of the present invention;

2 is a schematic diagram of the camera coordinate system and the virtual camera coordinate system provided in the implementation of the present invention;

Fig. 3 is the structural block diagram of the control system provided in the implementation of the present invention;

Detailed ways:

The technical solutions in the implementation of the present invention will be clearly and comprehensively described below in conjunction with the accompanying drawings in the implementation of the present invention. It appears that the described embodiments are only some of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.

A visual servo control method for a quadrotor UAV using a disturbance observer and a nonlinear velocity observer provided by the present invention includes the following steps:

S101 establishes a corresponding dynamic model according to the space motion of the quadrotor UAV;

S102 Select image moment as the feature of visual servoing, and integrate the virtual feature plane to establish image feature dynamics under the virtual camera plane;

S103 Design a dynamic image-based visual servo controller. First, analyze and design the disturbance observer according to the acceptance of the quadrotor UAV, then design the velocity observer in the virtual camera plane, and finally use the backstepping method to design the close sliding mode controller.

In specific implementation, in a visual servo control method for a quadrotor UAV using a disturbance observer and a nonlinear velocity observer provided by the present invention, step S101 establishes the corresponding power according to the spatial motion of the quadrotor UAV. In order to learn the model, it is first necessary to analyze the acceptance of the quadrotor UAV, so it is necessary to establish the inertial coordinate system and the body coordinate system as shown in Figure 1. According to the space motion of the quadrotor UAV, the establishment of the corresponding dynamic model includes:

F＝-U ₁ E ₃ +mgR ^T e ₃

Among them: ζ=(x, y, z) ^T is the representation of the origin of the body coordinate system in the inertial coordinate system, R: B→I is the rotation matrix between the body coordinate system and the inertial coordinate system, R ^T is the rotation matrix the transpose of R, is the linear velocity of the quadrotor UAV in the body coordinate system, ω=(ω ₁ ,ω ₂ ,ω ₃ ) ^T is the angular velocity of the quadrotor UAV in the body coordinate system, sk(ω) represents the skew symmetric matrix , m and J respectively represent the mass and inertia of the quadrotor UAV, F is the external force of the quadrotor UAV, d is the external disturbance, τ is the moment acting on the quadrotor UAV, U ₁ is the quadrotor UAV is the total thrust of the machine body, g is the gravitational acceleration, E ₃ =e ₃ =(0,0,1) ^T is a unit vector.

Step S102 selects the image moment as the feature of visual servoing, and fuses the virtual feature plane to establish the image feature dynamics under the virtual camera plane including:

For a fixed point ^IP =[ ^I x, ^I y, ^I z] ^T in the inertial coordinate system, assuming that the camera coordinate system C coincides with the body coordinate system B, and the camera has a downward field of view, the fixed point The expression of a point in the camera coordinate system is ^C P=[ ^C x, ^C y, ^C z] ^T . At the same time, it is assumed that there is a virtual camera plane whose pitch angle and roll angle are always 0, and the yaw angle has nothing to do with the quadrotor. Man-machine synchronization, the origin coincides with the camera coordinate system, as shown in Figure 2, then the expression of this point in the virtual camera coordinate system is ^ν P = [ ^ν x, ^ν y, ^ν z] ^T , and the point is in the inertial coordinate system The relationship with the virtual camera coordinate system is:

in: is the rotation matrix around the Z axis, and ψ represents the yaw angle. The following derivative relationship can be obtained as:

in: is the velocity vector, v=[ ^ν v _x , ^ν v _y , ^ν v _z ] ^T .

According to the perspective projection relationship, the point P is expressed in the virtual camera plane as:

Where: λ is the focal length of the camera, ( ^ν u, ^ν n) is the point coordinates in the virtual camera plane. According to the above relationship, the dynamics of the projected points can be obtained as follows:

At this time, it is assumed that there are N fixed points in the inertial coordinate system and in the same plane, and their image moment features are:

in: ^ν u _k and ^ν n _k are the kth point, a = ^ν μ ₀₂ + ^ν μ ₂₀ is the moment value of the image, /> a ^* is the expected value of a. Using the above relationship, the dynamics of the image moment feature can be obtained as:

Among them: q＝[q _x ,q _y ,q _z ] ^T , z ^* is the expected height value.

In order to describe the yaw motion of the quadrotor UAV, the following feature description is used:

Step S103 is to design a dynamic image-based visual servo controller. First, analyze and design the disturbance observer according to the acceptance of the quadrotor UAV, then design the velocity observer in the virtual camera plane, and finally use the backstepping method to design an index-based Approaching sliding mode controllers, including:

First, clarify the control structure of the current system, as shown in Figure 3. Using the translational dynamic equation of quadrotor UAV The expression of the external disturbance can be obtained directly /> Then design the nonlinear disturbance observer as follows:

in: is the estimated value of external disturbance, and K>0 is the gain constant. Introducing auxiliary vectors /> Then its derivative with respect to time is:

Finally, the disturbance observer of the system is obtained as:

Now design the nonlinear velocity observer of the image moment feature of the system in the virtual camera coordinate system, first take a set of desired image moment features and /> At this time, the image moment-translation feature error is expressed as q ₁ =q-(0,0,1) ^T , and then using this relationship and image moment feature dynamics, the image feature error dynamic equation is written as:

Where: f=((-R _φθ U ₁ E ₃ )/m)+ge ₃ , R _φθ ＝R _θ R _φ . The nonlinear velocity observer in the virtual image plane is designed as follows:

in: and /> are the estimated values of q ₁ and v, respectively, /> and /> is the corresponding estimation error, both k ₁ and k ₂ are positive parameters.

After completing the design of the disturbance observer and the nonlinear velocity observer, the visual servo controller based on exponential approach is then designed, and the following formula is used as the controller:

in: and /> are the second-order error term and the third-order error term respectively, s=q ₃ is the sliding mode surface, /> is the yaw error term, φ and θ are the roll angle and pitch angle of the quadrotor UAV, respectively, sgn( ) represents a signed function, k ₃ , k ₄ , c ₁ , and η are all positive parameters, and the control parameters should satisfy the following relationship:

Professionals can appreciate that the unit calculation steps of the examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. An embodiment of the present invention provides a visual servo control method for a quadrotor UAV using a disturbance observer and a nonlinear velocity observer, including: establishing a corresponding dynamic model according to the spatial motion of the quadrotor UAV; selecting an image Moment is used as the feature of visual servoing, and the virtual feature plane is integrated to establish the dynamics of image features under the virtual camera plane; to design a dynamic image-based visual servo controller, firstly, according to the acceptance of quadrotor drones, analyze and design disturbance observation Then, the speed observer in the virtual camera plane is designed, and finally the sliding mode controller based on exponential approach is designed by using the backstepping method.

Claims (4)

1. A visual servo control method for a quadrotor unmanned aerial vehicle using a disturbance observer and a nonlinear velocity observer, comprising: According to the space motion of the quadrotor UAV, the corresponding dynamic model is established, and the dynamic model is as follows: F＝-U ₁ E ₃ +mgR ^T e ₃ Among them: ζ=(x, y, z) ^T is the representation of the origin of the body coordinate system in the inertial coordinate system, R: B→I is the rotation matrix between the body coordinate system and the inertial coordinate system, R ^T is the rotation matrix the transpose of R, is the linear velocity of the quadrotor UAV in the body coordinate system, ω=(ω ₁ ,ω ₂ ,ω ₃ ) ^T is the angular velocity of the quadrotor UAV in the body coordinate system, sk(ω) represents the skew symmetric matrix , m and J respectively represent the mass and inertia of the quadrotor UAV, F is the external force of the quadrotor UAV, d is the external disturbance, τ is the moment acting on the quadrotor UAV, U ₁ is the quadrotor UAV The total thrust of the machine body, g is the gravitational acceleration, E ₃ =e ₃ =(0,0,1) ^T is a unit vector; Select the image moment as the feature of visual servoing, and integrate the virtual feature plane to establish the image feature dynamics under the virtual camera plane, including: and /> in: and /> is the defined image moment feature, λ is the focal length of the camera, /> ^ν u _k and ^ν n _k are the kth point, a = ^ν μ ₀₂ + ^ν μ ₂₀ is the moment value of the image, /> Right now a ^* is the expected value of a, /> Indicates the derivative of the yaw angle to time, z ^* is the desired height value, v=[v _x , v _y , v _z ] ^T is the linear velocity of the UAV in the virtual camera plane; To design a dynamic image-based visual servo controller, first analyze and design a disturbance observer according to the force of the quadrotor UAV, in the form of: where /> is the estimated value of external disturbance, K>0 is the gain constant, is the introduced auxiliary vector, and then design the speed observer in the virtual camera plane, its form is: in: and /> are the estimated values of q ₁ and v, respectively, /> and /> is the corresponding estimation error, both k ₁ and k ₂ are positive parameters; Using the backstepping method to design a sliding mode controller based on exponential approach, its form is: in: and /> are the second-order error term and the third-order error term respectively, s=q ₃ is the sliding mode surface, /> is the yaw error term, φ and θ are the roll angle and pitch angle of the quadrotor UAV, respectively, sgn(·) represents a sign function, and k ₃ , k ₄ , c ₁ , η are all positive parameters. 2. the four-rotor unmanned aerial vehicle visual servo control method using disturbance observer and nonlinear velocity observer according to claim 1, described selection image moment is used as the feature of visual servo, and merges virtual feature plane, establishes virtual Dynamics of image features below the camera plane, including: and /> Its derivation and establishment process is as follows: For a fixed point ^IP =[ ^I x, ^I y, ^I z] ^T in the inertial coordinate system, assuming that the camera coordinate system C coincides with the body coordinate system B, and the camera has a downward field of view, the fixed point The expression of a point in the camera coordinate system is ^C P=[ ^C x, ^C y, ^C z] ^T . At the same time, it is assumed that there is a virtual camera plane whose pitch angle and roll angle are always 0, and the yaw angle has nothing to do with the quadrotor. Man-machine synchronization, the origin coincides with the camera coordinate system, then the expression of this point in the virtual camera coordinate system is ^ν P = [ ^ν x, ^ν y, ^ν z] ^T , the point is in the inertial coordinate system and the virtual camera coordinate system The relationship is: in: is the rotation matrix around the Z axis, ψ represents the yaw angle, and the following derivative relationship can be obtained: in: is the velocity vector, v=[ ^ν v _x , ^ν v _y , ^ν v _z ] ^T , according to the relationship of perspective projection, the point P is represented in the virtual camera plane as: Where: λ is the focal length of the camera, ( ^ν u, ^ν n) is the point coordinates in the virtual camera plane, according to the above relationship, the dynamics of the projected point can be obtained as follows: At this time, it is assumed that there are N fixed points in the inertial coordinate system and in the same plane, and their image moment features are: in: ^ν u _k and ^ν n _k are the kth point, a = ^ν μ ₀₂ + ^ν μ ₂₀ is the moment value of the image, /> i.e. /> a ^* is the expected value of a, using the above relationship, the dynamics of the image moment feature can be obtained as: Among them: q＝[q _x ,q _y ,q _z ] ^T , z ^* is the expected height value, describing the yaw motion of the quadrotor UAV, using the following feature description: The final image feature dynamics are: and /> 3. the four-rotor unmanned aerial vehicle visual servo control method using disturbance observer and nonlinear velocity observer according to claim 1, described design dynamics is based on the visual servo controller of image, at first according to four-rotor unmanned aerial vehicle , analyze its external disturbance and design a special nonlinear disturbance observer, its form is as follows: Its design derivation process is as follows: Translational Dynamic Equation of Quadrotor UAV The expression of the external disturbance can be obtained directly Then design the nonlinear disturbance observer as follows: in: is the estimated value of external disturbance, K>0 is the gain constant, and the auxiliary vector is introduced /> Then its derivative with respect to time is: /> Finally, the disturbance observer of the system is: /> 4. the four-rotor unmanned aerial vehicle visual servo control method using disturbance observer and nonlinear speed observer according to claim 1, described utilization backstepping design is based on the sliding mode controller of exponential approach, control parameter should satisfy the following relationship: