A kind of odometer solving method of fusion gyro double steering wheel chassis

Abstract

This invention discloses a method for calculating the odometer of a dual-steering wheel chassis with integrated gyroscope. The steps are as follows: S1, Initialization: Set the initial pose of the vehicle body; S2, Keep the chassis stationary for several seconds and calculate the zero bias of the gyroscope; S3, Obtain the current steering wheel speed and angle from the steering wheel encoder information; Obtain the current gyroscope measurement value; S4, Determine whether the chassis is stationary. If the chassis is not stationary, update the odometer information of the chassis; if the chassis is stationary, the pose information of the current odometer is the same as the previous moment, and the speed information of the odometer is 0; S5, Repeat steps S3 to S4 to obtain the odometer information of the chassis at each moment. This invention can easily and quickly obtain the pose information of the vehicle body.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot technology, specifically relating to an odometer calculation method for a dual-steering wheel chassis with integrated gyroscope. Background Technology

[0002] The dual-steering wheel chassis is a mobile robot chassis equipped with two independently controlled steering wheels. It has the capabilities of omnidirectional movement, precise positioning, high load capacity and high adaptability, and is widely used in warehousing and logistics, intelligent manufacturing and other fields.

[0003] Wheel encoders and gyroscopes are among the most commonly used sensors in mobile robots. Ideally, based on encoder information and a corresponding kinematic model, accurate odometer information of the chassis can be obtained. However, in practical applications, uneven ground and wheel slippage can severely affect the accuracy of the odometer, especially causing significant deviations in the heading angle, further exacerbating the error in the odometer's position information. Gyroscopes, on the other hand, measure angular velocity information unaffected by uneven ground or wheel slippage, exhibiting high accuracy in short periods. However, they suffer from zero-bias drift during long-term operation and under varying environmental temperatures.

[0004] Chinese patent CN112050809B proposes a fusion algorithm for odometers and gyroscopes. It improves the accuracy of the chassis odometer through techniques such as sensor error and noise analysis, online estimation of sensor variance, and Kalman filtering. However, the algorithm is too complex and unsuitable for platforms with limited computing resources. Summary of the Invention

[0005] To address the problems of large errors in odometer readings for dual-steering-wheel chassis caused by ground environmental factors and the high computational complexity of existing sensor fusion models, this invention provides a method that directly integrates gyroscopes into the forward kinematics model of a dual-steering-wheel chassis. This method enables simple and rapid acquisition of vehicle pose information using encoder and gyroscope information from the dual steering wheels, while also balancing accuracy, robustness, and high performance. It is particularly suitable for odometer reading calculations of dual-steering-wheel chassis with fused gyroscopes on platforms with limited computing resources.

[0006] The technical solution of the present invention is as follows: A method for calculating the odometer of a dual-steering wheel chassis with integrated gyroscope, comprising the following steps:

[0007] S1. Initialization, setting the initial pose of the vehicle body as follows: ;

[0008] S2. Let the chassis remain still for several seconds, and calculate the zero bias of the gyroscope. ;

[0009] S3. Obtain the current steering wheel speed from the steering wheel encoder information. and angle ; Obtain the current measurement value of the gyroscope. ;

[0010] S4. Determine if the chassis is stationary. If the chassis is not stationary, update the chassis odometer information. If the chassis is stationary, the current odometer pose information is the same as the previous moment, and the odometer speed information is 0. Accumulate the gyroscope measurements during the stationary period. If the continuous stationary time is greater than a preset threshold, update the gyroscope's zero bias. ;

[0011] S5. Repeat steps S3 to S4 to obtain the chassis odometer information at each time point.

[0012] Preferably, during the initialization step S1, the two steering wheels of the dual-steering wheel chassis are usually installed on the left and right sides of the vehicle body, and the gyroscope is installed on the chassis, with the z-axis of the gyroscope aligned with the z-axis of the vehicle body coordinate system.

[0013] Preferably, in step S1 Represents the global coordinate system; Let the body coordinate system represent the center of the vehicle body, and all coordinate systems satisfy the right-hand rule.

[0014] Preferably, the forward kinematic equations of the dual-steering wheel chassis with integrated gyroscopes are as follows:

[0015] (1)

[0016] (2)

[0017] (3)

[0018] (4)

[0019] From formulas (1) to (4), we get:

[0020] (5)

[0021] (6)

[0022] According to formulas (5) and (6), there are a total of four equations for the two steering wheels, which can be written in matrix form as follows:

[0023] (7)

[0024] After sorting, we can obtain:

[0025] (8)

[0026] in Represents wheels The vector from the center of the vehicle body to the center of the vehicle body. =1, 2; Indicates the first The angle between each wheel and the vehicle's coordinate system; Represents wheels The magnitude of the speed; In the vehicle coordinate system, the wheels The velocity vector; This represents the velocity vector at the center of the vehicle in the vehicle coordinate system. This represents the velocity vector at the center of the vehicle in the global coordinate system. This indicates the magnitude of the angular velocity at the center of the vehicle body; This indicates the magnitude of the angular velocity along the z-axis as measured by the gyroscope. This represents the zero bias magnitude of the gyroscope's z-axis; This indicates the position and orientation of the chassis center in the global coordinate system.

[0027] Preferably, the specific process of updating the chassis odometer information in step S4 is as follows:

[0028] Let the time difference between the current moment and the previous moment be . Then, at the current moment, in the global coordinate system, the update equations for the angular velocity, angle, and velocity of the vehicle center are as follows:

[0029] (9)

[0030] (10)

[0031] (11)

[0032] From formulas (8) and (11), we get:

[0033] (12)

[0034] In the global coordinate system, the position update equation for the vehicle center is: (13);

[0035] Formulas (9) to (13) are the update equations for the chassis odometer.

[0036] Preferably, the specific process for calculating the zero-bias information of the gyroscope in step S2 is as follows:

[0037] When the chassis is stationary, we have:

[0038] (14)

[0039] remember time( The measured values ​​of the gyroscope (e.g., 1, 2, 3...n) are... ,like During the time period, the chassis remained stationary, and ( If the value is greater than the preset threshold, then:

[0040] (15).

[0041] Preferably, in step S2, the chassis is kept stationary for 2 to 3 seconds.

[0042] The beneficial effects of this invention are as follows: This invention directly integrates the gyroscope into the forward kinematic model of the dual-steering wheel chassis, which can easily and quickly obtain the vehicle's pose information through the encoder information and gyroscope information of the dual steering wheels; and takes into account accuracy, robustness and high performance, making it particularly suitable for platforms with limited computing resources. Attached Figure Description

[0043] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0044] Figure 1 This is a schematic diagram of the forward kinematics model of the chassis-integrated gyroscope of the present invention;

[0045] Figure 2 This is a schematic diagram of the positive kinematics model of the chassis fusion gyroscope vehicle center relative to the i-th steering wheel of the present invention;

[0046] Figure 3 This is a schematic diagram of the process of the present invention;

[0047] Figure 4 This is a schematic diagram of the experimental equipment that is only useful for verification and analysis of this invention;

[0048] Figure 5 This is a map of odometer location information obtained using the method in this invention;

[0049] Figure 6 The odometry position information map is obtained by using the forward motion model method without fused gyroscopes;

[0050] Figure 7 This is a graph of odometer angle information obtained using the method in this invention;

[0051] Figure 8This is a graph of odometer angle information obtained using the forward motion model method without fused gyroscopes. Detailed Implementation

[0052] 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.

[0053] Example 1: A method for calculating the odometer of a dual-steering wheel chassis with integrated gyroscopes. The devices, coordinate system, relevant variables, and constants involved are described below:

[0054] The two steering wheels of a dual-steering-wheel chassis are typically mounted on the left and right sides of the vehicle body, as shown in the diagram. Figure 1 and 2 As shown, the gyroscope is mounted on the chassis, and the z-axis of the gyroscope is aligned with the z-axis of the vehicle's coordinate system.

[0055] Represents the global coordinate system; Let the coordinate system represent the body coordinate system at the center of the vehicle. All coordinate systems satisfy the right-hand rule.

[0056] Represents wheels The vector from the center of the vehicle body to the center of the vehicle body. =1, 2; Indicates the first The angle between each wheel and the vehicle's coordinate system; Represents wheels The magnitude of the speed; In the vehicle coordinate system, the wheels The velocity vector; This represents the velocity vector at the center of the vehicle in the vehicle coordinate system. This represents the velocity vector at the center of the vehicle in the global coordinate system. This indicates the magnitude of the angular velocity at the center of the vehicle body; This indicates the magnitude of the angular velocity along the z-axis as measured by the gyroscope. This represents the zero bias magnitude of the gyroscope's z-axis; This indicates the position and orientation of the chassis center in the global coordinate system.

[0057] The odometer calculation method uses the following forward kinematic model of the fused gyroscope:

[0058] The entire chassis is a rigid body, as shown in the diagram. Figure 1 and 2As shown, the following equations exist:

[0059] (1)

[0060] (2)

[0061] (3)

[0062] (4)

[0063] From formulas (1) to (4), we get:

[0064] (5)

[0065] (6)

[0066] According to formulas (5) and (6), there are a total of four equations for the two steering wheels, which can be written in matrix form as follows:

[0067] (7)

[0068] After sorting, we can obtain:

[0069] (8)

[0070] Formulas (1) and (8) are the forward kinematic equations of the dual-steering wheel chassis with fused gyroscope.

[0071] The odometer calculation method uses the following updated chassis odometer information:

[0072] Let the time difference between the current moment and the previous moment be . Then, at the current moment, in the global coordinate system, the update equations for the angular velocity, angle, and velocity of the vehicle center are as follows:

[0073] (9)

[0074] (10)

[0075] (11)

[0076] From formulas (8) and (11), we get:

[0077] (12)

[0078] In the global coordinate system, the position update equation for the vehicle center is: (13);

[0079] Formulas (9) to (13) are the update equations for the chassis odometer.

[0080] The odometer calculation method uses the following estimated gyroscope bias information:

[0081] When the chassis is stationary, we have:

[0082] (14)

[0083] remember time( The measured values ​​of the gyroscope (e.g., 1, 2, 3...n) are... ,like During the time period, the chassis remained stationary, and ( If the value is greater than the preset threshold, then:

[0084] (15).

[0085] Example 2: A method for calculating the odometer of a dual-steering wheel chassis with integrated gyroscopes. The specific implementation process is as follows (e.g., Figure 3 (as shown)

[0086] (1) Initialization, setting the initial pose of the vehicle body as follows: ;

[0087] (2) Let the chassis remain still for 2-3 seconds, and calculate the zero bias of the gyroscope according to formula (15). ;

[0088] (3) Obtain the current steering wheel speed from the steering wheel encoder information. and angle ; Obtain the current measurement value of the gyroscope. ;

[0089] (4) Determine whether the chassis is stationary according to formula (14). If the chassis is not stationary, update the odometer information of the chassis according to formulas (9) to (13). If the chassis is stationary, the pose information of the current odometer is the same as the previous moment, and the speed information of the odometer is 0; accumulate the measurement value of the gyroscope during the stationary period. If the continuous stationary time is greater than the preset threshold, update the zero bias of the gyroscope according to formula (15). ;

[0090] (5) Repeat steps (3) to (4) to obtain the odometer information of the chassis at each time.

[0091] Example 3: The verification and analysis of the present invention are as follows, using the experimental equipment as follows: Figure 4As shown in the diagram, positions 1 and 2 of the experimental equipment are equipped with two steering wheels, positions 3 and 4 are equipped with two omnidirectional wheels, and position 5 is equipped with a gyroscope.

[0092] Experimental method: Control the experimental equipment to perform a circular motion and eventually return to the starting point; lift one wheel for a period of time during the motion to simulate slippage; record the raw data of the two steering wheel encoders in real time throughout the motion; based on the recorded encoder raw data, calculate the chassis odometer information during operation using the method in this invention and the forward motion model method without fusion gyroscopes.

[0093] The origin is taken as the position at the start of the experimental equipment's movement, with the x-axis pointing directly forward and the y-axis pointing to the left, forming a right-handed coordinate system. Theoretically, when the experimental equipment returns to the origin, the positional and angular deviations of the odometer between the starting and ending points should both be zero. However, due to mechanical errors, encoder errors, slippage, etc., the calculated odometer readings usually contain some errors.

[0094] Figure 5 , Figure 6 The x-axis represents the x-coordinate value of the odometer, and the y-axis represents the y-coordinate value of the odometer. As you can see, Figure 4 In the equation, the Euclidean distance between the starting point and the ending point is approximately 0.18 meters. Figure 5 In this case, the Euclidean distance between the starting and ending points is approximately 10.8 meters. That is, by using the method described in this invention, the positional accuracy of the odometer is greatly improved.

[0095] Figure 7 , Figure 8 The x-axis represents time, and the y-axis represents angle. As you can see, Figure 7 In the middle, the angular deviation between the starting point and the ending point is 0.002 radians; Figure 8 In this study, the angular deviation between the starting and ending points was 1.461 radians. That is, the angular accuracy of the odometer was greatly improved after using the method described in this invention.

[0096] In other words, the method in this invention significantly improves the accuracy and robustness of the odometer. It maintains good accuracy even in the presence of slippage.

[0097] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0098] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A method for calculating the odometer of a dual-steering wheel chassis with integrated gyroscope, characterized in that: The steps are as follows: S1. Initialization, setting the initial pose of the vehicle body as follows: ; S2. Let the chassis remain still for several seconds, and calculate the zero bias of the gyroscope. ; S3. Obtain the current steering wheel speed from the steering wheel encoder information. and angle ; Obtain the current measurement value of the gyroscope. ; S4. Determine if the chassis is stationary. If the chassis is not stationary, update the chassis odometer information. If the chassis is stationary, the current odometer pose information is the same as the previous moment, and the odometer speed information is 0; the gyroscope measurements during the stationary period are accumulated, and if the continuous stationary time exceeds a preset threshold, the gyroscope's zero bias is updated. ; S5. Repeat steps S3 to S4 to obtain the chassis odometer information at each time point; The forward kinematic equations for the dual-steering wheel chassis with integrated gyroscopes are as follows: （1） （2） （3） （4） From formulas (1) to (4), we get: （5） （6） According to formulas (5) and (6), there are a total of four equations for the two steering wheels, which can be written in matrix form as follows: （7） After sorting, we can obtain: （8） in Represents wheels The vector from the center of the vehicle body to the center of the vehicle body. =1, 2; Indicates the first The angle between each wheel and the vehicle's coordinate system; Represents wheels The magnitude of the speed; In the vehicle coordinate system, the wheels The velocity vector; This represents the velocity vector at the center of the vehicle in the vehicle coordinate system. This represents the velocity vector at the center of the vehicle in the global coordinate system. This indicates the magnitude of the angular velocity at the center of the vehicle body; This indicates the magnitude of the angular velocity along the z-axis as measured by the gyroscope. This represents the zero bias magnitude of the gyroscope's z-axis; This indicates the position and orientation of the chassis center in the global coordinate system.

2. The method for calculating the odometer of a dual-steering wheel chassis with fused gyroscopes according to claim 1, characterized in that: During the initialization in step S1, the two steering wheels of the dual-steering wheel chassis are installed on the left and right sides of the vehicle body, and the gyroscope is installed on the chassis. The z-axis of the gyroscope is aligned with the z-axis of the vehicle body coordinate system.

3. The method for calculating the odometer of a dual-steering wheel chassis with fused gyroscopes according to claim 2, characterized in that: In step S1 Represents the global coordinate system; Let the body coordinate system represent the center of the vehicle body, and all coordinate systems satisfy the right-hand rule.

4. The method for calculating the odometer of a dual-steering wheel chassis with fused gyroscopes according to claim 1, characterized in that: The specific process of updating the chassis odometer information in step S4 is as follows: Let the time difference between the current moment and the previous moment be . Then, at the current moment, in the global coordinate system, the update equations for the angular velocity, angle, and velocity of the vehicle center are as follows: （9） （10） （11） From formulas (8) and (11), we get: （12） In the global coordinate system, the position update equation for the vehicle center is: （13）； Formulas (9) to (13) are the update equations for the chassis odometer.

5. The method for calculating the odometer of a dual-steering wheel chassis with fused gyroscopes according to claim 1, characterized in that: The specific process for calculating the zero-bias information of the gyroscope in step S2 is as follows: When the chassis is stationary, we have: （14） remember time =1, 2, 3...n The measured values ​​of the gyroscope are... ,like During the time period, the chassis remained stationary, and If it exceeds the preset threshold, then: （15）。 6. The method for calculating the odometer of a dual-steering wheel chassis with fused gyroscopes according to claim 1, characterized in that: In step S2, the chassis is kept stationary for 2 to 3 seconds.

Images