SINS/OD fault-tolerant navigation algorithm based on tracked vehicle kinematic constraints

In this paper, for the problem that the accuracy declining of the SINS/OD navigation caused by the sliding and idling during the process of turning and straight running, and stopping output of the odometer, a kinematic constraint model of the tracked vehicle is established. The sliding and idling during the process of turning are compensated. In the filtering process, the difference of the displacement increment is used as the observed quantity, and a two-layer fault detection algorithm is used to detect the output fault of the odometer. The simulation experiments show that the SINS/OD integrated navigation algorithm assisted by the kinematic constraints of the tracked vehicle can detect the output fault of the odometer accurately and perform fault-tolerant navigation, which greatly improves the positioning accuracy after the odometer failure.


Introduction
With the change of the form of war, the modern war requires that the combat vehicles can be flexible in the wide battlefield, provide their own position information in time and accurately, and realize the real-time inspection and beating.The tracked vehicle navigation system composed of the SINS and odometer (OD) has the characteristics of complete autonomy, high accuracy and good real-time performance.SINS has the advantages of high accuracy and low failure rate.It is usually used as a reference system.The number of pulses output by OD unit time indicates the increment of total mileage, and it can't directly express the position of the carrier1.When the vehicle body's motion state is complex, its output can't describe the motion of the carrier.At this time, the rationality of the OD output should be detected2.
At present, the commonly used fault detection method in integrated navigation is  test is often used to detect and isolate faults4.In the process of the research, Zhao Hongsong 1detected three velocity components in the carrier coordinate system.Gong Jun4 designed a two-layer fault detection algorithm to detect the velocity components.All of them have achieved good results.
In the SINS/OD combination navigation, it is necessary to make a difference in the output of OD when measuring the speed, and it is easy to amplify the observation noise and then affect the filtering accuracy.For the position measurement, the dead reckoning (DR) error should be increased, the system dimension increases, and the error of the OD will accumulate gradually, causing the error to diverge.At the same time, the observation of the system is reduced, and it needs a long time to estimate the state.In this paper, the displacement increment in the unit time is selected as the observation measurement.At the same time, according to the characteristics of the tracked vehicle, the motion constraint model of the tracked vehicle is established, and the SINS/OD fault-tolerant navigation scheme based on the kinematic constraints of the tracked vehicle is proposed.

Characteristics of tracked vehicle movement
Tracked vehicles are different from wheeled vehicles.Tracked vehicles has no steering wheels for direct steering.Instead, they change the track ratio of the corresponding side through planet steering mechanism on the left and right sides, and use the speed difference between the two tracks to achieve steering.Two two-stage planet steering mechanisms are used on each of the left and right sides of a certain type tracked vehicle, and its joystick has three operating positions: the most former position, the position I and the position II.The corresponding motion states of the above three positions are as follows: (1) The most former position: When one side joystick is at the most former position, the transmission ratio is approximately 1:1 and it is in a direct transmission state.
(2) Position I: When one side of the joystick is at the position I, the gearbox rotation speed is a constant, and the transmission ratio of this test is approximately 1.42:1, which is at a low speed.
(3) Position II: When one side of the joystick is at the position II, the driving wheel of this side is in a locked state and the transmission ratio is 0.

Kinematic constraints and OD correction of tracked vehicles
Define coordinate system m as the odometer coordinate system.The coordinate origin O is located in the installation position of the odometer.The During turning, the movement of the high speed side track and the low speed side track is inconsistent, the high speed side track will idle, the low speed side track will slide5 and the lateral velocity of the vehicle is not zero, as shown in Figure 1.The kinematic constraint of ordinary wheeled vehicle can't meet the needs of tracked vehicle, so it is necessary to re-establish kinematic constraints and correct the OD output at the right time.According to the position of the two joysticks, the motion state of the tracked vehicle can be divided into five states: running straight line at high speed, running straight line at low speed, turning in situ, turning in curve and parking.

running straight line at high speed
When the two joysticks of the tracked vehicle are at the most former position, the planet steering mechanisms have a transmission ratio of 1:1, and the output of the OD is equal to the true speed.At this time:

running straight line at low speed
When both sides of the joystick are at the position  , the vehicle runs at low speed, and the transmission ratio on both sides is 1.42:1.At this time:

Turning in situ
When one joystick is in the position II and the other is in the most former position or position I, the vehicle performs turning in situ.At low speed side, the track driving wheel is in a locked state, the transmission ratio is 0, and the track on the other side is running in a high speed or a low speed.The turning radius R  is half of two track spans When sliding and idling occured in the process of turning in situ, the displacement of the vehicle body is small and the actual output of the odometer is larger, so the OD information should be abandoned and the navigation parameters are calculated only by using the inertial navigation data, and no measurement updates should be carried out until turning is completed.

Turning in curve
When one joystick is in the position I and the other joystick is in the most former position, the vehicle turns in curve.At this point the turning radius R  satisfies: In the process of turning in curve , the displacement of vehicle is large.It is necessary to modify and compensate the sliding and idling during the turning process based on the kinematic constraint of the tracked vehicle.Ignoring the influence of the offset of the vehicle body centroid, the lateral speed of vehicle body during turning in curve is as follow: Where  is the steering angle rate and L is the track ground length.The forward speed is: On the cement floor6, 1 0.
After correction, the speed output of tracked vehicle is 0 , and the increment of the position from

The model of integrated navigation system
The navigation coordinate system n is defined as the east-north-up (ENU) coordinate system, the carrier coordinate system b and the OD coordinate system m is the right front up (RFU) coordinate system. ) Among them, cos cos cos sin sin sin cos sin cos cos cos sin 0 sin cos

Measurement of displacement increment under the carrier coordinate system
The filtering period of integrated navigation is

Combination scheme
The attitude error  , velocity error Where Di ω ω is the observation noise.

 test
The idea of residual 2  test is to get the residual error of step k according to the filtering equation.
Its covariance as follow: ,1 If there is no fault in the system, k  should be Gauss white noise with zero mean, that is, (0, P )  does not satisfy this property, then the system will fail.Therefore, the fault detection function can be constructed as The threshold   , the system is judged to be normal.Otherwise, the system is faulty.

Fault classification
The reasons for the abnormal OD output in the course of tracked vehicle motion are as follows: sliding, idling, no output of OD, turning in situ, turning in curve, bumping and jumping, etc.The first type of fault: When the vehicle is sliding, idling and there is no output from the OD, the actual distance traveled by the vehicle between m yi S  are obviously inconsistent and is called the first type of fault.
The second type of fault: When the vehicle sidesliding, turning in situ or turning in curve, the crawler will slide or idle, causing 0 m xi S  , which is called the second type of fault.
The third type of fault: When the road condition is poor, the vehicle will bump and jump, causing the vehicle to move from high to low, which is called the third type of fault.

Two-layer fault detection scheme
According to the tracked vehicle kinematics constraint model, when the vehicle is running normally, there is no lateral and vertical movement, that is,   Otherwise, go to step 3.
Step 3.Only the SINS information is used for purely inertial navigation and no measurement updates are performed.

Simulation
The simulation conditions are set as follows: the constant drift of the gyro is 0.02 /h, the noise is 0.02 /h, the accelerometer bias is 1×10 -4 g, the noise is 0.5×10 -4 g, the calibration error of OD is [0.5 0.02 0. Running downhill with uniform velocity sliding The attitude error, velocity error, and position error of the SINS/OD integrated navigation are given in Figure 2, Figure 3 and Figure 4 respectively.The red curve represents the result of the algorithm without fault detection and fault tolerance.The black curve represents the result of the two-layer fault detection and fault tolerance algorithms under the vehicle kinematic constraints in the SINS/OD integrated navigation.And the moments of failure are indicated by the vertical dotted lines.
It can be seen from the yaw error in Fig. 1, the northward speed error in Fig. 2 and the latitude and longitude error in Fig. 3, the OD output information can't match the actual motion of the vehicle when a fault occurs.Conventional SINS/OD integrated navigation can't find the faults and isolate them in time, leading to sudden jumps in various errors and the quality of navigation sharply decreasing.In the conventional SINS/OD integrated navigation mode, the errors can't converge to the ideal accuracy in a short time even after the fault recovery.Fault-tolerant navigation is able to detect and isolate the OD faults effectively by using two-layer fault detection algorithm under the tracked vehicle kinematic constraints.It can be seen from the errors, there is no obvious changes of the accuracy of fault-tolerant SINS/OD navigation when the fault occurs, and the accuracy always maintained at a high level.

Conclusion
In this paper, a kinematic constraint model for tracked vehicles is established, and the OD output under the special motion conditions is compensated.The difference between position increments under the carrier coordinate system is used as the observation and the error output of OD is detected and isolated by the two-layer fault detection scheme.The simulation results show that the SINS/OD integrated navigation algorithm based on tracked vehicles kinematic constraints can isolate the fault information of OD in time and greatly improve the accuracy of positioning.It has guiding significance for the engineering practice.
to the right of the body transverse axis, the m y axis points to the front of the body longitudinal axis and the m z axis is perpendicular to the mm ox y plane and vertical upward.

Figure 1 .
Figure 1.Kinematic relation of the two side tracks when turning.

.
The installation deviation between OD and SINS are yaw deviation   , pitch deviation   and roll deviation   .There is a transformation matrix b m C between the coordinate system b and the coordinate system m .When the vehicle has no lateral displacement, the following relationship exists7: error of yaw installation deviation, scale factor error and the calibration error of pitch installation deviation of the OD.Neglecting the second order small quantity, the actual displacement increment is as follows: n δv , position error δp , gyro drift b ε , and accelerometer bias b  are selected as the state of SINS.The state of integrated navigation system consists of the state of the SINS and OD, that is, displacement increment of SINS and OD is taken as the observation.In a short period of time, consider that 


constraints are used to assist SINS to navigate.Otherwise, the pure inertial navigation is carried out.The concrete steps are as follows:Step 1.According to the Kalman filtering equation, the measurement is valid, the SINS/OD navigation is performed normally.Otherwise, the process jumps to step 2.Step 2. Construct a new observation equation and the detection method is the same as in step 1.When the observation is valid, the measurement is updated based on

Figure. 2
Figure. 2 The estimation of attitude error Where v  is the track speed on the low speed side and v  is the track speed on the high speed side.

Table 1
The type of faults and the time of its occurrence