Target positioning method and device for mobile robot

Abstract

The invention discloses a target positioning method and device for a mobile robot, a storage medium and electronic equipment. The method comprises the following steps: acquiring base station distribution information in the ultra-wideband positioning system; the ultra-wideband positioning system comprises a base station and a label, constructing a TDOA positioning equation based on the base station distribution information, and performing secondary estimation on the TDOA positioning equation through a Chan algorithm to obtain an initial coordinate value of the tag; taking the initial coordinate value as an initial value of Taylor series, and obtaining a coordinate correction value of the label through Taylor; calculating a label error value based on the coordinate correction value of the label; and when the label error value is smaller than a preset error threshold value, outputting a label coordinate estimation value, filtering the label coordinate estimation value through a Kalman filtering algorithm to obtain a label coordinate optimal estimation value, and taking the label coordinate optimal estimation value as a target position. The method can improve the positioning range and effect, and enables the positioning data to be more accurate.

Description

Technical Field

[0001] The present application relates to the technical field of mobile robot positioning, and in particular to a target positioning method, device, storage medium and electronic device for a mobile robot. Background Art

[0002] When the robot is performing path planning and following a target in a wide range, it is necessary to accurately locate the position of the robot and the target within the positioning range of the positioning base station.

[0003] UWB (Ultra-WideBand) is called an ultra-wideband positioning system, which is mainly composed of tags and base stations. The base station and the tag communicate through ultra-wideband signals, and the actual position of the positioning tag is measured through a certain positioning algorithm.

[0004] Common positioning methods in UWB positioning systems include: time of arrival (TOA), angle of arrival (AOA), and received signal strength (RSSI). Both TOA and AOA are suitable for deploying three positioning base stations, and the positioning range is not wide. RSSI has a low positioning accuracy due to the influence of indoor environment on signal multipath fading. Summary of the invention

[0005] The embodiments of the present application provide a target positioning method, device, storage medium and electronic device for a mobile robot, which can solve the technical problems of a narrow positioning range and low positioning accuracy.

[0006] The present application embodiment provides a target positioning method for a mobile robot, comprising:

[0007] Acquire base station distribution information in an ultra-wideband positioning system; wherein the ultra-wideband positioning system includes base stations and tags;

[0008] Constructing a TDOA positioning equation based on the base station distribution information, and performing a secondary estimation on the TDOA positioning equation through a Chan algorithm to obtain an initial coordinate value of the tag;

[0009] The initial coordinate value is used as the initial value of the Taylor series, and the coordinate correction value of the label is obtained through Taylor;

[0010] Calculating a label error value based on a coordinate correction value of the label;

[0011] When the tag error value is less than a preset error threshold, the tag coordinate estimate is output, and the tag coordinate estimate is filtered by a Kalman filter algorithm to obtain an optimal tag coordinate estimate, which is used as the target position.

[0012] Furthermore, the target positioning method for a mobile robot, wherein the TDOA positioning equation is constructed based on the base station distribution information, and the TDOA positioning equation is estimated twice by the Chan algorithm to obtain the coordinate value of the tag, includes:

[0013] Converting the TDOA positioning equation into a linear system of equations;

[0014] Solving the linear equations by weighted least squares method to obtain a first weighted least squares estimate;

[0015] An error equation group is established based on the first weighted least squares estimation value, and the error equation group is solved to obtain the initial coordinate value of the tag.

[0016] Furthermore, in the target positioning method for a mobile robot, the initial coordinate value is used as the initial value of the Taylor series to obtain the coordinate correction value of the tag through Taylor, including:

[0017] The relationship between the base station and the tag is expressed by the function f i express;

[0018] The coordinate value is used as the initial value of the Taylor series, and the function f i Performing Taylor expansion to obtain a Taylor expansion, and converting the Taylor expansion into a matrix to obtain an error vector;

[0019] Solving the error vector to obtain a coordinate error value;

[0020] The initial coordinate value is updated based on the coordinate error value, and the above steps are repeated. When the coordinate error value is less than a preset threshold value, the iteration is stopped to obtain the final coordinate correction value of the tag.

[0021] Furthermore, in the target positioning method for a mobile robot, the step of filtering the tag coordinate estimation value by a Kalman filter algorithm to obtain the optimal tag coordinate estimation value includes:

[0022] The tag horizontal coordinate and the tag vertical coordinate in the tag coordinate estimation value are respectively used as state parameters, and Kalman filtering is performed on the state parameters to obtain the optimal tag coordinate estimation value.

[0023] Furthermore, in the target positioning method for a mobile robot, the tag horizontal coordinate and the tag vertical coordinate in the tag coordinate estimation value are respectively used as state parameters, and Kalman filtering is performed on the state parameters, including:

[0024] Construct the equation of state:

[0025] X t+1 =A*X t +ω t

[0026] Among them, X t represents the state vector of the ultra-wideband positioning system at time t, X t =[x t ], x t is the label abscissa at time t in the label estimate, A represents the state transfer matrix, ω t represents the noise of the ultra-wideband positioning system;

[0027] The observation equation is constructed as:

[0028] Z t =HX t +υ t

[0029] Among them, Z t represents the observation vector, υ t is the observation noise, H is the observation matrix;

[0030] Predicted state estimation:

[0031]

[0032] Forecast covariance estimate:

[0033]

[0034] in, is the estimated value of the predicted state, B is the control input matrix, u t-1 is the input of the Kalman filter at the previous moment, P t - is the predicted covariance estimation matrix, Q is the covariance matrix of the UWB positioning system noise;

[0035] Calculate the Kalman gain:

[0036]

[0037] Among them, K t is the Kalman gain, R is the covariance matrix of the observation noise;

[0038] Update the state estimate:

[0039]

[0040] Update the covariance estimate:

[0041]

[0042] Where I is the identity matrix.

[0043] Furthermore, the target positioning method for a mobile robot, wherein the step of solving the linear equations by weighted least squares method to obtain a first weighted least squares estimate includes:

[0044] Order Z a =[xy R1] T is the solution of the linear equations, Z is the actual position of the label a The value of

[0045] Due to the noise in the TDOA positioning process, that is, GZ 0 ≠h, let the error vector be ψ, then we have the first formula:

[0046]

[0047] Assume Z a The elements are independent of each other, then Z a The maximum likelihood estimate of is expressed by the second formula, which is:

[0048] Z a =argmin{(hG a Z a ) T ψ(hG a Z a )}=(G a T ψ -1 G a ) -1 G a T ψ -1 h

[0049] Where ψ=E[ψψ T ]=C 2 BQB,

[0050] Where Q is the covariance of TDOA, n is the noise vector that obeys the normal distribution;

[0051] When the tag is far away from the base station and close to the actual distance, the second formula can be approximately expressed as the third formula:

[0052] Z a ≈(G a T ψ -1 G a ) -1 Ga T ψ -1 h

[0053] When the tag is close to the base station, the third formula is used to calculate the matrix B to obtain the value of ψ, and then the values ​​of B and ψ are substituted into the second formula to obtain the first weighted least squares estimate Z a .

[0054] Furthermore, the target positioning method for a mobile robot, wherein an error equation group is established based on the first weighted least squares estimate, and the error equation group is solved to obtain the initial coordinate value of the tag, includes:

[0055] Based on the Z a The error equations are established and the second position estimation is solved. The calculation formula is as follows:

[0056]

[0057] in,

[0058] The final initial coordinate value of the label is:

[0059]

[0060] The embodiment of the present application also provides a target positioning device for a mobile robot, comprising:

[0061] An acquisition module, used to acquire base station distribution information in an ultra-wideband positioning system; wherein the ultra-wideband positioning system includes base stations and tags;

[0062] A first processing module is used to construct a TDOA positioning equation based on the base station distribution information, and perform a secondary estimation on the TDOA positioning equation through a Chan algorithm to obtain an initial coordinate value of the tag;

[0063] A second processing module is used to use the initial coordinate value as the initial value of the Taylor series to obtain a coordinate correction value of the label through Taylor;

[0064] A third processing module, used for calculating a label error value based on the coordinate correction value of the label;

[0065] The filtering module is used to output a tag coordinate estimation value when the tag error value is less than a preset error threshold, and filter the tag coordinate estimation value through a Kalman filtering algorithm to obtain an optimal tag coordinate estimation value, and use the optimal tag coordinate estimation value as the target position.

[0066] An embodiment of the present application also provides a computer-readable storage medium, in which a plurality of instructions are stored, and the instructions are suitable for being loaded by a processor to execute any of the above-mentioned target positioning methods for a mobile robot.

[0067] An embodiment of the present application also provides an electronic device, including a processor and a memory, wherein the processor is electrically connected to the memory, the memory is used to store instructions and data, and the processor is used for the steps in any of the above-mentioned target positioning methods for a mobile robot.

[0068] The target positioning method, device, storage medium and electronic device for a mobile robot provided in the present application combine the advantages of each algorithm by combining the positioning algorithm, accurately solves the coordinate value through the Chan algorithm and uses it as the initial value of the Taylor algorithm to achieve a better positioning effect, and then filters out the noise through the Kalman filter algorithm to make the positioning data more accurate and the positioning range larger. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] The technical solution and other beneficial effects of the present application will be made apparent by describing in detail the specific implementation methods of the present application in conjunction with the accompanying drawings.

[0070] Figure 1 A flowchart of a target positioning method for a mobile robot provided in an embodiment of the present application.

[0071] Figure 2 A schematic diagram of the structure of a target positioning device for a mobile robot provided in an embodiment of the present application.

[0072] Figure 3 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application.

[0073] Figure 4 Another schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0074] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of this application.

[0075] The embodiments of the present application provide a method, device, storage medium and electronic device for target positioning of a mobile robot. The target positioning device for a mobile robot provided in the embodiments of the present application can be integrated in an electronic device, which can be a terminal, a server and other devices, wherein the terminal can include a tablet computer, a laptop computer, a personal computer (PC), a micro processing box, or other devices.

[0076] See also Figure 1 , Figure 1 A flow chart of a target positioning method for a mobile robot provided in an embodiment of the present application, which is applied to an electronic device, and the target positioning method for a mobile robot includes the following steps:

[0077] S1, obtaining base station distribution information in an ultra-wideband positioning system; wherein the ultra-wideband positioning system includes base stations and tags.

[0078] UWB (Ultra-WideBand) is called an ultra-wideband positioning system, which is mainly composed of tags and base stations. The base station and the tag communicate through ultra-wideband signals, and the actual position of the positioning tag is measured through a certain positioning algorithm. This application locates the target through an ultra-wideband positioning system, and the tag position in the ultra-wideband positioning system is the target position to be located.

[0079] S2, construct the TDOA positioning equation based on the base station distribution information, and perform secondary estimation on the TDOA positioning equation through the Chan algorithm to obtain the initial coordinate value of the tag.

[0080] In one embodiment, the tag is positioned using the TDOA algorithm. TDOA (Time Difference of Arrival) is called the time difference method for receiving signals, and is a positioning method based on the time difference of signal arrival to measure distance. This method calculates the time difference between the signal emitted by the tag to be tested arriving at the main base station and the time difference between the signal transmitted to other base stations, and then multiplies the time difference by the speed c to obtain the distance difference between the tag to be tested and the main base station and other base stations. Since the distance difference between the tag to be tested and the main base station and the tag to be tested and other base stations is a constant, the tag to be tested must be on a hyperbola with two base stations as foci. Thus, a set of equations with hyperbolic properties can be formed, and the positioning equation of TDOA is the solution to the set of hyperbolic equations. Step S2 specifically includes the following steps:

[0081] S21, convert the TDOA positioning equation into a system of linear equations.

[0082] Assume that the real position of the tag is (x, y) and the position of the i-th base station is (x i ,y i)(i=1,2,3,...,n),R i Represents the distance from the tag to the i-th base station. The difference between the distance from the tag to the i-th base station and the distance from the tag to the first base station is R i,1 .

[0083]

[0084] Where c is the propagation speed of the signal in the air, t i,1 is the measured value of TDOA. Square both sides of equation (1) to obtain:

[0085]

[0086] Among them, K i =x i 2 +y i 2 (i=1,2,3,…,n)

[0087]

[0088] Substituting (1) into (4), we get:

[0089]

[0090] Where: x i,1 =x i -x1,y i,1 =y i -y1, when x, y, and R1 in equation (5) are regarded as unknowns, the nonlinear system of equations becomes a linear system of equations, and the label coordinates (x, y) can be obtained.

[0091] Rearranging (5) yields:

[0092]

[0093] make: have to:

[0094] Order Z a =[xy R1] T is the solution of equation (6), Z is the actual position of the label a The value of .

[0095] S22, solving the linear equations by weighted least squares method to obtain a first weighted least squares estimate.

[0096] Due to the presence of noise in the TDOA positioning process, that is, GZ 0 ≠h, let the error vector be ψ, then:

[0097]

[0098] Assume Z a The elements are independent of each other, then Z a The maximum likelihood estimate of is:

[0099] Z a =argmin{(hG a Z a ) T ψ(hG a Z a )}=(G a T ψ -1 G a ) -1 G a T ψ -1 h (8)

[0100] Where ψ=E[ψψ T ]=C 2 BQB,

[0101] Q is the covariance of TDOA, n is a noise vector that follows a normal distribution, and since B is the distance between the tag and each base station, ψ is unknown, and an approximate calculation is required to find the estimated position of the tag. When the tag is far away from the base station and close to the actual distance, equation (8) can be approximately expressed as:

[0102] Z a ≈(G a T ψ -1 G a ) -1 G a T ψ -1 h (9)

[0103] When the tag is close to the base station, the matrix B can be calculated using formula (9) to obtain the value of ψ. Then, the values ​​of B and ψ are substituted into formula (8) to obtain the first weighted least squares estimate Z a .

[0104] S23, establishing an error equation group based on the first weighted least squares estimation value, solving the error equation group to obtain the initial coordinate value of the tag.

[0105] The second estimate converts the Z obtained in the first estimate into a To establish the error equations and solve the second position estimate, the calculation formula is as follows:

[0106]

[0107] in,

[0108] The final initial coordinate value of the label is:

[0109]

[0110] S3, using the initial coordinate value as the initial value of the Taylor series, and obtaining the coordinate correction value of the label through Taylor.

[0111] The Taylor series expansion algorithm is an iterative algorithm. The principle of the algorithm is to first provide a coordinate estimate as the initial value of the Taylor series, and then perform a weighted least squares (WLS) estimation, and then continuously iterate to correct the label estimate, and finally approach the true coordinates of the label. Step S3 includes the following steps:

[0112] S31, the relationship between the base station and the tag is expressed by the function f i express.

[0113] S32, taking the coordinate value as the initial value of the Taylor series, for the function f i Taylor expansion is performed to obtain the Taylor expansion, and the Taylor expansion is converted into a matrix to obtain the error vector.

[0114] S33, solving the error vector to obtain the coordinate error value.

[0115] S34, updating the initial coordinate value based on the coordinate error value, and repeating the above steps, stopping the iteration when the coordinate error value is less than a preset threshold value, and obtaining the final coordinate correction value of the tag.

[0116] Steps S31-S34 are specifically as follows:

[0117] Using the function f i (x,y,x i ,y i ) to represent the relationship between the tag (x, y) and the base station (x i ,y i ), assuming that the true coordinates of the tag are (x, y), the initial coordinates of the tag are (x0, y0), and the error between the true coordinates and the initial coordinates is recorded as (Δx, Δy), that is: x = x0 + Δx, y = y0 + Δy. η represents the error threshold, and the iterative calculation stops when the condition is met: |Δx + Δy| < η.

[0118] Function f i (x,y,x i ,y i) is expanded into a Taylor series at the initial value (x0, y0) as follows:

[0119]

[0120] Based on the TDOA positioning method, we know that the distance difference between the tag and the i-th base station is R i , then the distance difference between the i-th base station and the first base station from the tag is R i,1 =R i -R1, the formula is as follows:

[0121]

[0122] Constructor f i (x,y,x i ,y i ):

[0123]

[0124] f i (x,y,x i ,y i ) is Taylor expanded at the initial value (x0, y0) and the second-order and above components are ignored, then:

[0125]

[0126] in,

[0127]

[0128] make: Then it is transformed into matrix form: ψ = h i -G i δ

[0129] Where Ψ is the error vector, R i (i=1,2,3,...,L) is the distance between the tag and the i-th base station in the iterative calculation. The weighted least squares solution of the above equation is:

[0130]

[0131] Where Q represents the covariance matrix of TDOA measurements.

[0132] S4, calculating the tag error value based on the coordinate correction value of the tag.

[0133] The coordinate error value is the estimated coordinate value (x', y) obtained after each iteration of the Taylor algorithm. ’) and the true coordinates (x, y), that is: Δx = x′-x, Δy = y′-y

[0134] In the next recursion, let x′=x0+Δx, y′=y0+Δy, update the coordinate value of the label to iteratively calculate, and repeat the above process until the error is less than the set threshold value, that is: when |Δx|+|Δy|<η, stop the iterative calculation and output the coordinate correction value.

[0135] S5, when the tag error value is less than the preset error threshold, output the tag coordinate estimate, and filter the tag coordinate estimate through the Kalman filter algorithm to obtain the optimal tag coordinate estimate, and use the optimal tag coordinate estimate as the target position.

[0136] The positioning coordinates X (horizontal coordinate) and Y (vertical coordinate) calculated by the Chan-Taylor joint algorithm are used as state parameters, and Kalman filtering is used to filter and predict the positioning coordinates X and Y respectively. The positioning coordinate X is used as the state parameter for Kalman filtering, and the processing method of the positioning coordinate Y is the same as that of X. The positioning coordinate X calculated by the Chan-Taylor joint algorithm is used as the state parameter:

[0137] X=[x]. Similarly, the positioning coordinate Y calculated by the Chan-Taylor joint algorithm is used as the state parameter: Y=[y]. Taking the positioning coordinate X as an example, the following analysis is made:

[0138] Construct the equation of state:

[0139] X t+1 =A*X t +ω t

[0140] Among them, X t represents the state vector of the ultra-wideband positioning system at time t, X t =[x t ], x t is the label abscissa at time t in the label estimate, A represents the state transfer matrix, ω t Represents the noise of the ultra-wideband positioning system.

[0141] The observation equation is constructed as:

[0142] Z t =HX t +υ t

[0143] Among them, Z t represents the observation vector, υ t is the observation noise, H is the observation matrix. Since the state variable is a single x-coordinate value, the observation matrix is ​​set to the unit matrix, that is: H = [1].

[0144] The Kalman filter algorithm takes the system model and observed variables as input, and outputs the optimal estimate of the real state through recursive calculation and correction of observed data. The Kalman filter algorithm is based on two assumptions: one is that the system model is linear; the other is that the system noise and observation noise are white noises that obey Gaussian distribution. The process is mainly divided into two steps: prediction and update.

[0145] The prediction step of the Kalman filter algorithm is to predict the state estimate and covariance estimate of the system at the current moment based on the state estimate and control input at the previous moment.

[0146] Predicted State Estimates:

[0147]

[0148] Forecast covariance estimate:

[0149]

[0150] in, is the estimated value of the predicted state, B is the control input matrix, u t-1 is the input of the Kalman filter at the previous moment, P t - is the predicted covariance estimation matrix, Q is the covariance matrix of the ultra-wideband positioning system noise. The variance of Q is set very small, which means that the process noise is very small, and the calculated estimate is more trustworthy.

[0151] The update step of the Kalman filter algorithm is to use the observation vector to correct the predicted state estimate. Calculate the Kalman gain:

[0152]

[0153] Among them, K t is the Kalman gain, R is the covariance matrix of the observation noise. Since the observation noise basically does not change, the covariance matrix is ​​a 1×1 matrix and is set to a constant.

[0154] Update the state estimate:

[0155]

[0156] Update the covariance estimate:

[0157]

[0158] Where I is the identity matrix.

[0159] Through the above steps, the coordinates of the positioning tag are more accurate after the Kalman filter algorithm filters out the noise. The Kalman filter algorithm mainly integrates the state information at different times, and estimates the optimal estimate of the system at the current time by combining the past state estimate and the current observation vector.

[0160] The embodiment of the present application combines the advantages of each algorithm by combining the positioning algorithm, accurately solves the coordinate value through the Chan algorithm and uses it as the initial value of the Taylor algorithm to achieve better positioning effect, and then filters out the noise through the Kalman filter algorithm to make the positioning data more accurate.

[0161] According to the method described in the above embodiment, this embodiment will be further described from the perspective of a target positioning device for a mobile robot. The target positioning device for a mobile robot can be implemented as an independent entity or integrated in an electronic device. The electronic device can be a terminal, a server, etc., wherein the terminal can include a tablet computer, a laptop computer, a personal computer (PC), a micro processing box, or other devices.

[0162] See also Figure 2 , Figure 2 The target positioning device for a mobile robot provided by an embodiment of the present application is specifically described, and is applied to an electronic device. The target positioning device for a mobile robot may include:

[0163] An acquisition module, used to acquire base station distribution information in an ultra-wideband positioning system; wherein the ultra-wideband positioning system includes base stations and tags;

[0164] A first processing module is used to construct a TDOA positioning equation based on the base station distribution information, and perform a secondary estimation on the TDOA positioning equation through a Chan algorithm to obtain an initial coordinate value of the tag;

[0165] A second processing module is used to use the initial coordinate value as the initial value of the Taylor series to obtain a coordinate correction value of the label through Taylor;

[0166] A third processing module, used for calculating a label error value based on the coordinate correction value of the label;

[0167] The filtering module is used to output a tag coordinate estimation value when the tag error value is less than a preset error threshold, and filter the tag coordinate estimation value through a Kalman filtering algorithm to obtain an optimal tag coordinate estimation value, and use the optimal tag coordinate estimation value as the target position.

[0168] During specific implementation, the above modules and / or units can be implemented as independent entities, or can be arbitrarily combined to be implemented as the same or several entities. The specific implementation of the above modules and / or units can refer to the previous method embodiments. The specific beneficial effects that can be achieved can also refer to the beneficial effects in the previous method embodiments, which will not be repeated here.

[0169] In addition, the embodiment of the present application also provides an electronic device, which may be a computer, a tablet computer, or other device. Figure 3 As shown, the electronic device 400 includes a processor 401 and a memory 402. The processor 401 is electrically connected to the memory 402.

[0170] The processor 401 is the control center of the electronic device 400. It uses various interfaces and lines to connect various parts of the entire electronic device. It executes various functions of the electronic device and processes data by running or loading applications stored in the memory 402 and calling data stored in the memory 402, thereby monitoring the electronic device as a whole.

[0171] In this embodiment, the processor 401 in the electronic device 400 loads instructions corresponding to the processes of one or more application programs into the memory 402 according to the following steps, and the processor 401 runs the application programs stored in the memory 402 to implement various functions:

[0172] Acquire base station distribution information in an ultra-wideband positioning system; wherein the ultra-wideband positioning system includes base stations and tags;

[0173] Constructing a TDOA positioning equation based on the base station distribution information, and performing a secondary estimation on the TDOA positioning equation through a Chan algorithm to obtain an initial coordinate value of the tag;

[0174] The initial coordinate value is used as the initial value of the Taylor series, and the coordinate correction value of the label is obtained through Taylor;

[0175] Calculating a label error value based on a coordinate correction value of the label;

[0176] When the tag error value is less than a preset error threshold, the tag coordinate estimate is output, and the tag coordinate estimate is filtered by a Kalman filter algorithm to obtain an optimal tag coordinate estimate, which is used as the target position.

[0177] The electronic device can implement the steps in any embodiment of the target positioning method for a mobile robot provided in the embodiments of the present application. Therefore, it can achieve the beneficial effects that can be achieved by any target positioning method for a mobile robot provided in the embodiments of the present invention. Please refer to the previous embodiments for details and will not be repeated here.

[0178] Figure 4 The specific structural block diagram of the electronic device provided in the embodiment of the present invention is shown, and the electronic device can be used to implement the target positioning method for a mobile robot provided in the above embodiment. The electronic device 500 can be a terminal, a server, etc., wherein the terminal can include a tablet computer, a notebook computer, a personal computer (PC, Personal Computer), a micro processing box, or other devices.

[0179] The RF circuit 510 is used to receive and send electromagnetic waves, realize the mutual conversion between electromagnetic waves and electrical signals, and thus communicate with a communication network or other devices. The RF circuit 510 may include various existing circuit elements for performing these functions, such as antennas, radio frequency transceivers, digital signal processors, encryption / decryption chips, user identity module (SIM) cards, memories, etc. The RF circuit 510 can communicate with various networks such as the Internet, corporate intranets, wireless networks, or communicate with other devices through wireless networks. The above-mentioned wireless networks may include cellular telephone networks, wireless local area networks, or metropolitan area networks. The above-mentioned wireless networks can use various communication standards, protocols and technologies, including but not limited to Global System for Mobile Communication (GSM), Enhanced Data GSM Environment (EDGE), Wideband Code Division Multiple Access (WCDMA), Code Division Multiple Access (CDMA), Time Division Multiple Access (TDMA), Wireless Fidelity (Wi-Fi) (such as Institute of Electrical and Electronics Engineers standards IEEE 802.11a, IEEE 802.11b, IEEE802.11g and / or IEEE802.11n), Voice over Internet Protocol (VoIP), Worldwide Interoperability for Microwave Access (Wi-Max), other protocols for email, instant messaging and short messages, and any other suitable communication protocols, even those that have not yet been developed.

[0180] The memory 520 can be used to store software programs and modules, such as the corresponding program instructions / modules in the above embodiments. The processor 580 executes various functional applications and data processing by running the software programs and modules stored in the memory 520, that is, realizing the functions of taking pictures with the front camera, processing the captured images, and switching the display color of the display content on the display screen. The memory 520 may include a high-speed random access memory, and may also include a non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some examples, the memory 520 may further include a memory remotely arranged relative to the processor 580, and these remote memories may be connected to the electronic device 500 via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof.

[0181] The input unit 530 may be used to receive input digital or character information, and generate a keyboard and a mouse related to user settings and function control.

[0182] The display unit 540 may be used to display information input by the user or information provided to the user and various graphical user interfaces, which may be composed of graphics, text, icons, videos, and any combination thereof. The display unit 540 may include a display panel 541, which may be configured in the form of an LCD (Liquid Crystal Display), an OLED (Organic Light-Emitting Diode), or the like.

[0183] The audio circuit 560, the speaker 561, and the microphone 562 can provide an audio interface between the user and the electronic device 500. The audio circuit 560 can transmit the electrical signal converted from the received audio data to the speaker 561, which is converted into a sound signal for output; on the other hand, the microphone 562 converts the collected sound signal into an electrical signal, which is received by the audio circuit 560 and converted into audio data, and then the audio data is output to the processor 580 for processing, and then sent to another terminal through the RF circuit 510, or the audio data is output to the memory 520 for further processing. The audio circuit 560 may also include an earplug jack to provide communication between an external headset and the electronic device 500.

[0184] The electronic device 500 can help the user receive requests, send information, etc. through the transmission module 570 (such as a Wi-Fi module), which provides the user with wireless broadband Internet access. Although the transmission module 570 is shown in the figure, it is understandable that it is not a necessary component of the electronic device 500 and can be omitted as needed without changing the essence of the invention.

[0185] The processor 580 is the control center of the electronic device 500. It uses various interfaces and lines to connect various parts of the entire mobile phone. By running or executing software programs and / or modules stored in the memory 520, and calling data stored in the memory 520, it executes various functions of the electronic device 500 and processes data, thereby monitoring the electronic device as a whole. Optionally, the processor 580 may include one or more processing cores; in some embodiments, the processor 580 may integrate an application processor and a modem processor, wherein the application processor mainly processes the operating system, user interface, and application programs, etc., and the modem processor mainly processes wireless communications. It can be understood that the above-mentioned modem processor may not be integrated into the processor 580.

[0186] The electronic device 500 also includes a power supply 590 (such as a battery) for supplying power to various components. In some embodiments, the power supply can be logically connected to the processor 580 through a power management system, so that the power management system can manage charging, discharging, and power consumption management. The power supply 590 can also include any components such as one or more DC or AC power supplies, recharging systems, power failure detection circuits, power converters or inverters, and power status indicators.

[0187] Although not shown, the electronic device 500 also includes a camera (such as a front camera, a rear camera), a Bluetooth module, etc., which will not be described in detail here. Specifically in this embodiment, the display unit of the electronic device is a touch screen display, and the mobile terminal also includes a memory, and one or more programs, wherein one or more programs are stored in the memory, and are configured to be executed by one or more processors. One or more programs include instructions for performing the following operations:

[0188] Acquire base station distribution information in an ultra-wideband positioning system; wherein the ultra-wideband positioning system includes base stations and tags;

[0189] Constructing a TDOA positioning equation based on the base station distribution information, and performing a secondary estimation on the TDOA positioning equation through a Chan algorithm to obtain an initial coordinate value of the tag;

[0190] The initial coordinate value is used as the initial value of the Taylor series, and the coordinate correction value of the label is obtained through Taylor;

[0191] Calculating a label error value based on a coordinate correction value of the label;

[0192] When the tag error value is less than a preset error threshold, the tag coordinate estimate is output, and the tag coordinate estimate is filtered by a Kalman filter algorithm to obtain an optimal tag coordinate estimate, which is used as the target position.

[0193] In specific implementation, the above modules can be implemented as independent entities, or can be arbitrarily combined and implemented as the same or several entities. The specific implementation of the above modules can be found in the previous method embodiments, which will not be repeated here.

[0194] Those skilled in the art will appreciate that all or part of the steps in the various methods of the above embodiments can be completed by instructions, or by controlling related hardware through instructions, and the instructions can be stored in a computer-readable storage medium and loaded and executed by a processor. To this end, an embodiment of the present invention provides a storage medium, which stores a plurality of instructions, which can be loaded by a processor to execute the steps of any embodiment of the target positioning method for a mobile robot provided in an embodiment of the present invention.

[0195] The computer-readable storage medium may include: a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, etc.

[0196] Since the instructions stored in the storage medium can execute the steps in any embodiment of the target positioning method for a mobile robot provided in the embodiments of the present invention, the beneficial effects that can be achieved by any target positioning method for a mobile robot provided in the embodiments of the present invention can be achieved. Please refer to the previous embodiments for details and will not be repeated here.

[0197] The above is a detailed introduction to a target positioning method, device, storage medium and electronic device for a mobile robot provided in an embodiment of the present application. Specific examples are used in this article to illustrate the principles and implementation methods of the present application. The description of the above embodiments is only used to help understand the method of the present application and its core idea; at the same time, for technical personnel in this field, according to the idea of ​​the present application, there will be changes in the specific implementation method and application scope. In summary, the content of this specification should not be understood as a limitation on the present application.

Claims

1. A target positioning method for a mobile robot, characterized in that: The method comprises: Acquire base station distribution information in an ultra-wideband positioning system; wherein the ultra-wideband positioning system includes base stations and tags; Constructing a TDOA positioning equation based on the base station distribution information, and performing a secondary estimation on the TDOA positioning equation through a Chan algorithm to obtain an initial coordinate value of the tag; The initial coordinate value is used as the initial value of the Taylor series, and the coordinate correction value of the label is obtained through Taylor; Calculating a label error value based on a coordinate correction value of the label; When the tag error value is less than a preset error threshold, the tag coordinate estimate is output, and the tag coordinate estimate is filtered by a Kalman filter algorithm to obtain an optimal tag coordinate estimate, which is used as the target position.

2. The target positioning method for a mobile robot according to claim 1, characterized in that: The step of constructing a TDOA positioning equation based on the base station distribution information and performing a secondary estimation on the TDOA positioning equation by using a Chan algorithm to obtain a coordinate value of a tag includes: Converting the TDOA positioning equation into a linear system of equations; Solving the linear equations by weighted least squares method to obtain a first weighted least squares estimate; An error equation group is established based on the first weighted least squares estimation value, and the error equation group is solved to obtain the initial coordinate value of the tag.

3. The target positioning method for a mobile robot according to claim 1, characterized in that: The initial coordinate value is used as the initial value of the Taylor series, and the coordinate correction value of the label is obtained by Taylor, including: The relationship between the base station and the tag is expressed by the function f i express; The coordinate value is used as the initial value of the Taylor series, and the function f i Performing Taylor expansion to obtain a Taylor expansion, and converting the Taylor expansion into a matrix to obtain an error vector; Solving the error vector to obtain a coordinate error value; The initial coordinate value is updated based on the coordinate error value, and the above steps are repeated. When the coordinate error value is less than a preset threshold value, the iteration is stopped to obtain the final coordinate correction value of the tag.

4. The target positioning method for a mobile robot according to claim 1, characterized in that: The filtering of the tag coordinate estimation value by a Kalman filter algorithm to obtain an optimal tag coordinate estimation value includes: The tag horizontal coordinate and the tag vertical coordinate in the tag coordinate estimation value are respectively used as state parameters, and Kalman filtering is performed on the state parameters to obtain the optimal tag coordinate estimation value.

5. The target positioning method for a mobile robot according to claim 4, characterized in that: The method of using the tag abscissa and the tag ordinate in the tag coordinate estimation value as state parameters respectively and performing Kalman filtering on the state parameters includes: Construct the equation of state: X t+1 =A*X t +oh t Among them, X t represents the state vector of the ultra-wideband positioning system at time t, X t =[x t ], x t is the label abscissa at time t in the label estimate, A represents the state transfer matrix, ω t represents the noise of the ultra-wideband positioning system; The observation equation is constructed as: Z t =HX t +u t Among them, Z t represents the observation vector, υ t is the observation noise, H is the observation matrix; Predicted State Estimates: Forecast covariance estimate: in, is the estimated value of the predicted state, B is the control input matrix, u t-1 is the input of the Kalman filter at the previous moment, is the predicted covariance estimation matrix, Q is the covariance matrix of the UWB positioning system noise; Calculate the Kalman gain: Among them, K t is the Kalman gain, R is the covariance matrix of the observation noise; Update the state estimate: Update the covariance estimate: Where I is the identity matrix.

6. The target positioning method for a mobile robot according to claim 2, characterized in that: The step of solving the linear equations by weighted least squares method to obtain a first weighted least squares estimate includes: Order Z a =[xy R1] T is the solution of the linear equations, Z is the actual position of the label a The value of Due to the noise in the TDOA positioning process, that is, GZ 0 ≠h, let the error vector be ψ, then we have the first formula: Assume Z a The elements are independent of each other, then Z a The maximum likelihood estimate of is expressed by the second formula, which is: WITH a =argmin{(hG a WITH a ) T ψ(hG a WITH a )}=(G a T ψ -1 G a ) -1 G a T ψ -1 h Where, ψ=E[ψψ T ]=C 2 BQB, Where Q is the covariance of TDOA, n is the noise vector that obeys the normal distribution; When the tag is far away from the base station and close to the actual distance, the second formula can be approximately expressed as the third formula: Z a ≈(G a T ψ -1 G a ) -1 G a T ψ -1 h When the tag is close to the base station, the third formula is used to calculate the matrix B to obtain the value of ψ, and then the values ​​of B and ψ are substituted into the second formula to obtain the first weighted least squares estimate Z a .

7. The target positioning method for a mobile robot according to claim 6, characterized in that: Establishing an error equation group based on the first weighted least squares estimate, and solving the error equation group to obtain the initial coordinate value of the tag, including: Based on the Z a The error equations are established and the second position estimation is solved. The calculation formula is as follows: in, The final initial coordinate value of the label is:

8. A target positioning device for a mobile robot, characterized in that: include: An acquisition module, used to acquire base station distribution information in an ultra-wideband positioning system; wherein the ultra-wideband positioning system includes base stations and tags; A first processing module is used to construct a TDOA positioning equation based on the base station distribution information, and perform a secondary estimation on the TDOA positioning equation through a Chan algorithm to obtain an initial coordinate value of the tag; A second processing module is used to use the initial coordinate value as the initial value of the Taylor series to obtain a coordinate correction value of the label through Taylor; A third processing module, used for calculating a label error value based on the coordinate correction value of the label; The filtering module is used to output a tag coordinate estimation value when the tag error value is less than a preset error threshold, and filter the tag coordinate estimation value through a Kalman filtering algorithm to obtain an optimal tag coordinate estimation value, and use the optimal tag coordinate estimation value as the target position.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a plurality of instructions, and the instructions are suitable for being loaded by a processor to execute the target positioning method for a mobile robot according to any one of claims 1 to 7.

10. An electronic device, characterized in that: It comprises a processor and a memory, the processor is electrically connected to the memory, the memory is used to store instructions and data, and the processor is used to execute the steps in the target positioning method for a mobile robot as described in any one of claims 1 to 7.

Images