A seamless positioning method, device and storage medium for an indoor and outdoor mobile robot

Abstract

The present application relates to a seamless positioning method, device and storage medium for an indoor and outdoor mobile robot, the method comprising: S1, constructing a GNSS / IMU-based combined positioning sub-model outdoors and a vision / IMU-based combined positioning sub-model indoors; S2, inputting interaction; S3, model filtering; S4, transition probability correction: correcting a transition probability matrix using a new information vector; S5, model probability updating: calculating a likelihood function of the combined positioning sub-model and updating model probabilities of the combined positioning sub-models; S6, outputting interaction: fusing current time state estimation values and covariances output by the combined positioning sub-models, weighting the model probabilities to obtain system final state estimation values and covariances, and finally inputting the updated model probabilities to step S2 for next iteration. Compared with the prior art, the present application realizes seamless positioning service in indoor and outdoor navigation.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot positioning technology, in particular to a seamless positioning method, device and storage medium for indoor and outdoor mobile robots. BACKGROUND

[0002] At present, mobile robots mostly perform navigation tasks in a single environment, such as indoor guiding tasks or outdoor inspection tasks, etc., and the root cause is that the positioning system can only provide accurate positioning in a fixed scene, and once the scene changes, the positioning system will have problems such as sharp decline in accuracy or even unable to work, for example, the outdoor GPS positioning system will have a large system positioning error in an indoor environment due to satellite signal attenuation; at the junction of indoor and outdoor environments, the positioning system will have problems of accuracy decline in different degrees due to factors such as building obstruction, light change, limited coverage area, etc.

[0003] Therefore, it is urgent to design a seamless positioning method for mobile robots in indoor and outdoor environments. SUMMARY

[0004] The purpose of the present application is to overcome the defects of the prior art and provide a seamless positioning method, device and storage medium for indoor and outdoor mobile robots with high positioning accuracy, which uses a transition probability matrix (TPM) adaptive multi-model interactive (IMM) algorithm based on innovation correction, uses innovation as the divergence degree judgment basis of the current sub-model, and corrects the transition probability matrix, thereby realizing accurate provision of positioning information in the indoor and outdoor transition area and realizing seamless positioning service in indoor and outdoor navigation.

[0005] The purpose of the present application can be achieved by the following technical solutions:

[0006] According to a first aspect of the present application, a seamless positioning method for indoor and outdoor mobile robots is provided, which uses a multi-model interactive algorithm for positioning, and the method comprises the following steps:

[0007] Step S1, respectively constructing a GNSS / IMU-based combined positioning sub-model outdoors and a vision / IMU-based combined positioning sub-model indoors;

[0008] Step S2, input interaction: collecting state estimation values of each combined positioning sub-model at time k-1 and variance Using the mixing probability between the combined positioning sub-models at time k-1 as the weight, the state estimation input value of each combined positioning sub-model at time k-1 is calculated and variance input values

[0009] Step S3, model filtering: inputting the state estimation input values of each combined positioning sub-model obtained by the input interaction and variance input values As the input of each combined positioning sub-model, the state estimation value and variance of each combined positioning sub-model at time k are calculated;

[0010] Step S4, transition probability correction: correcting the transition probability matrix using the innovation vector;

[0011] Step S5, model probability updating: calculating the likelihood function of the combined positioning sub-model and updating the model probability of each combined positioning sub-model at time k;

[0012] Step S6, output interaction: fusing the state estimation value and covariance output by each combined positioning sub-model at the current time, and weighting the model probability updated in step S3 to obtain the final state estimation value of the system and covariance P(k), and finally the updated model probability is transmitted to the interaction stage in step S2 for the next iteration.

[0013] Preferably, the mathematical expression of the combined positioning sub-model is:

[0014] X(k) = A i X(k-1) + W i (k)

[0015] Z(k) = H i X(k) + V i (k)

[0016] In the formula, X(k) is the state at time k, A i is the state transition matrix, W i (k) is the process noise, Z(k) is the output of the combined positioning sub-model at time k, H i is the observation matrix, V i (k) is the observation noise; subscript i is the number of the combined positioning sub-model.

[0017] Preferably, the state estimation value of each combined positioning sub-model j at time k in step S2 is expressed as:

[0018]

[0019] In the formula, is the state estimation value of each combined positioning sub-model i at time k-1, μ ij(k-1) is the model probability between the combined positioning sub-model i and the combined positioning sub-model j after correction at the k-1 moment; and n is the number of combined positioning sub-models.

[0020] Preferably, the model probability μ ij (k-1) of the updated combined positioning sub-model i at the k-1 moment jumps to the combined positioning sub-model j, and the expression is:

[0021]

[0022] In the formula, c is a constant coefficient set, Λ j (k) is the likelihood function of the combined positioning sub-model j, π ij (k-1) is the transition probability between the combined positioning sub-model i and the combined positioning sub-model j at the k-1 moment, μ i (k-1) is the model probability of the combined positioning sub-model i at the k-1 moment, and n is the number of combined positioning sub-models.

[0023] Preferably, the combined positioning sub-model in the step S3 is subjected to filtering operation by using an extended Kalman filtering algorithm.

[0024] Preferably, the step S4 comprises the following sub-steps:

[0025] In the step S41, the innovation square sum is calculated according to the innovation vector of the combined positioning sub-model:

[0026] v s (k) = v T (k) v(k)

[0027] In the formula, v(k) is the innovation vector of the combined positioning sub-model; and the superscript T is the transpose;

[0028] In the step S42, the innovation square sum is normalized to calculate the difference between the normalized values of the innovation square sum:

[0029]

[0030] In the formula, V sj (k) is the normalized value of the innovation square sum of the combined positioning sub-model j, and n is the number of combined positioning sub-models; v j (k) is the innovation vector of the combined positioning sub-model j;

[0031] In the step S43, the difference between the normalized values of the innovation square sum at the k moment and the k-1 moment is calculated:

[0032] ΔV j (k) = V sj (k) - V sj (k-1)

[0033] where ΔV j (k) is the difference between the innovation squared sum normalized value of the combined positioning sub-model j; V sj (k), V sj (k-1) are the innovation squared sum normalized values of the combined positioning sub-model j at k-1 and k respectively;

[0034] Step S44, y = e x is selected as the correction function, and the difference ΔV j (k) between the innovation squared sum normalized values is combined to obtain the final correction factor:

[0035]

[0036] Step S45, the transition probability between the combined positioning sub-model i and the combined positioning sub-model j is corrected by using the correction factor α:

[0037]

[0038] wherein, is the transition probability between the combined positioning sub-model i and the combined positioning sub-model j at k after correction, π ij (k-1) is the normalized value of the transition probability between the combined positioning sub-model i and the combined positioning sub-model j at k-1, α j is the correction factor of the combined positioning sub-model j;

[0039] Step S46, the corrected transition probability is normalized:

[0040]

[0041] wherein, π ij (k) is the normalized value of the transition probability between the combined positioning sub-model i and the combined positioning sub-model j at k after correction, and n is the number of combined positioning sub-models.

[0042] Preferably, the likelihood function of the combined positioning sub-model in the step S5 is expressed as:

[0043]

[0044] wherein, subscript j is the number of the combined positioning sub-model, S j (k) is the covariance matrix, v j is the residual error.

[0045] Preferably, the system final state estimation value and the covariance P(k) in the step S6 are respectively expressed as:

[0046]

[0047] wherein, is the state estimation value of the combined positioning sub-model i at time k, and i (k) is the model probability of the combined positioning sub-model i at time k, is the covariance estimation value of the combined positioning sub-model i at time k, and n is the number of combined positioning sub-models.

[0048] According to a second aspect of the present application, there is provided an electronic device comprising a memory and a processor, the memory having stored thereon a computer program, the processor implementing any of the methods when executing the program.

[0049] According to a third aspect of the present application, there is provided a computer readable storage medium having stored thereon a computer program, the program being executed by a processor to implement any of the methods.

[0050] Compared with the prior art, the present application has the following advantages:

[0051] The present application is based on the multi-model interactive algorithm IMM, and aims at the problem that the elements in the transition probability matrix cannot be dynamically adjusted. The TPM adaptive multi-model interactive IMM algorithm based on innovation correction is used to determine the divergence degree of the current sub-model by using the innovation as the basis, and to correct the transition probability matrix, thereby improving the positioning accuracy and positioning model switching delay of the mobile robot in the indoor-outdoor transition area, that is, the ability to provide seamless positioning service in indoor and outdoor navigation. BRIEF DESCRIPTION OF DRAWINGS

[0052] Figure 1 is a multi-model interactive IMM algorithm flowchart;

[0053] Figure 2 is the overall framework of the TPM adaptive multi-model interactive IMM algorithm based on innovation correction of the present application. DETAILED DESCRIPTION

[0054] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor should fall within the scope of protection of the present application.

[0055] EMBODIMENT

[0056] The present embodiment provides a seamless positioning method for an indoor and outdoor mobile robot, which uses a multi-model interactive algorithm for positioning. The method comprises the following steps:

[0057] Step S1, respectively construct a GNSS / IMU-based combined positioning sub-model outdoors and a vision / IMU-based combined positioning sub-model indoors; wherein the mathematical expression of the combined positioning sub-model is:

[0058] X(k)=A i X(k-1)+W i (k)

[0059] Z(k)=H i X(k)+V i (k)

[0060] In the formula, X(k) is the state at time k, A i is a state transition matrix, W i (k) is process noise, Z(k) is the output of the combined positioning sub-model at time k, H i is an observation matrix, V i (k) is observation noise; subscript i is the number of the combined positioning sub-model.

[0061] As Figure 2 shown, for the vision / IMU combined positioning sub-model, the system state variable is generally determined by the IMU, and in this embodiment, the position, velocity, four-element, accelerometer zero offset and gyroscope zero offset are selected as the system state variable of the IMU, i.e., wherein G represents the world coordinate system, I represents the IMU coordinate system, G p I is the representation of the IMU in the world coordinate system, I q G is the unit quaternion of the world coordinate system to the IMU coordinate system, G v is the velocity of the IMU in the world system, b a is the accelerometer zero offset of the IMU, b g is the gyroscope zero offset of the IMU, G p′ I is the position of the previous key frame camera in the world system, I q′ G is the quaternion of the world coordinate system to the IMU coordinate system of the previous key frame.

[0062] Step S2, input interaction: collect the state estimation value and variance of each combined positioning sub-model at time k-1 and variance input value

[0063] wherein the state estimation value of each combined positioning sub-model j at time k is The expression is:

[0064]

[0065] In the formula, n is the number of combined positioning sub-models; is the state estimation value of each combined positioning sub-model i at time k-1, μ ij (k-1) is the model probability between the combined positioning sub-model i and the combined positioning sub-model j updated at time k-1, and the expression is:

[0066]

[0067] In the formula, c is a constant coefficient set, Λ j (k) is the likelihood function of the combined positioning sub-model j, π ij (k-1) is the transition probability between the combined positioning sub-model i and the combined positioning sub-model j at time k-1, μ i (k-1) is the model probability of the combined positioning sub-model i at time k-1, and n is the number of combined positioning sub-models.

[0068] Step S3, model filtering: input the state estimation input value of each combined positioning sub-model obtained through the input interaction and the variance input value as the input of each combined positioning sub-model, calculate the state estimation value and the variance of each combined positioning sub-model at time k; in this embodiment, the combined positioning sub-model is filtered by using the extended Kalman filtering algorithm.

[0069] Step S4, transition probability correction: correct the transition probability matrix by using the innovation vector, which specifically includes the following sub-steps:

[0070] Step S41, calculate the innovation square sum according to the innovation vector of the combined positioning sub-model:

[0071] v s (k) = v T (k) v(k)

[0072] In the formula, v(k) is the innovation vector of the combined positioning sub-model; the superscript T is the transpose;

[0073] Step S42, normalize the innovation square sum, and calculate the difference between the normalized values of the innovation square sum:

[0074]

[0075] In the formula, V sj (k) is the normalized value of the innovation square sum of the combined positioning sub-model j, and n is the number of combined positioning sub-models; v j(k) is the innovation vector of the combined positioning sub-model j;

[0076] Step S43, calculate the difference between the innovation squared sum normalized value at k time and the innovation squared sum normalized value at k-1 time:

[0077] ΔV j (k) = V sj (k) - V sj (k-1)

[0078] In the formula, ΔV j (k) is the difference between the innovation squared sum normalized value of the combined positioning sub-model j; V sj (k) and V sj (k-1) are respectively the innovation squared sum normalized value of the combined positioning sub-model j at k-1 time and k time.

[0079] Step S44, since the true physical meaning of each element in the state probability model TPM is the transition probability between models, which has non-negativity, the state probability model TPM after being corrected by the correction factor must also be non-negative, and when ΔV j (k) = 0, corresponding to the case that the model does not jump, at this time the correction factor should be 1, and in the embodiment, y = e x is selected as the correction function, combined with the difference ΔV j (k) between the innovation squared sum normalized values, to obtain the final correction factor:

[0080]

[0081] Step S45, use the correction factor α to correct the transition probability between the combined positioning sub-model i and the combined positioning sub-model j:

[0082]

[0083] In the formula, is the transition probability between the combined positioning sub-model i and the combined positioning sub-model j after correction at k time, π ij (k-1) is the transition probability between the combined positioning sub-model i and the combined positioning sub-model j at k-1 time, α j is the correction factor of the combined positioning sub-model j;

[0084] Step S46, in the Markov process, the sum of the probabilities of all combined positioning sub-models jumping to a certain combined positioning sub-model is one, that is, the transition probability has normalization, so the normalized processing is performed on the corrected transition probability:

[0085]

[0086] In the formula, π ij(k) is a normalized value of the transition probability between the combined positioning sub-model i and the combined positioning sub-model j after the correction at the k moment, and n is the number of the combined positioning sub-models.

[0087] Step S5, model probability updating: the likelihood function of the combined positioning sub-model is calculated, and the model probability of each combined positioning sub-model at the k moment is updated; wherein the likelihood function of the combined positioning sub-model is expressed as:

[0088]

[0089] In the formula, the subscript j is the number of the combined positioning sub-model, S j (k) is a covariance matrix, v j is a residual error.

[0090] Step S6, output interaction: the current moment state estimation value and the covariance output by each combined positioning sub-model are fused, the model probability updated in step S3 is weighted to obtain the system final state estimation value and the covariance P(k), and finally the updated model probability is input into the interaction stage in step S2 for the next iteration.

[0091] Wherein the system final state estimation value and the covariance P(k) are respectively expressed as:

[0092]

[0093]

[0094] In the formula, is the state estimation value of the combined positioning sub-model i at the k moment, μ i (k) is the model probability of the combined positioning sub-model i at the k moment, is the covariance estimation value of the combined positioning sub-model i at the k moment, and n is the number of the combined positioning sub-models.

[0095] The electronic device of the present application includes a central processing unit (CPU) which can perform various appropriate actions and processes according to computer program instructions stored in a read-only memory (ROM) or computer program instructions loaded from a storage unit into a random access memory (RAM). In the RAM, various programs and data required for device operation can also be stored. The CPU, the ROM, and the RAM are connected to each other through a bus. An input / output (I / O) interface is also connected to the bus.

[0096] A number of components in the device are connected to the I / O interface, including: input units, such as a keyboard, a mouse, etc.; output units, such as various types of displays, speakers, etc.; storage units, such as a magnetic disk, an optical disk, etc.; and communication units, such as a network card, a modem, a wireless communication transceiver, etc. The communication units allow the device to exchange TP information / data with other devices through a computer network, such as the Internet, and / or various telecommunication networks.

[0097] The processing unit performs various methods and processes described above, such as methods S1-S6. For example, in some embodiments, methods S1-S6 can be implemented as a computer software program tangibly embodied in a machine-readable medium, such as the storage unit. In some embodiments, part or all of the computer program can be loaded and / or installed on the device via the ROM and / or the communication unit. When the computer program is loaded into the RAM and executed by the CPU, one or more steps of methods S1-S6 described above can be performed. Alternatively, in other embodiments, the CPU can be configured to perform methods S1-S6 by any other suitable means, such as by means of firmware.

[0098] The functions described above in this document can be performed, at least in part, by one or more hardware logic components. For example, and without limitation, illustrative types of hardware logic components that can be used include Field-programmable Gate Arrays (FPGAs), Application-specific Integrated Circuits (ASICs), Application-specific Standard Products (ASSPs), System-on-a-chip systems (SOCs), Complex Programmable Logic Devices (CPLDs), etc.

[0099] Program code for carrying out methods of the present application can be written in any combination of one or more programming languages. This program code can be provided to a processor or controller of a general purpose computer, special purpose computer, or other programmable data processing apparatus, such that the program code, when executed by the processor or controller, causes the functions / operations specified in the flow charts and / or block diagrams to be implemented. The program code can execute entirely on a machine, partly on the machine, as a stand-alone software package, partly on the machine and partly on a remote machine or entirely on the remote machine or server.

[0100] In the context of the present application, a machine-readable medium can be a tangible medium that can contain or store a program for use by or in connection with an instruction execution system, apparatus, or device. The machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. Machine-readable storage medium can include, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples of machine-readable storage medium would include one or more lines of electrical wire, portable computer diskette, hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or Flash memory), optical fiber, portable compact disc read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination of the foregoing.

[0101] The above description is only specific embodiments of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of various equivalent modifications or replacements within the technical scope disclosed by the present application, and these modifications or replacements should be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A seamless positioning method for indoor and outdoor mobile robots, characterized in that, The localization method employs a multi-model interactive algorithm, which includes the following steps: Step S1: Construct a combined positioning sub-model based on GNSS / IMU outdoors and a combined positioning sub-model based on vision / IMU indoors; Step S2, Input Interaction: Data Collection State estimates of each combined positioning sub-model at time 1 and variance ,by The mixture probability among the time-combined localization sub-models is used as the weight to calculate the result. State estimation input values ​​of each combined localization sub-model at time 1 and variance input values ; Step S3, Model Filtering: Filter the state estimates of each combined localization sub-model obtained from the input interaction. and variance input values As input to each combined localization sub-model, calculate The state estimates and variances of each combined positioning sub-model at time points; Step S4, Transition Probability Correction: Correct the transition probability matrix using the innovation vector; Step S5, Model Probability Update: Calculate the likelihood function of the combined localization sub-model and update it. The model probability of each combined positioning sub-model at time step; Step S6, Output Interaction: The current state estimates and covariances output by each combined positioning sub-model are fused, and the model probabilities obtained in Step S3 are weighted to obtain the final system state estimate. Covariance Finally, the updated model probability is fed into the interaction stage in step S2 for the next iteration; The mathematical expression for the combined localization sub-model is: In the formula, for The state at any given moment, Here is the state transition matrix. For process noise, for The output of the combined localization sub-model at time step. For the observation matrix, For observing noise; subscript The numbering of the combined positioning sub-model; Step S4 includes the following sub-steps: Step S41: Calculate the sum of squared innovations based on the innovation vector of the combined localization sub-model. In the formula, For the information vector of the combined localization sub-model; superscript For transpose; Step S42: Normalize the sum of squares of the new ideas and calculate the difference between the normalized values ​​of the sum of squares of the new ideas: In the formula, For combined localization sub-model The normalized value of the sum of squares of the new information, The number of combined localization sub-models; For combined localization sub-model The new information vector; Step S43, Calculation Time and The difference between the sum of squares and normalized values ​​of the new information at time: In the formula, For combined localization sub-model The difference between the sum of squares and the normalized value of the new information; They are respectively , Moment-combined positioning sub-model The normalized value of the sum of squares of the new information; Step S44, Select As a correction function, it combines the difference between the sum of squared innovations and the normalized value. The final correction factor is obtained as follows: Step S45: Using the correction factor For combined localization sub-model Combined localization sub-model The transition probabilities between them are corrected: In the formula, for Timing-corrected combined positioning sub-model Combined localization sub-model The transition probability between them for Timing-corrected combined positioning sub-model Combined localization sub-model The normalized value of the transition probability between them. For combined localization sub-model Correction factor; Step S46: Normalize the corrected transition probabilities: In the formula, for Timing-corrected combined positioning sub-model Combined localization sub-model The normalized value of the transition probability between them. This represents the number of combined localization sub-models.

2. The seamless positioning method for an indoor / outdoor mobile robot according to claim 1, characterized in that, In step S2 Time-based combined positioning sub-model The state estimate is expressed as: In the formula, for Time-based combined positioning sub-model State estimates, for Constantly updated combinatorial localization sub-model Combined localization sub-model The model probabilities between; This represents the number of combined localization sub-models.

3. The seamless positioning method for an indoor / outdoor mobile robot according to claim 2, characterized in that, The Constantly updated combinatorial localization sub-model Jump to combined positioning sub-model Model probability The expression is: In the formula, For the set constant coefficient, For combined localization sub-model The likelihood function, for Time-based combined localization sub-model Combined localization sub-model The transition probability between them for Moment-combined positioning sub-model The model probability, This represents the number of combined localization sub-models.

4. The seamless positioning method for an indoor / outdoor mobile robot according to claim 1, characterized in that, The combined localization sub-model in step S3 is filtered using the extended Kalman filter algorithm.

5. The seamless positioning method for an indoor / outdoor mobile robot according to claim 1, characterized in that, The likelihood function of the combined localization sub-model in step S5 is expressed as follows: In the formula, the subscript This refers to the numbering of the combined positioning sub-model. Let covariance matrix be the variance matrix. This is the residual.

6. The seamless positioning method for an indoor / outdoor mobile robot according to claim 1, characterized in that, The system final state estimate in step S6 Covariance The expressions are as follows: In the formula, for Moment-combined positioning sub-model State estimates, for Moment-combined positioning sub-model The model probability, for Moment-combined positioning sub-model The covariance estimate, This represents the number of combined localization sub-models.

7. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the program, it implements the method as described in any one of claims 1 to 5.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the method as described in any one of claims 1 to 5.

Images