A global path planning method for unmanned sailboat

Abstract

This invention belongs to the field of path planning technology and discloses a global path planning method for unmanned sailboats. The method includes the following steps: Step 1, establishing an environmental model based on the starting position, the position of static obstacles, and the target position; Step 2, obtaining a particle swarm and a path based on the environmental model; then, updating the position and velocity of the particles on the path sequentially from front to back, updating the path based on the updated velocity and position, and finally iterating to obtain the globally optimal path; Step 3, fitting the particles on the globally optimal path using a Bezier curve to obtain a smooth planned path. This invention provides a global path planning method for unmanned sailboats that considers wind direction influence and has a short computation time, which can reduce path length, reduce the energy consumption of unmanned sailboats, and improve the long-distance navigation capability of unmanned sailboats.

Description

Technical Field

[0001] This invention belongs to the field of path planning technology, specifically relating to a global path planning method for unmanned sailboats. Background Technology

[0002] Since the beginning of the 21st century, marine environmental monitoring has been of great significance to the sustainable development of the marine economy. At the same time, the vast resources contained in the ocean have attracted widespread attention from various countries due to their political importance. Currently, marine monitoring work mainly focuses on underwater robots and unmanned surface vessels (USVs). However, most of these are limited by their fuel and battery capacity, preventing them from operating and cruising continuously for extended periods, and are also costly. Unmanned sailboats are a type of multifunctional, new type of unmanned mobile observation platform powered by clean marine energy (wind power). They mainly consist of a hull, a central deck, a rudder, and sails. They utilize sails to convert ocean winds into forward propulsion, and use the central deck, rudder, and sails to maintain their posture at sea, thus enabling navigation. Compared to USVs, unmanned sailboats are smaller, capable of operating in distant waters, and offer advantages such as real-time data communication and low operating costs. Generally, crew members plan the sailboat's route based on received marine weather information. However, wind fields at sea are time-varying and uneven, and since the unmanned sailboat's power comes from the wind, there is a 90° navigational hazard in headwinds, making route planning more complex.

[0003] Unmanned sailboats are underactuated and are greatly affected by wind and ocean currents. Currently mature path planning algorithms are not fully applicable to unmanned sailboats, as they do not take into account wind direction information that affects navigation, and the path planning convergence speed is slow and the computation time is long. Summary of the Invention

[0004] The purpose of this invention is to provide a global path planning method for unmanned sailboats that takes into account the influence of wind direction and has a short calculation time, which can reduce path length, reduce energy consumption of unmanned sailboats, and improve the long-distance sailing capability of unmanned sailboats.

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

[0006] A global path planning method for unmanned sailboats includes the following steps:

[0007] Step 1: Establish an environment model based on the starting position, the position of static obstacles, and the target position;

[0008] Step 2: Obtain the particle swarm and initial path based on the environment model; then update the position and velocity of the particles on the path sequentially from front to back, update the path based on the updated velocity and position, and finally iterate to find the globally optimal path.

[0009] Step 3: Fit the particles on the globally optimal path using a Bezier curve to obtain a smooth planned path.

[0010] Furthermore, in step 2, the mathematical expression for the position update is:

[0011] x n+1 =x n +v n +rk

[0012] Where, x n+1 It is the updated position of the particle; x n It is the position of the particle before the update; v n It is the velocity of the particle before it is updated, v n The initial value is 0; k is the disturbance from the ocean waves; r is a random number of 0 or 1;

[0013] The mathematical expression for the speed update is:

[0014] v n+1 =ωv n +c1r1(p best -x n )+c2r2(g best -x n )

[0015] Among them, v n+1 ω is the particle's updated velocity; c1 and c2 are learning factors; r1 and r2 are random numbers; p best For the individual's optimal position; g best It is the globally optimal position.

[0016] Furthermore, in step 2, the method for obtaining the individual optimal position is as follows: calculate the fitness function of the particle's position before the update and the fitness function of the particle's position after the update, and select the position with the smallest fitness function as the individual optimal position; the method for obtaining the global optimal position is as follows: replace the current particle's position with the individual optimal position and update the path; then calculate the fitness function of each particle on the path, and select the position with the smallest fitness function as the global optimal position.

[0017] Furthermore, in step 2, the expression for the fitness function is:

[0018]

[0019] Among them, G distance It is the cost function of distance; G wind It is the cost function of wind; G steer It is the cost function of turning;

[0020]

[0021] Where distance is the length of the path; if the unmanned sailboat collides with an obstacle, the collision is 0, otherwise the collision is 1.

[0022]

[0023] Among them, w wind It is the weighting coefficient for wind direction cost; It is the wind direction angle; It is the heading angle;

[0024]

[0025] Among them, w steer w1 is the weighting coefficient for the turning cost; w1 is the weighting coefficient for the distance. It is the current orientation angle of the unmanned sailboat relative to the target position.

[0026] Furthermore, in step 2, after obtaining the global optimal position, the probability pa is used to select whether to perform a mutation operation on the individual optimal position: pa is a random number uniformly distributed between [0, 1]. When pa > 0.5, the individual optimal position is mutated; then the mutated individual optimal position is used to replace the original individual optimal position to update the particle velocity and update the path.

[0027] Furthermore, in step 2, the optimal position p' of the mutated individual best =p best +Δm, where Δm follows a function with mean zero and variance σm. 2 The Gaussian distribution is denoted as Δm~(0,σm) 2 ).

[0028] Furthermore, in step 2, after performing a mutation operation on the individual optimal position, randomly select particles that have completed position updates and put them into the mutation pool for mutation. If the number of particles is greater than 1, the number of selected particles is even. Then, the selected particles are grouped into particle pairs in the order from front to back, and the position of each particle pair is mutated. Then, the mutated particles are used to replace the original particle update path, and then the global optimal position is calculated to update the velocity of the particles.

[0029] Furthermore, in step 2, the mutation method for the particle pairs is as follows:

[0030]

[0031] In the formula, X1 and X2 are the two particles in a particle pair; p c X'1 and X'2 are random numbers uniformly distributed between [0, 1]; X'1 and X'2 are mutated particles.

[0032] Furthermore, in step 2, the particle velocity is the combined velocity of the unmanned sailboat's velocity and the wind field's velocity.

[0033] An unmanned sailboat global path planning system includes an environment model building module for establishing the global environment of the unmanned sailboat; a wind field model building module connected to the environment model building module for establishing a wind field model of ocean winds; a fitness function calculation module connected to the environment model building module and the wind field model building module for establishing a fitness function model based on particle velocity, path length, wind field model, and turning information; a particle swarm path planning module connected to the environment model building module and the fitness function calculation module for calculating the initial path of the unmanned sailboat based on the global environment; the particle swarm path planning module also updates the particle positions, selects the individual optimal position and the global optimal position on the path based on the fitness function model, performs mutation operations on the particles on the path, updates the particle velocity based on the individual optimal position and the global optimal position, and obtains the global optimal path; and a Bézier curve fitting module connected to the particle swarm path planning module for fitting Bézier curves to the particles on the global optimal path to obtain a smooth planned path.

[0034] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0035] 1. This invention establishes an environmental model of the entire navigation area, describing obstacle information, starting and target position information, and establishes a wind field model; the wind field model is combined with the kinematics of the unmanned sailboat to establish a fitness function; an improved particle swarm optimization algorithm is used for optimal path planning, solving the problem that existing unmanned sailboats only consider position information and obstacle information for path planning; the path length is reduced by iterative path optimization, thereby reducing the energy consumption of the unmanned sailboat and enhancing its ability to navigate long distances.

[0036] 2. During velocity and position updates, influenced by the individual optimal position and the global optimal position, particles iterate continuously towards these positions, easily getting trapped in local minima and experiencing slow convergence. Therefore, mutating the individual and global optimal positions introduces more randomness, generating more possibilities, optimizing the local minima problem, and improving convergence speed; it also reduces computation time and energy consumption.

[0037] 3. Select the updated particles for mutation, further update the positions of some particles before the next iteration, and selectively optimize the global optimal position g. best Mutations can increase the diversity of bee colonies as much as possible. Attached Figure Description

[0038] Figure 1 This is a flowchart of Embodiment 1 of the present invention;

[0039] Figure 2 This is a schematic diagram of the wind field in Embodiment 1 of the present invention;

[0040] Figure 3 This is a schematic diagram of the resultant velocity model in Embodiment 1 of the present invention;

[0041] Figure 4 This is a flowchart of the improved particle swarm optimization algorithm according to Embodiment 1 of the present invention;

[0042] Figure 5 This is a schematic diagram of the particle swarm and initial path in Embodiment 1 of the present invention;

[0043] Figure 6 This is a schematic diagram of particle pair mutation in Embodiment 1 of the present invention;

[0044] Figure 7 This is a schematic diagram of the unmanned sailboat path planning system of Embodiment 1 of the present invention. Detailed Implementation

[0045] Example 1

[0046] A global path planning method for unmanned sailboats, such as Figure 1 As shown, it includes the following steps:

[0047] Step 1: Establish an environmental model based on the starting position, the position of static obstacles, and the target position. The environmental model includes position information and the range of obstacles. Then, perform safety inflation processing on the obstacles and the unmanned sailboat to ensure that the unmanned sailboat will not be at risk of colliding with obstacles under the interference of wind and waves.

[0048] Step 2: Establish an ocean wind field model based on the scope of the environmental model, such as... Figure 2 As shown, the mathematical expression for the wind field model is:

[0049]

[0050] Among them, V fx (x,t) represents the velocity component of the wind along the x-axis at time t; V fy (x,t) represents the wind velocity component on the y-axis at time t; the wind field velocity is constructed based on meteorological data, which can be obtained from the China Meteorological Data Network.

[0051] The resultant velocity model is expressed as:

[0052] V compose (Q,t)=V f(Q,t)+V sailboat (t)

[0053] Among them, V sailboat (t) represents the velocity of the unmanned sailboat at time t. The principle for establishing the resultant velocity model is as follows: Figure 3 As shown.

[0054] Step 3, establish the fitness function, whose mathematical expression is:

[0055]

[0056] Among them, G distance It is the cost function of distance; G wind It is the cost function of wind; G steer It is the cost function of turning.

[0057]

[0058] Where distance is the length of the path; if the unmanned sailboat collides with an obstacle, the collision is 0, otherwise the collision is 1.

[0059]

[0060] Among them, w wind It is the weighting coefficient for wind direction cost; It is the wind direction angle; It is the heading angle.

[0061]

[0062] Among them, w steer w1 is the weighting coefficient for the turning cost; w1 is the weighting coefficient for the distance. It is the orientation angle between the current unmanned sailboat and the target position, that is, the angle between the current orientation of the unmanned sailboat and the target direction (the line connecting the unmanned sailboat and the target position).

[0063] Step 4: An improved particle swarm optimization (PSO) algorithm is used for global path planning of the unmanned sailboat. The fitness function of each particle is calculated, and its size is compared to obtain the optimal position of each individual particle and the globally optimal position. The globally optimal path is then iteratively derived through velocity and position updates. The PSO algorithm flowchart is shown below. Figure 4 As shown.

[0064] Based on the particle swarm obtained from the environmental model, the particle swarm is first initialized: the initial paths for particle composition are randomly selected, such as... Figure 5As shown, the positions of particles on the path are then used as individual optimal positions, and the global optimal position among all particles on the path is calculated. Then, the positions and velocities of the particles on the path are updated sequentially from the starting position to the target position, from front to back.

[0065] Step 4.1: Following the order from the starting position to the target position, update the position and velocity of each particle along the path (excluding the starting and target positions). First, update the particle positions. The mathematical expression for position update is:

[0066] x n+1 =x n +v n +rk

[0067] Where, x n+1 It is the updated position of the particle; x n It is the position of the particle before the update; v n It is the velocity of the particle before it is updated, v n The initial value is 0; k is the interference from the waves. The unmanned sailboat is smaller in size and mass than a regular unmanned boat, so the interference is more obvious; r is a random number of 0 or 1.

[0068] Step 4.2, calculate the fitness function of the particle's position before the update and the fitness function of the particle's position after the update, from x n and x n+1 The position with the minimum fitness function is selected as the optimal position p of the individual. best The position of the currently updated particle is replaced with the individual optimal position; the path is based on the composition of each particle, and the path is updated along with the particle positions; then, the fitness function of each particle on the updated path is calculated, and the position of the particle with the smallest fitness function is selected as the global optimal position g. best .

[0069] Step 4.3: After obtaining the global optimal position, use probability pa to select whether to perform a mutation operation on the individual optimal position: pa is a random number uniformly distributed between [0, 1]; when pa ≤ 0.5, no operation is required; when pa > 0.5, a mutation operation is performed on the individual optimal position; the mutated individual optimal position p' best =p best +Δm, where Δm follows a function with mean zero and variance σm. 2 The Gaussian distribution is denoted as Δm~(0,σm) 2 Then use the optimal position p' of the mutated individual. best Replace the original individual's optimal position p best Update the particle velocity.

[0070] Step 4.4: After performing a mutation operation on the optimal position of an individual, select particles on the path that have already completed position updates with a certain probability (pb). Place these selected particles into the mutation pool for mutation, while leaving the unselected particles unchanged. If the number of particles is greater than 1, an even number of particles are selected. Then, pair the selected particles together in a front-to-back order, mutating the position of each pair. Replace the original particles with the mutated ones, and simultaneously delete the original particle pairs from the mutation pool. For example... Figure 6 As shown, the specific mutation process of the particle pair is as follows: Assume X1 and X2 represent the two particles undergoing the mutation operation, and X1' and X2' represent the particles after the mutation operation. The mathematical expression for mutation is:

[0071]

[0072] In the formula, p c It is a random number that is uniformly distributed between [0, 1].

[0073] The particle paths are updated based on the mutated particles, and then the fitness function of each particle on the path is recalculated. The position of the particle with the smallest fitness function is selected as the global optimal position g. best .

[0074] Step 4.5, finally, update the particle velocity. The mathematical expression for velocity update is:

[0075] v n+1 =ωv n +c1r1(p best -x n )+c2r2(g best -x n )

[0076] Among them, v n+1 It is the velocity of the particle after the update; the velocities before and after the update are both the resultant velocity V. compose (Q, t); ω is the weight coefficient; c1 and c2 are learning factors; r1 and r2 are random numbers; p best It is the optimal position for an individual; g best It is the globally optimal position.

[0077] After updating the particles on the path in front-to-back order once, update them again in front-to-back order, and so on until the maximum number of iterations is reached. The global optimal path is obtained based on the velocity and position of the last updated particles.

[0078] Step 5: Fit each path point (particle) in the global optimal path with a Bézier curve to obtain the final smooth planned path.

[0079] Example 2

[0080] A global path planning system for unmanned sailboats, such as Figure 7 As shown, an unmanned sailboat global path planning system includes an environment model building module, which is used to build the global environment in which the unmanned sailboat operates. The environment model building module is connected to a wind field model building module, which is used to build a wind field model of the ocean winds.

[0081] The environment model building module and the wind field model building module are connected to the fitness function calculation module. The fitness function calculation module is used to build a fitness function model based on the particle's velocity, path length, wind field model, and turning information.

[0082] The environment model building module and fitness function calculation module are connected to the particle swarm optimization (PSO) module. The PSO module generates a particle swarm based on the global environment and selects particles from the swarm to form the initial path of the unmanned sailboat, completing the initialization. The PSO module also updates particle positions, selects the optimal individual positions and the global optimal position on the path based on the fitness function model, randomly mutates the optimal individual positions, and then randomly mutates the updated particles on the path. From the mutated path, it selects the global optimal position and updates the particle velocity based on the mutated optimal individual and global optimal positions to obtain the globally optimal path.

[0083] The particle swarm path planning module is connected to a Bézier curve fitting module, which is used to fit Bézier curves to particles on the globally optimal path to obtain a smooth planned path.

Claims

1. A method for global path planning of an unmanned sailboat, characterized in that, Includes the following steps: Step 1: Establish an environment model based on the starting position, the position of static obstacles, and the target position; Step 2: Obtain the particle swarm and initial path based on the environment model; then update the position and velocity of the particles on the path in order from front to back, update the path based on the updated velocity and position, and finally iterate to find the globally optimal path. Step 3: Fit the particles on the global optimal path with a Bézier curve to obtain a smooth planned path; In step 2, the mathematical expression for position update is: wherein, is the position of the particle after update; is the position of the particle before update; is the velocity of the particle before update, has an initial value of 0; is the disturbance from the sea waves; is a random number of 0 or 1; The mathematical expression for the speed update is: ,in, It is the velocity of the particles after the update; These are weighting coefficients; , It is a learning factor; , It is a random number; The optimal position for the individual; It is the globally optimal position; In step 2, the method for obtaining the individual optimal position is as follows: calculate the fitness function of the particle's position before the update and the fitness function of the particle's position after the update, and select the position with the smallest fitness function as the individual optimal position; the method for obtaining the global optimal position is as follows: replace the current particle's position with the individual optimal position and update the path; then calculate the fitness function of each particle on the path, and select the position with the smallest fitness function as the global optimal position. In step 2, the expression for the fitness function is: ,in, It is the cost function of distance; It is the cost function of wind; It is the cost function of turning; ,in, It is the length of the path; if the unmanned sailboat collides with the obstacle, then If it is 0, otherwise, =1; ,in, It is the weighting coefficient for wind direction cost; It is the wind direction angle; It is the heading angle; ,in, It is the weighting coefficient for the turning cost; It is the distance weighting coefficient; It is the current orientation angle of the unmanned sailboat relative to the target position.

2. The global path planning method for unmanned sailboats as described in claim 1, characterized in that, In step 2, after obtaining the global optimal position, probability is used. To choose whether to perform a mutation operation on the optimal position of an individual: It is a random number uniformly distributed between [0, 1]. When the optimal position of an individual is mutated, the original optimal position of the individual is replaced with the mutated optimal position to update the particle's velocity and path.

3. The global path planning method for unmanned sailboats as described in claim 2, characterized in that, In step 2, the optimal position of the mutated individual ,in It follows a pattern with a mean of zero and a variance of . The Gaussian distribution of is denoted as . .

4. The global path planning method for unmanned sailboats as described in claim 3, characterized in that, In step 2, after performing a mutation operation on the individual optimal position, randomly select particles that have completed position updates and put them into the mutation pool for mutation. If the number of particles is greater than 1, the number of selected particles is even. Then, the selected particles are grouped into particle pairs in the order from front to back, and the position of each particle pair is mutated. The mutated particles are then used to replace the original particle update path. Finally, the global optimal position is calculated, and the velocity of the particles is updated.

5. The global path planning method for unmanned sailboats as described in claim 4, characterized in that, In step 2, the mutation method for the particle pairs is as follows: In the formula, and For two particles in a particle pair; It is a random number that is uniformly distributed between [0, 1]. and These are the mutated particles.

6. The global path planning method for unmanned sailboats as described in claim 5, characterized in that, In step 2, the particle's velocity is the resultant velocity of the unmanned sailboat's velocity and the wind field's velocity.

7. A global path planning system for an unmanned sailboat employing the method described in any one of claims 1-6, characterized in that, The system includes an environment model building module for establishing the global environment of the unmanned sailboat; a wind field model building module connected to the environment model building module for establishing a wind field model of ocean winds; a fitness function calculation module connected to the environment model building module and the wind field model building module for establishing a fitness function model based on particle velocity, path length, wind field model, and turning information; a particle swarm path planning module connected to the environment model building module and the fitness function calculation module for calculating the initial path of the unmanned sailboat based on the global environment; the particle swarm path planning module for updating particle positions, selecting individual optimal positions and global optimal positions on the path based on the fitness function model, performing mutation operations on particles on the path, updating particle velocities based on individual optimal positions and global optimal positions to obtain the global optimal path; and a Bézier curve fitting module connected to the particle swarm path fitting module for fitting Bézier curves to particles on the global optimal path to obtain a smooth planned path.

Images