An AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization

Abstract

The application discloses an AUV reactive collision avoidance control method based on a maximum posteriori strategy optimization, and relates to the technical field of control; the method comprises the following steps: S1, establishing an AUV model and a forward-looking sonar model; S2, constructing a state, an action and a reward function; S3, simulating and training a reactive collision avoidance strategy; S4, estimating unknown disturbances by using an ESO; S5, compensating the disturbances and obtaining an actual control amount; and S6, controlling the AUV to avoid collisions in real time and sail to a target area, so that the input dimension of a multi-beam sonar perception can be reduced, and the real-time performance, anti-disturbance capability, generalization capability and engineering deployability of the reactive collision avoidance can be improved.

Description

Technical Field

[0001] This invention relates to the field of control technology, and in particular to a reactive collision avoidance control method for AUVs based on maximum a posteriori strategy optimization. Background Technology

[0002] Autonomous underwater vehicles (AUVs) have wide applications in marine resource exploration, underwater inspection, emergency search and rescue, and operations in complex sea areas. To ensure mission safety, AUVs typically need to avoid obstacles and reach target areas in unknown underwater environments based on real-time perception results from onboard sensors.

[0003] Existing AUV collision avoidance methods can be broadly categorized into two types: planned collision avoidance and reactive collision avoidance. Planned collision avoidance typically requires planning a safe path based on environmental information, followed by a trajectory tracking controller controlling the AUV to complete path tracking. This type of method can integrate task constraints and environmental constraints, but it usually involves a large computational load and is quite sensitive to model accuracy and tracking controller performance. Reactive collision avoidance directly outputs control commands based on perception information, offering better real-time performance and making it more suitable for immediate safety decisions in unknown environments.

[0004] With the development of deep reinforcement learning technology, using learning methods to build reactive collision avoidance controllers for AUVs has become an important direction. However, existing solutions still have the following shortcomings: First, the raw detection data of multibeam forward-looking sonar has high dimensionality and redundancy, and directly using it as policy input will increase the inference burden and reduce training efficiency; Second, reinforcement learning is usually trained in a simulation environment, while actual AUVs have model mismatch, ocean current disturbances, and uncertainties in actuators, making it difficult to directly deploy the trained strategy; Third, underactuated AUVs lack direct actuators in the sway and heave directions, and unknown disturbances are more likely to cause performance degradation of the collision avoidance strategy.

[0005] Therefore, how to construct an underactuated AUV control method that can effectively encode multibeam forward-looking sonar information in a low dimension, ensure consistency between simulation training and actual ship deployment, and achieve real-time reactive collision avoidance under unknown disturbances has become a technical problem that urgently needs to be solved in this field. Summary of the Invention

[0006] This invention provides a reactive collision avoidance control method for AUVs based on maximum a posteriori strategy optimization, which solves the technical problems of high sonar input dimension, difficulty in directly deploying simulation training strategies, and insufficient control performance under unknown disturbances.

[0007] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows:

[0008] A reactive collision avoidance control method for AUVs based on maximum a posteriori strategy optimization includes the following steps:

[0009] Step S1: Establish the kinematic and dynamic models of the underactuated autonomous underwater vehicle in five degrees of freedom: sway, roll, heave, pitch, and yaw; establish the installation model and field-of-view model of the multibeam forward-looking sonar.

[0010] Step S2: Construct a reactive collision avoidance state space based on the multibeam forward-looking sonar detection results, target area information, autonomous underwater vehicle speed information and disturbance information; construct an action space based on the autonomous underwater vehicle actuators; and construct a multi-objective reward function that takes into account obstacle avoidance safety, target area arrival and control energy consumption.

[0011] Step S3: Construct a virtual autonomous underwater vehicle simulation environment for offline training. In the simulation environment, target areas, static obstacles and dynamic obstacles are randomly generated. Random perturbation samples are introduced. The reactive collision avoidance control strategy network is trained using the maximum a posteriori policy optimization algorithm.

[0012] Step S4: During the deployment phase of the actual autonomous underwater vehicle, extended state observers are set up on the five degrees of freedom of sway, roll, heave, pitch and yaw to obtain real-time estimates of the unknown total disturbance for each degree of freedom.

[0013] Step S5: Input the unknown total disturbance estimates in the sway and heave directions as disturbance states into the reactive collision avoidance control strategy network, and use the unknown total disturbance estimates in the pitch, yaw, and sway directions as feedforward compensation quantities to correct the output of the reactive collision avoidance control strategy network to obtain the actual control quantities.

[0014] Step S6: Control the autonomous underwater vehicle to perform reactive collision avoidance in an unknown underwater environment according to the actual control quantity, and then drive towards the predetermined target area after obstacle avoidance.

[0015] A further technical solution is that, in step S1, the five-degree-of-freedom kinematic model is represented as:

[0016]

[0017] In the formula, For pitch angle and yaw angle A defined coordinate transformation matrix, , , , , This represents the position of the AUV in the Earth-fixed coordinate system. The pitch angle, Yaw angle , , These are the sway, lateral sway, and heave velocities, respectively. and These are pitch rate and yaw rate, respectively.

[0018] ,

[0019] ,

[0020] ;

[0021] The dynamic model includes dynamic equations for five degrees of freedom: sway, roll, heave, pitch, and yaw. External ocean current disturbances, internal modeling errors, actuator errors, and unmodeled dynamics are uniformly represented as the unknown total disturbance. , This indicates a bounded external disturbance. Indicates internal uncertainty. ;

[0022] The multibeam forward-looking sonar is horizontally mounted on the nose of the AUV. Parameters of the multibeam forward-looking sonar include maximum detection range. Horizontal detection range Vertical detection range and beam angle resolution The multibeam forward-looking sonar is installed at a fixed distance from the AUV itself. .

[0023] A further technical solution is that: in step S2, the step of obtaining the multi-beam forward-looking sonar detection result includes the following steps,

[0024] Based on the number of beams that detect obstacles within the right half of the field of view and the number of beams that detect obstacles within the left half of the field of view Constructing the obstacle state vector The right half of the field of view is the side of the sonar horizontal field of view relative to the negative direction of the AUV's own transverse axis, and the left half of the field of view is the side of the sonar horizontal field of view relative to the positive direction of the AUV's own transverse axis.

[0025] Whether a sonar beam is occupied by an obstacle is determined by whether the beam segment intersects with the geometric region of the obstacle; when the beam endpoint falls inside the obstacle, or when the vertical distance from the center of the obstacle to the beam segment is not greater than the characteristic radius of the obstacle and the projection point is within the range of the beam segment, it is determined that the beam has detected an obstacle.

[0026] A further technical solution is that, in step S2, the obstacle state vector also includes the occupancy ratio of the left and right halves of the field of view, the difference in the number of occupants in the left and right halves of the field of view, the distance to the nearest obstacle in the left and right halves of the field of view, the average obstacle distance in the left and right halves of the field of view, and the obstacle risk score.

[0027] Construct task state When the multibeam forward-looking sonar does not detect an obstacle, When the multibeam forward-looking sonar detects at least one obstacle, =1;

[0028] Construct the target region state vector In the formula, The relative pitch angle of the AUV to the target area. The relative yaw angle of the AUV relative to the target area. For oscillation velocity error; when When =1, It is a zero vector to suppress the interference of the target region state on collision avoidance decision.

[0029] A further technical solution is that step S2 further includes the following steps:

[0030] Constructing the perturbation state vector During the training phase, a perturbation state vector is added to the state space. During the actual deployment phase, the perturbation estimation vector is used. Alternative disturbance state vector input reactive collision avoidance control strategy network;

[0031] Construct to obtain the velocity state vector , task status Target region state vector Obstacle state vector , Disturbance state vector Perturbation estimation vector and velocity state vector The assembly is used to simulate the reactive collision avoidance state during the training phase. Or reactive collision avoidance during the deployment phase .

[0032] A further technical solution is that, in step S2, the multi-objective reward function is: In the formula, As a safety reward for obstacle avoidance, Rewards for reaching the target area To control energy consumption and suppress rewards, and and Greater than Prioritize obstacle avoidance safety and target arrival.

[0033] A further technical solution is as follows: In step S3, the direct control inputs for sway, pitch, and yaw directions are retained, while random total disturbances are introduced in the sway and heave directions; target area parameters, obstacle position parameters, obstacle scale parameters, dynamic obstacle velocity parameters, and disturbance parameters are randomly sampled before the start of each training round; the training process includes a reactive collision avoidance strategy network, a state-action value network, a corresponding target network, and a sample replay buffer, wherein the sample replay buffer stores states, actions, next states, and reward samples. The network parameters are iteratively optimized through Q-value-based policy improvement and a constrained policy update mechanism.

[0034] A further technical solution is that, in step S4, an observation device for the expansion state is constructed for the sway direction. An observation device for the expansion state is constructed based on the direction of heave. The estimated values ​​of the total disturbance in the unknown sway direction were obtained respectively. Estimated total disturbance with unknown heave direction ; construct extended state observers for the sway, pitch, and yaw directions respectively. , and The corresponding estimates of the total disturbance in the unknown sway direction are obtained respectively. Estimated total disturbance value for unknown pitch direction Estimated total disturbance with unknown yaw direction .

[0035] A further technical solution is that, in step S5, the actual control quantity is: , The virtual control quantity output for the reactive collision avoidance state during the deployment phase. The feedforward compensation vector is composed of the estimates of sway, pitch, and yaw direction disturbances. The virtual longitudinal thrust output by the policy network. For virtual pitch control torque, This is the virtual yaw control torque.

[0036] The beneficial effects of adopting the above technical solution are as follows:

[0037] A reactive collision avoidance control method for AUVs based on maximum a posteriori strategy optimization includes the following steps: S1 establishing an AUV model and a forward-looking sonar model; S2 constructing state, action, and reward functions; S3 simulating and training a reactive collision avoidance strategy; S4 estimating unknown disturbances using ESO; S5 compensating for disturbances and obtaining actual control quantities; and S6 controlling the AUV to avoid collisions in real time and drive towards the target area. This method can reduce the dimensionality of multibeam sonar sensing input and improve the real-time performance, disturbance rejection capability, generalization ability, and engineering deployability of reactive collision avoidance. Attached Figure Description

[0038] Figure 1 This is a geometric diagram of the detection beams of the multi-beam forward-looking sonar in this invention;

[0039] Figure 2 This is a flowchart of the method of the present invention;

[0040] Figure 3 This is a diagram of the overall training-deployment structure of the present invention, which is based on multi-beam forward-looking sonar coding, maximum a posteriori strategy optimization training, and extended state observer deployment compensation. Detailed Implementation

[0041] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit this application or its application or use. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0042] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, this application may also be implemented in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the spirit of this application. Therefore, this application is not limited to the specific embodiments disclosed below.

[0043] Example 1:

[0044] like Figure 2 As shown, this invention discloses a reactive collision avoidance control method for AUVs based on maximum a posteriori strategy optimization, comprising the following steps:

[0045] Step S1: Establish the AUV model and the forward-looking sonar model.

[0046] A five-DOF kinematic and dynamic model of an underactuated AUV was established, along with an installation model and field-of-view model for a multibeam forward-looking sonar. The AUV has three types of actuators: a longitudinal thruster, a horizontal rudder, and a vertical rudder. Control inputs can be directly applied in the sway, pitch, and yaw directions; however, direct drive inputs are not available in the sway and heave directions. Details are as follows.

[0047] like Figure 1 As shown, an underactuated AUV equipped with a multibeam forward-looking sonar in the nose and employing a propeller plus horizontal and vertical rudder layout is selected as the control object. Since the roll motion of the AUV is self-stabilizing or has little impact on collision avoidance control, this embodiment ignores the roll degree of freedom and establishes a five-degree-of-freedom model of sway, heave, pitch, and yaw.

[0048] The five-degree-of-freedom kinematic model is represented as follows:

[0049]

[0050]

[0051] In the formula, For pitch angle and yaw angle A given coordinate transformation matrix can be written as:

[0052] ,

[0053]

[0054]

[0055] The five-degree-of-freedom dynamic model considering external disturbances and internal uncertainties is as follows:

[0056]

[0057] In the formula, , , , and Add mass and inertia-related parameters to the AUV. For hydrodynamic damping and friction terms, This indicates a bounded external disturbance. Indicates internal uncertainty. To uniformly handle external disturbances and internal uncertainties, the total unknown disturbance is defined as:

[0058] ,

[0059] As can be seen from the above model, the AUV only has direct control inputs in the sway, pitch, and yaw directions. , and Since there is no direct control input for the sway and heave directions, it belongs to an underactuated system.

[0060] like Figure 1 As shown, the multibeam forward-looking sonar is horizontally mounted at the front of the AUV. The sonar system is denoted as... AUV is denoted as Sonar installation point The coordinates in the AUV system are ( The position of the sonar in the Earth-fixed coordinate system is:

[0061]

[0062] The farthest detection point of the nth sonar beam within the horizontal field of view The coordinates are:

[0063]

[0064]

[0065] To reduce the dimensionality of raw detection data from multi-beam forward-looking sonar, this embodiment does not directly input all raw echo distances and angles into the strategy network. Instead, it divides the horizontal field of view into left and right halves, and separately counts the number of beams occupied by obstacles in the right and left halves. The right half of the field of view corresponds to the horizontal angular range of the sonar. The left half of the field of view corresponds to the horizontal angle range of the sonar. .

[0066] Step S2: Construct the state, action, and reward function.

[0067] A reactive collision avoidance state space, action space, and multi-objective reward function are constructed. The obstacle state vector describes the difference in obstacle distribution on the left and right sides; the task state distinguishes between the two navigation objectives: heading towards the target area and performing obstacle avoidance; the target area state vector guides the AUV to approach the target area and maintain the desired sway speed in an obstacle-free scenario; the disturbance state vector represents unknown disturbances in the sway and heave directions; and the velocity state vector reflects the AUV's current velocity information. Details are as follows.

[0068] definition This represents the number of beams that detected obstacles within the right half of the field of view. If the number of beams detecting obstacles in the left half of the field of view is given, then the obstacle state vector is:

[0069]

[0070] when > When the obstacle on the right side of the AUV is heavily obstructed, the policy network tends to output control to avoid it to the left; when <= When the left-side obstacle is more heavily occupied or the left and right sides are equally occupied, the policy network tends to output control to avoid to the right or maintain a safe course.

[0071] In the simulation environment, obstacles can be represented by spherical obstacles. Let the center of the i-th spherical obstacle be denoted as . , radius is For any beamline segment, it can be determined whether the beam is occupied by an obstacle as follows: if the beam endpoint is located inside the obstacle, or the vertical distance HE from the center of the obstacle to the beamline segment is not greater than 1 / 3. If the projection of the obstacle's center onto the line segment lies between the two ends of the beam segment, then the beam is determined to have detected an obstacle.

[0072]

[0073] or:

[0074] In the formula, A1 and A2 are the inner products used to determine whether the projection of the obstacle's center lies within the beam segment. This determination method avoids directly inputting large-scale raw sonar echoes into the neural network while preserving the key spatial distribution information required for reactive collision avoidance.

[0075] In an optional implementation, the obstacle state vector can be further extended to include low-dimensional statistical features such as the proportion of left and right halves of the field of view occupied, the difference in the number of left and right halves occupied, the distance to the nearest obstacle, the average obstacle distance, and the hazard score, for example:

[0076]

[0077] In the formula, and These represent the total number of beams in the right and left halves of the field of view, respectively. and These represent the distances to the nearest obstacles in the left and right halves of the field of view, respectively.

[0078] In reactive collision avoidance missions, AUVs have two mutually exclusive objectives: when the multibeam forward-looking sonar does not detect an obstacle, the AUV heads towards a known target area and tracks the desired sway velocity; when the multibeam forward-looking sonar detects an obstacle, the AUV performs reactive collision avoidance to ensure navigational safety. To describe the current navigation objective, the mission state is defined as follows:

[0079]

[0080] Let the coordinates of the center of the target area in the Earth-fixed coordinate system be... The position of the AUV relative to the target area is represented in the AUV body as follows:

[0081]

[0082] According to the line-of-sight (LOS) method, the relative pitch angle of the AUV relative to the target area is... and relative yaw angle for:

[0083]

[0084] Combined with expected oscillation speed With the current oscillation speed Construct the target region state vector:

[0085]

[0086] when When =1, The zero vector is used to avoid interference from target area information in collision avoidance decision-making; when When =0, Used to guide the AUV to the target area and maintain the desired oscillation speed.

[0087] To compensate for the disturbances caused by the lack of direct actuators in the sway and heave directions, a perturbation state vector is added to the state space during the training phase:

[0088]

[0089] During the actual deployment phase, the perturbation estimation vector is obtained from the extended state observer:

[0090]

[0091] Simultaneously, construct the velocity state vector:

[0092]

[0093] Therefore, the complete state vector during the training phase is:

[0094]

[0095] The complete state vector during the deployment phase is:

[0096]

[0097] The motion space is directly composed of the control outputs of the three controllable execution directions of the virtual AUV, represented as:

[0098]

[0099] In the formula, The virtual longitudinal thrust output by the policy network. For virtual pitch control torque, This is the virtual yaw control torque.

[0100] To enable the AUV to simultaneously learn obstacle avoidance, reach the target area, and reduce control energy consumption, a multi-objective reward function is constructed:

[0101]

[0102] In the formula, the obstacle avoidance safety reward is:

[0103]

[0104] The reward for reaching the target area is:

[0105]

[0106] The reward for controlling energy consumption and suppressing it is:

[0107]

[0108] The weighting coefficients satisfy and Preferred and Significantly greater than To ensure obstacle avoidance safety and target arrival priority, while minimizing the output amplitude of the actuator.

[0109] Step S3: Simulate and train reactive collision avoidance strategies.

[0110] A virtual AUV simulation environment for offline training is established. Target area locations, static obstacles, and / or dynamic obstacles are randomly generated within the simulation environment, and random total perturbations are introduced in both the sway and heave directions. A maximum a posteriori policy optimization algorithm is employed to train a reactive collision avoidance control policy offline within a learning framework consisting of a sample replay buffer, a policy network, a state-action value network, and a target network. Details are as follows.

[0111] like Figure 3 As shown, a virtual AUV simulation environment is constructed during the offline training phase. The virtual AUV uses the same kinematic model as the actual AUV, but the direct control inputs in the sway, pitch, and yaw directions are retained in the dynamic model, and random total disturbances are introduced in the sway and heave directions.

[0112]

[0113] Before each training round, the target region location, obstacle location, obstacle scale, dynamic obstacle motion parameters, and perturbation parameters are randomly sampled. The target region can be represented as:

[0114]

[0115] The center and scale of the i-th obstacle can be represented as:

[0116]

[0117] In the formula, t is a time variable. When the obstacle parameters are independent of t, it is a static obstacle; when the obstacle position parameters change with t, it is a dynamic obstacle. The union of multiple spherical obstacles can form scattered obstacles, concave obstacles, or irregular composite obstacles.

[0118] The total random disturbance in the sway and heave directions can be generated by a superposition of multiple frequency sinusoids:

[0119]

[0120] In the formula, , and For random variables, , and It can be a constant or a preset parameter.

[0121] The maximum a posteriori policy optimization training process includes a reactive collision avoidance policy network. State-Action Value Network Corresponding target network and and sample replay buffer After each interaction with the simulation environment, the sample will be... Store in sample replay buffer .

[0122] When the number of samples meets the training requirements, a batch of state transition samples is sampled from the sample replay buffer. The Q-value is estimated based on the state-action value network, and the policy network parameters are optimized through policy improvement based on the Q-value and a constrained policy update mechanism. After training iterations until the cumulative reward converges or the preset number of training steps is reached, a reactive collision avoidance control policy network is obtained.

[0123] Step S4: Use ESO to estimate unknown disturbances.

[0124] During the actual deployment phase, five extended state observers are constructed to estimate the unknown total disturbance in the sway, heave, pitch, and yaw directions, respectively. The disturbance estimates in the sway and heave directions form the disturbance state estimation vector and are input into the policy network; the disturbance estimates in the sway, pitch, and yaw directions are used for feedforward compensation of the policy output. Details are as follows.

[0125] The policy network can be one or more of the following: fully connected neural network, recurrent neural network, convolutional neural network, attention network, or Transformer network; the state-action value network can adopt the same or different network structure as the policy network.

[0126] like Figure 3 As shown, during actual deployment, real AUVs are affected by external ocean currents, model mismatch, actuator errors, and unmodeled dynamics. Therefore, it is necessary to estimate the total unknown disturbance for each degree of freedom. An extended state observer is constructed for the sway and heave directions. and :

[0127]

[0128]

[0129] Depend on and Obtained and Composition of perturbation estimation vector As input to the policy network, it is used to implement feedback compensation for perturbations of degrees of freedom that cannot be directly driven.

[0130] Construct an extended state observer for the sway, pitch, and yaw directions. , and :

[0131]

[0132]

[0133]

[0134] The nonlinear function in the above observer can be defined as:

[0135]

[0136] In the formula, and For observer gain, By adjusting the observer gain, a trade-off can be struck between convergence speed and noise sensitivity.

[0137] Step S5: Disturbance compensation and obtaining the actual control quantity.

[0138] The disturbance estimate obtained in step S4, along with the velocity measured by the sensors, the target area, and the sonar obstacle status, are input into the trained reactive collision avoidance control strategy network to obtain the virtual control output; subsequently, the actual control quantity is obtained using the feedforward compensation relationship. Details are as follows.

[0139] The policy network is based on the deployment phase status. Output virtual control quantity :

[0140]

[0141] In the formula, The virtual longitudinal thrust output by the policy network. For virtual pitch control torque, This is the virtual yaw control torque.

[0142] like Figure 3 As shown, the estimates of sway, pitch, and yaw direction disturbances are combined to form a feedforward compensation vector:

[0143]

[0144] The actual control quantity is:

[0145]

[0146]

[0147] During engineering deployment, the actual control quantity can be further input to the thruster, horizontal rudder and vertical rudder actuators after amplitude limiting, rate of change limiting and low-pass filtering, in order to avoid actuator saturation and sudden changes in control commands.

[0148] When an AUV actually performs a mission, the obstacle status is first generated by a multibeam forward-looking sonar. The target area status is generated by the navigation system or mission system. The velocity state is obtained by the inertial and velocity measurement units. The perturbation estimation vector is generated by the extended state observer. and feedforward compensation vector .

[0149] Then, Input the trained reactive collision avoidance strategy network to obtain the virtual control output. Reuse It obtains the actual control quantity and drives the thrusters and control surface actuators to work.

[0150] Step S6: Control the AUV to avoid collisions in real time and drive towards the target area.

[0151] The AUV is driven to perform real-time reactive collision avoidance based on the actual control input. When the sonar does not detect an obstacle, the AUV proceeds towards the target area based on the target area's condition and tracks the desired sway speed; when the sonar detects an obstacle, the AUV quickly outputs avoidance control input based on the obstacle and disturbance conditions, and continues towards the target area after passing through the obstacle area. Details are as follows.

[0152] If there are no obstacles in the sonar field of view, then The AUV navigates to the target area based on its condition and tracks the desired sway velocity; if obstacles exist within the sonar field of view, then... 1. The target area status is set to zero. The AUV quickly implements evasive maneuvers based on the degree of obstacle occupancy in the left and right halves of the field of view, the current speed, and the disturbance estimate. After passing the obstacle area, it resumes navigation in the target area.

[0153] In an optional implementation, when sonar data is abnormal, policy network output exceeds the safe range, extended state observer estimate diverges, or the distance between the AUV and the obstacle is below the safe threshold, a preset safety control law is activated, such as deceleration, hovering, buoyancy, or avoidance along a preset safe course.

[0154] In one optional implementation, the multibeam forward-looking sonar detection results may first undergo time-moving average, median filtering, Kalman filtering, consecutive frame confirmation, or confidence screening to reduce the impact of underwater noise, false alarms, and missed detections on obstacle status. The impact.

[0155] In another alternative implementation, the training environment may further employ a domain randomization method to randomize the AUV mass, added mass, hydrodynamic damping, thruster gain, rudder efficiency parameters, actuator delay, sensor noise, sonar miss rate, sonar false alarm rate, ocean current direction, and ocean current intensity, in order to improve the robustness of the policy network when migrating from the simulation environment to the actual underwater environment.

[0156] In another alternative implementation, the extended state observer can be replaced by a linear extended state observer, a nonlinear extended state observer, an adaptive extended state observer, a finite-time extended state observer, a high-gain extended state observer, or a sliding mode perturbation observer. As long as the unknown total perturbation on the corresponding degree of freedom can be estimated, it is an equivalent substitution in this application.

[0157] In another alternative embodiment, this application can also be implemented as an underactuated AUV reactive collision avoidance control system, which includes: a multibeam forward-looking sonar sensing module, a sonar low-dimensional encoding module, a mission state generation module, a target area state generation module, a strategy network module, an extended state observation module, a control compensation module, and an actuator drive module.

[0158] In another alternative embodiment, this application may also be implemented as an electronic device or embedded controller, including a processor, a memory, and a program stored in the memory and executable by the processor, wherein the program, when executed, implements any of the above method steps; this application may also be implemented as a computer-readable storage medium having a program stored thereon for implementing the above methods.

[0159] The beneficial effects of this application are as follows: First, by encoding the left and right half-field-of-view beams of the multi-beam forward-looking sonar detection information, the strategy input dimension and online inference burden are significantly reduced; Second, through the maximum a posteriori policy optimization algorithm, the AUV can learn a control strategy that balances obstacle avoidance safety, target arrival, and energy consumption suppression in various random obstacle and random disturbance environments; Third, by introducing five extended state observers in the actual deployment stage, disturbances on degrees of freedom that cannot be directly driven are used as feedback compensation inputs, and disturbances on degrees of freedom that can be directly driven are used as feedforward compensation quantities, thereby improving the anti-disturbance capability and engineering deployability under unknown disturbances.

[0160] Therefore, this application can reduce the sensing input dimension of multibeam sonar and improve the real-time performance, anti-interference capability, generalization capability and engineering deployability of reactive collision avoidance.

[0161] Example 2:

[0162] like Figure 2 As shown, this invention discloses a reactive collision avoidance control method for AUVs based on maximum a posteriori strategy optimization, comprising the following steps:

[0163] Step S1: Establish the AUV model and the forward-looking sonar model.

[0164] Establish kinematic and dynamic models of underactuated AUVs in five degrees of freedom: sway, roll, heave, pitch, and yaw. Also establish installation and field-of-view models for multibeam forward-looking sonars.

[0165] Step S2: Construct the state, action, and reward function.

[0166] Based on the detection results of the multibeam forward-looking sonar, target area information, AUV speed information and disturbance information, a reactive collision avoidance state space is constructed, an action space is constructed based on the AUV actuator, and a multi-objective reward function that takes into account obstacle avoidance safety, target area arrival and control energy consumption is constructed.

[0167] Step S3: Simulate and train reactive collision avoidance strategies.

[0168] A virtual AUV simulation environment for offline training is constructed. In the simulation environment, a target area, static obstacles and / or dynamic obstacles are randomly generated, and random perturbation samples are introduced. The reactive collision avoidance control strategy network is trained using the maximum a posteriori policy optimization algorithm.

[0169] Step S4: Use ESO to estimate unknown disturbances.

[0170] During the actual AUV deployment phase, extended state observers are set up in the five degrees of freedom of sway, roll, heave, pitch and yaw to obtain real-time estimates of the unknown total disturbance in each degree of freedom.

[0171] Step S5: Disturbance compensation and obtaining the actual control quantity.

[0172] The unknown total disturbance estimates in the sway and heave directions are input as disturbance states into the reactive collision avoidance control strategy network. The unknown total disturbance estimates in the pitch, yaw, and swell directions are used as feedforward compensation values ​​to correct the output of the control strategy network, thereby obtaining the actual control values.

[0173] Step S6: Control the AUV to avoid collisions in real time and drive towards the target area.

[0174] The AUV is controlled to perform reactive collision avoidance in an unknown underwater environment based on the actual control quantity, and then proceeds to the predetermined target area after obstacle avoidance.

[0175] The technical solution of Embodiment 2 can reduce the sensing input dimension of multi-beam sonar, improve the real-time performance, anti-interference capability, generalization capability and engineering deployability of reactive collision avoidance, which will not be elaborated further.

[0176] Based on Embodiment 2, in step S1, the five-degree-of-freedom kinematic model is further represented as follows: In the formula

[0177]

[0178]

[0179] , , This represents the position of the AUV in the Earth-fixed coordinate system. The pitch angle, Yaw angle , , These are the sway, lateral sway, and heave velocities, respectively. and These are pitch rate and yaw rate, respectively.

[0180] ,

[0181] In the formula

[0182] ,

[0183] .

[0184] Based on Embodiment 2, in step S1, the dynamic model includes dynamic equations for five degrees of freedom: sway, roll, heave, pitch, and yaw. Furthermore, one or more of the following—external ocean current disturbances, internal modeling errors, actuator errors, and unmodeled dynamics—are uniformly represented as the unknown total disturbance. .

[0185] Based on Embodiment 2, in step S1, the multibeam forward-looking sonar is horizontally mounted on the head of the AUV, and the parameters of the multibeam forward-looking sonar include at least the maximum detection range. Horizontal detection range Vertical detection range and beam angle resolution The multibeam forward-looking sonar is installed at a fixed distance from the AUV system. .

[0186] Based on Embodiment 2, in step S2, the number of beams detecting obstacles within the right half of the field of view is further... and the number of beams that detect obstacles within the left half of the field of view Constructing the obstacle state vector The right half of the field of view is the side of the sonar horizontal field of view relative to the negative direction of the AUV's own transverse axis, and the left half of the field of view is the side of the sonar horizontal field of view relative to the positive direction of the AUV's own transverse axis.

[0187] Further based on Embodiment 2, whether any sonar beam is occupied by an obstacle is determined based on whether the beam segment intersects with the geometric region of the obstacle; when the beam endpoint falls inside the obstacle, or when the vertical distance from the center of the obstacle to the beam segment is not greater than the characteristic radius of the obstacle and the projection point is within the range of the beam segment, it is determined that the beam has detected an obstacle.

[0188] Based on Embodiment 2, the obstacle state vector may further include one or more low-dimensional statistical features among the following: the proportion of the left and right halves of the field of view occupied, the difference in the number of items occupied by the left and right halves of the field of view, the distance of the nearest obstacle in the left and right halves of the field of view, the average distance of the obstacle in the left and right halves of the field of view, and the obstacle risk score.

[0189] Based on Embodiment 2, in step S2, the task state is constructed. When the multibeam forward-looking sonar does not detect an obstacle, When the multibeam forward-looking sonar detects at least one obstacle, =1.

[0190] Based on Embodiment 2, in step S2, a target region state vector is constructed. In the formula, The relative pitch angle of the AUV to the target area. The relative yaw angle of the AUV relative to the target area. For oscillation velocity error; when When =1, It is a zero vector to suppress the interference of the target region state on collision avoidance decision.

[0191] Furthermore, based on Embodiment 2,

[0192] ,

[0193] ,

[0194] In the formula, The coordinates are the center coordinates of the target area.

[0195] Based on Embodiment 2, in step S2, a disturbance state vector is constructed. In the actual deployment phase, a perturbation estimation vector is used. Replace the disturbance state vector input control strategy network.

[0196] A further aspect of Embodiment 2 is the construction of a velocity state vector. and the task status Target region state vector Obstacle state vector , Disturbance state vector or perturbation estimation vector and velocity state vector Connect to a reactive collision avoidance state or .

[0197] Based on Embodiment 2, in step S2, the action space consists of a longitudinal thruster virtual thrust command, a pitch virtual control torque command, and a yaw virtual control torque command, represented as follows: .

[0198] Based on Embodiment 2, the multi-objective reward function is further described as follows: In the formula, As a safety reward for obstacle avoidance, Rewards for reaching the target area To control energy consumption and suppress rewards, and and Greater than Prioritize obstacle avoidance safety and target arrival.

[0199] Based on Embodiment 2, in step S3, the virtual AUV simulation environment adopts a five-degree-of-freedom underactuated AUV model, retains the direct control inputs in the sway, pitch and yaw directions, and introduces random total disturbances in the sway and heave directions; the target area parameters, obstacle position parameters, obstacle scale parameters, dynamic obstacle velocity parameters and disturbance parameters are randomly sampled before the start of each training round.

[0200] Based on Embodiment 2, in step S3, the training process includes a policy network, a state-action value network, a corresponding target network, and a sample replay buffer; the sample replay buffer stores states, actions, next states, and reward samples. The network parameters are iteratively optimized through Q-value-based policy improvement and a constrained policy update mechanism.

[0201] Based on Embodiment 2, in step S4, an expansion state observer is constructed for the sway direction. Construct an expansion state observer for the heave direction. The estimated values ​​of the total disturbance in the unknown sway direction were obtained respectively. Estimated total disturbance with unknown heave direction .

[0202] Based on Embodiment 2, in step S4, an extended state observer is constructed for the sway direction, pitch direction, and yaw direction, respectively. , and The unknown total disturbance estimates were obtained respectively. , and The actual control quantity in step S5 is .

[0203] Based on Embodiment 2, the extended state observer is one or more of the following: linear extended state observer, nonlinear extended state observer, adaptive extended state observer, finite-time extended state observer, high-gain extended state observer, or sliding mode perturbation observer.

[0204] Based on Embodiment 2, the obstacle further includes a single spherical obstacle, an ellipsoidal obstacle, a cylindrical obstacle, or a composite obstacle formed by the union of multiple basic geometric shapes; the composite obstacle is used to characterize scattered obstacles, concave obstacles, irregular static obstacles, and / or dynamic obstacles.

Claims

1. A reactive collision avoidance control method for AUVs based on maximum a posteriori strategy optimization, characterized in that: Includes the following steps, Step S1: Obtain the kinematic and dynamic models of the underactuated autonomous underwater vehicle in five degrees of freedom: sway, roll, heave, pitch, and yaw; obtain the installation model and field-of-view model of the multibeam forward-looking sonar. Step S2: Obtain the reactive collision avoidance state space, action space, and multi-objective reward function; Step S3: Obtain a virtual autonomous underwater vehicle simulation environment for offline training. In the simulation environment, target areas, static obstacles and dynamic obstacles are randomly generated. Random perturbation samples are introduced, and the reactive collision avoidance control strategy network is trained using the maximum a posteriori policy optimization algorithm. Step S4: During the deployment phase of the actual autonomous underwater vehicle, extended state observers are set up on the five degrees of freedom of sway, roll, heave, pitch and yaw to obtain real-time estimates of the unknown total disturbance for each degree of freedom. Step S5: Input the unknown total disturbance estimates in the sway and heave directions as disturbance states into the reactive collision avoidance control strategy network, and use the unknown total disturbance estimates in the pitch, yaw, and sway directions as feedforward compensation quantities to correct the output of the reactive collision avoidance control strategy network to obtain the actual control quantity. Step S6: Control the autonomous underwater vehicle to perform reactive collision avoidance in an unknown underwater environment according to the actual control quantity, and then drive towards the predetermined target area after obstacle avoidance.

2. The AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization according to claim 1, characterized in that: In step S1 The five-degree-of-freedom kinematic model is represented as follows: In the formula, For pitch angle and yaw angle A defined coordinate transformation matrix, , , , , This represents the position of the autonomous underwater vehicle in a geofixed coordinate system. The pitch angle, Yaw angle , , These are the sway, lateral sway, and heave velocities, respectively. and These are pitch rate and yaw rate, respectively. ， ， ； The dynamic model includes dynamic equations for five degrees of freedom: sway, roll, heave, pitch, and yaw. External ocean current disturbances, internal modeling errors, actuator errors, and unmodeled dynamics are uniformly represented as the unknown total disturbance. , This indicates a bounded external disturbance. Indicates internal uncertainty. ; The multibeam forward-looking sonar is horizontally mounted on the bow of the autonomous underwater vehicle. Parameters of the multibeam forward-looking sonar include maximum detection range. Horizontal detection range Vertical detection range and beam angle resolution The multibeam forward-looking sonar is installed at a fixed distance from the main body of the autonomous underwater vehicle. .

3. The AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization according to claim 1, characterized in that: In step S2, a reactive collision avoidance state space is constructed based on the multibeam forward-looking sonar detection results, target area information, autonomous underwater vehicle speed information, and disturbance information. An action space is constructed based on the autonomous underwater vehicle actuator. The multi-objective reward function is a multi-objective reward function that takes into account obstacle avoidance safety, target area arrival, and control energy consumption.

4. The AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization according to claim 3, characterized in that: Step S2, the step of obtaining the multibeam forward-looking sonar detection result, includes the following steps. Based on the number of beams that detect obstacles within the right half of the field of view and the number of beams that detect obstacles within the left half of the field of view Constructing the obstacle state vector The right half of the field of view is the side of the sonar horizontal field of view relative to the negative direction of the lateral axis of the autonomous underwater vehicle system, and the left half of the field of view is the side of the sonar horizontal field of view relative to the positive direction of the lateral axis of the autonomous underwater vehicle system. Whether a sonar beam is occupied by an obstacle is determined by whether the beam segment intersects with the geometric region of the obstacle; when the beam endpoint falls inside the obstacle, or when the vertical distance from the center of the obstacle to the beam segment is not greater than the characteristic radius of the obstacle and the projection point is within the range of the beam segment, it is determined that the beam has detected an obstacle.

5. The AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization according to claim 4, characterized in that: In step S2, the obstacle state vector also includes the occupancy ratio of the left and right halves of the field of view, the difference in the number of occupants in the left and right halves of the field of view, the distance to the nearest obstacle in the left and right halves of the field of view, the average distance to the obstacle in the left and right halves of the field of view, and the obstacle risk score. Construct task state When the multibeam forward-looking sonar does not detect an obstacle, ; When the multibeam forward-looking sonar detects at least one obstacle =1; Construct the target region state vector In the formula, The relative pitch angle of the autonomous underwater vehicle relative to the target area. The relative yaw angle of the autonomous underwater vehicle relative to the target area. For oscillation velocity error; when When =1, It is a zero vector to suppress the interference of the target region state on collision avoidance decision.

6. The AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization according to claim 5, characterized in that: Step S2 also includes the following steps. Constructing the perturbation state vector During the training phase, a perturbation state vector is added to the state space. During the actual deployment phase, the perturbation estimation vector is used. Alternative disturbance state vector input reactive collision avoidance control strategy network; Construct to obtain the velocity state vector , to change the task status Target region state vector Obstacle state vector , Disturbance state vector Perturbation estimation vector and velocity state vector The assembly is used to simulate the reactive collision avoidance state during the training phase. Or reactive collision avoidance during the deployment phase .

7. The AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization according to claim 6, characterized in that: In step S2, the multi-objective reward function is: In the formula, As a safety reward for obstacle avoidance, Rewards for reaching the target area To control energy consumption and suppress rewards, and and Greater than Prioritize obstacle avoidance safety and target arrival.

8. The AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization according to claim 1, characterized in that: In step S3, the direct control inputs for sway, pitch, and yaw directions are retained, while random total disturbances are introduced in the sway and heave directions; target area parameters, obstacle position parameters, obstacle scale parameters, dynamic obstacle velocity parameters, and disturbance parameters are randomly sampled before the start of each training round. The training process includes a reactive collision avoidance policy network, a state-action value network, a target correspondence network, and a sample replay buffer. The sample replay buffer stores states, actions, next states, and reward samples. The network parameters are iteratively optimized through Q-value-based policy improvement and a constrained policy update mechanism.

9. The AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization according to claim 1, characterized in that: In step S4, an expansion state observer is constructed for the sway direction. An observation device for the expansion state is constructed based on the direction of heave. The estimated values ​​of the total disturbance in the unknown sway direction were obtained respectively. Estimated total disturbance with unknown heave direction ; construct extended state observers for the sway, pitch, and yaw directions respectively. , and The corresponding estimates of the total disturbance in the unknown sway direction are obtained respectively. Estimated total disturbance value for unknown pitch direction Estimated total disturbance with unknown yaw direction .

10. The AUV reactive collision avoidance control method based on maximum a posteriori strategy optimization according to claim 9, characterized in that: In step S5, the actual control quantity is , The virtual control quantity output for the reactive collision avoidance state during the deployment phase. The feedforward compensation vector is composed of the estimates of sway, pitch, and yaw direction disturbances. The virtual longitudinal thrust output by the policy network. For virtual pitch control torque, This is the virtual yaw control torque.

Images