A robot posture acquisition method and system based on forward kinematics model

Abstract

The present invention discloses a robot posture acquisition method and system based on a forward kinematics model, comprising: S1, constructing a forward kinematics model of a four-wheel drive four-turn robot; S2, setting the initial posture of the four-wheel drive four-turn robot body; S3, obtaining the distance increment of each wheel and the angle between each wheel and the body; S4, judging whether the four-wheel drive four-turn robot only performs linear motion based on the forward kinematics model, the distance increment of each wheel, and the angle between each wheel and the body, and calculating the posture increment of the body using different calculation methods according to the judgment result; S5, updating the posture information of the body at the current moment based on the forward kinematics model and the posture increment of the body; S6, looping through S3-S5 to obtain the posture information of the body of the four-wheel drive four-turn robot during its movement. The present invention can accurately and quickly obtain the posture information of the robot body through encoder information.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robots, and in particular to a robot posture acquisition method and system based on a forward kinematics model. Background Art

[0002] A four-wheel drive, four-turn (four-wheel independent drive and four-wheel independent steering) robot has four wheels, each equipped with two independent drive motors: one for steering and one for rolling. This means each wheel can independently steer and move. It offers advantages such as omnidirectional mobility, ability to navigate complex terrain, and high-speed stability. Wheel encoders are one of the most commonly used sensors in mobile robots. The position information derived from the encoders and corresponding kinematic models can effectively enhance the robot's autonomous positioning and navigation capabilities.

[0003] The common four-wheel drive four-turn kinematic model is usually based on speed, such as the literature (Lee, M. - H., & Li, T. - HS (2015). Kinematics, dynamics and control design of 4WIS4WID mobilerobots. The Journal of Engineering, 2015(1), 1 - 10.), and its forward kinematic model results are as follows:

[0004]

[0005] The basic principle is to obtain the speed of each wheel through the information of the drive motor encoder, and then obtain the speed of the robot body through the speed information of the four wheels. To obtain the position information of the robot body, the speed must be integrated.

[0006] This velocity-based model involves multiple integration operations. The discrete-time integration introduces errors and increases computational complexity. This is especially true in scenarios with severe environmental interference, such as strong magnetic fields. This increases noise in the encoder signal transmission line, further increasing the velocity error derived from the encoder signal. Velocity integration further amplifies this error, reducing the pose accuracy of the model output. Summary of the Invention

[0007] In response to the shortcomings of the existing technology, such as large posture information errors and high computational complexity, the present invention provides a robot posture acquisition method and system based on the forward kinematics model, which can accurately and quickly obtain the posture information of the robot body through encoder information.

[0008] The present invention provides a robot posture acquisition method based on a forward kinematics model. The robot is a four-wheel drive four-rotation robot having four wheels. Each wheel is equipped with two independent drive motors for driving the wheel to turn and roll, respectively corresponding to four drive wheel encoders and four steering wheel encoders. The method comprises:

[0009] S1. Construct a forward kinematics model of a four-wheel drive four-turn robot;

[0010] S2, setting the initial posture of the four-wheel drive four-turn robot body;

[0011] S3. Obtaining the pulse values ​​of the four driving wheel encoders and the four steering wheel encoders at a current moment, and comparing them with the pulse values ​​at a previous moment to obtain the distance increment of each wheel and the angle between each wheel and the vehicle body;

[0012] S4. Based on the forward kinematics model, the distance increment of each wheel, and the angle between each wheel and the vehicle body, determine whether the four-wheel drive four-turn robot only performs linear motion, and calculate the posture increment of the vehicle body using different calculation methods according to the determination result;

[0013] S5. Based on the forward kinematics model and the posture increment of the vehicle body, updating the posture information of the vehicle body at the current moment;

[0014] S6. Execute S3-S5 in a loop to obtain the posture information of the vehicle body during the movement of the four-wheel drive four-turn robot.

[0015] Preferably, in step S1, the forward kinematics model is used to:

[0016] (1) Define the coordinate system, related constants and variables

[0017] represents the global coordinate system, Represents the body coordinate system of the four-wheel drive four-turn robot body center, represents the body coordinate system of the wheel, i=1 represents the left front wheel, i=2 represents the right front wheel, i=3 represents the left rear wheel, and i=4 represents the right rear wheel. All coordinate systems satisfy the right-hand rule;

[0018] Represents the distance between the front and rear wheels, which is a constant. Indicates the distance between the left and right wheels, which is a constant. represents the position increment of the vehicle center in the global coordinate system, Represents the position of the vehicle center in the global coordinate system, Indicates the The angle between the wheel and the vehicle coordinate system represents the The angle increment of each wheel, Indicates the The distance increment of each wheel, Indicates the The rotation radius of the wheels;

[0019] (2) Calculate the vehicle's posture increment

[0020] When the vehicle body moves only in a straight line, that is:

[0021] (1)

[0022] Vehicle posture increment for:

[0023] (2)

[0024] When the vehicle body moves in a non-linear manner, the angle between the wheel and the vehicle body coordinate system is The following relationship is satisfied:

[0025] (3)

[0026] For the left front and right front wheels, namely wheels 1 and 2, their rotation radius is 、 Satisfies the following equation:

[0027] (4)

[0028] (5)

[0029] The rotation radius of the left front and right front wheels can be obtained:

[0030] (6)

[0031] Similarly, the rotation radius of the left and right rear wheels, namely wheels 3 and 4, can be obtained:

[0032] (7)

[0033] The vehicle body is the coordinate of the vehicle body in the global coordinate system and the coordinates of the wheel body in the global coordinate system The following relationship is satisfied:

[0034] (8)

[0035] (9)

[0036] (10)

[0037] in, represents the rotation matrix of the vehicle body relative to the global coordinate system, represents the rotation angle of the vehicle body relative to the global coordinate system, Indicates the coordinates of the wheel body in the vehicle body coordinate system;

[0038] When the vehicle body moves slightly around an instantaneous center, the displacement increment of the vehicle body is , angle increment and The displacement increment of each wheel The following relations are satisfied:

[0039] (11)

[0040] in, represents the exponential mapping between two-dimensional Lie algebras and Lie groups, , is a very small quantity, discarding the higher-order infinitesimal terms, we have:

[0041] (12)

[0042] From formulas (8) to (12), we can get:

[0043] (13)

[0044] For the left side of formula (13), for the i-th wheel, there is the following equation:

[0045] (14)

[0046] (15)

[0047] (16)

[0048] (17)

[0049] in, 、 Respectively Components in the x and y directions of the global coordinate system;

[0050] Combining formulas (8) to (17), the following equations can be obtained for the four wheels:

[0051] (18)

[0052] definition 、 、 (i=1, 2, 3, 4), 、 as follows:

[0053] (19)

[0054] (20)

[0055] (twenty one)

[0056] (twenty two)

[0057] (twenty three)

[0058] (twenty four)

[0059] (25)

[0060] (26)

[0061] Then, formula (18) can be expressed as:

[0062] (27)

[0063] matrix The generalized inverse matrix of for:

[0064] (28)

[0065] (29)

[0066] Then, we have:

[0067] (30)

[0068] Then, the vehicle body’s posture increment is:

[0069] (31)

[0070] Preferably, in step S1, the forward kinematics model is further used to:

[0071] (3) Update the vehicle's posture information

[0072] The current pose information can be obtained by adding the pose increment of the vehicle body at the previous moment to the pose information of the current moment. The calculation formula is as follows:

[0073] (32)

[0074] Preferably, step S4 includes:

[0075] S41, based on the angle between each wheel and the vehicle body , according to formula (1) and formula (3), determine whether the four-wheel drive four-turn robot only moves in a straight line;

[0076] S42. If only linear motion is performed, then the distance increment based on each wheel , the angle between each wheel and the vehicle body The angle between the vehicle coordinate system and the global coordinate system , according to formula (2), calculate the vehicle body's posture increment ;

[0077] S43. If non-linear motion is performed, then the distance increment based on each wheel The angle between each wheel and the vehicle body The angle between the vehicle coordinate system and the global coordinate system , according to formula (31), calculate the vehicle body's posture increment .

[0078] Preferably, step S5 includes:

[0079] According to formula (32) and the vehicle body's posture increment , update the vehicle’s current position information.

[0080] Based on the same inventive concept, the present invention also provides a robot posture acquisition system based on a forward kinematics model. The robot is a four-wheel drive four-turn robot with four wheels. Each wheel is equipped with two independent drive motors for driving the wheel to turn and roll, corresponding to four drive wheel encoders and four steering wheel encoders, respectively. The system includes:

[0081] Building module for constructing the forward kinematics model of the four-wheel drive four-turn robot;

[0082] An initialization module is used to set the initial position of the four-wheel drive four-turn robot body;

[0083] a comparison module, configured to obtain the pulse values ​​of the four driving wheel encoders and the four steering wheel encoders at a current moment, and compare them with the pulse values ​​at a previous moment to obtain the distance increment of each wheel and the angle between each wheel and the vehicle body;

[0084] a calculation module, configured to determine whether the four-wheel drive four-turn robot performs only linear motion based on the forward kinematics model, the distance increment of each wheel, and the angle between each wheel and the vehicle body, and calculate the posture increment of the vehicle body using different calculation methods according to the judgment result;

[0085] An updating module, configured to update the current posture information of the vehicle body based on the forward kinematics model and the posture increment of the vehicle body;

[0086] The acquisition module is used to cyclically execute the comparison module, the calculation module and the update module to obtain the posture information of the vehicle body during the movement of the four-wheel drive four-turn robot.

[0087] Compared with the prior art, the present invention has the following beneficial effects:

[0088] The method provided by the present invention can accurately and quickly obtain the position information of the vehicle body through the encoder information of the wheels of the four-wheel drive four-turn robot, effectively improving the robot's autonomous positioning and navigation capabilities. BRIEF DESCRIPTION OF THE DRAWINGS

[0089] Figure 1 A schematic flow chart of a robot posture acquisition method based on a forward kinematics model provided by the present invention;

[0090] Figure 2 A schematic diagram of the four-wheel drive four-rotation robot body and four wheels rotating around the same instantaneous center provided by the present invention;

[0091] Figure 3 A schematic diagram of the movement of wheel i when the four-wheel drive four-rotation robot body according to the present invention makes a small movement around an instantaneous center;

[0092] Figure 4 A schematic flow chart of a robot posture acquisition method based on a forward kinematics model provided by the present invention. DETAILED DESCRIPTION

[0093] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of the present invention.

[0094] The present invention is described in further detail below with reference to the accompanying drawings:

[0095] like Figure 1 As shown, an embodiment of the present invention provides a robot posture acquisition method based on a forward kinematics model. The robot is a four-wheel drive four-turn robot with four wheels. Each wheel is equipped with two independent drive motors for driving the wheel to turn and roll, corresponding to four drive wheel encoders and four steering wheel encoders, respectively, including:

[0096] S1. Construct a forward kinematics model of a four-wheel drive four-turn robot;

[0097] S2, setting the initial position of the four-wheel drive four-turn robot body;

[0098] S3. Obtain the current pulse values ​​of the four driving wheel encoders and the four steering wheel encoders, compare them with the pulse values ​​at the previous moment, and obtain the distance increment of each wheel and the angle between each wheel and the vehicle body;

[0099] S4. Based on the forward kinematics model, the distance increment of each wheel, and the angle between each wheel and the vehicle body, determine whether the four-wheel drive four-turn robot is only performing linear motion. Based on the determination result, calculate the position increment of the vehicle body using different calculation methods;

[0100] S5. Based on the forward kinematics model and the vehicle's posture increment, update the vehicle's current posture information;

[0101] S6. Loop through S3-S5 to obtain the body position information of the four-wheel drive four-turn robot during its movement.

[0102] The four-wheel drive four-turn robot of the embodiment of the present invention has four wheels, each of which is equipped with two independent drive motors, one for driving the wheel to turn, and the other for driving the wheel to roll, that is, each wheel can turn and walk independently. It has the advantages of omnidirectional mobility, complex terrain passability, high-speed stability, etc. The four driving wheel encoders and the four steering wheel encoders all use wheel encoders. Compared with the speed-based kinematic model of the prior art, the present invention uses four driving wheel encoders and four steering wheel encoders, and adopts a displacement-based forward kinematic model to calculate the posture information of the vehicle body, which can effectively solve the problems of large posture information error and high computational complexity, and effectively improve the robot's autonomous positioning and navigation capabilities.

[0103] In the embodiment of the present invention, in step S1, the forward kinematics model is used to:

[0104] (1) Define the coordinate system, related constants and variables

[0105] represents the global coordinate system, Represents the body coordinate system of the four-wheel drive four-turn robot body center, Indicates the The body coordinate system of each wheel, i=1 represents the left front wheel, i=2 represents the right front wheel, i=3 represents the left rear wheel, i=4 represents the right rear wheel, and all coordinate systems satisfy the right-hand rule;

[0106] Represents the distance between the front and rear wheels, which is a constant. Indicates the distance between the left and right wheels, which is a constant. represents the position increment of the vehicle center in the global coordinate system, Represents the position of the vehicle center in the global coordinate system, Indicates the The angle between the wheel and the vehicle coordinate system, Indicates the The angle increment of each wheel, Indicates the The distance increment of each wheel, Indicates the The rotation radius of the wheels;

[0107] (2) Calculate the vehicle's posture increment

[0108] When the vehicle body moves only in a straight line,

[0109] (1)

[0110] Vehicle posture increment for:

[0111] (2)

[0112] When the vehicle body performs non-linear motion,

[0113] Since the four wheels are fixedly connected to the vehicle body, the vehicle body and the four wheels rotate around the same instantaneous center of rotation (ICR). Figure 2 As shown, the angle between the wheel and the vehicle body coordinate system is The following relationship is satisfied:

[0114] (3)

[0115] For the left front and right front wheels, namely wheels 1 and 2, their rotation radius is 、 Satisfies the following equation:

[0116] (4)

[0117] (5)

[0118] The rotation radius of the left front and right front wheels can be obtained:

[0119] (6)

[0120] Similarly, the rotation radius of the left and right rear wheels, namely wheels 3 and 4, can be obtained:

[0121] (7)

[0122] The vehicle body is the coordinate of the vehicle body in the global coordinate system and the coordinates of the wheel body in the global coordinate system The following relationship is satisfied:

[0123] (8)

[0124] (9)

[0125] (10)

[0126] in, represents the rotation matrix of the vehicle body relative to the global coordinate system, represents the rotation angle of the vehicle body relative to the global coordinate system, Indicates the coordinates of the wheel body in the four-wheel drive four-turn robot coordinate system;

[0127] When the vehicle body moves slightly around an instantaneous center, the displacement increment of the vehicle body is , Angle increment Hedi The displacement increment of each wheel The following relationship is satisfied:

[0128] (11)

[0129] in, represents the exponential mapping between two-dimensional Lie algebras and Lie groups,

[0130] , is a very small quantity, discarding the higher-order infinitesimal terms, we have:

[0131] (12)

[0132] From formulas (8) to (12), we can get:

[0133] (13)

[0134] For the left side of formula (13), for the i-th wheel, if Figure 3 As shown, the following equations exist:

[0135] (14)

[0136] (15)

[0137] (16)

[0138] (17)

[0139] in, 、 Respectively Components in the x and y directions of the global coordinate system;

[0140] Combining formulas (8) to (17), the following equations can be obtained for the four wheels:

[0141] (18)

[0142] definition 、 、 (i=1, 2, 3, 4), 、 as follows:

[0143] (19)

[0144] (20)

[0145] (twenty one)

[0146] (twenty two)

[0147] (twenty three)

[0148] (twenty four)

[0149] (25)

[0150] (26)

[0151] Then, formula (18) can be expressed as:

[0152] (27)

[0153] matrix The generalized inverse matrix of for:

[0154] (28)

[0155] (29)

[0156] Then, we have:

[0157] (30)

[0158] Then, the vehicle body’s posture increment is:

[0159] (31)

[0160] (3) Update the vehicle's posture information

[0161] The current pose information can be obtained by adding the pose increment of the vehicle body at the previous moment to the pose information of the current moment. The calculation formula is as follows:

[0162] (32)

[0163] In the embodiment of the present invention, step S4 includes:

[0164] S41, based on the angle between each wheel and the vehicle body ,According to formula (1) and formula (3), determine whether the four-wheel drive four-turn robot only moves in a straight line;

[0165] S42. If only linear motion is performed, then the distance increment based on each wheel , the angle between each wheel and the vehicle body The angle between the vehicle coordinate system and the global coordinate system , according to formula (2), calculate the vehicle body's posture increment, ;

[0166] S43. If non-linear motion is performed, then the distance increment based on each wheel , the angle between each wheel and the vehicle body The angle between the vehicle coordinate system and the global coordinate system , according to formula (31), calculate the vehicle body's posture increment, .

[0167] In the embodiment of the present invention, step S5 includes:

[0168] According to formula (32) and the vehicle body's posture increment , update the vehicle’s current position information.

[0169] like Figure 4 As shown, the specific implementation process of the method provided in the embodiment of the present invention is as follows:

[0170] (1) Initialization, set the initial posture of the vehicle body to ;

[0171] (2) Obtain the pulse values ​​of the four driving wheel encoders and the four steering wheel encoders at the current moment; compare the pulse values ​​at the previous moment to obtain the distance increment of the four wheels and the angles between the four wheels and the vehicle body ;

[0172] (3) Determine whether the vehicle moves only in a straight line according to formula (1) and formula (3);

[0173] (4) When only linear motion is performed, the vehicle body posture increment is calculated according to formula (2): ;

[0174] (5) If step (3) determines that the vehicle is moving in a non-linear manner, then, according to formula (31), the vehicle posture increment is calculated as ;

[0175] (6) Update the vehicle’s current position information according to formula (32);

[0176] (7) Repeat steps (2) to (6) to obtain the position information of the vehicle during its movement.

[0177] The embodiment of the present invention further provides a robot posture acquisition system based on a forward kinematics model. The robot is a four-wheel drive four-turn robot with four wheels. Each wheel is equipped with two independent drive motors for driving the wheel to turn and roll, corresponding to four drive wheel encoders and four steering wheel encoders, respectively. The system includes:

[0178] Building module for constructing the forward kinematics model of the four-wheel drive four-turn robot;

[0179] Initialization module, used to set the initial position of the four-wheel drive four-turn robot body;

[0180] The comparison module is used to obtain the current pulse values ​​of the four driving wheel encoders and the four steering wheel encoders, and compare them with the pulse values ​​at the previous moment to obtain the distance increment of each wheel and the angle between each wheel and the vehicle body;

[0181] The calculation module is used to determine whether the four-wheel drive four-turn robot is only moving in a straight line based on the forward kinematics model, the distance increment of each wheel, and the angle between each wheel and the vehicle body. According to the judgment result, different calculation methods are used to calculate the position increment of the vehicle body;

[0182] The update module is used to update the vehicle's current posture information based on the forward kinematics model and the vehicle's posture increment;

[0183] The acquisition module is used to cyclically execute the comparison module, calculation module and update module to obtain the position information of the four-wheel drive four-turn robot during its movement.

[0184] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A robot posture acquisition method based on a forward kinematics model, characterized in that: The robot is a four-wheel drive four-rotation robot having four wheels, each wheel being equipped with two independent drive motors for driving the wheel to turn and roll, corresponding to four drive wheel encoders and four steering wheel encoders, respectively. The method comprises: S1. Construct a forward kinematics model of a four-wheel drive four-turn robot; S2, setting the initial posture of the four-wheel drive four-turn robot body; S3. Obtaining the pulse values ​​of the four driving wheel encoders and the four steering wheel encoders at a current moment, and comparing them with the pulse values ​​at a previous moment to obtain the distance increment of each wheel and the angle between each wheel and the vehicle body; S4. Based on the forward kinematics model, the distance increment of each wheel, and the angle between each wheel and the vehicle body, determine whether the four-wheel drive four-turn robot only performs linear motion, and calculate the posture increment of the vehicle body using different calculation methods according to the determination result; S5. Based on the forward kinematics model and the posture increment of the vehicle body, updating the posture information of the vehicle body at the current moment; S6. Execute S3-S5 in a loop to obtain the posture information of the vehicle body during the movement of the four-wheel drive four-turn robot.

2. The method according to claim 1, characterized in that In step S1, the forward kinematics model is used to: (1) Define the coordinate system, related constants and variables represents the global coordinate system, Represents the body coordinate system of the four-wheel drive four-turn robot body center, Indicates the The body coordinate system of each wheel, i=1 represents the left front wheel, i=2 represents the right front wheel, i=3 represents the left rear wheel, i=4 represents the right rear wheel, and all coordinate systems satisfy the right-hand rule; Represents the distance between the front and rear wheels, which is a constant. Indicates the distance between the left and right wheels, which is a constant. represents the position increment of the vehicle center in the global coordinate system, Represents the position of the vehicle center in the global coordinate system, Indicates the The angle between the wheel and the vehicle coordinate system, Indicates the The angle increment of each wheel, Indicates the The distance increment of each wheel, Indicates the The turning radius of the wheels; (2) Calculate the vehicle's posture increment When the vehicle body moves only in a straight line, that is: （1） Vehicle posture increment for: （2） When the vehicle body moves in a non-linear manner, the angle between the wheel and the vehicle body coordinate system is The following relationship is satisfied: （3） For the left front and right front wheels, namely wheels 1 and 2, their rotation radius is 、 Satisfies the following equation: （4） （5） The rotation radius of the left front and right front wheels can be obtained: （6） Similarly, the rotation radius of the left and right rear wheels, namely wheels 3 and 4, can be obtained: （7） The vehicle body is the coordinate of the vehicle body in the global coordinate system and the coordinates of the wheel body in the global coordinate system The following relationship is satisfied: （8） （9） （10） in, represents the rotation matrix of the vehicle body relative to the global coordinate system, represents the rotation angle of the vehicle body relative to the global coordinate system, Indicates the coordinates of the wheel body in the vehicle body coordinate system; When the vehicle body moves slightly around an instantaneous center, the displacement increment of the vehicle body is , Angle increment Hedi The displacement increment of each wheel The following relationship is satisfied: （11） in, represents the exponential mapping between two-dimensional Lie algebras and Lie groups, , is a very small quantity, discarding the higher-order infinitesimal terms, we have: （12） From formulas (8) to (12), we can get: （13） For the left side of formula (13), for the i-th wheel, there is the following equation: （14） （15） （16） （17） in, 、 Respectively Components in the x and y directions of the global coordinate system; Combining formulas (8) to (17), the following equations can be obtained for the four wheels: （18） definition 、 、 (i=1, 2, 3, 4), 、 as follows: （19） （20） （21） （22） （23） （24） （25） （26） Then, formula (18) can be expressed as: （27） Generalized inverse of a matrix for: （28） （29） Then, we have: （30） Then, the vehicle body’s posture increment is: （31）。 3. The method according to claim 2, characterized in that In step S1, the forward kinematics model is also used to: (3) Update the vehicle's posture information The current pose information can be obtained by adding the pose increment of the vehicle body at the previous moment to the pose information of the current moment. The calculation formula is as follows: （32）。 4. The method according to claim 3, characterized in that Step S4 includes: S41, based on the angle between each wheel and the vehicle body , according to formula (1) and formula (3), determine whether the four-wheel drive four-turn robot only moves in a straight line; S42. If only linear motion is performed, then the distance increment based on each wheel , the angle between each wheel and the vehicle body The angle between the vehicle coordinate system and the global coordinate system , according to formula (2), calculate the vehicle body's posture increment ; S43. If non-linear motion is performed, then the distance increment based on each wheel , the angle between each wheel and the vehicle body The angle between the vehicle coordinate system and the global coordinate system , according to formula (31), calculate the vehicle body's posture increment .

5. The method according to claim 4, characterized in that Step S5 includes: According to formula (32) and the vehicle body's posture increment , update the vehicle’s current position information.

6. A robot posture acquisition system based on a forward kinematics model, used to implement the method according to any one of claims 1 to 5, characterized in that: The robot is a four-wheel drive four-turn robot with four wheels. Each wheel is equipped with two independent drive motors for driving the wheel to turn and roll, corresponding to four drive wheel encoders and four steering wheel encoders respectively. The system includes: Building module for constructing the forward kinematics model of the four-wheel drive four-turn robot; An initialization module is used to set the initial position of the four-wheel drive four-turn robot body; a comparison module, configured to obtain the pulse values ​​of the four driving wheel encoders and the four steering wheel encoders at a current moment, and compare them with the pulse values ​​at a previous moment to obtain the distance increment of each wheel and the angle between each wheel and the vehicle body; a calculation module, configured to determine whether the four-wheel drive four-turn robot performs only linear motion based on the forward kinematics model, the distance increment of each wheel, and the angle between each wheel and the vehicle body, and calculate the posture increment of the vehicle body using different calculation methods according to the judgment result; An updating module, configured to update the current posture information of the vehicle body based on the forward kinematics model and the posture increment of the vehicle body; The acquisition module is used to cyclically execute the comparison module, the calculation module and the update module to obtain the posture information of the vehicle body during the movement of the four-wheel drive four-turn robot.

Images