Method, device, equipment and readable storage medium for positioning self-mobile equipment

Abstract

The present application discloses a method, apparatus, device and readable storage medium for positioning a self-mobile device, belonging to the technical field of positioning a self-mobile device. The method determines the second environmental position data of the self-mobile device in the map coordinate system according to the first environmental position data of the positioning tag in the map coordinate system, wherein the map coordinate system is generated based on the position coordinates of the positioning base station; the self-mobile device is positioned by fusing the second environmental position data and the first body position data of the self-mobile device in the device coordinate system, wherein the first body position data is acquired by the positioning sensor, thereby solving the technical problem that the positioning accuracy of the self-mobile device is reduced in a complex environment, ranging errors are prone to occur, and the walking safety of the self-mobile device is reduced.

Description

Technical Field

[0001] The present invention relates to the field of self-mobile equipment positioning, and in particular to a self-mobile equipment positioning method, device, equipment and readable storage medium. Background Art

[0002] With the continuous development of science and technology, more and more autonomous mobile devices are no longer limited to working in a small range indoors, but can move and work accurately both indoors and outdoors.

[0003] When a self-moving device needs to be positioned in indoor and outdoor scenes, some self-moving devices will directly use a wheel speed meter and a gyroscope as positioning sensors, but this positioning method can only provide direction information on the coordinate system related to the self-moving device itself, but cannot provide direction information related to the external environment, and the errors generated in the positioning process will be accumulated, resulting in a gradual increase in positioning errors; some self-moving devices will use UWB (Ultra Wide-Band) positioning systems, which are positioned by setting up base stations in the environment and calculating the position of the self-moving device relative to the base station. Although the UWB positioning system has a wide range of applicable scenarios, high positioning accuracy and low calculation amount, this method is only applicable in areas surrounded by base stations and without large obstacles. When the self-moving device is close to a wall or some large objects, the UWB signal is not penetrable enough, which may cause certain interference to the positioning results of the UWB positioning system. As a result, when the self-moving device uses the positioning device to obtain location information, it is easy to have ranging errors or large positioning errors, resulting in low positioning accuracy of the self-moving device positioning system.

[0004] The above contents are only used to assist in understanding the technical solution of the present invention and do not constitute an admission that the above contents are prior art. Summary of the invention

[0005] The main purpose of the present invention is to provide a method, device, equipment and readable storage medium for positioning a self-moving device, aiming to solve the technical problem that the existing UWB positioning system is prone to ranging errors near large obstacles, resulting in low working safety of the self-moving device and reduced positioning accuracy.

[0006] To achieve the above object, the present invention provides a method for positioning a self-moving device, which is applied to a UWB positioning system. The UWB positioning system is provided with a self-moving device, a positioning sensor, a positioning tag and at least one positioning base station. The method for positioning a self-moving device includes:

[0007] Determine the second environmental position data of the mobile device in the map coordinate system according to the first environmental position data of the positioning tag in the map coordinate system, wherein the map coordinate system is generated based on the position coordinates of the positioning base station;

[0008] The self-moving device is positioned by fusing the second environment position data and the first body position data of the self-moving device in the device coordinate system, wherein the first body position data is acquired by the positioning sensor.

[0009] In addition, in order to achieve the above purpose, the present application also provides a self-mobile device positioning device, which is applied to a UWB positioning system. The UWB positioning system is provided with a self-mobile device, a positioning sensor, a positioning tag and at least one positioning base station. The self-mobile device positioning device includes:

[0010] A determination module, configured to determine second environmental position data of the self-mobile device in the map coordinate system according to first environmental position data of the positioning tag in the map coordinate system, wherein the map coordinate system is generated based on the position coordinates of the positioning base station;

[0011] A fusion module is used to locate the self-moving device by fusing the second environment position data and the first body position data of the self-moving device in the device coordinate system, wherein the first body position data is collected by the positioning sensor.

[0012] The present application provides a self-moving device, which includes: a vehicle head, a vehicle body, a positioning device, a memory, a processor, and a self-moving device positioning program stored in the memory and executable on the processor, wherein the vehicle head is detachably connected to the vehicle body, the positioning device, the memory, and the processor are arranged on the vehicle body, and the self-moving device positioning program is configured to implement the steps of the self-moving device positioning method as described.

[0013] The present application also provides a readable storage medium, which is a computer-readable storage medium. A program for implementing a cross-chain data processing method is stored on the computer-readable storage medium. The program for implementing the cross-chain data processing method is executed by a processor to implement the steps of the cross-chain data processing method as described above.

[0014] The present application provides a method for positioning a self-mobile device. The present application determines second environmental position data of the self-mobile device in the map coordinate system according to first environmental position data of the positioning tag in the map coordinate system, wherein the map coordinate system is generated based on the position coordinates of the positioning base station; and positions the self-mobile device by fusing the second environmental position data and first body position data of the self-mobile device in the device coordinate system, wherein the first body position data is collected by the positioning sensor.

[0015] The present application uses a UWB positioning system for positioning, so that when a self-mobile device is positioned using the UWB positioning system, a map coordinate system is first generated according to the position coordinates of each positioning base station, and the first environmental position data of each tag in the map coordinate system is determined, and the second environmental position data of the self-mobile device in the map coordinate system is determined by the first environmental position data, and a positioning sensor is added, and the first body position data of the self-mobile device in the device coordinate system is determined by the positioning sensor, and the second environmental position data is fused and calculated with the first body position data obtained by the positioning sensor, thereby positioning the self-mobile device. Since each positioning base station in the environment and the positioning tag fixed on the self-mobile device can provide the position data of the self-mobile device, and the positioning sensor is added to fuse the position data obtained by the tag, the positioning accuracy is improved, and the influence of environmental factors on the positioning accuracy is reduced. Therefore, the technical defects that when the self-mobile device is positioned in a complex environment, the positioning accuracy is easily reduced, the ranging error is large, and the self-mobile device collides with other safety issues are overcome, so the positioning accuracy of the self-mobile device positioning system is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0017] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0018] Figure 1 It is a flowchart of a method for positioning a mobile device provided in Embodiment 1 of the present application;

[0019] Figure 2 A schematic diagram of the installation positions of the positioning tags and base stations of the self-mobile device positioning method provided in the first embodiment of the present application;

[0020] Figure 3 A schematic diagram of the relationship between the device coordinate system and the map coordinate system provided in the first embodiment of the method for positioning a mobile device of the present application;

[0021] Figure 4 A schematic diagram of a process for obtaining a positioning tag and a position of a self-mobile device by a UWB positioning device and a positioning sensor in a self-mobile device positioning method provided in Embodiment 1 of the present application;

[0022] Figure 5A schematic diagram of a flow chart of two fusion processes of a positioning method for a mobile device provided in Embodiment 1 of the present application;

[0023] Figure 6 A schematic diagram of the module structure of a UWB positioning system for a method for positioning a mobile device provided in Example 1 of the present application;

[0024] Figure 7 A flowchart of a method for positioning a mobile device provided in Embodiment 2 of the present application;

[0025] Figure 8 A schematic diagram of the module structure of a self-mobile equipment positioning device provided in Embodiment 3 of the present application;

[0026] Fig. 9 Schematic diagram of the device structure of the hardware operating environment involved in the method for positioning a mobile device in an embodiment of the present application.

[0027] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings in conjunction with the embodiments. DETAILED DESCRIPTION

[0028] In order to make the above-mentioned purposes, features and advantages of the present invention more obvious and easy to understand, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention.

[0029] Embodiment 1

[0030] When a self-moving device is working indoors or outdoors, in order to walk and work more safely and accurately, it is necessary to obtain the current position and movement direction of the self-moving device through a positioning device, and determine the next working position and direction to avoid collisions.

[0031] Conventional positioning systems for self-moving devices usually include wheel speed meters, global navigation satellite systems (GNSS), simultaneous localization and mapping (SLAM), and ultra-wideband (UWB) communication positioning systems. Among them, the wheel speed meter calculates the wheel speed and integrates it with the angular velocity from the gyroscope to obtain the position coordinates and direction information of the self-moving device. The positioning cost of the wheel speed meter is low, and a more accurate positioning value can be obtained in a short time. However, during the positioning process, the positioning errors generated by the wheel speed meter and the gyroscope will be accumulated, causing the positioning error to gradually increase. The GNSS method receives the signal of the geosynchronous satellite to obtain the distance between the self-moving device and the satellite, and then further calculates the longitude and latitude of the self-moving device on the earth to obtain centimeter-level accuracy position information, but the GNSS method is suitable for open outdoor scenes. In indoor or sky-covered scenes, the positioning accuracy of GNSS is low. The SLAM method uses a camera to collect video or continuous images, compares two adjacent frames of images to obtain the deviation of a specific point, and then estimates the displacement and speed of the camera based on the deviation value. On this basis, closed-loop detection is performed, and after the accumulated error is processed to form a map, the coordinates can be obtained through image matching. Although SLAM is not affected by wheel slippage, it requires large computing resources and is prone to failure when there are many similar scenes, large changes in lighting, and large changes in scene landscape. The UWB communication positioning system can obtain the distance between the base station and the mobile station by measuring the time difference between the signal transmission between the base station and the mobile station. By obtaining the distance between the mobile station and more than three base stations, its position relative to the base station can be calculated. Although the UWB positioning method has high positioning accuracy and small calculation amount when the signal is good, it is only applicable within the area surrounded by the base station. When the device is close to the wall or other large objects, ranging errors may occur, making the area near the wall and obstacles a "blind spot" with poor positioning accuracy.

[0032] In addition, the tachometer and gyroscope obtain direction information from the mobile device itself, but cannot provide direction information related to the external environment. GNSS or UWB calculates the current direction by recording the change in position of the mobile device after it moves a certain distance. However, this method is usually suitable for situations where the ground conditions are good. In scenes where it is easy to slip or difficult to walk, the speed of obtaining the correct direction information from the mobile device is slow.

[0033] Based on this, the present application proposes a first embodiment of a method for positioning a self-equipped device, please refer to Figure 1 , the self-mobile device positioning method comprises:

[0034] Step S10: determining the second environmental position data of the mobile device in the map coordinate system according to the first environmental position data of the positioning tag in the map coordinate system, wherein the map coordinate system is generated based on the position coordinates of the positioning base station;

[0035] Step S20: Positioning the self-moving device by fusing the second environment position data and the first body position data of the self-moving device in the device coordinate system, wherein the first body position data is acquired by the positioning sensor.

[0036] It should be noted that the self-mobile device includes a positioning device, which refers to a device composed of a positioning tag and a positioning sensor, and is used to obtain all positioning data of the self-mobile device, wherein the first body position data obtained comes from the positioning sensor installed on the self-mobile device, and the positioning sensor is used to obtain the center position and moving direction of the self-mobile device. Specifically, it can be composed of a wheel speed meter and an IMU module (Inertial Measurement Unit), and the first body position data includes the position data and angular data of the self-mobile device. The first environmental position data obtained is data obtained by calculating the position of the positioning tag installed on the self-mobile device by the UWB positioning system. Specifically, two positioning tags can be installed on the self-mobile device. When determining the position coordinates of the self-mobile device, the position coordinates of the two positioning tags installed on the self-mobile device, that is, the first environmental position data, must be obtained respectively.

[0037] In addition, it should be noted that the positioning base station is used to receive and estimate the measurement signal sent from the tag, and can be specifically installed in the working area of ​​the self-mobile device. Since the UWB positioning device sends the measurement signal to each base station through the tag, the distance between the tag and the base station is determined according to the time taken by the measurement signal to reach each base station, and the distance between the base stations is determined by comparing the time difference of the measurement signal reaching each base station. Therefore, the tag installed on the self-mobile device can output the distance between the tag and all base stations, as well as the distance between the base stations. In addition, the map coordinate system (O map , x map ,y map ) is used to determine the position of the self-moving device in the working area, which is established based on the positional relationship between base stations. Specifically, based on the position of the base station in the map coordinate system and the distance between the tag and the base station, the least square positioning method can be used to calculate the coordinates of each tag at any time. For example, in one possible implementation, two tags are installed on the self-moving device, and there are four base stations in the working area of ​​the self-moving device, where the distance between the tag and the base station is represented by r, the tag is represented by T, the base station is represented by B, k represents the kth tag, k = {0,1}, and r_T can be read.k B0, r_T k B1, r_T k B2, r_T k B3, as well as r_B0B1, r_B0B2, r_B0B3, r_B1B2, r_B1B3, r_B2B3.

[0038] In addition, it should be noted that the device coordinate system is used to indicate the installation position of the driving wheel and the positioning tag on the self-moving device. The installation position is represented by the coordinate value on the device coordinate system. For example, the device coordinate system is a coordinate system fixedly connected to the self-moving device, with the motion center of the self-moving device as the origin, the straight-line forward direction of the self-moving device as the x-axis, the z-axis is perpendicular to the horizontal plane and upward, and the y-axis direction satisfies the right-hand rule.

[0039] In addition, it should be noted that a directional antenna is installed on the positioning base station, while an omnidirectional antenna is installed on the positioning tag. A directional antenna refers to an antenna that transmits and receives electromagnetic waves in one or several specific directions with particularly strong intensity, while transmitting and receiving electromagnetic waves in other directions with zero or extremely small intensity. A directional antenna appears as radiation within a certain angle range on the horizontal radiation pattern, and a directional antenna can suppress potential interference signals and reduce the intensity of noise signals reflected near obstacles. An omnidirectional antenna refers to an antenna that radiates uniformly at 360° in the horizontal radiation pattern, usually without directionality, and generally does not have a signal transmission dead spot. The directional antenna is installed on the base station, and the omnidirectional antenna is installed on the tag.

[0040] The present application uses a UWB positioning system for positioning, so that when the UWB positioning system positions the self-mobile device, it first generates a map coordinate system according to the position coordinates of each positioning base station, and determines the first environmental position data of each tag in the map coordinate system, and determines the second environmental position data of the self-mobile device in the map coordinate system from the first environmental position data, and adds a positioning sensor, and the positioning sensor determines the first body position data of the self-mobile device in the device coordinate system, and the second environmental position data is fused and calculated with the first body position data obtained by the positioning sensor, thereby positioning the self-mobile device. Since each positioning base station in the environment and the positioning tag fixed on the self-mobile device can provide the position data of the self-mobile device, and the positioning sensor is added to fuse the position data obtained by the tag, the positioning accuracy is improved, and the influence of environmental factors on the positioning accuracy is reduced. Therefore, the self-mobile device overcomes the technical defects that when the self-mobile device is performing back-charging positioning and walking near a wall or a large object, the UWB signal has weak penetration, which leads to a decrease in positioning accuracy and a large ranging error, which causes the self-mobile device to collide with other safety issues, so the positioning accuracy of the self-mobile device positioning system is improved.

[0041] In a possible implementation, the step of generating a map coordinate system according to the position coordinates of each positioning base station includes:

[0042] Step A10, constructing at least one pre-selected map coordinate system with each of the positioning base stations as the origin center;

[0043] Step A20, converting the position coordinates of each positioning base station in each pre-selected map coordinate system to obtain the center coordinates of each positioning base station;

[0044] Step A30, generating the map coordinate system according to each of the center coordinates.

[0045] It should be noted that when there are multiple base stations in the working area, each base station needs to be used as the origin to establish a coordinate system. Therefore, more than one map coordinate system will be constructed, that is, multiple pre-selected map coordinate systems. One of the multiple pre-selected coordinate systems is selected as the reference coordinate system, and the base station coordinates required in each pre-selected map coordinate system are converted to the reference coordinate system. Since there will be more than one coordinate position of each base station, it is necessary to select the center position of the multiple coordinate positions and establish the map coordinate system based on the determined center position.

[0046] For example, to help understand the technical concept or technical principle of this application, please refer to Figure 2 , Figure 2 A schematic diagram of the installation position of tags and base stations is provided. Assume that there are four base stations in total, and there are two tags with omnidirectional antennas, which are fixed on the self-moving device. The distance between the two tags needs to be as large as possible, such as Figure 2 The tags T0 and T1 in the figure are placed around the working area of ​​the mobile device. It is necessary to avoid placing all the base stations in the same straight line, such as Figure 2 For B0, B1, B2 and B3 in the figure, the signal transmission direction of the base station is all directed to the center of the area surrounded by the base station. The distance between base stations and between base stations and tags shall not exceed 100m, and there shall be no obstruction between base stations and between base stations and tags as much as possible.

[0047] As an example, the base station may specifically include a UWB positioning device, a directional antenna with a horizontal beam width <90deg, a battery and a power supply circuit. Assuming that there are four base stations in total, any three base stations arranged counterclockwise are known, such as B0, B1 and B2, where the distances between B0, B1 and B2 are r_B0B1, r_B0B2 and r_B1B2 respectively. A set of coordinates can be determined by the triangular relationship between the base stations, that is, let the coordinate of base station B0 be the origin (0,0), and the line connecting B0 and B1 be the x-axis, then the coordinate of B1 is (r_B0B1,0), and the coordinate of B2 (x2,y2) can be obtained as

[0048] x2=r_B0B2×cos(α)

[0049] y2=r_B0B2×sin(α)

[0050] Among them, α=∠B1B0B2, which can be determined by the cosine theorem, and a map coordinate system is determined by three base stations. Since there are four base stations in total, three arranged base stations are randomly selected from the four base stations to establish a map coordinate system and determine the position coordinates of the base stations. A total of four selected map coordinate systems can be obtained, among which one of the base stations is taken as the origin, and the map coordinate system constructed by the origin and another base station is taken as the reference coordinate system. The calculation results of the remaining three coordinate systems for the base station can be transformed to the reference coordinate system through translation and rotation. Due to the existence of ranging errors, each base station can obtain three calculation results, and the center point of the three calculation results of each base station is taken as the final positioning result of the base station.

[0051] For example, to help understand the technical concept or technical principle of this application, please refer to Figure 3 , Figure 3 A schematic diagram of the relationship between the device coordinate system and the map coordinate system is provided. It is assumed that four base stations are installed in the working area. The device coordinate system is located on the body of the self-moving device, with the center of the body of the self-moving device as the origin, and the self-moving device is located on the map coordinate system, which is determined based on the base stations.

[0052] In a possible implementation, the step of determining the second environment location data of the mobile device in the map coordinate system according to the first environment location data of the positioning tag in the map coordinate system includes:

[0053] Step B10, obtaining the second fuselage position data of the positioning tag in the map coordinate system;

[0054] Step B20, determining the second environment position data of the mobile device in the map coordinate system by fusing the second body position data and the first environment position data of the positioning tag in the map coordinate system.

[0055] It should be noted that the second body position data refers to the position of the positioning tag of the self-mobile device in the map coordinate system, and is determined by the first body position data. The fusion positioning algorithm is used to fuse the various positioning data, specifically an extended Kalman filter (Extended Kalman Filter, EKF) algorithm. The EKF algorithm is an algorithm that can fuse multiple measurement data, and obtains more accurate and reliable stand-by positioning data by fusing the data measured by different positioning devices.

[0056] The extended Kalman filter related formula is:

[0057] The optimal state estimate covariance matrix:

[0058]

[0059] Kalman filter matrix:

[0060]

[0061] Update best estimate status:

[0062]

[0063] Update the covariance matrix:

[0064]

[0065] Where Z = [x obs y obs θ obs 1] T represents the observed value, i.e., the first environment position data of the positioning tag in the map coordinate system, Q represents the noise covariance matrix of the predicted value, which is determined according to the second fuselage position data, R represents the noise covariance matrix determined by the observed value, which is determined according to the first environment position data, and X k It is the positioning data of the positioning label obtained by fusion positioning calculation.

[0066] It should be noted that the main steps of the extended Kalman filter algorithm include: prediction and update, where the prediction is to predict the current state value X based on the state value of the previous step and the state transfer matrix F k , the noise covariance matrix Q determined by the first positioning state value predicts the current covariance matrix The update is done by taking the current covariance matrix The Kalman gain K is calculated based on the noise covariance matrix R determined by the observation value, and then the Kalman gain K is used to update the current state value X k and the covariance matrix P k The above steps are the steps of one iteration of the EKF algorithm, and the obtained state value X k and the covariance matrix P k It will be used as the initial state value and covariance matrix for the next iteration. By combining the second fuselage position data and the first environment position data, the estimation result is continuously corrected until a more accurate state value X is updated. k , which is the fused positioning data of the label.

[0067] In addition, it should be noted that since the first body position data obtained by the positioning sensor represents the positioning data of the self-mobile device, and the first environmental position data obtained through the UWB positioning base station is the positioning data of the positioning tag installed on the self-mobile device, therefore, before performing the fusion calculation, it is necessary to first convert the first body position data into the positioning data of the positioning tag on the self-mobile device, that is, by determining the installation position of the tag on the self-mobile device, the first body position data is converted to obtain the second body position data of each tag, and then the first environmental position data and the second body position data are fused. Among them, since the positioning tags installed on the self-mobile device can be two, three or four, etc., therefore, when performing the fusion processing, it is necessary to fuse the first environmental position data and the second body position data of each positioning tag separately to obtain the fused positioning data corresponding to all tags.

[0068] In a possible implementation, the step of positioning the self-moving device by fusing the second environment position data and the first body position data of the self-moving device in the device coordinate system includes:

[0069] Step C10, obtaining first body position data of the mobile device in the device coordinate system;

[0070] Step C20, determining second direction angle data of the mobile device in the map coordinate system according to the first environmental position data;

[0071] Step C30, positioning the self-mobile device by obtaining target positioning data by fusing the first body position data, the second environment position data and the second direction angle data.

[0072] It should be noted that the fusion positioning method involved in this embodiment is specifically the same as the fusion positioning method involved in the previous embodiment. In this embodiment, the first body data is used as the prediction value in the fusion positioning algorithm, and the second environment data and the second direction angle data are jointly used as the observation value in the fusion positioning algorithm. According to the above-mentioned fusion positioning method, the positioning data of the self-mobile device is fused and positioned, thereby outputting the positioning data closest to the true value of the self-mobile device.

[0073] For example, to help understand the technical concept or technical principle of this application, please refer to Figure 4 and Figure 5 , Figure 4 Provides a flow chart of how UWB positioning devices and positioning sensors obtain positioning tags and self-mobile device locations. Figure 5A flow chart of two fusion processes is provided. It is assumed that there are two tags on the self-mobile device, namely T0 and T1, and the carrier is the self-mobile device. The current position of the self-mobile device is positioned and calculated 1 through the wheel speed meter and IMU module, and the positioning results are position 1 and direction 1. Then, according to the positions of the two positioning tags on the self-mobile device and the position of the self-mobile device on the map coordinate system, the position 2 and direction 2 of the positioning tag T0 on the map coordinate system and the position 3 and direction 3 of the positioning tag T1 on the map coordinate system are calculated respectively. At the same time, according to the position of the UWB base station and the distance between the two positioning tags and the base station, the position 4 of the positioning tag T0 on the map coordinate system and the position 5 of the positioning tag T1 on the map coordinate system are calculated respectively, and the self-mobile device is determined according to the installation position of the tag on the self-mobile device. Direction 4 is determined, and then the position 2 and direction 2 of tag T0 determined by the tachometer and IMU module are fused with the position 4 determined by the UWB positioning device, and the position 3 and direction 3 of tag T1 determined by the tachometer and IMU module are fused with the position 5 determined by the UWB positioning device, and the position 6 of tag T0 and the position 7 of tag T1 are calculated respectively. By using geometric relationships, according to the position of the tag in the map coordinate system and the position of the tag on the self-moving device, the position 6 and the position 7 are converted to obtain the position of the self-moving device in the map coordinate system, that is, position 8. Finally, the position 8, the direction 4 of the self-moving device, and the position 1 and direction 1 determined by the tachometer and IMU module are fused again to obtain the position 9 and direction 5, that is, the real position data of the self-moving device at the current moment.

[0074] In a possible implementation, the step of fusing the second fuselage position data with the first environment position data of the positioning tag in a map coordinate system includes:

[0075] Step D10, subtracting the second body position data from the first environment position data to obtain a position data difference corresponding to the positioning tag;

[0076] Step D20, detecting whether the position data difference is greater than a preset credibility threshold;

[0077] Step D30: If yes, the second body position data and the first environment position data are merged.

[0078] It should be noted that during the walking process of the self-moving device, it may encounter scenes of slipping or difficulty in walking, and the positioning signal may be interfered with by large obstacles, resulting in deviations in the positioning results. The judgment of the deviation in the positioning result is to obtain the positioning result obtained by the UWB positioning device on the self-moving device, and compare it with the conventional distance generated by the conventional movement speed and direction of the self-moving device. When the difference between the measured positioning result and the conventional distance is large, the positioning result measured by the UWB positioning device is not used as the current position of the self-moving device; when the difference between the measured positioning result and the conventional distance is small, the positioning results of the self-moving device by the positioning sensor and the UWB positioning device are compared. When the difference between the positioning results of the positioning sensor and the UWB positioning device is greater than the preset credibility threshold, the positioning result of the positioning sensor will not be added to the fusion positioning. Among them, the difference between the positioning results of the positioning sensor and the UWB positioning device is greater than the preset credibility threshold, indicating that the measurement result obtained by the positioning sensor is much greater than the measurement result obtained by the UWB positioning device, that is, the self-moving device may slip.

[0079] In addition, it should be noted that the wheel speed meter and IMU module both rely on the integration (of the speed / angular velocity) to calculate the position of the self-moving device. The positioning result is relatively accurate in a short period of time. When the self-moving device slips, the measurement data of the wheel speed meter and IMU will have a large error. When the wheel speed meter measurement result is much larger than the UWB measurement result, the fusion calculation can be performed by adjusting Q and R in the fusion calculation, that is, the value of the noise covariance matrix of the state value and the observation value.

[0080] For example, to help understand the technical concept or technical principle of this application, please refer to Figure 6 , Figure 6 A schematic diagram of the module structure of a UWB positioning system is provided. The UWB positioning system at least includes a wheel speed meter, an IMU module and a UWB system, as well as a computing unit and an actuator. The wheel speed meter measures the wheel speed data and a two-wheel differential model of a mobile device, the IMU module measures the angular direction data of the mobile device, and the RTK system measures the position data of a tag on the mobile device. The computing unit performs a fusion calculation on the wheel speed data measured by the wheel speed meter, i.e., the two-wheel differential model, the angular direction data measured by the IMU module, and the position data measured by the UWB system.

[0081] Embodiment 2

[0082] Based on the first embodiment of the present application, in another embodiment of the present application, the same or similar contents as those in the above-mentioned embodiment 1 can be referred to the above introduction, and will not be repeated in the following. Figure 7 The step of obtaining the second fuselage position data of the positioning tag in the map coordinate system includes:

[0083] Step E10, constructing a state transfer matrix of the self-moving device according to the first body position data and the single-step increment of the position of the self-moving device;

[0084] Step E20: determining the second fuselage position data of the positioning tag in the map coordinate system according to the state transfer matrix.

[0085] It should be noted that in order to determine the second body position data of the positioning tag and obtain the predicted value in the fusion positioning, the predicted value of the system state can be estimated, and the variance of the estimated predicted value is as follows:

[0086] Construct the state transfer matrix:

[0087]

[0088] Where, X = [xy θ 1] T is the system status, x and y are the position coordinates of the mobile device on the map coordinate system, is the state transfer matrix, θ is the angle between the orientation of the mobile device and the east direction, v and w are the single-step increments of the first body position data in position and direction, k represents the result of iterative calculation by the extended Kalman filter algorithm to the kth step, and the value of the state transfer matrix is That is the second body position data.

[0089] In a possible implementation manner, the step of obtaining the second fuselage position data of the positioning tag in the map coordinate system includes:

[0090] Step F10, obtaining wheel speed data, two-wheel differential model and first direction angle data from the mobile device in the device coordinate system;

[0091] Step F20, determining the second fuselage position data of the positioning tag in the map coordinate system according to the first fuselage data, the wheel speed data, the two-wheel differential model and the first direction angle data.

[0092] It should be noted that when selecting the predicted value in the fusion calculation, since the fusion calculation frequency is lower than the sampling frequency of the positioning sensor, the predicted value calculated from the original reading of the positioning sensor is more accurate. Therefore, the calculation result of the original reading is used instead of the predicted value obtained in the above embodiment.

[0093] In addition, it should be noted that the chassis of the self-moving device used in the present application has two driving wheels, and the wheels can rotate forward and backward. According to the size and installation method of the two driving wheels, the distance traveled by the driving wheels of the self-moving device within a time step is calculated respectively, that is, the motion characteristics of the two wheels are described by a two-wheel differential model, and the wheel speed data is obtained by counting the number of pulses of the photoelectric sensor of the wheel speed meter within a period of time, and converting the number of pulses to obtain the rotation speed data of the driving wheel. In addition, the IMU module can calculate three attitude angles, namely roll angle, pitch angle and azimuth angle, by measuring the angular velocity and acceleration of the self-moving device. At the same time, the change data of the azimuth angle within the current time is the azimuth angle data. Through the two-wheel differential model, combined with the wheel speed and the azimuth angle data given by the IMU module, the current position and azimuth angle of the self-moving device in the carrier coordinate system can be calculated. The incremental value, which can be transformed to the map coordinate system through rotation and translation, thereby obtaining the first body position data of the self-moving device in the map coordinate system.

[0094] In addition, it should be noted that in order to determine the second body position data of the tag in the map coordinate system, it can also be determined by the positional relationship between the tag and the self-mobile device. According to the first body position of the self-mobile device and combined with the position coordinates of the positioning tag in the device coordinate system, the first body position data is converted into the position data of the positioning tag. Similarly, the wheel speed data, two-wheel differential data and first direction angle data obtained by the IMU module and the speedometer are used to obtain the speed, angular velocity and predicted value of the movement direction of the self-mobile device based on the position coordinates of the positioning tag in the device coordinate system. Thus, the position data and direction data are combined to obtain the second body position data of the positioning tag in the map coordinate system.

[0095] For example, in one possible implementation, the two-wheel differential model, combined with the angular data provided by the wheel speed meter and the IMU module, can be used to calculate the increment dx of the current position and angular direction of the self-mobile device in the device coordinate system. odom ,dy odom and dθ odom , increment dx odom ,dy odom and dθ odom The center position prediction value of the mobile device is obtained by transforming it to the map coordinate system through rotation and translation. and The predicted value of the center position of the self-moving device is used to represent the position of the center point of the movement of the self-moving device on the map coordinate system when the self-moving device is moving.

[0096] Let the position of the label in the coordinate system of the mobile device be x tag_b ,y tag_b, the predicted position of the label in the map coordinate system can be obtained through translation and rotation transformation and

[0097]

[0098]

[0099] According to the speed v of the mobile device center and angular velocity w center , the speed v of the tag can be obtained through geometric relationship tag and angular velocity w tag , and the predicted value of the movement direction of the mobile device

[0100] w tag =w center

[0101]

[0102]

[0103] in

[0104] v tag_x =v center +v tag_b ×cosθ tag_b

[0105] v tag_y =v center +v tag_b ×sinθ tag_b

[0106] in

[0107] v tag_b =w center × tag_b

[0108] θ tag_b =arctan(y tag_b / x tag_b )

[0109] r tag_b =(x tag_b 2 +y tag_b 2 ) 0.5

[0110] Embodiment 3

[0111] The present invention also provides a self-moving device positioning device, which is applied to a UWB positioning system. The UWB positioning system is provided with a self-moving device, a positioning sensor, a positioning tag and at least one positioning base station. Figure 8 , the self-mobile equipment positioning device comprises:

[0112] A determination module 10, configured to determine second environmental position data of the self-mobile device in the map coordinate system according to first environmental position data of the positioning tag in the map coordinate system, wherein the map coordinate system is generated based on the position coordinates of the positioning base station;

[0113] The fusion module 20 is used to locate the self-moving device by fusing the second environment position data and the first body position data of the self-moving device in the device coordinate system, wherein the first body position data is collected by the positioning sensor.

[0114] Optionally, the self-moving device positioning device further includes a generating module, and the generating module is used to:

[0115] Constructing at least one pre-selected map coordinate system with each of the positioning base stations as the origin center;

[0116] Converting the position coordinates of each positioning base station in each pre-selected map coordinate system to obtain the center coordinates of each positioning base station;

[0117] The map coordinate system is generated according to the central coordinates.

[0118] Optionally, the determining module 10 is further configured to:

[0119] Acquire the second fuselage position data of the positioning tag in the map coordinate system;

[0120] The second environment position data of the mobile device in the map coordinate system is determined by fusing the second body position data and the first environment position data of the positioning tag in the map coordinate system.

[0121] Optionally, the self-moving device positioning device further includes an acquisition module, and the acquisition module is used to:

[0122] Constructing a state transfer matrix of the self-moving device according to the first body position data and the single-step increment of the position of the self-moving device;

[0123] According to the state transfer matrix, second fuselage position data of the positioning tag in the map coordinate system is determined.

[0124] Optionally, the acquisition module is further used for:

[0125] Obtain wheel speed data, two-wheel differential model and first direction angle data from the mobile device in the device coordinate system;

[0126] The second body position data of the positioning tag in the map coordinate system is determined according to the first body data, the wheel speed data, the two-wheel differential model and the first direction angle data.

[0127] Optionally, the fusion module 20 is further used for:

[0128] Acquire first body position data of the self-mobile device in a device coordinate system;

[0129] Determine second direction angle data of the mobile device in the map coordinate system according to the first environmental position data;

[0130] The self-mobile device is positioned by obtaining target positioning data by fusing the first body position data, the second environment position data and the second direction angle data.

[0131] Optionally, the self-moving device positioning device further includes a detection module, and the detection module is further used to:

[0132] Subtract the second body position data from the first environment position data to obtain a position data difference corresponding to the positioning tag;

[0133] Detecting whether the position data difference is greater than a preset credibility threshold;

[0134] If so, the second body position data and the first environment position data of the positioning tag in the map coordinate system are fused.

[0135] Embodiment 4

[0136] An embodiment of the present invention provides a self-moving device, which includes: a front end, a body; at least one processor; and a positioning device and a memory that are communicatively connected to the at least one processor; wherein the memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the self-moving device positioning method in the above-mentioned embodiment 1 to control the positioning of the positioning device.

[0137] like Fig. 9As shown, the self-mobile device may include: a processor 1001, such as a central processing unit (CPU), a communication bus 1002, a user interface 1003, a network interface 1004, a memory 1005, a positioning device 1006, a vehicle body 1007, and a front 1008. Among them, the communication bus 1002 is used to realize the connection and communication between these components. The user interface 1003 may include a display screen (Display), an input unit such as a keyboard (Keyboard), and the optional user interface 1003 may also include a standard wired interface and a wireless interface. The network interface 1004 may optionally include a standard wired interface and a wireless interface (such as a wireless fidelity (WIreless-FIdelity, WI-FI) interface). The memory 1005 may be a high-speed random access memory (Random Access Memory, RAM) memory, or a stable non-volatile memory (Non-Volatile Memory, NVM), such as a disk memory. The memory 1005 may also be a storage device independent of the aforementioned processor 1001. The positioning device 1006 may include an RTK module and a positioning sensor. The positioning device 1006 may also be a self-propelled device positioning device independent of the aforementioned processor 1001. The head 1008 is detachably connected to the vehicle body 1007, the processor 1001, the communication bus 1002, the user interface 1003, the network interface 1004, the memory 1005, and the positioning device 1006 are arranged on the vehicle body 1007.

[0138] Those skilled in the art will understand that Fig. 9 The structure shown in the figure does not constitute a limitation on the self-moving device, and may include more or less components than shown in the figure, or combine certain components, or arrange the components differently.

[0139] like Fig. 9 As shown, the memory 1005 as a storage medium may include an operating system, a data storage module, a network communication module, a user interface module, and a self-mobile device positioning program.

[0140] exist Fig. 9 In the self-mobile device shown, the network interface 1004 is mainly used for data communication with other devices; the user interface 1003 is mainly used for data interaction with the user; the processor 1001, memory 1005 and positioning device 1006 in the self-mobile device of the present invention can be set in the self-mobile device, and the self-mobile device calls the self-mobile device positioning program stored in the memory 1005 through the processor 1001, and controls the positioning device 1006 to execute the self-mobile device positioning method provided in the embodiment of the present invention.

[0141] The self-moving device provided by the present invention adopts the self-moving device positioning method in the above embodiment, which can solve the technical problem that the existing UWB positioning system is prone to ranging errors near large obstacles, resulting in low working safety of the self-moving device and reduced positioning accuracy. Compared with the prior art, the beneficial effects of the readable storage medium provided by the embodiment of the present invention are the same as the beneficial effects of the self-moving device positioning method provided by the above embodiment 1 or embodiment 2, and will not be repeated here.

[0142] It should be understood that the various parts of the present disclosure can be implemented with hardware, software, firmware or a combination thereof. In the description of the above embodiments, specific features, structures, materials or characteristics can be combined in any one or more embodiments or examples in a suitable manner.

[0143] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

[0144] Embodiment 5

[0145] An embodiment of the present invention provides a readable storage medium having readable program instructions stored thereon, and the readable program instructions are used to execute the self-mobile device positioning method in the above-mentioned embodiment 1.

[0146] The readable storage medium provided in the embodiment of the present invention may be, for example, a USB flash drive, but is not limited to electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, systems or devices, or any combination thereof. More specific examples of readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof. In this embodiment, the readable storage medium may be any tangible medium containing or storing a program that may be used by or in combination with an instruction execution system, system or device. The program code contained on the readable storage medium may be transmitted using any appropriate medium, including, but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination thereof.

[0147] The above-mentioned readable storage medium may be included in the electronic device; or may exist independently without being assembled into the electronic device.

[0148] The above-mentioned readable storage medium carries one or more programs. When the above-mentioned one or more programs are executed by an electronic device, the electronic device: determines the second environmental position data of the self-mobile device in the map coordinate system according to the first environmental position data of the positioning tag in the map coordinate system, wherein the map coordinate system is generated based on the position coordinates of the positioning base station; and locates the self-mobile device by fusing the second environmental position data and the first body position data of the self-mobile device in the device coordinate system, wherein the first body position data is collected by the positioning sensor.

[0149] Computer program code for performing the operations of the present disclosure may be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a separate software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0150] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to various embodiments of the present invention. In this regard, each square box in the flow chart or block diagram can represent a module, a program segment or a part of a code, and the module, the program segment or a part of the code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two square boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each square box in the block diagram and / or flow chart, and the combination of the square boxes in the block diagram and / or flow chart can be implemented with a dedicated hardware-based system that performs the specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0151] The modules involved in the embodiments described in the present disclosure may be implemented by software or hardware, wherein the name of the module does not limit the unit itself in some cases.

[0152] The readable storage medium provided by the present invention is a computer-readable storage medium, which stores readable program instructions for executing the above-mentioned self-moving device positioning method, and can solve the technical problem that the existing UWB positioning system is prone to ranging errors near large obstacles, resulting in low working safety of the self-moving device and reduced positioning accuracy. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided in the embodiment of the present invention are the same as the beneficial effects of the self-moving device positioning method provided in the above-mentioned embodiment 1 or embodiment 2, and will not be repeated here.

[0153] Embodiment 6

[0154] An embodiment of the present invention further provides a computer program product, including a computer program, which implements the steps of the above-mentioned method for positioning a mobile device when executed by a processor.

[0155] The computer program product provided by the present application can solve the technical problem that the existing UWB positioning system is prone to ranging errors near large obstacles, resulting in low working safety of the self-moving device and reduced positioning accuracy. Compared with the prior art, the beneficial effects of the computer program product provided by the embodiment of the present invention are the same as the beneficial effects of the self-moving device positioning method provided by the above-mentioned embodiment 1, embodiment 2 or embodiment 3, and will not be repeated here.

[0156] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or system including the element.

[0157] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0158] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes a number of instructions for a terminal device (which can be a mobile phone, a computer, a server, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0159] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for positioning a self-moving device, characterized in that: A UWB positioning system applied to a large obstacle, wherein the UWB positioning system is provided with a self-moving device, a positioning sensor, a positioning tag and at least one positioning base station, and the self-moving device positioning method comprises: Determine the installation position of the positioning tag on the self-mobile device, convert the first body position data through a state transfer matrix, and obtain the second body position data of the positioning tag in a map coordinate system; Subtract the second body position data from the first environment position data to obtain a position data difference value corresponding to the positioning tag, and when the position data difference value is greater than a preset credibility threshold, fuse the second body position data with the first environment position data of the positioning tag in a map coordinate system to determine the second environment position data of the mobile device in the map coordinate system, wherein the map coordinate system is generated based on the position coordinates of the positioning base station; Acquire first body position data of the self-mobile device in the device coordinate system, determine second direction angle data of the self-mobile device in the map coordinate system according to the first environment position data, and locate the self-mobile device by obtaining target positioning data by fusing the first body position data, the second environment position data and the second direction angle data, wherein the first body position data is acquired by the positioning sensor; The step of locating the second body position data of the positioning tag in the map coordinate system includes: constructing a state transfer matrix of the self-mobile device according to the first body position data and the position single-step increment of the self-mobile device, and determining the second body position data of the positioning tag in the map coordinate system according to the state transfer matrix; or The wheel speed data, the two-wheel differential model and the first direction angle data of the mobile device in the device coordinate system are obtained, and the second body position data of the positioning tag in the map coordinate system is determined according to the first body position data, the wheel speed data, the two-wheel differential model and the first direction angle data.

2. The method for positioning a self-moving device according to claim 1, characterized in that: Before the step of determining the installation position of the positioning tag on the self-mobile device, converting the first body position data through a state transfer matrix, and obtaining the second body position data of the positioning tag in a map coordinate system, the self-mobile device positioning method includes: Constructing at least one pre-selected map coordinate system with each of the positioning base stations as the origin center; Converting the position coordinates of each positioning base station in each pre-selected map coordinate system to obtain the center coordinates of each positioning base station; The map coordinate system is generated according to the central coordinates.

3. A self-propelled equipment positioning device, characterized in that: A UWB positioning system applied to a large obstacle, wherein the UWB positioning system is provided with a self-moving device, a positioning sensor, a positioning tag and at least one positioning base station, and the self-moving device positioning device comprises: A determination module, used to determine the installation position of the positioning tag on the self-mobile device, convert the first fuselage position data through a state transfer matrix, and obtain the second fuselage position data of the positioning tag in a map coordinate system; A first fusion module is used to make a difference between the second fuselage position data and the first environment position data to obtain a position data difference value corresponding to the positioning tag, and when the position data difference value is greater than a preset credibility threshold, fuse the second fuselage position data and the first environment position data of the positioning tag in a map coordinate system to determine the second environment position data of the mobile device in the map coordinate system, wherein the map coordinate system is generated based on the position coordinates of the positioning base station; a second fusion module, configured to obtain first body position data of the self-mobile device in the device coordinate system, determine second azimuth data of the self-mobile device in the map coordinate system according to the first environmental position data, and locate the self-mobile device by obtaining target positioning data by fusing the first body position data, the second environmental position data and the second azimuth data, wherein the first body position data is acquired by the positioning sensor; The determining module is also used for: constructing a state transfer matrix of the self-mobile device according to the first body position data and the position single-step increment of the self-mobile device, and determining the second body position data of the positioning tag in the map coordinate system according to the state transfer matrix; or The wheel speed data, the two-wheel differential model and the first direction angle data of the mobile device in the device coordinate system are obtained, and the second body position data of the positioning tag in the map coordinate system is determined according to the first body position data, the wheel speed data, the two-wheel differential model and the first direction angle data.

4. A self-propelled device, characterized in that: The self-moving device comprises: a vehicle head, a vehicle body, a positioning device, a memory, a processor and a self-moving device positioning program stored in the memory and executable on the processor, the vehicle head is detachably connected to the vehicle body, the positioning device, the memory and the processor are arranged on the vehicle body, and the self-moving device positioning program is configured to implement the steps of the self-moving device positioning method as described in any one of claims 1 to 2.

5. A readable storage medium, characterized in that: The readable storage medium stores a self-mobile device positioning program, and when the self-mobile device positioning program is executed by the processor, the steps of the self-mobile device positioning method according to any one of claims 1 to 2 are implemented.

Images