Indoor amr single base station positioning device and method

Abstract

The application discloses an indoor AMR single base station positioning device and method, which comprises a mobile robot body, a sensor module arranged at the middle part of the mobile robot body, and a driving element, a transmission part, a rotating disc and a label arranged on the surface of the mobile robot body; the driving element is installed on the surface of the mobile robot body, and the output end of the driving element is connected with the transmission part; one end of the transmission part is connected with the driving element, and the other end is connected with the rotating disc; the transmission part is used for driving the rotating disc to move regularly and periodically under the driving of the driving part; the rotating disc is installed above the transmission part and is used for driving the label to move periodically; the inside of the label is provided with a UWB communication module, which is used for data communication with a base station and measuring the distance from the label to the base station. The regular motion control part of the label is designed, the investment of the base station is reduced, and the high-precision ranging technology of UWB is adopted, so that the high-precision positioning of the indoor AMR can be realized.

Description

Technical Field

[0001] This invention belongs to the field of indoor positioning technology, specifically relating to an indoor AMR single base station positioning device and method. Background Technology

[0002] Autonomous Mobile Robot (AMR) is an intelligent robot that can move independently in its environment. AMR does not rely on ground tracks, but uses sophisticated sensors, such as LiDAR, to achieve laser-based localization and navigation within the environment.

[0003] To locate an AMR moving indoors, tags are typically placed on its surface. A common positioning method involves the tag communicating with multiple base stations (at least three). The distance between the tags is measured based on the communication time or time difference, and then the positioning is estimated.

[0004] Currently, methods for positioning using a single base station all employ array antennas to process the received signal angle (AOA) and time difference to achieve positioning. However, in practical applications, multiple base stations and array antennas significantly increase the cost of high-precision positioning and place high demands on the array antenna arrangement. In indoor environments with many obstacles, the presence of non-line-of-sight errors and multipath propagation can easily lead to signal distortion and observation errors, thus affecting the positioning effect. Summary of the Invention

[0005] The purpose of this invention is to provide an indoor AMR single base station positioning device to solve the problems mentioned in the background art.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] An indoor AMR single-base station positioning device includes a mobile robot body, wherein a sensor module is disposed in the middle of the mobile robot body, characterized in that: a driving component, a transmission component, a rotating disk and a tag are disposed on the surface of the mobile robot body;

[0008] The driving component is mounted on the surface of the mobile robot, and the output end of the driving component is connected to the transmission unit. The driving component drives the rotating disk to perform regular periodic motion through the transmission unit.

[0009] One end of the transmission unit is connected to the driving member, and the other end is connected to the rotating disk. The transmission unit is used to drive the rotating disk to perform regular periodic motion under the drive of the driving unit.

[0010] The rotating disk is mounted above the transmission unit and is used to drive the label to move periodically;

[0011] The tag is equipped with a UWB communication module for data communication with the base station and for measuring the distance between the tag and the base station.

[0012] Preferably, the transmission unit includes a driving gear and a driven gear, the driving gear meshing with the driven gear, the middle part of the driving gear being connected to the output end of the driving member, and the middle part of the driven gear being connected to the rotating disk.

[0013] Preferably, a drive column is provided on one side of the driving gear, and an auxiliary rod is provided on one side of the driven gear, with the drive column abutting against the auxiliary rod.

[0014] Another object of the present invention is to provide an indoor AMR single base station positioning method, comprising the following steps:

[0015] Includes the following steps:

[0016] S1. Utilize UWB communication technology to measure the transient distance between a single base station and a tag;

[0017] S2. Use a driving component to drive the rotating disk to move in a regular motion, so that the label moves to the next quadrant point;

[0018] S3. Collect UWB ranging information from the tag to the base station at each quadrant point in sequence, and perform confidence fitting processing.

[0019] S4. Use sensor modules to collect motion characteristics of indoor AMRs;

[0020] S5. Using UWB fitting ranging values, AMR motion characteristics, and the spatial geometric relationship between the tag and the base station, the center position of the AMR coordinate system is preliminarily calculated;

[0021] S6. Compare the observation residuals of the ranging system and the sensor system, and adaptively correct the prediction error covariance and the observation error covariance;

[0022] S7. Complete the optimal estimation for adaptive extended Kalman filter localization.

[0023] Preferably, in step S3, during each pause period, the tag and base station can obtain multiple sets of ranging values ​​through communication ranging. Weights are assigned to these multiple sets of ranging values, and confidence processing is performed to obtain a fitted ranging value. In the i-th pause period, the tag and base station can obtain N... i Let the k-th distance measurement value be d. k And its time metric from the sampling time is ξ k Then the fitted ranging value L between the tag and the base stationi As shown below:

[0024]

[0025] Preferably, in step S5, the tag rotates according to the pattern described in step S2, and the ranging values ​​at times i and i+1 (i = 1, 2, ..., n) are used iteratively to calculate the AMR position coordinates, thereby suppressing the negative impact of the cumulative error of the motor rotation system on positioning.

[0026] The position at time i is (x i ,y i The center position (x) can be obtained by rotating the label twice. o ,y o ), that is, the AMR position coordinates, and the AMR's turning angle is α. The relationship between the two positions of the label and the center of the circle can be listed as follows:

[0027]

[0028]

[0029] Therefore, we can conclude that:

[0030]

[0031] Preferably, in step S6, the slope is represented by both the ranging value and the gyroscope observation angle, and the signal arrival angle is represented by both the ranging value and the gyroscope observation angle. By comparing the real-time observation information of the ranging system and the sensor system, Δλ can be obtained. Δθ、 Four correction factors are used to adaptively correct the prediction error covariance and the observation error covariance.

[0032] First, the slope is expressed from both the range measurement value and the gyroscope observation angle to obtain the correction factor Δλ.

[0033] The line connecting the i-th and i+2-th positions of the label is l. AC The line connecting the (i+1)th and (i+3)th positions is l. BD Then the slopes of the two lines should satisfy K AC *K BD =-1, and its slope is expressed using distance measurement information as:

[0034]

[0035] Observation residual correction factor:

[0036]

[0037] The slope of a straight line is expressed as:

[0038]

[0039] Slope residual correction factor

[0040] Secondly, by representing the signal arrival angle from both the ranging value and the gyroscope observation angle, the correction factor Δθ can be obtained.

[0041] Let θ be the angle of arrival of the communication signals between the base station and the tag at times i and i+1. i θ i+1 The angular residual correction factor Δθ is obtained as follows:

[0042]

[0043] From the perspective of gyroscope observation, we get:

[0044]

[0045] The difference between the two can also be obtained from the following formula.

[0046]

[0047] Preferably, in step S7, the Kalman filter is extended through the UWB communication module and the sensor module. The sensor module measures the velocity v, acceleration a, and attitude angle α of the AMR, and the state equation is:

[0048]

[0049] in, These are the AMR perturbation accelerations, which are normally 0; w k+1 For system process noise, the observation equation is:

[0050]

[0051] v k+ 1 represents observation noise, v k+1 w k+1 All are normally distributed Gaussian white noise;

[0052] The localization estimation process is as follows:

[0053] First, the ranging and positioning information and sensor observation information are substituted into the state equation for one-step state prediction. Then, the observation equation is constructed, and the correction factor is obtained by comparing the real-time information of the two observation systems. The prediction error covariance Q is then calculated. k+1 After correction, the following results were obtained. And calculate the one-step prediction covariance P.k+1 :

[0054]

[0055] Then the covariance matrix R of the observation noise was calculated. k+1 Correction is performed to obtain Obtain the adaptive extended Karl gain K k+1 :

[0056]

[0057] Wherein, the state transition matrix Φ k+1 / k and observation matrix H k+1 Let be the Jacobian matrix of the function. This completes the optimal positioning estimate at time k+1. With Estimated Covariance

[0058]

[0059]

[0060] The technical effects and advantages of this invention are as follows:

[0061] 1. This invention only requires a single base station in the indoor positioning system, without the need for an antenna array. It only needs to communicate with regularly moving tags at high speed to achieve the center positioning of the AMR, which greatly reduces the application cost;

[0062] 2. By using the regular motion information of the rotating tag to adaptively correct the error covariance of the extended Kalman filter, the positioning accuracy can be further improved and the indoor positioning error can be reduced. Attached Figure Description

[0063] Figure 1 This is a structural diagram of the indoor AMR positioning system of the present invention;

[0064] Figure 2 This is a schematic diagram of the label rotating device and transmission module of the present invention;

[0065] Figure 3 This is a schematic diagram of the rotating gear of the present invention.

[0066] Figure 4 This is a schematic diagram illustrating the communication between the rotating tag and the base station according to the present invention;

[0067] Figure 5 The flowchart shows the localization estimation process for adaptive extended Kalman filtering.

[0068] In the diagram: 1. Mobile robot body; 2. Rotary disk; 3. Tag; 4. Base station; 5. Drive component; 6. Passive gear; 7. Active gear; 8. Drive column; 9. Auxiliary rod; 10. Sensor module. Detailed Implementation

[0069] The following will refer to the appendices in the embodiments of the present invention. Figures 1-5 The technical solutions in the embodiments of the present invention are clearly and completely described herein. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. The specific embodiments described herein are merely illustrative of the present invention and are not intended to limit the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0070] Example:

[0071] Ultra-wideband (UWB) signals have bandwidths on the order of GHz by directly modulating impulse pulses with very steep rise and fall times. It has advantages such as insensitivity to channel fading, low transmitted signal power spectral density, low interception capability, low system complexity, and the ability to provide positioning accuracy of several centimeters. It facilitates high-speed communication and has strong penetration capability.

[0072] like Figures 1-3 As shown, the present invention provides an indoor AMR single base station 4 positioning device, including a mobile robot body 1. A sensor module 10 is arranged in the middle of the mobile robot body 1. The sensor module 10 includes an odometer, a gyroscope, an accelerometer, a lidar, etc., which can realize the all-round acquisition of the motion characteristics of the AMR. The measured speed, acceleration, attitude angle and other information can be substituted into the extended Kalman filter equation for positioning estimation. The surface of the mobile robot body 1 is provided with a drive component 5, a transmission part, a rotating disk 2 and a tag 3.

[0073] The drive unit 5 is mounted on the surface of the mobile robot. The output end of the drive unit 5 is connected to the transmission unit. The drive unit 5 drives the rotating disk 2 to perform regular periodic motion through the transmission unit. In this embodiment, the drive unit 5 is driven by a micro DC motor with a reduction gearbox. In other embodiments, other drive devices can also be used. The speed of the micro DC motor is 3500 rpm. After being reduced by the reduction gearbox, the speed of the active gear 7 is 2400 rpm, that is, it rotates once every 25 ms.

[0074] One end of the transmission unit is connected to the drive component 5, and the other end is connected to the rotating disk 2. The transmission unit includes a driving gear 7 and a driven gear 6. The driving gear 7 and the driven gear 6 mesh with each other. The middle part of the driving gear 7 is connected to the output end of the drive component 5, and the middle part of the driven gear 6 is connected to the rotating disk 2. A drive column 8 is provided on one side of the driving gear 7, and an auxiliary rod 9 is provided on one side of the driven gear 6. The drive column 8 and the auxiliary rod 9 abut against each other. The transmission unit is used to drive the rotating disk 2 to perform regular periodic motion under the drive of the drive unit. It is driven by a micro DC motor. The motor and the gearbox drive the driving gear 7 to rotate through the shaft. The driving gear 7 is linked with the driven gear 6 to realize that the rotating disk 2 and the tag 3 rotate N times in a regular manner within a sampling period of T seconds. In this process, the driving gear 7 rotates once, driving the driven gear 6 to rotate 90 degrees and pause for 20ms. The driving column 8 and the auxiliary rod 9 help the two gears rotate and mesh. The driven gear 6 is a unidirectional rotating gear. During the pause, the driving gear 7 is stuck between the two sets of teeth, so the rotational inertia problem can be ignored when performing calculations.

[0075] The rotating disk 2 is installed above the transmission unit and is located at the center of the AMR. The center of the rotating disk 2 is the positioning center of the AMR, and its radius is greater than the maximum ranging error. The rotating disk 2 can drive the tag 3 to move periodically.

[0076] Tag 3 has an internal UWB communication module for data communication with base station 4 and for measuring the distance between tag 3 and base station 4. A main control module is installed on the AMR to be located. The main control module uses, but is not limited to, an STM32 chip. The main control module is connected to the UWB communication module and can measure the distance between tag 3 and base station 4. The measured distance data is stored in the STM32 main control unit. Base station 4 can upload the ranging and positioning information to the server via Wi-Fi through a remote communication module, which can be used by the host computer for subsequent development.

[0077] An indoor AMR single-base station 4-positioning method includes the following steps:

[0078] S1. Utilize UWB communication technology to measure the transient distance between a single base station 4 and tag 3. During positioning, base station 4 communicates with tag 3 and collects transient distance information between base station 4 and tag 3.

[0079] S2. Using the driving component 5 to drive the rotating disk 2 to move regularly, the label 3 moves to the next quadrant point. If the sampling time is determined to be 0.1s, the diameter of the rotating disk 2 is 30cm, the speed of the micro DC motor is 3500rpm, and the speed of the drive gear 7 can be achieved to 2400rpm through the reduction gearbox, that is, one rotation every 25ms, the working process of the two gears is as follows:

[0080] like Figure 3As shown, when the driving gear 7 rotates to the position shown, the driving column 8 will push the auxiliary rod 9 to help the passive gear 6 rotate and mesh. The short arc of the rotation trajectory takes 5ms and drives the passive gear 6 to rotate 90 degrees every 5ms; the long arc takes 20ms and causes the passive gear 6 to stop for 20ms.

[0081] S3. Sequentially collect UWB ranging information from tag 3 to base station 4 at each quadrant point, and perform confidence fitting processing. During each stagnation period, tag 3 and base station 4 can obtain multiple sets of ranging values ​​through communication ranging. Weights are assigned to these multiple sets of ranging values, and confidence processing is performed to obtain the fitted ranging value. In the i-th stagnation period, tag 3 and base station 4 can obtain N... i Let the k-th distance measurement value be d. k And its time metric from the sampling time is ξ k Then the fitted ranging value L between tag 3 and base station 4 i As shown below:

[0082]

[0083] S4. Use sensor module 10 to collect motion characteristics of the indoor AMR;

[0084] S5. Using UWB fitting of ranging values, AMR motion characteristics, and the spatial geometric relationship between tag 3 and base station 4, the initial position of the AMR coordinate system center is calculated. Tag 3 rotates according to the pattern described in S2, and its position at time i is (x...). i ,y i The center position (x) can be obtained by rotating label 3 twice. o ,y o ), that is, the AMR position coordinates. For example... Figure 4 As shown, the AMR's turning angle is α. Taking the two moments i=1 and i=2 as examples, the relationship between the two positions of label 3 and the center of the circle can be listed as follows:

[0085]

[0086]

[0087] Therefore, we can conclude that:

[0088]

[0089] S6. Compare the observation residuals of the ranging system and the sensor system, and adaptively correct the prediction error covariance and the observation error covariance;

[0090] The slope is represented by both the ranging value and the gyroscope observation angle, and the signal arrival angle is represented by both the ranging value and the gyroscope observation angle. By comparing the real-time observation information of the ranging system and the sensor system, Δλ can be obtained. Δθ、 Four correction factors are used to adaptively correct the prediction error covariance and the observation error covariance.

[0091] (1) Express the slope from both the distance measurement value and the gyroscope observation angle.

[0092] According to the set rotation rule, the line connecting the 1st and 3rd positions of label 3 is l. AC The line connecting the 2nd and 4th positions is l. BD Then the slopes of the two lines should satisfy K AC *K BD = -1. Its slope can be expressed using distance measurement information as:

[0093]

[0094]

[0095] Within a very short time, the change in the AMR's own rotation angle is negligible. Due to the existence of ranging error, the observation residual correction factor can be obtained by comparing it with the ideal slope:

[0096]

[0097] From the perspective of gyroscope observation, the slope of the straight line can also be expressed as:

[0098]

[0099] Since gyroscopes also have systematic errors, the slope residual correction factor can be obtained similarly.

[0100]

[0101] (2) The signal arrival angle is expressed from two aspects: the distance measurement value and the gyroscope observation angle.

[0102] like Figure 3 As shown, taking the times i=1 and i=2 as examples, the arrival angles of the communication signals between base station 4 and tag 3 are θ1 and θ2, respectively. Starting from the ranging angle, the angle residual correction factor Δθ can be obtained:

[0103]

[0104] From the perspective of gyroscope observation, we can obtain:

[0105]

[0106] The difference between the two can also be obtained from the following formula.

[0107]

[0108] Due to inherent systematic errors in UWB ranging technology and sensor systems, and the complexity of the environment and the variability of AMR travel paths, prediction and observation errors cannot be accurately modeled. Therefore, Δλ can be obtained by comparing real-time observation information from two different observation systems. Δθ、 The four correction factors can intuitively reflect the degree of deviation of the observations. They can compensate and correct for any bad observations at a certain time, avoid the impact of occasional bad values ​​on positioning accuracy, and improve the anti-interference capability of the positioning system.

[0109] S7. Complete the adaptive extended Kalman filter localization optimal estimate.

[0110] Through the UWB communication module and sensor module 10, the Kalman filter is extended. Sensor module 10 measures the velocity v, acceleration a, and attitude angle α of the AMR. The state equation is:

[0111]

[0112] in, These are the AMR perturbation accelerations, which are normally 0; w k+1 For system process noise, the observation equation is:

[0113]

[0114] v k+1 To observe the noise, v k+1 w k+1 All are normally distributed Gaussian white noise;

[0115] The positioning estimation process is as follows: Figure 5 As shown:

[0116] First, the ranging and positioning information and sensor observation information are substituted into the state equation for one-step state prediction. Then, the observation equation is constructed, and the correction factor is obtained by comparing the real-time information of the two observation systems. The prediction error covariance Q is then calculated. k+1 After correction, the following results were obtained. And calculate the one-step prediction covariance P. k+1 :

[0117]

[0118] Then the covariance matrix R of the observation noise was calculated. k+1 Correction is performed to obtain Obtain the adaptive extended Karl gain K k+1 :

[0119]

[0120] Wherein, the state transition matrix Φ k+1 / k and observation matrix H k+1 Let be the Jacobian matrix of the function. This completes the optimal positioning estimate at time k+1. With Estimated Covariance

[0121]

[0122]

[0123] In summary, the core of this invention for single-base station positioning lies in the design of a circular mechanical motion. First, the communication tag is fixed at a quadrant point of a rotating disk, and a micro DC motor and gear transmission device are used to make it rotate regularly. Four samples are taken for each rotation. Confidence processing is performed on multiple sets of ranging values ​​between the moving tag and a single base station, and the center position of the AMR is calculated by combining the spatial geometric positional relationship. Finally, the motion characteristics of the AMR are collected using a sensor module, an extended Kalman filter is constructed, and a correction factor is constructed by comparing the observation residuals of the ranging system and the sensor system. The estimated and observation covariances are weighted and corrected, thereby achieving adaptive EKF optimal positioning estimation for a single base station. This invention reduces the investment in base stations by designing a regular movement control component for the tag, and with the addition of UWB high-precision ranging technology, high-precision positioning of indoor AMRs can be achieved.

[0124] Finally, it should be noted that the above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A positioning method for an indoor AMR single-base station positioning device, wherein the indoor AMR single-base station positioning device includes a mobile robot body, and a sensor module is disposed in the middle of the mobile robot body, characterized in that: The surface of the mobile robot body is provided with a drive unit, a transmission unit, a rotating disk, and a label; The driving component is mounted on the surface of the mobile robot, and the output end of the driving component is connected to the transmission unit. The driving component drives the rotating disk to perform regular periodic motion through the transmission unit. One end of the transmission unit is connected to the driving member, and the other end is connected to the rotating disk. The transmission unit is used to drive the rotating disk to perform regular periodic motion under the drive of the driving member. The tag is equipped with a UWB communication module for data communication with the base station and for measuring the distance between the tag and the base station. The transmission unit includes a driving gear and a driven gear, the driving gear meshing with the driven gear, the middle part of the driving gear being connected to the output end of the driving member, and the middle part of the driven gear being connected to the rotating disk; A drive column is provided on one side of the driving gear, and an assist rod is provided on one side of the driven gear. The drive column abuts against the assist rod. The positioning method for an indoor AMR single base station positioning device includes the following steps: S1. Utilize UWB communication technology to measure the transient distance between a single base station and a tag; S2. Use a driving component to drive the rotating disk to move in a regular motion, so that the label moves to the next quadrant point; S3. Collect UWB ranging information from the tag to the base station at each quadrant point in sequence, and perform confidence fitting processing; S4. Use sensor modules to collect motion characteristics of indoor AMRs; S5. Using UWB fitting ranging values, AMR motion characteristics, and the spatial geometric relationship between the tag and the base station, the center position of the AMR coordinate system is preliminarily calculated; S6. Compare the observation residuals of the ranging system and the sensor system, and adaptively correct the prediction error covariance and the observation error covariance; S7. Complete the optimal estimation for adaptive extended Kalman filtering localization. In step S3, during each pause period, the tag and base station can obtain multiple sets of ranging values ​​through communication ranging. Weights are assigned to these multiple sets of ranging values, and confidence processing is performed to obtain a fitted ranging value. During the i-th pause period, the tag and base station can obtain N ranging values. i Let the k-th distance measurement value be d. k， And its time metric from the sampling time is Then the fitted ranging value L between the tag and the base station i As shown below: 。 2. The positioning method according to claim 1, characterized in that: In S5, the tag rotates according to the pattern described in S2, and the ranging values ​​at times i and i+1 (i=1,2,⋯,n) are used iteratively to calculate the AMR position coordinates, thereby suppressing the negative impact of the cumulative error of the motor rotation system on positioning. The position at time i is (x i ,y i The center position (x) can be obtained by rotating the label twice. o ,y o ), that is, the AMR position coordinates, and the AMR's turning angle is α. The relationship between the two positions of the label and the center of the circle can be listed as follows: Therefore, we can conclude that: 。 3. The positioning method according to claim 2, characterized in that: In step S6, the slope is represented by both the ranging value and the gyroscope observation angle, and the signal arrival angle is represented by both the ranging value and the gyroscope observation angle. By comparing the real-time observation information of the ranging system and the sensor system, the following can be obtained: Four correction factors are used to adaptively correct the prediction error covariance and the observation error covariance. First, the slope is expressed from two aspects: the distance measurement value and the gyroscope observation angle, to obtain the correction factors. : The line connecting the i-th and i+2-th positions of the label is l. AC The line connecting the (i+1)th and (i+3)th positions is l. BD Then the slopes of the two lines should satisfy K AC *K BD =−1, and its slope is expressed using distance measurement information as: Observation residual correction factor: The slope of the straight line is expressed as: Slope residual correction factor ; Secondly, by representing the signal arrival angle from both the ranging value and the gyroscope observation angle, the correction factor can be obtained. Let θ be the angle of arrival of the communication signals between the base station and the tag at times i and i+1. i θ i+1 The angle residual correction factor is obtained. From the perspective of gyroscope observation, we get: The difference between the two can also be obtained from the following formula. : 。 4. The positioning method according to claim 2, characterized in that: In step S7, the Kalman filter is extended through the UWB communication module and the sensor module. The sensor module measures the AMR's velocity v, acceleration a, and attitude angle α. The state equation is: in, , These are the AMR disturbance accelerations, which are normally 0; For system process noise, the observation equation is: To observe the noise, , All noise is normally distributed Gaussian white noise; the localization estimation process is as follows: First, the ranging and positioning information and sensor observation information are substituted into the state equation for one-step state prediction. Then, the observation equation is constructed, and the correction factor is obtained by comparing the real-time information of the two observation systems. The prediction error covariance Q is then calculated. k+1 After correction, the following results were obtained. And obtain the one-step prediction covariance. : Then the covariance matrix of the observation noise Correction is performed to obtain The adaptive extended Karl gain is obtained. : Wherein, the state transition matrix and observation matrix The Jacobian matrix of the function is now complete. optimal value of location estimation at time With Estimated Covariance : 。

Images