Ultra wide band and inertial navigation fused indoor positioning system and method

Through the indoor positioning system that integrates ultra-wideband and inertial guide, combined with ultra-wideband, inertial measurement unit and odometer, the extended Kalman filtering technology is used to solve the problem of low positioning accuracy of indoor robots, and long-term high-precision positioning and low positioning are achieved Cost positioning effect.

CN119022932APending Publication Date: 2024-11-26CHENGDU UNIV OF INFORMATION TECH +2
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202411145632.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-20
Publication Date
2024-11-26

AI Technical Summary

Technical Problem

In the prior art, when positioning a mobile robot in an indoor environment, the positioning accuracy of a long time cannot be guaranteed due to the accumulated time error of the inertial measurement unit and the slippage of the wheel speedometer.

Method used

The indoor positioning system that combines ultra-wideband and inertial guides, combines ultra-wideband unit, inertial measurement unit and odometer unit, integrates the data of the three through extended Kalman filtering technology, and utilizes the high precision positioning of ultra-wideband and the high inertial measurement unit. Frequency characteristics and position constraints of wheel odometers eliminate cumulative errors and achieve long-term precise positioning.

Benefits of technology

It improves the accuracy and robustness of the positioning system, ensures long-term positioning accuracy, and realizes a low-cost positioning solution.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119022932A_ABST
    Figure CN119022932A_ABST
Patent Text Reader

Abstract

The invention discloses an ultra wide band and inertial navigation fused indoor positioning system and method, and relates to the technical field of positioning, and the system comprises an ultra wide band unit UWB which is used for configuring a base station and positioning a first coordinate of a movable target based on an ultra wide band signal of the base station; the inertial measurement unit IMU is used for positioning a second coordinate of the movable target by adopting the inertial measurement unit IMU; the odometer unit ODOM is used for obtaining the calibrated acceleration and the calibrated angular velocity of the movable target by adopting a wheel type odometer; and the Kalman unit EKF is used for carrying out fusion positioning on the first coordinate, the second coordinate, the calibration acceleration and the calibration angular velocity based on extended Kalman filtering to obtain a third coordinate of the movable target, and the problem that the positioning precision cannot be guaranteed under the long-time condition in the prior art can be solved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to an indoor positioning system and method integrating ultra-wideband and inertial navigation. Background Art

[0002] Kalman Filter (KF) is a practical technique for estimating the state of a system. It combines the predictions of the system model and the measurements from the sensors to produce an optimal estimate of the current state of the system. Extended Kalman Filter (EKF) is an extension of Kalman Filter that is applicable to nonlinear systems. In EKF, the system model and the measurement model can be nonlinear. By linearly approximating them, EKF is able to perform state estimation in nonlinear systems.

[0003] With the development of science and technology and the progress of life, there are more and more mobile robots in industry and even in life. Mobile robots must establish environmental perception and positioning when moving in highly complex indoor environments. Mobile robots must rely on positioning technology to obtain their own posture and position. Currently, positioning technology usually only uses a tachometer and an inertial measurement unit (IMU) for combined navigation. However, this technology cannot guarantee long-term accurate positioning due to the characteristic of IMU's cumulative error over time. Even if the tachometer provides speed and position constraints, it cannot completely eliminate the cumulative error due to factors such as slippage, and thus cannot guarantee positioning accuracy over a long period of time. Summary of the invention

[0004] In order to solve the problem that the prior art cannot guarantee the positioning accuracy under long-term conditions, the present invention provides an indoor positioning system integrating ultra-wideband and inertial navigation, the system comprising:

[0005] Ultra-wideband unit UWB: used to configure a base station and locate a first coordinate of a movable target based on an ultra-wideband signal of the base station;

[0006] Inertial measurement unit IMU: used to locate the second coordinate of the movable target using an inertial measurement unit;

[0007] The odometer unit ODOM is used to obtain the calibrated acceleration and calibrated angular velocity of the movable target using a wheel odometer;

[0008] Kalman unit EKF: used for fusing the first coordinate, the second coordinate, the calibrated acceleration and the calibrated angular velocity to obtain the third coordinate of the movable target based on the extended Kalman filter.

[0009] Ultra-wideband (UWB) positioning technology is a method of using broadband radio waves to determine position, which can achieve high-precision distance measurement and positioning. UWB has the characteristics of high precision and low latency, requires the deployment of dedicated base stations, does not have drift, and has low cost; Inertial Measurement Unit (IMU), including accelerometers, gyroscopes and other sensors to track the movement and posture changes of the device, can be used in conjunction with other positioning systems to improve positioning accuracy and reduce drift; Wheel speed meter positioning, commonly used in robots, calculates the moving path by measuring the distance and speed of wheel rotation.

[0010] IMU and wheel tachometer have the advantages of high frequency and low cost, but will drift over time, while UWB will not drift over time, but has the disadvantage of low frequency; utilizing the positioning advantages of UWB and the high frequency characteristics of IMU and wheel tachometer, the EKF technology is adopted to integrate the three, combining the complementary advantages, using the angular velocity and acceleration information output by the IMU to update the state of the movable target system, using the position output by the wheel odometer to constrain the system state, and then using UWB positioning to eliminate the accumulated error caused by IMU drift and wheel odometer slippage, which can ensure long-term positioning accuracy, increase the accuracy and robustness of the positioning system, and ensure long-term positioning accuracy. At the same time, UWB only needs to deploy base stations, which is relatively low in cost. When used in combination with IMU and wheel odometer, low-cost positioning can be achieved.

[0011] Time of Arrival (TOA) measurement method: generally adopts a bilateral two-way ranging algorithm to measure the precise time when the signal is transmitted from the transmitter to the receiver. By calculating the signal propagation time and the known signal propagation speed (speed of light), the distance between the transmitter and the receiver can be accurately calculated. Therefore, the present invention adopts this measurement method as the UWB positioning strategy, which has the advantages of low debugging cost and high accuracy.

[0012] Furthermore, the ultra-wideband unit UWB is specifically used to: configure the base station and obtain the base station coordinates of the base station; the base station transmits the ultra-wideband signal to the movable target, obtains the transmission time of the ultra-wideband signal to the movable target, and obtains the distance between the ultra-wideband base station and the movable target based on the transmission time; and obtains the first coordinate based on the distance and the base station coordinates.

[0013] Since the wheel speed sensor only feeds back the angular velocity of the left and right wheels of the movable target, calculating the coordinates of the movable target from the angular velocity feedback value requires establishing a motion model for it in order to obtain more accurate coordinates.

[0014] Furthermore, the inertial measurement unit IMU is specifically used to: construct a motion model of the movable target and configure a wheel speed meter sensor, obtain first data of the movable target based on the motion modeling, and obtain second data based on the wheel speed meter sensor, the first data including a first width W1, a radius R and a second width W2 of a mobile device of the movable target, and the second data including a first angular velocity ω1 and a second angular velocity ω2 of the mobile device; obtain the second coordinate based on the first data and the second data.

[0015] Since there are errors in the installation of accelerometers and gyroscopes, the acceleration and angular velocity of the X, Y and Z axes need to be calibrated. The three-axis acceleration data is collected and fitted into an ellipse. The higher the accuracy of the ellipse fitting, the more accurate the acceleration scale error factor and the acceleration zero bias factor. The angular velocity is calculated by the angular velocity integral value. Due to the existence of zero drift, the angular velocity integral value will increase with the accumulation of time. This is also the reason for the cumulative error of inertial navigation. The zero bias factor is calculated by the long-term integral value and compensated on the original measurement value to obtain more accurate data.

[0016] Furthermore, the odometer unit ODOM is specifically used to: configure the accelerometer and the gyroscope of the movable target, and obtain the measured acceleration and the measured angular velocity of the movable target based on the accelerometer and the gyroscope, respectively; perform ellipse fitting on the measured acceleration to obtain the acceleration scale error factor and the acceleration zero bias factor; integrate the measured angular velocity to obtain the angular velocity scale error factor and the angular velocity zero bias factor; obtain the calibrated acceleration based on the measured acceleration, the acceleration scale error factor and the acceleration zero bias factor; obtain the calibrated angular velocity based on the measured angular velocity, the angular velocity scale error factor and the angular velocity zero bias factor.

[0017] Furthermore, the Kalman unit EKF specifically includes:

[0018] Initialization unit: used to initialize system parameters, the system parameters include initial system state estimation vector X, initial state covariance matrix P, initial process noise covariance matrix Q1 and initial measurement noise covariance matrix Q2;

[0019] UWB unit: used to measure the relative transformation of the first coordinate between time k and time k-1 to obtain first data; extract the transformation matrix of the first data to obtain second data; correct the second data to obtain third data; obtain a first noise covariance matrix based on the third data;

[0020] IMU unit: used to measure the relative transformation of the calibrated acceleration and the calibrated angular velocity between time k and time k-1 to obtain fourth data, where k is an integer greater than or equal to 2; extract rotation data of the fourth data, correct the rotation data, and obtain fifth data; obtain a second noise covariance matrix based on the fifth data;

[0021] ODOM unit: used for measuring the relative transformation of the second coordinate between time k and time k-1 to obtain sixth data; extracting the transformation matrix of the sixth data to obtain seventh data; correcting the seventh data to obtain eighth data; and obtaining a third noise covariance matrix based on the eighth data;

[0022] Prediction unit: used to obtain the first system state at time k-1 based on the initialization system parameters; predict the second system state at time k based on the first system state and the system state transfer equation; obtain the state covariance matrix P based on the second system state, the first noise covariance matrix, the second noise covariance matrix and the third noise covariance matrix k ; Based on the state covariance matrix P k The third coordinate is obtained.

[0023] Furthermore, the EKF unit further includes an updating unit, which is used to:

[0024] Based on the state covariance matrix P k Get the Kalman gain K k Based on the Kalman gain K k and the state covariance matrix P k Get the first state covariance matrix P k1 ;

[0025] Based on the first state covariance matrix P k1 A fourth coordinate is obtained, and the third coordinate is updated to the fourth coordinate.

[0026] Furthermore, the system further comprises a control unit, wherein the control unit is configured to:

[0027] configuring a signal transmitter, wherein the signal transmitter transmits a control command to the movable target, and parses the control command to obtain a preset acceleration and a preset angular velocity;

[0028] The measured acceleration and the measured angular velocity are updated to the preset acceleration and the preset angular velocity respectively.

[0029] Furthermore, a first calculation method for obtaining the second system state is:

[0030] X k=f(X k-1 ,u k );

[0031]

[0032] Among them, X k represents the system state at time k, f() represents the system state transition equation, X k-1 represents the system state at time k-1, u k represents the system input matrix, P k represents the state covariance matrix at time k, F k represents the Jacobian matrix, P k-1 represents the state covariance matrix at time k-1, Q k represents the noise covariance matrix at time k, k represents an integer greater than or equal to 2, and T represents transpose.

[0033] Further, the first state covariance matrix P is obtained k1 The second calculation method is:

[0034] P k1 =(IK k H k ) k ;

[0035] Among them, P k1 represents the updated state covariance matrix at time k, I represents the identity matrix, K k represents the Kalman gain, H k represents the measurement Jacobian matrix, P k Represents the state covariance matrix at time k before the update.

[0036] The present invention also provides an indoor positioning method integrating ultra-wideband and inertial navigation, the method comprising:

[0037] A base station is configured to locate a first coordinate of a movable target based on an ultra-wideband signal of the base station; an inertial measurement unit is used to locate a second coordinate of the movable target; a wheel odometer is used to obtain a calibrated acceleration and a calibrated angular velocity of the movable target; and based on an extended Kalman filter, the first coordinate, the second coordinate, the calibrated acceleration and the calibrated angular velocity are fused and located to obtain a third coordinate of the movable target.

[0038] The principle and effect of this method are similar to those of this system, and this method will not be described in detail.

[0039] One or more technical solutions provided by the present invention have at least the following technical effects or advantages:

[0040] 1. Taking advantage of the positioning advantages of UWB and the high frequency characteristics of IMU and wheel speed meter, the EKF technology fusion algorithm is used to integrate the three, combining their complementary advantages, using the angular velocity and acceleration information output by the IMU to update the state of the movable target system, using the position output by the wheel odometer to constrain the system state, and then using UWB positioning to eliminate the accumulated error caused by IMU drift and wheel odometer slippage, which can ensure long-term positioning accuracy, increase the accuracy and robustness of the positioning system, and achieve low-cost positioning.

[0041] 2. The arrival bilateral two-way ranging algorithm is used as the UWB positioning strategy to measure the precise time for the signal to be transmitted from the base station to the movable target. Through its time and the known signal propagation speed, the distance between the two is accurately calculated, thereby accurately positioning, and it has the advantages of low debugging cost and high accuracy.

[0042] 3. Construct a motion model of the movable target and configure a wheel speed sensor to calculate more accurate coordinates of the movable target from the angular velocity feedback value.

[0043] 4. Configure the accelerometer and gyroscope of the movable target, collect acceleration data, fit it into an ellipse, and obtain a more accurate acceleration scale error factor and acceleration zero bias factor; calculate the zero bias factor through the angular velocity integral value, and compensate it on the original measurement value to obtain a more accurate angular velocity. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] The drawings described herein are used to provide a further understanding of the embodiments of the present invention, constitute a part of the present invention, and do not constitute a limitation on the embodiments of the present invention;

[0045] Figure 1 It is a flow chart of an indoor positioning system integrating ultra-wideband and inertial navigation in the present invention;

[0046] Figure 2 It is a schematic diagram of a bilateral two-way ranging process of an indoor positioning system integrating ultra-wideband and inertial navigation in the present invention;

[0047] Figure 3 It is a schematic diagram of a motion model of a movable target in an indoor positioning system integrating ultra-wideband and inertial navigation in the present invention;

[0048] Figure 4 It is a schematic diagram of an ellipse fitting of an indoor positioning system integrating ultra-wideband and inertial navigation in the present invention. DETAILED DESCRIPTION

[0049] In order to more clearly understand the above-mentioned purpose, features and advantages of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict.

[0050] In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those within the scope of this description. Therefore, the protection scope of the present invention is not limited to the specific embodiments disclosed below.

[0051] Embodiment 1

[0052] refer to Figure 1 This embodiment provides an indoor positioning system integrating ultra-wideband and inertial navigation, the system comprising:

[0053] Ultra-wideband unit UWB: used to configure a base station, and locate the first coordinate of a movable target based on the ultra-wideband signal of the base station; the ultra-wideband unit UWB is specifically used to: configure the base station, and obtain the base station coordinates of the base station; the base station transmits the ultra-wideband signal to the movable target, obtains the transmission time of the ultra-wideband signal to the movable target, and obtains the distance between the ultra-wideband base station and the movable target based on the transmission time; obtains the first coordinate based on the distance and the base station coordinates. In this embodiment, the movable target can be a mobile robot or a mobile positioning device.

[0054] Since UWB carries measurement noise, filters can be used to fuse the data and the least squares method can be used to obtain an approximate solution to obtain more accurate data calculation coordinates.

[0055] Inertial measurement unit IMU: used to locate the second coordinate of the movable target using an inertial measurement unit; the inertial measurement unit IMU is specifically used to: construct a motion model of the movable target and configure a wheel speed sensor, obtain first data of the movable target based on the motion modeling, obtain second data based on the wheel speed sensor, the first data includes a first width W1, a radius R and a second width W2 of the movable target's mobile device, the second data includes a first angular velocity ω1 and a second angular velocity ω2 of the mobile device; obtain the second coordinate based on the first data and the second data. In this embodiment, the mobile device may be a tire or a conveyor belt, which may be located at the bottom of the movable target.

[0056] Odometer unit ODOM: used to obtain the calibrated acceleration and calibrated angular velocity of the movable target by using a wheel odometer; the odometer unit ODOM is specifically used to: configure the accelerometer and gyroscope of the movable target, and obtain the measured acceleration and measured angular velocity of the movable target based on the accelerometer and the gyroscope respectively; perform ellipse fitting on the measured acceleration to obtain an acceleration scale error factor and an acceleration zero bias factor; integrate the measured angular velocity to obtain an angular velocity scale error factor and an angular velocity zero bias factor; obtain the calibrated acceleration based on the measured acceleration, the acceleration scale error factor and the acceleration zero bias factor; obtain the calibrated angular velocity based on the measured angular velocity, the angular velocity scale error factor and the angular velocity zero bias factor.

[0057] Kalman unit EKF: used for fusing the first coordinate, the second coordinate, the calibrated acceleration and the calibrated angular velocity to obtain the third coordinate of the movable target based on the extended Kalman filter.

[0058] The Kalman unit EKF specifically includes:

[0059] Initialization unit: used to initialize system parameters, the system parameters include initial system state estimation vector X, initial state covariance matrix P, initial process noise covariance matrix Q1 and initial measurement noise covariance matrix Q2;

[0060] UWB unit: used to measure the relative transformation of the first coordinate between time k and time k-1 to obtain first data; extract the transformation matrix of the first data to obtain second data; correct the second data to obtain third data; obtain a first noise covariance matrix based on the third data;

[0061] IMU unit: used to measure the relative transformation of the calibrated acceleration and the calibrated angular velocity between time k and time k-1 to obtain fourth data, where k is an integer greater than or equal to 2; extract rotation data of the fourth data, correct the rotation data, and obtain fifth data; obtain a second noise covariance matrix based on the fifth data;

[0062] ODOM unit: used for measuring the relative transformation of the second coordinate between time k and time k-1 to obtain sixth data; extracting the transformation matrix of the sixth data to obtain seventh data; correcting the seventh data to obtain eighth data; and obtaining a third noise covariance matrix based on the eighth data;

[0063] Prediction unit: used to obtain the first system state at time k-1 based on the initialization system parameters; predict the second system state at time k based on the first system state and the system state transfer equation; obtain the state covariance matrix P based on the second system state, the first noise covariance matrix, the second noise covariance matrix and the third noise covariance matrix k ; Based on the state covariance matrix P k The third coordinate is obtained.

[0064] The EKF unit further comprises an updating unit, which is used to:

[0065] Based on the state covariance matrix P k Get the Kalman gain K k Based on the Kalman gain K k and the state covariance matrix P k Get the first state covariance matrix P k1 ;

[0066] Based on the first state covariance matrix P k1 A fourth coordinate is obtained, and the third coordinate is updated to the fourth coordinate.

[0067] Embodiment 2

[0068] On the basis of the first embodiment, in this embodiment, the system further comprises a control unit, the control unit being used to: configure a signal transmitter, the signal transmitter transmitting a control command to the movable target, parsing the control command to obtain a preset acceleration and a preset angular velocity; and updating the measured acceleration and the measured angular velocity to the preset acceleration and the preset angular velocity, respectively. In this embodiment, the signal transmitter may be a remote control or a computer, etc.

[0069] Embodiment 3

[0070] refer to Figure 2 Based on the above embodiment, a first calculation method for obtaining the second system state is:

[0071] X k =f(X k-1 ,u k );

[0072]

[0073] Among them, X k represents the system state at time k, f() represents the system state transition equation, X k-1 represents the system state at time k-1, u k represents the system input matrix, P krepresents the state covariance matrix at time k, F k represents the Jacobian matrix, P k-1 represents the state covariance matrix at time k-1, F k-1 represents the Jacobian matrix at time k-1, Q k represents the noise covariance matrix at time k, k represents an integer greater than or equal to 2, and T represents transpose.

[0074] Get the first state covariance matrix P k1 The second calculation method is:

[0075] P k1 =(IK k H k ) k ;

[0076] Among them, P k1 represents the updated state covariance matrix at time k, I represents the identity matrix, K k represents the Kalman gain, H k represents the measurement Jacobian matrix, P k Represents the state covariance matrix at time k before the update.

[0077] Embodiment 4

[0078] refer to Figure 2 Based on the above embodiment, in this embodiment, a bilateral two-way ranging algorithm is used to obtain the transmission time of the ultra-wideband signal to the movable target. Figure 2 In the example, UWB ANCHOR represents the base station, and UWB TAG represents the movable target. The base station sends the request signal POLL at time T1, and the movable target receives it at time T2. The duration is T flight , the mobile target response time is T replyb , a response signal RESPONE is sent to the base station at time T3, the base station receives it at T4, a feedback signal FINALL is sent at time T5, the movable target receives it at T6, and the time difference from the base station sending the request signal POLL to the base station receiving the feedback signal RESPONE is a complete T rouunda The time difference from the movable target sending the response RESPONE to receiving the feedback signal FINALL is a complete T rouundb The time difference between receiving data and sending a response signal is T replyb , the distance from the base station to the movable target can be calculated according to the following formula:

[0079]

[0080] DIstance=C light ×T flight ;

[0081] Where Distance represents the distance, C light Indicates the speed at which a signal travels.

[0082] Embodiment 5

[0083] refer to Figure 3 Based on the above embodiment, in this embodiment, the motion model of the movable target can refer to Figure 3 , where the angular velocities of the left and right wheels are ω left and ω right , width W wheel , with a radius of R wheel The linear velocity is v x , the robot rotates clockwise at an angular velocity of ω z , the length of the vehicle is L, the width is W, and O represents a point. The coordinates can be calculated according to the following formula:

[0084]

[0085] Among them, x k ,y k ,θ k They represent the XYZ coordinates at time k, respectively. k-1 ,y k-1 ,θ k-1 They represent the XYZ coordinates at time k-1, ω k represents the angular velocity at time k, ω k-1 It represents the angular velocity at time k-1. Since there is some noise in the angular velocity feedback, it is processed by the mean interpolation algorithm. represents the angular velocity after processing, represents the angular velocity of the left wheel after processing, represents the angular velocity of the right wheel after processing, and t represents time.

[0086] Embodiment 6

[0087] Based on the above embodiment, in this embodiment, three-axis accelerometers and three-axis gyroscopes are installed in a chip, and the chip is installed on a movable target to measure the acceleration and angular velocity of the XYZ axes. An IMU model is constructed, and the acceleration and angular velocity are calibrated based on the IMU model. The IMU model can be as follows:

[0088]

[0089] Where [a x a y a z ] and [w x w yw z ] are the ideal values ​​of measured acceleration and angular velocity, and are the measured acceleration and angular velocity values, and is the zeroing factor, and is the scale factor matrix, and It represents the scaled value of the measured value and the actual value under the XYZ three-axis. The others are the axis rotation parameters caused by the internal axis error of the IMU, which are called axis error factors. However, since the axis error factor has little effect on the plane, the axis error is ignored and can be written as follows:

[0090]

[0091] By changing the posture of the IMU model in space multiple times, the three-axis acceleration of the IMU is collected and plotted in space, and the scale error factor and zero bias factor are obtained by fitting the ellipsoid, such as Figure 4 As shown, the acceleration scale error factor and acceleration zero bias factor are calculated according to the following formula:

[0092]

[0093] Among them, O x ,,O y , O z are the coordinates of the center of the ellipsoid, corresponding to the acceleration R x , R y , R z are the length of each axis of the ellipsoid, corresponding to the acceleration

[0094] The angular velocity is then calculated using the integral value of the angular velocity. Due to the existence of zero drift, the integral value of the angular velocity will increase over time, which is also the reason for the cumulative error of inertial navigation. Place the IMU statically along the Z axis under the positive and negative vectors of unit gravity, and calculate the zero bias b using the long-term integral value according to the following formula: gz , and the angular velocity scale error factor and angular velocity zero bias factor are compensated and calculated on the original acceleration measurement value:

[0095] ω z1 =b gz t+(1+δg az )ω z ;

[0096] ω z2 =b gz t-(1+δg az )ω z ;

[0097]

[0098] Among them, δ represents the relevant known parameters.

[0099] Embodiment 7

[0100] On the basis of the above embodiment, in this embodiment, assuming that the current moment is k and the previous moment is k-1, the EKF is divided into two steps: initialization and iteration;

[0101] Initialization: Initialize the initial system state estimation vector X, the initial state covariance matrix P, the initial process noise covariance matrix Q1 and the initial measurement noise covariance matrix Q2. The initial process noise covariance matrix Q1 and the initial measurement noise covariance matrix Q2 are set according to the specific sensor. The system state vector formula can be:

[0102]

[0103] Among them, x k ,y k ,θ k represents the XYZ coordinates at time k, Q1 and Q2 represent the initial process noise covariance matrix Q1 and the initial measurement noise covariance matrix Q2, respectively. k represents the system state estimation vector at time k;

[0104] The iteration step is divided into prediction step and update step;

[0105] Prediction step: Use the system state equation and the estimated state of the system at the previous moment to estimate the system state at the current moment. The system state equation can be:

[0106] X k =f(X k-1 ,u k );

[0107]

[0108] Among them, X k represents the system state at time k, f() represents the system state transition equation, X k-1 represents the system state at time k-1, u k represents the system input matrix, P k represents the state covariance matrix at time k, F k represents the Jacobian matrix, P k-1 represents the state covariance matrix at time k-1, Q k represents the noise covariance matrix at time k, k represents an integer greater than or equal to 2, and T represents transpose.

[0109] State covariance matrix P k The proportion of uncertain signals in the system is estimated, and its diagonal elements represent the independent estimated variance of each state variable. The larger the variance, the higher the estimated uncertainty of the state variable; the off-diagonal elements represent the covariance between the state variables, which can provide information about the correlation between the two state variables. If the covariance between the two state variables is positive, then when one variable increases, the other variable also tends to increase; if it is negative, when one increases, the other tends to decrease. The state covariance matrix introduces the noise covariance matrix Q k , the noise covariance matrix represents the uncertainty between state variables. In real-world dynamic systems, the model cannot fully and accurately describe the behavior of the system, which is usually affected by various random disturbances or noise. The noise covariance matrix is ​​used to represent these unpredictable disturbances, which may come from measurement errors, external disturbances, parameter uncertainties, etc., and the noise covariance matrix is ​​introduced to better estimate the disturbances in the system.

[0110] Update step: First, the Kalman gain K needs to be calculated k , which is used to determine how much weight the observation data should take in the state estimation process. The Kalman gain determines the system's trust in the state estimation matrix and is calculated by the system uncertainty (state covariance) and measurement uncertainty (measurement covariance). k is the measurement Jacobian matrix, which is used to transform the state variables in the state space model into the observation space. k is the noise covariance matrix of the measurement at time k. If the measurement noise of a sensor is larger, the diagonal elements of the matrix calculated in the end will be larger, and the uncertainty of its signal estimation will be larger. If the value is set too large or too small, it may lead to poor filter performance. If it is too large, it may lead to the neglect of the measurement data, and if it is too small, the filter may be too sensitive to noise. It can be calculated in the following way:

[0111]

[0112] P k1 =(IK k H k ) k ;

[0113] Among them, P k1 represents the updated state covariance matrix at time k, I represents the identity matrix, K k represents the Kalman gain, H k represents the measurement Jacobian matrix, P k represents the state covariance matrix at time k before the update, H k represents the measurement Jacobian matrix, Kk represents the Kalman gain, R k represents the measurement noise covariance matrix at time k.

[0114] Embodiment 8

[0115] Based on the above embodiment, this embodiment further provides an indoor positioning method integrating ultra-wideband and inertial navigation, the method comprising:

[0116] A base station is configured to locate a first coordinate of a movable target based on an ultra-wideband signal of the base station; an inertial measurement unit is used to locate a second coordinate of the movable target; a wheel odometer is used to obtain a calibrated acceleration and a calibrated angular velocity of the movable target; and based on an extended Kalman filter, the first coordinate, the second coordinate, the calibrated acceleration and the calibrated angular velocity are fused and located to obtain a third coordinate of the movable target.

[0117] Embodiment 9

[0118] On the basis of the above embodiments, in this embodiment, when the movable target is used for indoor cleaning, it can usually only be manually set by the user to clean a specific area or manually control the cleaning, and the movable target can be customized to clean the entire area; for dirty and stubborn stains that cannot be cleaned at one time, and in areas with dense furniture or areas that the movable target cannot reach due to narrow passages, it is difficult to clean, and it is difficult for the user to know. For the above problems, the system also includes a detection unit, which is used to:

[0119] The movable target is configured with a collection device, a sensor device and a cleaning device, wherein the collection device obtains a first image of the target area, obtains a stain area based on the first image, and obtains a first proportion of the stain area to the first image; determines whether the first proportion is greater than a preset proportion, and if so, marks the target area; after the cleaning device finishes cleaning the target area, obtains a second image of the target area, and updates the first image to the second image; if not, cleans the next area;

[0120] The first image is processed to obtain a line image, a first distance between two adjacent lines is obtained, and it is determined whether the first distance is greater than a preset distance. If so, the coordinates of the lines are obtained, and the first image is marked based on the coordinates to obtain a third image, and the third image is transmitted to the user terminal.

[0121] In this embodiment, the acquisition device can be a camera, and the sensor device can include a drop sensor: used to detect the height of the ground to avoid the danger of falling; a collision sensor: when colliding with an object, the sensor will detect the collision force and change the direction of movement to avoid further collision; an infrared sensor: used to detect walls and other obstacles to avoid collisions; a distance sensor: used to measure the distance between the robot and the ground through infrared, laser and other technologies to determine the characteristics and obstacles of the ground. The cleaning device can include a rotating brush: through a rapidly rotating brush, the dust and debris on the ground are swept into the dust box; a vacuum cleaner: the robot has a built-in vacuum cleaner, which sucks dust and debris into the dust box through negative pressure and prevents them from scattering again; a mop device that can wipe the ground while cleaning to provide a more thorough cleaning effect, etc.

[0122] Wet mopping function: Some sweeping robots also have a wet mopping function, which can add water and wet wipes to wipe the stains on the ground to further improve the cleaning effect.

[0123] Embodiment 10

[0124] On the basis of the above embodiments, in this embodiment, the environment of captive musk deer needs to be checked regularly, but musk deer are timid and easily frightened, so irrelevant personnel should be avoided as much as possible to reduce contact with humans. In order to ensure the health of musk deer, remote operation is used to detect and observe musk deer. This embodiment uses a movable target for musk deer breeding scenarios, which can flexibly avoid musk deer, and its operating noise is lower than the normal walking noise of humans.

[0125] LIMO comes from Songling Robotics. It is the world's first ROS development platform that integrates four motion modes. It can realize four motion modes: four-wheel differential, Ackerman, crawler, and Mecanum wheels, and supports the creative design of robots to the greatest extent. LIMO uses Jetson Nano as the main control system and ROS as the core software environment. It supports a variety of applications such as SLAM, autonomous navigation, and image recognition. It is also equipped with a professional simulation sandbox. In this embodiment, the movable target selects the LIMO robot, which has a stable travel speed and will not scare the musk deer.

[0126] In this embodiment, the LIMO robot is equipped with sensor devices such as odometer, inertial navigation, laser radar and ultra-wideband (UWB) board for positioning. The UWB board is divided into UWB base station and UWB beacon, which only differ in name and deployment location. The UWB beacon is carried by the mobile robot, and the UWB base station (more than 3) is deployed indoors. The UWB board uses the DW1000 chip, and the main control chip uses the STM32F103C8T6. The two communicate through the serial peripheral interface (SPI). The UWB base station and the UWB beacon communicate through UWB signals. The UWB beacon receives the signal from the UWB base station and transmits it to the main control chip through SPI. The main control chip transmits it to the LIMO robot equipped with the beacon through the serial port.

[0127] Odometer unit ODOM: obtains the angular velocity of the left and right wheels, establishes a differential robot kinematic model to solve the robot position, and updates the position equation after processing the noise caused by the angular velocity feedback through the mean interpolation method, that is, the coordinate formula in the fifth embodiment;

[0128] Inertial Measurement Unit (IMU): Inertial navigation units may produce drift, cumulative error and other errors. By calibrating them, the errors can be reduced to a certain extent, and a more accurate positioning effect can be obtained. The internal parts of the inertial navigation unit are generally three-axis accelerometers and gyroscopes, which need to be calibrated separately. The ellipsoid is used to fit the acceleration data, and the angle is calculated by integrating the angular velocity to obtain the cumulative error, which is compensated in subsequent calculations.

[0129] Ultra-wideband unit UWB: uses the time-of-arrival method to calculate the position of the UWB beacon;

[0130] Kalman unit EKF: The inertial navigation unit can directly obtain data, calibrate it to ensure data accuracy, and then fuse the position information obtained by the odometer, the information obtained by the inertial navigation unit and the position information obtained by UWB through extended Kalman;

[0131] In this embodiment, it also includes:

[0132] LiDAR unit: used to build scene maps by scanning the surrounding environment and measuring the distance of objects, identifying feature points in the environment, and feature matching and positioning updates in mapping algorithms.

[0133] Fusion unit: fuses the data from the lidar unit with the data from the inertial navigation unit to build a more accurate indoor map;

[0134] The specific steps include: the LIMO robot uses sensors such as laser radar and inertial navigation unit to collect environmental data and its own motion data, extracts environmental feature points from the laser radar data, and uses the inertial navigation unit data to estimate the robot's motion state, and fuses the laser radar and inertial navigation unit data to obtain comprehensive positioning and mapping information. The robot's positioning is updated based on the fused data to determine its position in the map, and the environmental map is updated based on the new laser radar data and inertial navigation unit data, including information such as obstacle locations and robot trajectories.

[0135] UWB positioning has the characteristics of high precision and low frequency. In this embodiment, UWB is combined with an odometer and inertial navigation with high frequency characteristics to improve the problem of low UWB positioning frequency, so that the LIMO robot has the characteristics of high positioning accuracy and fast signal update, avoiding direct contact between the robot and the musk deer; the inertial navigation unit can provide the robot's acceleration and angular velocity information for estimating the robot's motion state, which can help compensate for the drift error of the lidar sensor and improve the accuracy and stability of positioning and mapping.

[0136] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0137] 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. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. An indoor positioning system integrating ultra-wideband and inertial navigation, characterized in that: The system comprises: Ultra-wideband unit UWB: used to configure a base station and locate a first coordinate of a movable target based on an ultra-wideband signal of the base station; Inertial measurement unit IMU: used to locate the second coordinate of the movable target using an inertial measurement unit; The odometer unit ODOM is used to obtain the calibrated acceleration and calibrated angular velocity of the movable target using a wheel odometer; Kalman unit EKF: used for fusing the first coordinate, the second coordinate, the calibrated acceleration and the calibrated angular velocity to obtain the third coordinate of the movable target based on the extended Kalman filter.

2. The indoor positioning system integrating ultra-wideband and inertial navigation according to claim 1, characterized in that: Ultra-wideband unit UWB is specifically used for: The base station is configured to obtain the base station coordinates of the base station; the base station transmits the ultra-wideband signal to the movable target, obtains the transmission time of the ultra-wideband signal to the movable target, and obtains the distance between the ultra-wideband base station and the movable target based on the transmission time; and the first coordinates are obtained based on the distance and the base station coordinates.

3. The indoor positioning system integrating ultra-wideband and inertial navigation according to claim 1, characterized in that: The inertial measurement unit IMU is specifically used for: Constructing a motion model of the movable target and configuring a wheel speed sensor, acquiring first data of the movable target based on the motion modeling, and acquiring second data based on the wheel speed sensor, wherein the first data includes a first width W1, a radius R, and a second width W2 of a moving device of the movable target, and the second data includes a first angular velocity ω1 and a second angular velocity ω2 of the moving device; The second coordinates are obtained based on the first data and the second data.

4. The indoor positioning system integrating ultra-wideband and inertial navigation according to claim 1, characterized in that: The odometer unit ODOM is specifically used for: configuring an accelerometer and a gyroscope of the movable object, and obtaining a measured acceleration and a measured angular velocity of the movable object based on the accelerometer and the gyroscope, respectively; Performing ellipse fitting on the measured acceleration to obtain an acceleration scale error factor and an acceleration zero bias factor; Integrate the measured angular velocity to obtain an angular velocity scale error factor and an angular velocity zero bias factor; Obtaining the calibrated acceleration based on the measured acceleration, the acceleration scale error factor and the acceleration zero bias factor; The calibrated angular velocity is obtained based on the measured angular velocity, the angular velocity scale error factor and the angular velocity zero bias factor.

5. The indoor positioning system integrating ultra-wideband and inertial navigation according to claim 1, characterized in that: The Kalman unit EKF specifically includes: Initialization unit: used to initialize system parameters, the system parameters include initial system state estimation vector X, initial state covariance matrix P, initial process noise covariance matrix Q1 and initial measurement noise covariance matrix Q2; UWB unit: used to measure the relative transformation of the first coordinate between time k and time k-1 to obtain first data; extract the transformation matrix of the first data to obtain second data; correct the second data to obtain third data; obtain a first noise covariance matrix based on the third data; IMU unit: used to measure the relative transformation of the calibrated acceleration and the calibrated angular velocity between time k and time k-1 to obtain fourth data, where k is an integer greater than or equal to 2; extract rotation data of the fourth data, correct the rotation data, and obtain fifth data; obtain a second noise covariance matrix based on the fifth data; ODOM unit: used for measuring the relative transformation of the second coordinate between time k and time k-1 to obtain sixth data; extracting the transformation matrix of the sixth data to obtain seventh data; correcting the seventh data to obtain eighth data; and obtaining a third noise covariance matrix based on the eighth data; Prediction unit: used to obtain the first system state at time k-1 based on the initialization system parameters; predict the second system state at time k based on the first system state and the system state transfer equation; obtain the state covariance matrix P based on the second system state, the first noise covariance matrix, the second noise covariance matrix and the third noise covariance matrix k ; Based on the state covariance matrix P k The third coordinate is obtained.

6. The indoor positioning system integrating ultra-wideband and inertial navigation according to claim 5, characterized in that: The EKF unit further comprises an updating unit, which is used to: Based on the state covariance matrix P k Get the Kalman gain K k Based on the Kalman gain K k and the state covariance matrix P k Get the first state covariance matrix P k1 ; Based on the first state covariance matrix P k1 A fourth coordinate is obtained, and the third coordinate is updated to the fourth coordinate.

7. The indoor positioning system integrating ultra-wideband and inertial navigation according to claim 4, characterized in that: The system further comprises a control unit, which is configured to: configuring a signal transmitter, wherein the signal transmitter transmits a control command to the movable target, and parses the control command to obtain a preset acceleration and a preset angular velocity; The measured acceleration and the measured angular velocity are updated to the preset acceleration and the preset angular velocity respectively.

8. The indoor positioning system integrating ultra-wideband and inertial navigation according to claim 5, characterized in that: A first calculation method for obtaining the second system state is: X k =f(X k-1 ,u k ); Among them, X k represents the system state at time k, f() represents the system state transition equation, X k-1 represents the system state at time k-1, u k represents the system input matrix, P k represents the state covariance matrix at time k, F k represents the Jacobian matrix, P k-1 represents the state covariance matrix at time k-1, Q k represents the noise covariance matrix at time k, k represents an integer greater than or equal to 2, and T represents transpose.

9. The indoor positioning system integrating ultra-wideband and inertial navigation according to claim 8, characterized in that: Get the first state covariance matrix P k1 The second calculation method is: P k1 =(I-K k H k )P k ; Among them, P k1 represents the updated state covariance matrix at time k, I represents the identity matrix, K k represents the Kalman gain, H k represents the measurement Jacobian matrix, P k Represents the state covariance matrix at time k before the update.

10. An indoor positioning method integrating ultra-wideband and inertial navigation, characterized in that: The method comprises: A base station is configured to locate a first coordinate of a movable target based on an ultra-wideband signal of the base station; an inertial measurement unit is used to locate a second coordinate of the movable target; a wheel odometer is used to obtain a calibrated acceleration and a calibrated angular velocity of the movable target; and based on an extended Kalman filter, the first coordinate, the second coordinate, the calibrated acceleration and the calibrated angular velocity are fused and located to obtain a third coordinate of the movable target.

Citation Information

Cited By

  • Multi-sensor fusion positioning method for magnetic adsorption wall-climbing robot

    CN120846312A