A method, algorithm and device for multi-sensor positioning of salt field movable ballast machine

Abstract

The present invention belongs to the technical field of outdoor positioning in salt fields, and in particular, relates to a method, algorithm and device for multi-sensor positioning of a salt field movable ballast machine. The method for multi-sensor positioning of a salt field movable ballast machine comprises the following steps: S1: using GPS and IMU inertial navigation sensors to obtain data information, and synchronizing the data between the two sensors in time according to the timestamp of the data; S2: determining the conversion relationship matrix between the two sensors according to the relationship between the relative positions and relative postures of the two sensors, and synchronizing the data of the two sensors to the same coordinate system; S3: fusing the synchronized data through an improved extended Kalman filter strategy to obtain the position data of the movable ballast machine; S4: feeding back the positioned position data to the movable ballast machine control console. The present invention fuses GPS and IMU data information to obtain more accurate and stable position information of the movable ballast machine, and the algorithm provides the movable ballast machine with its own position information for autonomous operation.

Description

Technical Field

[0001] The invention belongs to the technical field of outdoor positioning of salt pans, and in particular relates to a method, an algorithm and a device for multi-sensor positioning of a salt pan movable mucking machine. Background Art

[0002] With the improvement of economic conditions, industrial equipment is undergoing a series of reforms such as intelligence and automation. For example, in the salt field production, many large salt fields still use manually operated slag machines to repeatedly rake the salt crystal nuclei in the salt field so that the required salt crystals can be smoothly produced. On the one hand, it causes a large amount of manpower and financial resources to be invested, resulting in increased production costs. On the other hand, the production environment of the salt field is poor and unfriendly to workers. Relying on navigation and positioning technology, the manual operation of the slag machine is replaced by unmanned operation. Through sensor technology and positioning technology, the position of the slag machine at work is confirmed. Workers do not need to drive the equipment to the site for operation. On the one hand, it reduces labor costs and improves production efficiency, while production safety is also guaranteed to a certain extent.

[0003] For the problem of positioning the active ballast machine in salt fields, using sensors such as GPS for positioning is currently a common solution. However, GPS has problems such as multipath effect, low sampling frequency and susceptibility to electromagnetic interference; using IMU can provide high-precision, high-frequency speed and acceleration information of the active ballast machine, and the location information and trajectory of the active ballast machine can be obtained through integration, but it has a large cumulative error and cannot provide accurate location information for a long time. Summary of the invention

[0004] The purpose of the present invention is to address the deficiencies in the current technology in this field and to propose a multi-sensor positioning method, algorithm and device for a salt field active mucking machine, which integrates GPS and IMU data information to obtain more accurate and stable active mucking machine position information. The algorithm provides the active mucking machine with its own position information for autonomous operation.

[0005] The specific technical solutions provided by the present invention are as follows:

[0006] In a first aspect, the present invention provides a method for multi-sensor positioning of a salt field movable ballast machine, comprising the following steps:

[0007] S1: Use GPS and IMU inertial navigation sensors to obtain data information, and synchronize the data between the two sensors according to the timestamp of the data;

[0008] S2: According to the relative position and relative posture of the two sensors, the transformation relationship matrix between the two sensors is determined, and the data of the two sensors are synchronized to the same coordinate system;

[0009] S3: After the synchronized data is integrated through the improved extended Kalman filter strategy, the position data of the ballast machine is obtained;

[0010] S4: Feedback the positioning position data to the ballast machine control console.

[0011] In a second aspect, the present invention provides an algorithm based on multi-sensor positioning for a ballast machine, comprising the following steps:

[0012] S1: Use GPS and IMU inertial navigation sensors to collect data related to the position of the ballast machine and align the coordinate system;

[0013] S2: According to the operation law of the movable ballast machine, the terrain influence factor is introduced to establish the corresponding motion state model, and at the same time, the corresponding position observation model is established;

[0014] S3: Determine the improved EKF filtering steps and fusion strategy.

[0015] Furthermore, step S1 specifically includes:

[0016] S11: fix the relative position of the two sensors and start the GPS and IMU sensors;

[0017] S12: Perform translation and rotation transformation according to the installation position of the sensor. Assume that the coordinate systems of IMU and GPS are and , and know the transformation matrix between them , the IMU data can be obtained from Convert to :

[0018] ;

[0019] in, It is determined by the relative position and relative posture between sensors.

[0020] Furthermore, step S2 specifically includes:

[0021] S21: State transfer equations in the generalized extended Kalman filter On the basis of the above, according to the operating conditions of the active ballast machine, the motion state equation is adjusted to improve the filtering effect. The specific motion state equation formula is as follows:

[0022] ;

[0023] in, is the state transition matrix, is the state vector; is the control input matrix; is the terrain influence matrix, describing the influence of terrain on the state. is the control input vector (acceleration control); is the terrain influence factor vector (slope or terrain change); is the process noise, generally Gaussian white noise;

[0024] Among them, the state vector Contains the following elements:

[0025] ;

[0026] in, is a position variable, is the speed variable, is the acceleration variable, and the first obtained GPS position information is used as the initial state data of the state vector;

[0027] In the working environment of the ballast machine, the bottom terrain that the chassis track contacts will have larger or smaller terrain changes, which will cause uncertain error interference to the IMU inertial navigation positioning estimation of the equipment. To correct this part of the interference, it mainly includes the correction of the two variables of speed and acceleration. The specific formula is as follows:

[0028] ;

[0029] in, ， are the influence coefficients of terrain on velocity and acceleration respectively;

[0030] S22: Based on the motion state equation defined in S21, assuming that the ballast machine is in a uniform acceleration state, the matrices are as follows:

[0031] ;

[0032] ;

[0033] ;

[0034] in, as well as are the influence coefficients of terrain on velocity and acceleration in different directions;

[0035] S23: According to the operating conditions of the ballast machine, its position observation equation is defined as follows:

[0036] ;

[0037] in, Contains GPS location information, To measure noise, Gaussian white noise is generally used; is the observation transfer matrix; the specific expression is as follows:

[0038] ;

[0039] ;

[0040] In this process, GPS data with a lower data release frequency is used as observation data, and IMU data with a higher data release frequency is used as prediction data and put into the improved extended Kalman filter to achieve better positioning effect.

[0041] ;

[0042] ;

[0043] In this process, GPS data with a lower data release frequency is used as observation data, and IMU data with a higher data release frequency is used as prediction data and put into the improved extended Kalman filter to achieve better positioning effect.

[0044] Furthermore, step S3 specifically includes:

[0045] S31: According to the improved strategy of extended Kalman filtering, the specific steps are designed as follows:

[0046] First, the prediction equation includes state prediction and covariance prediction:

[0047] ;

[0048] ;

[0049] in, It is at the moment based on Information status prediction; is the state transfer matrix; State estimation at the moment; is the control input matrix, describing the control input Impact on status; is the process noise impact matrix, used to describe the terrain disturbance Impact on status; is the state prediction error, which is assumed to be zero-mean Gaussian noise; For the moment The state covariance prediction of ; For the moment The state covariance of

[0050] Second, the update equation includes Kalman gain calculation update, state update, and covariance update:

[0051] ;

[0052] ;

[0053] ;

[0054] in, is the Kalman gain; is the process noise covariance matrix, is the observation noise covariance matrix:

[0055] ;

[0056] ;

[0057] in, , , , , , are the process noise variances of each state variable;

[0058] , They are the observation noise variances in different directions of GPS;

[0059] S32: According to the actual situation that the release frequencies of the two sensors are inconsistent, the present invention proposes a fusion strategy suitable for this situation. On the one hand, IMU data information is first selected as prediction data and GPS data information as observation data, and then the two are fused to update the motion state model to correct the covariance to reduce the positioning error; on the other hand, when GPS data is not received, the velocity and acceleration information provided by the IMU is used to integrate and predict the displacement, and when GPS data is received, the GPS information with higher accuracy is used as observation data to correct the prediction data.

[0060] In a third aspect, the present invention provides a device for multi-sensor positioning of a salt field ballast machine, which applies the above-mentioned algorithm based on multi-sensor positioning for the ballast machine, and comprises:

[0061] A data acquisition module, which includes a GPS and an IMU inertial navigation sensor installed on the ballast machine to obtain data information;

[0062] The motion state model building module introduces terrain influence factors to establish the corresponding motion state model according to the operation law of the active ballast machine, and uses the generated IMU data as prediction data;

[0063] The position observation model building module introduces terrain influence factors to establish the corresponding position observation model according to the operation law of the movable ballast machine, and uses the generated GPS data as observation data;

[0064] Data synchronization module, which is used to determine the conversion relationship matrix between GPS and IMU, and synchronize the data of both to the same coordinate system;

[0065] The filter fusion module fuses the synchronized data through an improved extended Kalman filter strategy to obtain the position data of the ballast machine.

[0066] In a fourth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed in a computer, causes the computer to execute the algorithm based on multi-sensor positioning for a movable ballast machine.

[0067] In a fifth aspect, the present invention provides a computing device, including a memory and a processor, wherein the memory stores executable code, and when the processor executes the executable code, the algorithm based on multi-sensor positioning for active ballast machine is implemented.

[0068] The beneficial effects of the present invention are:

[0069] 1. GPS has high global positioning accuracy, but it is not suitable for small-scale positioning and places with high positioning frequency requirements. The present invention uses a GPS and IMU fusion positioning strategy to utilize the high frequency of IMU information release to compensate for the defects of GPS low frequency and positioning drift, and obtains the location of the active ballast machine that is more in line with the actual situation.

[0070] 2. In view of the actual situation of the operating environment of the active ballast machine, the present invention adds the influence of the bottom terrain of the salt field on the IMU positioning effect into the motion state model, improves the positioning filtering effect, and thereby obtains more accurate positioning data.

[0071] 3. The present invention adopts an unmanned active slag machine to reduce labor costs for domestic salt fields, promote the modernization of industrial salt production, improve the production efficiency of the salt drying process, and achieve good economic benefits. It is expected to have huge commercial value after transformation. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 : A flow chart of a multi-sensor positioning method for a salt field active mucking machine.

[0073] Figure 2 : Flowchart of an improved algorithm for multi-sensor fusion positioning. DETAILED DESCRIPTION

[0074] The following is a further detailed description of the specific implementation of the present invention in conjunction with the accompanying drawings and specific implementations. The following implementations or drawings are used to illustrate the present invention, but are not used to limit the scope of the present invention. Example 1

[0075] See also Figure 1 This embodiment provides a multi-sensor positioning method for a salt field movable ballast machine, comprising the following steps:

[0076] S1: Use GPS and IMU to obtain data information, which includes data timestamp and valid data frame. Synchronize the valid data between the two sensors in time according to the data timestamp, that is, obtain the valid data at the same time. Since the release frequency of GPS is slow, after data synchronization, the IMU data between the GPS release frequencies can be obtained to prepare data for subsequent fusion;

[0077] S2: Determine the transformation relationship matrix between the two sensors based on the relative position difference between the two sensors installed on the movable ballast machine and the relative position relationship between the two sensor coordinate systems. ,pass Synchronize the data of both to the same coordinate system;

[0078] S3: Based on the data synchronized by steps S1 and S2 and the actual operating conditions of the ballast machine, the terrain influence factor is introduced into the state transfer equation, that is, The state model has been improved, and more accurate ballast machine position data can be obtained after fusion through the improved extended Kalman filter strategy;

[0079] S4: Feedback the positioning data processed by step S3 to the ballast machine control console.

[0080] Example 2

[0081] See also Figure 2 This embodiment provides an improved strategy for multi-sensor positioning of a salt field active ballast machine, comprising the following steps:

[0082] S1: First, select IMU data information as prediction data and GPS data information as observation data, and then use the two to fuse and update the motion state model to correct the covariance to reduce the positioning error;

[0083] S2: When no GPS data is received, the velocity and acceleration information provided by the IMU is used to integrate and predict the displacement. When GPS data is received, the GPS information with higher accuracy is used as the observation data to correct the predicted data.

[0084] S3: According to the operating conditions of the ballast machine, adjust its motion state equation to improve the filtering effect. The specific motion state equation formula is as follows:

[0085] in, is the state transition matrix. is the control input matrix. is the terrain influence matrix, describing the influence of terrain on the state. is the control input vector (acceleration control). is the terrain influence factor vector (slope or terrain change). is the process noise, generally Gaussian white noise;

[0086] Among them, the state vector Contains the following elements:

[0087] ;

[0088] in, is a position variable, is the speed variable, is the acceleration variable, and the first obtained GPS position information is used as the initial state data of the state vector.

[0089] In the working environment of the ballast machine, the bottom terrain that the chassis track contacts will have larger or smaller terrain changes, which will cause uncertain error interference to the IMU inertial navigation positioning estimation of the equipment. To correct this part of the interference, it mainly includes the correction of the two variables of speed and acceleration. The specific formula is as follows:

[0090] ;

[0091] ， are the influence coefficients of terrain on velocity and acceleration respectively.

[0092] According to the improved strategy of extended Kalman filtering, the specific steps are designed as follows:

[0093] First, the prediction equation includes state prediction and covariance prediction:

[0094] ;

[0095] Second, the update equation includes Kalman gain calculation update, state update, and covariance update: ;

[0096] ;

[0097] ;

[0098] in, is the process noise covariance matrix, is the observation noise covariance matrix:

[0099] ;

[0100] ;

[0101] in, , , , , , are the process noise variances of each state variable; , are the observation noise variances of GPS in different directions.

[0102] Example 3

[0103] A device for multi-sensor positioning of a salt field ballast machine, which applies the above-mentioned algorithm based on multi-sensor positioning for the ballast machine, comprises:

[0104] A data acquisition module, which includes a GPS and an IMU inertial navigation sensor installed on the ballast machine to obtain data information;

[0105] The motion state model building module introduces terrain influence factors to establish the corresponding motion state model according to the operation law of the active ballast machine, and uses the generated IMU data as prediction data;

[0106] The position observation model building module introduces terrain influence factors to establish the corresponding position observation model according to the operation law of the movable ballast machine, and uses the generated GPS data as observation data;

[0107] Data synchronization module, which is used to determine the conversion relationship matrix between GPS and IMU, and synchronize the data of both to the same coordinate system;

[0108] The filter fusion module fuses the synchronized data through an improved extended Kalman filter strategy to obtain the position data of the ballast machine.

[0109] Application Example 1

[0110] This is a description of a method for positioning a ballast machine using GPS and IMU. The algorithm includes multiple steps and equations, namely, obtaining sensor data, determining the state vector of the system, establishing an improved state model and an extended Kalman filter equation for the observation model, proposing a fusion strategy between sensors, and finally obtaining the position information of the ballast machine through an extended Kalman filter. The following are the implementation steps of this method:

[0111] Acquiring sensor data: This step includes synchronizing sensor data in time, acquiring the relative position of the sensor installed on the ballast machine, determining the conversion matrix between the sensor coordinate systems, and obtaining sensor data under time and space synchronization;

[0112] Determine the system state vector and related equations: This step includes determining the state vector and introducing terrain influence factors to improve the state equation;

[0113] Determine the fusion strategy: This step proposes to select IMU data information as prediction data and GPS data information as observation data, and then use the two to fuse and update the motion state model to correct the covariance to reduce the positioning error; when GPS data is not received, the velocity and acceleration information provided by the IMU is used to integrate and predict the displacement. When GPS data is received, the GPS information with higher accuracy is used as observation data to correct the prediction data;

[0114] Obtaining the position information of the movable ballast machine: This step obtains the data filtered by step 3 and feeds it back to the control console to obtain the final position of the movable ballast machine.

[0115] The multi-sensor positioning algorithm for salt field active ballast machine proposed in the present invention improves the problems of large drift error and low release frequency of GPS positioning, and improves the extended Kalman filter fusion strategy, introduces the influence factor of the bottom terrain of the salt field, and is more suitable for the positioning problem of salt field active ballast machine, making certain preparations for the intelligentization and modernization of salt field active ballast machine. Furthermore, the EKF state equation is improved for outdoor salt field working conditions, and a suitable fusion strategy is proposed for sensors.

[0116] Application Example 2

[0117] A computer-readable storage medium stores a computer program, which, when executed in a computer, causes the computer to execute the multi-sensor positioning algorithm for a movable ballast machine.

[0118] Application Example 3

[0119] A computing device comprises a memory and a processor, wherein the memory stores executable codes, and when the processor executes the executable codes, the algorithm based on multi-sensor positioning for a movable ballast machine is implemented.

[0120] The present invention implements all or part of the steps in the above-mentioned embodiment method, which can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, the steps of each of the above-mentioned method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium can at least include: any entity or device that can carry the computer program code to the camera device / terminal device, recording medium, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium. For example, a USB flash drive, a mobile hard disk, a disk or an optical disk.

[0121] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A multi-sensor positioning method for a movable ballast machine, characterized in that: The following steps are involved: S1: Use GPS and IMU inertial navigation sensors to collect data related to the position of the ballast machine and align the coordinate system; S2: According to the operation law of the ballast machine, the terrain influence factor is introduced to establish the corresponding motion state model, and at the same time, the corresponding position observation model is established, which specifically includes the following steps: S21: State transfer equations in the generalized extended Kalman filter On the basis of the above, according to the operating conditions of the active ballast machine, the motion state equation is adjusted to improve the filtering effect. The specific motion state equation formula is as follows: ； in, is the state transition matrix; is the state vector; is the control input matrix; is the terrain influence matrix, describing the influence of terrain on the state; is the control input vector; is the terrain influence factor vector; is the process noise, which is Gaussian white noise; S22: Based on the motion state equation defined in S21, assuming that the ballast machine is in a uniform acceleration state, the matrices are as follows: in, as well as are the influence coefficients of terrain on velocity and acceleration in different directions; S23: According to the operating conditions of the ballast machine, its position observation equation is defined as follows: ； in, Contains GPS location information; To measure noise, Gaussian white noise is generally used; is the observation transfer matrix; the specific expression is as follows: ； ； S3: Determine the improved EKF filtering steps and fusion strategy; S4: Obtain the location information of the ballast machine.

2. The multi-sensor positioning method for a ballast machine according to claim 1, characterized in that: Step S1 specifically includes the following steps: S11: fix the relative positions of the two sensors and start the GPS and IMU sensors; S12: Perform translation and rotation transformations according to the installation positions of the two sensors. Assume that the coordinate systems of IMU and GPS are and , and know the transformation matrix between them , IMU data from Convert to : ； in, It is determined by the relative position and relative posture between sensors.

3. The multi-sensor positioning method for a movable ballast machine according to claim 1, characterized in that: The state vector in step S21 Contains the following elements: ； in, is a position variable; is the speed variable; is the acceleration variable; the first obtained GPS position information is used as the initial state data of the state vector.

4. The multi-sensor positioning method for a movable ballast machine according to claim 1, characterized in that: In the working environment of the ballast machine, the bottom terrain that the chassis track contacts will have larger or smaller terrain changes, which will cause uncertain error interference to the IMU inertial navigation sensor's positioning estimation of the equipment. To correct this part of the interference, it mainly includes the correction of the two variables of speed and acceleration. The specific formula is as follows: ； in, ， are the influence coefficients of terrain on velocity and acceleration respectively.

5. The multi-sensor positioning method for a ballast machine according to claim 1, characterized in that: Step S3 specifically includes the following steps: S31: According to the improved strategy of extended Kalman filtering, the specific steps are designed as follows: First, the prediction equation includes state prediction and covariance prediction: ； in, It is at the moment based on Information status prediction; is the state transfer matrix, State estimation at the moment; is the control input matrix, describing the control input Impact on status; is the process noise impact matrix, used to describe the terrain disturbance Impact on status; is the state prediction error, which is assumed to be zero-mean Gaussian noise; For the moment The state covariance prediction of ; For the moment The state covariance of Second, the update equation includes Kalman gain calculation update, state update, and covariance update: ； in, is the Kalman gain; is the process noise covariance matrix, is the observation noise covariance matrix: ； in, , , , , , are the process noise variances of each state variable; , They are the observation noise variances in different directions of GPS; S32: Based on the actual situation that the release frequencies of the two sensors are inconsistent, a fusion strategy suitable for this situation is designed, as follows: On the one hand, IMU data information is first selected as prediction data and GPS data information as observation data, and then the two are fused to update the motion state model to correct the covariance to reduce the positioning error; On the other hand, when GPS data is not received, the velocity and acceleration information provided by the IMU is used to integrate and predict displacement. When GPS data is received, the more accurate GPS information is used as observation data to correct the predicted data.

6. A multi-sensor positioning device for a salt field movable ballast machine, characterized in that: The application of the multi-sensor positioning method for a movable ballast machine as described in any one of claims 1 to 5 comprises: A data acquisition module, which includes a GPS and an IMU inertial navigation sensor installed on the ballast machine to obtain data information; The motion state model building module introduces terrain influence factors to establish the corresponding motion state model according to the operation law of the active ballast machine, and uses the generated IMU data as prediction data; The position observation model building module introduces terrain influence factors to establish the corresponding position observation model according to the operation law of the movable ballast machine, and uses the generated GPS data as observation data; Data synchronization module, which is used to determine the conversion relationship matrix between GPS and IMU, and synchronize the data of both to the same coordinate system; The filter fusion module fuses the synchronized data through an improved extended Kalman filter strategy to obtain the position data of the ballast machine.

7. A computer-readable storage medium, characterized in that: A computer program is stored thereon, and when the computer program is executed in a computer, the computer is caused to execute the multi-sensor based positioning method for a movable ballast machine as described in any one of claims 1-5.

8. A computing device, characterized in that: It comprises a memory and a processor, wherein the memory stores executable codes, and when the processor executes the executable codes, the method for multi-sensor positioning for a movable ballast machine as described in any one of claims 1 to 5 is implemented.

Images