CN115407768A - Underwater robot marine organism efficient fishing path planning method - Google Patents
Underwater robot marine organism efficient fishing path planning method Download PDFInfo
- Publication number
- CN115407768A CN115407768A CN202210920410.2A CN202210920410A CN115407768A CN 115407768 A CN115407768 A CN 115407768A CN 202210920410 A CN202210920410 A CN 202210920410A CN 115407768 A CN115407768 A CN 115407768A
- Authority
- CN
- China
- Prior art keywords
- cost
- robot
- path
- fishing
- underwater
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 239000012636 effector Substances 0.000 claims abstract description 31
- 238000004364 calculation method Methods 0.000 claims abstract description 17
- 230000006870 function Effects 0.000 claims description 31
- 230000008569 process Effects 0.000 claims description 16
- 239000000126 substance Substances 0.000 claims description 7
- 238000013528 artificial neural network Methods 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 3
- 238000011176 pooling Methods 0.000 claims description 3
- 230000002787 reinforcement Effects 0.000 claims description 2
- 230000009286 beneficial effect Effects 0.000 abstract description 2
- 230000009471 action Effects 0.000 description 12
- 238000004088 simulation Methods 0.000 description 6
- 238000004458 analytical method Methods 0.000 description 3
- 238000007728 cost analysis Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000003116 impacting effect Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012502 risk assessment Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
The invention provides a path planning method for efficient marine organism fishing of an underwater robot, and belongs to the technical field of path planning. The invention provides an efficient fishing path planning strategy suitable for an underwater environment, which specifically comprises the following steps: firstly, defining fishing loss cost comprising path cost, turning cost and motion cost of a mechanical arm; then defining a collision risk cost consisting of the collision risk of the robot hull and the collision risk of the end effector, and related to the speed of the collision; and finally, establishing a value iterative network, inputting an underwater cost map obtained based on cost calculation, and calculating the average grabbing cost in a two-dimensional discrete environment to obtain underwater information which is beneficial to improving the grabbing task efficiency, so that the efficient fishing path planning of marine organisms of the underwater robot is realized.
Description
Technical Field
The invention belongs to the technical field of path planning, and particularly relates to a path planning method for efficient marine organism fishing of an underwater robot.
Background
With the vigorous development of our country on ocean resources, the demand of people on marine products is increasing day by day, the tendency that an underwater fishing robot replaces a diver for fishing is more obvious, the threat of large underwater pressure and low temperature to the life health of the diver can be reduced, and the underwater grabbing efficiency is also improved. The fishing efficiency of the current underwater robot is far from the difference of human beings, wherein the path of underwater fishing operation is important for improving the fishing efficiency of the fishing robot. The underwater fishing environment faces complex marine organism distribution and obstacle conditions, the traditional underwater robot path planning method is difficult to apply, and efficient machine fishing can be realized only by analyzing underwater target distribution, the position relation of a robot and an obstacle and ocean current conditions.
The patent No. 201811521156.9, which discloses a method for parallel operation of an autonomous underwater robot and geological sampling equipment, provides a method for parallel operation of an autonomous underwater robot and geological sampling equipment. Mainly through the detection to the sample to the full coverage and the location parallel operation of sample scope are carried out, are difficult to combine the distribution and the position that marine organism grows to improve and catch efficiency. Based on the problems, a marine organism efficient catching path planning method for the underwater robot is provided to improve the efficiency of marine organism catching path planning of the underwater robot.
Disclosure of Invention
The invention aims to provide a method for planning a marine organism efficient fishing path of an underwater robot.
The purpose of the invention is realized by the following technical scheme:
a marine organism efficient fishing path planning method for an underwater robot specifically comprises the following steps:
the method comprises the following steps: calculating the fishing loss cost;
defining the fishing loss cost of the underwater robot, wherein the fishing loss cost of the underwater robot consists of the path movement cost of the robot moving to a destination, the bow turning cost in the movement process and the movement consumption cost of a mechanical arm for grabbing at the base bottom near a grabbing target;
step two: calculating collision risk cost;
defining collision risk cost of the underwater robot, wherein the collision risk cost consists of the collision risk of a robot boat body and the collision risk of an end effector, and the collision risk is related to the speed of collision; for a scene with multiple obstacles, calculating the collision risk of the robot on the hull and the end effector of each obstacle;
step three: adopting a value iterative network to realize path planning based on cost calculation;
according to the underwater environment, solving the collision risk cost, and assigning a value to the risk degree of each position of the value iteration network input map to obtain an underwater cost map; and inputting an underwater cost map in a value iterative network to obtain a planned path, calculating the motion consumption cost in a two-dimensional discrete environment, evaluating the grabbing difficulty degree of each task point, obtaining underwater information for improving the grabbing task efficiency, and obtaining the fishing path plan of marine organisms of the underwater robot.
The present invention may further comprise:
1. in the first step, path motion cost, bow turning cost and motion consumption cost of the mechanical arm during grabbing are defined as follows:
(1) Path motion cost f s The definition of (a) is expressed as follows:
wherein the content of the first and second substances,
which is indicative of the speed of the robot,
representing the speed, t, of the ocean current s And t f Representing the start time and the end time of the movement;
(2) Cost f of turning bow t The definition of (a) is expressed as follows:
wherein the heading angle is the angle before the change
The angle of the change of the heading angle is stopped
The bow-turning cost f in the motion process t Is the sum of changes of the heading angle;
(3) The motion consumption cost of the mechanical arm during grabbing is as follows:
wherein, theta is Indicating the angle at which the i-th joint begins to change, θ if Angle, f, representing the i-th joint not changing after a time-off a Representing the motion cost of the rotation of each joint;
motion loss cost formula f pick Can be expressed as:
wherein v is c Representing the magnitude of the flow rate, C α4 Represents the biological density coefficient, f s 、f t 、f a Respectively representing the path consumption cost, the turning consumption cost, the mechanical arm movement cost and C of the fishing robot in the moving process when the target is grabbed α1 、C α2 、C α3 Respectively representThe distance coefficient, the bow turning coefficient and the rotation coefficient of the mechanical arm movement of the fishing robot are obtained when the target is grabbed.
2. In the second step, the collision risk of the robot hull, the collision risk of the end effector, and the correlation between the collision risk and the collision speed are defined:
(1) Defining the collision risk cost of the robot hull at a task point, namely multiplying the speed by a risk function, and integrating in the grabbing time; the collision risk cost of the robot hull is:
A. defining a safety distance epsilon v The expression of (c) is as follows:
wherein l rob For the length of the fishing robot, k 1 To a safety factor, v c Is the flow rate of the operating area;
B. defining a collision risk function
The following were used:
wherein, the first and the second end of the pipe are connected with each other,
is the robot-obstacle distance;
C. defining the speed v of the impact of the hull body As follows:
wherein p is n Is the position of the robot, (v) x ，v y ，v z ) Is the speed of the collision;
(2) Defining the collision risk cost of the robot end effector at a task point, wherein the collision risk cost is obtained by multiplying the speed by a risk function and integrating in the grabbing time; the collision risk penalty of the robot end effector at the task point is:
A. defining an end effector catch Risk function
Comprises the following steps:
wherein the content of the first and second substances,
to capture the distance between the end-effector and the obstacle, d h Is the height of the obstacle, epsilon e For a safe distance between the fishing target and the obstacle, k 1 ，k 2 Is the undetermined coefficient;
B. velocity v of impact of end effector e The expression is as follows:
wherein, the first and the second end of the pipe are connected with each other,
speed of impact of end-effector, p n Is the position of the robot end effector;
(3) Defining a collision risk cost formula of the robot as follows:
wherein the content of the first and second substances,
as a function of the risk of the robot hull,
is a risk function of the end effector, v body Projection of the velocity of the robot hull in the direction of impact with an obstacle, v e Is the projection of the velocity in the direction of the end effector impacting the obstacle.
3. In the third step, a path planning method based on cost calculation is provided to realize marine organism fishing path planning of the underwater robot, and the path planning process based on the underwater cost calculation is as follows:
(1) Generating a cost map;
the m × m continuous environment is divided into 1 × 1 grid environments with each grid, and each location (i, j) in the grid environment has only two states: the method comprises the steps that with obstacles and without obstacles, under the condition that the distance between a fishing robot and the obstacles is within a safe range, a safe distance is calculated according to the flow velocity, according to the states of eight grids around, collision risk cost basis of each grid environment is calculated through a collision risk cost function and the speed component of the robot in the direction where collision is likely to occur, and the collision risk cost basis is used as a cost value of an environment map under a two-dimensional discrete environment;
(2) Establishing a value iterative network to solve the problem of path planning;
establishing a value iteration network: embedding a value iteration module in a neural network, associating convolution operation and pooling operation of the neural network with a value iteration algorithm in reinforcement learning, endowing the neural network with a planning function, and describing the variable by using a state value function:
v(s)＝E[R t+1 +γ v (S t+1 )|S t ＝s]
wherein γ is a discount coefficient;
the optimal strategy is found in the following way, so that the value of the total decision sequence is maximized:
inputting an underwater cost map based on cost calculation in a value iterative network to obtain a planned path;
(3) Calculating average grabbing cost;
the average grabbing cost τ s is calculated as follows:
wherein q is sum Representing the sum of states, f pick Represents the exercise expenditure cost, num i Indicating the target number of task points, mu 1 And mu 2 Respectively representing the value and the weight of the exercise consumption cost;
cost of exercise consumption f pick Solving through the difference between the actual planning path and the effective motion path; the path difference item is obtained by calculating the difference between the path length planned under the condition of no obstacle and the path length planned under the condition of an obstacle; calculating a difference value between a bow turning angle of a planned path without an obstacle and a bow turning angle under the condition that the obstacle exists to obtain a bow turning angle;
τ s and evaluating the path condition of the task points and the difficulty degree of grabbing, and selecting an optimal grabbing path.
The invention has the beneficial effects that:
aiming at the requirement of efficient path planning, the invention provides a path planning method based on cost calculation, which is characterized in that a collision risk analysis of underwater capture is carried out, a calculation formula of collision risk cost is designed from multiple angles by considering the distance between a robot and an obstacle, the state of the robot and the number of the obstacles, and the collision risk is evaluated. From the underwater motion efficiency, the underwater fishing consumption cost is calculated on the three aspects of the motion path cost, the bow turning cost and the mechanical arm motion cost by analyzing the underwater motion path of the robot, and the influence of ocean current on the motion speed and bow turning of the underwater robot is considered. And calculating the risk cost in the underwater environment by using the collision risk cost to obtain a cost map, and replacing the original map as input to provide more prior information. By designing a cost calculation-based path planning simulation experiment, path planning is carried out by comparing input different map use value iterative networks, cost-based planning is more prone to selecting actions with low risk cost in decision making, the cost of fishing consumption is reduced by keeping away from obstacles, the average grabbing cost is lower than that of planning based on an original value iterative network, and the efficient marine organism fishing path planning of the underwater robot is realized.
Drawings
FIG. 1 is a graph of path motion cost analysis according to the present invention;
FIG. 2 is a collision cost analysis diagram of the present invention;
FIG. 3 is a schematic view of the multi-obstacle collision risk of the present invention;
FIG. 4 is a value iteration network structure of the present invention;
FIG. 5 is a schematic diagram of calculating a collision risk cost value according to the present invention;
FIG. 6 is a flow chart of path planning in accordance with the present invention;
FIG. 7 is a value iteration path planning result of the present invention;
fig. 8 shows the result of path planning for multiple working points of the robot according to the present invention.
Detailed Description
The invention is further described below with reference to the accompanying drawings.
A method for planning a marine organism efficient fishing path of an underwater robot is characterized by being suitable for the method for planning the marine organism efficient fishing path of the underwater robot, and specifically comprising the following steps:
the fishing loss cost of the underwater robot is defined to be composed of the path cost of the robot moving to the destination, the turning cost in the moving process and the movement cost of the grabbing mechanical arm when the underwater robot reaches the base near the grabbing target.
The ocean current velocity can be represented to influence the robot speed through making a difference between the robot hull speed and the ocean current velocity and calculating the magnitude, and the path cost of the ocean current on the movement influence of the robot can be represented through integral in the whole movement process. Path motion cost f s The definition of (a) is expressed as follows:
As shown in fig. 1, if the robot reaches the point n along the red dotted line without an obstacle, the robot needs to rotate counterclockwise by a certain angle to reach the designated pose, and if the robot moves with an obstacle, the robot needs to turn forward clockwise by a certain angle and then continuously turn forward counterclockwise to reach the final pose. According to a yaw moment equation in a submarine six-degree-of-freedom equation, calculating the integral of yaw moment continuously changed in the motion process of the robot in the motion process to serve as the heading cost of the robot, wherein the heading cost f t The definition of (c) is expressed as follows:
where ρ represents the density of the execution water area, L represents the length of the robot hull, N' r′ And N' r|r| And r is an angular velocity component around the z axis when the robot performs bow turning motion.
The robot approaches according to the target after reaching the position near the obstacle, so that the target is in the range capable of being grabbed by the robot, when the robot grabs, the mechanical arm has catching consumption cost, the targets at different positions can lead to different motion processes of the mechanical arm, and similarly, under the condition that the obstacle exists, the mechanical arm needs to bypass the obstacle to grab the target. The complexity of the grasping is described by the sum of the rotation angles of each joint of the mechanical arm. Taking a three-joint mechanical arm as an example, from a contracted state to a gripping state of the mechanical arm, theta is Indicates the angle at which the ith joint begins to change, θ if The angle of the ith joint which does not change after changing for a certain time is shown, and the movement cost of the rotation of each joint is f a Then, the motion consumption cost of the mechanical arm during grabbing can be obtained as follows:
from the above analysis, the motion loss cost formula f pick Can be expressed as:
the cost of consumption is generated in the motion process of the robot, wherein C α4 Representing the biological density coefficient, and is proportional to the number of target organisms in a certain direction in the region, when C α4 The cost of consumption for the entire path is reduced when larger. f. of s ，f t ，f a The path consumption cost, the turning consumption cost and the mechanical arm movement cost of the fishing robot in the moving process during target grabbing are represented respectively. C α1 ， C α2 And C α3 Respectively representing the distance coefficient, the turning coefficient and the rotation coefficient of the mechanical arm movement of the fishing robot when the target is grabbed.
Defining the collision risk cost of an underwater robot consists of the collision risk of the robot hull and the collision risk of the end effector and is related to the speed of the collision. For a scene of a plurality of obstacles, calculating the collision risk of the robot on the hull and the end effector of each obstacle;
the required distance for different types of obstacles is the minimum distance between the robot and the obstacle, the situation of the figure 2 is taken as an example to show the state that the robot grabs near the obstacle, a green envelope ring represents a task point, the robot moves near the obstacle to realize grabbing at a plurality of task points, the obstacle and the robot boat body are represented by red minimum envelope circles, and the radii are R respectively rob And R obs The distance between the two circle centers is d 0 Then the distance between the robot and the obstacle is defined as d obs ：
d obs ＝d 0 -R obs -R rob
From the viewpoint of safety, it is considered that v Within is a risk, falling to 0 when out of range, and within a safe range the risk is greater when the distance is smaller. The safety distance is the flow velocity v from the working area c In relation to the above, when the flow velocity is increased, the safety distance is exponentially increased, and according to the actual underwater experiment of the fishing robot, when the flow velocity exceeds 0.6m/s, the robot cannot normally move, so that the robot is considered to move at v c The flow velocity is within 0-0.6m/s, the safe distance is increased linearly along with the increase of the flow velocity under the condition that the robot can move normally, and the length l of the fishing robot is different rob Can be based on the parameter k 1 Adjustment of safety distance, k 1 The safety distance is controlled for the safety factor. Safety distance epsilon v The expression of (a) is as follows:
collision risk function
When the distance between the robot and the obstacle is less than the safety distance, the safety distance is usedThe distance minus the square of the robot-obstacle distance difference represents the degree of risk and divided by the safe distance, which if larger, represents that the risk of collision is smaller.
When the robot-obstacle distance d v obs When the distance is smaller, the difference value between the distance and the safety distance is larger, the collision risk is increased, and when the robot collides with an obstacle, d v obs Is 0, the collision risk function reaches a maximum of ε v When the distance d is v obs The greater the risk of collision, the minimum is 0. The collision risk is also inversely proportional to the safe distance, with the reward for collision risk decreasing when the safe distance is greater and increasing otherwise.
The collision risk is also related to the collision speed, when the speed of the speed in the direction of connecting the two enveloping circle centers is larger, the collision risk cost is larger, and the collision speed (v) is assumed to be that an obstacle collided with the robot is fixed x ，v y ，v z ) Can be measured and projected by a Doppler velocimeter equipped with the fishing robot, the speed of the robot is considered as an approaching speed due to estimation, and the speed is projected to the direction of the collision of the robot with the obstacle, and the expression v of the speed is body As follows:
and finally, multiplying the speed by a risk function, and integrating in the capturing time to obtain the collision risk cost at the task point. The collision risk penalty of the robot hull is:
in a similar way, when the paw executes the grabbing task, collision risk cost can be generated, and the catching risk function of the paw is as follows:
to work out the cost function of the working space in combination with the situation of obstacles in the working environment, it is assumed that the obstacles in the fishing environment are all static, passing through the spatial distance
Representing the distance between the end effector and the obstacle. d h Representing the height of the obstacle. Epsilon e The safe distance between the fishing target and the obstacle is represented, because the robot lands on the seabed during operation, the robot boat body is not influenced by the flow velocity of ocean currents, the end effector can be considered to be not influenced by the ocean currents and is a fixed value, the length of the mechanical arm can be generally considered, the risk of collision between the mechanical arm and the obstacle exists only within the safe distance, k 1 ，k 2 The undetermined coefficient can be set as the most suitable parameter according to specific situations.
The risk of collision is also related to the speed of collision, and when the speed of the speed in the direction of connecting the centers of the two enveloping circles is higher, the risk cost of collision is higher, and the speed of collision (v) e x ，v e y ，v e z ) Can be measured and projected by the catching robot, and the velocity of the end grab is considered as a constant value due to estimation, and the velocity is projected to the direction of the collision of the robot with the obstacle, and the expression v thereof e As follows:
and finally, multiplying the speed by a risk function, and integrating in the capturing time to obtain the collision risk cost at the task point. The collision risk penalty of the robot hull is:
in summary, the collision risk cost formula of the robot is as follows:
When the fishing robot grabs, a scene with a plurality of obstacles exists nearby, and the collision risk of the robot on the hull and the paw of each obstacle needs to be calculated respectively, which means that the collision risk increases with the number of obstacles. Assuming that num obstacles, i = {1,2.., num } exist around the time of fishing, the total collision risk penalty for the robot is:
f obs ＝∑f i obs
through the process analysis of grabbing the operation to underwater robot execution, catch consumption cost and collision risk cost to the robot and carry out the analysis, to sum up, the robot is grabbing total cost under water in the executive task in-process:
f＝f pick +f obs
f obs ＝f body +f effector
the structure of the value iterative network is shown in figure 4, when the value iterative network is used for solving the problem of path planning, the first step is to calculate a state action cost function q l+1 (s, a), namely performing convolution operation on the input image at a state s, wherein the number of channels is equal to the number of actions a in the action space to obtain a state action value function corresponding to the actions, and the second step of value iteration is to find out an optimal state value function v through a greedy strategy l+1 (s), which can be regarded as a one-step pooling operation of the obtained state cost function in the state s, and the two operations are expressed by the following formula:
q l+1 (s, a) -State action value function representing the l +1 th round of State s in an iteration taking action a
Gamma-represents the discount coefficient
v l+1 (s) -represents the 1 st +1 st round state s state value function in an iteration
After iteration of k rounds, a final state action value function can be obtained, an optimal strategy is obtained through selection, and the optimal strategy is expressed by the following formula:
dividing the continuous m × m environment into 1 × 1 grid environments, considering that each position (i, j) in the grid environment is only oneTwo states: and respectively calculating collision risk cost of each grid environment with obstacles and without obstacles, wherein the risk cost value of each grid can be calculated according to the states of eight grids around the grid, and the risk cost value can be used as the cost value of the environment map in the two-dimensional discrete environment. Under the condition that the distance between the fishing robot and the obstacle is within a safe range, a safe distance is calculated according to the flow velocity, and collision risk cost is calculated through a collision risk cost function and the velocity component of the robot on the direction where collision is likely to occur, so that a cost map is generated. As shown in fig. 5, the green triangle is the current position, and the risk cost value corresponding to the two-dimensional discrete grid corresponding to the green triangle is the collision risk cost calculated for the case where two black obstacles are combined with the ocean current
And (3) the sum:
f obs ＝∑f i obs
and finally, calculating the average grabbing cost. The average grabbing cost τ s is calculated as follows:
wherein q is sum Representing the sum of states, f pick Represents the exercise expenditure cost, num i Indicating the target number of task points, mu 1 And mu 2 Representing the value and the weight of the cost of the exercise consumption, respectively.
Cost of exercise consumption f pick The solution is made by the difference between the actual planned path and the effective motion path. The path difference item is obtained by calculating the difference between the path length planned under the condition of no obstacle and the path length planned under the condition of an obstacle; the heading turning angle is obtained by calculating the difference between the heading turning angle of the planned path when no obstacle exists and the heading turning angle under the condition that the obstacle exists. By τ s And evaluating the path condition of the task points and the difficulty degree of grabbing, and selecting the optimal grabbing path to realize the high-efficiency marine life catching path planning of the underwater robot.
As shown in fig. 6, in the path planning based on cost calculation, firstly, an obtained underwater cost map is input in a network, secondly, reward images are designed according to different task requirements, a state-action value function of each point is calculated, a planned path based on a value iteration network is obtained, finally, the movement consumption cost under a two-dimensional discrete environment is calculated, the grabbing difficulty degree of each task point is evaluated, the optimal action is selected, a final path is output, and the efficient fishing path planning of marine organisms of the underwater robot is realized.
And simulating the path planning based on the underwater cost calculation, calculating the average grabbing cost according to the simulation result, and judging whether the path planned by the proposed algorithm can improve the safety of the path and reduce the average grabbing cost. In the simulation environment, on a 28m × 28m grid map, (i, j) represents the corresponding position of the grid, the starting point is at the upper left corner of the grid map, the end point is the task point for grabbing, and 30 grabbed objects exist. The planning method is to improve value iterative network, and the input network is an original map and a cost map subjected to cost calculation respectively.
TABLE 1 value index for different input categories
As can be seen from the simulation, according to the planning result shown in fig. 5, the planned path (a) of the value iteration network based on the cost map as the input and the path (b) of the value iteration network based on the original input are more inclined to be far away from the obstacle and are shorter, so that the safety and the fishing efficiency of the robot in motion are improved.
For the path planning simulation of multiple operation points, the simulation environment is that the fishing robot catches 8 targets, the positions of the task points and the target number of the task points are known, and the positions of the fishing robot are known. The results obtained using the path planning method based on the value iterative network are shown in FIG. 8
In conclusion, the average grabbing cost of the path based on the cost calculation planning is less than that of the path based on the original value iterative network, because the cost-based planning is more prone to selecting actions with low risk cost during decision making, the fishing consumption cost is reduced due to the fact that the cost-based planning is far away from obstacles, and the path planning is more suitable for planning the path for executing the underwater grabbing task after the underwater collision risk and the risk of motion consumption are considered.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (4)
1. A marine organism efficient fishing path planning method for an underwater robot is characterized by comprising the following steps: the method specifically comprises the following steps:
the method comprises the following steps: calculating the fishing loss cost;
defining the fishing loss cost of the underwater robot, wherein the fishing loss cost of the underwater robot consists of the path movement cost of the robot moving to a destination, the turning cost in the movement process and the movement consumption cost of a mechanical arm for grabbing at the base near a grabbing target;
step two: calculating collision risk cost;
defining a collision risk cost of the underwater robot to be composed of a collision risk of the robot hull and a collision risk of the end effector, and the collision risks are related to a speed of the collision; for a scene with multiple obstacles, calculating the collision risk of the robot on the hull and the end effector of each obstacle;
step three: adopting a value iterative network to realize path planning based on cost calculation;
according to the underwater environment, solving the collision risk cost, and assigning a value to the risk degree of each position of the value iteration network input map to obtain an underwater cost map; and inputting an underwater cost map in a value iterative network to obtain a planned path, calculating the motion consumption cost in a two-dimensional discrete environment, evaluating the grabbing difficulty degree of each task point, obtaining underwater information for improving the grabbing task efficiency, and obtaining the fishing path plan of marine organisms of the underwater robot.
2. The underwater robot marine organism efficient fishing path planning method of claim 1, characterized in that: in the first step, path motion cost, bow turning cost and motion consumption cost of the mechanical arm during grabbing are defined as follows:
(1) Path motion cost f s The definition of (a) is expressed as follows:
wherein, the first and the second end of the pipe are connected with each other,
which is indicative of the speed of the robot,
representing the speed of the ocean current, t s And t f Representing the start time and the end time of the movement;
(2) Cost f of bow turning t The definition of (c) is expressed as follows:
wherein the heading angle is the angle before the change
The angle at which the heading angle stops changing is
The bow-turning cost f in the motion process t Is the sum of changes of the heading angle;
(3) The motion consumption cost of the mechanical arm during grabbing is as follows:
wherein, theta is Indicates the angle at which the ith joint begins to change, θ if Angle, f, representing the i-th joint not changing after a time of change a Representing the motion cost of the rotation of each joint;
motion loss cost formula f pick Can be expressed as:
wherein v is c Representing the magnitude of the flow rate, C α4 Represents the biological density coefficient, f s 、f t 、f a Respectively representing the path consumption cost, the turning consumption cost, the mechanical arm movement cost and C of the fishing robot in the moving process when the target is grabbed α1 、C α2 、C α3 Respectively representing the distance coefficient, the turning coefficient and the rotation coefficient of the mechanical arm movement of the fishing robot when the target is grabbed.
3. The underwater robot marine organism efficient fishing path planning method of claim 1, characterized in that: in the second step, the collision risk of the robot hull, the collision risk of the end effector, and the correlation between the collision risk and the collision speed are defined:
(1) Defining the collision risk cost of the robot hull at a task point, namely multiplying the speed by a risk function, and integrating in the grabbing time; the collision risk penalty of the robot hull is:
A. defining a safety distance epsilon v The expression of (a) is as follows:
wherein l rob For the length of the fishing robot, k 1 To a safety factor, v c Is the flow rate of the work area;
B. defining a collision risk function
The following were used:
wherein the content of the first and second substances,
is the robot-obstacle distance;
C. defining the speed v of the impact of the hull body As follows:
wherein p is n Is the position of the robot, (v) x ,v y ,v z ) Is the speed of the collision;
(2) Defining the collision risk cost of the robot end effector at a task point, wherein the collision risk cost is obtained by multiplying the speed by a risk function and integrating in the grabbing time; the collision risk penalty of the robot end effector at the task point is:
A. defining a catch risk function for an end effector
Comprises the following steps:
wherein the content of the first and second substances,
to catch the distance between the end-effector and the obstacle, d h Is the height of the obstacle, epsilon e For a safe distance between the fishing target and the obstacle, k 1 ，k 2 Is a undetermined coefficient;
B. velocity v of impact of end effector e The expression is as follows:
wherein the content of the first and second substances,
speed of impact of end-effector, p n Is the position of the robot end effector;
(3) Defining a collision risk cost formula of the robot as follows:
wherein the content of the first and second substances,
as a function of the risk of the robot hull,
is a risk function of the end effector, v body Projection of the velocity of the robot hull in the direction of impact with an obstacle, v e For the end to holdThe speed projection in the direction of the line impact on the obstacle.
4. The underwater robot marine organism efficient fishing path planning method of claim 1, characterized in that: in the third step, a path planning method based on cost calculation is provided to realize marine organism fishing path planning of the underwater robot, and the path planning process based on the underwater cost calculation is as follows:
(1) Generating a cost map;
the m × m continuous environment is divided into 1 × 1 grid environments with each grid, and each location (i, j) in the grid environment has only two states: the method comprises the steps that with obstacles and without obstacles, under the condition that the distance between a fishing robot and the obstacles is within a safe range, a safe distance is calculated according to the flow velocity, according to the states of eight grids around, collision risk cost basis of each grid environment is calculated through a collision risk cost function and the speed component of the robot in the direction where collision is likely to occur, and the collision risk cost basis is used as a cost value of an environment map under a two-dimensional discrete environment;
(2) Establishing a value iterative network to solve the problem of path planning;
establishing a value iteration network: embedding a value iteration module in a neural network, associating convolution operation and pooling operation of the neural network with a value iteration algorithm in reinforcement learning, endowing the neural network with a planning function, and describing the variable by using a state value function:
v(s)＝E[R t+1 +γv(S t+1 )|S t ＝s]
wherein γ is the discount coefficient;
the optimal strategy is found in the following way, so that the value of the total decision sequence is maximum:
inputting an underwater cost map based on cost calculation in a value iterative network to obtain a planned path;
(3) Calculating average grabbing cost;
average grab cost τ s The calculation formula of (a) is as follows:
wherein q is sum Represents a state value sum, f pick Represents the exercise expenditure cost, num i Indicating the target number of task points, mu 1 And mu 2 Respectively representing the value and the weight of the exercise consumption cost;
cost of exercise consumption f pick Solving through the difference between the actual planning path and the effective motion path; the path difference item is obtained by calculating the difference between the path length planned under the condition of no obstacle and the path length planned under the condition of an obstacle; calculating a difference value between a bow-turning angle of a planned path without obstacles and a bow-turning angle under the condition of the existence of the obstacles to obtain a bow-turning angle;
τ s and evaluating the path condition of the task points and the difficulty degree of grabbing, and selecting the optimal grabbing path.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210920410.2A CN115407768B (en) | 2022-08-02 | 2022-08-02 | Underwater robot marine organism efficient catching path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210920410.2A CN115407768B (en) | 2022-08-02 | 2022-08-02 | Underwater robot marine organism efficient catching path planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115407768A true CN115407768A (en) | 2022-11-29 |
| CN115407768B CN115407768B (en) | 2023-12-12 |
Family
ID=84158737
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210920410.2A Active CN115407768B (en) | 2022-08-02 | 2022-08-02 | Underwater robot marine organism efficient catching path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115407768B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08202445A (en) * | 1995-01-20 | 1996-08-09 | Mitsubishi Heavy Ind Ltd | Controller of autonomous underwater robot |
| CN108281043A (en) * | 2018-03-23 | 2018-07-13 | 上海海事大学 | A kind of ship collision Warning System and method for early warning |
| CN109725650A (en) * | 2019-03-08 | 2019-05-07 | 哈尔滨工程大学 | A kind of AUV barrier-avoiding method under intensive obstacle environment |
| CN111951606A (en) * | 2020-07-29 | 2020-11-17 | 武汉理工大学 | Ship collision risk assessment and early warning method and system |
| CN112068576A (en) * | 2020-07-21 | 2020-12-11 | 华中科技大学 | Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method |
-
2022
- 2022-08-02 CN CN202210920410.2A patent/CN115407768B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08202445A (en) * | 1995-01-20 | 1996-08-09 | Mitsubishi Heavy Ind Ltd | Controller of autonomous underwater robot |
| CN108281043A (en) * | 2018-03-23 | 2018-07-13 | 上海海事大学 | A kind of ship collision Warning System and method for early warning |
| CN109725650A (en) * | 2019-03-08 | 2019-05-07 | 哈尔滨工程大学 | A kind of AUV barrier-avoiding method under intensive obstacle environment |
| CN112068576A (en) * | 2020-07-21 | 2020-12-11 | 华中科技大学 | Task-hierarchical timing optimization-based underwater unmanned ship-double mechanical arms cooperative control method |
| CN111951606A (en) * | 2020-07-29 | 2020-11-17 | 武汉理工大学 | Ship collision risk assessment and early warning method and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115407768B (en) | 2023-12-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6854549B2 (en) | AUV action planning and motion control methods based on reinforcement learning | |
| Ruan et al. | Mobile robot navigation based on deep reinforcement learning | |
| Zhu et al. | Task assignment and path planning of a multi-AUV system based on a Glasius bio-inspired self-organising map algorithm | |
| Wu et al. | Reinforcement learning and particle swarm optimization supporting real-time rescue assignments for multiple autonomous underwater vehicles | |
| CN109540151A (en) | A kind of AUV three-dimensional path planning method based on intensified learning | |
| CN113485371B (en) | Underwater multi-AUV path planning method based on improved sparrow search algorithm | |
| CN104794526A (en) | Automatic ship anti-collision method optimized by wolf colony search algorithm | |
| CN108196453A (en) | A kind of manipulator motion planning Swarm Intelligent Computation method | |
| CN113534668B (en) | Maximum entropy based AUV (autonomous Underwater vehicle) motion planning method for actor-critic framework | |
| Ji-Yong et al. | Design and vision based autonomous capture of sea organism with absorptive type remotely operated vehicle | |
| Sun et al. | A novel fuzzy control algorithm for three-dimensional AUV path planning based on sonar model | |
| Song et al. | Method of emergency collision avoidance for unmanned surface vehicle (USV) based on motion ability database | |
| Mina et al. | Maneuvering ability-based weighted potential field framework for multi-USV navigation, guidance, and control | |
| Pal et al. | Mobile robot navigation using a neural net | |
| CN117315025A (en) | Mechanical arm 6D pose grabbing method based on neural network | |
| Wang et al. | Adaptive environment modeling based reinforcement learning for collision avoidance in complex scenes | |
| Fu et al. | Multi-behavior fusion based potential field method for path planning of unmanned surface vessel | |
| Castaño et al. | Trajectory tracking control of rowing pectoral fin-actuated robotic fish | |
| CN115407768A (en) | Underwater robot marine organism efficient fishing path planning method | |
| CN116852347A (en) | State estimation and decision control method for non-cooperative target autonomous grabbing | |
| CN112124537B (en) | Intelligent control method for underwater robot for autonomous absorption and fishing of benthos | |
| Liu et al. | UUV path planning method based on QPSO | |
| CN115562313A (en) | Autonomous underwater vehicle motion control method for pier flaw detection | |
| Yang et al. | Research on dynamic path planning of auv based on forward looking sonar and fuzzy control | |
| Kim et al. | Simulation and feasibility test of mini-rovs with auv for the manipulation purpose |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |