An active vision-based leader-follower method for unmanned surface vehicle formation control

Abstract

This invention discloses a navigation-following unmanned surface vessel (USV) formation control method based on active vision, comprising the following steps: constructing a kinematic model of the USV based on a geodetic coordinate system; establishing a gimbal monocular camera model for the gimbal camera mounted on the USV; defining servo tracking errors for the navigation-following USV formation system by combining the USV kinematic model and the gimbal monocular camera model; defining image coordinate errors based on the viewing angle constraints of the gimbal monocular camera; designing a motion controller and adaptive parameter update law for the USV using the Lyapunov synthesis method; and designing a gimbal camera steering controller and adaptive parameter update law using the Lyapunov synthesis method. This method ensures the smooth implementation of visual servo control of the USV by defining servo tracking errors and image coordinate errors, and then incorporating these two errors into the Lyapunov synthesis method to design the USV motion controller, the gimbal camera steering controller, and the adaptive parameter update law.

Description

Technical Field

[0001] This invention belongs to the field of visual servo control technology for unmanned surface vessels (USVs), specifically relating to a navigator-follower USV formation control method based on active vision. Background Technology

[0002] Unmanned surface vessels (USVs) have wide applications in marine engineering, including marine mapping, fault detection, cruising, and target search. In these applications, cooperative formation control is crucial for improving efficiency, robustness, and coverage. To achieve formation control, various methods have been proposed, including pilot-follower strategies, virtual structures, and behavior-based approaches. Among these, the pilot-follower strategy is popular due to its convenience and scalability.

[0003] Traditional pilot-follower formation control methods require global position information for each unmanned surface vessel (USV) and communication between them. This approach suffers from several problems, such as communication latency and packet loss. Furthermore, formation control becomes challenging when GPS signals are unavailable or communication is disrupted. To overcome these issues, researchers have extensively explored the application of visual cameras in cooperative formation control of USVs. Compared to other sensors, visual cameras offer advantages such as low cost and rich information content, potentially expanding the application scope of pilot-follower methods.

[0004] In visual camera applications, monocular vision has attracted researchers' attention due to its low cost, ease of use, and maintenance. However, due to the limited field of view of monocular cameras, observation of the target may be lost. To address this issue, omnidirectional vision has been proposed as a solution, as it can avoid the field-of-view limitation. However, omnidirectional vision suffers from severe image distortion, which limits its application. Therefore, how to design a control method that can overcome the field-of-view limitation of monocular vision and avoid the image distortion problem of omnidirectional vision is one of the technical problems that those skilled in the art must solve. Summary of the Invention

[0005] This invention focuses on the problem of unmanned surface vessel (USV) swarm tracking control based on active vision. It proposes a novel method that can complete the swarm tracking task using only two-dimensional image information acquired by a gimbal camera. Compared to traditional methods, this method eliminates the need to measure global or relative position information or to communicate between USVs. Furthermore, it ensures that the target's field of view is not lost during movement, thus achieving stable swarm tracking control. In addition, this invention separates motion control and camera field of view constraints, designing separate motion controllers for the USVs and gimbal camera steering controllers, significantly reducing controller complexity.

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

[0007] In a first aspect, the present invention provides a method for controlling unmanned surface vessels (USVs) in formation based on active vision-based navigation-following, the method comprising the following steps:

[0008] A kinematic model of the unmanned surface vessel (USV) is constructed based on the geodetic coordinate system. This kinematic model is used to describe the dynamic relationship between the USV's position, heading angle, and velocity.

[0009] Based on the imaging principle of a gimbal monocular camera, a gimbal monocular camera model is established for a gimbal camera mounted on an unmanned surface vessel. The gimbal monocular camera model is used to describe the mapping of three-dimensional spatial points projected onto the image plane of the gimbal camera.

[0010] Combining the kinematic model of the unmanned surface vessel (USV) and the gimbal monocular camera model, a servo tracking error is defined for the pilot-follower USV formation system. The servo tracking error represents the deviation between the actual position coordinates and the desired position coordinates of the following USV.

[0011] Based on the viewing angle constraint of the gimbal monocular camera, image coordinate error is defined to ensure that the target point remains within the camera's field of view;

[0012] The motion controller and adaptive parameter update law of the unmanned surface vessel were designed by Lyapunov synthesis method, and the motion parameters of the navigator were estimated online.

[0013] The gimbal camera steering controller and adaptive parameter update law are designed using the Lyapunov synthesis method to dynamically adjust the monocular orientation of the gimbal.

[0014] As a preferred technical solution, the kinematic model of the unmanned surface vessel is represented as follows:

[0015]

[0016] Among them, (x i ,y i θ represents the position of the i-th unmanned surface vessel in the geodetic coordinate system; i Let u be the heading angle of the i-th unmanned surface vessel in the geodetic coordinate system; i ν i and ω i Let be the sway velocity, yaw velocity, and bow roll velocity of the i-th unmanned surface vessel relative to the Earth coordinate system, respectively.

[0017] As a preferred technical solution, the gimbal monocular camera model is specifically as follows:

[0018] Based on the imaging principle of a gimbal-mounted monocular camera, a point (X) in three-dimensional space... c ,Y c Z cThe pixel coordinates (m,n) of the camera and its projection onto the image plane of the gimbal camera are collinear with the optical center of the gimbal monocular camera. Therefore, for a gimbal monocular camera mounted on an unmanned surface vessel, the model can be represented as:

[0019]

[0020] Among them, a m a n These are the camera volume coordinate system X c axis and Y c The magnification factor along the axial direction, (m0, n0) are the pixel coordinates of the camera center point, and the camera's intrinsic parameter a m a n The center pixel coordinates (m0, n0) can be obtained using Zhang Zhengyou's calibration method.

[0021] As a preferred technical solution, the servo tracking error is specifically as follows:

[0022]

[0023] Among them, (X) d ,Y d ) is the desired position coordinate of the follower in the navigator's hull coordinate system, and the tracking error (X) is the following: e ,Y e (x) represents the desired position coordinates in the follower's hull coordinate system. l ,y l ) and (x f ,y f θ represents the positions of the navigator and follower unmanned surface vessels in the geodetic coordinate system. f β is the angle through which the follower rotates relative to the Earth's coordinate system, and β is the angle through which the navigator rotates relative to the follower, satisfying β = θ. l -θ f θ l d is the angle that the navigator has rotated relative to the geodetic coordinate system, and d is the distance between the geometric center of the follower unmanned surface vessel and the origin of the camera's body coordinate system.

[0024] Tracking error (X) e ,Y e The transformation is performed to avoid using global position information. The transformed result is as follows:

[0025]

[0026] Where, x e =X e / Z c ,y e =Y e / Z c xd =X d / Z c ,y d =Y d / Z C x c =X c / Z c y c =Y c / Z c , R(β) is the angle rotated by the gimbal monocular camera relative to the follower UAV, which can be directly read from the encoder of the gimbal camera; R(β) is the coordinate system O of the navigator. l X l Y l To the follower's hull coordinate system O c X f Y f The rotation matrix, The camera volume coordinate system O c X c Y c To the follower's hull coordinate system O c X f Y f The rotation matrix.

[0027] As a preferred technical solution, the image coordinate error is expressed as follows:

[0028]

[0029] When the image coordinate error approaches zero, the target point is located in the center of the camera's field of view.

[0030] As a preferred technical solution, the design of the unmanned surface vessel motion controller and adaptive parameter update law using the Lyapunov synthesis method is specifically as follows:

[0031] The motion controller for an unmanned surface vessel (USV) is designed according to the Lyapunov direct method, and is expressed as follows:

[0032]

[0033] Where, k x k v k ω All design parameters are positive; the design adaptive parameter update law estimates the unknown parameters. for:

[0034]

[0035]

[0036] Where, ε d ε u ε v and ε ω It is a positive constant close to zero; Ω d =-y e ω f Ω u =-x e cosβ-y e sinβ, Ω v =x e sinβ-y e cosβ, Ω ω =x e (x d sinβ+y d cosβ)-y e (x d cosβ-y d sinβ); The longitudinal and lateral velocities and bow roll angular velocities and their accelerations of the Navigator unmanned surface vessel are all bounded, i.e., they possess positive constants δ. c u , c v , Make:

[0037]

[0038] Because ω z =ω l Z c Therefore, the present invention can select appropriate constants. κ ω and Make:

[0039]

[0040] As a preferred technical solution, the gimbal camera steering controller is designed according to the Lyapunov direct method, and is expressed as follows:

[0041]

[0042] in,

[0043]

[0044] Design an adaptive parameter update law to estimate unknown parameters. for:

[0045]

[0046]

[0047] Where, η α η d η u η v These are positive constants; some positive constants are defined as follows: λ α , λ d ,

[0048] λ u , λ v , And σ, such that the unknown variables α, d z υ u υ v The following constraints must be satisfied:

[0049]

[0050] Secondly, the present invention provides an active vision-based navigation-following unmanned surface vessel (USV) formation control system, which is applied to the active vision-based navigation-following USV formation control method, including a USV kinematic model construction module, a camera model construction module, a tracking error determination module, an image coordinate error determination module, a USV controller design module, and a camera steering controller design module.

[0051] The unmanned surface vessel (USV) kinematic model construction module is used to construct a USV kinematic model based on a geodetic coordinate system. The USV kinematic model is used to describe the dynamic relationship between the USV's position, heading angle, and velocity.

[0052] The camera model construction module is used to build a gimbal monocular camera model for the gimbal camera mounted on the unmanned surface vessel based on the imaging principle of the gimbal monocular camera. The gimbal monocular camera model is used to describe the mapping of three-dimensional spatial points projected onto the image plane of the gimbal camera.

[0053] The tracking error determination module is used to combine the kinematic model of the unmanned surface vessel and the gimbal monocular camera model to define the servo tracking error for the navigator-follower unmanned surface vessel formation system. The servo tracking error represents the deviation between the actual position coordinates and the expected position coordinates of the following vessel.

[0054] The image coordinate error determination module is used to define the image coordinate error based on the viewing angle constraint of the pan-tilt monocular camera, so as to ensure that the target point remains within the camera's field of view.

[0055] The unmanned surface vessel controller design module is used to design the motion controller and adaptive parameter update law of the unmanned surface vessel using the Lyapunov synthesis method, and to estimate the navigator motion parameters online.

[0056] The camera steering controller design module is used to design a gimbal camera steering controller and an adaptive parameter update law using the Lyapunov synthesis method, and to dynamically adjust the monocular orientation of the gimbal.

[0057] Thirdly, the present invention provides an unmanned surface vessel equipped with a gimbal camera, and designs an unmanned surface vessel motion controller and a gimbal camera controller. The controller is constructed using the aforementioned active vision-based lead-follow unmanned surface vessel formation control method.

[0058] Fourthly, the present invention provides a computer-readable storage medium storing a program, which, when executed by a processor, implements the aforementioned active vision-based navigation-following unmanned surface vessel formation control method.

[0059] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0060] 1. This invention uses a gimbal monocular camera as the sole sensor in the formation system, overcoming the inherent field-of-view constraints of monocular cameras and the severe image distortion of omnidirectional cameras. Furthermore, it enables formation control tasks to be completed solely based on the two-dimensional image information acquired by the camera, without the need to measure the global or relative position information of the unmanned surface vessel.

[0061] 2. This invention uses a parameter estimation method to fit the longitudinal, lateral and yaw angular velocities of the navigator, eliminating the need to rely on communication to obtain the speed information of the navigator unmanned surface vessel required for controller design.

[0062] 3. This invention designs a motion controller for the unmanned surface vessel and a gimbal camera steering controller to realize the navigation-following formation task. This design method greatly reduces the complexity of each controller. Attached Figure Description

[0063] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0064] Figure 1 This is a flowchart of the unmanned surface vessel formation control method based on active vision in an embodiment of the present invention;

[0065] Figure 2 This is a schematic diagram of the positional relationships of the unmanned surface vessel (USV) formation system using the pilot-follower method in an embodiment of the present invention.

[0066] Figure 3 This is a motion trajectory diagram of the unmanned surface vessel using the navigation-following method in an embodiment of the present invention;

[0067] Figure 4 The unmanned surface vessel servo tracking error in the embodiments of the present invention and Simulation diagram;

[0068] Figure 5 The image coordinate error e of the gimbal camera in this embodiment of the invention y1 and e y2 The simulation diagram. Detailed Implementation

[0069] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of the present application, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present application without creative effort are within the scope of protection of the present application.

[0070] In this application, the reference to "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a mutually exclusive, independent, or alternative embodiment. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described in this application can be combined with other embodiments.

[0071] Active vision combines monocular vision with rotational freedom, overcoming the field-of-view limitations of monocular vision and avoiding image distortion problems in omnidirectional vision. Active vision involves mounting a monocular camera on a 360° rotating gimbal and adjusting the camera's orientation towards the target based on the current motion state. This method requires designing two controllers: a motion controller for the unmanned surface vessel (USV) and a gimbal camera controller. By independently handling motion control and field-of-view limitations, the complexity of controller design is simplified, and the effectiveness and stability of formation control are enhanced.

[0072] The pilot-follower unmanned surface vessel (USV) swarm system is a classic and widely used multi-agent coordinated control strategy for USVs. Its core idea is to establish a hierarchical control system structure, in which:

[0073] Lead vessel: One (or more) USVs are designated as the leader in a formation. It typically undertakes core mission roles (such as planning the route, performing major reconnaissance, and making decisions), or navigates autonomously according to pre-set mission instructions.

[0074] Follower vessels: Other USVs in the formation act as followers. Their primary objective is not to directly track a pre-defined global waypoint, but to maintain a specific relative position and attitude relationship with the lead vessel and any nearby follower vessels.

[0075] like Figure 1 , 2 As shown in Figures 3, 4, and 5, this embodiment provides a method for unmanned surface vessel (USV) formation control based on active vision, including the following steps:

[0076] S1. Based on the geodetic coordinate system, construct a kinematic model for all unmanned surface vessels, specifically as follows:

[0077] In the geodetic coordinate system, a kinematic model is constructed for all unmanned surface vessels, represented as follows:

[0078]

[0079] Among them, (x i ,y i θ represents the position of the i-th unmanned surface vessel in the geodetic coordinate system; i Let u be the heading angle of the i-th unmanned surface vessel in the geodetic coordinate system; i ν i and ω i Let be the sway velocity, yaw velocity, and bow roll velocity of the i-th unmanned surface vessel relative to the Earth coordinate system, respectively.

[0080] like Figure 2 As shown, this embodiment uses three unmanned surface vessels (USVs): one as the navigator (code 0) and two as followers (code 1 and 2). The navigator's initial state is [x0(0), y0(0)]. T =[1,1] T θ0(0)=π / 2rad, the initial state of the follower unmanned surface vessel is [x1(0),y1(0)]. T =[2,-1] T , θ1(0)=π / 3rad, [x2(0),y2(0)] T =[0,-1] T θ2(0)=-π / 3rad. The expected coordinates of follower 1 and follower 2 relative to the navigator UAV are respectively Given the speed of the navigator unmanned surface vessel:

[0081]

[0082] The reference trajectory is a straight line when t < 15s, and a gradually shrinking arc after t > 15s. The motion trajectories of the navigator UAV and the follower UAV implementing servo control are as follows: Figure 3 As shown.

[0083] S2. Based on the imaging principle of a gimbal-mounted monocular camera, a mathematical model is established for the gimbal-mounted camera mounted on the unmanned surface vessel, specifically as follows:

[0084] Based on the imaging principle of a gimbal-mounted monocular camera, a point (X) in three-dimensional space... c ,Y c Z c The pixel coordinates (m,n) of the camera and its projection onto the image plane of the gimbal camera are collinear with the optical center of the gimbal camera. Therefore, the model of the gimbal camera mounted on the unmanned surface vessel can be represented as follows:

[0085]

[0086] Among them, a m a n These are the camera volume coordinate system ω c axis and Y c The magnification factor along the axial direction, (m0, n0) are the pixel coordinates of the camera center point. The camera's intrinsic parameter a... m a n The center point pixel coordinates (m0, n0) can be obtained using Zhang Zhengyou's calibration method. In this embodiment, the resolution of the gimbal camera is 768×576, and the volume coordinate system X... c axis and Y c Amplification factor a in the axial direction m =902, a n =902, center pixel coordinates (m0,n0) = (384,288), feature point height Z on the Navigator unmanned surface vessel. c =0.3.

[0087] S3. Combining the kinematic model of the unmanned surface vessel (USV) and the gimbal monocular camera model, the servo tracking error is defined for the pilot-follower USV formation system as follows:

[0088]

[0089] like Figure 2 As shown, (X) d ,Y d ) is the desired position coordinate of the follower in the navigator's hull coordinate system, and the tracking error (X) is the following: e ,Y e (x) represents the desired position coordinates in the follower's hull coordinate system. l ,y l) and (x f ,y f θ represents the positions of the navigator and follower unmanned surface vessels (USVs) in the geodetic coordinate system. f β is the angle through which the follower rotates relative to the Earth's coordinate system, and β is the angle through which the navigator rotates relative to the follower, satisfying β = θ. l -θ f θ l d is the angle the navigator has rotated relative to the Earth's coordinate system. d is the distance between the geometric center of the follower UAV and the origin of the camera's body coordinate system. It is worth noting that the above error equation requires measuring the global position of the UAV. This invention will avoid using global position information; therefore, the above error is modified as follows:

[0090]

[0091] Where, x e =X e / Z c ,y e =Y e / Z c x d =X d / Z c ,y d =Y d / Z C x c =X c / Z c y c =Y c / Z c , The angle that the gimbal camera has turned relative to the following unmanned surface vessel can be directly read from the encoder of the gimbal camera. The camera volume coordinate system O c X c Y c To the follower's hull coordinate system O c X f Y f The rotation matrix, R(β), is the coordinate system of the navigator hull. l X l Y l To the follower's hull coordinate system O c X f Y f The rotation matrix.

[0092] S4. Based on the viewing angle constraints of the pan-tilt monocular camera, define the image coordinate error as follows:

[0093]

[0094] The viewing angle constraint of the pan-tilt monocular camera refers to the fact that the camera has a horizontal viewing angle range and a vertical viewing angle range. The range of pixel coordinates on the image plane is limited. Therefore, the following pixel coordinate error is designed, and through subsequent controller design, it is made to approach zero, so as to ensure that the target point is always within the camera's field of view.

[0095] S5. Based on the definition of unmanned surface vessel error in step S3 above, the following Lyapunov equation can be selected:

[0096]

[0097] in, These are the offset d and the estimation errors of the navigator's velocity in three directions, ω and ω, respectively. z =ω l Z c γ d γ u γ v and γ ω These are design parameters. The motion controller for the unmanned surface vessel (USV) is designed according to the Lyapunov direct method, and is expressed as:

[0098]

[0099] Where, k x k v k ω All design parameters are positive. The design adaptive parameter update law estimates the unknown parameters. for:

[0100]

[0101] Where, ε d ε u ε v and ε ω It is a positive constant close to zero. Ω d =-y e ω f Ω u =-x e cosβ-y e sinβ, Ω v =x e sinβ-y e cosβ, Ω ω =x e (x d sinβ+y d cosγ)-y e (x d cosβ-y dThe longitudinal and lateral velocities and bow roll angular velocities and their accelerations of the Navigator unmanned surface vessel are all bounded, meaning they possess positive constants δ. c u , c v , Make:

[0102]

[0103] Because ω z =ω l Z c Therefore, the present invention can select appropriate constants. κ ω and Make:

[0104]

[0105] In this embodiment, d = 0.1, k x1 =2,k y1 =2,k x2 =2.5, k x2 =2.5, c d =0.04, c u =0.02, c v =0.01, κ ω =-0.1, δ=0.25, γ d =0.01, γ u =0.04, γ v =0.03, γ ω =0.003, ε d =0.01, ε u =0.01, ε v =0.01, ε ω =0.01. Where k x1 k y1 and k x2 k x2 ... e1 y e1 and x e2 y e2 e represents the servo tracking error of follower UV1 and follower UV2, respectively. y1 and e y2These represent the image coordinate errors of the onboard gimbal cameras of follower UAV 1 and follower UAV 2, respectively.

[0106] Figure 4 Demonstrates servo tracking error and The graph shows the changes, and it can be seen that both tracking errors converge quickly to near zero, which further verifies the... Figure 3 The accuracy of the trajectory diagram shown.

[0107] S6. Based on the definition of image coordinate error in step S4 above, the following Lyapunov equation can be selected:

[0108]

[0109] Where, μ α μ d μ u and μ v It is a positive number. α = 1 / Z c ,d z =d / Z c ,υ u =u l / Z c ,υ v =v l / Z c The gimbal camera steering controller is designed according to Lyapunov's direct method, and is expressed as follows:

[0110]

[0111] in,

[0112]

[0113]

[0114] Design an adaptive parameter update law to estimate unknown parameters. for:

[0115]

[0116] Where, η α η d η u η v These are positive constants. To facilitate the estimation of the above parameters, this invention defines the following positive constants: λ α , λ d , λu , λ v , And σ, such that the unknown variables α, d z υ u v v The following constraints must be satisfied:

[0117]

[0118] In this embodiment, k c =5, λ α =2, λ d =0.2, λ u =0.2, λ v =0.1, σ = 0.2, μ α =0.0001, μ d =0.0001, μ u =0.0001, μ v =0.0001, η α =0.1, η d =0.1, η u =0.1, η v =0.1.

[0119] Figure 5 The image coordinate error e is shown y1 and e y2 The changes in the graph show that both errors converge quickly to near zero, proving that the field of view constraint of the gimbal camera is satisfied, that is, the target feature points are always within the field of view of the gimbal camera of the follower UAV during the formation movement.

[0120] The motion controller and gimbal camera steering controller of this embodiment enable the servo control system composed of the unmanned surface vessel and the gimbal monocular camera to track specified feature points using only the two-dimensional image information acquired by the unmanned surface vessel itself, and the servo tracking error can converge to a small neighborhood of zero. Although the system model contains unknown parameters and dynamic variables, the closed-loop system can still achieve asymptotic stability; moreover, it can ensure that the feature point is always within the field of view of the gimbal camera.

[0121] This invention addresses the navigator-follower unmanned surface vessel (USV) formation tracking control problem by designing a USV motion controller and a gimbal camera steering controller to achieve the navigator-follower formation task. To address the inherent field-of-view constraints of monocular cameras and the severe image distortion of omnidirectional cameras, this invention employs a gimbal-mounted monocular camera as the sensor for the formation system. Furthermore, it achieves formation control solely based on the two-dimensional image information acquired by the camera, without requiring the measurement of the USV's global or relative position information. Considering the inability to obtain the velocity information of the navigator USV in a formation system without communication, this invention uses a parameter estimation method to fit the navigator's longitudinal, lateral, and yaw angular velocities. To verify the effectiveness of the constructed control algorithm, this invention conducts a simulation using the Matlab simulation environment. The results show that the designed control algorithm can effectively maintain formation control.

[0122] It should be noted that, for the sake of simplicity, the aforementioned method embodiments are all described as a series of actions. However, those skilled in the art should understand that the present invention is not limited to the described order of actions, because according to the present invention, some steps can be performed in other orders or simultaneously.

[0123] Based on the same concept as the active vision-based navigation-following unmanned surface vessel (USV) formation control method in the above embodiments, this invention also provides an active vision-based navigation-following USV formation control system. This system can be used to execute the aforementioned active vision-based navigation-following USV formation control method. For ease of explanation, the structural diagram of the active vision-based navigation-following USV formation control system embodiment only shows the parts related to the embodiments of this invention. Those skilled in the art will understand that the illustrated structure does not constitute a limitation on the device, and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0124] In another embodiment of this application, a navigation-following unmanned surface vessel (USV) formation control system based on active vision is provided. The system includes a USV kinematic model building module, a camera model building module, a tracking error determination module, an image coordinate error determination module, a USV controller design module, and a camera steering controller design module.

[0125] The unmanned surface vessel (USV) kinematic model construction module is used to construct a USV kinematic model based on a geodetic coordinate system. The USV kinematic model is used to describe the dynamic relationship between the USV's position, heading angle, and velocity.

[0126] The camera model construction module is used to build a gimbal monocular camera model for the gimbal camera mounted on the unmanned surface vessel based on the imaging principle of the gimbal monocular camera. The gimbal monocular camera model is used to describe the mapping of three-dimensional spatial points projected onto the image plane of the gimbal camera.

[0127] The tracking error determination module is used to combine the kinematic model of the unmanned surface vessel and the gimbal monocular camera model to define the servo tracking error for the navigator-follower unmanned surface vessel formation system. The servo tracking error represents the deviation between the actual position coordinates and the expected position coordinates of the following vessel.

[0128] The image coordinate error determination module is used to define the image coordinate error based on the viewing angle constraint of the pan-tilt monocular camera, so as to ensure that the target point remains within the camera's field of view.

[0129] The unmanned surface vessel controller design module is used to design the motion controller and adaptive parameter update law of the unmanned surface vessel using the Lyapunov synthesis method, and to estimate the navigator motion parameters online.

[0130] The camera steering controller design module is used to design a gimbal camera steering controller and an adaptive parameter update law using the Lyapunov synthesis method, and to dynamically adjust the monocular orientation of the gimbal.

[0131] It should be noted that the active vision-based navigation-following unmanned surface vessel (USV) formation control system of the present invention corresponds one-to-one with the active vision-based navigation-following USV formation control method of the present invention. The technical features and beneficial effects described in the embodiments of the active vision-based navigation-following USV formation control method are applicable to the embodiments of the active vision-based navigation-following USV formation control. For details, please refer to the description in the embodiments of the present invention, which will not be repeated here.

[0132] Furthermore, in the above embodiments of the active vision-based navigation-following unmanned surface vessel (USV) swarm control system, the logical division of each program module is merely illustrative. In actual applications, the above functions can be assigned to different program modules as needed, for example, for the sake of corresponding hardware configuration requirements or the convenience of software implementation. That is, the internal structure of the active vision-based navigation-following USV swarm control system can be divided into different program modules to complete all or part of the functions described above.

[0133] Another embodiment of this application provides an unmanned surface vessel (USV) equipped with a controller, which is constructed using the aforementioned active vision-based USV formation control method, specifically as follows:

[0134] A kinematic model of the unmanned surface vessel (USV) is constructed based on the geodetic coordinate system. This kinematic model is used to describe the dynamic relationship between the USV's position, heading angle, and velocity.

[0135] Based on the imaging principle of a gimbal monocular camera, a gimbal monocular camera model is established for a gimbal camera mounted on an unmanned surface vessel. The gimbal monocular camera model is used to describe the mapping of three-dimensional spatial points projected onto the image plane of the gimbal camera.

[0136] Combining the kinematic model of the unmanned surface vessel (USV) and the gimbal monocular camera model, a servo tracking error is defined for the pilot-follower USV formation system. The servo tracking error represents the deviation between the actual position coordinates and the desired position coordinates of the following USV.

[0137] Based on the viewing angle constraint of the gimbal monocular camera, image coordinate error is defined to ensure that the target point remains within the camera's field of view;

[0138] The motion controller and adaptive parameter update law of the unmanned surface vessel were designed by Lyapunov synthesis method, and the motion parameters of the navigator were estimated online.

[0139] The gimbal camera steering controller and adaptive parameter update law are designed using the Lyapunov synthesis method to dynamically adjust the monocular orientation of the gimbal.

[0140] Furthermore, if the modules / units integrated into the unmanned surface vessel are implemented as software functional units and sold or used as independent products, they can be stored in a non-volatile computer-readable storage medium. The computer-readable medium may include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, or a read-only memory (ROM).

[0141] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments described above. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0142] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0143] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A platoon control method for unmanned surface vessels based on active vision-guided navigation-following, characterized in that, The method includes the following steps: A kinematic model of the unmanned surface vessel (USV) is constructed based on the geodetic coordinate system. This kinematic model is used to describe the dynamic relationship between the USV's position, heading angle, and velocity. Based on the imaging principle of a gimbal monocular camera, a gimbal monocular camera model is established for a gimbal camera mounted on an unmanned surface vessel. The gimbal monocular camera model is used to describe the mapping of three-dimensional spatial points projected onto the image plane of the gimbal camera. Combining the kinematic model of the unmanned surface vessel (USV) and the gimbal monocular camera model, a servo tracking error is defined for the pilot-follower USV formation system. The servo tracking error represents the deviation between the actual position coordinates and the desired position coordinates of the following USV. Based on the viewing angle constraint of the gimbal monocular camera, image coordinate error is defined to ensure that the target point remains within the camera's field of view; The motion controller and adaptive parameter update law of the unmanned surface vessel were designed by Lyapunov synthesis method, and the motion parameters of the navigator were estimated online. The gimbal camera steering controller and adaptive parameter update law are designed using the Lyapunov synthesis method to dynamically adjust the monocular orientation of the gimbal. The design of the unmanned surface vessel motion controller and adaptive parameter update law using the Lyapunov synthesis method is as follows: The motion controller for an unmanned surface vessel (USV) is designed according to the Lyapunov direct method, and is expressed as follows: in, , , All are positive design parameters. It is the angle from which the leader turns relative to the followers. Design an adaptive parameter update law to estimate unknown parameters. , , , for: in, , , and It is a positive constant close to zero; , , , The longitudinal and lateral velocities, as well as the bow angular velocity and acceleration of the Navigator unmanned surface vessel, are all bounded, meaning they possess a constant δ. , , , , , , Make: because Therefore, the present invention can select appropriate constants. and , so that: 。 2. The unmanned surface vessel (USV) formation control method based on active vision according to claim 1, characterized in that, The kinematic model of the unmanned surface vessel is represented as follows: in, Indicates the first The position of the unmanned surface vessel in the geodetic coordinate system; For the first The heading angle of the unmanned surface vessel in the geodetic coordinate system; , and The first The sway velocity, roll velocity, and bow angular velocity of the unmanned surface vessel relative to the Earth's coordinate system.

3. The unmanned surface vessel (USV) formation control method based on active vision according to claim 1, characterized in that, The specific model of the gimbal monocular camera is as follows: Based on the imaging principle of a gimbal-mounted monocular camera, a point in three-dimensional space and the pixel coordinates of its projection onto the image plane of the gimbal camera. The three points of the gimbal monocular camera are collinear with the optical center of the camera. Therefore, the model for the gimbal monocular camera mounted on the unmanned surface vessel is represented as follows: in, , These are the camera volume coordinate system shaft and Magnification factor in the axial direction, These are the pixel coordinates of the camera's center point, the camera's intrinsic parameters. , and center point pixel coordinates All of these can be obtained using Zhang Zhengyou's calibration method.

4. The unmanned surface vessel (USV) formation control method based on active vision according to claim 3, characterized in that, The servo tracking error is specifically: in, It is the expected position coordinate of the follower in the navigator's hull coordinate system, and the tracking error. These are the desired position coordinates within the follower's hull coordinate system. and These are the positions of the navigator and follower unmanned surface vessels in the geodetic coordinate system. It is the angle that the follower has rotated relative to the Earth's coordinate system. It is the angle that the navigator turns relative to the follower, and satisfies... , It is the angle that the navigator has rotated relative to the Earth's coordinate system. It is the distance between the geometric center of the follower unmanned surface vessel and the origin of the camera's body coordinate system; Tracking error A transformation is performed to avoid using global position information. The transformed result is as follows: in, , , , It is the angle that the gimbal monocular camera has turned relative to the follower unmanned surface vessel, which can be directly read from the encoder of the gimbal camera; The navigator's hull coordinate system To the follower hull coordinate system The rotation matrix, Camera volume coordinate system To the follower hull coordinate system The rotation matrix.

5. The unmanned surface vessel (USV) formation control method based on active vision according to claim 4, characterized in that, The image coordinate error is expressed as follows: When the image coordinate error approaches zero, the target point is located in the center of the camera's field of view.

6. A platooning control system for unmanned surface vessels based on active vision-guided navigation and following methods, characterized in that: The active vision-based unmanned surface vessel (USV) formation control method applied to any one of claims 1-5 includes an USV kinematics model building module, a camera model building module, a tracking error determination module, an image coordinate error determination module, an USV controller design module, and a camera steering controller design module. The unmanned surface vessel (USV) kinematic model construction module is used to construct a USV kinematic model based on a geodetic coordinate system. The USV kinematic model is used to describe the dynamic relationship between the USV's position, heading angle, and velocity. The camera model construction module is used to build a gimbal monocular camera model for the gimbal camera mounted on the unmanned surface vessel based on the imaging principle of the gimbal monocular camera. The gimbal monocular camera model is used to describe the mapping of three-dimensional spatial points projected onto the image plane of the gimbal camera. The tracking error determination module is used to combine the kinematic model of the unmanned surface vessel and the gimbal monocular camera model to define the servo tracking error for the navigator-follower unmanned surface vessel formation system. The servo tracking error represents the deviation between the actual position coordinates and the expected position coordinates of the following vessel. The image coordinate error determination module is used to define the image coordinate error based on the viewing angle constraint of the pan-tilt monocular camera, so as to ensure that the target point remains within the camera's field of view. The unmanned surface vessel controller design module is used to design the motion controller and adaptive parameter update law of the unmanned surface vessel using the Lyapunov synthesis method, and to estimate the navigator motion parameters online. The camera steering controller design module is used to design a gimbal camera steering controller and an adaptive parameter update law using the Lyapunov synthesis method, and to dynamically adjust the monocular orientation of the gimbal.

7. An unmanned surface vessel (USV) equipped with a gimbal camera, comprising a motion controller for the USV and a gimbal camera controller, characterized in that, The controller is constructed using the active vision-based navigation-following unmanned surface vessel formation control method as described in any one of claims 1-5.

8. A computer-readable storage medium storing a program, characterized in that, When the program is executed by the processor, it implements the active vision-based unmanned surface vessel formation control method according to any one of claims 1-5.

Images