A snake robot pose acquisition method and system based on single imu and d-h parameters

Abstract

The application relates to a snake robot pose acquisition method and system based on a single IMU and D-H parameters. The snake robot pose acquisition method based on the single IMU and the D-H parameters comprises the following steps: collecting initial data of a head IMU; solving the initial data to obtain original pose data of the head, optimizing the original pose data of the head through a complementary filtering algorithm, and obtaining accurate head pose data; establishing a single-section coordinate transformation model, and constructing an initial coordinate transformation matrix; connecting the coordinate transformation matrices from the head to the tail section by section, and constructing a full-link coordinate transformation chain; and solving the absolute pose of a robot tail track. The snake robot pose acquisition method based on the single IMU and the D-H parameters has the advantages that multiple IMUs are not needed, and the measurement precision is high.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a method and system for acquiring the pose of a snake robot based on a single IMU and DH parameters. Background Technology

[0002] With the development of biomimetic robots, snake-like robots have attracted increasing attention. Snake-like robots possess advantages such as flexible movement, strong terrain adaptability, and high scalability.

[0003] However, in existing snake robot technology, due to factors such as joint clearance, structural flexibility, and changes in the contact state between the track and the ground, there are often significant differences between the robot's head posture and the posture of the tail track actuator, making it difficult to accurately obtain the spatial pose directly through a single sensor.

[0004] However, if multiple sensors are used for detection, it will lead to problems such as complex electrical wiring, increased communication load, increased system power consumption and decreased reliability, which will make the engineering implementation more difficult and increase costs. Summary of the Invention

[0005] Based on this, the purpose of the present invention is to provide a method and system for acquiring the pose of a snake robot based on a single IMU and DH parameters, which has the advantages of not requiring multiple IMUs to cooperate and having high measurement accuracy.

[0006] A method for obtaining the pose of a snake robot based on a single IMU and DH parameters, specifically including the following steps:

[0007] S1 acquires initial data from the head IMU; S2 calculates the initial data to obtain the original head pose data, and optimizes the original head pose data through a complementary filtering algorithm to obtain accurate head pose data. S3 Establish a single-section coordinate transformation model and construct the initial coordinate transformation matrix; S4 connects the coordinate transformation matrices of each joint of the robot from head to tail to construct a full-link coordinate transformation chain; S5 calculates the absolute attitude of the robot's tail track through a full-link coordinate transformation chain.

[0008] The snake robot pose acquisition method based on a single IMU and DH parameters described in this invention obtains accurate head pose data through a complementary filtering algorithm, thereby improving the accuracy of subsequent calculations and measurements; and it only requires a single IMU for measurement, reducing measurement costs.

[0009] Furthermore, in step S1, the initial data includes the real-time angular velocity of the gyroscope acquired by the IMU. Accelerometer triaxial acceleration , , The x and y axis measurements of the magnetometer in the body coordinate system , .

[0010] Furthermore, step S2 specifically includes the following steps: S21: Perform gyroscope attitude integration to obtain the gyroscope's calculated attitude angle at the current moment. ;

[0011] in, Calculate the attitude angle for the gyroscope at the previous moment. This represents the sampling time interval.

[0012] S22: Perform accelerometer attitude calculation to obtain the roll angle. With pitch angle ;

[0013]

[0014] S23: Calculate the magnetometer heading angle and obtain the heading angle. ;

[0015] S24: The original head pose data is optimized using a complementary filtering algorithm to obtain accurate head pose data. The complementary filtering algorithm includes:

[0016] in, These are the filter weighting coefficients. The value can be between 0 and 1; The roll angle obtained from the accelerometer attitude calculation or pitch angle .

[0017] Furthermore, step S2 specifically includes the following steps: S21: Perform gyroscope attitude integration to obtain the gyroscope's calculated attitude angle at the current moment. ;

[0018] in, Calculate the attitude angle for the gyroscope at the previous moment. This represents the sampling time interval.

[0019] S22: Perform accelerometer attitude calculation to obtain the roll angle. With pitch angle ;

[0020]

[0021] S23: Calculate the magnetometer heading angle and obtain the heading angle. ;

[0022] S24: The original head pose data is optimized using a complementary filtering algorithm to obtain accurate head pose data. The complementary filtering algorithm includes:

[0023] in, These are the filter weighting coefficients. The value can be between 0 and 1; The roll angle obtained from the accelerometer attitude calculation or pitch angle .

[0024] Further, step S3 includes: For each segment of the robot, an independent spatial coordinate system is established. The spatial position and orientation relationships between adjacent coordinate systems of each segment are determined using a standard homogeneous coordinate transformation matrix based on DH parameters. express:

[0025] in, For the robot Segmental joint angles; , , The preset robot structural dimensions are link length, link offset, and link torsion angle.

[0026] Further, step S4 includes: multiplying the coordinate transformation matrices of each joint of the robot's tracks from head to tail in sequence to form a full-link coordinate transformation chain:

[0027] in, Let be the attitude rotation transformation matrix of the robot's tail track. This is the orientation rotation matrix of the robot's head.

[0028] Further, step S5 includes: S51: Substitute the optimized and precise head pose data into the end-to-end coordinate transformation chain, and extract the orientation matrix of the tail track in the world coordinate system from the end-to-end coordinate transformation chain:

[0029] in, This is the orientation matrix of the robot's head IMU. ; for The 3x3 attitude matrix in the top left corner; S52: Solve the orientation matrix of the tail track;

[0030] Obtain the pitch angle of the tail track Roll angle Yaw angle .

[0031] A snake robot pose acquisition system based on a single IMU and DH parameters, used to implement the above-mentioned method, is characterized by comprising: an attitude perception module, a joint angle sensing module, and a main control calculation module; the attitude perception module is located at the head of the snake robot, the joint angle sensing module is located at each joint of the snake robot, and the main control calculation module is signal-connected to the attitude perception module and the joint angle sensing module respectively.

[0032] Furthermore, the attitude sensing module includes an IMU; the IMU collects initial data, including the real-time angular velocity of the gyroscope. Accelerometer triaxial acceleration , , The x and y axis measurements of the magnetometer in the body coordinate system , It is then output to the main control computing module for processing.

[0033] Furthermore, the joint angle sensing module collects the joint angles of each joint of the snake robot. The joint angle sensing module includes a joint motor or a joint angle encoder mounted on the robot.

[0034] Furthermore, the main control computing module includes an initial data calculation unit, a coordinate transformation matrix construction unit, a full-link coordinate transformation chain construction unit, and a robot tail posture calculation unit; The initial data processing unit processes the initial data collected by the attitude perception module and outputs accurate head attitude data optimized by the complementary filtering algorithm. The coordinate transformation matrix construction unit constructs a standard homogeneous coordinate transformation matrix between adjacent coordinate systems of each segment using DH parameters, including the joint angles of each joint of the snake robot. The full-link coordinate transformation chain construction unit multiplies the coordinate transformation matrices of each joint sequentially to form a full-link coordinate transformation chain; The robot tail posture calculation unit substitutes the precise head posture data into the full-link coordinate transformation chain to calculate the absolute posture of the robot tail track.

[0035] To better understand and implement this invention, the following detailed description is provided in conjunction with the accompanying drawings. Attached Figure Description

[0036] Figure 1 This is a schematic diagram of a snake robot pose acquisition method based on a single IMU and DH parameters according to an embodiment of the present invention.

[0037] Figure 2 This is a schematic diagram of a snake-shaped robot according to an embodiment of the present invention. Detailed Implementation

[0038] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0039] In the description of this invention, it should be noted that the terms "first", "second", "third" and "fourth" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0040] Example 1 Please see Figure 1 This invention provides a method and system for acquiring the pose of a snake robot based on a single IMU and DH parameters. The snake robot pose acquisition method based on a single IMU and DH parameters is used for snake robots with at least one IMU in their head, and includes the following steps: S1: Acquire initial data from the head IMU; S2: Solve the initial data to obtain the original head pose data, and optimize the original head pose data through a complementary filtering algorithm to obtain accurate head pose data. S3: Establish a single-section coordinate transformation model and construct the initial coordinate transformation matrix; S4: Connect the coordinate transformation matrices of each joint of the robot from head to tail to construct a full-link coordinate transformation chain; S5: Solve the absolute attitude of the robot's tail track through the full-link coordinate transformation chain.

[0041] Furthermore, in step S1, the initial data includes the real-time angular velocity of the gyroscope acquired by the IMU. Accelerometer triaxial acceleration , , The x and y axis measurements of the magnetometer in the body coordinate system , .

[0042] In step S2, the raw pose data from the head IMU includes the roll angle of the snake robot's head. With pitch angle and heading angle .

[0043] Step S2 specifically includes the following steps: S21: Perform gyroscope attitude integration to obtain the gyroscope's calculated attitude angle at the current moment. ;

[0044] in, Calculate the attitude angle for the gyroscope at the previous moment. This represents the sampling time interval.

[0045] S22: Perform accelerometer attitude calculation to obtain the roll angle. With pitch angle ;

[0046]

[0047] S23: Calculate the magnetometer heading angle and obtain the heading angle. ;

[0048] S24: The original head pose data is optimized using a complementary filtering algorithm to obtain accurate head pose data. The complementary filtering algorithm includes:

[0049] in, These are the filter weighting coefficients. The value can be between 0 and 1; The roll angle obtained from the accelerometer attitude calculation or pitch angle By employing a complementary filtering algorithm, IMU drift, measurement noise, and recursive cumulative errors can be suppressed, resulting in stable and high-precision head posture data. Weighted fusion is used to achieve complementary high-frequency and low-frequency errors.

[0050] Furthermore, step S3 specifically includes: For each segment of the robot, an independent spatial coordinate system is established. The spatial position and orientation relationships between adjacent coordinate systems of each segment are determined using a standard homogeneous coordinate transformation matrix based on DH parameters. express:

[0051] in, For the robot Joint angles of the segment; , , The preset robot structural dimensions are link length, link offset, and link torsion angle.

[0052] Further, step S4 includes: multiplying the coordinate transformation matrices of each joint of the robot's tracks from head to tail in sequence to form a full-link coordinate transformation chain:

[0053] in, Let be the attitude rotation transformation matrix of the robot's tail track. This is the pose rotation matrix of the robot's head.

[0054] Further, step S5 includes: S51: Substitute the optimized and precise head pose data into the end-to-end coordinate transformation chain, and extract the orientation matrix of the tail track in the world coordinate system from the end-to-end coordinate transformation chain:

[0055] in, This is the orientation matrix of the robot's head IMU. ; for The 3x3 attitude matrix in the top left corner.

[0056] S52: Solve the orientation matrix of the tail track to obtain the pitch angle of the tail track. Roll angle Yaw angle ;

[0057] Please see Figure 2This invention also provides a snake robot pose acquisition system based on a single IMU and DH parameters, for a snake robot including a head 1, a tail 2, several segments 3, and joints 4. The head 1, segments 3, and tail 2 of the snake robot are connected sequentially via joints 4. The snake robot pose acquisition system based on a single IMU and DH parameters according to this embodiment includes: an attitude sensing module, a joint angle sensing module, and a main control computing module. The attitude sensing module is located at the head 1 of the snake robot, the joint angle sensing modules are located at each joint 4 of the snake robot, and the main control computing module is signal-connected to both the attitude sensing module and the joint angle sensing module.

[0058] The attitude sensing module is used to acquire initial data of the snake robot's head 1. In some embodiments, the attitude sensing module includes an IMU; the initial data, including the real-time angular velocity of a gyroscope, is acquired through the IMU. Accelerometer triaxial acceleration , , The x and y axis measurements of the magnetometer in the body coordinate system , It is then output to the main control computing module for processing.

[0059] The main control computing module includes an initial data calculation unit, a coordinate transformation matrix construction unit, a full-link coordinate transformation chain construction unit, and a robot tail posture calculation unit.

[0060] The initial data processing unit processes the initial data collected by the attitude perception module and outputs accurate head attitude data optimized by the complementary filtering algorithm.

[0061] The joint angle sensing module collects the joint angles of each joint of the snake robot and outputs them to the coordinate transformation matrix construction unit; in some embodiments, the joint angle sensing module includes joint motors or joint angle encoders mounted on the robot.

[0062] The coordinate transformation matrix construction unit constructs a standard homogeneous coordinate transformation matrix between adjacent coordinate systems of each segment using DH parameters, including the joint angles of each joint of the snake robot.

[0063] The full-link coordinate transformation chain construction unit multiplies the coordinate transformation matrices of each joint sequentially to form the full-link coordinate transformation chain.

[0064] The robot's tail posture calculation unit substitutes the precise head posture data into the full-link coordinate transformation chain to calculate the absolute posture of the robot's tail track.

[0065] Example 2 This embodiment presents specific data obtained from experiments on the snake robot pose acquisition system based on a single IMU and DH parameters in Embodiment 1. In this embodiment, the snake robot has four joints; the main control computing module uses an RK3588 octa-core processor; the joint angle sensing module uses an Arduino Nano distributed controller and joint motors / servos, with each Arduino Nano connected in series via an RS485 bus, and the main Arduino Nano connected to the main control computing module via a serial port (UART); the posture perception module includes an IMU located in the head of the snake robot, which establishes a communication link with the main control computing module via a UART serial interface.

[0066] In this example, a static test was conducted by selecting a typical single-section bending condition, and the tail posture was measured using a digital display level. The test results are shown in Table 1 below:

[0067] The tests revealed that the absolute error of the tail roll angle was 1.69° and the absolute error of the tail pitch angle was 5.85°. The snake robot pose acquisition method and system based on a single IMU and DH parameters in this invention have the advantages of high measurement accuracy and low error.

[0068] The embodiments described above are merely examples of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and the present invention also intends to include these modifications and variations.

Claims

1. A method for acquiring the pose of a snake robot based on a single IMU and DH parameters, characterized in that, Specifically, the following steps are included: S1 acquires initial data from the head IMU; S2 calculates the initial data to obtain the original head pose data, and optimizes the original head pose data through a complementary filtering algorithm to obtain accurate head pose data. S3 Establish a single-section coordinate transformation model and construct the initial coordinate transformation matrix; S4 connects the coordinate transformation matrices of each joint of the robot from head to tail to construct a full-link coordinate transformation chain; S5 calculates the absolute attitude of the robot's tail track through a full-link coordinate transformation chain.

2. The snake robot pose acquisition method based on a single IMU and DH parameters according to claim 1, characterized in that: In step S1, the initial data includes the real-time angular velocity of the gyroscope acquired by the IMU. Accelerometer triaxial acceleration , , The x and y axis measurements of the magnetometer in the body coordinate system , .

3. The snake robot pose acquisition method based on a single IMU and DH parameters according to claim 2, characterized in that, Step S2 specifically includes the following steps: S21: Perform gyroscope attitude integration to obtain the gyroscope's calculated attitude angle at the current moment. ; in, Calculate the attitude angle for the gyroscope at the previous moment. The sampling time interval; S22: Perform accelerometer attitude calculation to obtain the roll angle. With pitch angle ; S23: Calculate the magnetometer heading angle and obtain the heading angle. ; S24: The original head pose data is optimized using a complementary filtering algorithm to obtain accurate head pose data. The complementary filtering algorithm includes: in, These are the filter weighting coefficients. The value can be 0 to 1; The roll angle obtained from the accelerometer attitude calculation or pitch angle .

4. The snake robot pose acquisition method based on a single IMU and DH parameters according to claim 3, characterized in that, Step S3 includes: For each segment of the robot, an independent spatial coordinate system is established. The spatial position and orientation relationships between adjacent coordinate systems of each segment are determined using a standard homogeneous coordinate transformation matrix based on DH parameters. express: in, For the robot Segmental joint angles; , , The preset robot structural dimensions are link length, link offset, and link torsion angle.

5. The snake robot pose acquisition method based on a single IMU and DH parameters according to claim 4, characterized in that, Step S4 includes: multiplying the coordinate transformation matrices of each joint of the robot's tracks from head to tail in sequence to form a full-link coordinate transformation chain. in, Let be the attitude rotation transformation matrix of the robot's tail track. This is the pose rotation matrix of the robot's head.

6. The snake robot pose acquisition method based on a single IMU and DH parameters according to claim 5, characterized in that, Step S5 includes: S51: Substitute the optimized and precise head pose data into the end-to-end coordinate transformation chain, and extract the orientation matrix of the tail track in the world coordinate system from the end-to-end coordinate transformation chain: in, The orientation matrix of the robot's head IMU. ; for The 3x3 attitude matrix in the top left corner; S52: Solve the orientation matrix of the tail track; Obtain the pitch angle of the tail track Roll angle Yaw angle .

7. A snake robot pose acquisition system based on a single IMU and DH parameters, used to implement the method described in any one of claims 1-6, characterized in that: include: The module consists of a posture perception module, a joint angle sensing module, and a main control computing module. The attitude sensing module is located in the head of the snake robot, the joint angle sensing module is located at each joint of the snake robot, and the main control computing module is connected to the attitude sensing module and the joint angle sensing module respectively.

8. The snake robot pose acquisition system based on a single IMU and DH parameters according to claim 7, characterized in that: The attitude sensing module includes an IMU; it collects initial data, including the real-time angular velocity of the gyroscope. Accelerometer triaxial acceleration , , The x and y axis measurements of the magnetometer in the body coordinate system , It is then output to the main control computing module for processing.

9. The snake robot pose acquisition system based on a single IMU and DH parameters according to claim 8, characterized in that: The joint angle sensing module collects the joint angles of each joint of the snake robot. The joint angle sensing module includes joint motors or joint angle encoders installed on the robot.

10. The snake robot pose acquisition system based on a single IMU and DH parameters according to claim 9, characterized in that: The main control computing module includes an initial data calculation unit, a coordinate transformation matrix construction unit, a full-link coordinate transformation chain construction unit, and a robot tail posture calculation unit. The initial data processing unit processes the initial data collected by the attitude perception module and outputs accurate head attitude data optimized by the complementary filtering algorithm. The coordinate transformation matrix construction unit constructs a standard homogeneous coordinate transformation matrix between adjacent coordinate systems of each segment using DH parameters, including the joint angles of each joint of the snake robot. The full-link coordinate transformation chain construction unit multiplies the coordinate transformation matrices of each joint sequentially to form a full-link coordinate transformation chain; The robot tail posture calculation unit substitutes the precise head posture data into the full-link coordinate transformation chain to calculate the absolute posture of the robot tail track.

Images