AUV recovery method and device based on uncalibrated model predictive visual servoing

Abstract

The application discloses an AUV recovery method based on a non-calibration model prediction visual servo, and comprises the following steps: arranging a light source on a recovery device to generate a guide signal; an AUV collects a light source image, takes the light source as a feature point, and confirms a self pose; a feature point tracking error is calculated according to a preset light source expected pose, and an interaction matrix and a covariance matrix are estimated through a forgetting factor-based recursive least square method; the AUV performs visual servo on the recovery device according to the interaction matrix and the covariance matrix, obtains an expected trajectory of the self, inputs the expected trajectory into a pre-constructed controller, outputs a control quantity, and arrives at a specified pose until; the forgetting factor-based recursive least square method is directly used to estimate the interaction matrix, the acquisition of camera parameters and feature point depth is avoided, the method is more suitable for underwater operation, and the accuracy of underwater control is improved.

Description

Technical Field

[0001] The present invention relates to the field of intelligent control technology, and more particularly to an AUV recovery method and device based on uncalibrated model predictive visual servoing. Background Art

[0002] At present, AUV (Autonomous Underwater Vehicle) can carry energy independently without relying on the mother ship, and complete operational tasks in an autonomous decision-making manner, playing an important role in civilian and military fields such as marine resource exploration, seabed topography mapping, and unfamiliar waters reconnaissance. Since AUV needs to be driven by its own energy (mostly batteries), it is difficult for the mother ship to exchange large amounts of data with the AUV through underwater acoustic communication. Therefore, the AUV is recovered through a recovery device to charge the AUV and complete data exchange. The AUV recovery process is generally divided into two stages. When the AUV is far away from the recovery device, it mainly relies on acoustic guidance to approach the recovery device. When it is closer, it relies on optical devices for precise docking. Traditional AUVs face the following problems during recovery, which reduces the recovery efficiency:

[0003] When docking an AUV at close range, either position-based visual servoing (PBVS) or image-based visual servoing (IBVS) is typically used. Both require precise acquisition of the camera's intrinsic and extrinsic parameters. Light reflected from underwater targets must pass through water, glass, and air before being perceived by the visual element. Furthermore, the large number of suspended particles in the underwater environment causes light scattering. This makes underwater calibration complex, time-consuming, and inaccurate.

[0004] The principle of PBVS is to use a binocular camera to obtain the three-dimensional position of the target and drive it close to the docking device. However, the binocular camera processes a large amount of visual signals and has a slow processing speed, which makes it difficult to meet real-time requirements. IBVS directly drives the AUV close to the docking device based on the image plane position of feature points. It only requires a monocular camera, but the feature points may exceed the camera's field of view, causing the target to be lost. At the same time, it is also difficult to limit the AUV's speed.

[0005] Therefore, how to accurately control and recover an AUV in an underwater environment is an urgent problem that needs to be solved by those skilled in the art. Summary of the Invention

[0006] In view of this, the present invention provides an AUV recovery method based on uncalibrated model predictive visual servoing, which avoids the acquisition of camera parameters and feature point depth, making it more suitable for underwater operations and helping to improve the accuracy of underwater control.

[0007] To achieve the above object, the present application adopts the following technical solutions:

[0008] An AUV recovery method based on a non-calibration model prediction visual servo, comprising the following steps:

[0009] A light source is arranged on the recovery device for generating a guide signal.

[0010] The AUV collects the light source image, takes the light source as a feature point, and confirms its own pose.

[0011] The feature point tracking error is calculated according to a preset light source expected pose, and the interaction matrix and the covariance matrix are estimated by a forgetting factor-based recursive least square method.

[0012] The AUV performs visual servo on the recovery device according to the interaction matrix and the covariance matrix, and obtains its own expected trajectory.

[0013] The expected trajectory is input to a pre-constructed controller, and a control amount is output until a specified pose is reached.

[0014] Preferably, the guide path is composed of a plurality of discrete pose features, each pose feature including one or more degrees of freedom.

[0015] Preferably, the confirmation of the own pose is specifically: the own pose is represented by a feature point attitude.

[0016] A plurality of feature points are acquired to confirm the feature point pose:

[0017]

[0018] Wherein, s represents the AUV pose, N represents the number of groups of feature points, u1 and v1 represent different two degrees of freedom in the first group of feature points respectively, u N and v N represent different two degrees of freedom in the first group of feature points respectively.

[0019] Preferably, the light source tracking error is calculated according to a preset light source expected path, and the interaction matrix and the covariance matrix are estimated by a forgetting factor-based recursive least square method, and the step comprises:

[0020] The light source tracking error Δs is calculated, Δs = s-s d ; wherein s is the current attitude of the feature point, and s d is the expected attitude of the feature point.

[0021] The forgetting factor-based recursive least square method is used for estimation:

[0022]

[0023]

[0024] where, is the estimated interaction matrix, P k is the covariance matrix, h k-1 = ζ k - ζ k-1 is the AUV six degrees of freedom motion step size, is the AUV generalized velocity, u, v, w, p, q, r represent different degrees of freedom respectively, λ ∈ (0, 1] is a forgetting factor, k represents the current time.

[0025] Preferably, the visual servoing step comprises:

[0026] constructing a target function of the AUV desired trajectory and the recovery device predicted trajectory.

[0027] constructing a constraint condition according to the interaction matrix, and constraining the recovery device predicted trajectory.

[0028] solving an optimal solution of the target function to obtain a final desired trajectory.

[0029] Preferably, the step of constructing a constraint condition according to the interaction matrix specifically comprises:

[0030]

[0031]

[0032]

[0033] where, is a predicted state vector of the recovery device from a starting time k to a time j, is a velocity input vector of the AUV from the starting time k to the time j, T is a system sampling time, s min , s max are a maximum value and a minimum value in an image plane constraint respectively, is a maximum constraint velocity of the AUV.

[0034] Preferably, the visual servoing target function is:

[0035]

[0036]

[0037] where, is a predicted state vector of the recovery device from a starting time k to a time j, is a velocity input vector of the AUV from the starting time k to the time j, N p is a prediction time domain, NC To control the time domain, and N c ≤N p , Q and R are weight matrices of tracking error and AUV velocity input vectors respectively, and determine and In the weight of the objective function, U k is the desired trajectory, represented by the six-degree-of-freedom desired generalized displacement of the AUV.

[0038] Preferably, the controller is an S-face controller based on the AUV dynamics model, and the output of the S-face controller is:

[0039]

[0040] Wherein, τ is the output of the controller, are the nominal inertia matrix, the Coriolis matrix, the damping matrix and the restoring force matrix of the AUV respectively, and ψ p , ψ I are the inverse normalization matrices, which inverse normalize the normalized motion state and inverse normalize the normalized motion state, K p , K I are the gains, Sig(K, x) is the S-face operator, and e represents the velocity error between the desired velocity and the current velocity of the AUV.

[0041] Preferably, a monocular camera is arranged on the AUV to collect light source images.

[0042] Preferably, an inertial navigation sensor, a Doppler log and a depth gauge are also arranged on the AUV to obtain real-time position and attitude; and a rudder and a propeller are arranged to drive the AUV to move to a specified pose.

[0043] According to the above technical solution, compared with the prior art, the application provides an AUV recovery method based on a non-calibration model predictive visual servo, which directly estimates the interaction matrix by using the recursive least square method based on the forgetting factor, avoids the calibration of the camera parameters and the acquisition of the feature point depth, is more suitable for underwater operation, and helps to improve the accuracy of underwater control; in the visual servo process, the state prediction of the recovery device is constrained based on the estimated interaction matrix, solving the problem of the target point exceeding the field of view of the camera in the traditional visual servo; an S-face controller based on the AUV dynamics model is designed, feedforward compensation is performed through the inertia matrix, the Coriolis matrix, the damping matrix and the restoring force matrix, feedback control is performed through the S-face operator, robust and stable control effect is realized, and the AUV is driven to complete the final recovery task. BRIEF DESCRIPTION OF DRAWINGS

[0044] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0045] Figure 1 A schematic diagram of an AUV recovery method based on uncalibrated model predictive visual servoing provided by the present invention.

[0046] Figure 2 It is a schematic diagram of the guiding principle in the implementation of the present invention.

[0047] Figure 3 Schematic diagram of characteristic point errors during the AUV recovery and docking process in an embodiment of the present invention.

[0048] Figure 4 Schematic diagram of the trajectory of characteristic points during the AUV recovery and docking process in an embodiment of the present invention.

[0049] Figure 5 Schematic diagram of the AUV position change trajectory during the guidance process of the present invention.

[0050] Figure 6 Schematic diagram of the trajectory of AUV posture changes during the guidance process of the present invention.

[0051] Figure 7 Schematic diagram of the internal structure of the AUV in an embodiment of the present invention. DETAILED DESCRIPTION

[0052] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0053] The embodiment of the present invention discloses an AUV recovery method based on uncalibrated model predictive visual servoing, such as Figure 1 and Figure 2 , including the following steps:

[0054] S1: Arrange a light source on the recovery device to generate a guide signal.

[0055] S2: The AUV collects light source images, uses the light source as a feature point, and confirms its own position.

[0056] S3: Calculate the feature point tracking error according to the preset light source expected pose, and estimate the interaction matrix and covariance matrix through the recursive least squares method based on the forgetting factor.

[0057] S4: The AUV performs visual servoing on the recovery device according to the interaction matrix and the covariance matrix to obtain the expected trajectory of itself.

[0058] The expected trajectory is input into a pre-constructed controller to output a control quantity until a specified pose is reached. The present application guides the AUV to run through the recovery device to achieve the recovery of the AUV.

[0059] Wherein, the present application establishes a model prediction framework to perform steps S3 and S4. The present application estimates the interaction matrix through the Broyden method without camera system internal and external parameters, and uses the model prediction framework to solve the camera image plane and AUV speed limitation problem. Finally, through the S surface control method, the expected trajectory given by the tracking guide law is tracked to complete the AUV recovery.

[0060] In one embodiment, a plurality of groups of LED light sources are fixed on the edge of the recovery device for generating a guide signal, and a monocular camera is arranged on the bow of the AUV to observe the light sources. The light sources are presented in the form of feature points on the image plane of the monocular camera. Since three feature points in space that are not on the same straight line can determine the unique pose of the AUV, the AUV can be recovered by driving the AUV to make the feature points on the image plane converge to the expected position.

[0061] To further implement the above technical solution, the AUV in the present application obtains the pose of the feature points by observing the feature points. Since the AUV adopts different poses for observation and the obtained feature point poses have a one-to-one correspondence, the pose of the AUV can be confirmed through the feature point pose. The data representing the pose is composed of one or more degrees of freedom.

[0062] Further, the pose of the AUV can be confirmed by collecting a plurality of groups of feature points. Each group of feature points corresponds to a specified pose. When the AUV is driven to make each group of feature points reach the specified pose, the recovery of the AUV is completed.

[0063] Specifically, the position s of the feature point on the image plane and the expected position s d can be represented as:

[0064]

[0065]

[0066] Wherein, u1 and v1 represent two different degrees of freedom in the first group of feature points; u N and v Nrespectively represent different two degrees of freedom in the first group of feature points.

[0067] In order to further implement the above technical solutions, for S3, the traditional IBVS is under the condition of camera internal parameter, external parameter and feature point depth, and according to the camera parameter information, the interaction matrix is constructed, so that the error converges exponentially, and the visual servo task is completed. However, the present application takes into account the particularity of underwater operation, and under the condition that the camera parameters and the feature point depth are unknown, it is difficult to directly obtain the interaction matrix, therefore, the recursive least square method based on the forgetting factor is used to estimate the interaction matrix, and the camera calibration process is avoided, and the implementation is as follows:

[0068] S31: calculate the light source tracking error Δs, Δs=s-s d ; wherein s is the current posture of the feature point, s d is the expected posture of the feature point.

[0069] S32: estimate based on the recursive least square method of the forgetting factor:

[0070]

[0071]

[0072] wherein, is the estimated interaction matrix, P k is the covariance matrix, h k-1 = ζ k - ζ k-1 is the six-degree-of-freedom motion step of the AUV, is the generalized velocity of the AUV, u, v, w, p, q, r respectively represent different degrees of freedom, λ∈(0, 1] is the forgetting factor, and k represents the current time.

[0073] In order to further implement the above technical solutions, for S4, the present application is based on the estimation of the interaction matrix, and avoids the underwater calibration process, and on this basis, the present application provides a visual servo method based on a non-calibration model, and the implementation is as follows:

[0074] S41: construct the target function of the expected trajectory of the AUV and the predicted trajectory of the recovery device;

[0075] S42: construct the constraint condition according to the interaction matrix, and constrain the predicted trajectory of the recovery device;

[0076] S43: solve the optimal solution of the target function to obtain the final expected trajectory.

[0077] In the embodiment, the AUV performs iterative prediction based on an initial feature point posture, and confirms a velocity input vector that makes a target function minimum based on a predicted state vector, so as to obtain a desired trajectory of the AUV itself.

[0078] Further, in order to further solve the problem that the target point exceeds the camera field of view in the traditional visual servo, a constraint condition is established based on an interaction matrix, so that the finally obtained solution satisfies the camera field of view restriction and the AUV velocity limit at the same time, and the specific implementation is as follows:

[0079] The target function is as follows:

[0080]

[0081]

[0082] wherein, is a predicted state vector of the recovery device from a starting time k to a time j, is a velocity input vector of the AUV from the starting time k to the time j, N p is a prediction time domain, N C is a control time domain, and N c ≤N p , Q and R are weight matrices of tracking error and AUV velocity input vector respectively, and determine and In the weight of the target function, U k is a desired trajectory, represented by a six-degree-of-freedom desired generalized displacement of the AUV.

[0083] The constraint condition is as follows:

[0084]

[0085]

[0086]

[0087] wherein, T is a system sampling time, s min , s max are a maximum value and a minimum value in the image plane constraint respectively, is a maximum constraint velocity of the AUV.

[0088] In order to further implement the above technical solution, the controller is an S-surface controller based on a dynamics model of the AUV, and an output of the S-surface controller is as follows:

[0089]

[0090] wherein, τ is an output of the controller, respectively, are the nominal inertia matrix, the Coriolis matrix, the damping matrix and the restoring torque matrix of the AUV dynamics model, respectively, is the inverse of the normalized motion state, K p I respectively, are the inverse of the normalized motion state, K p I is the gain, Sig(K,x) is the sigmod function, is the S face operator, e represents the velocity error of the AUV desired velocity and the current velocity.

[0091] The specific form of the S face operator is:

[0092]

[0093] wherein, is a unit vector, K is a gain matrix.

[0094] As Figure 3 , the present application monitors the error change of each feature point in the guiding process, converges to the expected position between 40-50s after the guiding occurs, and realizes the AUV recovery. Figure 4 is the trajectory of each feature point and the expected position, the feature point trajectory is stable and does not exceed the camera field of view limit. Figure 5 and Figure 6 , it is indicated that the AUV reaches the designated recovery position and attitude, and completes the recovery docking task.

[0095] In order to further implement the above technical scheme, an AUV device is provided in the embodiments of the present application, which can be applied to the above method, such as Figure 7 , the battery is arranged in the AUV watertight control cabin to supply power to the AUV system; the autonomous navigation core processor protects the motion control module and the image processing module, and communicates with the monocular camera through Ethernet or serial port and the like, the image processing module in the autonomous navigation core processor solves the real-time position s of the feature point; at the same time, the autonomous navigation core processor communicates with the inertial navigation, Doppler velocity log (DVL), depth gauge and the like through Ethernet, serial port, CAN bus and the like, to obtain the real-time position and attitude ζ of the AUV; the motion control module in the autonomous navigation core processor solves the AUV control force and control torque according to the data collected by the monocular camera, the inertial navigation, the DVL and the depth gauge, and controls the rudder, the propeller through DA, CAN bus, serial port and the like, to complete the AUV motion control and docking recovery.

[0096] ​​The various embodiments described in this specification are implemented in a progressive manner, each embodiment focusing on the differences from other embodiments, and the same or similar parts between embodiments can be mutually referred to. For the apparatus disclosed by the embodiments, since it corresponds to the method disclosed by the embodiments, the description is relatively simple, and the relevant parts can be referred to the description of the method part.

[0097] The above description of disclosed embodiments enables one of ordinary skill in the art to make or use the application. Various modifications to these embodiments will be readily apparent to those of ordinary skill in the art, and the generic principles defined herein can be applied to other embodiments without departing from the spirit or scope of the application. Therefore, the application is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. An AUV recovery method based on uncalibrated model predictive visual servoing, characterized in that: The following steps are involved: Arranging a light source on the recovery device to generate a guidance signal; The AUV collects light source images, uses the light source as a feature point, and confirms its own position; The feature point tracking error is calculated based on the preset expected light source pose, and the interaction matrix and covariance matrix are estimated by recursive least squares method based on forgetting factor; Calculate the light source tracking error Δs, Δs = ss d ; Where s is the current posture of the feature point, s d is the expected pose of the feature point; Estimation based on the recursive least squares method of the forgetting factor: in, is the estimated interaction matrix, P k is the covariance matrix, h k-1 =ζ k -ζ k-1 is the AUV six-degree-of-freedom motion step length, is the generalized speed of AUV, u, v, w, p, q, r represent different degrees of freedom, λ∈(0,1] is the forgetting factor, and k represents the current moment; The AUV performs visual servoing on the recovery device according to the interaction matrix and the covariance matrix to obtain its own desired trajectory; the visual servoing step includes: Construct the objective function of the AUV's expected trajectory and the recovery device's predicted trajectory; Constructing constraint conditions based on the interaction matrix to constrain the predicted trajectory of the recovery device; Solve the optimal solution of the objective function and obtain the final desired trajectory; The desired trajectory is input to a pre-built controller, which outputs the control variable until the specified pose is reached.

2. The AUV recovery method based on uncalibrated model predictive visual servoing according to claim 1, characterized in that: The guidance signal is composed of a plurality of discrete posture features, each of which includes one or more degrees of freedom.

3. The AUV recovery method based on uncalibrated model predictive visual servoing according to claim 2, characterized in that: The confirming of the self-posture is specifically: expressing the self-posture through the posture of feature points; Get multiple sets of feature points and confirm their positions and poses: Among them, S represents the AUV pose, N represents the number of feature points, u1 and v1 represent the two different degrees of freedom in the first group of feature points, u N and v N They represent two different degrees of freedom in the Nth group of feature points.

4. The AUV recovery method based on uncalibrated model predictive visual servoing according to claim 1, characterized in that: The constraint conditions constructed according to the interaction matrix are specifically: in, is the predicted state vector of the recovery device from the starting time j to the time k, is the velocity input vector of AUV from the starting time j to the time k, T is the system sampling time, s min 、s max are the maximum and minimum values ​​in the image plane constraints, respectively. is the maximum constrained speed of the AUV.

5. The AUV recovery method based on uncalibrated model predictive visual servoing according to claim 1, characterized in that: The objective function of the visual servoing is: in, is the predicted state vector of the recovery device from the starting time j to the time k, is the velocity input vector of AUV from the starting time j to the time k, N p is the prediction time domain, N C To control the time domain, and there are N c ≤N p , Q and R are the weight matrices of the tracking error and AUV velocity input vector, respectively, U k is the expected trajectory, which is represented by the expected generalized displacement of the AUV in six degrees of freedom.

6. The AUV recovery method based on uncalibrated model predictive visual servoing according to claim 1, characterized in that: The controller is an S-plane controller based on the AUV dynamic model, and the output of the S-plane controller is: Where τ is the output of the controller, are the nominal inertia matrix, Coriolis force matrix, damping matrix and restoring torque of the AUV dynamic model, ψ p , ψ I are the corresponding anti-normalized matrices, K p , K I is the gain, Sig(K,x) is the S-plane operator, and e represents the speed error between the desired speed and the current speed of the AUV.

7. The AUV recovery method based on uncalibrated model predictive visual servoing according to claim 1 is characterized in that: A monocular camera is arranged on the AUV for collecting light source images.

8. The AUV recovery method based on uncalibrated model predictive visual servoing according to claim 7 is characterized in that: The steps also include: An inertial navigation sensor, a Doppler odometer, and a depth meter are arranged on the AUV to obtain real-time position and attitude; a rudder and a thruster are arranged to drive the AUV to move to a specified position and attitude.

Images