Robot head stability compensation control method and system based on inertial sensors

Abstract

A robot head stability compensation control method and system based on inertial sensors, belonging to the field of hyper-redundant robot motion planning. In order to solve the problem that the existing snake-like robot has an oscillating swing of the head during movement, which affects the robot navigation. The control method of the present invention divides the robot into a head and a torso. The pitch angle of the robot head is obtained by IMU measurement. The head controller obtains the planned joint angle of the head through the PD control algorithm based on the desired attitude angle and angular velocity, as well as the pitch angle and its angular velocity of the head. The first joint of the head is controlled by the joint controller; then it is judged whether the robot head is in a stable state. According to the pitch angle range of the lidar, when the pitch angle of the snake-like robot head is within the range of the lidar pitch angle, it is determined that the head is in a stable state; otherwise, the head joint angle planning is carried out again.

Description

Technical Field

[0001] The present invention relates to a robot head stability compensation control method and system, belonging to the field of hyper-redundant robot motion planning. Background Art

[0002] A snake robot is a multi-joint hyper-redundant robot with high degrees of freedom and can work in various complex environments. Robots equipped with visual sensors such as cameras or lidar can perform detection and navigation in some unknown environments.

[0003] Due to the motion characteristics of the snake robot, its head will fluctuate during motion, which will cause the head visual sensor to be unable to obtain external visual information normally, seriously affecting the navigation of the robot.

[0004] The patent application "A motion control method for a quadruped robot based on bionic principles and intuition" with the publication number CN109760761A proposes a control method for a quadruped robot based on a CPG network, which uses a VMC controller to correct the joint attitude angles so that the robot moves forward in the desired attitude. The patent with the publication number CN109093626A, "A body attitude control method and device for a quadruped robot", detects the body attitude angle and attitude angular velocity based on an IMU, calculates the foot end position coordinates and plane angles through the joint angles, thereby obtaining the control quantity, planning the target position and performing control. They both use inertial sensors and other devices to detect data such as attitude angles, and then control the robot through the calculation of intermediate variables, with poor real-time performance and large errors. When a snake robot uses a similar method for head control, due to poor real-time performance, the compensation control effect of the pitch angle of the snake robot's head is not good, and the head still swings greatly. Summary of the Invention

[0005] The present invention aims to solve the problems that the existing snake robot has head oscillation and swing during motion, which affects the robot navigation, and the problems of poor real-time performance and large errors in controlling the robot through the calculation of intermediate variables.

[0006] A robot head stability compensation control method based on an inertial sensor includes the following steps:

[0007] Step 1: Divide the robot into two parts: the head and the torso. The robot is a non-wheeled snake robot with an orthogonal structure. The robot head includes the first joint of the robot, as well as a head controller, a lidar, and an inertial sensor IMU device. The first joint contains a joint controller, and the first joint of the head is used to control the robot head to ensure stability.

[0008] The robot torso includes the remaining joints except the first joint, and the joints of the torso operate in a peristaltic gait of the robot to achieve the movement of the robot; the joint angle θ operates according to the peristaltic gait equation.

[0009] Step 2: Obtain the pitch angle of the robot head through IMU measurement and transmit the pitch angle data to the robot head controller.

[0010] Step 3: Transmit the robot joint angle data to the head controller. The head controller, according to the desired attitude angle and angular velocity as well as the head pitch angle and its angular velocity through the PD control algorithm, obtains the head planned joint angle θ h :

[0011]

[0012] where k p and k d are PD control parameters.

[0013] Step 4: Transmit the planned joint angle in the head controller to the joint controller of the first joint of the head, and control the first joint of the head through the joint controller.

[0014] Step 5: Determine whether the robot head is in a stable state: According to the pitch angle range of the lidar, when the pitch angle of the snake-shaped robot head is within the pitch angle range of the lidar, it is determined that the head is in a stable state; otherwise, return to the head controller to re-plan the head joint angle.

[0015] Furthermore, the process of determining whether the robot head is in a stable state is implemented in the upper computer.

[0016] Furthermore, the pitch angle range of the lidar is [-1.5°, 1.5°].

[0017] Furthermore, when obtaining the head planned joint angle in Step 3, set to obtain the head planned joint angle

[0018] Furthermore, the joint angle θ operating according to the peristaltic gait equation is as follows:

[0019]

[0020] where A represents the joint angle amplitude, ω t and ω n represent the time frequency and phase frequency respectively.

[0021] Furthermore, the robot torso further includes a tail battery module for powering the robot.

[0022] A robot head stabilization compensation control system based on an inertial sensor, the system includes a host computer, a head controller, a lidar, an inertial sensor IMU disposed on the robot head, and a joint controller included in the first joint of the robot head;

[0023] Inertial sensor IMU: Measures and obtains the pitch angle of the robot head And transmits the pitch angle data to the robot head controller;

[0024] Head controller: According to the desired attitude angle And angular velocity And the head pitch angle And its angular velocity Through the PD control algorithm, the head planned joint angle is obtained Where k p And k d Are PD control parameters; and transmits the planned joint angle to the joint controller of the first joint of the head; at the same time transmits the head pitch angle data to the host computer;

[0025] The joint controller in the first joint of the head: Controls the first joint of the head;

[0026] Host computer: Judges whether the robot head is in a stable state; the process of judging whether the robot head is in a stable state includes the following steps:

[0027] According to the lidar pitch angle range, when the snake robot head pitch angle is within the lidar pitch angle range, it is determined that the head is in a stable state; otherwise, it returns to the head controller to re-plan the head joint angle.

[0028] Furthermore, the lidar pitch angle range is [-1.5°, 1.5°].

[0029] Furthermore, when the head controller obtains the head planned joint angle It is also used to set And then obtains the head planned joint angle

[0030] Furthermore, the system further includes a wifi communication device for realizing communication between the host computer and the head controller.

[0031] Technical effects:

[0032] (1) The present invention can effectively solve the problem that the robot head oscillates with a quasi-sinusoidal waveform during the peristaltic gait, and the efficiency is relatively high.

[0033] (2) The present invention uses an inertial sensor to directly obtain the head pitch angle, reducing errors. At the same time, the present invention directly uses the head pitch angle detected by the IMU to perform feedback control on the head joints, with good real-time performance, small errors, and high efficiency. Description of the Drawings

[0034] Figure 1 is a segmented diagram of the robot.

[0035] Figure 2 is a flowchart of the head stability control method.

[0036] Figure 3 is a block diagram of the head stability control.

[0037] Figure 4 is a schematic diagram of the head stability control method.

[0038] Figure 5 is a simulation of the robot's creeping gait motion without a stability algorithm.

[0039] Figure 6 is a simulation of the robot's creeping gait motion with a stability algorithm.

[0040] Figure 7 is a comparison diagram of the simulated pitch angles with and without a stability algorithm. Detailed Implementation Manner

[0041] Detailed Implementation Manner 1: In combination with Figure 2 explain this implementation manner,

[0042] This implementation manner is a robot head stability compensation control method based on an inertial sensor, including the following steps:

[0043] Step 1: To ensure the stability of the robot's head, the robot is divided into two parts: the head and the torso, as Figure 1 shown. At the same time, the communication between the host computer and the head controller is realized in the form of wifi communication to detect and judge the pitch angle of the snake-shaped robot.

[0044] The robot head includes the first joint of the robot, the head controller, lidar, inertial sensor IMU and other devices; the first joint contains a joint controller, and the first joint of the head is used to control the robot head to ensure its stability;

[0045] The head controller and the joint controller are two types of controllers. The head controller is used to plan joint angles and process sensor information, and the joint sensor is used to drive the joints.

[0046] The robot torso includes the remaining joints other than the first joint and the tail battery module. The joints of the torso operate in a peristaltic gait of the robot to ensure the movement of the robot. For example, Figure 4 as shown Figure 4 in which 1, 2, and 3 represent the robot joint linkages, where 1 represents the linkage where the head is located, and h represents the desired direction of the head (horizontal direction); the joint angles operate according to the peristaltic gait equation. The peristaltic gait is a two-dimensional motion gait of a non-wheeled snake robot. The joint angles are as follows:

[0047]

[0048] where A represents the joint angle amplitude, ω t and ω n represent the time frequency and the phase frequency respectively.

[0049] In the movement of a non-wheeled snake robot with an orthogonal structure, the even joints of the robot are used for the yaw of the robot, and the odd joints are used for the pitch of the robot. Among them, the joint angle θ of the even joints (yaw) is 0, and the joint angle θ of the odd joints (pitch) is Acos(ω t t + ω n n). In this way, a quasi-sine waveform curve is applied in the pitch direction of the robot to complete the peristaltic gait movement of the robot.

[0050] Step 2: Obtain the pitch angle of the robot head by IMU measurement and transmit the pitch angle data to the robot head controller.

[0051] The head stability control block diagram is as Figure 3 shown Figure 3 in which represents the desired attitude angle of the robot head joint, represents the pitch angle of the robot head, θ h represents the planned joint angle of the head joint.

[0052] Step 3: Transmit the robot joint angle data to the head controller. The head controller, according to the desired attitude angle and the angular velocity and the pitch angle of the head and its angular velocity through the PD control algorithm, obtain the planned joint angle of the head:

[0053]

[0054] where k p and k d are the PD control parameters.

[0055] To ensure that the robot's head can be stably positioned near the horizontal position, in practical applications, the head attitude angle of the robot is set to 0 degrees, that is

[0056]

[0057] Finally, the planned joint angle of the head is

[0058]

[0059] Step 4: Transmit the planned joint angle in the head controller to the joint controller of the first joint of the head, and control the first joint of the head through the joint controller.

[0060] Step 5: Transmit the head pitch angle data to the host computer in real time through WiFi communication, and judge whether the robot's head is in a stable state in the host computer: Since the pitch angle range of the lidar is [-1.5°, 1.5°], when the pitch angle of the snake robot's head is within the range of [-1.5°, 1.5°], the lidar can operate normally, and at this time, it is determined that the head is in a stable state; otherwise, return to the head controller to re-plan the head joint angle.

[0061] Based on the above steps, the invention is simulated and verified in the ROS GAZEBO simulation software. Figure 5 and Figure 6 are respectively the simulation of the robot's peristaltic gait motion without and with a stability algorithm, Figure 5 and Figure 6 The (a) to (d) in are respectively the peristaltic gait diagrams in different states. Figure 7 is the comparison diagram of the simulated pitch angles with and without the stability algorithm, where (a) and (b) respectively correspond to the simulated pitch angles with and without the stability algorithm. The feasibility of the present invention can be verified through simulation. The present invention can effectively solve the problem that the head of the robot oscillates with a quasi-sinusoidal waveform during peristaltic gait, has relatively high efficiency, and uses an inertial sensor to directly obtain the head pitch angle, reducing errors. In addition, the present invention directly uses the head pitch angle detected by the IMU to feedback control the head joint, with good real-time performance, small error, and high efficiency. Specific Embodiment 2:

[0063] This embodiment is a robot head stability compensation control system based on an inertial sensor. The system includes a wifi communication device, a host computer, a head controller, a lidar, an inertial sensor IMU arranged on the robot head, and a joint controller contained in the first joint of the robot head;

[0064] wifi communication device: used to realize the communication between the host computer and the head controller.

[0065] Inertial sensor IMU: Measure the pitch angle of the robot's head And transmit the pitch angle data to the robot's head controller;

[0066] Head controller: According to the desired attitude angle And angular velocity And the head pitch angle And its angular velocity Through the PD control algorithm, obtain the planned joint angle of the head:

[0067]

[0068] Where, k p And k d Are PD control parameters.

[0069] In order to ensure that the robot's head can be stably near the horizontal position, in practical applications, the robot's head attitude angle is set to 0 degrees, that is

[0070]

[0071] Finally, the planned joint angle of the head is

[0072]

[0073] And transmit the planned joint angle to the joint controller of the first joint of the head; at the same time, transmit the head pitch angle data to the joint controller in the first joint of the upper computer head: control the first joint of the head;

[0074] Upper computer: Judge whether the robot's head is in a stable state; the process of judging whether the robot's head is in a stable state includes the following steps:

[0075] Since the pitch angle range of the lidar is [-1.5°, 1.5°], when the pitch angle of the snake robot's head is in the range of [-1.5°, 1.5°], the lidar can operate normally, and at this time, it is determined that the head is in a stable state; otherwise, return to the head controller to re-plan the head joint angle.

[0076] The above examples of the present invention are only to illustrate in detail the calculation model and calculation process of the present invention, rather than a limitation on the implementation manner of the present invention. For those of ordinary skill in the art, other different forms of changes or variations can be made on the basis of the above description. It is impossible to list all the implementation manners here. Any obvious changes or variations derived from the technical solution of the present invention are still within the protection scope of the present invention.

Claims

1. A robot head stable compensation control method based on an inertial sensor, characterized in that, Including the following steps: Step 1: Divide the robot into two parts, namely the head and the torso. The robot is an orthogonal-structured non-wheeled snake robot. The robot head includes the first joint of the robot, as well as a head controller, a lidar, and an inertial sensor IMU device. The first joint contains a joint controller, and the first joint of the head is used to control the robot head to ensure stability. The robot torso includes the remaining joints except the first joint. The joints of the torso operate in a peristaltic gait of the robot to achieve the movement of the robot. The joint angle θ operates according to the peristaltic gait equation. Step 2: Obtain the pitch angle of the robot's head through IMU measurement And transmit the pitch angle data to the robot head controller; Step 3: Transmit the robot joint angle data to the head controller. The head controller, based on the desired attitude angle and angular velocity as well as the head pitch angle and its angular velocity Through the PD control algorithm, obtain the head planned joint angle θ h : where k p and k d are PD control parameters; Step 4: Transmit the planned joint angle in the head controller to the joint controller of the first joint of the head, and control the first joint of the head through the joint controller. Step 5: Determine whether the robot head is in a stable state. According to the lidar pitch angle range, when the pitch angle of the snake robot head is within the lidar pitch angle range, it is determined that the head is in a stable state; otherwise, return to the head controller to re-plan the head joint angle.

2. The robot head stable compensation control method based on an inertial sensor according to claim 1, characterized in that, The process of determining whether the robot head is in a stable state is implemented in the host computer.

3. The robot head stable compensation control method based on an inertial sensor according to claim 2, wherein, The lidar pitch angle range is [-1.5°, 1.5°].

4. The robot head stable compensation control method based on an inertial sensor according to claim 1, 2 or 3, characterized in that In step 3, the head planned joint angles are obtained When the head planned joint angles are obtained 5. The robot head stable compensation control method based on an inertial sensor according to claim 4, characterized in that, The joint angle θ operating according to the peristaltic gait equation is as follows: where A represents the joint angle amplitude, ω t and ω n represent the time frequency and the phase frequency respectively.

6. The robot head stability compensation control method based on an inertial sensor according to claim 5, wherein The robot torso also includes a tail battery module for powering the robot.

7. A robot head stable compensation control system based on an inertial sensor, characterized in that The system includes a host computer, a head controller, a lidar, and an inertial sensor IMU set on the robot head, as well as a joint controller contained in the first joint of the robot head. Inertial sensor IMU: Measure the pitch angle of the robot's head And transmit the pitch angle data to the robot head controller; Head controller: Based on the desired attitude angle and angular velocity as well as the head pitch angle and its angular velocity Using the PD control algorithm, obtain the planned joint angle of the head where k p and k d are PD control parameters; and transmit the planned joint angle to the joint controller of the first joint of the head; at the same time, transmit the head pitch angle data to the host computer; The joint controller in the first joint of the head: Controls the first joint of the head. Host computer: Determines whether the robot head is in a stable state. The process of determining whether the robot head is in a stable state includes the following steps: According to the lidar pitch angle range, when the pitch angle of the snake robot head is within the lidar pitch angle range, it is determined that the head is in a stable state; otherwise, return to the head controller to re-plan the head joint angle.

8. The robot head stable compensation control system based on an inertial sensor according to claim 7, wherein The lidar pitch angle range is [-1.5°, 1.5°].

9. The robot head stable compensation control system based on an inertial sensor according to claim 8, wherein, The head controller obtains the head planned joint angles and is also used to set so as to obtain the head planned joint angles 10. The inertial sensor-based robot head stability compensation control system according to claim 7, 8 or 9, characterized in that The system also includes a wifi communication device for realizing the communication between the host computer and the head controller.

Images