WO2021082571A1

WO2021082571A1 - Robot tracking method, device and equipment and computer readable storage medium

Info

Publication number: WO2021082571A1
Application number: PCT/CN2020/105997
Authority: WO
Inventors: 郑鑫江; 李铭浩; 樊锅旭; 赵井全
Original assignee: 苏宁云计算有限公司
Priority date: 2019-10-29
Filing date: 2020-07-30
Publication date: 2021-05-06
Also published as: CN110849369B; CA3158929A1; CN110849369A

Abstract

A robot tracking method, device and equipment and a computer readable storage medium, belonging to the field of intelligent robot control. The method comprises: at each moment of tracking, obtaining observation data obtained by observing a robot by at least two ultrasonic arrays (101); and estimating the motion state of the robot of each moment by using a preset augmented IMM-EKF algorithm, specifically including: respectively obtaining, by means of a number m of augmented EKF filters matched with m motion models corresponding to m motion states at k moment, the corresponding state estimation of the robot under each of the motion models at the k moment to obtain m state estimations, and performing weighting calculation on the m state estimations to obtain a state estimation result of the robot at the k moment, wherein each moment is represented by k moment and k and m are integers greater than zero (102). The intelligent robot can be stably and effectively tracked when the motion state of the robot is unknown and changeable, the phenomenon of mistracking or tracking loss is reduced, and the method is suitable for application scenarios with multiple ultrasonic arrays.

Description

Robot tracking method, device, equipment and computer readable storage medium

Technical field

The invention relates to the field of intelligent robot manipulation, and in particular to a robot tracking method, device, equipment and computer readable storage medium.

Background technique

At present, intelligent robots have been used in various fields such as ocean exploration, security, and medical treatment, which have brought great convenience to the development of science and technology and people's lives. Therefore, it is necessary to track the robot in real time. However, when smart robots are working underwater or indoors, satellite positioning cannot be used. The visual navigation method has the advantages of complete information acquisition and wide detection range. It occupies an important position in robot navigation. The disadvantage is that the visual image processing time is long and the real-time performance is poor. Therefore, scholars in the industry have carried out research on the positioning of mobile robots based on radio frequency identification (RFID) technology. It should be pointed out that the accuracy of the above-mentioned wireless positioning technology is meter-level, which cannot meet the requirements of high-precision navigation and positioning of indoor robots. Intelligent robots are used to transmit ultrasonic waves. . The acoustic signals emitted by the robot are received by a multi-ultrasonic array, the observation position of the robot is obtained through processing, and finally the position estimation of the robot is filtered by the tracking algorithm. This method satisfies the real-time performance and has high tracking accuracy.

Common tracking algorithms mainly include extended Kalman filter, unscented Kalman filter, particle filter, etc. These algorithms have a good tracking effect when the target motion model is known and the motion state is basically unchanged. However, in the actual target tracking process, the motion model is often unknown, and the motion state of the robot often changes. The tracking effect of the above algorithm will decrease or even diverge. Compared with the single ultrasonic receiving array, the multi-array tracking system can obtain more information about the target's motion state, and use the corresponding fusion algorithm to improve the tracking accuracy.

Summary of the invention

In order to solve the problems of the prior art, the embodiments of the present invention provide a robot tracking method and device, which improve the tracking accuracy when tracking a robot indoors. The tracking error is small and the amount of calculation is relatively low, thereby realizing that the state of the intelligent robot is unknown and many It can also track it stably and effectively under changing circumstances, reducing the occurrence of mistracking or tracking loss. The technical solution is as follows:

In one aspect, a robot tracking method is provided, and the method includes:

Obtain the observation data of the robot by at least two ultrasonic arrays at each moment of tracking;

The preset extended-dimensional IMM-EKF algorithm is used to estimate the motion state of the robot at each time. Through m extended-dimensional EKF filters that match the m motion models corresponding to the m motion states at time k, each type at time k is obtained. The state of the robot under the motion model is estimated, m states are obtained, and the m states are weighted to obtain the robot state estimation result at time k, where each time is represented by k time, and k and m are both greater than 0 Integer.

Further, at each moment of tracking, the observation data of at least two ultrasonic arrays on the robot is acquired, including:

At time k, obtain the observation data of at least two ultrasonic arrays on the robot

Where k and n are both integers greater than 0,

Both are vectors of the angle and distance data of the robot measured by the at least two ultrasonic arrays.

Further, the preset extended-dimensional IMM-EKF algorithm is used to estimate the motion state of the robot at each time, and k is obtained by using m extended-dimensional EKF filters that match the m motion models corresponding to the m motion states at time k. The corresponding state estimation of the robot under each motion model at time is obtained, and m states are obtained, and the weighted calculation is performed on the m states to obtain the estimation result of the robot state at time k, including:

The steps of establishing the robot tracking system: establishing the robot tracking system, which includes the motion equation and the observation equation of the robot, which are expressed as follows:

Motion equation:

Observation equation:

C _ij =P(M _k =M ^j |M _k-1 =M ⁱ );

Where i, j = 1, 2...m represents the number of models, n = 1, 2...n represents the number of ultrasonic arrays, m and n are both integers greater than or equal to 1, k ∈ N represents time, C _ij represents k -1 The probability that the target transfers from model i to model j at time k,

Represents the i-th model state transition matrix at time k,

Indicates the target state under the i-th motion model at time k,

Means the observation matrix of the nth array at time k,

Represents the target state observation received by the nth array at time k.

Represents the process noise of model i,

Denoted as the observed noise of the nth array, both types of noise are assumed to be zero mean, and the covariances are respectively

Gaussian white noise;

Model input interaction steps: set

Is the state estimation of the extended dimension EKF filter i at k-1

For the corresponding covariance matrix estimation,

Is the probability of model i at time k-1. After interactive calculation, the input calculation formula of the extended-dimension EKF filter j at time k is as follows:

among them

Sub-model filtering step: calculate the corresponding input in each extended dimension EKF filter

Take advantage of the measurement

Update the corresponding state estimation under each model;

Model probability update step: Calculate the model probability for each model i=1, 2,...m, the calculation formula is as follows:

among them,

Estimated fusion output step: According to the update probability and state estimation of each model and the estimated covariance matrix estimation, calculate the state estimation and estimated covariance matrix estimation of the target at the current moment. The calculation formula is as follows:

x _k|k represents the estimation of the target state at time k, and P _k|k represents the estimation of the covariance matrix of the target state at time k.

Further, the sub-model filtering step includes:

State prediction sub-step: For each model i=1, 2...m, calculate the corresponding prediction state and prediction covariance matrix, the calculation formula is as follows:

Data fusion sub-step: data fusion is carried out using the expansion algorithm, and the formula of each corresponding variable is as follows:

Corresponding to the model i=1, 2...m, the calculation formulas for the respective measurement prediction residuals and measurement covariances are as follows:

At the same time, the likelihood function corresponding to model i is calculated. Under the assumption that it obeys the Gaussian distribution, the likelihood function is as follows:

Filtering update sub-step: corresponding to the model i = 1, 2, ... m, respectively calculate the respective filter gain, state estimation update, and error covariance matrix, the calculation formula is as follows:

In another aspect, a robot tracking device is provided, which includes:

The data acquisition module is used to: acquire observation data of the robot by at least two ultrasonic arrays at each moment of tracking;

The calculation module is used to estimate the motion state of the robot at each time by using the preset extended-dimensional IMM-EKF algorithm, and pass m extended-dimensional EKF filters that match the m motion models corresponding to the m motion states at time k, Obtain the state estimation corresponding to the robot under each motion model at time k, obtain m states, and perform weighted calculation on the m states to obtain the state estimation result at time k, where each time is represented by k, k, m Both are integers greater than 0.

Further, the data acquisition module is used for:

Where k and n are both integers greater than 0,

Further, the calculation module includes a robot tracking system establishment module for:

The robot tracking system is established, and the robot tracking system includes the robot's motion equation and observation equation as follows:

Motion equation:

Observation equation:

C _ij =P(M _k =M ^j |M _k-1 =M ⁱ );

Represents the i-th model state transition matrix at time k,

Indicates the target state under the i-th motion model at time k,

Means the observation matrix of the nth array at time k,

Represents the target state observation received by the nth array at time k.

Represents the process noise of model i,

Gaussian white noise;

Model input interaction module, used to: set

Is the state estimation of the extended dimension EKF filter i at k-1

For the corresponding covariance matrix estimation,

among them

The sub-model filter module is used to calculate the corresponding input in each extended dimension EKF filter

Use acquired measurements

Update the corresponding state estimation under each model;

The model probability update module is used to calculate the model probability for each model i=1, 2,...m, the calculation formula is as follows:

among them,

The estimation fusion output module is used to calculate the state estimation and covariance matrix estimation of the target at the current moment according to the update probability and state estimation of each model and the covariance matrix estimation. The calculation formula is as follows:

Further, the sub-model filtering module includes:

The state prediction sub-module is used to calculate the corresponding prediction state and prediction covariance matrix for each model i=1, 2...m, and the calculation formula is as follows:

The data fusion sub-module is used for data fusion using the expansion algorithm, and the formulas of each corresponding variable are as follows:

The filter update sub-module is used to calculate the respective filter gain, state estimation update, and error covariance matrix corresponding to the model i=1, 2,...m. The calculation formula is as follows:

In another aspect, a robot tracking device includes:

processor;

A memory for storing executable instructions of the processor;

Wherein, the processor is configured to execute the steps of the robot tracking method according to any one of the above solutions via the executable instructions.

In yet another aspect, a computer-readable storage medium is provided, and the computer-readable storage medium stores a computer program that, when executed by a processor, implements the steps of the robot tracking method according to any one of the above solutions.

Beneficial effects brought about by the technical solutions provided by the embodiments of the present invention:

1. By arranging multiple ultrasonic arrays, the observation data is acquired at each time of the robot tracking, and the dimension expansion is performed on the basis of the IMM-EKF algorithm through the preset dimension expansion IMM-EKF algorithm, and each step of the iterative process is measured and expanded to obtain more information. Multi-target motion state information, suitable for multi-ultrasound arrays;

2. Make full use of the original observation data, the fusion effect is optimal, and the tracking accuracy when tracking the robot indoors is improved. The tracking error is small and the calculation amount is relatively low, so that the intelligent robot can be performed even when the state of the intelligent robot is unknown and changeable. Stable and effective tracking to reduce the occurrence of mistracking or tracking loss;

3. Using the extended-dimensional IMM-EKF algorithm to track the intelligent robot can effectively weaken the influence of reverberation and noise on the tracking accuracy. The tracking error is significantly smaller than the traditional IMM-EKF algorithm, and it also has the ability to track scenes with missing observation data. Good robustness.

Description of the drawings

In order to explain the technical solutions in the embodiments of the present invention more clearly, the following will briefly introduce the drawings needed in the description of the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained from these drawings without creative work.

Fig. 1 is a flowchart of a robot tracking method provided by an embodiment of the present invention;

Figure 2 is a flowchart of sub-steps of step 102 in Figure 1;

Figure 3 is a flowchart of sub-steps of step 1023 in Figure 2;

4 is a flowchart of a robot tracking method provided by an embodiment of the present invention;

5 is a schematic diagram of a calculation process of a state calculation result in a robot tracking method provided by an embodiment of the present invention;

Figure 6 is a schematic structural diagram of a robot tracking device provided by an embodiment of the present invention;

FIG. 7 is a schematic diagram of the composition of a robot tracking device provided by an embodiment of the present invention;

FIG. 8 is a comparison diagram of the tracking effect of the robot tracking solution provided by the embodiment of the present invention and the existing solution;

FIG. 9 is a comparison diagram of the tracking error effect of the robot tracking solution provided by the embodiment of the present invention and the existing solution.

Detailed ways

In order to make the objectives, technical solutions and advantages of the present invention clearer, the following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only A part of the embodiments of the present invention, but not all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative work shall fall within the protection scope of the present invention. In the description of the present invention, "plurality" means two or more, unless otherwise specifically defined.

The robot tracking method, device, equipment, and computer-readable storage medium provided by the embodiments of the present invention can obtain observation data at each time of robot tracking by arranging multiple ultrasonic arrays, and use the preset expanded dimension IMM-EKF algorithm in IMM- Based on the EKF algorithm, each step of the iterative process is measured and expanded to obtain more target motion state information. It is suitable for multi-ultrasonic arrays, makes full use of the original observation data, has the best fusion effect, and improves the tracking accuracy when tracking the robot indoors. , The tracking error is small and the amount of calculation is relatively low, so that the intelligent robot can be tracked stably and effectively even when the state of the intelligent robot is unknown and changeable, reducing the occurrence of false follow or follow-up. Therefore, the robot tracking method is suitable for application fields involving intelligent robot manipulation, and is especially suitable for application scenarios of multiple ultrasonic arrays.

The robot tracking method, device, equipment, and computer-readable storage medium provided by the embodiments of the present invention will be described in detail below with reference to specific embodiments and drawings.

Fig. 1 is a flowchart of a robot tracking method provided by an embodiment of the present invention. Fig. 2 is a flowchart of sub-steps of step 102 in Fig. 1. Fig. 3 is a flowchart of sub-steps of step 1023 in Fig. 2.

As shown in Figure 1, the robot tracking method provided by the embodiment of the present invention includes the following steps:

101. Obtain observation data of the robot by at least two ultrasonic arrays at each moment of tracking.

At each time of tracking, obtain the observation data of the robot by at least two ultrasonic arrays, including:

Where k and n are both integers greater than 0,

Both are vectors of measured robot angle and distance data.

It is worth noting that, in addition to the manner described in the foregoing steps, the process of step 101 may also be implemented in other manners, and the embodiment of the present invention does not limit the specific manner.

102. Utilize the preset extended dimension IMM-EKF algorithm to estimate the motion state of the robot at each time, and obtain each type of the robot at time k through m extended-dimensional EKF filters that match the m motion models corresponding to the m motion states at time k. The state estimation corresponding to the robot under the motion model obtains m states, and the weighted calculation of the m states obtains the robot state estimation result at k time, where each time is represented by k time, and k and m are integers greater than 0.

As shown in Figure 2, the above 102 step further includes the following sub-steps:

1021-Stochastic hybrid system calculation step: Robot tracking system establishment step: The robot tracking system is established, and the robot tracking system includes the motion equation and observation equation of the robot, which are expressed as follows:

Motion equation:

Observation equation:

C _ij =P(M _k =M ^j |M _k-1 =M ⁱ );

Represents the i-th model state transition matrix at time k,

Indicates the target state under the i-th motion model at time k,

Means the observation matrix of the nth array at time k,

Represents the target state observation received by the nth array at time k.

Represents the process noise of model i,

Gaussian white noise;

1022-Model input interaction steps: set

Is the state estimation of the extended dimension EKF filter i at time k-1,

For the corresponding covariance matrix estimation,

among them

1023-Sub-model filtering step: Calculate the corresponding input in each extended dimension EKF filter

Use acquired measurements

Update the corresponding state estimation under each model;

1024-model probability update step: calculate the model probability for each model i=1, 2,...m, the calculation formula is as follows:

among them,

1025-Estimation fusion output step: According to the update probability and state estimation of each model and the estimated covariance matrix estimation, calculate the state estimation and covariance matrix estimation of the target at the current moment. The calculation formula is as follows:

As shown in Fig. 3, the aforementioned sub-model filtering step further includes the following sub-steps:

1023a-State prediction sub-step: For each model i=1, 2...m, calculate the corresponding prediction state and prediction covariance matrix, and the calculation formula is as follows:

1023b-Data fusion sub-step: data fusion is carried out using the expansion algorithm, the formula of each corresponding variable is as follows:

1023c-Filtering update sub-step: corresponding to the model i=1, 2,...m, respectively calculate the respective filter gain, state estimation update, and error covariance matrix, the calculation formula is as follows:

FIG. 4 is a flowchart of a robot tracking method provided by an embodiment of the present invention. FIG. 5 is a schematic diagram of a state calculation result calculation process in the robot tracking method provided by an embodiment of the present invention, and jointly demonstrates an implementation of selecting two ultrasonic arrays.

It is worth noting that, in addition to the manner described in the foregoing steps, the process of step 102 may also be implemented in other ways, and the embodiment of the present invention does not limit the specific manner.

FIG. 6 is a schematic structural diagram of a robot tracking device provided by an embodiment of the present invention. As shown in FIG. 6, the robot tracking device provided by an embodiment of the present invention includes a data acquisition module 1 and a calculation module 2.

Among them, the data acquisition module 1 is used to acquire observation data of the robot by at least two ultrasonic arrays at each moment of tracking. Specifically, the data acquisition module 1 is used to: at time k, acquire observation data of the robot by at least two ultrasonic arrays

Where k and n are both integers greater than 0,

Both are vectors of robot angle and distance data measured by at least two ultrasonic arrays.

The calculation module 2 is used to: use the preset extended dimension IMM-EKF algorithm to estimate the motion state of the robot at each moment, and obtain respectively through m extended dimensional EKF filters matching m motion models corresponding to the m motion states at time k The state estimation of the robot corresponding to each motion model at k time is obtained, and m states are obtained, and the weighted calculation of m states is performed to obtain the state estimation result at time k, where each time is represented by k time, and k and m are integers greater than 0 .

Specifically, the calculation module 2 includes a robot tracking system establishment module 21, a model input interaction module 22, a sub-model filtering module 23, a model probability update module 24, and an estimated fusion output module 25.

Among them, the robot tracking system establishment module 21 is used for:

Motion equation:

Observation equation:

C _ij =P(M _k =M ^j |M _k-1 =M ⁱ );

Represents the i-th model state transition matrix at time k,

Indicates the target state under the i-th motion model at time k,

Means the observation matrix of the nth array at time k,

Represents the target state observation received by the nth array at time k.

Represents the process noise of model i,

Gaussian white noise.

The model input interaction module 22 is used to:

Assume

Is the state estimation of the extended dimension EKF filter i at time k-1,

For the corresponding covariance matrix estimation,

among them

The sub-model filtering module 23 is used to:

Calculate the corresponding input in each extended dimension EKF filter

Use acquired measurements

Update the corresponding state estimation under each model;

The model probability update module 24 is used to calculate the model probability for each model i=1, 2,...m, and the calculation formula is as follows:

among them,

The estimated fusion output module 25 is used for:

According to the update probability, state estimation and covariance matrix estimation of each model, the state estimation and covariance matrix estimation of the target at the current moment are calculated. The calculation formula is as follows:

Further, the sub-model filtering module 23 includes a state prediction sub-module 231, a data fusion sub-module 232, and a filtering update sub-module 233.

Among them, the state prediction sub-module 231 is used to:

For each model i=1, 2...m, respectively calculate the corresponding prediction state and prediction covariance matrix, the calculation formula is as follows:

The data fusion sub-module 232 is used for:

The data fusion is carried out using the expansion algorithm, and the formulas of each corresponding variable are as follows:

The filtering update sub-module 233 is used to:

Corresponding to the model i = 1, 2, ... m, respectively calculate the respective filter gain, state estimation update, and error covariance matrix, the calculation formula is as follows:

FIG. 7 is a schematic diagram of the composition of a robot tracking device provided by an embodiment of the present invention. As shown in FIG. 7, the robot tracking device provided by an embodiment of the present invention includes a processor 3 and a memory 4, and the memory 4 is used to store the processor 3 Executing instructions; wherein, the processor 3 is configured to execute the steps of the robot tracking method described in any one of the above solutions via the aforementioned executable instructions.

The embodiment of the present invention also provides a computer-readable storage medium, the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the robot tracking method described in any of the above solutions are implemented.

It should be noted that when the robot tracking device provided in the above embodiment triggers the robot tracking service, only the division of the above functional modules is used as an example for illustration. In actual applications, the above functions can be allocated by different functional modules according to needs. , That is, divide the internal structure of the device into different functional modules to complete all or part of the functions described above. In addition, the robot tracking device, robot tracking device, computer-readable storage medium and the robot tracking method for triggering the robot tracking service provided in the above embodiments belong to the same concept. For the specific implementation process, please refer to the method embodiment. I won't repeat it here.

All the above optional technical solutions can be combined in any way to form an optional embodiment of the present invention, which will not be repeated here.

To illustrate the advantages of the robot tracking solution provided by the embodiments of the present invention in tracking indoor automation equipment, the robot tracking method, the IMM-EKF method, and the weighted IMM-EKF provided by the embodiments of the present invention are used to process the robot measurement data. The state of the robot is estimated, and the result is shown in Figure 8.

FIG. 8 is a comparison diagram of the tracking effect of the robot tracking solution provided by the embodiment of the present invention and the existing solution. FIG. 9 is a comparison diagram of the tracking error effect of the robot tracking solution provided by the embodiment of the present invention and the existing solution.

As shown in Figure 9, in order to further characterize the performance of different methods, the tracking error of the above estimation results is calculated for performance evaluation. The error formula of state estimation at t _{k is:}

among them,

Represents the estimated position coordinates of the target state at time _{t k} _{, and (x k} , y _k ) represents the true position of the target at time _{t k.}

Table 1 below shows the average target tracking error of the three methods, as shown below:

Table 1

It can be seen that the tracking accuracy of the robot tracking method provided by the embodiment of the present invention is significantly better than the IMM-EKF algorithm, and compared with the weighted IMM-EKF algorithm, the tracking error is reduced by nearly 50%.

In summary, the robot tracking method, device, device, and computer-readable storage medium provided by the embodiments of the present invention have the following beneficial effects compared with the prior art:

A person of ordinary skill in the art can understand that all or part of the steps in the above embodiments can be implemented by hardware, or by a program to instruct relevant hardware. The program can be stored in a computer-readable storage medium. The storage medium mentioned can be a read-only memory, a magnetic disk or an optical disk, etc.

The embodiments of the present application are described with reference to the flowcharts and/or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and/or block in the flowchart and/or block diagram, and the combination of processes and/or blocks in the flowchart and/or block diagram can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing equipment to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing equipment are generated It is a device that realizes the functions specified in one process or multiple processes in the flowchart and/or one block or multiple blocks in the block diagram.

These computer program instructions can also be stored in a computer-readable memory that can guide a computer or other programmable data processing equipment to work in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture including the instruction device. The device implements the functions specified in one process or multiple processes in the flowchart and/or one block or multiple blocks in the block diagram.

These computer program instructions can also be loaded on a computer or other programmable data processing equipment, so that a series of operation steps are executed on the computer or other programmable equipment to produce computer-implemented processing, so as to execute on the computer or other programmable equipment. The instructions provide steps for implementing the functions specified in one process or multiple processes in the flowchart and/or one block or multiple blocks in the block diagram.

Although the preferred embodiments in the embodiments of the present application have been described, those skilled in the art can make additional changes and modifications to these embodiments once they learn the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications falling within the scope of the embodiments of the present application.

Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. In this way, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection of the present invention. Within range.

Claims

A robot tracking method, characterized in that the method includes:

Obtain the observation data of the robot by at least two ultrasonic arrays at each moment of tracking;

The preset extended-dimensional IMM-EKF algorithm is used to estimate the motion state of the robot at each time. Through m extended-dimensional EKF filters that match the m motion models corresponding to the m motion states at time k, each type at time k is obtained. The state of the robot under the motion model is estimated, m states are obtained, and the m states are weighted to obtain the robot state estimation result at time k, where each time is represented by k time, and k and m are both greater than 0 Integer.
The method according to claim 1, wherein at each moment of tracking, obtaining observation data of the robot by at least two ultrasonic arrays comprises:

At time k, obtain the observation data of at least two ultrasonic arrays on the robot
Where k and n are both integers greater than 0,
Both are vectors of the angle and distance data of the robot measured by the at least two ultrasonic arrays.
The method according to claim 2, characterized in that a preset extended dimension IMM-EKF algorithm is used to estimate the motion state of the robot at each time, and the motion state of the robot at each time is estimated by m motion models corresponding to the m motion states at time k. An extended-dimensional EKF filter respectively obtains the state estimation corresponding to the robot under each motion model at time k, obtains m states, and performs weighted calculation on the m states to obtain the robot state estimation result at time k, including:

The steps of establishing the robot tracking system: establishing the robot tracking system, which includes the motion equation and the observation equation of the robot, which are expressed as follows:

Motion equation:

Observation equation:

C ij =P(M k =M j |M k-1 =M i );

Among them, i, j = 1, 2...m represents the number of models, n = 1, 2...n represents the number of ultrasonic arrays, m and n are both integers greater than or equal to 1, k ∈ N represents time, C ij represents k -1 The probability that the target transfers from model i to model j at time k,
Represents the i-th model state transition matrix at time k,
Indicates the target state under the i-th motion model at time k,
Means the observation matrix of the nth array at time k,
Represents the target state observation received by the nth array at time k.
Represents the process noise of model i,
Denoted as the observed noise of the nth array, both types of noise are assumed to be zero mean, and the covariances are respectively
Gaussian white noise;

Model input interaction steps: set
Is the state estimation of the extended dimension EKF filter i at time k-1,
For the corresponding covariance matrix estimation,
Is the probability of model i at time k-1. After interactive calculation, the input calculation formula of the extended-dimension EKF filter j at time k is as follows:

among them

Sub-model filtering step: calculate the corresponding input in each extended dimension EKF filter

Use acquired measurements
Update the corresponding state estimation under each model;

Model probability update step: Calculate the model probability for each model i=1, 2,...m, the calculation formula is as follows:

among them,

Estimated fusion output step: According to the update probability and state estimation of each model and the covariance matrix estimation, calculate the state estimation and covariance matrix estimation of the target at the current moment. The calculation formula is as follows:

x k|k represents the estimation of the target state at time k, and P k|k represents the estimation of the covariance matrix of the target state at time k.
The method according to claim 3, wherein the sub-model filtering step comprises:

State prediction sub-step: For each model i=1, 2...m, calculate the corresponding prediction state and prediction covariance matrix, the calculation formula is as follows:

Data fusion sub-step: data fusion is carried out using the expansion algorithm, and the formula of each corresponding variable is as follows:

Corresponding to the model i=1, 2...m, the calculation formulas for the respective measurement prediction residuals and measurement covariances are as follows:

At the same time, the likelihood function corresponding to model i is calculated. Under the assumption that it obeys the Gaussian distribution, the likelihood function is as follows:

Filtering update sub-step: corresponding to the model i = 1, 2, ... m, respectively calculate the respective filter gain, state estimation update, and error covariance matrix, the calculation formula is as follows:
A robot tracking device, characterized in that the device includes:

The data acquisition module is used to: acquire observation data of the robot by at least two ultrasonic arrays at each moment of tracking;

The calculation module is used to estimate the motion state of the robot at each time by using the preset extended-dimensional IMM-EKF algorithm, and pass m extended-dimensional EKF filters that match the m motion models corresponding to the m motion states at time k, Obtain the state estimation corresponding to the robot under each motion model at time k, obtain m states, and perform weighted calculation on the m states to obtain the state estimation result at time k, where each time is represented by k, k, m Both are integers greater than 0.
The device according to claim 5, wherein the data acquisition module is configured to:

At time k, obtain the observation data of at least two ultrasonic arrays on the robot
Where k and n are both integers greater than 0,
Both are vectors of the angle and distance data of the robot measured by the at least two ultrasonic arrays.
7. The device according to claim 6, wherein the calculation module comprises a robot tracking system establishment module for:

The robot tracking system is established, and the robot tracking system includes the robot's motion equation and observation equation as follows:

Motion equation:

Observation equation:

C ij =P(M k =M j |M k-1 =M i );

Where i, j = 1, 2...m represents the number of models, n = 1, 2...n represents the number of ultrasonic arrays, m and n are both integers greater than or equal to 1, k ∈ N represents time, C ij represents k -1 The probability that the target transfers from model i to model j at time k,
Represents the i-th model state transition matrix at time k,
Indicates the target state under the i-th motion model at time k,
Means the observation matrix of the nth array at time k,
Represents the target state observation received by the nth array at time k.
Represents the process noise of model i,
Denoted as the observed noise of the nth array, both types of noise are assumed to be zero mean, and the covariances are respectively
Gaussian white noise;

Model input interaction module, used to: set
Is the state estimation of the extended dimension EKF filter i at time k-1,
For the corresponding covariance matrix estimation,
Is the probability of model i at time k-1. After interactive calculation, the input calculation formula of the extended-dimension EKF filter j at time k is as follows:

among them

The sub-model filter module is used to calculate the corresponding input in each extended dimension EKF filter
Use acquired measurements
Update the corresponding state estimation under each model;

The model probability update module is used to calculate the model probability for each model i=1, 2,...m, the calculation formula is as follows:

among them,

The estimation fusion output module is used to calculate the state estimation and covariance matrix estimation of the target at the current moment according to the update probability and state estimation of each model and the covariance matrix estimation. The calculation formula is as follows:

x k|k represents the estimation of the target state at time k, and P k|k represents the estimation of the covariance matrix of the target state at time k.
The device according to claim 7, wherein the sub-model filtering module comprises:

The state prediction sub-module is used to calculate the corresponding prediction state and prediction covariance matrix for each model i=1, 2...m, and the calculation formula is as follows:

The data fusion sub-module is used for data fusion using the expansion algorithm, and the formulas of each corresponding variable are as follows:

Corresponding to the model i=1, 2...m, the calculation formulas for the respective measurement prediction residuals and measurement covariances are as follows:

At the same time, the likelihood function corresponding to model i is calculated. Under the assumption that it obeys the Gaussian distribution, the likelihood function is as follows:

The filter update sub-module is used to calculate the respective filter gain, state estimation update, and error covariance matrix corresponding to the model i=1, 2,...m. The calculation formula is as follows:
A robot tracking device, which is characterized in that it comprises:

processor;

A memory for storing executable instructions of the processor;

Wherein, the processor is configured to execute the steps of the robot tracking method according to any one of claims 1 to 4 via the executable instructions.
A computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the robot tracking method according to any one of claims 1 to 4 is implemented. step.