CN113885540B - Motion planning and control method for climbing and crossing of wall surface of underwater hexapod robot - Google Patents
Motion planning and control method for climbing and crossing of wall surface of underwater hexapod robot Download PDFInfo
- Publication number
- CN113885540B CN113885540B CN202111213908.7A CN202111213908A CN113885540B CN 113885540 B CN113885540 B CN 113885540B CN 202111213908 A CN202111213908 A CN 202111213908A CN 113885540 B CN113885540 B CN 113885540B
- Authority
- CN
- China
- Prior art keywords
- underwater
- robot
- angle
- leg
- hexapod robot
- 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
- 241000238631 Hexapoda Species 0.000 title claims abstract description 209
- 230000009194 climbing Effects 0.000 title claims abstract description 63
- 238000000034 method Methods 0.000 title claims abstract description 59
- 230000033001 locomotion Effects 0.000 title claims abstract description 33
- 230000005021 gait Effects 0.000 claims abstract description 73
- 230000008859 change Effects 0.000 claims abstract description 24
- 238000013507 mapping Methods 0.000 claims abstract description 8
- 210000002414 leg Anatomy 0.000 claims description 211
- 210000004394 hip joint Anatomy 0.000 claims description 35
- 210000001503 joint Anatomy 0.000 claims description 28
- 238000005096 rolling process Methods 0.000 claims description 19
- 239000011159 matrix material Substances 0.000 claims description 16
- 210000001364 upper extremity Anatomy 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 230000008878 coupling Effects 0.000 claims description 3
- 238000010168 coupling process Methods 0.000 claims description 3
- 238000005859 coupling reaction Methods 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 230000010355 oscillation Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 17
- 238000004088 simulation Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 7
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- VNWKTOKETHGBQD-UHFFFAOYSA-N methane Chemical compound C VNWKTOKETHGBQD-UHFFFAOYSA-N 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 239000002002 slurry Substances 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 239000000725 suspension Substances 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 101100186920 Campylobacter jejuni subsp. jejuni serotype O:2 (strain ATCC 700819 / NCTC 11168) legI gene Proteins 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000026058 directional locomotion Effects 0.000 description 1
- 229910052500 inorganic mineral Inorganic materials 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 239000011707 mineral Substances 0.000 description 1
- 239000003345 natural gas Substances 0.000 description 1
- 235000001968 nicotinic acid Nutrition 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 239000003209 petroleum derivative Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000004083 survival effect Effects 0.000 description 1
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/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-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 hybrid-driven underwater hexapod robot wall climbing and crossing motion planning and control method. The method specifically comprises the following steps: s1: aiming at a wall surface with a known inclination angle, acquiring the forward distance from the mass center of the underwater hexapod robot to the wall surface by using a distance measuring device; s2: judging what gait (climbing gait/crossing gait) should be adopted according to the forward distance between the centroid and the wall surface and the error between the pitch angle of the underwater hexapod robot and the inclination angle of the wall surface; s3: aiming at climbing gait, a knife-edge leg support angle change rule is designed according to course errors, and a CPG-based tripodia gait planning method is provided; s4: aiming at the crossing gait, according to the wall inclination angle and the current pitch angle of the underwater hexapod robot, the mapping relation between the output signal of the oscillator and the expected joint angle of the blade leg is designed, and a CPG-based crossing gait planning method is provided. The planning and control method can enable the underwater hexapod robot to span the wall surfaces with different inclinations and realize stable climbing.
Description
Technical Field
The invention relates to the technical field of underwater robots, in particular to a hybrid-driven underwater hexapod robot wall climbing and crossing motion planning and control method.
Background
Ocean contains abundant mineral resources, biological resources and petroleum and natural gas resources, and reasonable and effective development of ocean resources is a necessary path for human survival and development. Traditional Autonomous Underwater Vehicles (AUV) or unmanned remote control underwater vehicles (ROV) cannot meet the requirements of large-scale tour detection operation and local refinement operation at the same time, and the underwater robots driven by the propeller and the blade leg in a mixed mode have the capability of tour, landing and walking on the wall surface in water at the same time.
Gait planning methods of existing foot robots are generally divided into two categories: gait planning method based on motion calculation and gait planning method based on bionics. The method is characterized in that on the basis of acquiring a kinematic model of the foot robot, motion constraint conditions of climbing wall surfaces are analyzed, expected motion reference quantity is set, and the current expected gait is acquired by utilizing an optimization method or a random search method. The gait of the multi-legged robot is controlled by designing a self-oscillating central mode generator (CPG) model, and traction reflection, gesture reflection and obstacle reflection mechanisms are designed for common terrains by combining a biological reflection principle, so that the climbing of a working surface is finally realized. The optimal climbing gait can be planned, but the dependence on the robot leg model is high and the calculation is complex; the latter has the advantage of simple and rapid planning, but the generated periodic gait is difficult to adapt to the transition situation between the wall surfaces with different inclinations.
The underwater hexapod robot usually needs to work on different wall surfaces, if walking gait is not reasonably planned, the underwater hexapod robot is difficult to flexibly span between different working surfaces, namely, the underwater hexapod robot cannot smoothly transition from one working surface to the other working surface, and finally, the working task cannot be continued. Therefore, how to coordinate the walking actions of a plurality of legs to achieve smooth crossing between walls with different inclinations and stable climbing between walls with different inclinations is a very challenging research topic.
Disclosure of Invention
(1) Technical problem
The following difficulties mainly exist in designing a hybrid-driven underwater six-foot robot wall climbing and crossing motion planning and control method: 1. how to design the mapping relation between CPG output signals and the blade leg expected joint angles so that the gait is adapted to the wall inclination angle and the current attitude angle of the underwater robot; 2. how to design propeller input so that the underwater robot can meet the requirement of longitudinal non-slip when crossing the wall surface; 3. how to improve the self-adaptability of the system, so that the underwater hexapod robot can stably span and climb the wall surfaces with different inclinations.
Aiming at the problems that the existing gait planning method is poor in self-adaptability and cannot enable an underwater hexapod robot to stably climb and stably span on walls with different dip angles, the invention provides a hybrid-drive motion planning and control method for climbing and span of the underwater hexapod robot based on a CPG algorithm. The method not only maintains the advantage of easy planning of basic gait, but also solves the thrust of the propeller which is suitable for the torque of the blade leg joint through a certain foot force analysis, so that the underwater robot is capable of stably crossing between wall surfaces with different dip angles.
(2) Technical proposal
The invention provides a hybrid-driven underwater hexapod robot wall climbing and crossing motion planning and control method, which comprises the following steps:
s1: aiming at a wall surface with a known inclination angle, acquiring the forward distance from the mass center of the underwater hexapod robot to the wall surface;
s2: judging what gait (climbing gait/crossing gait) should be adopted according to the forward distance between the centroid and the wall surface and the error between the pitch angle of the underwater hexapod robot and the inclination angle of the wall surface;
s3: aiming at climbing gait, a knife-edge leg support angle change rule is designed according to course errors, and a CPG-based tripodia gait planning method is provided; on the basis, in order to improve the climbing stability of the underwater hexapod robot, on one hand, basic thrust is applied to the propeller of the underwater hexapod robot, so that the blade legs of the underwater hexapod robot are not slipped during climbing, and on the other hand, the influence of restoring force and restoring moment on the stable climbing of the underwater hexapod robot is reduced, and according to the current attitude angle of the underwater hexapod robot, additional thrust is applied to the propeller of the underwater hexapod robot so as to overcome the restoring force and restoring moment;
s4: aiming at the crossing gait, according to the wall inclination angle and the current pitch angle of the underwater hexapod robot, designing a mapping relation between an oscillator output signal and a blade leg expected joint angle, and providing a CPG-based crossing gait planning method; on the basis, in order to improve the stability of crossing among the wall surfaces with different inclination angles of the underwater hexapod robot, on one hand, basic thrust is applied to the propeller of the underwater hexapod robot, so that the blade legs of the underwater hexapod robot are not slipped during crossing, and on the other hand, the transverse rolling of the underwater hexapod robot during crossing is reduced, and according to the current transverse rolling angle of the underwater hexapod robot, additional thrust is applied to the propeller of the underwater hexapod robot so as to overcome the transverse rolling of the underwater hexapod robot.
For the exemplary embodiment of the invention, the forward distance from the centroid to the wall surface of the underwater hexapod robot is obtained in S1 by using a distance measuring device.
For the exemplary embodiment of the present invention, in S2, it is determined what gait (climbing gait/crossing gait) should be adopted according to the forward distance between the centroid and the wall surface and the error between the pitch angle of the underwater hexapod robot and the inclination angle of the wall surface.
According to the distance information of the mass center and the inclined wall surface of the underwater hexapod robot acquired by the distance measuring equipment, a switching signal c representing the change from climbing to crossing gait is set 1 Specifically, it is
Wherein D is the distance from the mass center of the underwater hexapod robot to the inclined wall surface, and D is a distance threshold.
According to the pitch angle theta of the underwater hexapod robot and the inclination angle of the wall surface k to be climbedError between, setting a switch signal c characterizing the switch from crossing to climbing gait 2 Specifically, it is
Wherein,,is the error threshold.
Aiming at the exemplary embodiment of the invention, in S3, aiming at climbing gait, a blade leg support angle change rule is designed according to heading errors, and a tripod gait planning method based on CPG is provided; on the basis, in order to improve the climbing stability of the underwater hexapod robot, on one hand, basic thrust is applied to the propeller of the underwater hexapod robot, so that the blade legs of the underwater hexapod robot are not slipped during climbing, and on the other hand, the influence of restoring force and restoring moment on the stable climbing of the underwater hexapod robot is reduced, and according to the current attitude angle of the underwater hexapod robot, additional thrust is applied to the propeller of the underwater hexapod robot so as to overcome the restoring force and restoring moment. The method comprises the following steps:
In order to realize stable climbing and crossing of the underwater hexapod robot on different wall surfaces, the invention adopts a tripodal gait, namely, legs 2, 3 and 6 and legs 1, 4 and 5 move in an alternate ground contact/suspension swing mode, and therefore, a CPG-based gait planning method is adopted. The CPG gait generator consists of 6 Hopf oscillators, and the 6 Hopf oscillators correspond to the 6 blade legs respectively. The mathematical model of the ith (i=1, 2, …, 6) Hopf oscillator can be expressed as
Wherein X is i ＝[u i v i ] T In the state of the ith oscillator, sigma is a convergence factor, R is the amplitude of the output of the oscillator, w is the oscillation frequency of the oscillator, lambda is a coupling coefficient,the effect of the jth (j=1, 2, …, 6) oscillator on the ith oscillator in the CPG network can be expressed as
Wherein,,the phase difference of the jth oscillator to the ith oscillator.
Output of ith Hopf oscillator [ u ] i v i ] T The relation between the expected joint angle of the ith leg of the underwater hexapod robot and the relation between the expected joint angle of the ith leg of the underwater hexapod robot is mapped as follows
Wherein θ si The support angle of the ith leg is the angle through which the ith leg rotates in the support phase stage; θ ti Is the swing angle of the ith leg, namely the angle through which the ith leg rotates in the swing phase stage, and meets theta ti ＝2π-θ si 。
According to the supporting phase and the swinging phase of the blade leg, a variable a representing whether the blade leg touches the ground or not is defined i When the angle of the blade leg joint rotates to the supporting phase, the blade leg touches the ground, otherwise the blade leg is suspended, which is as follows
In order to enable the underwater robot to have the directional climbing function, the supporting angle of the ith leg can be designed as
θ si ＝k ψ M(i)(ψ-ψ d )+θ s0
Wherein k is ψ Is a constant greater than zero, ψ d The expected course angle of the underwater hexapod robot is, and psi is the actual course angle of the underwater hexapod robot, theta s0 For the initial value of the support angle, M (i) may be defined as
PD control is adopted to enable the blade leg of the underwater robot to expect joint anglesConversion into blade leg joint torque tau i (i=1, …, 6), the control law is as follows
Wherein k is p 、k d Q is the control parameter to be adjusted i 、The real joint angle and the real joint angular velocity of the blade leg are respectively.
In order to improve climbing stability of the underwater hexapod robot, a basic thrust T is applied to a propeller of the underwater hexapod robot i b (i=1, …, 8) to ensure no slip of blade legs of the underwater hexapod robot during climbing, and on the other hand, to reduce the influence of restoring force and restoring torque on stable climbing of the underwater hexapod robot, to apply additional thrust T to a propeller of the underwater hexapod robot according to the current attitude angle of the underwater hexapod robot i a (i=1, …, 8) to overcome the restoring force and Restoring the moment. The propulsor thrust is thus made up of two parts, the basic thrust and the additional thrust.
According to the joint torque tau of the ground contact blade leg i Can be obtained
Wherein F is τix 、F τiy Respectively the components of the torque of the blade leg joint to the acting force at the touchdown point under the touchdown point coordinate system, l r Is the radius of the blade leg of the underwater robot.
Obtaining the vertical acting force F required by the blade leg touchdown point according to the non-slip constraint Tiy Concretely, the method is as follows
Wherein μ is the ground friction coefficient; n (N) i Is ground supporting force applied to the ground contact point.
F according to D-H coordinate transformation Tiy With the thrust T of the propeller borne by the mass center of the robot x 、T y 、T z Satisfy the following relation
Wherein a is i The ground contact state of the ith blade leg,is a rotation matrix from the touchdown point coordinate system to the body coordinate system.
Therefore, the basic thrust T of 8 propellers of the underwater hexapod robot i b (i=1, …, 8) is:
wherein T is i b (i=1, 2, …, 8) is the base thrust value of the i-th propeller; c (C) t A matrix is assigned for thrust forces.
Restoring force F of underwater hexapod robot gx 、F gy 、F gz And a restoring moment M gx 、M gy 、M gz (component of the buoyancy moment in the body coordinate system) is as follows
Wherein B is the buoyancy of the underwater robot, G is the gravity of the underwater robot, theta is the pitch angle of the underwater robot,is the roll angle, x of the underwater robot c ,y c ,z c Is the coordinates of the floating center of the underwater robot in a body coordinate system.
Thus, the extra thrust T of 8 propellers of the underwater hexapod robot i a (i=1, …, 8) is:
wherein T is i a (i=1, 2, …, 8) is the additional thrust value of the i-th propeller; c (C) t A matrix is assigned for thrust forces.
In combination, the total thrust value of the ith propeller of the underwater hexapod robot is T i ＝T i b +T i a (i＝1,2,…,8)。
For the exemplary embodiment of the invention, in S4, aiming at the crossing gait, according to the wall inclination angle and the current pitch angle of the underwater hexapod robot, the mapping relation between the oscillator output signal and the blade leg expected joint angle is designed, and a crossing gait planning method based on CPG is provided; on the basis, in order to improve the stability of crossing among the wall surfaces with different inclination angles of the underwater hexapod robot, on one hand, basic thrust is applied to the propeller of the underwater hexapod robot, so that the blade legs of the underwater hexapod robot are not slipped during crossing, and on the other hand, the transverse rolling of the underwater hexapod robot during crossing is reduced, and according to the current transverse rolling angle of the underwater hexapod robot, additional thrust is applied to the propeller of the underwater hexapod robot so as to overcome the transverse rolling of the underwater hexapod robot.
With the Hopf oscillator described above, the output of the ith Hopf oscillator [ u ] i v i ] T The relation between the robot ith leg and the expected joint angle is mapped as follows
Wherein, delta theta i Is the wall surface normal line and the body coordinate system O b Y b Included angle of axial negative direction satisfies The inclination angle of the wall surface where the ith ground contact leg is positioned; θ is the pitch angle of the underwater hexapod robot; θ si The support angle of the ith leg is the angle through which the ith leg rotates in the support phase stage; θ ti Is the swing angle of the ith leg, namely the angle through which the ith leg rotates in the swing phase stage, and meets theta ti ＝2π-θ si +kΔθ i K is a constant greater than zero.
When the underwater hexapod robot spans between walls with different inclination angles, the inclination angles of the walls where the front legs and the rear legs of the robot are positioned are different, and the underwater hexapod robot body is a rigid body, so that the displacement generated by hip joints of the front legs and the rear legs of the robot is different under the condition that the front legs and the rear legs do not slip. The hip joint displacement corresponds to the supporting angle of the blade leg, so that the blade leg rolling on the wall surface 1 aims at meeting the motion constraint of the underwater hexapod robotSupporting angleBlade leg support angle with rolling on wall 2 +.>The following relationship should be satisfied
Wherein L is the distance between the hip joint of the front leg and the hip joint of the rear leg of the underwater hexapod robot,the inclination angles of the wall surface 1 and the wall surface 2 are respectively.
PD control is adopted to enable the blade leg of the underwater robot to expect joint angles Conversion into blade leg joint torque tau i (i=1, …, 6), the control law is as follows
Wherein k is p 、k d Q is the control parameter to be adjusted i 、The real joint angle and the real joint angular velocity of the blade leg are respectively.
Meanwhile, in order to realize stable crossing of the underwater hexapod robot, the propeller can provide enough positive pressure to ensure that the blade legs do not slip and roll along the wall surface.
According to the joint torque tau of the ground contact blade leg i Can be obtained
Wherein F is τix 、F τiy The components of the blade leg joint torque to the force at the touchdown point are in the touchdown point coordinate system respectively.
Obtaining the vertical acting force F required by the blade leg touchdown point according to the non-slip constraint Tiy Concretely, the method is as follows
Wherein μ is the ground friction coefficient; n (N) i Is ground supporting force applied to the ground contact point.
F according to D-H coordinate transformation Tiy With the thrust T of the propeller borne by the mass center of the robot x 、T y 、T z Satisfy the following relation
Wherein a is i The ground contact state of the ith blade leg,is a rotation matrix from the touchdown point coordinate system to the body coordinate system.
When the underwater hexapod robot spans on walls with different inclinations by adopting a tripodal gait, always the front single leg (leg 1 or leg 2) of the underwater hexapod robot spans firstly, so as to reduce the roll angle of the robot in the spanning stageThe following roll control law is designed
Wherein,,is a constant greater than zero.
Therefore, the thrust of each propeller of the underwater robot should be
Wherein T is i (i=1, 2, …, 8) is the thrust value of the i-th propeller; c (C) t A matrix is assigned for thrust forces.
(3) Advantageous effects
The beneficial effects of the invention are mainly represented by the following two aspects:
(1) The method for planning the climbing gait of the underwater hexapod robot based on the CPG comprises the steps of adjusting a blade leg supporting angle according to a course error, and generating the climbing gait with a directional function based on the CPG; meanwhile, given the thrust of the basic propeller and according to the current attitude angle of the underwater robot, calculating additional thrust to offset the influence of restoring force and restoring moment, so that the underwater robot can stably climb on the wall surfaces with different dip angles;
(2) The method comprises the steps of providing a CPG-based underwater hexapod robot crossing gait planning method, adjusting a blade leg supporting angle according to a wall inclination angle and a current attitude angle of the underwater robot, and designing a mapping relation between CPG output signals and a blade leg expected joint angle to adjust a supporting phase position so as to generate a crossing gait; meanwhile, the thrust of the propeller and the roll control moment which are suitable for the torque of the blade leg joint are calculated, so that the stability of the underwater robot in the crossing process between wall surfaces with different dip angles is ensured.
Drawings
Fig. 1 is a schematic diagram of a hybrid drive underwater hexapod robot wall climbing and crossing motion planning and control method.
Fig. 2 is a schematic view of a hexapod robot and inclined working surface under a Gazebo simulation platform.
Fig. 3 (a) and (b) are a schematic view of the hip joint position of six legs of the underwater hexapod robot and a schematic view of the position of eight thrusters of the underwater hexapod robot, respectively.
Fig. 4 is a schematic diagram of the positional relationship between the underwater hexapod robot and the inclined wall surface.
Fig. 5 is a schematic diagram of a geodetic coordinate system, an underwater hexapod robot coordinate system, a hip joint coordinate system, and a touchdown point coordinate system.
Fig. 6 is a schematic diagram of the relationship between hip joint and wall surface of the underwater hexapod robot in the climbing stage.
Fig. 7 is a schematic diagram of the displacement relationship of the hip joints of the underwater hexapod robot at the stage of crossing the wall surfaces with different inclinations.
Fig. 8 is a schematic diagram of a blade leg kinematics analysis of an underwater hexapod robot.
Fig. 9 (a) and (b) are schematic views of the articulation of the blade leg of the underwater hexapod robot across the wall surfaces of different inclinations.
Fig. 10 is a schematic diagram of a force analysis at a blade leg touchdown point of an underwater hexapod robot.
Fig. 11 is a schematic diagram of the relationship between hip joint and wall surface of the underwater hexapod robot in the crossing stage.
Fig. 12 is a three-dimensional motion trajectory diagram when an underwater hexapod robot climbs.
Fig. 13 is a horizontal plane motion trajectory graph and a vertical plane motion trajectory graph when the underwater hexapod robot climbs.
Fig. 14 is a course angle change curve and a course angle error change curve of the underwater hexapod robot.
Fig. 15 is a pitch angle change curve and a roll angle change curve of the underwater hexapod robot.
Fig. 16 (a) and (b) are partial enlarged views of the desired blade leg joint angle change curve and the desired blade leg joint angle in the crossing stage of the underwater hexapod robot, respectively.
Fig. 17 is a graph showing the actual value change of the angle of the blade leg joint of the underwater hexapod robot.
Fig. 18 is a graph showing torque variation of a blade leg joint of the underwater hexapod robot.
Fig. 19 (a) and (b) are a vertical thrust curve and a main thrust curve of the underwater hexapod robot, respectively.
Fig. 20 (a) - (i) are schematic diagrams of the results of the motion of the hexapod robot under water in the Gazebo simulation platform.
Detailed Description
The invention will now be further described with reference to examples, figures:
fig. 1 is a schematic diagram of a hybrid-driven underwater hexapod robot wall climbing and crossing motion planning and control method. Firstly, according to the forward distance d between the mass center of the underwater hexapod robot and the wall surface, the pitch angle theta of the underwater hexapod robot and the inclination angle of the wall surface k where the underwater hexapod robot is currently positioned Error between them, setting switch signal c for characterizing gait switching 1 、c 2 A climbing gait or a striding gait is selected. Aiming at climbing gait, introducing heading errors into blade leg supporting angle design, so as to realize directional control; in addition, in order to ensure that the blade legs and the ground have enough positive pressure so that the blade legs do not slip, a certain basic thrust is set, and meanwhile, the underwater six-foot robot can be subjected to the action of restoring force and restoring moment when working on the wall surfaces with different inclined angles, and in order to reduce the influence of the action, the thrust of an additional propeller is calculated according to the current attitude angle of the underwater six-foot robot so as to overcome the restoring force and the restoring moment. Aiming at the crossing gait, according to the inclination angle of the wall surface>Adjusting the blade leg joint support angle, the swing angle and the support phase position by using the pitch angle theta of the underwater six-foot robot and the kinematic constraint of the underwater six-foot robot, and then designing the mapping relation between the CPG output signal and the blade leg expected joint angle to generate a crossing gait; meanwhile, the propeller thrust acting on the mass center of the underwater hexapod robot is transferred to the blade leg touchdown point through D-H coordinate transformation, so that the propeller thrust required by meeting the longitudinal non-slip constraint of the supporting leg is solved; in addition, roll control is designed to reduce the roll angle of the underwater hexapod robot brought by the crossing gait >Wave motion.
Fig. 2 is a schematic view of an underwater hexapod robot and inclined wall surface under a Gazebo simulation platform, which simulates an underwater climbing environment of the underwater hexapod robot.
FIG. 3 (a) is a schematic view of the positions of the hip joints of 6 legs of an underwater hexapod robot, the left and right sides (upper and lower) of the hip joint being along the axis O b Z b The direction distance is d 1 Hip joint of front leg and middle leg, hip joint edge O of middle leg and rear leg b X b The direction distance is d 2 . FIG. 3 (b) is a schematic view of the positions of 8 propellers of an underwater hexapod robot, wherein 4 propellers are vertically distributed and 4 propellers are horizontally vector-distributed, the 8 propellers are embedded in the robot body, and two vertical propellers are oppositely arranged along O b Z b The direction distance is l 1 Edge O b X b The direction distance is l 2 The vertical propeller positive slurry direction is vertical to the main body and is directed downwards. Two horizontal propeller edges O arranged oppositely b Z b The direction distance is l 3 Edge O b X b The direction distance is l 4 The positive slurry direction of the horizontal propeller is shown as arrow direction in the figure, namely the horizontal propeller is along O b X b The direction is set at 45 degrees.
Fig. 4 is a schematic diagram of the positional relationship between the underwater hexapod robot and the inclined wall surface. In the embodiment, C is the position of the mass center of the underwater hexapod robot, namely the mounting position of the range finder for obtaining the forward distance between the mass center and the wall surface of the underwater hexapod robot. Is the inclination angle of the inclined wall surface k to be climbed. According to the distance information of the mass center and the inclined wall surface of the underwater hexapod robot acquired by the distance measuring equipment, a switching signal c representing the change from climbing to crossing gait is set 1 For judging whether to start crossing the inclined plane, the switch signal is
Wherein d is the distance from the mass center of the underwater hexapod robot to the inclined wall surface; d is a distance threshold.
According to the pitch angle theta of the underwater hexapod robot and the wall to be climbedInclination angle of face kError between, setting a switching signal c characterizing the switch from the step-over climbing gait 2 Specifically, it is
Wherein,,is the error threshold.
Fig. 5 is a schematic diagram of a geodetic coordinate system, an underwater hexapod robot coordinate system, a hip joint coordinate system, and a touchdown point coordinate system. Geodetic coordinate system O 0 X 0 Y 0 Z 0 Is fixedly connected with the earth and is static relative to the ground. Body coordinate system O b X b Y b Z b Is fixedly connected with the underwater hexapod robot body, is static relative to the underwater hexapod robot body, and the origin of a body coordinate system is selected from the gravity center of the underwater hexapod robot body, O b X b The shaft points forward along the longitudinal axis of the underwater robot body, O b Y b The axis being perpendicular to O b X b The axis points upwards, O b Z b The axis being perpendicular to axis O b X b And O b Y b The direction of which satisfies the right-hand coordinate system rotation law. Hip joint coordinate system O 1i X 1i Y 1i Z 1i The robot is fixedly connected with the robot body, the origin of coordinates is selected at the hip joint of each blade leg, and the directions of axes are consistent with the directions of coordinate axes of a body coordinate system. Touchdown point coordinate system O 3i X 3i Y 3i Z 3i The origin of coordinates is selected at the touchdown point of each blade leg, O 3i X 3i The axis is arranged on the intersection line of the plane of the ground contact leg and the wall surface and points to the advancing direction of the robot, O 3i Y 3i The axis is directed upwards perpendicular to the wall surface, O 3i Z 3i The axis being perpendicular to axis O 3i X 3i And O 3i Y 3i The direction of which satisfies the right-hand coordinate system rotation law. O (O) 2i X 2i Y 2i Z 2i The coordinate origin of the coordinate system is selected at the hip joint of each blade leg, and the directions of all axes are consistent with the coordinate axis directions of the coordinate system of the touchdown point.
Aiming at climbing gait, a knife-edge leg support angle change rule is designed according to course errors, and a CPG-based tripodia gait planning method is provided; on the basis, in order to improve the climbing stability of the underwater hexapod robot, on one hand, basic thrust is applied to the propeller of the underwater hexapod robot, so that the blade legs of the underwater hexapod robot are not slipped during climbing, and on the other hand, the influence of restoring force and restoring moment on the stable climbing of the underwater hexapod robot is reduced, and according to the current attitude angle of the underwater hexapod robot, additional thrust is applied to the propeller of the underwater hexapod robot so as to overcome the restoring force and restoring moment. The method comprises the following steps:
In order to realize stable climbing and crossing of the underwater hexapod robot on different wall surfaces, the invention adopts a tripodal gait, namely, legs 2, 3 and 6 and legs 1, 4 and 5 move in an alternate ground contact/suspension swing mode, and therefore, a CPG-based gait planning method is adopted. The CPG gait generator consists of 6 Hopf oscillators, and the 6 Hopf oscillators correspond to the 6 blade legs respectively. The mathematical model of the ith (i=1, 2, …, 6) Hopf oscillator can be expressed as
Wherein X is i ＝[u i v i ] T In the state of the ith oscillator, sigma is a convergence factor, R is the amplitude of the output of the oscillator, w is the oscillation frequency of the oscillator, lambda is a coupling coefficient,the effect of the jth (j=1, 2, …, 6) oscillator on the ith oscillator in the CPG network can be expressed as
Wherein,,the phase difference of the jth oscillator to the ith oscillator.
Definition of the desired articulation angle for the ith blade legIs a blade leg tangent line at the hip joint and a body coordinate system O b X b And an included angle in the positive direction of the shaft. In order to reduce the up-and-down fluctuation of the mass center of the underwater hexapod robot along the direction vertical to the wall surface in the climbing process, the position of the blade leg at the beginning time of the supporting phase and the position of the blade leg at the ending time of the supporting phase are required to be symmetrically distributed relative to the normal line of the wall surface. In order to improve the climbing stability of the underwater hexapod robot, it is desirable that the underwater robot body is parallel to the wall surface during climbing, so that the joint angle corresponding to the center position of the support center is q i Pi. For this purpose, the output [ u ] of the ith Hopf oscillator is set i v i ] T The relation between the expected joint angle of the ith leg of the underwater hexapod robot and the relation between the expected joint angle of the ith leg of the underwater hexapod robot is mapped as follows
Wherein,,a desired joint angle for the ith leg; θ si The support angle of the ith leg is the angle through which the ith leg rotates in the support phase stage; θ ti Is the swing angle of the ith leg, namely the angle through which the ith leg rotates in the swing phase stage, and meets theta ti ＝2π-θ si 。
According to the supporting phase and the swinging phase of the blade leg, a variable a representing whether the blade leg touches the ground or not is defined i When the angle of the blade leg joint rotates to the supporting phase, the blade leg touches the ground, otherwise the blade leg is suspended, which is as follows
In order to enable the underwater hexapod robot to have the directional climbing function, the supporting angle of the ith leg can be designed as
θ si ＝k ψ M(i)(ψ-ψ d )+θ s0
Wherein k is ψ Is a constant greater than zero, ψ d The expected course angle of the underwater hexapod robot is, and psi is the actual course angle of the underwater hexapod robot, theta s0 For the initial value of the support angle, M (i) may be defined as
PD control is adopted to enable the blade leg of the underwater hexapod robot to have the expected joint angleConversion into blade leg joint torque tau i (i=1, …, 6), the control law is as follows
Wherein k is p 、k d Q is the control parameter to be adjusted i 、The real joint angle and the real joint angular velocity of the blade leg are respectively.
In order to improve climbing stability of the underwater hexapod robot, a basic thrust T is applied to a propeller of the underwater hexapod robot i b (i=1, …, 8) to ensure no slip of blade legs of the underwater hexapod robot during climbing, and on the other hand, to reduce the influence of restoring force and restoring torque on stable climbing of the underwater hexapod robot, to apply additional thrust T to a propeller of the underwater hexapod robot according to the current attitude angle of the underwater hexapod robot i a (i=1, …, 8) to overcome the restoring force and the restoring moment. The propulsor thrust is thus made up of two parts, the basic thrust and the additional thrust.
As shown in fig. 6, when the underwater hexapod robot climbs the wall, the robot body is parallel to the wall, i.e. the pitch angle of the robot is equal to the inclination angle of the wall, so that
According to the joint torque tau of the ground contact blade leg i Can be obtained
Wherein F is τix 、F τiy Respectively the components of the torque of the blade leg joint to the acting force at the touchdown point under the touchdown point coordinate system, l r Is the radius of the blade leg.
Obtaining the vertical acting force F required by the blade leg touchdown point according to the non-slip constraint Tiy Concretely, the method is as follows
Wherein μ is the ground friction coefficient; n (N) i Is ground supporting force applied to the ground contact point.
In FIG. 6, L xi L is the distance between the blade leg hip joint and the ground contact point along the wall surface direction yi Is the distance between the blade leg hip joint and the ground contact point along the direction vertical to the wall surface.
According to D-H coordinate transformation, the rotation matrix from the touchdown point coordinate system to the body coordinate system is obtainedIs that
Wherein s (alpha) and c (alpha) respectively represent the sine value and the cosine value of the angle alpha,is the roll angle of the underwater hexapod robot.
F according to D-H coordinate transformation Tiy With the thrust T of the propeller borne by the mass center of the robot x 、T y 、T z Satisfy the following relation
Wherein a is i The ground contact state of the ith blade leg,is a rotation matrix from the touchdown point coordinate system to the body coordinate system.
Therefore, the underwater hexapod robot has 8 propeller basic thrust forces T i b (i=1, …, 8) is:
wherein T is i b (i=1, 2, …, 8) is the base thrust value of the i-th propeller; c (C) t Distributing a matrix for thrust, which may be expressed as
Wherein,,l i (i=1, 2,3, 4) is the distance between each propeller of the underwater robot.
Restoring force F of underwater hexapod robot gx 、F gy 、F gz And a restoring moment M gx 、M gy 、M gz (component of the buoyancy moment in the body coordinate system) is as follows
Wherein B is the buoyancy of the robot, G is the gravity of the robot, θ is the pitch angle of the underwater robot body,is the roll angle, x of the underwater robot body c ,y c ,z c Is the coordinates of the floating center of the underwater robot in a body coordinate system.
Therefore, the extra thrust T of 8 propellers of the underwater hexapod robot i a (i=1, …, 8) is:
wherein T is i a (i=1, 2, …, 8) is the additional thrust value of the i-th propeller; c (C) t A matrix is assigned for thrust forces.
In summary, the total thrust value of the ith propeller is T i ＝T i b +T i a (i＝1,2,…,8)。
Aiming at the crossing gait, according to the wall inclination angle and the current pitch angle of the underwater hexapod robot, designing a mapping relation between an oscillator output signal and a blade leg expected joint angle, and providing a CPG-based crossing gait planning method; on the basis, in order to improve the stability of crossing among the wall surfaces with different inclination angles of the underwater hexapod robot, on one hand, basic thrust is applied to the propeller of the underwater hexapod robot, so that the blade legs of the underwater hexapod robot are not slipped during crossing, and on the other hand, the transverse rolling of the underwater hexapod robot during crossing is reduced, and according to the current transverse rolling angle of the underwater hexapod robot, additional thrust is applied to the propeller of the underwater hexapod robot so as to overcome the transverse rolling of the underwater hexapod robot. The method comprises the following steps:
when the front leg of the underwater hexapod robot contacts the wall surface and starts to span between the wall surfaces with different inclination angles, as shown in fig. 7, the ground contact leg runs out of one supporting phase, the underwater hexapod robot body moves from the position 1 to the position 2, the blade leg hip joints move from A, B, C to A ', B ', C ', a is the distance that the front leg hip joint moves along the wall surface 2, B is the distance that the rear leg hip joint moves along the wall surface 1, and L is the distance between the front leg hip joint and the rear leg hip joint of the underwater hexapod robot. Because the inclination angles of the wall surfaces of the front leg and the rear leg of the robot are different, and the underwater robot body is a rigid body, under the condition of no slip, after the mass center of the underwater robot moves, the rolling distances of the hip joints of the front leg and the rear leg of the underwater robot along the wall surfaces of the front leg and the rear leg of the underwater robot are different, namely a and b meet the following constraint
Fig. 8 is a schematic diagram of a blade leg kinematics analysis of an underwater hexapod robot. As shown in fig. 8, the position 1 is a position where the supporting phase of the blade leg starts, and at this time, the contact point of the blade leg is point P; position 2 is the blade leg supporting phase end position, and the blade leg touchdown point is the Q point this moment, and position 3 is blade leg supporting phase central point position, and blade leg diameter is perpendicular to the wall under this position. From the mathematical relationship, the touchdown point rolls from P to Q, the blade leg goes through the process of position 1-position 3-position 2, wherein the touchdown point in the stage 1-position 3 is changed at all times, the moving distance of the hip joint S is the sum of the length of the arc AP and the distance from the P point to the line segment SM, the touchdown point in the stage 3-position 2 is not changed any more, and the moving distance of the hip joint S is the distance from the Q point to the line segment SM, so the moving distance l of the hip joint in the whole support phase is the same x Is that
Wherein θ si The angle of the ith blade point leg is the angle through which the ith blade point leg rotates in the supporting phase.
As can be seen from the above relationship, FIG. 7Blade leg support angle for rolling on wall 1>Blade leg support angle with rolling on wall 2 +.>The following relationship should be satisfied
Wherein,,the inclination angles of the wall surface 1 and the wall surface 2 are respectively.
When the front leg of the underwater hexapod robot contacts the next wall and starts to span between walls with different inclinations, the blade leg joint movement schematic of the underwater hexapod robot is shown in fig. 9 (a). When the underwater hexapod robot encounters a wall surface with abrupt inclination, if the original climbing gait is kept to continue to move, the blade leg support phase cannot be guaranteed to roll along the wall surface, and conversely, the blade leg can contact the wall surface in a swinging way, and as the swinging phase angle speed is too high, the ground acting force born by the underwater hexapod robot in the moment of contact with the wall surface is too high, so that the underwater hexapod robot can roll over or skid, and cannot climb up the next wall surface. In addition, if the underwater hexapod robot has both blade leg touchdown in the swing phase and blade leg touchdown in the support phase, the three-foot gait constraint of the underwater hexapod robot cannot be met, so that the underwater hexapod robot cannot realize the crossing between wall surfaces with different dip angles. Therefore, in order to improve the different inclinations of the underwater hexapod robot The stability and success rate of crossing between the angular wall surfaces, and the fluctuation of the mass center of the underwater hexapod robot along the direction vertical to the wall surfaces in the motion process are reduced, and the joint angles corresponding to the starting time and the ending time of the blade leg support phase are required to be changed, so that the position of the blade leg at the starting time of the support phase and the position of the blade leg at the ending time of the support phase are symmetrically distributed relative to the normal line of the wall surfaces, namely the blade leg support correspondingly rotates anticlockwise by delta theta i ，Δθ i Is the wall surface normal line and the body coordinate system O b Y b The angle between the axial and negative directions is known from mathematical relationship The inclination angle of the wall surface where the ith ground contact leg is positioned.
As the underwater hexapod robot gradually climbs up the wall surface 2, the pitch angle of the underwater hexapod robot gradually approaches the inclination angle of the wall surface 2, so that the pitch angle is |delta theta i As shown in fig. 9 (b), the phase of the swing of the blade leg is reduced to 0, and the starting position of the supporting phase of the next cycle is changed, and the change is the change of the pitch angle of the robot in the current cycle. Therefore, in order to ensure that the swing phase and the support phase of the next period are continuously and alternately performed, the swing angle is slightly larger than 2 pi-theta under the condition of constant support angle si To simplify the problem, kΔθ is used i Characterizing the change in the swing angle, k is a constant greater than zero.
Thus, the output of the ith Hopf oscillator [ u ] i v i ] T The relation between the expected joint angle of the ith leg of the underwater hexapod robot and the relation between the expected joint angle of the ith leg of the underwater hexapod robot is mapped as follows
Wherein, delta theta i Is the wall surface normal line and the body coordinate system O b Y b An included angle in the axial negative direction; θ si Is the supporting angle of the ith ground contact leg, namely the angle through which the ith leg rotates in the supporting phase stage；θ ti Is the swing angle of the ith leg, namely the angle through which the ith leg rotates in the swing phase stage, and meets theta ti ＝2π-θ si +kΔθ i 。
PD control is adopted to enable the blade leg of the underwater robot to expect joint anglesConversion into blade leg joint torque tau i (i=1, |, 6), the control law is as follows
Wherein k is p 、k d Q is the control parameter to be adjusted i 、The real joint angle and the real joint angular velocity of the blade leg are respectively.
Meanwhile, in order to ensure the stability of the movement of the underwater robot in the crossing stage, the propeller can provide enough positive pressure to ensure that the blade legs of the robot do not slip when rolling along the wall surface. As shown in fig. 10, the blade leg joint torque is τ i ，Therefore(s)>Therefore, the acting force of the blade leg torque to the touchdown point is
Wherein F is τxi 、F τyi The components of the blade leg joint torque to the force at the touchdown point are in the touchdown point coordinate system respectively.
Force analysis at the blade leg touchdown point as shown in FIG. 10, the required sag for the blade leg touchdown point is determined from the non-slip constraint Force F to Tiy Concretely, the method is as follows
Wherein μ is the ground friction coefficient; n (N) i Is ground supporting force applied to the ground contact point.
The position relationship between the blade leg hip joint and the wall surface is shown in FIG. 11, L xi L is the distance between the blade leg hip joint and the ground contact point along the wall surface direction yi Is the distance between the blade leg hip joint and the ground contact point along the direction vertical to the wall surface.
Wherein,,the inclination angle of the wall surface where the ith ground contact leg is positioned.
Coordinate transformation matrix from touchdown point coordinate system to body coordinate system according to D-H coordinate transformationIs that
Wherein s (alpha) and c (alpha) respectively represent the sine value and the cosine value of the angle alpha, g 1 (i) And g 2 (i) Respectively defined as
Thus, touchdown point coordinatesOrigin of the system (x) 3i ,y 3i ,z 3i ) Can be expressed as in a volumetric coordinate system
Rotation matrix of touchdown point coordinate system to body coordinate systemIs that
F according to D-H coordinate transformation Tiy With the thrust T of the propeller borne by the mass center of the robot x 、T y 、T z Satisfy the following relation
Wherein a is i The contact state of the ith blade leg is the ground contact state.
When the underwater hexapod robot spans on walls with different inclinations by adopting a tripodal gait, always the front single leg (leg 1 or leg 2) of the underwater hexapod robot spans firstly, so as to reduce the roll angle of the robot in the spanning stageThe following roll control law is designed
Wherein,,is a constant greater than zero.
Therefore, the thrust of each propeller of the underwater robot should be
Wherein T is i (i=1, 2, …, 8) is the thrust value of the i-th propeller; c (C) t A matrix is assigned for thrust forces.
In the present embodiment, the simulation step size is 0.1s, and the inclination angle of the wall surface 2 is set to beInclination angle of wall surface 1The desired heading angle is ψ d =0rad, initial heading angle ψ of underwater robot init =0.37 rad, initial blade leg support angle θ s0 =0.78 rad, friction coefficient μ=0.2 in simulation environment, control parameter is set to k=0.2, k ψ ＝5，k p ＝20，k d =0.3. Fig. 12 to 20 are simulation results.
Fig. 12 is a three-dimensional motion trajectory diagram when an underwater hexapod robot climbs. Fig. 13 is a horizontal plane motion trajectory graph and a vertical plane motion trajectory graph when the underwater hexapod robot climbs. According to the three-dimensional motion track diagram, the underwater hexapod robot can be continuously adjusted according to the expected course angle, and the crossing and climbing of the wall surfaces with different dip angles can be completed. As can be seen from fig. 13, on the inclined wall surface, the underwater hexapod robot is located along the geodetic coordinate system O 0 Y 0 The axis climbs 5.5m in the forward direction.
Fig. 14 is a course angle change curve and a course angle error change curve of the underwater hexapod robot. As can be seen from the results of the drawing, the underwater hexapod robot can track a desired heading angle when the wall surface 1 moves. In t=65s ~ 75s, six sufficient robots under water enter the stage of crossing, the course appears undulant, and course error increases, after t=75s, six sufficient robots under water have climbed the second wall and readjust the course in order to track the expected value, and course error remains within 0.05 rad.
Fig. 15 is a pitch angle change curve and a roll angle change curve of the underwater hexapod robot. As shown in the figure, the underwater hexapod robot moves in the wall surface 1 before t=65s, and the pitch angle starts to change at t=65s, and continuously adapts to the inclination angle of the wall surface 2, and is switched from the wall surface 1 to the inclination angle of the wall surface 2About 10s after which the pitch angle does not fluctuate much. The illustrated result shows that the underwater hexapod robot can span between walls with abrupt inclination angles. During the crossing phase, the roll angle fluctuates due to the use of a tripodal gait, and the roll angle does not fluctuate by more than 0.2rad under the roll control described above.
Fig. 16 (a) and (b) are partial enlarged views of the desired blade leg joint angle change curve and the desired blade leg joint angle in the crossing stage of the underwater hexapod robot, respectively. As can be seen from fig. 16 (b), the support phase position, the support angle size, and the swing angle size are changed according to theoretical analysis. For a blade leg rolling along the wall 1,so that its support will rotate clockwise, for the blade leg rolling along the wall 2 +.>Therefore, the supporting phase can rotate anticlockwise, and the simulation result accords with theoretical analysis.
Fig. 17 is a graph showing the actual values of the angles of the blade legs of the underwater hexapod robot, from which it can be seen that the actual angles of the joints of the six legs of the underwater hexapod robot can track the desired angles of the joints in fig. 16 (a) above. Fig. 18 is a graph showing the response of the actual joint torque change of the blade leg of the underwater hexapod robot.
Fig. 19 (a) and (b) are a vertical thrust curve and a main thrust curve of the underwater hexapod robot, respectively. The illustrated results show that in the period of t=65s to 75s, the thrust of the propeller is continuously adjusted to ensure the stability of the robot, and after t=75s, the underwater hexapod robot climbs the wall surface 2, the attitude angle of the underwater hexapod robot is stable, and therefore the propeller can provide almost constant thrust to offset the influence of the restoring force/restoring moment of the robot body.
Fig. 20 is a schematic diagram of the result of the motion of the six-legged underwater robot in the Gazebo simulation platform. Fig. 20 (a) to 20 (c) show the directional motion process of the underwater robot on the wall surface 1, and the heading of the robot is gradually adjusted to 0 from an initial value as shown in the figures. Fig. 20 (d) to 20 (h) show the movement process of the underwater robot in the crossing stage, and fig. 20 (i) shows the movement process of the underwater robot on the wall surface 2. Therefore, the underwater hexapod robot can stably finish the tasks of crossing and climbing the wall surfaces with different inclinations.
In summary, the gait planning method and the control method provided by the invention can enable the underwater robot to finish the task of crossing and climbing the wall surfaces with different inclinations.
The present invention is not limited to the above-mentioned embodiments, but is intended to be limited to the following embodiments, and any modifications, equivalents and modifications can be made to the above-mentioned embodiments without departing from the scope of the invention.
Claims (4)
1. An underwater six-foot robot wall climbing and crossing motion planning and control method, the underwater robot comprises: 8 propellers, 6 semicircular arc type blade legs and a robot body, wherein 4 propellers are vertically distributed, 4 propellers are horizontally distributed in vector, 6 semicircular arc type blade leg hip joints are connected with 6 driving motors on the left side and the right side of the robot body, and the method for planning and controlling the movement of the wall surface of the underwater robot is characterized by comprising the following steps:
s1: aiming at a wall surface with a known inclination angle, acquiring the forward distance from the mass center of the underwater hexapod robot to the wall surface;
s2: according to the forward distance between the mass center and the wall surface and the error between the pitch angle of the underwater hexapod robot and the inclination angle of the wall surface, the climbing gait or the crossing gait is adopted;
s3: aiming at climbing gait, a blade leg support angle change rule is designed according to course errors, and a CPG-based tripodia gait planning method is obtained; according to the current attitude angle of the underwater hexapod robot, applying additional thrust to a propeller of the underwater hexapod robot so as to overcome restoring force and restoring moment;
s4: aiming at the crossing gait, according to the wall inclination angle and the current pitch angle of the underwater hexapod robot, designing a mapping relation between an oscillator output signal and a blade leg expected joint angle to obtain a crossing gait planning method based on CPG; according to the current roll angle of the underwater hexapod robot, applying additional thrust to a propeller of the underwater hexapod robot so as to overcome the roll of the underwater hexapod robot;
Wherein, step S3 includes:
the six-foot robot adopts a CPG-based gait planning method, wherein the CPG gait generator consists of 6 Hopf oscillators, which respectively correspond to 6 semicircular arc blade legs, and the mathematical model of the ith Hopf oscillator can be expressed as:
wherein X is i ＝[u i v i ] T In the state of the ith oscillator, sigma is a convergence factor, R is the amplitude of the output of the oscillator, w is the oscillation frequency of the oscillator, lambda is a coupling coefficient,the effect of the jth oscillator on the ith oscillator in the CPG network can be expressed as:
wherein,,i=1, 2 for the phase difference of the j-th oscillator to the i-th oscillator, 6,j =1, 2, 6;
output of ith Hopf oscillator [ u ] i v i ] T The relationship with the expected joint angle of the ith leg of the underwater hexapod robot is mapped as:
wherein θ si Is the support angle of the ith leg; θ ti Is the swing angle of the ith leg and satisfies theta ti ＝2π-θ si ；
According to the supporting phase and the swinging phase of the blade leg, a variable a representing whether the blade leg touches the ground or not is defined i When the angle of the blade leg joint rotates to the supporting phase, the blade leg touches the ground, otherwise, the blade leg is suspended, and the method is as follows:
in order to enable the underwater hexapod robot to have the directional climbing function, the supporting angle of the ith leg can be designed as
θ si ＝k ψ M(i)(ψ-ψ d )+θ s0
Wherein k is ψ Is a constant greater than zero, ψ d The expected course angle of the underwater hexapod robot is, and psi is the actual course angle of the underwater hexapod robot, theta s0 For the initial value of the support angle, M (i) may be defined as
PD control is adopted to enable the blade leg of the underwater hexapod robot to have the expected joint angleConversion into blade leg joint torque tau i I=1,..6, control law is as follows
Wherein k is p 、k d Q is the control parameter to be adjusted i 、The real joint angle and the real joint angular velocity of the blade leg are respectively;
according to the joint torque tau of the ground contact blade leg i Can be obtained
Wherein F is τix 、F τiy Respectively the components of the torque of the blade leg joint to the acting force at the touchdown point under the touchdown point coordinate system, l r The radius of the blade leg of the underwater robot is the radius of the blade leg of the underwater robot;
obtaining the vertical acting force F required by the blade leg touchdown point according to the non-slip constraint Tiy Concretely, the method is as follows
Wherein μ is the ground friction coefficient; n (N) i Ground support forces applied at the touchdown point;
f according to D-H coordinate transformation Tiy With the thrust T of the propeller borne by the mass center of the robot x 、T y 、T z Satisfy the following relation
Wherein a is i The ground contact state of the ith blade leg,a rotation matrix from the touchdown point coordinate system to the body coordinate system;
therefore, the underwater hexapod robot has 8 propeller basic thrust forces T i b I=1,..8 is:
Wherein T is i b I=1, 2,..8 is the base thrust value of the i-th propeller; c (C) t Distributing a matrix for thrust;
restoring force F of underwater hexapod robot gx 、F gy 、F gz And a restoring moment M gx 、M gy 、M gz The following are provided:
wherein B is the buoyancy of the robot, G is the gravity of the robot, θ is the pitch angle of the underwater robot body,is the roll angle, x of the underwater robot body c ,y c ,z c Is the coordinates of the floating center of the underwater robot in a body coordinate system;
therefore, the extra thrust T of 8 propellers of the underwater hexapod robot i a I=1,..8 is:
wherein T is i a I=1, 2,..8 is the additional thrust value of the i-th propeller;
in combination, the total thrust value of the ith propeller of the underwater hexapod robot is T i ＝T i b +T i a ，i＝1,2,...,8。
2. The motion planning and control method of claim 1 wherein the forward distance from the center of mass of the underwater robot to the wall is obtained using a ranging device.
3. The motion planning and control method according to claim 1, characterized in that step S2 comprises:
according to the forward distance between the centroid of the underwater hexapod robot and the inclined wall surface, which is acquired by the distance measuring equipment, a switch signal c representing the change from climbing to gait crossing is set 1 Specifically, it is
Wherein D is the forward distance from the mass center of the underwater hexapod robot to the inclined wall surface, and D is a distance threshold;
According to the pitch angle theta of the underwater hexapod robot and the inclination angle of the wall surface k to be climbedError between, setting a switch signal c characterizing the switch from crossing to climbing gait 2 Specifically, it is
Where θ is the error threshold.
4. The motion planning and control method according to claim 1, characterized in that step S4 comprises:
the ith Hopf oscillatorOutput [ u ] i v i ] T The relationship with the desired joint angle of the i-th leg of the robot is mapped as:
wherein, delta theta i Is the wall surface normal line and the body coordinate system O b Y b Included angle of axial negative direction satisfiesThe inclination angle of the wall surface where the ith ground contact leg is positioned; θ is the pitch angle of the underwater hexapod robot; θ si Is the support angle of the ith leg; θ ti Is the swing angle of the ith leg and satisfies theta ti ＝2π-θ si +kΔθ i K is a constant greater than zero;
when the underwater robot spans between the wall surfaces with different inclination angles, the blade leg supporting angle rolling on the wall surface 1 is realized under the condition of no slipBlade leg support angle with rolling on wall 2 +.>The following relationship should be satisfied
Wherein L is the distance between the hip joint of the front leg and the hip joint of the rear leg of the underwater robot,inclination angles of the wall surface 1 and the wall surface 2 are respectively;
PD control is adopted to enable the blade leg of the underwater hexapod robot to have the expected joint angleConversion to knife edge Leg joint torque τ i I=1,..6, control law is as follows:
wherein k is p 、k d Q is the control parameter to be adjusted i 、The real joint angle and the real joint angular velocity of the blade leg are respectively;
according to the joint torque tau of the ground contact blade leg i Can be obtained
Wherein F is τix 、F τiy The components of the blade leg joint torque to the acting force at the touchdown point under the touchdown point coordinate system are respectively;
obtaining the vertical acting force F required by the blade leg touchdown point according to the non-slip constraint Tiy Concretely, the method is as follows
Wherein μ is the ground friction coefficient; n (N) i Ground support forces applied at the touchdown point;
f according to D-H coordinate transformation Tiy With the thrust T of the propeller borne by the mass center of the robot x 、T y 、T z Satisfy the following relation
Wherein a is i The ground contact state of the ith blade leg,a rotation matrix from the touchdown point coordinate system to the body coordinate system;
to reduce the roll angle of the robot in the crossing stageThe following roll control law is designed
Wherein,,a constant greater than zero;
therefore, the thrust of each propeller of the underwater robot should be
Wherein T is i I=1, 2,..8 is the thrust value of the i-th propeller; c (C) t A matrix is assigned for thrust forces.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111213908.7A CN113885540B (en) | 2021-10-19 | 2021-10-19 | Motion planning and control method for climbing and crossing of wall surface of underwater hexapod robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111213908.7A CN113885540B (en) | 2021-10-19 | 2021-10-19 | Motion planning and control method for climbing and crossing of wall surface of underwater hexapod robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113885540A CN113885540A (en) | 2022-01-04 |
| CN113885540B true CN113885540B (en) | 2023-07-25 |
Family
ID=79003345
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111213908.7A Active CN113885540B (en) | 2021-10-19 | 2021-10-19 | Motion planning and control method for climbing and crossing of wall surface of underwater hexapod robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113885540B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118090503B (en) * | 2024-04-29 | 2024-07-09 | 攀枝花市中西医结合医院 | Method and device for detecting sharpness of scissors |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20030063865A (en) * | 2002-01-24 | 2003-07-31 | 엘지전자 주식회사 | Move control method for robot |
| CN111949039A (en) * | 2020-09-09 | 2020-11-17 | 西北工业大学 | Semi-circular bionic blade leg-based six-legged robot course control method |
| CN112558622A (en) * | 2020-11-30 | 2021-03-26 | 华南理工大学 | Control method of foot end trajectory tracking controller of wall-climbing hexapod robot |
| CN113325859A (en) * | 2021-06-10 | 2021-08-31 | 西北工业大学 | Control method for vertically climbing and walking of walking hybrid underwater robot along inclined plane |
| CN113353217A (en) * | 2021-07-23 | 2021-09-07 | 西北工业大学 | Underwater robot directional and stable walking method |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110053039B (en) * | 2018-01-17 | 2021-10-29 | 深圳市优必选科技有限公司 | Gravity compensation method and device in robot walking and robot |
-
2021
- 2021-10-19 CN CN202111213908.7A patent/CN113885540B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20030063865A (en) * | 2002-01-24 | 2003-07-31 | 엘지전자 주식회사 | Move control method for robot |
| CN111949039A (en) * | 2020-09-09 | 2020-11-17 | 西北工业大学 | Semi-circular bionic blade leg-based six-legged robot course control method |
| CN112558622A (en) * | 2020-11-30 | 2021-03-26 | 华南理工大学 | Control method of foot end trajectory tracking controller of wall-climbing hexapod robot |
| CN113325859A (en) * | 2021-06-10 | 2021-08-31 | 西北工业大学 | Control method for vertically climbing and walking of walking hybrid underwater robot along inclined plane |
| CN113353217A (en) * | 2021-07-23 | 2021-09-07 | 西北工业大学 | Underwater robot directional and stable walking method |
Non-Patent Citations (7)
| Title |
|---|
| Adaptive sliding-mode attitude control for autonomous underwater vehicles with input nonlinearities;崔荣鑫;《Ocean Engineering》;全文 * |
| Control and Optimization of a Bionic Robotic Fish Through a Combination of CPG model and PSO;Ming Wang;《Neurocomputing》;全文 * |
| CPG Based Motion Control for an Underwater Thruster with Undulating Long-Fin;Xiang Dong;《IFAC Proceedings Volumes》;全文 * |
| Gait Generation With Smooth Transition Using CPG-Based Locomotion Control for Hexapod Walking Robot;Haitao Yu;《IEEE Transactions on Industrial Electronics》;全文 * |
| 基于CPG控制的六足爬壁机器人自主越障研究;王云倩;《 哈尔滨工程大学学报》;全文 * |
| 基于CPG的六足机器人运动步态控制方法;林知微;《计算机应用研究》;全文 * |
| 基于模型参考的多自主水下航行器自适应覆盖控制;严卫生;《系统工程与电子技术》;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113885540A (en) | 2022-01-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Siegwart et al. | Innovative design for wheeled locomotion in rough terrain | |
| CN106708068B (en) | Bionical fluctuation fin promotes submarine navigation device path tracking control method | |
| CN112148022A (en) | System and method for tracking and controlling recovery three-dimensional path of full-drive autonomous underwater robot | |
| CN106737659A (en) | A kind of hand ship control method for coordinating of underwater unmanned vehicle and arm-and-hand system | |
| CN111208845B (en) | Path planning method and device for movable butt joint recovery of underwater robot | |
| CN114022824A (en) | Narrow environment-oriented quadruped robot motion planning method | |
| CN113353217B (en) | Underwater robot directional and stable walking method | |
| CN108663938A (en) | A kind of UUV cluster-coordinator control methods considering communication topological transformation | |
| CN111857165B (en) | Trajectory tracking control method of underwater vehicle | |
| CN113885540B (en) | Motion planning and control method for climbing and crossing of wall surface of underwater hexapod robot | |
| Ma et al. | Cpg-based motion planning of hybrid underwater hexapod robot for wall climbing and transition | |
| Wettergreen et al. | Sun-synchronous robotic exploration: Technical description and field experimentation | |
| CN108829126A (en) | A kind of AUV cluster-coordinator control method considering communication delay | |
| Yu et al. | Hierarchical jumping optimization for hydraulic biped wheel-legged robots | |
| Vladareanu et al. | Modeling and hybrid position-force control of walking modular robots | |
| Bourbakis | Kydonas-an autonomous hybrid robot: walking and climbing | |
| CN113419545B (en) | Underwater robot optimization control method with slippage control | |
| CN102033545B (en) | Driving traction characteristic control method of wheel-legged robot | |
| Bian et al. | An autonomous underwater vehicle manipulator system for underwater target capturing | |
| Dettmann et al. | Towards a generic navigation and locomotion control system for legged space exploration | |
| Xiao | Development of miniature climbing robots: Modeling, control and motion planning | |
| CN115390573A (en) | Manta ray type bionic fish control method and device based on reinforcement learning and storage medium | |
| Chen et al. | Slip-Regulated Optimal Control for Hybrid-Driven Underwater Hexapod Robot With Thrusters and C-Shaped Legs | |
| Gim et al. | Ringbot: Monocycle Robot With Legs | |
| Liu et al. | Locomotion of Underwater Quadruped Robot Based on Ground Reaction Force Optimization |
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 |