A snake robot climbing control method for rescue or exploration

Abstract

The application discloses a snake-shaped robot climbing control method for rescue or detection, relates to the field of bionic robot control, and realizes that the snake-shaped robot is parallel to stairs through a series of instructions, such as action instruction 1: arc-shaped advancing; action instruction 2: resetting; action instruction 3: snake head structure climbing stairs; action instruction 4: unit joints of the snake body structure sequentially climbing stairs; action instruction 5: the 14th unit joint of the snake body structure climbing stairs, so that the whole snake body structure climbs on a step; and action instruction 6: arc-shaped rolling, thereby strengthening the adaptive capacity of the snake-shaped robot in a three-dimensional environment.

Description

Technical Field

[0001] This invention relates to the field of biomimetic robot control, specifically to a method for controlling the climbing of a snake-like robot used for rescue or exploration. Background Technology

[0002] As is well known, snakes have degenerated limbs and move using their bones and scales. When a snake moves forward, its ribs pull on its muscles, which in turn cause the scales to stand upright or lie horizontally. It glides forward using the friction between its belly and the ground. Due to the unique body structure and distinctive locomotion of snakes, research into them in the field of bionic robotics has become increasingly in-depth and widespread. Bionic snakes can be applied to rescue and exploration fields to replace humans in carrying out tasks in hard-to-reach locations.

[0003] Current research on snake-like robots largely focuses on gait studies in a two-dimensional plane, such as forward gait, obstacle avoidance, and obstacle crossing. Research on gait control in three-dimensional space is scarce. In complex and variable terrains, such as earthquake or fire scenes, snake-like robots cannot navigate freely using only two-dimensional pose and gait. Therefore, conducting research on the pose and gait of snake-like robots in three-dimensional space has significant practical implications.

[0004] With the climbing ability of snake-like robots as the research direction, it was found that there are currently no examples of snake-like robots climbing stairs when searching for literature. Therefore, it is proposed to propose a snake-like robot climbing control method for rescue or exploration. Summary of the Invention

[0005] This invention focuses on the climbing ability of snake-like robots. Targeting rescue and exploration environments, it simplifies the complex and ever-changing three-dimensional environment and selects stairs as a landmark unstructured terrain for research, enabling snake-like robots to climb stairs and enhancing their adaptability in three-dimensional environments.

[0006] A method for controlling the climbing of a snake-like robot for rescue or exploration, wherein the snake-like robot includes a snake head structure and a snake body structure;

[0007] The snake head structure includes a snake head drive servo, a three-dimensional angle sensor and an OpenMV drive servo, a snake head shell, an OpenMV, and a first U-shaped frame;

[0008] The OpenMV drive servo is fixedly connected inside the snake-head housing. The OpenMV drive servo rotor extends out of the snake-head housing and is fixedly connected to the 3D angle sensor and the OpenMV servo via a mounting plate. The snake-head drive servo is also fixedly connected to the snake-head housing, and a first U-shaped bracket is fixedly connected to the servo's shaft. The OpenMV drive servo is responsible for the connection between the 3D angle sensor and the OpenMV servo. Axial motion;

[0009] The snake's body structure includes ( Each unit joint comprises a vertical drive servo and a horizontal drive servo, orthogonally connected and mounted inside the unit joint housing. A second U-shaped frame and a third U-shaped frame are fixedly connected to the shafts of the vertical and horizontal drive servos, respectively. A connecting block is fixedly connected to the third U-shaped frame. The second U-shaped frame on the first unit joint is orthogonally connected to the first U-shaped frame. ( The connecting block on the unit joint and the adjacent unit joint ( The second U-shaped frame is fixedly connected to each unit joint;

[0010] Vertical drive servo motor The axis moves, providing vertical drive, while the snake-head drive servo and the horizontal drive servo rotate. The axis moves, providing horizontal drive;

[0011] Its attitude control method is as follows:

[0012] OpenMV detects the surrounding environment to determine if it belongs to a staircase structure, and if so, detects the height of the staircase. and the height of the stairs The data is sent back to the main controller to execute action command 1.

[0013] Action command 1 is to move forward in an arc, causing the snake-like robot, which is perpendicular to the stairs, to turn parallel to the stairs, making it easier for the snake-like robot to climb the stairs. The distance between the snake-like robot and the stairs is... ;

[0014] During the turning process, the three-dimensional angle sensor of the snake-head structure transmits data back in real time. Axis data is used to detect whether the snake robot turns to... If not arrived Position, execute forward steering; if beyond... Position, execute reverse turn; upon arrival When in position, execute action instruction 2;

[0015] Action instruction 2 is to reset, so that the snake robot's pose conforms to the robust structure, and the snake robot is parallel to the stairs. After the reset is completed, action instruction 3 is executed.

[0016] Action command 3 is for the snake-head structure to climb stairs, with the snake-head drive servo and the vertical drive servo of the second unit joint rotating. Simultaneously, the horizontal drive servo of the first unit joint rotates at an angle... ,angle From the height of the stairs The decision is made, satisfying the following formula:

[0017] ;

[0018] in The distance between the snake-like robot and the stairs;

[0019] Action instruction 4 involves the snake-like structure's unit joints climbing the stairs sequentially, the first... ( The vertical drive servo of the unit joint and the first ( Vertical drive servo motor rotation of each unit joint ,when At that time, the first The vertical drive servo of each unit joint and the first The horizontal drive servo motor of each unit joint resets; when It is only used to reset the snake head drive servo, and at the same time the first ( The horizontal drive servo motor rotation angle of each unit joint ,angle Similarly, satisfying the above formula, the first... The horizontal drive servo motor of each unit joint rotates at an angle. ,angle From the height of the stairs Decide and define intermediate variables Satisfy the following formula:

[0020] ;

[0021] Then angle The formula is as follows:

[0022] ;

[0023] in The distance between the snake-like robot and the stairs. For the unit joint length of the snake body structure, the first... ( The first joint of the snake body completes the motion command 4 in sequence. Each joint completes the climb, executing action command 5;

[0024] Action command 5 is for the 14th joint of the snake-like structure to climb stairs. ( The horizontal drive servo of the 13th unit joint rotates inward to bring the center of gravity of the snake robot inward. Then, the horizontal drive servo of the 13th unit joint and the vertical drive servo of the 14th unit joint are reset in sequence to make the snake body structure climb the steps as a whole.

[0025] OpenMV then checks if there are any steps to the side. If not, it executes a forward step; if so, it checks the width of the stairs. And execute action instruction 6;

[0026] Action instruction 6 is an arc-shaped scroll, the first ( The angles of the horizontal drive servo and the snake-head drive servo of each unit joint are related to the first joint. ( The vertical drive servo angle of each unit joint is changed to complete one roll operation, and the number of rolls is [number missing]. Based on the width of the stairs The decision is made, satisfying the following formula:

[0027] ;

[0028] in The distance between the snake-like robot and the stairs. The number of rolls per unit joint width of the snake's body structure. This represents the number of times action instruction 6 will be executed.

[0029] The beneficial effects of this invention are as follows:

[0030] Stairs were selected as a landmark unstructured terrain for study, enabling snake robots to climb stairs and enhancing their adaptability in three-dimensional environments. Attached Figure Description

[0031] Figure 1 This is a schematic diagram of the overall structure of the snake-like robot;

[0032] Figure 2 This is a schematic diagram showing the snake head structure and the connection of the first unit joint;

[0033] Figure 3 A schematic diagram showing the dissection of the snake's head structure;

[0034] Figure 4 This is a schematic diagram of the disassembled joint;

[0035] Figure 5 This is a schematic diagram of the spatial coordinate system of a snake-like robot.

[0036] Figure 6 A flowchart for a snake-like robot climbing stairs;

[0037] Figure 7The rotation angle of the first horizontal drive servo in action command 3 A schematic diagram of the formula;

[0038] Figure 8 For action instruction 4, the first ( ) horizontal drive servo rotation angle A schematic diagram of the formula;

[0039] Figure 9 Number of rolls A schematic diagram of the formula;

[0040] Figure 10 This is a schematic diagram of the action command.

[0041] Figure 11 This is the schematic diagram of the second action instruction;

[0042] Figure 12 The diagram shows the three principles of the action command.

[0043] Figure 13 This is the principle diagram for the four action instructions;

[0044] Figure 14 This is the principle diagram for the five action instructions;

[0045] Figure 15 This is the principle diagram for the six action instructions.

[0046] In the diagram: 1. Three-dimensional angle sensor; 2. OpenMV drive servo motor; 3. Snake head shell; 4. OpenMV; 5. First U-shaped frame; 6. Second U-shaped frame; 7. Third U-shaped frame; 8. Connecting block; 9. Unit joint shell. Detailed Implementation

[0047] See Figures 1 to 5 As shown, a climbing control method for a snake-like robot used for rescue or exploration is disclosed, wherein the snake-like robot includes a snake head structure and a snake body structure;

[0048] The snake head structure includes a snake head drive servo, a three-dimensional angle sensor 1 and an OpenMV drive servo 2, a snake head shell 3, an OpenMV 4, and a first U-shaped frame 5;

[0049] The OpenMV drive servo 2 is fixedly connected inside the snake-head housing 3. After its rotor extends out of the snake-head housing 3, it is fixedly connected to the 3D angle sensor 1 and OpenMV 4 via a mounting plate. The snake-head drive servo is also fixedly connected inside the snake-head housing 3, and a first U-shaped bracket 5 is fixedly connected to its shaft. The OpenMV drive servo 2 is responsible for the operation of the 3D angle sensor 1 and OpenMV 4. Axial motion;

[0050] The snake's body structure includes ( Each unit joint includes a vertical drive servo and a horizontal drive servo, which are orthogonally connected and installed inside the unit joint housing 9. A second U-shaped frame 6 and a third U-shaped frame 7 are fixedly connected to the shafts of the vertical and horizontal drive servos, respectively. A connecting block 8 is fixedly connected to the third U-shaped frame 7. The second U-shaped frame 6 on the first unit joint is orthogonally connected to the first U-shaped frame 5. ( The connecting block 8 on the unit joint and the adjacent unit joint ( The second U-shaped bracket 6 on each unit joint is fixedly connected. It should be noted that: the... ( The second U-shaped frame 6 on the unit joint is always connected to the adjacent first U-shaped frame 6. ( The third U-shaped frame 7 on each unit joint is in an orthogonal posture;

[0051] Vertical drive servo motor The axis moves, providing vertical drive, while the snake-head drive servo and the horizontal drive servo rotate. The axis moves, providing horizontal drive. Figure 1 Unit joint length of the snake-like structure 158mm, width It is 48mm;

[0052] Its attitude control method is as follows: (See reference) Figures 6 to 15 As shown:

[0053] Figure 6 The flowchart shows a snake robot climbing stairs. The snake robot has 6 sets of action commands, namely gait movements. The 6 sets of action commands are executed sequentially. Action command 1 and action command 2, and action command 5 and action command 6 require 3D angle sensor 1 and OpenMV4 to transmit data information in real time and feed it back to the main controller (the main controller is existing technology and is not shown in the figure). The main controller determines the switching of action commands based on the data information to achieve a complete closed-loop control effect.

[0054] OpenMV4 (machine vision module) detects the surrounding environment to determine if it belongs to a staircase structure, and if the determination is successful, it detects the height of the staircase. and the height of the stairs The data is sent back to the main controller to execute action command 1.

[0055] Action command 1 is to move forward in an arc, causing the snake-like robot, which is perpendicular to the stairs, to turn parallel to the stairs, making it easier for the snake-like robot to climb the stairs. The distance between the snake-like robot and the stairs... It is 50mm;

[0056] During the turning process, the three-dimensional angle sensor 1 of the snake head structure transmits data back in real time. Axis data is used to detect whether the snake robot turns to... If not arrived Position, execute forward steering; if beyond... Position, execute reverse turn; upon arrival When in position, execute action instruction 2;

[0057] Action instruction 2 is to reset, so that the snake robot's pose conforms to the robust structure, and the snake robot is parallel to the stairs. After the reset is completed, action instruction 3 is executed.

[0058] Action command 3 is for the snake-head structure to climb stairs, with the snake-head drive servo and the vertical drive servo of the second unit joint rotating. Simultaneously, the horizontal drive servo of the first unit joint rotates at an angle... ,angle From the height of the stairs The decision is made, satisfying the following formula:

[0059] ;

[0060] in The distance between the snake-like robot and the stairs. =50mm; Figure 7 The rotation angle of the horizontal drive servo motor of the first unit joint The diagram shows that this process enables the first unit joint of the snake-like structure to fully fit the edge of the stairs, and then executes action command 4.

[0061] Action instruction 4 involves the unit joints of the snake-like structure climbing the stairs sequentially, starting with the first joint of the snake-like structure... ( Taking the first unit joint as an example, the first... ( The vertical drive servo of the unit joint and the first ( Vertical drive servo motor rotation of each unit joint ,when At that time, the first ( The vertical drive servo of the unit joint and the first ( The horizontal drive servo motor of each unit joint resets; when =2 is only used to reset the snake-head drive servo, and at the same time the first ( The horizontal drive servo motor rotation angle of each unit joint ,angle Similarly, satisfying the above formula, the first... ( The horizontal drive servo motor rotation angle of each unit joint ,angle From the height of the stairs Decide and define intermediate variables Satisfy the following formula:

[0062] ;

[0063] Then angle The formula is as follows:

[0064] ;

[0065] in The distance between the snake-like robot and the stairs. =50mm; The unit joint length of the snake's body structure. =158mm; Figure 8 For the first ( Each joint completes the climb, executing action command 5;

[0066] Action command 5 is for the 14th joint of the snake-like structure to climb stairs. ( The horizontal drive servo of the 13th unit joint rotates inward to bring the center of gravity of the snake robot inward. Then, the horizontal drive servo of the 13th unit joint and the vertical drive servo of the 14th unit joint are reset in sequence to make the snake body structure climb the steps as a whole.

[0067] OpenMV4 then checks if there are any steps to the side. If not, it executes a forward step; if so, it checks the width of the stairs. And execute action instruction 6;

[0068] Action instruction 6 is an arc-shaped scroll, the first ( The vertical drive servo of the first unit joint bends by 4°~8°; then the second... ( The angles of the horizontal drive servo and the snake-head drive servo of each unit joint are related to the first joint. ( The vertical drive servo angle of each unit joint is changed to complete one roll operation, and the number of rolls is [number missing]. Based on the width of the stairs The decision is made, satisfying the following formula:

[0069] ;

[0070] in The distance between the snake-like robot and the stairs. =50mm; The unit joint width of the snake's body structure. =48mm; Number of tumbles This represents the number of times action instruction 6 will be executed; Figure 9 Number of rolls A schematic diagram of the formula;

[0071] Figures 10 to 15 This diagram illustrates the action commands for a snake-like robot climbing stairs. By executing six sets of action commands sequentially, the snake-like robot can climb one staircase. By repeating the six sets of action commands, it can climb multiple staircases.

Claims

1. A method for controlling the climbing of a snake-like robot used in rescue or exploration, characterized in that: The snake-like robot includes a snake head structure and a snake body structure; The snake head structure includes a snake head drive servo, a three-dimensional angle sensor (1) and an OpenMV drive servo (2), a snake head shell (3), an OpenMV (4) and a first U-shaped frame (5); The OpenMV drive servo (2) is fixedly connected inside the snake head shell (3), and the rotor of the OpenMV drive servo (2) extends out of the snake head shell (3) and is fixedly connected to the three-dimensional angle sensor (1) and OpenMV (4) through a mounting plate. The snake head drive servo is fixedly connected inside the snake head shell (3), and a first U-shaped bracket (5) is fixedly connected to the shaft of the snake head drive servo. The OpenMV drive servo (2) is responsible for the three-dimensional angle sensor (1) and OpenMV (4) in... Axial motion; The snake's body structure includes ( Each unit joint includes a vertical drive servo and a horizontal drive servo, which are installed inside the unit joint housing (9) in an orthogonal connection manner. A second U-shaped frame (6) and a third U-shaped frame (7) are fixedly connected to the shafts of the vertical drive servo and the horizontal drive servo, respectively. A connecting block (8) is fixedly connected to the third U-shaped frame (7). The second U-shaped frame (6) on the first unit joint is fixedly connected to the first U-shaped frame (5) in an orthogonal connection manner. ( The connecting block (8) on the unit joint and the adjacent unit joint ( The second U-shaped frame (6) on each unit joint is fixedly connected; Vertical drive servo motor The axis moves, providing vertical drive, while the snake-head drive servo and the horizontal drive servo rotate. The axis moves, providing horizontal drive; Its attitude control method is as follows: OpenMV(4) detects the environment in front to determine whether it belongs to a staircase structure. If the determination is successful, it detects the height of the staircase. and the height of the stairs The data is sent back to the main controller to execute action command 1. Action command 1 is to move forward in an arc, causing the snake-like robot, which is perpendicular to the stairs, to turn parallel to the stairs, making it easier for the snake-like robot to climb the stairs. The distance between the snake-like robot and the stairs is... ; During the turning process, the three-dimensional angle sensor (1) of the snake head structure transmits data back in real time. Axis data is used to detect whether the snake robot turns to... If not arrived Position, execute forward steering; if beyond... Position, execute reverse turn; upon arrival When in position, execute action instruction 2; Action instruction 2 is to reset, so that the snake robot's pose conforms to the robust structure, and the snake robot is parallel to the stairs. After the reset is completed, action instruction 3 is executed. Action command 3 is for the snake-head structure to climb stairs, with the snake-head drive servo and the vertical drive servo of the second unit joint rotating. Simultaneously, the horizontal drive servo of the first unit joint rotates at an angle... ,angle From the height of the stairs The decision is made, satisfying the following formula: ； in The distance between the snake-like robot and the stairs; Action instruction 4 involves the snake-like structure's unit joints climbing the stairs sequentially, the first... ( The vertical drive servo of the unit joint and the first ( Vertical drive servo motor rotation of each unit joint ,when At that time, the first The vertical drive servo of each unit joint and the first The horizontal drive servo motor of each unit joint resets; when It is only used to reset the snake head drive servo, and at the same time the first ( The horizontal drive servo motor rotation angle of each unit joint ,angle Similarly, satisfying the above formula, the first... The horizontal drive servo motor of each unit joint rotates at an angle. ,angle From the height of the stairs Decide and define intermediate variables Satisfy the following formula: ； Then angle The formula is as follows: ； in The distance between the snake-like robot and the stairs. For the unit joint length of the snake body structure, the first... ( The first joint of the snake body completes the motion command 4 in sequence. Each joint completes the climb, executing action command 5; Action command 5 is for the 14th joint of the snake-like structure to climb stairs. ( The horizontal drive servo of the 13th unit joint rotates inward to bring the center of gravity of the snake robot inward. Then, the horizontal drive servo of the 13th unit joint and the vertical drive servo of the 14th unit joint are reset in sequence to make the snake body structure climb the steps as a whole. Then OpenMV(4) checks if there are any steps on the side. If not, it executes the forward step state. If present, check the stair width. And execute action command 6; Action instruction 6 is an arc-shaped scroll, the first ( The angles of the horizontal drive servo and the snake-head drive servo of each unit joint are related to the first joint. ( The vertical drive servo angle of each unit joint is changed to complete one roll operation, and the number of rolls is [number missing]. Based on the width of the stairs The decision is made, satisfying the following formula: ； in The distance between the snake-like robot and the stairs. The number of rolls per unit joint width of the snake's body structure. This represents the number of times action instruction 6 will be executed.

Images