Optimal energy consumption path planning method for underwater robot based on optimal propulsion speed

Abstract

The present invention discloses an optimal energy consumption path planning method for an underwater robot based on optimal propulsion speed. The method comprises the following steps: S1: initializing quantum particle swarm algorithm parameters, establishing an energy consumption model of an autonomous underwater vehicle, and obtaining ocean current data and an environmental model; S2: using the quantum particle swarm algorithm to generate a candidate path in the form of a B-spline curve; S3: using a vector analysis method to calculate the optimal propulsion speed of each path segment and the energy consumption under the optimal propulsion speed, and accumulating the energy consumption of each path segment to obtain the energy consumption of the candidate path; S4: updating the optimal position of individual particles and the optimal position of the population; S5: updating the particle speed and position information; S6: judging whether the termination condition is met, if so, ending the iteration and outputting the optimal result; if not, returning to step S2. The calculation efficiency of the optimal energy consumption path planning is improved.

Description

Technical Field

[0001] The present invention relates to the field of robot intelligent control, and in particular to an optimal energy consumption path planning method for an autonomous underwater vehicle based on an optimal propulsion speed. Background Art

[0002] In the marine environment, ocean currents are a widespread phenomenon of seawater flow, and their impact on the navigation of autonomous underwater vehicles (AUVs) cannot be ignored. How to reasonably utilize ocean current energy has become one of the main contents of AUV research in recent years.

[0003] At present, the AUV path planning algorithms for energy saving are mainly divided into two types: time optimal path planning and energy consumption optimal path planning. The time optimal path planning algorithm assumes that the AUV propulsion speed is constant. This assumption reduces the dimension of the search space and has high computational efficiency. However, the AUV cannot adjust its own propulsion speed according to the ocean current, which limits the navigation freedom of the AUV; the energy consumption optimal path planning does not limit the propulsion speed. The AUV can adjust its propulsion speed according to the ocean current, and energy consumption can be further reduced. However, the search space dimension is increased in the energy consumption optimal path planning, the algorithm has low computational efficiency, and poor real-time performance.

[0004] For example, a "method for optimizing energy consumption of underwater unmanned vehicles" disclosed in Chinese patent documents, with the announcement number CN112613180B, includes constructing a first model for optimizing the distribution of the output power of each fuel cell stack inside the fuel cell system, so that the equivalent instantaneous hydrogen consumption of the fuel cell system is minimized under the preset output power of the fuel cell system; constructing a second model for optimizing the distribution of the output power of the fuel cell system and the battery system in the UUV hybrid power system, so that the equivalent instantaneous hydrogen consumption of the UUV hybrid power system is minimized under the preset UUV speed; constructing a third model for planning the UUV navigation path, so that the equivalent hydrogen consumption of the UUV is minimized under the preset UUV speed; jointly solving the first model, the second model, and the third model to obtain the navigation path of the UUV and the minimum equivalent hydrogen consumption. Although this solution can effectively reduce the energy consumption of underwater unmanned vehicles, its algorithm calculation efficiency is low and its real-time performance is poor. Summary of the invention

[0005] The present invention mainly solves the problems of low calculation efficiency and poor real-time performance of optimal energy consumption path planning in the prior art, and provides an optimal energy consumption path planning method for an underwater robot based on optimal propulsion speed.

[0006] The above technical problems of the present invention are mainly solved by the following technical solutions:

[0007] The optimal energy consumption path planning method for an underwater robot based on an optimal propulsion speed comprises the following steps:

[0008] S1: Initialize the parameters of quantum particle swarm algorithm, establish the energy consumption model of autonomous underwater vehicle, and obtain ocean current data and environmental model;

[0009] S2: Use quantum particle swarm algorithm to generate candidate paths in the form of B-spline curves;

[0010] S3: using a vector analysis method to calculate the optimal propulsion speed of each path segment and the energy consumption under the optimal propulsion speed, and accumulating the energy consumption of each path segment to obtain the energy consumption of the candidate path;

[0011] S4: Update the best position of individual particles and the best position of the population;

[0012] S5: Update particle velocity and position information;

[0013] S6: Determine whether the termination condition is met. If so, end the iteration and output the optimal result; if not, return to step S2.

[0014] This solution uses vector analysis to calculate the optimal propulsion speed of each path segment and the energy consumption under the optimal propulsion speed, and accumulates the energy consumption of each path segment to obtain the energy consumption of the candidate path, thereby improving the calculation efficiency of the optimal energy consumption path planning.

[0015] Preferably, the step S1 comprises the following steps:

[0016] S101: Set the number of particles in the quantum particle swarm and the learning parameters of the quantum particle swarm;

[0017] S102: Establish energy consumption model of autonomous underwater vehicle:

[0018]

[0019] Among them, K d is the drag coefficient;

[0020] represents the navigation vector of the autonomous underwater vehicle relative to the water;

[0021] F drag The navigation resistance of the autonomous underwater vehicle;

[0022] is the propulsion speed of the autonomous underwater vehicle;

[0023] t represents the autonomous underwater vehicle along the pre-navigation vector Sailing time;

[0024] S103: Obtain ocean current data and an environmental model from an ocean current measurement or ocean current prediction system.

[0025] Preferably, in the quantum particle swarm algorithm, the dimension of each particle is the space dimension D×the number of control points n, which represents the positions of the n control points. A B-spline curve is generated according to the positions of the control points, and the B-spline curve is used as a candidate path.

[0026] Preferably, the shape of the B-spline curve is changed by modifying one or more control parameters among the control point positions, node distribution mode and the degree of the curve;

[0027] By selecting the control point position, the curve shape can be modified in the following ways:

[0028] 1) Force the curve segment to become a straight line segment: let p+1 adjacent control points be collinear;

[0029] 2) Force the B-spline curve to pass through a control point: let p adjacent control points coincide;

[0030] 3) Force the B-spline curve to be tangent to one side of the control line: Let the control point C i-p , C i-p+1 =C i-p+2 =…=C i-1 and C i+1 Collinear.

[0031] The position of the control point determines the shape of the control polyline, the degree of the curve determines how close the spline curve is to the control polyline, and the repetition of the node determines whether the curve is tangent to the control polyline and the continuity of the curve.

[0032] Preferably, the step S3 includes the following process:

[0033] S301: Discretize the B-spline curve, and divide the path into N segments according to the direction of the line connecting the starting point and the end point. The larger N is, the closer the discretized path is to the original curve, but the amount of calculation will increase; vice versa. The value of N can be determined based on the resolution of the ocean current data, the performance of the AUV, and the length of the path.

[0034] S302: For each path segment, calculating the energy consumption optimal propulsion speed, and calculating the energy consumption of each path segment through an energy consumption model;

[0035] S303: Accumulate the energy consumption of each path segment as the evaluation function value.

[0036] As a preferred embodiment, the energy consumption optimal propulsion speed expression is:

[0037]

[0038]

[0039] Where, θ is the angle between the ocean current vector and the pre-navigation vector;

[0040] is the minimum achievable speed; equal to Direction perpendicular to the pre-flight vector

[0041] Optimal propulsion speed for energy consumption;

[0042] a * is the speed coefficient;

[0043] is the ocean current vector.

[0044] Preferably, the termination condition of step S6 is specifically:

[0045] In addition to the maximum number of iterations, additional termination judgment is added:

[0046]

[0047] When W is less than the set value, the termination criterion is met. In the formula, r represents the current iteration number, and E represents the corresponding fitness value.

[0048] The beneficial effects of the present invention are:

[0049] By using vector analysis method to calculate the optimal propulsion speed of each path segment and the energy consumption under the optimal propulsion speed, the energy consumption of each path segment is accumulated to obtain the energy consumption of the candidate path; thus improving the calculation efficiency of the optimal energy consumption path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 It is a schematic diagram of the velocity vector relationship under the condition of θ=0 of the present invention.

[0051] Figure 2 For the present invention Schematic diagram of velocity vector relationship under different conditions.

[0052] Figure 3 For the present invention Schematic diagram of velocity vector relationship under different conditions.

[0053] Figure 4 This is a flow chart of the optimal energy consumption path planning method for an underwater robot of the present invention.

[0054] Figure 5 Schematic diagram of the control point distribution method of the present invention. DETAILED DESCRIPTION

[0055] The technical solution of the present invention is further specifically described below through embodiments and in conjunction with the accompanying drawings.

[0056] Example:

[0057] The optimal energy consumption path planning method of an underwater robot based on the optimal propulsion speed of this embodiment is as follows: Figure 4 As shown, the following steps are included:

[0058] Step 1: Initialize the parameters of quantum particle swarm algorithm. Establish the energy consumption model of autonomous underwater vehicle and obtain the ocean current data and environmental model.

[0059] S101: Set the number of particles in the quantum particle swarm and set the learning parameters of the quantum particle swarm.

[0060] In this embodiment, the number of particles in the quantum particle swarm is set to 100, and the maximum number of iterations is 500. The learning parameters are set to change gradually with the number of iterations, and the specific form is

[0061]

[0062] Among them, τ is the adaptive learning coefficient;

[0063] X max is the maximum number of iterations;

[0064] X c is the current iteration number.

[0065] S102: The energy consumption model of the autonomous underwater vehicle is:

[0066]

[0067] Among them, K d is the drag coefficient, which is a constant and determined by the AUV design;

[0068] represents the navigation vector of the AUV relative to the water;

[0069] F drag The navigation resistance of the autonomous underwater vehicle;

[0070] is the AUV propulsion speed;

[0071] t represents the AUV along of sailing time.

[0072] S103: Ocean current data can be obtained from ocean current prediction systems (such as ROMS). Environmental models can be obtained from the National Ocean Science Data Center.

[0073] Step 2: Use quantum particle swarm optimization to generate candidate paths in the form of B-spline curves.

[0074] The B-spline curve is a linear combination of B-spline basis curves and is a generalized form of the Bezier curve. Given n+1 control points p0, p1, ..., p n and a node vector U = {u0, u1, ..., u m}, then the p-order B-spline curve is controlled by these control points and node vectors, and its expression is:

[0075]

[0076] Where C(u) is the coordinate of the point on the curve;

[0077] N i，p (u) is the p-order B-spline curve basis function;

[0078] p i are the coordinates of the control points;

[0079] p is a parameter in the spline curve. The value of p is any integer between 2 and the number of control points n+1. It controls the degree of the polynomial in the basis function. In a B-spline curve, the number of control points n+1, the degree of basis function p, and the number of knot vectors m must satisfy m=n+p+1.

[0080] The recursive definition of the B-spline basis function is:

[0081]

[0082]

[0083] Each basis function is defined on a p subinterval of the range of u. The selected set of subinterval endpoints u i is called a node, and the selected set of subinterval endpoints is collectively called a node vector. i ≤u i+1 As the endpoint of the subinterval.

[0084] In a B-spline curve, the shape of the curve can be changed by modifying one or more control parameters including the control point position, node distribution method and the degree of the curve. The control point position determines the shape of the control polyline, the degree of the curve determines how close the spline curve is to the control polyline, and the repetition of the nodes determines whether the curve is tangent to the control polyline and the continuity of the curve.

[0085] The dimension of each particle is the spatial dimension D × the number of control points n, which represents the position of n control points. Once the position of the particle is determined, the B-spline curve can be generated by the position of the control points. The shape of the curve can be modified to some extent by selecting the control point position in the following way.

[0086] 1) Force the curve segment to become a straight line segment: let p+1 adjacent control points be collinear;

[0087] 2) Force the B-spline curve to pass through a control point: let p adjacent control points coincide;

[0088] 3) Force the B-spline curve to be tangent to one side of the control line: Let C i-p , C i-p+1 =C i-p+2 =…=C i-1 and C i+1 Collinear.

[0089] In the quantum particle swarm algorithm, each particle represents a set of control point positions, and a B-spline curve is generated according to the control point positions, and the B-spline curve is used as a candidate path.

[0090] According to the requirements, the starting point and the end point are the points that must be passed, so p control points need to be set at the starting point and the end point. The remaining control points can be evenly distributed in the search space for exploration, such as Figure 5 shown.

[0091] Step 3: Use the vector analysis method to calculate the optimal propulsion speed of each path segment and the energy consumption under the optimal propulsion speed, and accumulate the energy consumption of each path segment to obtain the energy consumption of the candidate path;.

[0092] First, the B-spline curve is discretized, that is, the path is evenly divided into N segments according to the direction of the line connecting the starting point and the end point. The larger N is, the closer the discretized path is to the original curve, but the amount of calculation will increase; vice versa. The value of N can be determined based on the resolution of ocean current data, AUV performance, and path length.

[0093] Secondly, for each path segment, the energy-optimal propulsion speed is calculated according to the following expression, and the energy consumption of each path segment is calculated through the energy consumption model. The energy-optimal propulsion speed expression is:

[0094]

[0095] Where, θ is the angle between the ocean current vector and the pre-navigation vector;

[0096]

[0097] Finally, the energy consumption of each path segment is accumulated as the evaluation function value.

[0098] Case 1: θ = 0

[0099] Figure 1 is the velocity vector relationship when θ = 0. In this case, the direction of the ocean current is consistent with the pre-navigation direction, so the AUV can only rely on the ocean current to navigate along the Drift without consuming energy.

[0100] Condition:

[0101] Figure 2 is the velocity vector relationship. To facilitate analysis, an intermediate variable is introduced

[0102] like Figure 2 As shown, The size is equal to Its direction is perpendicular to the pre-navigation vector It is called the minimum achievable speed.

[0103] In addition, the advancement speed The size can be replaced by Among them, a is called the speed coefficient and a ≥ 0. By replacing, we can solve the energy consumption optimal speed Transformed into solving the optimal speed coefficient a * .

[0104] according to Figure 2 The following relationship can be obtained:

[0105]

[0106] AUV The energy consumption for navigation is:

[0107]

[0108] In the above formula, only the speed coefficient a is an unknown quantity, and the remaining parameters can be obtained directly or indirectly, that is, this formula is a single variable equation.

[0109] Therefore, the optimal speed coefficient a can be obtained by using the extreme value method * .

[0110] Taking the derivative of a, we get

[0111]

[0112] Let E′=0, then we can easily get

[0113]

[0114] According to the definition of speed coefficient, the optimal speed for energy consumption is Using the sine theorem, we can get the yaw angle β = arcsin (a * ) -1 .

[0115] Case 3:

[0116] Figure 3 is a vector relationship diagram. Among them, vector In vector On the reverse extension line.

[0117] Similar to The analysis and solution method of the case, using the minimum theorem, can obtain the optimal speed coefficient expression under this condition:

[0118]

[0119] The optimal speed for energy consumption is Using the sine theorem, we can get β = arcsin (a * ) -1 .

[0120] It can be seen that in case 2 and case 3, the optimal speed coefficient a * The expression of is the same, so the energy consumption optimal speed in both cases is The magnitude and direction expressions are also the same.

[0121] Summarizing the conclusions of the above four situations, we can get:

[0122]

[0123] Step 4: Update the optimal position of individual particles and the optimal position of the population.

[0124] At the current number of iterations, if the evaluation function value of a particle is the minimum evaluation function value of the particle, then the position of the particle is recorded as the best position of the individual particle; if the evaluation function value of a particle is the minimum evaluation value of the population at the current number of iterations, then the position of the particle is recorded as the best position of the population. The mathematical expression of this step is as follows

[0125] Individual particle optimal position update:

[0126]

[0127] Where P i (X c ) is the number of iterations X c The best position of particle i when ;

[0128] f(P i (X c )) is the particle i at position P i (X C ) is the evaluation function value when .

[0129] Population best position update:

[0130]

[0131] In the formula, G(X c ) is the number of iterations X c When , the particle position with the smallest evaluation function value among all the best positions of particles is the best position of the population;

[0132] Step 5: Update particle velocity and position information. Specifically:

[0133]

[0134]

[0135]

[0136] ψ i，j (X c )=τ·|mbest j (X c )-P i，j (X c )|

[0137]

[0138] Among them, mbest(X c ) is the average value of the best position of individual particles;

[0139] τ is the learning coefficient, and its value decreases with the increase of the number of iterations. The purpose is to increase the exploration ability in the early stage of the algorithm and the local search ability in the later stage, making it easier to search for the optimal result;

[0140] v i (X c ) is the particle position randomly generated according to the optimal position of particle i and the global optimal position, which is used for subsequent updating of the particle position;

[0141] ψ i (X C ) is the velocity of particle i, which is used to update the particle position later;

[0142] X max is the maximum number of iterations;

[0143] X c is the current iteration number;

[0144] is a random number evenly distributed in (0, 1);

[0145] P i (X c +1) is the updated particle position.

[0146] Step 6: Determine whether the termination condition is met. If so, end the iteration and output the optimal result; if not, return to step 2.

[0147] The termination conditions are as follows:

[0148] In addition to the maximum number of iterations, additional termination judgment is added:

[0149]

[0150] When W is less than the set value, the termination criterion is met. In the formula, r represents the current iteration number, and E represents the corresponding fitness value.

[0151] The solution of this embodiment uses vector analysis to calculate the optimal propulsion speed of each path segment and the energy consumption under the optimal propulsion speed, and accumulates the energy consumption of each path segment to obtain the energy consumption of the candidate path, thereby improving the calculation efficiency of the optimal energy consumption path planning.

[0152] It should be understood that the embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention. In addition, it should be understood that after reading the content taught by the present invention, those skilled in the art can make various changes or modifications to the present invention, and these equivalent forms also fall within the scope limited by the appended claims of the application.

Claims

1. An optimal energy consumption path planning method for an underwater robot based on optimal propulsion speed, characterized in that: The following steps are involved: S1: Initialize the parameters of quantum particle swarm algorithm, establish the energy consumption model of autonomous underwater vehicle, and obtain ocean current data and environmental model; S2: Use quantum particle swarm algorithm to generate candidate paths in the form of B-spline curves; S3: Use vector analysis to calculate the optimal propulsion speed of each path segment and the energy consumption under the optimal propulsion speed, and accumulate the energy consumption of each path segment to obtain the energy consumption of the candidate path; discretize the B-spline curve, divide the path into N segments according to the direction of the line connecting the starting point and the end point, and determine the N value according to the resolution of the ocean current data, the performance of the AUV and the path length; For each path segment, the energy consumption optimal propulsion speed is calculated, and the energy consumption of each path segment is calculated through the energy consumption model, and the energy consumption of each path segment is accumulated as the evaluation function value; S4: Update the best position of individual particles and the best position of the population; S5: Update particle velocity and position information; S6: Determine whether the termination condition is met. If so, end the iteration and output the optimal result; if not, return to step S2.

2. The optimal energy consumption path planning method for an underwater robot based on optimal propulsion speed according to claim 1 is characterized in that: The step S1 comprises the following steps: S101: Set the number of particles in the quantum particle swarm and the learning parameters of the quantum particle swarm; S102: Establish energy consumption model of autonomous underwater vehicle: Among them, K d is the drag coefficient; represents the navigation vector of the autonomous underwater vehicle relative to the water; F drag The navigation resistance of the autonomous underwater vehicle; is the propulsion speed of the autonomous underwater vehicle; t represents the autonomous underwater vehicle along the pre-navigation vector Sailing time; S103: Obtain ocean current data and an environmental model from an ocean current measurement or ocean current prediction system.

3. The optimal energy consumption path planning method for an underwater robot based on optimal propulsion speed according to claim 1 is characterized in that: In the quantum particle swarm algorithm, the dimension of each particle is the spatial dimension D×the number of control points n, which represents the position of n control points. A B-spline curve is generated according to the position of the control points, and the B-spline curve is used as a candidate path.

4. The optimal energy consumption path planning method for an underwater robot based on optimal propulsion speed according to claim 3 is characterized in that: Changing the shape of the B-spline curve by modifying one or more control parameters among the control point positions, node distribution mode and the degree of the curve; By selecting the control point position, the curve shape can be modified in the following ways: 1) Force the curve segment to become a straight line segment: let p+1 adjacent control points be collinear; 2) Force the B-spline curve to pass through a control point: let p adjacent control points coincide; 3) Force the B-spline curve to be tangent to one side of the control line: Let the control point C i-p , C i-p+1 =C i-p+2 =…=C i-1 and C i+1 Collinear.

5. The optimal energy consumption path planning method for an underwater robot based on optimal propulsion speed according to claim 1 is characterized in that: The step S3 includes the following process: S301: discretize the B-spline curve, and divide the path into N segments evenly according to the direction of the line connecting the starting point and the end point; S302: For each path segment, calculating the energy consumption optimal propulsion speed, and calculating the energy consumption of each path segment through an energy consumption model; S303: Accumulate the energy consumption of each path segment as the evaluation function value.

6. The optimal energy consumption path planning method for an underwater robot based on optimal propulsion speed according to claim 5 is characterized in that: The energy consumption optimal propulsion speed expression is: Where, θ is the angle between the ocean current vector and the pre-navigation vector; is the minimum achievable speed; equal to Direction perpendicular to the pre-flight vector Optimal propulsion speed for energy consumption; a * is the speed coefficient; is the ocean current vector.

7. The optimal energy consumption path planning method for an underwater robot based on optimal propulsion speed according to claim 1 is characterized in that: The termination condition of step S6 is specifically: In addition to the maximum number of iterations, additional termination judgment is added: When W is less than the set value, the termination criterion is met; where r represents the current number of iterations and E represents the corresponding fitness value.

Images