Multi-mode composite robot and control method thereof

Abstract

The invention relates to the technical field of robots, in particular to a multi-mode composite robot and a control method thereof. Modularized four-limb design is adopted, each four-limb module comprises a shoulder joint assembly, a limb assembly, a driving assembly and a wheel carrier assembly, the rotating direction and plane of each joint are controlled through linkage of multiple steering engines, efficient switching of a wheel type mode, a foot type mode and a wheel-foot type mode is ensured, the rotating planes of the shoulder joint assemblies and the rotating planes of the wheel carrier assemblies are perpendicular to each other, and the rotating directions and planes of all the joints are controlled. A bionic limb structure composed of thighs, shanks and driving rods is combined, stable foot type walking can be achieved in complex terrains, a wheel carrier assembly rapidly adjusts the working states of hubs and fan blades through a fourth steering engine, a planetary gear speed reducing mechanism balances the high rotating speed and high torque requirements, land and water switching does not need to replace a chassis, and the working efficiency is improved. The switching time and the mechanical failure rate are obviously reduced; the problems that in the using process of an existing multi-modal robot, modal transformation is complex, and modal transformation is prone to failure are solved.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a multimodal composite robot and a control method thereof. Background Art

[0002] With the continuous innovation of technology and the continuous expansion of actual needs, garbage cleaning devices for floating garbage on land and water are constantly emerging. Through prior art retrieval, a multimodal robot is disclosed in the Chinese patent document with application number CN202320885408.6, but this technology performs mode transition by switching the chassis, the overall size is large, and switching the chassis still takes a long time. In the Chinese patent document with application number CN202420859259.0, an omnidirectional multimodal robot is disclosed, including a chassis, a gear transmission deformation mechanism and a paddle wheel mechanism, but the structure of this technology is relatively complex and its adaptability to the environment is relatively low. The mode switching process relies on the first servo to drive the wheel to flip, and it is easy to fail in complex terrain (such as sand and mud). Summary of the Invention

[0003] In view of the deficiencies in the prior art, the present invention provides a multimodal composite robot and a control method thereof, which solves the problems of complex modal transitions and easy failure of modal transitions in the use of the existing multimodal robots.

[0004] To achieve the above-mentioned object, the present invention provides the following technical solutions: a multimodal composite robot, comprising a body module for support, and a plurality of limb modules mounted on the body module for coordinated movement; The limb module includes a shoulder joint assembly rotatably mounted on the body module, the plane where the shoulder joint assembly rotates is perpendicular to the forward direction, a limb assembly rotatably mounted on the shoulder joint assembly and connected in sequence by multiple limbs, the plane where the limb assembly rotates is perpendicular to the plane where the shoulder joint assembly rotates, a drive assembly for controlling the bending of the limb assembly is mounted on the limb assembly, a rotatable wheel frame assembly is mounted on the end of the limb assembly away from the shoulder joint assembly, the rotation plane of the wheel frame assembly is perpendicular to the extension direction of the limb of the limb assembly to which the wheel frame assembly is connected, a wheel hub that can roll on the ground is mounted on the wheel frame assembly, and fan blades are mounted inside the wheel hub.

[0005] Preferably, the shoulder joint assembly includes a limb connecting plate rotatably mounted on the body module, and a first servo for driving the limb connecting plate to rotate is mounted on the body module.

[0006] Preferably, the limb assembly comprises a thigh rotatably mounted on the limb connecting plate, and a calf is rotatably mounted on one end of the thigh away from the limb connecting plate.

[0007] Preferably, the drive assembly includes a second servo installed on the limb connecting plate for driving the thigh to rotate, a first drive plate rotatably installed on the rotating bearing of the shoulder joint assembly of the limb assembly, a first drive rod rotatably installed on the limb connecting plate, a second drive rod rotatably connected between the first drive rod and the first drive plate, a third servo installed on the limb connecting plate for driving the first drive rod to rotate, and a third drive rod rotatably connected between the limb connecting plate and the end of the calf close to the thigh.

[0008] Preferably, the wheel frame assembly includes a fourth servo installed at one end of the limb assembly away from the shoulder joint assembly, and a wheel foot connecting plate is installed on the output shaft of the fourth servo, and a motor for driving the wheel hub and fan blades to rotate is installed on the wheel foot connecting plate.

[0009] Preferably, the fan blades are fixed on the output shaft of the motor, and a planetary gear reduction mechanism for reducing the rotational speed is provided between the output shaft of the motor and the rim of the hub.

[0010] The present invention also provides a control method for a multimodal composite robot, which specifically includes the following steps: S1. Obtain the wheel radius r, the distance L between the plane where the left wheel group is located and the plane where the right wheel group is located, and the angular velocity of the left wheel group rotation and the angular velocity of the right wheel group Robot data; S2, obtain the mode of the robot; If the robot is in wheeled mode, proceed to step S3; If not, proceed to step S5; S3. Calculate the velocity and angular velocity of the robot in the local coordinate system based on the robot data. The calculation formula is: In the above formula, and They represent the velocity and angular velocity of the robot in the local coordinate system respectively; S4. Calculate the speed and rotation angle of the robot in the global coordinate system based on its speed and angular velocity in the local coordinate system, and then end. The calculation formula is: In the above formula, Indicates the speed of the robot in the positive direction of the x-axis in the global coordinate system. Indicates the speed of the robot in the positive direction of the y-axis in the global coordinate system. Indicates the angle of rotation of the robot in the global coordinate system, Indicates the angular velocity of the robot in the global coordinate system; S5. When the robot is in the foot mode, the forward kinematic equation of the robot leg is: In the above formula, Indicates the position of the end of the robot's supporting leg in the positive direction of the x-axis in the global coordinate system. Indicates the position of the end of the robot's supporting leg in the positive direction of the y-axis in the global coordinate system. Indicates the position of the end of the robot's supporting leg in the positive direction of the y-axis in the global coordinate system. Indicates the length from the robot's thigh joint to the calf joint, Indicates the length from the robot's lower leg joint to the end of the supporting leg. Indicates the length from the robot's shoulder joint to the thigh joint, represents the joint angle of the robot shoulder joint, represents the joint rotation angle of the robot's thigh joint, represents the joint rotation angle of the robot's lower leg joint; S6. Calculate the joint angles of each joint of the robot according to the forward kinematics equation. The calculation formula is: In the above formula, represents the joint rotation angle of the robot shoulder joint, represents the joint angle of the robot's thigh joint, Represents the joint rotation angle of the robot's lower leg joint.

[0011] Compared with the prior art, the present invention provides a multimodal composite robot and a control method thereof, which has the following beneficial effects: 1. Through modular limb design and multi-joint collaborative drive, the flexibility and reliability of multi-modal switching are significantly improved. The present invention adopts a modular limb design. Each limb module includes a shoulder joint assembly, a limb assembly, a drive assembly and a wheel frame assembly. The rotation direction and plane of each joint are controlled by multiple servos to ensure efficient switching of wheeled, footed and wheel-foot modes. The rotation planes of the shoulder joint assembly and the wheel frame assembly are perpendicular to each other. Combined with the bionic limb structure composed of the thigh, calf and drive rod, stable foot-type walking can be achieved in complex terrain. The wheel frame assembly quickly adjusts the working status of the wheel hub and fan blades through the fourth servo. The planetary gear reduction mechanism balances the high speed and high torque requirements, so that water and land switching does not require chassis replacement, significantly reducing switching time and mechanical failure rate, and effectively solving the problems of complex mode switching and easy failure in the existing technology.

[0012] 2. The limb components adopt a bionic dog-like joint design. The thigh and calf achieve multi-level linkage through the second servo, third servo and drive rod, simulating biological gait and improving walking stability on uneven ground. The drive component achieves precise control of limb bending through the coordinated action of the first drive plate, drive rod and servo. Combined with the horizontal rotation ability of the shoulder joint, the robot can dynamically adjust its center of gravity and stride to adapt to complex terrain such as steep slopes and ravines.

[0013] 3. The integrated design of the wheel hub and fan blades at the end of the wheel frame assembly allows the robot to seamlessly switch between high-speed wheeled driving, foot-based climbing, and surface navigation without the need for additional mechanical adjustments, significantly expanding its application scenarios, such as water garbage cleaning and post-disaster rescue. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings: Figure 1 Schematic diagram of the three-dimensional structure of the wheel-foot modal of the present invention; Figure 2 is a schematic diagram of a shoulder joint assembly of the present invention; Figure 3 is a schematic diagram of the shoulder joint assembly and the drive assembly of the present invention; Figure 4 Schematic diagram of the planetary gear reduction mechanism of the present invention; Figure 5 is a schematic diagram of the wheel-type modal three-dimensional structure of the present invention; Figure 6 Schematic diagram of the foot-type modal three-dimensional structure of the present invention.

[0015] In the figure: 1. Body module; 2. Limb module; 21. Shoulder joint assembly; 211. Limb connecting plate; 212. First servo; 22. Limb assembly; 221. Thigh; 222. Calf; 23. Drive assembly; 231. Second servo; 232. First drive plate; 233. First drive rod; 234. Second drive rod; 235. Third servo; 236. Third drive rod; 24. Wheel frame assembly; 241. Fourth servo; 242. Wheel-foot connecting plate; 243. Motor; 25. Wheel hub; 26. Fan blade; 27. Planetary gear reduction mechanism. DETAILED DESCRIPTION

[0016] The following will describe the implementation methods of the present application in detail with reference to the accompanying drawings and examples, so that the implementation process of how the present application applies technical means to solve technical problems and achieve technical effects can be fully understood and implemented accordingly.

[0017] In order to solve the problems of complex mode transition and easy failure of mode transition in the existing multimodal robot during use, the present invention provides a multimodal composite robot, such as Figure 1-6 As shown, it includes a body module 1 for support, and also includes a number of limb modules 2 installed on the body module 1 for coordinated movement. Generally, the limb modules 2 are installed at the four corners of the body module 1, and there are four of them.

[0018] The limb module 2 includes a shoulder joint assembly 21 rotatably mounted on the body module 1, that is, the shoulder joint assembly 21 can rotate on the body module 1, and the plane where the rotation direction is located is perpendicular to the forward direction. A limb assembly 22 composed of multiple limbs rotatably connected in sequence is rotatably mounted on the shoulder joint assembly 21, and the plane where the rotation direction of the limb assembly 22 is located is perpendicular to the plane where the rotation direction of the shoulder joint assembly 21 is located. A driving assembly 23 for controlling its bending is installed on the limb assembly 22, and a rotatable wheel frame assembly 24 is installed at one end of the limb assembly 22 away from the shoulder joint assembly 21. The rotation plane of the wheel frame assembly 24 is perpendicular to the extension direction of the limb connected to the wheel frame assembly 24 on the limb assembly 22. The shoulder joint assembly 21 and the wheel frame assembly 24 rotate in coordination, thereby enabling the robot to switch between multiple modes, such as Figure 1 is the wheel-foot mode, Figure 5 is the wheel mode, Figure 6 It is a foot-type mode, and a wheel hub 25 that can roll on the ground is installed on the wheel frame assembly 24, so that it can move on the ground. Fan blades 26 are installed inside the wheel hub 25, and the rotation of the fan blades 26 can enable it to swim in the water. The rotation direction of the wheel frame assembly 24 on the limb assembly 22 is the same as the rotation direction of the limb assembly 22 on the shoulder joint assembly 21, so that the switching of three modes can be achieved through the above structure, so as to face different environments more flexibly. The multimodal robot of the present application has simpler and more stable mode transitions during use.

[0019] The shoulder joint assembly 21 as a whole can rotate on the body module 1. Its structure is now introduced in detail. The shoulder joint assembly 21 includes a limb connecting plate 211 rotatably mounted on the body module 1. The body module 1 is equipped with a first servo 212 for driving the limb connecting plate 211 to rotate. The relative rotation of the limb connecting plate 211 and the body module 1 is driven by the first servo 212 mounted on the body module 1.

[0020] The limb assembly 22 is a bionic structure to adapt to walking on the ground. The limb assembly 22 includes a thigh 221 rotatably mounted on the limb connecting plate 211, and a calf 222 rotatably mounted on the end of the thigh 221 away from the limb connecting plate 211, so that the overall extension and contraction can be achieved through the rotation of the thigh 221 and the calf 222, imitating the walking of a dog.

[0021] The driving assembly 23 is used to drive the rotation of the limbs on the limb assembly 22. It adopts a bionic structure. The driving assembly 23 includes a second servo 231 installed on the limb connecting plate 211 for driving the thigh 221 to rotate. The limb assembly 22 is rotatably mounted with a first driving plate 232 on the rotating bearing of the shoulder joint assembly 21. A first driving rod 233 is rotatably mounted on the limb connecting plate 211. A second driving rod 234 is rotatably connected between the first driving rod 233 and the first driving plate 232. The third servo 235 is used to drive the first drive rod 233 to rotate, and the third drive rod 236 is rotatably connected between the limb connecting plate 211 and the end of the calf 222 close to the thigh 221. When the output shaft of the third servo 235 rotates, the calf 222 is driven to rotate through the linkage of the first drive rod 233, the second drive rod 234, the first drive plate 232 and the third drive rod 236, and the thigh 221 is driven to rotate through the second servo 231, so that the limb assembly 22 can imitate the walking of a dog.

[0022] The rotation of the wheel frame assembly 24 is achieved through the following structure: the wheel frame assembly 24 includes a fourth servo 241 installed at the end of the limb assembly 22 away from the shoulder joint assembly 21, and a wheel foot connecting plate 242 is installed on the output shaft of the fourth servo 241. The wheel foot connecting plate 242 is driven to rotate by the output shaft of the fourth servo 241, so that the rotation of the wheel foot connecting plate 242 is perpendicular to the extension direction of the calf 222. A motor 243 for driving the wheel hub 25 and the fan blades 26 to rotate is installed on the wheel foot connecting plate 242, so that the limb 22 can move on the ground and in water through the rotation of the wheel hub 25 and the fan blades 26.

[0023] The fan blades 26 are fixed to the output shaft of the motor 243 , and a planetary gear reduction mechanism 27 for reducing the rotational speed is provided between the output shaft of the motor 243 and the rim of the hub 25 .

[0024] The multimodal robot designed in this invention operates in various modes as follows: the robot sails on the water surface through the rotation of blades 26 in the water, the robot travels on the ground through the rolling of wheel hubs 25, and the robot crawls on the ground through the rotation of limb assembly 22. Specifically, the wheel hubs 25 must meet high rotational speed requirements in the water surface navigation mode, while they must meet low rotational speed requirements in the ground travel mode. Both modes require sufficiently high torque. A motor 243 with a maximum speed of 7000 r / min is used, and the motor shaft directly drives the rotation of blades 26, achieving the high rotational speed requirement of blades 26 in the water surface navigation mode. The motor shaft then drives the wheel rim through a planetary gear reduction mechanism 27. For example, if the transmission ratio is designed to be i=5, since motor 243 is loaded in the wheeled mode and cannot reach its maximum speed, the loaded speed is estimated as 50% of the no-load speed. This means that the speed of wheel hub 25 can be adjusted within a range of 0-7000 r / min. Assume the maximum output torque of the motor is , the torques distributed to the blades 26 and the hub 25 are and , the efficiency of the gear train is , then: Therefore, the load torque of the wheel rim in the water surface navigation mode is , the maximum torque of the blade is ; The load torque of the blade in the ground driving mode is , the maximum torque of the wheel rim is , so it can meet the requirements of bearing high torque in both modes.

[0025] The present invention also provides a control method for a multimodal composite robot. In order to achieve control, the robot generally includes a Beidou GPS satellite positioning module, a WIFI module, a single-chip control module and a camera module. The Beidou GPS satellite positioning module, the WIFI module and the camera module are in data communication with the single-chip control module. The single-chip control module is in data communication with the sensor module, the steering gear and the motor. The single-chip control adopts hierarchical control. In the wheel mode, the motor and the steering gear are controlled by the PWM of the STM32F103C8T6. Each PWM independently controls a motor. Differential control logic is adopted when turning. The foot mode adopts the RTrobot-16-way steering gear control board. The quadruped modal timing control is adopted, the Beidou GPS satellite positioning module 35 obtains the position information, the WIFI module 36 provides the network, and transmits the robot's position information, motion information and surrounding information to the staff, so that the staff can set or modify the robot's motion trajectory and speed in time. The camera module 38 can capture the surrounding environment information and transmit the information to the staff and the single-chip control module 37. The single-chip control module 37 controls the robot's motion path and mode through the situation transmitted by the camera module 38. The staff can also remotely set the robot's motion path according to the surrounding environment information. Of course, it also includes a power supply for power supply. The control method specifically includes the following steps: S1. Obtain the wheel radius r, the distance L between the plane where the left wheel group is located and the plane where the right wheel group is located, and the angular velocity of the left wheel group rotation and the angular velocity of the right wheel group Robot data; S2, obtain the mode of the robot; If the robot is in wheeled mode, proceed to step S3; If not, proceed to step S5; S3. When the robot is in wheeled mode, assume that the robot moves on a two-dimensional plane, ignoring the vertical direction. The wheels are in pure rolling contact with the ground without sideslip. The wheel speeds can be controlled individually. The center of mass of the robot is located at the geometric center. The kinematic modeling of the robot in wheeled mode refers to a typical differential drive system. A four-wheel differential drive structure is used. The left and right sets of wheels are controlled by independent motors, and the front and rear wheels rotate synchronously. The speed and angular velocity of the robot in the local coordinate system are calculated based on the robot data. The calculation formula is: In the above formula, and They represent the velocity and angular velocity of the robot in the local coordinate system respectively; the local coordinate system is defined as follows: at any time, the origin is defined as the center of mass of the robot, the positive direction of the x-axis is defined as the direction directly in front of the robot, the positive direction of the y-axis is defined as the direction directly to the left of the robot, and the positive direction of the z-axis is defined as the direction perpendicular to the top of the robot.

[0026] S4. Calculate the speed and rotation angle of the robot in the global coordinate system based on its speed and angular velocity in the local coordinate system, and then end. The calculation formula is: In the above formula, Indicates the speed of the robot in the positive direction of the x-axis in the global coordinate system. Indicates the speed of the robot in the positive direction of the y-axis in the global coordinate system. Indicates the angle of rotation of the robot in the global coordinate system, Indicates the angular velocity of the robot in the global coordinate system; The global coordinate system is defined as follows: At the initial moment, the origin is defined as the robot's center of mass, the positive x-axis is defined as the direction directly in front of the robot, the positive y-axis is defined as the direction directly to the robot's left, and the positive z-axis is defined as the direction perpendicular to the top of the robot. This coordinate system does not change with the robot's movement. By adjusting the speed difference between the left and right wheels, the robot can achieve behaviors such as straight driving, rotating in place, or smooth turns.

[0027] S5. When the robot is in the foot mode, the forward kinematic equation of the robot leg is: In the above formula, Indicates the position of the end of the robot's supporting leg in the positive direction of the x-axis in the global coordinate system. Indicates the position of the end of the robot's supporting leg in the positive direction of the y-axis in the global coordinate system. Indicates the position of the end of the robot's supporting leg in the positive direction of the y-axis in the global coordinate system. Indicates the length from the robot's thigh joint to the calf joint, Indicates the length from the robot's calf joint to the end of the supporting leg. Indicates the length from the robot's shoulder joint to the thigh joint, represents the joint angle of the robot shoulder joint, represents the joint angle of the robot's thigh joint, represents the joint rotation angle of the robot's lower leg joint; S6. Calculate the joint rotation angles of each joint of the robot based on the forward kinematics equation. Based on the forward kinematics equation, if the rotation angles of each joint of the supporting leg are known, the position of the end of the supporting leg in the center of mass coordinate system can be calculated, and then the position of the robot trunk relative to the ground can be solved. To do this, it is necessary to perform inverse kinematics analysis on the quadruped robot and solve the variables of each joint of the supporting leg based on the position of the robot trunk relative to the ground. The calculation formula is: In the above formula, represents the joint angle of the robot shoulder joint, represents the joint angle of the robot's thigh joint, Represents the joint rotation angle of the robot's lower leg joint.

[0028] It should be noted that, in this document, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that includes a list of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus.

[0029] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A multimodal composite robot comprising a body module (1) for supporting, characterized in that, It also includes a plurality of limb modules (2) mounted on the body module (1) for coordinated movement; The limb module (2) includes a shoulder joint assembly (21) rotatably mounted on the body module (1), the plane where the rotation direction of the shoulder joint assembly (21) is located is perpendicular to the forward direction, a limb assembly (22) composed of multiple limbs rotatably connected in sequence is rotatably mounted on the shoulder joint assembly (21), the plane where the rotation direction of the limb assembly (22) is located is perpendicular to the plane where the rotation direction of the shoulder joint assembly (21) is located, a driving assembly (23) for controlling the bending of the limb assembly (22) is installed, a rotatable wheel frame assembly (24) is installed at one end of the limb assembly (22) away from the shoulder joint assembly (21), the rotation plane of the wheel frame assembly (24) is perpendicular to the extension direction of the limb connected to the wheel frame assembly (24) on the limb assembly (22), a wheel hub (25) that can roll on the ground is installed on the wheel hub (25), and a fan blade (26) is rotatably installed in the wheel hub (25).

2. The multimodal compound robot according to claim 1, characterized in that: The shoulder joint assembly (21) comprises a limb connecting plate (211) rotatably mounted on the body module (1); a first servo (212) for driving the limb connecting plate (211) to rotate is mounted on the body module (1).

3. The multimodal compound robot according to claim 2, characterized in that: The limb assembly (22) comprises a thigh (221) rotatably mounted on a limb connection plate (211), and a calf (222) is rotatably mounted on one end of the thigh (221) away from the limb connection plate (211).

4. The multimodal compound robot according to claim 3, characterized in that: The driving assembly (23) comprises a second steering gear (231) mounted on a limb connecting plate (211) for driving the thigh (221) to rotate; a first driving plate (232) is rotatably mounted on the limb assembly (22) on a rotating bearing of the shoulder joint assembly (21); a first driving rod (233) is rotatably mounted on the limb connecting plate (211); a second driving rod (234) is rotatably connected between the first driving rod (233) and the first driving plate (232); a third steering gear (235) is mounted on the limb connecting plate (211) for driving the first driving rod (233) to rotate; and a third driving rod (236) is rotatably connected between the limb connecting plate (211) and an end of the calf (222) close to the thigh (221).

5. The multimodal compound robot according to claim 1, characterized in that: The wheel frame assembly (24) includes a fourth steering gear (241) mounted on an end of the limb assembly (22) away from the shoulder joint assembly (21); a wheel foot connecting plate (242) is mounted on an output shaft of the fourth steering gear (241); and a motor (243) for driving the wheel hub (25) and the fan blades (26) to rotate is mounted on the wheel foot connecting plate (242).

6. The multimodal compound robot according to claim 5, characterized in that: The fan blades (26) are fixed on the output shaft of the motor (243), and a planetary gear reduction mechanism (27) for reducing the rotational speed is provided between the output shaft of the motor (243) and the rim of the wheel hub (25).

7. A control method for a multimodal composite robot according to any one of claims 1 to 6, characterized in that: The control method specifically includes the following steps: S1. Obtain the wheel radius r, the distance L between the plane where the left wheel group is located and the plane where the right wheel group is located, and the angular velocity of the left wheel group rotation and the angular velocity of the right wheel group Robot data; S2, obtain the mode of the robot; If the robot is in wheeled mode, proceed to step S3; If not, proceed to step S5; S3. Calculate the velocity and angular velocity of the robot in the local coordinate system based on the robot data. The calculation formula is: In the above formula, and They represent the velocity and angular velocity of the robot in the local coordinate system respectively; S4. Calculate the speed and rotation angle of the robot in the global coordinate system based on its speed and angular velocity in the local coordinate system, and then end. The calculation formula is: In the above formula, Indicates the speed of the robot in the positive direction of the x-axis in the global coordinate system. Indicates the speed of the robot in the positive direction of the y-axis in the global coordinate system, Indicates the angle of rotation of the robot in the global coordinate system, Indicates the angular velocity of the robot in the global coordinate system; S5. When the robot is in the foot mode, the forward kinematic equation of the robot leg is: In the above formula, Indicates the position of the end of the robot's supporting leg in the positive direction of the x-axis in the global coordinate system. Indicates the position of the end of the robot's supporting leg in the positive direction of the y-axis in the global coordinate system. Indicates the position of the end of the robot's supporting leg in the positive direction of the y-axis in the global coordinate system. Indicates the length from the robot's thigh joint to the calf joint, Indicates the length from the robot's lower leg joint to the end of the supporting leg. Indicates the length from the robot's shoulder joint to the thigh joint, represents the joint angle of the robot shoulder joint, represents the joint angle of the robot's thigh joint, represents the joint rotation angle of the robot's lower leg joint; S6. Calculate the joint angles of each joint of the robot according to the forward kinematics equation. The calculation formula is: In the above formula, represents the joint angle of the robot shoulder joint, represents the joint angle of the robot's thigh joint, Represents the joint rotation angle of the robot's lower leg joint.

Images