CN115615433A - A Hybrid Positioning Method and System Based on Extended Kalman and R-T-S Smoothing Algorithm - Google Patents

Abstract

The invention discloses a hybrid positioning method and a system based on an extended Kalman and R-T-S smoothing algorithm, which comprises the following steps: obtaining the prediction of the position of the robot at each moment, and judging the position change conditions of the robot in the X direction and the Y direction; and for the direction of which the position change does not exceed the set threshold, smoothing the position of the robot in the direction by utilizing an R-T-S smoothing algorithm, and averaging the smoothed positions to obtain the optimal position estimation of the robot in the direction. The invention respectively judges the position change conditions of the robot in the X direction and the Y direction, smoothes the direction of which the position change is less than the set threshold value, can effectively improve the navigation estimation precision of the local direction in a static state, and further improves the whole navigation precision.

Description

A hybrid positioning method based on extended Kalman and R-T-S smoothing algorithm and its system

technical field

The invention relates to the technical field of combined positioning in complex environments, in particular to a hybrid positioning method and system based on extended Kalman and R-T-S smoothing algorithms.

Background technique

The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

Mobile robot navigation and positioning, as the basis for mobile robots to provide high-quality services to human beings, is attracting more and more attention from scholars from all over the world, and has gradually become a research hotspot in this field. However, with the expansion of the application range of mobile robots, the navigation environment they face becomes more and more complex. Especially in the indoor environment, the indoor layout of the building, building materials, and even the size of the space will affect the navigation signal, thereby affecting the positioning accuracy. At the same time, the relatively small platform of mobile robots for indoor environments makes it impossible to install some high-precision navigation equipment. Although the accuracy of small navigation equipment has been improved with the miniaturization of navigation devices in recent years, there is still a gap in its performance compared with traditional large-scale high-precision navigation devices. In the indoor environment, how to use the limited information obtained to eliminate the influence of the indoor complex navigation environment on the accuracy and real-time performance of the mobile robot's navigation information acquisition, and to ensure the continuous stability of the mobile robot's navigation accuracy in the indoor environment is an important scientific issue. Theoretical significance and practical application value.

Among the existing positioning methods, the Global Navigation Satellite System (GNSS) is the most commonly used method. Although GNSS can provide continuous and stable position information with high precision, its disadvantages of being easily affected by external environments such as electromagnetic interference and occlusion limit its application range, especially in some closed and complex environments such as indoors and underground roadways. It is heavily shaded and cannot perform effective work. In recent years, UWB (Ultra Wideband) has shown great potential in the field of short-distance local positioning due to its high positioning accuracy in complex environments. Scholars proposed to apply UWB-based target tracking to pedestrian navigation in GNSS failure environment. Although this method can achieve indoor positioning, due to the complex and changeable indoor environment, UWB signals are very susceptible to interference, resulting in a decrease in positioning accuracy or even loss of lock; at the same time, because the communication technology used by UWB is usually short-distance wireless communication technology, Therefore, if you want to complete large-scale indoor target tracking and positioning, you need a large number of network nodes to complete it together, which will inevitably introduce a series of problems such as network organizational structure optimization design, multi-node multi-cluster network collaborative communication, etc. Therefore, at this stage, UWB-based target tracking still faces many challenges in the field of indoor navigation.

In terms of navigation model, the loose combination navigation model is currently widely used in the field of indoor pedestrian combination navigation. This model has the advantage of being easy to implement, but it should be pointed out that the realization of this model requires multiple technologies involved in integrated navigation to be able to complete navigation and positioning independently. For example, UWB devices are required to provide pedestrian navigation information, which requires that the environment where the target pedestrian is located must be able to obtain at least three reference node information, which greatly reduces the application range of the integrated navigation model. The sub-technology completes the positioning independently, but also introduces new errors, which is not conducive to the improvement of the accuracy of the integrated navigation technology. In order to overcome this problem, scholars proposed to apply the tight combination model to the field of indoor pedestrian navigation. The tight combination model directly applies the original sensor data of the sub-techniques involved in the combined navigation to the final navigation information calculation, reducing the sub-techniques’ self-discipline. Solve the risk of introducing new errors and improve the accuracy of integrated navigation. However, it should be pointed out that the existing tight integrated navigation models all use a centralized model. This method makes the system poor in fault tolerance and is not conducive to increasingly accurate and complex combinations. Navigate the model.

The accuracy of existing UWB positioning technology depends heavily on the position accuracy of UWB reference nodes, but it is difficult to achieve in practical applications; on the other hand, the UWB positioning algorithm that only relies on distance is not accurate to the accuracy of UWB reference nodes.

In the prior art, the smoothing algorithm is used to smooth the position of the UWB reference node estimated by the extended Kalman filter, and finally the optimal estimation of the mobile robot and the UWB reference node is obtained; this method can solve the position accuracy of the UWB reference node The problem is not high, but this method only predicts the stationary reference node. Since the target carrier is usually movable in the actual application process, the effect on navigation is relatively small.

Contents of the invention

In order to solve the above problems, the present invention proposes a hybrid positioning method and system based on extended Kalman and R-T-S smoothing algorithms, which can effectively improve the accuracy of navigation estimation when the local direction is in a static state, and then improve the accuracy of the entire navigation.

In some embodiments, the following technical solutions are adopted:

A hybrid positioning method based on extended Kalman and R-T-S smoothing algorithm, including:

Obtain an estimate of the robot's position at each moment, and judge the position change of the robot in the X and Y directions;

For the direction where the position change does not exceed the set threshold, the R-T-S smoothing algorithm is used to smooth the position of the robot in this direction, and the smoothed position is averaged to obtain the optimal position estimate of the robot in this direction.

In other embodiments, the following technical solutions are adopted:

A hybrid positioning system based on extended Kalman and R-T-S smoothing algorithm, including:

The position estimation module is used to obtain the estimation of the position of the robot at each moment, and judge the position change of the robot in the X direction and the Y direction;

The position smoothing module is used to smooth the position of the robot in this direction by using the R-T-S smoothing algorithm for the direction where the position change does not exceed the set threshold, and average the smoothed positions to obtain the optimal position estimate of the robot in this direction .

In other embodiments, the following technical solutions are adopted:

A terminal device, which includes a processor and a computer-readable storage medium, the processor is used to implement instructions; the computer-readable storage medium is used to store multiple instructions, and the instructions are suitable for being loaded by the processor and executing the above-mentioned extension-based Hybrid localization method of Kalman and R-T-S smoothing algorithms.

In other embodiments, the following technical solutions are adopted:

A computer-readable storage medium, in which a plurality of instructions are stored, and the instructions are suitable for being loaded by a processor of a terminal device and executing the above-mentioned hybrid positioning method based on extended Kalman and R-T-S smoothing algorithms.

Compared with prior art, the beneficial effect of the present invention is:

The present invention respectively judges the position changes of the robot in the X direction and the Y direction, and smooths the directions where the position changes are smaller than the set threshold, which can effectively improve the accuracy of navigation estimation when the local direction is in a static state, and further improve the accuracy of the entire navigation. precision.

Description of drawings

FIG. 1 is a schematic structural diagram of a robot positioning system in Embodiment 1 of the present invention;

FIG. 2 is a schematic diagram of a hybrid positioning method based on extended Kalman and R-T-S smoothing algorithms in Embodiment 1 of the present invention.

detailed description

It should be pointed out that the following detailed description is exemplary and intended to provide further explanation to the present application. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

It should be noted that the terminology used here is only for describing specific implementations, and is not intended to limit the exemplary implementations according to the present application. As used herein, unless the context clearly dictates otherwise, the singular is intended to include the plural, and it should also be understood that when the terms "comprising" and/or "comprising" are used in this specification, they mean There are features, steps, operations, means, components and/or combinations thereof.

Embodiment one

In one or more embodiments, a hybrid positioning method based on extended Kalman and R-T-S smoothing algorithm is disclosed, the positioning method is based on the robot positioning system as shown in Figure 1; specifically includes: UWB, electronic compass and code wheel are all It is fixed on the mobile robot and connected with the data processing unit through the serial port RS232.

in,

UWB: used to measure the distance between the mobile robot and the UWB reference node;

Electronic compass: used to measure the heading angle of the mobile robot;

Code disc: used to measure the speed of the mobile robot;

Data processing unit: used for data fusion of collected sensor data.

Based on the above robot positioning system, with reference to Fig. 2, a kind of hybrid positioning method based on extended Kalman and R-T-S smoothing algorithm disclosed in this embodiment, the specific process is as follows:

(1) Obtain the estimated position of the robot at each moment through the extended Kalman filter, and judge the position change of the robot in the X direction and the Y direction;

(2) For the direction where the position change does not exceed the set threshold, use the R-T-S smoothing algorithm to smooth the position of the robot in this direction, and average the smoothed positions to obtain the optimal position estimate of the robot in this direction.

In this embodiment, the smoothing position is determined according to the moment when the robot is relatively stationary in a certain local direction, therefore, the smoothing position is not fixed.

Specifically, the estimation of the position of the robot at each moment is obtained, and the specific process is as follows:

The position, velocity, heading angle and the position of the UWB reference node in the x and y directions are used as the state vector of the extended Kalman filter; the distance between the robot measured by UWB and the UWB reference node is used as the observation vector of the extended Kalman filter Perform data fusion to obtain the optimal position estimation of the mobile robot.

Among them, the state equation of the extended Kalman filter is:

为k时刻的航向；T为采样周期，ω_n为k时刻的系统噪声，其协方差阵为Q。Among them, (x _k , y _k ) is the position of the mobile robot in the x and y directions at time k; ( _xi , y _i ), i∈[1,2,...,g] is the UWB reference node k at time The position in the x and y directions; V _k is the velocity at k moment;

is the heading at time k; T is the sampling period, ω _n is the system noise at time k, and its covariance matrix is Q.

The observation equation of the extended Kalman filter is:

Among them, d _i is the distance between the robot and the i-th reference node measured by UWB at time n; ν _k is the observation noise, and its covariance matrix is R.

The iterative equation of the extended Kalman filter is:

X _k|k-1 ＝A _k-1 X _k-1 +ω _k-1

P _k|k-1 ＝A _k-1 P _k-1 (A _k-1 ) ^T +Q

K _k ＝P _k|k-1 (H _k ) ^T (H _k P _k|k-1 (H _k ) ^T +R _k ) ^-1

X _k ＝X _k|k-1 +K _k [Y _k -h(X _k|k-1 )]

P _k ＝(IK _k H _k )P _k|k-1

in,

Judging the position changes of the robot in the X and Y directions, including:

According to the estimation of the position of the robot at each moment, the difference between the estimated position of the robot in the X direction at the current moment and the position in the X direction at the previous moment is made to obtain the position change of the robot in the X direction;

Make a difference between the estimated position of the robot in the Y direction at the current moment and the position in the Y direction at the previous moment to obtain the position change of the robot in the Y direction.

When the position change in a certain direction does not exceed the set threshold, it can be considered that the robot is in a static state in this direction; at this time, use the R-T-S smoothing algorithm to smooth the position of the robot in this direction, and average the smoothed position , to obtain the optimal position estimate of the robot in this direction at the current moment.

For the direction where the position change exceeds the set threshold, the robot is considered to be in motion in this direction, and the motion state is not smoothed. The result of the Kalman filter is directly used as the estimation of the position of the mobile robot at the current moment.

In this embodiment, the process of smoothing the position of the robot in this direction using the R-T-S smoothing algorithm specifically includes:

为平滑的误差增益、P_k|k为k时刻的误差矩阵、

为系统矩阵的转置、P_k+1|k为由k-1到k时刻的误差矩阵、

为k时刻的平滑状态预估、

为k时刻的状态预估、

为k+1时刻的平滑状态预估、

为由k-1到k时刻的平滑状态预估、

为k时刻的平滑误差矩阵、

为k+1时刻的平滑误差矩阵、P_k|k-1为由k-1到k时刻的误差矩阵。in,

is the smooth error gain, P _k|k is the error matrix at time k,

is the transposition of the system matrix, P _k+1|k is the error matrix from k-1 to k time,

is the smooth state estimation at time k,

is the state estimation at time k,

is the smooth state estimate at time k+1,

For the smooth state estimation from k-1 to k time,

is the smooth error matrix at time k,

is the smooth error matrix at time k+1, and P _k|k-1 is the error matrix from time k-1 to k.

Embodiment two

In one or more embodiments, a hybrid positioning system based on extended Kalman and R-T-S smoothing algorithm is disclosed, including:

The position estimation module is used to obtain the estimation of the position of the robot at each moment, and judge the position change of the robot in the X direction and the Y direction;

The position smoothing module is used to smooth the position of the robot in this direction by using the R-T-S smoothing algorithm for the direction where the position change does not exceed the set threshold, and average the smoothed positions to obtain the optimal position estimate of the robot in this direction .

It should be noted that the specific implementation process of the above modules has been described in Embodiment 1, and will not be described in detail here.

Embodiment three

In one or more embodiments, a terminal device is disclosed, including a server, the server includes a memory, a processor, and a computer program stored on the memory and operable on the processor, and the processor executes the The program realizes the hybrid positioning method based on the extended Kalman and R-T-S smoothing algorithm in the first embodiment. For the sake of brevity, details are not repeated here.

It should be understood that in this embodiment, the processor can be a central processing unit CPU, and the processor can also be other general-purpose processors, digital signal processors DSP, application specific integrated circuits ASIC, off-the-shelf programmable gate array FPGA or other programmable logic devices , discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor, or the processor may be any conventional processor, or the like.

The memory may include read-only memory and random access memory, and provide instructions and data to the processor, and a part of the memory may also include non-volatile random access memory. For example, the memory may also store device type information.

In the implementation process, each step of the above method can be completed by an integrated logic circuit of hardware in a processor or an instruction in the form of software.

The hybrid positioning method based on the extended Kalman and R-T-S smoothing algorithm in Embodiment 1 can be directly implemented by a hardware processor, or implemented by a combination of hardware and software modules in the processor. The software module may be located in a mature storage medium in the field such as random access memory, flash memory, read-only memory, programmable read-only memory or electrically erasable programmable memory, register. The storage medium is located in the memory, and the processor reads the information in the memory, and completes the steps of the above method in combination with its hardware. To avoid repetition, no detailed description is given here.

Although the specific implementation of the present invention has been described above in conjunction with the accompanying drawings, it does not limit the protection scope of the present invention. Those skilled in the art should understand that on the basis of the technical solution of the present invention, those skilled in the art do not need to pay creative work Various modifications or variations that can be made are still within the protection scope of the present invention.

Claims (10)

1. A hybrid positioning method based on an extended Kalman and R-T-S smoothing algorithm is characterized by comprising the following steps:

obtaining the pre-estimation of the position of the robot at each moment, and judging the position change conditions of the robot in the X direction and the Y direction;

and for the direction of which the position change does not exceed the set threshold, smoothing the position of the robot in the direction by utilizing an R-T-S smoothing algorithm, and averaging the smoothed positions to obtain the optimal position estimation of the robot in the direction.

2. The hybrid localization method based on extended Kalman and R-T-S smoothing algorithm as claimed in claim 1, wherein for the direction where the position change exceeds the set threshold, the obtained estimated value of the robot position at the current time is directly used as the optimal location estimate of the robot in the direction.

3. The hybrid positioning method based on the extended Kalman and R-T-S smoothing algorithm as claimed in claim 1, wherein the obtaining of the prediction of the robot position at each moment comprises the following specific processes:

taking the position, the speed and the course angle of the x direction and the y direction and the position of a UWB reference node as state vectors of the extended Kalman filter;

and performing data fusion by taking the distance between the UWB measured robot and the UWB reference node as an observation vector of the extended Kalman filter to obtain the prediction of the position of the robot.

4. The hybrid localization method based on extended Kalman and R-T-S smoothing algorithm according to claim 3, wherein the state equation of the extended Kalman filter is:

wherein (x) _k ,y _k ) The position of the mobile robot k in the x and y directions at the moment; (x) _i ,y _i ),i∈[1,2,...,g]The position of a UWB reference node k in x and y directions at the moment; v _k Is the velocity at time k;

the course at the moment k is taken as the course; t is the sampling period, omega _n The covariance matrix is Q, which is the system noise at time k.

5. The hybrid localization method based on extended Kalman and R-T-S smoothing algorithm as claimed in claim 3, wherein the observation equation of the extended Kalman filter is:

wherein d is _i,k Distance between the robot and the ith reference node is measured by UWB at the moment k; (x) _k ,y _k ) Is the robot position at time k; v is _k To observe noise, the covariance matrix is R.

6. The hybrid positioning method based on the extended Kalman and R-T-S smoothing algorithm as claimed in claim 1, wherein the position change condition of the robot in X direction and Y direction is judged by the following specific processes:

according to the estimation of the position of the robot at each moment, the difference is made between the position of the robot at the current moment in the X direction and the position of the robot at the previous moment in the X direction, so that the position change condition of the robot in the X direction is obtained;

and performing difference between the estimated position of the robot in the Y direction at the current moment and the estimated position of the robot in the Y direction at the previous moment to obtain the position change condition of the robot in the Y direction.

7. The hybrid positioning method based on the extended Kalman and the R-T-S smoothing algorithm as claimed in claim 1, wherein the R-T-S smoothing algorithm is used to smooth the position of the robot in the direction, and the specific process is as follows:

wherein,

error gain, P, for smoothing _k|k Is the error matrix at time k,

Is a transposition of the system matrix, P _k+1|k Is an error matrix from k-1 to k times,

Estimation of the smooth state at time k,

Estimation of the state at time k,

Estimated for the smooth state at time k +1,

Estimated for the smooth state from time k-1 to k,

Is a smoothed error matrix at time k,

Smoothed error matrix, P, for time k +1 _k|k-1 Is the error matrix from time k-1 to time k.

8. A hybrid positioning system based on an extended Kalman and R-T-S smoothing algorithm, comprising:

the position estimation module is used for acquiring estimation of the position of the robot at each moment and judging the position change conditions of the robot in the X direction and the Y direction;

and the position smoothing module is used for smoothing the position of the robot in the direction in which the position change does not exceed the set threshold value by utilizing an R-T-S smoothing algorithm, and averaging the smoothed positions to obtain the optimal position estimation of the robot in the direction.

9. A terminal device comprising a processor and a computer-readable storage medium, the processor being configured to implement instructions; a computer readable storage medium for storing a plurality of instructions adapted to be loaded by a processor and to perform the hybrid localization method based on extended Kalman and R-T-S smoothing algorithms of any one of claims 1 to 7.

10. A computer readable storage medium having stored therein a plurality of instructions, characterized in that said instructions are adapted to be loaded by a processor of a terminal device and to perform the hybrid positioning method based on an extended Kalman and R-T-S smoothing algorithm according to any one of claims 1 to 7.

Images