Navigation deviation correction method suitable for crawler chassis

Abstract

The invention discloses a navigation deviation correction method suitable for a crawler chassis, which relates to the technical field of navigation positioning and comprises the following steps: step 1, establishing a local positioning system model; a speedometer and an inertial sensor IMU are arranged on the robot, and the speedometer, the IMU and an extended Kalman filter EKF form a system; according to the method, the wheel speed information of the crawler chassis and the measurement data of the IMU are fused to construct the local positioning system model, the advantages are made up of the disadvantages, the measurement data of the IMU are effectively utilized, the positioning information of the robot under the local positioning system model is optimally estimated by using the Kalman filtering method, and the positioning accuracy of the robot is improved. The method is suitable for local short-distance navigation where SLAM cannot be carried out, accurate local positioning information can be obtained without depending on a laser radar, and the stability and accuracy of short-distance positioning are achieved under the condition that the road surface working condition is poor.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation and positioning, and more specifically, to a navigation deviation correction method applicable to a crawler chassis. Background Art

[0002] The environment in which the crawler chassis operates is usually applied to ground scenarios with poor passability and stability. Due to the uneven height of the vehicle chassis, the overall height of the detection robot is only designed to be 107 mm, and the working sites are diverse. It is impossible to use related technologies such as SLAM to achieve navigation, and the running distance is short. Therefore, it is a more appropriate choice to perform small-range local positioning through an odometer. The existing technical solution 1 usually calculates the speed information of the chassis centroid through parameters such as the rotation speed, wheel diameter, wheelbase, and wheelbase of the chassis wheels, and further integrates the time according to the motion model to calculate the odometer information of the robot; Another solution is to use an IMU to achieve attitude calculation. The IMU integrates an accelerometer and a gyroscope, which can measure the acceleration and angular velocity information of the robot. Given an initial value, the pose of the robot can be calculated by integrating the acceleration and angular velocity.

[0003] For the existing technical solution 1, which is based on wheel rotation for calculation, it requires an ideal grip scenario to have a better effect, and has high requirements for the scenario. When there is slipping or manual movement, this method cannot correctly sense the movement of the robot. In addition, when the crawler chassis rotates, the error generated is larger than that of a differential chassis. These factors will cause the error of the odometer to be too large to meet the use requirements. For the existing technical solution 2, due to the existence of IMU accelerometer noise, even when stationary, there is always a speed, and the displacement is the second integral of the acceleration, which is obviously too large for direct position calculation. However, its angular velocity has extremely small errors in a short time and is suitable for attitude calculation. Summary of the Invention

[0004] The purpose of the present invention is to: in order to solve the above technical problems, the present invention provides a navigation deviation correction method applicable to a crawler chassis, and intends to fuse the wheel speed information of the crawler chassis with the measurement data of the IMU, learn from each other's strengths, and achieve a more accurate odometer to achieve accurate local positioning.

[0005] The present invention specifically adopts the following technical solutions to achieve the above purpose:

[0006] A navigation deviation correction method applicable to a crawler chassis includes the following steps:

[0007] Step 1, establish a local positioning system model;

[0008] 1) The robot is equipped with an odometer and an inertial sensor IMU. The odometer, IMU, and extended Kalman filter EKF form a system to establish the system state, and the system state is μ:

[0009] μ = [x y θ v] T (1);

[0010] Among them, both x and y are used to represent the position of the robot, θ is the heading of the robot, and ν represents the forward speed of the robot;

[0011] The prior state estimate of the system is The noise is The posterior estimated state is The noise is

[0012] 2) Let the control input u be the IMU data, and take u = [a ω] (2);

[0013] Among them, a is the acceleration in the forward direction of the robot, and ω is the rotation of the robot around the z-axis;

[0014] 3) Take the system state transition equation as:

[0015] Take the Jacobian matrix

[0016] Among them, t is the current sampling time, and Δt represents the sampling time interval, that is, the time interval from the last execution of EKF to the current moment;

[0017] When the IMU data is received, substitute the corresponding a and ω in Equation (2) into Equations (3) and (4), and the system state transition equation and the Jacobian matrix at the current moment can be calculated;

[0018]

[0019]

[0020] Substitute the and in Equations (5) and (6) into the EKF;

[0021] 4) Let the observed quantity z be the linear velocity of the odometer, then z = [v] (7);

[0022] Let the observation noise be R t , and take

[0023] Among them, α is a constant, and take α = 0.005;

[0024] Let the observation matrix be H = [0 0 0 1] (9);

[0025] 5) Prior estimation noise of the system state

[0026] Among them, Q t is a test parameter;

[0027] If the system does not receive the speed information from the odometer of the robot's crawler chassis, the calculated system state transition equation will be directly output as the current system state, which is the positioning information of the robot;

[0028] 6) When receiving the odometer data of the crawler chassis, since only its linear velocity is used,

[0029] so zt = [vt] (11);

[0030] 7) Then the Kalman gain of the system is:

[0031] 8) Then the posterior state estimation of the current system And the current state of the system is output, which is the positioning information of the robot;

[0032] The posterior noise of the current system is:

[0033] Step 2, establish a robot trajectory correction model;

[0034] A. When starting the detection, record the current positioning information p0 = [x0 y0 θ0] T (15);

[0035] Among them, p0 is the pose of the crawler chassis in the local positioning system model coordinate system in Step 1, x0 and y0 represent the position coordinates of the robot in the local positioning system model coordinate system, and θ0 represents the heading of the robot in the local positioning system model coordinate system;

[0036] B. Determine the target point p b =[x b y b θ b T (16);

[0037] Among them, p b represents the representation of the detection target in the crawler chassis coordinate system, x b , y b represent the position coordinates of the detection target in the crawler chassis coordinate system (the origin of the crawler chassis coordinate system is at the rotation center of the crawler chassis, the x-axis is in front of the robot, the y-axis is on the left side of the robot, and the z-axis points to the sky), θ b ​Indicates the heading of the robot when it reaches the detection target (also represented in the robot coordinate system);

[0038] C. Calculate the representation of the detected target pose in the local positioning system model coordinate system:

[0039]

[0040] where p g represents the pose of the detected target point in the local positioning system model coordinate system;

[0041] Then when the robot navigates through the control algorithm, it can use p g as a reference point to correct the detection trajectory of the robot in real time.

[0042] In step 1, take

[0043] In step 1, take the confidence of the initial state of the system

[0044] In step 1, initialize the system state :

[0045] The beneficial effects of the present invention are as follows:

[0046] The present invention integrates the wheel speed information of the tracked chassis and the measurement data of the IMU to construct a local positioning system model, making use of their respective advantages and effectively utilizing the measurement data of the IMU. The present invention uses the Kalman filtering method to optimally estimate the positioning information of the robot under the local positioning system model. The present invention constructs a robot trajectory correction model through the local positioning system model to correct the detection trajectory of the robot, improving the stability and accuracy of the robot's positioning. The present invention is applicable to local short-distance navigation where SLAM cannot be performed, can obtain a relatively accurate local positioning information without relying on lidar, and can achieve the stability and accuracy of short-distance positioning under poor road conditions. Brief Description of the Drawings

[0047] Figure 1 is the principle block diagram of the present invention. Detailed Embodiments

[0048] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Usually, the components of the embodiments of the present invention described and illustrated herein can be arranged and designed in various different configurations.

[0049] Accordingly, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely represents selected embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the scope of protection of the present invention.

[0050] Embodiment 1

[0051] As Figure 1 shown, a navigation deviation correction method applicable to a crawler chassis is characterized by including the following steps:

[0052] Step 1, establish a local positioning system model;

[0053] 1) An odometer and an inertial sensor IMU are provided on the robot. The odometer, IMU, and extended Kalman filter EKF form a system, and a system state is established. The system state is μ:

[0054] μ = [x y θ v] T (1);

[0055] where x and y are both used to represent the position of the robot, θ is the heading of the robot, and ν represents the forward speed of the robot;

[0056] The prior state estimate of the system is The noise is The posterior estimated state is The noise is

[0057] 2) Let the control input u be the IMU data, and take u = [a ω] (2);

[0058] where a is the acceleration in the forward direction of the robot, and ω is the rotation of the robot around the z-axis;

[0059] 3) Take the system state transition equation as:

[0060] Take the Jacobian matrix

[0061] where t is the current sampling time, and Δt represents the sampling time interval, that is, the time interval from the last execution of the EKF to the current moment;

[0062] When the IMU data is received, substitute the corresponding a and ω in Equation (2) into Equations (3) and (4), and the system state transition equation and the Jacobian matrix at the current moment can be calculated;

[0063]

[0064]

[0065] Substitute the and in Equations (5) and (6) into the EKF;

[0066] 4) Assume the observed quantity z is the linear velocity of the odometer, then z = [v] (7);

[0067] Assume the observation noise is R t , and take

[0068] where α is a constant, and take α = 0.005;

[0069] Assume the observation matrix is H =

[0001] (9);

[0070] 5) The prior estimation noise of the system state is:

[0071] where Q t is a test parameter;

[0072] If the system does not receive the speed information of the odometer of the robot's crawler chassis, directly output the calculated system state transition equation as the current system state, which is the positioning information of the robot;

[0073] 6) When receiving the odometer data of the crawler chassis, since only its linear velocity is used,

[0074] so z t = [v t (11);

[0075] 7) Then the Kalman gain of the system is:

[0076] 8) Then the posterior state estimation of the current system is: And output the current system state as the positioning information of the robot;

[0077] The posterior noise of the current system is:

[0078] Step 2, establish a robot trajectory correction model;

[0079] A. When starting the detection, record the current positioning information p0 = [x0 y0 θ0] T (15);

[0080] Among them, p0 is the pose of the crawler chassis in the local positioning system model coordinate system in step 1, x0 and y0 represent the position coordinates of the robot in the local positioning system model coordinate system, and θ0 represents the heading of the robot in the local positioning system model coordinate system;

[0081] B. Determine the target point p b =[x b y b θ b T (16);

[0082] Among them, p b represents the representation of the detection target in the crawler chassis coordinate system. x b , y b represent the position coordinates of the detection target in the crawler chassis coordinate system (the origin of the crawler chassis coordinate system is at the rotation center of the crawler chassis, the x-axis is in front of the robot, the y-axis is on the left side of the robot, and the z-axis points to the sky), and θ b represents the heading of the robot when it reaches the detection target (also the representation in the robot coordinate system);

[0083] C. Calculate the representation of the detection target pose in the local positioning system model coordinate system:

[0084]

[0085] Among them, p g represents the pose of the detection target point in the local positioning system model coordinate system;

[0086] Then when the robot navigates through the control algorithm, it can use p g as a reference point to correct the detection trajectory of the robot in real time.

[0087] In step 1, take Among them, the Q t value is a relatively reasonable value obtained based on engineering practice experience. Specifically, the setting of the Q t value is equivalent to the weight of the system prediction and the predicted value. The smaller the Q t value, the more the system state tends to believe the predicted value. The larger the Q t value, the greater the uncertainty, and the more the system state tends to believe the observed value. The accuracies of the IMU and the crawler chassis driver are both influencing factors of the Q t value. In this application, the IMU model is CH104, and the crawler chassis driver model is motec's COBRA4812-SM-G-C. Combining the above influencing factors, the above Q t value is obtained through multiple platform tests. The above Q t ​The setting of the value enables the system to have a relatively accurate and robust positioning effect.

[0088] In step 1, the confidence level of the initial state of the system is taken : In step 1, the system state is initialized :

Claims

1. A navigation correction method applicable to a crawler chassis, characterized in that, It includes the following steps: Step 1, establish a local positioning system model; 1) An odometer and an inertial sensor IMU are provided on the robot. The odometer, IMU, and extended Kalman filter EKF form a system to establish the system state, and the system state is μ: μ = [x y θ v] T (1); Among them, both x and y are used to represent the position of the robot, θ is the heading of the robot, and ν represents the forward speed of the robot; The prior state estimate of the system is The noise is The posterior estimated state is The noise is 2) Let the control input u be the IMU data, and take u = [a ω] (2); Among them, a is the acceleration in the forward direction of the robot, and ω is the rotation of the robot around the z-axis; 3) Take the system state transition equation as: Take the Jacobian matrix Among them, t is the current sampling moment, and Δt represents the sampling time interval, that is, the time interval from the last execution of the EKF to the current moment; When receiving the IMU data, substitute the corresponding a and ω in Equation (2) into Equations (3) and (4) to calculate the system state transition equation and the Jacobian matrix at the current moment; Confidence of the initial state of the system Set the system status to initialization Substitute and in equations (5) and (6) into the EKF; and into the EKF; 4) Let the observed quantity z be the linear velocity of the odometer, then z = [v] (7); Let the observation noise be R t , take Among them, α is a constant, and take α = 0.005; Let the observation matrix be H = [0 0 0 1] (9); 5) Prior estimation noise of system state is as follows: Among them, Q t is a test parameter; If the system does not receive the speed information of the odometer of the robot's crawler chassis, directly output the calculated system state transition equation as the current system state, which is the positioning information of the robot; 6) When receiving the odometer data of the crawler chassis, since only its linear velocity is used, Therefore, z t = [v t (11); 7) Then the Kalman gain of the system is as follows: 8) Then the posterior state estimate of the current system And output the current state of the system which is the positioning information of the robot; The posterior noise of the current system is: Step 2, establish a robot trajectory correction model; A, When starting the detection, record the current positioning information p0 = [x0 y0 θ0] T (15); Among them, p0 is the pose of the crawler chassis in the coordinate system of the local positioning system model in Step 1, x0 and y0 represent the position coordinates of the robot in the coordinate system of the local positioning system model, and θ0 represents the heading of the robot in the coordinate system of the local positioning system model; B, Determine the target point p b = [x b y b θ b T (16);​ Among them, p b represents the representation of the detection target in the crawler chassis coordinate system, x b , y b represent the position coordinates of the detection target in the crawler chassis coordinate system (the coordinate origin of the crawler chassis coordinate system is at the rotation center of the crawler chassis, the x-axis is in front of the robot, the y-axis is on the left side of the robot, and the z-axis points to the sky), and θ b represents the heading of the robot when it reaches the detection target (also the representation in the robot coordinate system); C, calculate the representation of the detected target pose in the coordinate system of the local positioning system model: Among them, p g represents the pose of the detected target point in the coordinate system of the local positioning system model; when the robot navigates through the control algorithm, it can use p g as a reference point to correct the detection trajectory of the robot in real time.

2. The navigation deviation correction method applicable to a crawler chassis according to claim 1, wherein, In the said step 1, take 3. The navigation deviation correction method applicable to a crawler chassis according to claim 1, wherein In step 1, take the confidence level of the initial state of the system 4. The navigation deviation correction method applicable to a crawler chassis according to claim 1, characterized in that, In step 1, the system status is initialized

Images