Automatic obstacle avoidance strategy training method for underwater vehicles based on deep reinforcement learning

Abstract

A method for training an underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning, firstly constructing a physical and dynamic model, a world and a body coordinate system of the underwater vehicle, then constructing an algorithm environment based on Gym, and defining its state space, action space, reward function and end condition, then updating the parameters of the position and state space using quaternion transformation, then training using a deep reinforcement learning algorithm based on the algorithm environment to obtain an obstacle avoidance neural network model, and finally implementing an obstacle avoidance strategy based on the model; in the application, by constructing a physical and dynamic model of the underwater vehicle to accurately reflect the complex environment, and combining two coordinate systems to intuitively describe the position and posture of the vehicle, then using deep reinforcement learning to learn complex obstacle avoidance strategies, so that it can be extended to task scenarios such as automatic path planning in three-dimensional space, so as to autonomously formulate navigation and obstacle avoidance strategies in unknown environments. Therefore, the scope of application of the present invention is relatively wide.

Description

Technical Field

[0001] The present invention relates to an obstacle avoidance strategy training method, and belongs to the field of underwater equipment intelligence, and in particular to an underwater vehicle automatic obstacle avoidance strategy training method based on deep reinforcement learning. Background Art

[0002] Underwater vehicles have the advantages of strong deep-sea exploration capabilities, high-precision data collection, high safety of underwater operations, no casualties, strong concealment, low operating costs, no need for frequent human operation, and the ability to operate for a long time and over long distances. They have become an important research and development direction in the fields of marine science and marine military.

[0003] The movement of underwater vehicles is characterized by high flexibility and complex motion equations. At the same time, because they operate in the seabed environment, they are affected by various environmental factors such as gravity and buoyancy, and sometimes they are disturbed by obstacles on the navigation path, which poses a great challenge for underwater vehicles to automatically and safely reach the target location. At present, the research on underwater vehicles in the existing technology is mostly focused on obstacle avoidance on a two-dimensional plane, and there is less research on obstacle avoidance of six-degree-of-freedom underwater vehicles in three-dimensional space. Therefore, obstacle avoidance on a three-dimensional plane puts forward higher scene requirements for underwater vehicles, which limits its application scope to a certain extent and has a narrow scope of application. Therefore, a method with a wider scope of application is urgently needed to solve the above-mentioned defects in the existing technology. Summary of the invention

[0004] The purpose of the present invention is to overcome the above-mentioned defects and problems existing in the prior art and to provide a method for training underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning with a wide range of applications.

[0005] To achieve the above objectives, the technical solution of the present invention is: a method for training an underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning, the method comprising:

[0006] S1. Construct the physical model and dynamic model of the underwater vehicle, as well as the world coordinate system fixed on the horizontal plane when the underwater vehicle is running. and the body coordinate system fixed to the center of gravity of the underwater vehicle ;

[0007] S2. Construct an underwater vehicle artificial intelligence algorithm environment based on Gym based on the physical model of the underwater vehicle and its dynamic model, the world coordinate system, and the body coordinate system, and define the state space, action space, reward function, and end condition of the artificial intelligence algorithm environment;

[0008] S3, using quaternion transformation to update the position of the underwater vehicle in the dynamic model in the world coordinate system after the underwater vehicle performs the action, and at the same time update the parameters in the state space, the parameters including the state of the underwater vehicle itself, the distance between the underwater vehicle and the target point, the angle between the underwater vehicle and the target point, and the angle and distance between the underwater vehicle and the obstacle;

[0009] S4. Based on the underwater vehicle artificial intelligence algorithm environment and updated parameters, the updated dynamics model is trained using a deep reinforcement learning algorithm to obtain an obstacle avoidance neural network model;

[0010] S5. Based on the obstacle avoidance neural network model, the automatic obstacle avoidance strategy of the underwater vehicle in an unknown environment is implemented by setting target points and obstacles.

[0011] The step S2 specifically includes:

[0012] S21, with the origin in the world coordinate system , respectively along Axis and The positive and negative directions of the axis The negative distances of the axis construct the motion space of the underwater vehicle;

[0013] S22, randomly generate a target point and several obstacles in the motion space, and point the bow of the underwater vehicle to the position of the underwater vehicle's center of gravity in the world coordinate system. Axis positive direction, Plane and Parallel is the initial attitude of the underwater vehicle;

[0014] S23, constructing the state space of the underwater vehicle based on the speed and angular velocity in the three axis directions of the underwater vehicle, and the direction, distance and relative position between the target point and the obstacle; constructing the action space of the underwater vehicle based on the actions of the bow rudder, the stern rudder and the rudder;

[0015] S24, constructing a reward function; the reward function includes: a reward for approaching the target point, a reward for stability and safety, and a reward for avoiding obstacles;

[0016] S25, the termination conditions are reaching the target point, exceeding the allowed depth range, exceeding the allowed number of steps, and hitting an obstacle.

[0017] The state space includes the state of the underwater vehicle itself, the relative position state between the underwater vehicle and the target point, and the relative position state between the underwater vehicle and the obstacle;

[0018] The underwater vehicle's own state is expressed as follows:

[0019] ;

[0020] in: , , They represent the lower edge of the underwater vehicle in the body coordinate system. , , The speeds in the three axes, , , They represent the underwater vehicle's rotation in the body coordinate system. , , The angular velocity of the three axes, is the vector representation of the underwater vehicle's bow pointing vector in the world coordinate system, are the rudder angles of the three rudders of the underwater vehicle;

[0021] The relative position state between the underwater vehicle and the target point It is expressed as follows:

[0022] ;

[0023] in: is the Euclidean distance from the current position of the underwater vehicle to the target point, , , The underwater vehicle to the target point , , Straight-line distances in the three axis directions; is the angle between the underwater vehicle bow pointing vector and the underwater vehicle current position pointing vector to the target point, , The underwater vehicles are , The angle between the bow vector in the plane and the vector pointing to the target point from the current position;

[0024] The relative position state of the underwater vehicle and the obstacle is the product of the state of a single obstacle and the number of sensors; the state of the single obstacle The representation is as follows:

[0025] ;

[0026] in: The current position of the underwater vehicle The straight-line distance from the center of the obstacle detected by each sensor; The underwater vehicle's bow direction and current position direction The angle between the vectors of the obstacle centers detected by the sensors; , The underwater vehicles are , The bow vector in the plane points to the current position The angle between the obstacle center vectors detected by the sensors; , , The underwater vehicle and obstacle center are , , The straight-line distances in the three axis directions.

[0027] The reward function It includes rewards for approaching the target point, rewards for stability and safety, and rewards for avoiding obstacles. The expressions are as follows:

[0028] ;

[0029] The reward function for pointing close to the target point The expression is as follows:

[0030] ;

[0031] in: is the angle between the underwater vehicle bow pointing vector and the underwater vehicle to the target point vector before taking action, The angle between the heading vector of the underwater vehicle after taking the action and the vector from the underwater vehicle to the target point;

[0032] The stable and safe reward function The expression is as follows:

[0033] ;

[0034] in: The current position of the underwater vehicle The coordinates in the axis direction, Penalty for underwater vehicles at depths within the danger range, The underwater vehicle is at a pitch angle greater than or less than punishment when

[0035] The obstacle avoidance reward is a number of sensors The sum of rewards for detecting surrounding obstacles, its reward function The expression is as follows:

[0036] ;

[0037] in: Before executing an action for the underwater vehicle, The angle between the bow direction of the underwater vehicle detected by the sensor and the vector formed by the current position of the underwater vehicle and the center of the nearest obstacle, After performing the action for the underwater vehicle, The angle between the heading vector of the underwater vehicle detected by a sensor and the vector formed by the position of the underwater vehicle and the position of the nearest obstacle.

[0038] The end conditions include abnormal end and normal end, and the expressions are as follows:

[0039] ;

[0040] in: This is a normal end, indicating that the underwater vehicle has reached the target point. Within distance range; , , Abnormal termination, indicating that the underwater vehicle depth exceeds the allowed range, the number of steps exceeds the limit, or hits an obstacle.

[0041] In step S3, the position of the underwater vehicle in the dynamic model in the world coordinate system after the underwater vehicle performs the action is updated by using quaternion transformation, which specifically includes:

[0042] S31, based on the position increment matrix in the body coordinate system after the underwater vehicle performs the current action The quaternion after the last action is executed Multiply it by the previous position matrix Add together to obtain the current position coordinates of the underwater vehicle in the world coordinate system ;

[0043] The expression of the position increment matrix is ​​as follows:

[0044] ;

[0045] in: , , They represent the position of the underwater vehicle at this moment and the previous moment in the body coordinate system. , , The speed in the three axes, The time to perform an action;

[0046] S32, according to , , The quaternion of the underwater vehicle at the current moment is updated in the order of; the updated expression is as follows:

[0047] ;

[0048] in: is the quaternion at the current moment, is the quaternion of the previous moment, , , is the distance between the current body coordinates of the underwater vehicle and the previous body coordinates , , The quaternion of the angle between the three axes;

[0049] S33. Based on the updated quaternion, calculate the vector of the underwater vehicle bow pointing vector in the world coordinate system at the current moment; the expression is as follows:

[0050] ;

[0051] in: is the pointing vector of the underwater vehicle’s bow, is the bow pointing vector of the underwater vehicle in the body coordinate system.

[0052] In step S3, updating the parameters in the state space specifically includes:

[0053] S34, updating the parameters of the underwater vehicle's own state in the state space; the updated expression is as follows:

[0054] ;

[0055] in: is the value after parameter normalization, is the actual value of the parameter, is the minimum value of the parameter, is the maximum value of the parameter;

[0056] S35. Update the distance between the underwater vehicle and the target point ; The calculation expression of the distance between the underwater vehicle and the target point is as follows:

[0057] ;

[0058] in: is the target point in the world coordinate system The coordinates of the direction, is the current position of the underwater vehicle in the world coordinate system The coordinates of the direction, for time, is the target point in the world coordinate system The coordinates of the direction, is the current position of the underwater vehicle in the world coordinate system The coordinates of the direction, is the target point in the world coordinate system The coordinates of the direction, is the current position of the underwater vehicle in the world coordinate system Coordinates of the direction;

[0059] The underwater vehicle and the target point are , , The distances in the three axis directions are:

[0060] ;

[0061] ;

[0062] ;

[0063] S36: Update the angle between the underwater vehicle and the target point ; The calculation expression of the angle between the underwater vehicle and the target point is as follows:

[0064] ;

[0065] in: is the pointing vector of the underwater vehicle’s bow, The vector pointing from the current position of the underwater vehicle to the target point;

[0066] The underwater vehicle and the target point are and The angles in the plane are:

[0067] ;

[0068] ;

[0069] in: is the underwater vehicle bow pointing vector and the underwater vehicle pointing obstacle vector , Directional component; is the underwater vehicle bow pointing vector and the underwater vehicle pointing obstacle vector , Directional component;

[0070] S37. Update the angle between the underwater vehicle and the obstacle ; The calculation expression of the angle between the underwater vehicle and the obstacle is as follows:

[0071] ;in: The underwater vehicle's bow direction and current position direction The angle between the vectors of the obstacle centers detected by the sensors

[0072] The underwater vehicle and the obstacle are , , The distances in the three axis directions are:

[0073] ;

[0074] in: For the The obstacles detected by the sensors are in the world coordinate system The coordinates of the direction, For the The obstacles detected by the sensors are in the world coordinate system The coordinates of the direction, For the The obstacles detected by the sensors are in the world coordinate system Coordinates of the direction;

[0075] Then the underwater vehicle pointing vector is Obstacles detected by sensors Plane and The angles in the plane are:

[0076] ;

[0077] ;

[0078] in: They are respectively the bow pointing vector and the underwater vehicle's current position pointing to the The obstacle vector detected by the sensor is Projection vector in the plane; They are respectively the bow pointing vector and the underwater vehicle's current position pointing to the The obstacle vector detected by the sensor is Projection vector into the plane.

[0079] The step S4 specifically includes:

[0080] S41. The state of the environment at each moment when training the dynamics model Input to the deep reinforcement learning algorithm and output action Provide artificial intelligence algorithm environment for underwater vehicles;

[0081] S42: Underwater vehicle physical model execution action Afterwards, get reward And update the status to Then the underwater vehicle artificial intelligence algorithm environment determines whether the state meets the end condition and outputs the end state ;

[0082] S43. Deep reinforcement learning algorithm collects data sets , and repeat the cycle until the amount of data meets the requirements of the experience library, and then use the Bellman equation to calculate the optimal value return under each strategy; the expression of the Bellman equation is as follows:

[0083] ;

[0084] in: Indicates in status Take action The best after Value, that is, the optimal value return; is the state transition probability distribution The state sampling in is the reward value of the current state, is the discount factor, For new status Next execution Best value;

[0085] S44. Using data group , calculate the mean squared Bellman error ;in The parameters of the obstacle avoidance neural network are calculated. Value is ; The mean square Bellman error is as follows:

[0086] ;

[0087] S45. Repeat steps S41-S44 to minimize the mean square Bellman error after multiple iterations, so that the obstacle avoidance neural network model calculates the best obstacle avoidance strategy.

[0088] The step S5 specifically refers to: based on the obstacle avoidance neural network model, the obstacle avoidance neural network model is used in two ways: customizing the target point position, obstacle position, and number method or randomly generating the target point position and obstacle position, to implement an obstacle avoidance strategy for the underwater vehicle to avoid obstacles and successfully reach the target point in an unknown environment.

[0089] The world coordinate system middle The plane is fixed on a horizontal plane. is the origin, The axis is positive;

[0090] The origin of the body coordinate system is fixed to the center of gravity of the underwater vehicle. The plane is parallel to the deck plane, Axis perpendicular to The plane is downward, The axis is positive along the axis of the underwater vehicle toward the bow. Axis perpendicular to The axis is positive to the right.

[0091] Compared with the prior art, the present invention has the following beneficial effects:

[0092] 1. In a method for training an underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning of the present invention, firstly, a physical model of the underwater vehicle and its dynamic model, as well as a world coordinate system fixed to the horizontal plane and a body coordinate system fixed to the center of gravity of the underwater vehicle are constructed, and then an artificial intelligence algorithm environment based on Gym is constructed, and its state space, action space, reward function and end condition are defined, and then the position of the underwater vehicle in the world coordinate system after executing the action is updated by using quaternion transformation, and the parameters in the state space are updated at the same time, and then the updated dynamic model is trained based on the algorithm environment and the deep reinforcement learning algorithm is used. The design constructs a physical and dynamic model of the underwater vehicle to accurately reflect the complexity of the underwater environment, combines the two coordinate systems, and intuitively describes the position and attitude changes of the underwater vehicle. Then, it combines deep learning and reinforcement learning to effectively learn complex obstacle avoidance strategies from a large amount of data, so that it can be extended to task scenarios such as automatic path planning of underwater vehicles in three-dimensional space, so as to autonomously formulate navigation and obstacle avoidance strategies in unknown environments, thereby improving the scope of application of the present invention. Therefore, the scope of application of the present invention is relatively wide.

[0093] 2. In the underwater vehicle automatic obstacle avoidance strategy training method based on deep reinforcement learning of the present invention, by setting the reward functions of approaching the target point, stable safety and obstacle avoidance, as well as abnormal and normal ending conditions, the underwater vehicle can be guided to learn the desired behavior pattern, and positive rewards are used to encourage correct behaviors, and negative rewards or penalties are used to suppress bad behaviors, which helps to converge faster to obtain the best optimization strategy, and solves the sparse rewards used in the reinforcement learning environment in the prior art, which give a large reward after reaching the goal, resulting in large fluctuations in the reinforcement neural network and difficulty in convergence. In addition, the clear reward signal and ending conditions in the present invention can reduce the time wasted in exploring invalid strategies, thereby improving the overall learning efficiency. Therefore, the present invention is not only more accurate, but also more efficient.

[0094] 3. In the underwater vehicle automatic obstacle avoidance strategy training method based on deep reinforcement learning of the present invention, the position of the underwater vehicle in the world coordinate system after executing the action is updated by quaternion transformation, and the parameters in the state space are updated, which reduces the amount of calculation during optimization training, helps to reduce the numerical cumulative error generated during training, and improves the stability of long-term operation. Therefore, the training stability of the present invention is better. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] Figure 1 It is a schematic flow chart of the method steps of the present invention.

[0096] Figure 2 It is a schematic diagram of the physical model of the present invention.

[0097] Figure 3 It is a schematic diagram of the operating space range, initial position and posture of the underwater vehicle of the present invention.

[0098] Figure 4 It is a schematic diagram of the interaction between the artificial intelligence algorithm environment and the deep reinforcement learning algorithm of the present invention.

[0099] Figure 5 It is a schematic diagram of the position and posture changes of the underwater vehicle of the present invention after performing an action and its relationship with the target point.

[0100] Figure 6 It is a schematic diagram of the relative positions of the underwater vehicle of the present invention and the target point or obstacle.

[0101] Figure 7 It is a schematic diagram of the calculation method of the state and reward parameters of the present invention.

[0102] Figure 8 It is a schematic diagram of the relationship between the distance and angle between the underwater vehicle of the present invention and the obstacle and the target point. DETAILED DESCRIPTION

[0103] The present invention is further described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0104] Embodiment 1:

[0105] See also Figure 1-Figure 8 , a method for training an underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning, the method comprising:

[0106] S1. Construct the physical model and dynamic model of the underwater vehicle, as well as the world coordinate system fixed on the horizontal plane when the underwater vehicle is running. and the body coordinate system fixed to the center of gravity of the underwater vehicle ;

[0107] Furthermore, the underwater vehicle includes a propeller, three rudders (a bow rudder, a stern rudder and a direction rudder) and five sensors, and the detection range of the sensors is within 150m around the underwater vehicle.

[0108] The physical model includes the underwater vehicle mass, length, width, , , The moment of inertia in the three-axis direction; the six-degree-of-freedom motion equation satisfied by this physical model is used to reinforce the dynamic model response after each output action of the learning algorithm, including changes in speed, angular velocity, position and posture.

[0109] The movement of an underwater vehicle underwater belongs to the movement in three-dimensional space. In this embodiment, the movement is described by using a world coordinate system fixed on the horizontal plane and a body coordinate system fixed on the center of gravity of the vehicle. middle The plane is fixed on a horizontal plane. is the origin, The axis is positive; the origin of the body coordinate system is fixed at the center of gravity of the underwater vehicle. The plane is parallel to the deck plane, Axis perpendicular to The plane is downward, The axis is positive along the axis of the underwater vehicle toward the bow. Axis perpendicular to The axis is positive to the right.

[0110] S2. Construct an underwater vehicle artificial intelligence algorithm environment based on Gym based on the physical model of the underwater vehicle and its dynamic model, the world coordinate system, and the body coordinate system, and define the state space, action space, reward function, and end condition of the artificial intelligence algorithm environment;

[0111] The action space includes the actions of the bow rudder, stern rudder and rudder. The allowable rotation range of the bow rudder, stern rudder and rudder is .

[0112] Further, it specifically includes:

[0113] S21, with the origin in the world coordinate system , respectively along Axis and 1200m in the positive and negative directions of the axis and The negative 300m of the axis constructs the underwater vehicle movement space;

[0114] S22, randomly generate a target point and a number of obstacles (preferably no less than forty spherical obstacles with a radius of thirty) in the motion space, and use the center of gravity of the underwater vehicle in the world coordinate system. The underwater vehicle's bow is pointed at the location Axis positive direction, Plane and Parallel is the initial attitude of the underwater vehicle;

[0115] S23, constructing the state space of the underwater vehicle based on the speed and angular velocity in the three axis directions of the underwater vehicle, as well as the direction, distance and relative position between the target point and the obstacle; constructing the action space of the underwater vehicle based on the three actions of the bow rudder, the stern rudder and the direction rudder;

[0116] S24, constructing a reward function; the reward function includes: a reward for approaching the target point, a reward for stability and safety, and a reward for avoiding obstacles;

[0117] S25, the termination conditions are reaching the target point, exceeding the allowed depth range, exceeding the allowed number of steps, and hitting an obstacle.

[0118] The state space includes the state of the underwater vehicle itself, the relative position state between the underwater vehicle and the target point, and the relative position state between the underwater vehicle and the obstacle;

[0119] The underwater vehicle's own state is expressed as follows:

[0120] ;

[0121] in: , , They represent the lower edge of the underwater vehicle in the body coordinate system. , , The speeds in the three axes, , , They represent the underwater vehicle's rotation in the body coordinate system. , , The angular velocity of the three axes, is the vector representation of the bow pointing vector of the underwater vehicle in the world coordinate system, are the rudder angles of the three rudders of the underwater vehicle;

[0122] The relative position state between the underwater vehicle and the target point It is expressed as follows:

[0123] ;

[0124] in: is the Euclidean distance from the current position of the underwater vehicle to the target point, , , The underwater vehicle to the target point , , Straight-line distances in the three axis directions; is the angle between the underwater vehicle bow pointing vector and the underwater vehicle current position pointing vector to the target point, , The underwater vehicles are , The angle between the bow vector in the plane and the vector pointing to the target point from the current position, which respectively represents the "left-right" and "up-down" relationship between the underwater vehicle and the target point.

[0125] The relative position state of the underwater vehicle and the obstacle is the product of the state of a single obstacle and the number of sensors; the state of the single obstacle The representation is as follows:

[0126] ;

[0127] in: The current position of the underwater vehicle The straight-line distance from the center of the obstacle detected by each sensor; The underwater vehicle's bow direction and current position direction The angle between the vectors of the obstacle centers detected by the sensors; , The underwater vehicles are , The bow vector in the plane points to the current position The angle between the obstacle center vectors detected by the sensors; , , The underwater vehicle and obstacle center are , , The straight-line distance in the three axis directions. The obstacle state should generate a signal only when the obstacle is within the detection range of the sensor, and adopt a gradient state change. The closer the obstacle is to the underwater vehicle, the stronger the signal.

[0128] The reward function It includes rewards for approaching the target point, rewards for stability and safety, and rewards for avoiding obstacles. The expressions are as follows:

[0129] ;

[0130] The reward function for pointing close to the target point The expression is as follows:

[0131] ;

[0132] in: is the angle between the underwater vehicle bow pointing vector and the underwater vehicle to the target point vector before taking action, The angle between the heading vector of the underwater vehicle after taking the action and the vector from the underwater vehicle to the target point;

[0133] The stable and safe reward function The expression is as follows:

[0134] ;

[0135] in: The current position of the underwater vehicle The coordinates in the axis direction, Penalty for underwater vehicles at depths within the danger range, For underwater vehicles with a pitch angle greater than or less than punishment when

[0136] The obstacle avoidance reward is a number of sensors The sum of rewards for detecting surrounding obstacles, its reward function The expression is as follows:

[0137] ;

[0138] in: Before executing an action for the underwater vehicle, The angle between the bow direction of the underwater vehicle detected by the sensor and the vector formed by the current position of the underwater vehicle and the center of the nearest obstacle, After performing the action for the underwater vehicle, The angle between the heading vector of the underwater vehicle detected by the sensor and the vector formed by the position of the underwater vehicle and the position of the nearest obstacle is in the range of , when the sensor does not detect an obstacle, the reward is 0;

[0139] In this embodiment, the underwater vehicle can detect the positions of the five obstacles closest to it, so the reward for approaching an obstacle is the sum of the rewards calculated by these five sensors.

[0140] The end conditions include abnormal end and normal end, and the expressions are as follows:

[0141] ;

[0142] in: This is a normal end, indicating that the underwater vehicle has reached the target point. Within distance range; , , Abnormal termination, indicating that the underwater vehicle depth exceeds the allowed range, the number of steps exceeds the limit, or hits an obstacle.

[0143] For example: If it is 30, it means that the underwater vehicle has reached the target point within 30 meters; , , They respectively indicate that the underwater vehicle depth exceeds the allowed range of 0 to -300, the number of steps exceeds the limit of 2000 steps, and the number of obstacles hit is less than 30 or 30 times.

[0144] S3, using quaternion transformation to update the position of the underwater vehicle in the dynamic model in the world coordinate system after the underwater vehicle performs the action, and at the same time update the parameters in the state space, the parameters including the state of the underwater vehicle itself, the distance between the underwater vehicle and the target point, the angle between the underwater vehicle and the target point, and the angle and distance between the underwater vehicle and the obstacle;

[0145] Furthermore, the position of the underwater vehicle in the dynamic model in the world coordinate system after the action is performed is updated by using quaternion transformation, specifically including:

[0146] S31, based on the position increment matrix in the body coordinate system after the underwater vehicle performs the current action The quaternion after the last action is executed Multiply it by the previous position matrix Add together to obtain the current position coordinates of the underwater vehicle in the world coordinate system ;

[0147] The expression of the position increment matrix is ​​as follows:

[0148] ;

[0149] in: , , They represent the position of the underwater vehicle at this moment and the previous moment in the body coordinate system. , , The speed in the three axes, The time to perform an action;

[0150] S32, according to , , The quaternion of the underwater vehicle at the current moment is updated in the order of; the updated expression is as follows:

[0151] ;

[0152] in: is the quaternion at the current moment, is the quaternion of the previous moment, , , is the distance between the current body coordinates of the underwater vehicle and the previous body coordinates , , The quaternion of the angle between the three axes;

[0153] S33. Based on the updated quaternion, calculate the vector of the underwater vehicle bow pointing vector in the world coordinate system at the current moment; the expression is as follows:

[0154] ;

[0155] in: is the pointing vector of the underwater vehicle’s bow, is the bow pointing vector of the underwater vehicle in the body coordinate system.

[0156] Furthermore, the parameters in the state space are updated, including:

[0157] S34, updating the parameters of the underwater vehicle's own state in the state space; the updated expression is as follows:

[0158] ;

[0159] in: is the value after parameter normalization, is the actual value of the parameter, is the minimum value of the parameter, is the maximum value of the parameter;

[0160] S35. Update the distance between the underwater vehicle and the target point ; The calculation expression of the distance between the underwater vehicle and the target point is as follows:

[0161] ;

[0162] in: is the target point in the world coordinate system The coordinates of the direction, is the current position of the underwater vehicle in the world coordinate system The coordinates of the direction, for time, is the target point in the world coordinate system The coordinates of the direction, is the current position of the underwater vehicle in the world coordinate system The coordinates of the direction, is the target point in the world coordinate system The coordinates of the direction, is the current position of the underwater vehicle in the world coordinate system Coordinates of the direction;

[0163] The underwater vehicle and the target point are , , The distances in the three axis directions are:

[0164] ;

[0165] ;

[0166] ;

[0167] In this embodiment, all angles between the underwater vehicle and the target point and obstacles are calculated by the following expressions:

[0168] ;

[0169] The above expression means that the dot product of the two vectors is divided by the modulus of the two vectors and then the arc cosine is taken, and then the cross product of the two vectors is calculated. When the cross product is negative, the calculated angle is taken as its opposite, and when the cross product is positive, the calculated angle is taken as itself.

[0170] After calculation, normalization is required; the normalized expression is as follows:

[0171] ;

[0172] in: is the angle between the two vectors, is the angle that needs to be normalized;

[0173] S36: Update the angle between the underwater vehicle and the target point ; The calculation expression of the angle between the underwater vehicle and the target point is as follows:

[0174] ;

[0175] in: is the pointing vector of the underwater vehicle’s bow, The vector pointing from the current position of the underwater vehicle to the target point;

[0176] The underwater vehicle and the target point are and The angles in the plane are:

[0177] ;

[0178] ;

[0179] in: is the underwater vehicle bow pointing vector and the underwater vehicle pointing obstacle vector , Directional component; is the underwater vehicle bow pointing vector and the underwater vehicle pointing obstacle vector , Direction component.

[0180] In this embodiment, when the distance between the obstacle and the underwater vehicle is greater than 150m, the obstacle state is 0. When the distance is less than 150m, the distance between the underwater vehicle and the obstacle is as follows:

[0181] ;

[0182] Then, the normalization formula is used to normalize the underwater vehicle state, and its expression is as follows:

[0183] ;

[0184] in: is the detection range of the sensor, is the radius of the obstacle.

[0185] S37. Update the angle between the underwater vehicle and the obstacle ; The calculation expression of the angle between the underwater vehicle and the obstacle is as follows:

[0186] ;in: The underwater vehicle's bow direction and current position direction The angle between the vectors of the obstacle centers detected by the sensors

[0187] The underwater vehicle and the obstacle are , , The distances in the three axis directions are:

[0188] ;

[0189] in: For the The obstacles detected by the sensors are in the world coordinate system The coordinates of the direction, For the The obstacles detected by the sensors are in the world coordinate system The coordinates of the direction, For the The obstacles detected by the sensors are in the world coordinate system Coordinates of the direction;

[0190] Then the underwater vehicle pointing vector is Obstacles detected by sensors Plane and The angles in the plane are:

[0191] ;

[0192] ;

[0193] in: They are respectively the bow pointing vector and the underwater vehicle's current position pointing to the The obstacle vector detected by the sensor is Projection vector in the plane; They are respectively the bow pointing vector and the underwater vehicle's current position pointing to the The obstacle vector detected by the sensor is Projection vector into the plane.

[0194] S4. Based on the underwater vehicle artificial intelligence algorithm environment and updated parameters, the updated dynamics model is trained using a deep reinforcement learning algorithm to obtain an obstacle avoidance neural network model;

[0195] Further, it specifically includes:

[0196] S41. The state of the environment at each moment when training the dynamics model Input to the deep reinforcement learning algorithm and output action Provide artificial intelligence algorithm environment for underwater vehicles;

[0197] S42: Underwater vehicle physical model execution action Afterwards, get reward And update the status to , then the underwater vehicle artificial intelligence algorithm environment determines whether the state meets the end condition and outputs the end state ; If the end condition is met, the output is 1, if the end condition is not met, the output is 0.

[0198] S43. Deep reinforcement learning algorithm collects data sets , and repeat the cycle until the amount of data meets the requirements of the experience library (preferably greater than one-third of the experience library), and use the Bellman equation to calculate the optimal value return under each strategy; the expression of the Bellman equation is as follows:

[0199] ;

[0200] in: Indicates in status Take action The best after Value, that is, the optimal value return; is the state transition probability distribution The state sampling in is the reward value of the current state, is the discount factor, For new status Next execution Best value;

[0201] S44. Using data group , calculate the mean squared Bellman error ;in is the parameter of the neural network, calculated Value is ; The mean square Bellman error is as follows:

[0202] ;

[0203] S45. Repeat steps S41-S44 to minimize the mean square Bellman error after multiple iterations, so that the obstacle avoidance neural network model calculates the best obstacle avoidance strategy.

[0204] For example, after 600 iterations, the model that successfully avoids obstacles and reaches the target point will be saved, and each saved model will be automatically tested for 20 iterations, and the model success rate of 20 iterations will be calculated to make a preliminary judgment on the quality of the model. The obstacle avoidance neural network model with a success rate greater than 95% will be used as the application model of the obstacle avoidance strategy.

[0205] S5. Based on the obstacle avoidance neural network model, the automatic obstacle avoidance strategy of the underwater vehicle in an unknown environment is implemented by setting target points and obstacles.

[0206] Furthermore, it specifically means: based on the obstacle avoidance neural network model, the obstacle avoidance neural network model is used in two ways: custom target point position, obstacle position, number method or randomly generated target point position and obstacle position, to implement the underwater vehicle in an unknown environment to formulate an obstacle avoidance strategy to avoid obstacles and successfully reach the target point.

[0207] Although the embodiments of the present invention have been shown and described above, it should be understood that the above embodiments are exemplary and should not be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and vary the above embodiments within the scope of the present invention.

Claims

1. A method for training underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning, characterized in that: The method comprises: S1. Construct a physical model and a dynamic model of the underwater vehicle, as well as a world coordinate system ηEξζ fixed to the horizontal plane and a body coordinate system xoyz fixed to the center of gravity of the underwater vehicle when the underwater vehicle is running; S2. Construct an underwater vehicle artificial intelligence algorithm environment based on Gym based on the physical model of the underwater vehicle and its dynamic model, the world coordinate system, and the body coordinate system, and define the state space, action space, reward function, and end condition of the artificial intelligence algorithm environment; S3, using quaternion transformation to update the position of the underwater vehicle in the dynamic model in the world coordinate system after the underwater vehicle performs the action, and at the same time update the parameters in the state space, the parameters including the state of the underwater vehicle itself, the distance between the underwater vehicle and the target point, the angle between the underwater vehicle and the target point, and the angle and distance between the underwater vehicle and the obstacle; S4. Based on the underwater vehicle artificial intelligence algorithm environment and updated parameters, the updated dynamics model is trained using a deep reinforcement learning algorithm to obtain an obstacle avoidance neural network model; S5. Based on the obstacle avoidance neural network model, the automatic obstacle avoidance strategy of the underwater vehicle in an unknown environment is implemented by setting target points and obstacles; The reward function includes rewards for approaching the target point, rewards for stability and safety, and rewards for avoiding obstacles. Its expression is as follows: The reward function for pointing close to the target point The expression is as follows: in: is the angle between the underwater vehicle bow pointing vector and the underwater vehicle to the target point vector before taking action, The angle between the heading vector of the underwater vehicle after taking the action and the vector from the underwater vehicle to the target point; The stable and safe reward function z_penalty is expressed as follows: Where: z t is the coordinate of the underwater vehicle in the z-axis direction at the current position, z_penalty is the penalty for the underwater vehicle's depth being within the dangerous range, tilt_penalty is the penalty for the underwater vehicle's pitch angle being greater than or less than punishment when The obstacle avoidance reward is the sum of the rewards for several sensors i detecting surrounding obstacles, and its reward function is reward(i) obstacle The expression is as follows: in: is the angle between the bow direction of the underwater vehicle detected by the ith sensor and the vector formed by the current position of the underwater vehicle and the center of the nearest obstacle before the underwater vehicle performs the action. The angle between the heading vector of the underwater vehicle detected by the ith sensor and the vector formed by the position of the underwater vehicle and the position of the nearest obstacle after the underwater vehicle performs the action.

2. According to claim 1, a method for training underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning is characterized by: The step S2 specifically includes: S21, constructing the underwater vehicle motion space with the origin E in the world coordinate system along the positive and negative directions of the η axis and the ξ axis and the negative direction of the ζ axis; S22, randomly generate target points and several obstacles in the motion space, and point the bow of the underwater vehicle to the positive direction of the η axis based on the position of the center of gravity of the underwater vehicle in the world coordinate system, and take the xoy plane parallel to ηEξ as the initial posture of the underwater vehicle; S23, constructing the state space of the underwater vehicle based on the speed and angular velocity in the three axis directions of the underwater vehicle, and the direction, distance and relative position between the target point and the obstacle; constructing the action space of the underwater vehicle based on the actions of the bow rudder, the stern rudder and the rudder; S24, constructing a reward function; the reward function includes: a reward for approaching the target point, a reward for stability and safety, and a reward for avoiding obstacles; S25, the termination conditions are reaching the target point, exceeding the allowed depth range, exceeding the allowed number of steps, and hitting an obstacle.

3. The method for training underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning according to claim 2, characterized in that: The state space includes the state of the underwater vehicle itself, the relative position state between the underwater vehicle and the target point, and the relative position state between the underwater vehicle and the obstacle; The underwater vehicle's own state is expressed as follows: [u, v, w, p, q, r, bow_direction, roll, rudder, elevator]; Where: u, v, w represent the speed of the underwater vehicle along the x, y, and z axes in the body coordinate system, p, q, and r represent the angular speed of the underwater vehicle around the x, y, and z axes in the body coordinate system, bow_direction is the vector representation of the underwater vehicle's bow pointing vector in the world coordinate system, roll, rudder, and elevator are the rudder angles of the three rudders of the underwater vehicle, respectively; The relative position state target_state between the underwater vehicle and the target point is expressed as follows: Wherein: distance_target is the Euclidean distance from the current position of the underwater vehicle to the target point, distance_target_η, distance_target_ξ, and distance_target_ζ are the straight-line distances from the underwater vehicle to the target point in the directions of the three axes η, ξ, and ζ respectively; angle_target is the angle between the underwater vehicle's bow pointing vector and the underwater vehicle's current position pointing to the target point position vector, and angle_target_ηEξ and angle_target_ηEζ are the angles between the underwater vehicle's bow vector and the current position pointing to the target point vector in the ηEξ and ηEζ planes respectively; The relative position state of the underwater vehicle and the obstacle is the product of the state of a single obstacle and the number of sensors; the state of the single obstacle is obstacle s The expression of tate is as follows: Wherein: distance_obstacle(i) is the straight-line distance from the current position of the underwater vehicle to the center of the obstacle detected by the i-th sensor; angle_obstacle(i) is the angle between the underwater vehicle bow pointing to the center of the obstacle detected by the i-th sensor from the current position; angle_obstacle_ηEξ(i) and angle_obstacle_ηEζ(i) are the angles between the underwater vehicle bow vector in the ηEξ and ηEζ planes and the vector of the obstacle center detected by the i-th sensor from the current position; distance_obstacle_η(i), distance_obstacle_ξ(i) and distance_obstacle_ζ(i) are the straight-line distances between the underwater vehicle and the obstacle center in the directions of the three axes η, ξ and ζ, respectively.

4. The method for training underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning according to claim 2, characterized in that: The end conditions include abnormal end and normal end, and the expressions are as follows: Where: Where: distance_target t ≤A1 is a normal end, indicating that the underwater vehicle has reached the target point within the distance of A1; t ≤A2∪z t ≥0, steps≥A3, and distance_obstacle(i)≤A4 are abnormal terminations, indicating that the underwater vehicle depth exceeds the allowed range, the number of steps exceeds the limit, and it hits an obstacle.

5. The method for training underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning according to claim 1, characterized in that: In step S3, the position of the underwater vehicle in the dynamic model in the world coordinate system after the underwater vehicle performs the action is updated by using quaternion transformation, which specifically includes: S31, based on the position increment matrix [Δx t , Δy t , Δz t ] and the quaternion rotation after the last action t-1 Multiply it by the previous position matrix [η t-1 ,ξ t-1 , t-1 ] to obtain the current position coordinates of the underwater vehicle in the world coordinate system [η t ,ξ t , t ]; The expression of the position increment matrix is ​​as follows: Δx t =(u t +u t-1 ) / 2*t Δy=(v t +v t-1 ) / 2*t; Δz t =(w t +w t-1 ) / 2*t Where: u t +u t-1 、v t +v t-1 、w t +w t-1 They represent the speed of the underwater vehicle in the x, y, and z axes at this moment and the previous moment in the body coordinate system, respectively, and t is the time to perform one action; S32, updating the quaternion of the underwater vehicle at the current moment in the order of x, y, and Z; the updated expression is as follows: rotation t =rotation_z t *rotation_y t *rotation_x t *rotation t-1 ; Among them: rotation t is the quaternion at the current moment, rotation t-1 is the quaternion of the previous moment, rotation_x t 、rotation_y t 、rotation_z t The quaternion is the angle between the current body coordinates of the underwater vehicle and the previous body coordinates in the directions of the three axes x, y, and z; S33. Based on the updated quaternion, calculate the vector of the underwater vehicle bow pointing vector in the world coordinate system at the current moment; the expression is as follows: bow_direction=rotation t *[1,0,0]; Wherein: bow_direction is the pointing vector of the underwater vehicle bow, and [1, 0, 0] is the pointing vector of the underwater vehicle bow in the body coordinate system.

6. The underwater vehicle automatic obstacle avoidance strategy training method based on deep reinforcement learning according to claim 1, characterized in that: In step S3, updating the parameters in the state space specifically includes: S34, updating the parameters of the underwater vehicle's own state in the state space; the updated expression is as follows: Among them: a′ is the normalized value of the parameter, a is the actual value of the parameter, a min is the minimum value of the parameter, a max is the maximum value of the parameter; S35, updating the distance distance_target between the underwater vehicle and the target point; the calculation expression of the distance between the underwater vehicle and the target point is as follows: Where: η target is the coordinate of the target point in the η direction in the world coordinate system, η t is the coordinate of the underwater vehicle in the η direction in the world coordinate system at the current moment, t is the time t, ξ target is the coordinate of the target point in the ξ direction in the world coordinate system, ξ t is the coordinate of the underwater vehicle in the ξ direction in the world coordinate system at the current moment, ζ target is the coordinate of the target point in the ζ direction in the world coordinate system, ζ t is the coordinate of the underwater vehicle in the z direction in the world coordinate system at the current moment; Then the distances between the underwater vehicle and the target point in the directions of the three axes η, ξ, and ζ are: diatance_target_η=(η target -or t ); diatance_target_ξ=(ξ target -x t ); diatance_target_ζ=(ζ target -g t ); S36, updating the angle angle_target between the underwater vehicle and the target point; the calculation expression of the angle between the underwater vehicle and the target point is as follows: angle_target=angle_between_vectory(bow_direction, point_target); Where: bow_direction is the pointing vector of the underwater vehicle's bow, point_target is the vector pointing from the underwater vehicle's current position to the target point; Then the angles between the underwater vehicle and the target point in the ηEξ and ηEζ planes are: angle_target_ηEξ=angle_between_vectory(bow_direction[0, 1], point_target[0, 1]); angle_target_ηEζ=angle_between_vectory(bow_direction[0, 2], point_target[0, 2]); Wherein: bow_direction[0,1], point_target[0,1] are the x- and y-direction components of the underwater vehicle bow pointing vector and the underwater vehicle pointing vector toward the obstacle; bow_direction[0,2], point_target[0,2] are the x- and z-direction components of the underwater vehicle bow pointing vector and the underwater vehicle pointing vector toward the obstacle; S37, updating the angle angle_obstacle(i) between the underwater vehicle and the obstacle; the calculation expression of the angle between the underwater vehicle and the obstacle is as follows: angle_obstacle(i)=angle_between_vectory(bow_direction, point_obstacle(i)); Where: angle_obstacle(i) is the angle between the underwater vehicle bow pointing to the vector pointing to the center of the obstacle detected by the i-th sensor at the current position Then the distances between the underwater vehicle and the obstacle in the directions of the three axes η, ξ, and ζ are: (or obstacle(i) -or t ),(ξ obstacle(i) -x t ),(ζ obstacle(i) -g t ); Where: η obstacle (i) is the coordinate of the obstacle detected by the i-th sensor in the η direction in the world coordinate system, ξ obstacle(i) is the coordinate of the obstacle detected by the i-th sensor in the ξ direction in the world coordinate system, ζ obstacle(i) is the coordinate of the obstacle detected by the i-th sensor in the z direction in the world coordinate system; Then the angles between the underwater vehicle pointing vector and the obstacle detected by the i-th sensor in the ηEξ plane and the ηEζ plane are: Wherein: bow_direction[0,1], point_obstacle(i)[0,1] are respectively the bow pointing vector and the projection vector of the obstacle vector detected by the i-th sensor at the current position of the underwater vehicle in the ηEξ plane; bow_direction[0,2], point_obstacle(i)[0,2] are respectively the bow pointing vector and the projection vector of the obstacle vector detected by the i-th sensor at the current position of the underwater vehicle in the ηEζ plane.

7. The underwater vehicle automatic obstacle avoidance strategy training method based on deep reinforcement learning according to claim 1, characterized in that: The step S4 specifically includes: S41, the state of the environment at each moment when training the dynamics model s t Input into the deep reinforcement learning algorithm and output action a t Provide artificial intelligence algorithm environment for underwater vehicles; S42: The underwater vehicle physical model performs action a t After that, you will get the reward r t And update the state to s t+1 Then the underwater vehicle artificial intelligence algorithm environment determines whether the state meets the end condition and outputs the end state d t ; S43, deep reinforcement learning algorithm collects data sets (s t , a t , r t ,s t+1 , d t ), and repeat the cycle until the amount of data meets the requirements of the experience library, and then use the Bellman equation to calculate the optimal value return under each strategy; the expression of the Bellman equation is as follows: Where: Q * (s, a) represents the best Q value after taking action a in state s, that is, the optimal value return; s t+1 ~P is the state sampling in the state transition probability distribution P, r(s, a) is the reward value of the current state, γ is the discount factor, The new state s t+1 Execute a t+1 The best Q value of S44. Using data group Calculate the mean squared Bellman error Where φ is the parameter of the obstacle avoidance neural network, and the calculated Q value is Q φ (s, a); the mean square Bellman error is as follows: S45. Repeat steps S41-S44 to minimize the mean square Bellman error after multiple iterations, so that the obstacle avoidance neural network model calculates the best obstacle avoidance strategy.

8. The method for training underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning according to claim 1, characterized in that: The step S5 specifically refers to: based on the obstacle avoidance neural network model, the obstacle avoidance neural network model is used in two ways: customizing the target point position, obstacle position, and number method or randomly generating the target point position and obstacle position, to implement an obstacle avoidance strategy for the underwater vehicle to avoid obstacles and successfully reach the target point in an unknown environment.

9. The method for training underwater vehicle automatic obstacle avoidance strategy based on deep reinforcement learning according to claim 1, characterized in that: In the world coordinate system ηEξζ, the ηEξ plane is fixed on the horizontal plane, E is the origin, and the ζ axis is positive upward; The origin of the body coordinate system is fixed at the center of gravity of the underwater vehicle, the xoy plane is parallel to the deck plane, the Z axis is perpendicular to the xoy plane and is positive downward, the x axis is positive along the axis of the underwater vehicle toward the bow, and the y axis is perpendicular to the x axis and is positive to the right.

Images