CN117055586B - Underwater robot tour search and grabbing method and system based on self-adaptive control - Google Patents
Underwater robot tour search and grabbing method and system based on self-adaptive control Download PDFInfo
- Publication number
- CN117055586B CN117055586B CN202310778064.3A CN202310778064A CN117055586B CN 117055586 B CN117055586 B CN 117055586B CN 202310778064 A CN202310778064 A CN 202310778064A CN 117055586 B CN117055586 B CN 117055586B
- Authority
- CN
- China
- Prior art keywords
- current
- underwater robot
- state information
- control input
- actual state
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 80
- 230000008569 process Effects 0.000 claims abstract description 21
- 230000006870 function Effects 0.000 claims description 66
- 230000008859 change Effects 0.000 claims description 42
- 239000011159 matrix material Substances 0.000 claims description 32
- 230000003044 adaptive effect Effects 0.000 claims description 29
- 238000004590 computer program Methods 0.000 claims description 13
- 230000014509 gene expression Effects 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 239000000725 suspension Substances 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000013016 damping Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 16
- 238000004891 communication Methods 0.000 description 5
- 230000007547 defect Effects 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 239000000463 material Substances 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 229910052500 inorganic mineral Inorganic materials 0.000 description 1
- 239000011707 mineral Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013049 sediment Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a self-adaptive control-based underwater robot tour search and grabbing method and system, wherein the method comprises the following steps: acquiring current actual state information of the underwater robot, and determining a current error of the underwater robot based on current expected state information corresponding to a current search path of the underwater robot and the current actual state information; under the condition that the current error is larger than the preset error, optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information to obtain the optimized current control input; based on the optimized current control input, controlling the underwater robot to carry out tour search along the current search path, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process. The invention improves the autonomous searching and operating capacity of the underwater robot for the target object in the unknown area.
Description
Technical Field
The invention relates to the technical field of robot control, in particular to an underwater robot tour search and grabbing method and system based on self-adaptive control.
Background
In recent years, with the continuous deep exploration of the ocean and innovative development of ocean technology, the underwater robot plays a very important application value in various fields such as ocean mineral sampling, underwater article salvage and the like. However, at present, most of the realization of underwater searching and working tasks is realized as a platform by carrying out remote operation on the underwater robot by engineers, so that a large amount of manpower and material resources are consumed, and the defects of low response speed and the like exist in manual operation. In addition, although some underwater robots can perform autonomous operation, the autonomous operation tasks are completed in a known fixed area, autonomous searching cannot be performed, and actual underwater operation requirements are difficult to meet.
Therefore, how to improve the autonomous searching and working capabilities of the underwater robot for the target object in the unknown area becomes a problem to be solved.
Disclosure of Invention
The invention provides a self-adaptive control-based tour search and grabbing method and system for an underwater robot, which are used for solving the defect that the underwater robot cannot perform autonomous search in the prior art.
The invention provides an underwater robot tour search and grabbing method based on self-adaptive control, which comprises the following steps:
Acquiring current actual state information of an underwater robot, and determining a current error of the underwater robot based on current expected state information corresponding to a current search path of the underwater robot and the current actual state information;
under the condition that the current error is larger than a preset error, optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information to obtain the optimized current control input;
And controlling the underwater robot to carry out tour search along the current search path based on the optimized current control input, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process.
According to the method for searching and grabbing the tour of the underwater robot based on the self-adaptive control, the current control input corresponding to the current actual state information is optimized based on the current actual state information and the current expected state information, so as to obtain the optimized current control input, and the method comprises the following steps:
Constructing a current cost function based on the current actual state information and the current expected state information;
and optimizing the current control input by taking the minimization of the current cost function as a target to obtain the optimized current control input.
According to the method for searching and grabbing the tour of the underwater robot based on the self-adaptive control, which is provided by the invention, a current cost function is constructed based on the current actual state information and the current expected state information, and the method comprises the following steps:
constructing a state error cost function based on the current actual state information and the current expected state information;
constructing a first control error cost function based on a control step length of the control input and a weight matrix of the control input;
Constructing a second control error cost function based on the change amplitude of the control input at the next moment and the current control input;
and determining the current cost function based on the state error cost function, the first control error cost function and the second control error cost function.
According to the underwater robot tour search and grabbing method based on self-adaptive control, the method further comprises the following steps:
And under the condition that the current error is smaller than or equal to the preset error, controlling the underwater robot to carry out tour search along the current search path based on the current control input until the current search path is tour-finished, updating the current actual state information based on the next search path, and updating the current control input based on the updated current actual state information.
According to the underwater robot tour search and grabbing method based on self-adaptive control, the method further comprises the following steps:
and under the condition that an obstacle is detected in the tour search process, updating the current actual state information based on a next search path, and updating the current control input based on the updated current actual state information.
According to the invention, the method for searching and grabbing the tour of the underwater robot based on self-adaptive control comprises the following steps:
Controlling the underwater robot to grasp the target object under the condition that the target object is detected to be positioned in the working space of the underwater robot;
And under the condition that the target object is detected not to be positioned in the working space of the underwater robot, controlling the underwater robot to keep a suspension state and adjusting the relative position between the underwater robot and the target object until the target object is positioned in the working space of the underwater robot, and controlling the underwater robot to descend and grasp the target object.
According to the invention, the current actual state information of the underwater robot is obtained, and the method comprises the following steps:
Acquiring the depth, yaw angle, roll angle and pitch angle of the underwater robot through a depth sensor and an inertial element;
Acquiring the depth change rate, yaw angular velocity, roll angular velocity and pitch angular velocity of the underwater robot through a differential tracker;
Determining initial current actual state information based on the depth, the yaw angle, the roll angle, the pitch angle, the depth change rate, the yaw angular velocity, the roll angle angular velocity, and the pitch angle angular velocity;
and filtering the initial current actual state information to obtain the current actual state information.
The invention also provides an underwater robot tour search and grabbing system based on self-adaptive control, which comprises the following steps:
The determining unit is used for obtaining current actual state information of the underwater robot and determining a current error of the underwater robot based on current expected state information corresponding to a current search path of the underwater robot and the current actual state information;
the optimizing unit is used for optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information under the condition that the current error is larger than a preset error, so as to obtain the optimized current control input;
the control unit is used for controlling the underwater robot to carry out tour search along the current search path based on the optimized current control input, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process;
The underwater robot is provided with the determining unit, the optimizing unit and the control unit.
The invention also provides electronic equipment, which comprises a memory, a processor and a computer program stored on the memory and capable of running on the processor, wherein the processor realizes the underwater robot tour search and grabbing method based on the adaptive control when executing the computer program.
The present invention also provides a non-transitory computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements an underwater robot tour search and grab method based on adaptive control as described in any of the above.
The invention also provides a computer program product comprising a computer program which, when executed by a processor, implements the underwater robot tour search and grab method based on adaptive control as described in any of the above.
According to the tour search and grabbing method and system for the underwater robot based on the adaptive control, under the condition that the current error is larger than the preset error, the current control input is optimized based on the current actual state information and the current expected state information, and the optimized current control input is obtained, so that the current actual state information can be adjusted based on the optimized current control input, the difference between the adjusted current actual state information and the current expected state information is minimum, the underwater robot can conduct autonomous search along the current search path, grabbing is conducted under the condition that the target object is detected, and the autonomous search and operation capability of the underwater robot for the target object in an unknown area is improved.
Drawings
In order to more clearly illustrate the invention or the technical solutions of the prior art, the following description will briefly explain the drawings used in the embodiments or the description of the prior art, and it is obvious that the drawings in the following description are some embodiments of the invention, and other drawings can be obtained according to the drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic flow chart of an adaptive control-based underwater robot tour search and grabbing method provided by the invention;
FIG. 2 is a graphical representation of depth and depth rate of change provided by the present invention;
FIG. 3 is a graph showing the pitch angle and the rate of change of angular velocity of the pitch angle according to the present invention;
FIG. 4 is a schematic diagram of a change curve of depth in the autonomous motion process of the underwater robot;
FIG. 5 is a graphical representation of roll angle and roll angle angular rate of change provided by the present invention;
FIG. 6 is a second graphical representation of pitch angle and rate of change of angular velocity of pitch angle provided by the present invention;
fig. 7 is a schematic diagram of a depth change curve in the experimental process of the underwater robot provided by the invention;
FIG. 8 is a schematic diagram of a change curve of a roll angle and a pitch angle in the experimental process of the underwater robot;
fig. 9 is a schematic structural diagram of an electronic device provided by the present invention.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is apparent that the described embodiments are some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
At present, most of underwater searching and operation tasks are realized by using engineers to remotely operate an underwater robot as a platform, so that a large amount of manpower and material resources are consumed, and the defects of low response speed and the like exist in manual operation. In addition, although some underwater robots can perform autonomous operation, the autonomous operation tasks are completed in a known fixed area, autonomous searching cannot be performed, and actual underwater operation requirements are difficult to meet.
In this regard, the invention provides an underwater robot tour search and grabbing method based on self-adaptive control. Fig. 1 is a schematic flow chart of an adaptive control-based underwater robot tour search and grabbing method provided by the invention, as shown in fig. 1, the method comprises the following steps:
Step 110, obtaining current actual state information of the underwater robot, and determining a current error of the underwater robot based on the current expected state information corresponding to the current search path of the underwater robot and the current actual state information.
Specifically, the current search path refers to a path corresponding to the underwater robot when performing autonomous search, and the current search path may be generated according to a preset rule, for example, the preset rule may be to perform traversal search along the north direction, the east direction, the south direction and the east direction as one cycle, and set the current expected depth to be 0.59 m, or may be randomly generated. Alternatively, the current search path may be extracted from a search path library in which a plurality of different search paths are stored.
The current actual state information refers to the actual state of the underwater robot at the current moment, and the current expected state information refers to the expected state of the underwater robot at the current moment corresponding to the current search path. The state information may include a depth, a yaw angle, a roll angle, a pitch angle, a depth change rate, a yaw angular rate, a roll angular rate, a pitch angular rate, and the like of the underwater robot.
In addition, the current error is used for representing the difference between the current actual state information and the current expected state information, and the larger the difference between the current actual state information and the current expected state information is, the larger the current error is, namely, the larger the probability that the underwater robot deviates from the current search path is. Wherein the current error may comprise a current depth error and/or a current yaw error.
In some embodiments, the current actual state information may include a current actual depth and a current actual yaw angle, the current desired state information may include a current desired depth and a current desired yaw angle, and the current depth error may be determined based on the current actual depth and the current desired depth; based on the current actual yaw angle and the current desired yaw angle, a current depth error may be determined.
Step 120, optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information when the current error is greater than the preset error, to obtain the optimized current control input.
Specifically, when the current error is greater than the preset error, it indicates that the underwater robot has deviated from the current search path, and at this time, the current actual state information needs to be adjusted so that the underwater robot advances along the current search path. Alternatively, in case the current depth error is less than 0.1 meter and the current yaw angle error is less than 8 °, it may be determined that the underwater robot has deviated from the current search path.
In addition, the current control input refers to an input command for controlling the underwater robot to advance according to the current actual state information. Under the condition that the current error is larger than the preset error, based on the current actual state information and the current expected state information, the current control input corresponding to the current actual state information is optimized, so that the current actual state information is adjusted based on the optimized current control input, the difference between the adjusted current actual state information and the current expected state information is minimum, and the underwater robot is enabled to advance along the current search path.
And 130, controlling the underwater robot to carry out tour search along the current search path based on the optimized current control input, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process.
Specifically, based on the optimized current control input, current actual state information of the underwater robot, such as current depth, current yaw angle and the like of the underwater robot, can be adjusted, so that the underwater robot can perform tour search along a current search path, i.e. the underwater robot can realize autonomous tour search.
In some embodiments, the target object may be detected in real time by a binocular vision system, and if the target object is detected in the course of the tour search, the underwater robot is controlled to grasp the target object. In addition, if a plurality of target objects are detected, after the grabbing of the plurality of target objects is completed, the underwater robot is controlled to continue the tour search.
According to the tour search and grabbing method for the underwater robot based on the adaptive control, under the condition that the current error is larger than the preset error, the current control input is optimized based on the current actual state information and the current expected state information, and the optimized current control input is obtained, so that the current actual state information can be adjusted based on the optimized current control input, the difference between the adjusted current actual state information and the current expected state information is minimum, the underwater robot can conduct autonomous search along the current search path, grabbing is conducted under the condition that the target object is detected, and the autonomous search and operation capability of the underwater robot for the target object in an unknown area is improved.
Based on the above embodiment, based on the current actual state information and the current expected state information, the optimizing the current control input corresponding to the current actual state information to obtain the optimized current control input includes:
Constructing a current cost function based on the current actual state information and the current expected state information;
And optimizing the current control input by taking the minimization of the current cost function as a target to obtain the optimized current control input.
Specifically, the current cost function is used for representing the difference between the current actual state information and the current expected state information, and the smaller the current cost function is, the smaller the difference between the current actual state information and the current expected state information is, and the smaller the degree of the underwater robot deviating from the current search path is.
In this regard, the embodiment of the present invention aims at minimizing the current cost function, and optimizes the current control input to obtain the optimized current control input. Therefore, the current actual state information can be adjusted based on the optimized current control input, so that the difference between the adjusted current actual state information and the current expected state information is minimum, and the underwater robot can conduct autonomous search along the current search path.
Based on any of the above embodiments, constructing a current cost function based on the current actual state information and the current expected state information, including:
constructing a state error cost function based on the current actual state information and the current expected state information;
constructing a first control error cost function based on a control step length of the control input and a weight matrix of the control input;
Constructing a second control error cost function based on the change amplitude of the control input and the current control input at the next moment;
the current cost function is determined based on the state error cost function, the first control error cost function, and the second control error cost function.
In some embodiments, the state error cost function is determined based on the steps of:
According to the actual state vector eta corresponding to the current actual state information, the dynamic equation of the underwater robot under the world coordinate system is obtained as follows:
wherein M is an inertial matrix of the underwater robot, Is nonlinear expressions such as Coriolis force, damping force and the like of the underwater robot, namely/>For the current control input,/>For external disturbance (such as water flow fluctuation) suffered by the underwater robot, letThe method can obtain:
Wherein R 6×12 and A are state transition matrixes, B is a control input coefficient matrix, and the control input coefficient matrix is discretized by adopting a forward difference method to obtain a discretized kinetic equation of the underwater robot:
wherein T is the sampling time.
According to the discretized dynamics equation of the underwater robot, a system state expression at the next moment is obtained:
Wherein, And obtaining the state of the underwater robot system with the predicted step length of N p by iterative calculation:
Wherein, The state transition matrix at time N p, and the control input coefficient matrix.
According to the expected state vector eta d corresponding to the current expected state information and the actual state vector eta returned by the underwater robot sensor, determining a state error cost function as follows:
Wherein, Q is an adaptive state weight matrix, and the adaptive weight function for calculating the position and the gesture of the underwater robot according to the current error is calculated as follows:
Wherein, alpha i is a weight coefficient, e is a current error, and lambda i is a set function coefficient. The self-adaptive weight function of the speed and the attitude change rate is calculated as follows:
Wherein, beta i is a weight coefficient, and mu i is a set function coefficient.
Finally, an adaptive state weight matrix Q is obtained:
Q＝diag(αx,αy,αz,αφ,αθ,αφ,βx,βy,βz,βφ,βθ,βψ)
Wherein x, y, z represent three spatial directions, Θ, ψ represent the roll angle, pitch angle and yaw angle, respectively.
In some embodiments, the first control error cost function and the second control error cost function are determined based on the steps of:
discretizing according to a forward difference method to obtain a discretization expression of the control input, wherein the discretization expression is as follows:
Uk＝[u(k|k)T u(k+1|k)T…u(k+Np-1|k)T]T
N p is the prediction step length of the system state of the underwater robot. According to the physical system limitation of the underwater robot, the constraint relation between the system state and the control input is given:
Wherein η (k) min is a lower limit of the state of the underwater robot system, η (k) max is an upper limit of the state of the underwater robot system, U kmin is a lower limit of the control input at the kth time, and U kmax is an upper limit of the control input at the kth time.
In order to avoid the problem of input saturation caused by overlarge control input, a first control error cost function is designed for the control input as follows:
Wherein, N u is the control step length of the control input, and R 1 is the weight matrix of the optimized control input.
To ensure continuity of the two control inputs to ensure feasibility and smoothness of the control inputs, a second control error cost function is defined for the magnitude of the change of the two adjacent control inputs (the next time control input and the current control input) as follows:
wherein R 2 is a weight matrix for optimizing the two control input amplitude changes.
After determining the state error cost function, the first control error cost function, and the second control error cost function, the current cost function is as follows:
By optimizing the cost function, the optimal control input sequence is obtained And taking a first control input u *(k|k)T of the optimal control sequence as an optimized current control input to control the underwater robot to perform tour search along the current search path in a stable posture.
Based on any of the above embodiments, further comprising:
And under the condition that the current error is smaller than or equal to the preset error, controlling the underwater robot to carry out tour search along the current search path based on the current control input, updating the current actual state information based on the next search path after the tour of the current search path is finished, and updating the current control input based on the updated current actual state information.
Specifically, when the current error is smaller than or equal to the preset error, the difference between the current actual state information and the current expected state information is smaller, that is, the underwater robot can be accurately controlled to perform tour search along the current search path based on the current control input, and at the moment, the underwater robot can adopt constant-speed open-loop motion, that is, the advancing distance of the underwater robot is controlled by setting the open-loop motion time. After the current search path tour search is finished, the current actual state information can be updated based on the next search path, and the current control input can be updated based on the updated current actual state information, so that the underwater robot can be controlled to perform tour search along the next search path based on the updated current control input.
Based on any of the above embodiments, further comprising:
and under the condition that an obstacle is detected in the tour search process, updating current actual state information based on a next search path, and updating current control input based on the updated current actual state information.
Specifically, when an obstacle is detected in the tour search process, the current search path is indicated to be unable to continue, and at this time, the current actual state information is updated based on the next search path, and the current control input is updated based on the updated current actual state information, so that the underwater robot can be controlled to perform tour search along the next search path based on the updated current control input. And if the distance between the underwater robot and the obstacle is detected to be smaller than the early warning distance, switching to the next search path so as to enable the underwater robot to carry out tour search along the next search path.
Based on any of the above embodiments, controlling the underwater robot to grasp the target object includes:
Under the condition that the target object is detected to be positioned in the working space of the underwater robot, the underwater robot is controlled to grasp the target object;
Under the condition that the target object is not located in the working space of the underwater robot, controlling the underwater robot to keep a suspension state, adjusting the relative position between the underwater robot and the target object until the target object is located in the working space of the underwater robot, and controlling the underwater robot to descend and grab the target object.
Specifically, in the case where it is detected that the target object is located in the working space of the underwater robot, the underwater robot is controlled to descend to the water bottom and grasp the target object. Under the condition that the target object is not located in the working space of the underwater robot, controlling the underwater robot to keep a suspension state, adjusting the relative position between the underwater robot and the target object until the target object is located in the working space of the underwater robot, and controlling the underwater robot to descend and grab the target object. The stability of the fluctuation fin propeller is high, and the small displacement is accurately adjusted, so that the relative position between the underwater robot and the target object can be adjusted in a suspending manner by adjusting the fluctuation frequency and the fluctuation direction of the fluctuation fin. In addition, the embodiment of the invention can determine the position relation of the target object relative to the working arm of the underwater robot through the hand-eye conversion matrix, and determine whether the target object is positioned in the working space of the underwater robot or not based on the position relation.
According to the embodiment of the invention, the relative position between the underwater robot and the target object is adjusted under the condition that the underwater robot is kept in a floating state, so that the underwater robot can be prevented from being influenced by objects such as underwater sediment, stones and the like, and the position adjustment of the underwater robot is prevented.
Based on any of the above embodiments, obtaining current actual state information of the underwater robot includes:
Acquiring the depth, yaw angle, roll angle and pitch angle of the underwater robot through the depth sensor and the inertial element;
acquiring the depth change rate, yaw angular velocity, roll angular velocity and pitch angular velocity of the underwater robot through a differential tracker;
determining initial current actual state information based on depth, yaw angle, roll angle, pitch angle, depth change rate, yaw angular velocity, roll angle angular velocity, and pitch angular velocity;
And filtering the initial current actual state information to obtain the current actual state information.
In some specific embodiments, the underwater robot is provided with a depth sensor capable of sensing depth change through water pressure and an inertial element capable of sensing attitude angle change, and depth, yaw angle, roll angle and pitch angle information of the underwater robot are obtained in real time through the depth sensor and the inertial element; and calculating the change rates of the depth, the yaw angle, the roll angle and the pitch angle of the underwater robot through the differential tracker to obtain the depth change rate, the yaw angular velocity, the roll angular velocity and the pitch angular velocity of the underwater robot.
And determining initial current actual state information based on the depth, the yaw angle, the roll angle, the pitch angle, the depth change rate, the yaw angular velocity, the roll angle angular velocity and the pitch angle angular velocity, and filtering the initial current actual state information through a Kalman filter to obtain filtered current actual state information.
Fig. 2 is a schematic diagram of a depth and a depth change rate according to the present invention, and in fig. 2, a dotted line shows a schematic diagram of a depth and a depth change rate of an underwater robot according to the prior art (MPC), and a solid line shows a schematic diagram of a depth and a depth change rate of an underwater robot according to A Method (AMPC) according to an embodiment of the present invention, and as shown in fig. 2, the method according to an embodiment of the present invention can enable the underwater robot to dive to a desired depth more quickly. Fig. 3 is one of schematic diagrams of a pitch angle and a pitch angle angular rate of change provided by the present invention, and in fig. 3, a dashed line represents a schematic diagram of a pitch angle and a pitch angle angular rate of change of an underwater robot using a prior art (MPC), and a solid line represents a schematic diagram of a pitch angle and a pitch angle angular rate of change of an underwater robot using A Method (AMPC) according to an embodiment of the present invention, and as shown in fig. 3, the method provided by the embodiment of the present invention can stabilize a pitch angle of an underwater robot more rapidly. In addition, fig. 2 and fig. 3 respectively show that the underwater robot is disturbed by instantaneous disturbance at 9 seconds, the depth and the pitch angle of the underwater robot are recovered to the change of the expected state, and the method provided by the embodiment of the invention still has a relatively fast control response speed.
Fig. 4 is a schematic diagram of a depth change curve in an autonomous motion process of an underwater robot provided by the invention, a dashed line in fig. 4 shows a schematic diagram of a depth change curve of an underwater robot adopting the prior art (MPC), and a solid line shows a schematic diagram of a depth change curve of an underwater robot adopting A Method (AMPC) according to an embodiment of the invention, as shown in fig. 4, the method provided by the embodiment of the invention has better control precision.
Fig. 5 is a graph showing the roll angle and the roll angle angular velocity change rate provided by the present invention, and fig. 5 is a graph showing the roll angle and the roll angle angular velocity change rate of an underwater robot using the related art (MPC), and a solid line shows the roll angle and the roll angle angular velocity change rate of an underwater robot using the method (AMPC) according to the embodiment of the present invention. Fig. 6 is a second graph of the change rate of the pitch angle and the angular velocity of the pitch angle, the dashed-dotted line in fig. 6 shows the graph of the change rate of the pitch angle and the angular velocity of the pitch angle of the underwater robot using the prior art (MPC), and the solid line shows the graph of the change rate of the pitch angle and the angular velocity of the pitch angle of the underwater robot using the method (AMPC) according to the embodiment of the invention. As can be seen from fig. 5 and 6, the method provided by the embodiment of the invention has better control precision in roll and pitch attitude control.
Fig. 7 is a schematic diagram of a depth change curve in the experimental process of the underwater robot provided by the invention, and fig. 8 is a schematic diagram of a roll angle and a pitch angle change curve in the experimental process of the underwater robot provided by the invention, wherein the maximum amplitude of the roll angle and the pitch angle of the underwater robot is smaller than 13 degrees as shown in fig. 8. In addition, as shown in fig. 7 and 8, the underwater robot maintains good attitude stability in the searching and position adjusting processes, and finally the underwater robot completes the task of traversing searching and grabbing the target object in the unknown area.
The following describes the adaptive control-based underwater robot tour search and grasping system, and the adaptive control-based underwater robot tour search and grasping system and the adaptive control-based underwater robot tour search and grasping method described above can be referred to correspondingly.
Based on any one of the embodiments, the invention further provides an underwater robot tour search and grabbing system based on adaptive control, which comprises:
the determining unit is used for acquiring current actual state information of the underwater robot and determining a current error of the underwater robot based on current expected state information corresponding to a current search path of the underwater robot and the current actual state information;
The optimizing unit is used for optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information under the condition that the current error is larger than the preset error, so as to obtain the optimized current control input;
The control unit is used for controlling the underwater robot to carry out tour search along the current search path based on the optimized current control input, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process;
The underwater robot is provided with a determining unit, an optimizing unit and a control unit.
Fig. 9 is a schematic structural diagram of an electronic device provided by the present invention, and as shown in fig. 9, the electronic device may include: processor 910, memory 920, communication interface (Communications Interface) 930, and communication bus 940, where processor 910, memory 920, and communication interface 930 communicate with each other via communication bus 940. The processor 910 may invoke logic instructions in the memory 920 to perform an adaptive control-based underwater robot tour search and grab method comprising: acquiring current actual state information of an underwater robot, and determining a current error of the underwater robot based on current expected state information corresponding to a current search path of the underwater robot and the current actual state information; under the condition that the current error is larger than a preset error, optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information to obtain the optimized current control input; and controlling the underwater robot to carry out tour search along the current search path based on the optimized current control input, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process.
Further, the logic instructions in the memory 920 may be implemented in the form of software functional units and stored in a computer readable storage medium when sold or used as a stand alone product. Based on this understanding, the technical solution of the present invention may be embodied essentially or in a part contributing to the prior art or in a part of the technical solution, in the form of a software product stored in a storage medium, comprising several instructions for causing a computer device (which may be a personal computer, a server, a network device, etc.) to perform all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a usb disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
In another aspect, the present invention also provides a computer program product comprising a computer program stored on a non-transitory computer readable storage medium, the computer program comprising program instructions which, when executed by a computer, enable the execution of the method of adaptive control-based underwater robot tour search and capture provided by the above methods, the method comprising: acquiring current actual state information of an underwater robot, and determining a current error of the underwater robot based on current expected state information corresponding to a current search path of the underwater robot and the current actual state information; under the condition that the current error is larger than a preset error, optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information to obtain the optimized current control input; and controlling the underwater robot to carry out tour search along the current search path based on the optimized current control input, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process.
In still another aspect, the present invention further provides a non-transitory computer readable storage medium having stored thereon a computer program which, when executed by a processor, is implemented to perform the above-provided adaptive control-based tour search and grasping method for an underwater robot, the method comprising: acquiring current actual state information of an underwater robot, and determining a current error of the underwater robot based on current expected state information corresponding to a current search path of the underwater robot and the current actual state information; under the condition that the current error is larger than a preset error, optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information to obtain the optimized current control input; and controlling the underwater robot to carry out tour search along the current search path based on the optimized current control input, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process.
The apparatus embodiments described above are merely illustrative, wherein the elements illustrated as separate elements may or may not be physically separate, and the elements shown as elements may or may not be physical elements, may be located in one place, or may be distributed over a plurality of network elements. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art will understand and implement the present invention without undue burden.
From the above description of the embodiments, it will be apparent to those skilled in the art that the embodiments may be implemented by means of software plus necessary general hardware platforms, or of course may be implemented by means of hardware. Based on this understanding, the foregoing technical solution may be embodied essentially or in a part contributing to the prior art in the form of a software product, which may be stored in a computer readable storage medium, such as ROM/RAM, a magnetic disk, an optical disk, etc., including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method described in the respective embodiments or some parts of the embodiments.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims (8)
1. An underwater robot tour search and grabbing method based on self-adaptive control is characterized by comprising the following steps:
Acquiring current actual state information of an underwater robot, and determining a current error of the underwater robot based on current expected state information corresponding to a current search path of the underwater robot and the current actual state information;
under the condition that the current error is larger than a preset error, optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information to obtain the optimized current control input;
Controlling the underwater robot to carry out tour search along the current search path based on the optimized current control input, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process;
The optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information to obtain the optimized current control input includes:
Constructing a current cost function based on the current actual state information and the current expected state information;
optimizing the current control input by taking the minimization of the current cost function as a target to obtain the optimized current control input;
the constructing a current cost function based on the current actual state information and the current expected state information includes:
constructing a state error cost function based on the current actual state information and the current expected state information;
constructing a first control error cost function based on a control step length of the control input and a weight matrix of the control input;
Constructing a second control error cost function based on the change amplitude of the control input at the next moment and the current control input;
Determining the current cost function based on the state error cost function, the first control error cost function, and the second control error cost function;
The state error cost function is determined based on the steps of:
According to the actual state vector corresponding to the current actual state information, the kinetic equation of the underwater robot under the world coordinate system is obtained as follows:
Wherein eta is an actual state vector corresponding to the current actual state information, M is an inertial matrix of the underwater robot, Is a nonlinear expression of Coriolis force and damping force of the underwater robot, i.e./>For the current control input,/>For external disturbance to the underwater robot, let/>The method can obtain:
Wherein R 6×12 and A are state transition matrixes, B is a control input coefficient matrix, and the control input coefficient matrix is discretized by adopting a forward difference method to obtain a discretized kinetic equation of the underwater robot:
Wherein T is the sampling time;
according to the discretized dynamics equation of the underwater robot, a system state expression at the next moment is obtained:
Wherein, And obtaining the state of the underwater robot system with the predicted step length of N p by iterative calculation:
Wherein, The state transition matrix is the state transition matrix at the moment N p, and B is the control input coefficient matrix;
According to the expected state vector eta d corresponding to the current expected state information and the actual state vector eta returned by the underwater robot sensor, determining a state error cost function as follows:
Wherein Q is an adaptive state weight matrix;
The adaptive state weight matrix Q is determined based on the following manner:
Wherein alpha i is a weight coefficient, e is a current error, and lambda i is a set function coefficient;
Wherein, beta i is a weight coefficient, mu i is a set function coefficient;
finally, an adaptive state weight matrix Q is obtained:
Q＝diag(αx,αy,αz,αφ,αθ,αψ,βx,βy,βz,βφ,βθ,βψ)
Wherein x, y, z represent three spatial directions, The roll angle, θ, pitch angle, and ψ represent yaw angle.
2. The adaptive control-based underwater robot tour search and grasping method according to claim 1, further comprising:
And under the condition that the current error is smaller than or equal to the preset error, controlling the underwater robot to carry out tour search along the current search path based on the current control input until the current search path is tour-finished, updating the current actual state information based on the next search path, and updating the current control input based on the updated current actual state information.
3. The adaptive control-based underwater robot tour search and grasping method according to claim 1 or 2, further comprising:
and under the condition that an obstacle is detected in the tour search process, updating the current actual state information based on a next search path, and updating the current control input based on the updated current actual state information.
4. The adaptive control-based underwater robot tour search and grasp method according to any of claims 1 to 2, characterized in that the controlling the underwater robot to grasp the target object includes:
Controlling the underwater robot to grasp the target object under the condition that the target object is detected to be positioned in the working space of the underwater robot;
And under the condition that the target object is detected not to be positioned in the working space of the underwater robot, controlling the underwater robot to keep a suspension state and adjusting the relative position between the underwater robot and the target object until the target object is positioned in the working space of the underwater robot, and controlling the underwater robot to descend and grasp the target object.
5. The adaptive control-based tour search and grasping method of an underwater robot according to any one of claims 1 to 2, wherein the obtaining current actual state information of the underwater robot includes:
Acquiring the depth, yaw angle, roll angle and pitch angle of the underwater robot through a depth sensor and an inertial element;
Acquiring the depth change rate, yaw angular velocity, roll angular velocity and pitch angular velocity of the underwater robot through a differential tracker;
Determining initial current actual state information based on the depth, the yaw angle, the roll angle, the pitch angle, the depth change rate, the yaw angular velocity, the roll angle angular velocity, and the pitch angle angular velocity;
and filtering the initial current actual state information to obtain the current actual state information.
6. An underwater robot tour search and grabbing system based on self-adaptive control is characterized by comprising:
The determining unit is used for obtaining current actual state information of the underwater robot and determining a current error of the underwater robot based on current expected state information corresponding to a current search path of the underwater robot and the current actual state information;
the optimizing unit is used for optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information under the condition that the current error is larger than a preset error, so as to obtain the optimized current control input;
the control unit is used for controlling the underwater robot to carry out tour search along the current search path based on the optimized current control input, and controlling the underwater robot to grasp the target object under the condition that the target object is detected in the tour search process;
The underwater robot is provided with the determining unit, the optimizing unit and the control unit;
The optimizing the current control input corresponding to the current actual state information based on the current actual state information and the current expected state information to obtain the optimized current control input includes:
Constructing a current cost function based on the current actual state information and the current expected state information;
optimizing the current control input by taking the minimization of the current cost function as a target to obtain the optimized current control input;
the constructing a current cost function based on the current actual state information and the current expected state information includes:
constructing a state error cost function based on the current actual state information and the current expected state information;
constructing a first control error cost function based on a control step length of the control input and a weight matrix of the control input;
Constructing a second control error cost function based on the change amplitude of the control input at the next moment and the current control input;
Determining the current cost function based on the state error cost function, the first control error cost function, and the second control error cost function;
The state error cost function is determined based on the steps of:
According to the actual state vector corresponding to the current actual state information, the kinetic equation of the underwater robot under the world coordinate system is obtained as follows:
Wherein eta is an actual state vector corresponding to the current actual state information, M is an inertial matrix of the underwater robot, Is a nonlinear expression of Coriolis force and damping force of the underwater robot, i.e./>For the current control input,/>For external disturbance to the underwater robot, let/>The method can obtain:
Wherein R 6×12 and A are state transition matrixes, B is a control input coefficient matrix, and the control input coefficient matrix is discretized by adopting a forward difference method to obtain a discretized kinetic equation of the underwater robot:
Wherein T is the sampling time;
according to the discretized dynamics equation of the underwater robot, a system state expression at the next moment is obtained:
Wherein, And obtaining the state of the underwater robot system with the predicted step length of N p by iterative calculation:
Wherein, The state transition matrix is the state transition matrix at the moment N p, and B is the control input coefficient matrix;
According to the expected state vector eta d corresponding to the current expected state information and the actual state vector eta returned by the underwater robot sensor, determining a state error cost function as follows:
Wherein Q is an adaptive state weight matrix;
The adaptive state weight matrix Q is determined based on the following manner:
Wherein alpha i is a weight coefficient, e is a current error, and lambda i is a set function coefficient;
Wherein, beta i is a weight coefficient, mu i is a set function coefficient;
finally, an adaptive state weight matrix Q is obtained:
Q＝diag(αx,αy,αz,αφ,αθ,αψ,βx,βy,βz,βφ,βθ,βψ)
Wherein x, y, z represent three spatial directions, The roll angle, θ, pitch angle, and ψ represent yaw angle.
7. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the adaptive control-based underwater robot tour search and grab method according to any of claims 1 to 5 when executing the computer program.
8. A non-transitory computer readable storage medium having stored thereon a computer program, wherein the computer program when executed by a processor implements the adaptive control-based underwater robot tour search and grab method according to any of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310778064.3A CN117055586B (en) | 2023-06-28 | 2023-06-28 | Underwater robot tour search and grabbing method and system based on self-adaptive control |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310778064.3A CN117055586B (en) | 2023-06-28 | 2023-06-28 | Underwater robot tour search and grabbing method and system based on self-adaptive control |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117055586A CN117055586A (en) | 2023-11-14 |
| CN117055586B true CN117055586B (en) | 2024-05-14 |
Family
ID=88657877
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310778064.3A Active CN117055586B (en) | 2023-06-28 | 2023-06-28 | Underwater robot tour search and grabbing method and system based on self-adaptive control |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117055586B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2280241A2 (en) * | 2009-07-30 | 2011-02-02 | Qinetiq Limited | Vehicle control |
| CN113084817A (en) * | 2021-04-15 | 2021-07-09 | 中国科学院自动化研究所 | Object searching and grabbing control method of underwater bionic robot in turbulent flow environment |
| CN113335277A (en) * | 2021-04-27 | 2021-09-03 | 北京工业大学 | Intelligent cruise control method and device, electronic equipment and storage medium |
| CN113359757A (en) * | 2021-06-30 | 2021-09-07 | 湖北汽车工业学院 | Improved hybrid A-x algorithm unmanned vehicle path planning and trajectory tracking method |
| CN114115262A (en) * | 2021-11-23 | 2022-03-01 | 上海交通大学 | Multi-AUV actuator saturated collaborative formation control system and method based on azimuth information |
| CN115494733A (en) * | 2022-10-27 | 2022-12-20 | 南方电网调峰调频发电有限公司储能科研院 | Underwater robot self-adaptive control method based on gazebo |
| CN116341109A (en) * | 2023-03-24 | 2023-06-27 | 电子科技大学 | Adaptive filtering method and system suitable for modeling of aircraft system |
-
2023
- 2023-06-28 CN CN202310778064.3A patent/CN117055586B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2280241A2 (en) * | 2009-07-30 | 2011-02-02 | Qinetiq Limited | Vehicle control |
| CN113084817A (en) * | 2021-04-15 | 2021-07-09 | 中国科学院自动化研究所 | Object searching and grabbing control method of underwater bionic robot in turbulent flow environment |
| CN113335277A (en) * | 2021-04-27 | 2021-09-03 | 北京工业大学 | Intelligent cruise control method and device, electronic equipment and storage medium |
| CN113359757A (en) * | 2021-06-30 | 2021-09-07 | 湖北汽车工业学院 | Improved hybrid A-x algorithm unmanned vehicle path planning and trajectory tracking method |
| CN114115262A (en) * | 2021-11-23 | 2022-03-01 | 上海交通大学 | Multi-AUV actuator saturated collaborative formation control system and method based on azimuth information |
| CN115494733A (en) * | 2022-10-27 | 2022-12-20 | 南方电网调峰调频发电有限公司储能科研院 | Underwater robot self-adaptive control method based on gazebo |
| CN116341109A (en) * | 2023-03-24 | 2023-06-27 | 电子科技大学 | Adaptive filtering method and system suitable for modeling of aircraft system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117055586A (en) | 2023-11-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Xiang et al. | Robust fuzzy 3D path following for autonomous underwater vehicle subject to uncertainties | |
| CN108803321B (en) | Autonomous underwater vehicle track tracking control method based on deep reinforcement learning | |
| Wu et al. | Depth control of model-free AUVs via reinforcement learning | |
| CN107168312B (en) | Space trajectory tracking control method for compensating UUV kinematic and dynamic interference | |
| Manzanilla et al. | Super-twisting integral sliding mode control for trajectory tracking of an Unmanned Underwater Vehicle | |
| JP4406436B2 (en) | Autonomous mobile robot motion planning method, autonomous mobile robot control method using autonomous mobile robot motion planning method, autonomous mobile robot motion planning device, autonomous mobile robot motion planning program and its recording medium, autonomous mobile robot control program | |
| Anderlini et al. | Control of a ROV carrying an object | |
| CN113885534B (en) | Intelligent predictive control-based water surface unmanned ship path tracking method | |
| CN104635773B (en) | A kind of dynamic localization method for ship based on improvement Strong tracking filter state observer | |
| CN112068440B (en) | AUV recovery butt joint power positioning control method based on model prediction control | |
| CN115016496A (en) | Water surface unmanned ship path tracking method based on deep reinforcement learning | |
| CN111240345A (en) | Underwater robot trajectory tracking method based on double BP network reinforcement learning framework | |
| CN111290270A (en) | Underwater robot backstepping speed and heading control method based on Q-learning parameter adaptive technology | |
| Campos et al. | A nonlinear controller based on saturation functions with variable parameters to stabilize an AUV | |
| Hu et al. | Trajectory tracking and re-planning with model predictive control of autonomous underwater vehicles | |
| CN114879671A (en) | Unmanned ship trajectory tracking control method based on reinforcement learning MPC | |
| CN114967714A (en) | Anti-interference motion control method and system for autonomous underwater robot | |
| CN113954077B (en) | Underwater swimming mechanical arm trajectory tracking control method and device with energy optimization function | |
| Ali et al. | Stabilizing the dynamic behavior and position control of a remotely operated underwater vehicle | |
| Zhang et al. | AUV 3D docking control using deep reinforcement learning | |
| CN114217603A (en) | Safety optimization control system for formation of multiple unmanned ships | |
| CN117055586B (en) | Underwater robot tour search and grabbing method and system based on self-adaptive control | |
| CN116540717A (en) | AUV local path planning method based on improved DWA | |
| Børhaug et al. | Cross-track formation control of underactuated autonomous underwater vehicles | |
| CN113984045B (en) | Method and system for estimating motion state of movable butt-joint target of underwater robot |
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 |