Ultrahigh-precision positioning method and system
Through the combination of UWB base station and lidar, high-precision positioning data of the target to be located are obtained and matched, which solves the problem of insufficient UWB positioning accuracy and the inability to identify lidar, and realizes cm-level positioning accuracy and accurate labeling to meet intelligent manufacturing demand.
Patent Information
- Application Number
- CN202411165270.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-23
- Publication Date
- 2024-12-27
AI Technical Summary
The existing UWB high-precision positioning system cannot achieve positioning accuracy below 10cm in intelligent manufacturing, and although lidar can achieve object recognition below 10cm, it cannot obtain the specific identification of the object through the communication process, and cannot constitute an applicable positioning system.
By combining UWB base station and lidar, the position data of the lidar and the preliminary position data of the target to be located are obtained, and whether it is within the coverage range of the lidar is determined, the relative position data of the target to be located is calculated, and the position data of the lidar is combined with the position data of the lidar is determined. and match relationship data to achieve high-precision positioning, and solve the problem of matching static object positioning and identification through the matching of device identification and positioning data.
It realizes cm-level positioning accuracy, meets the needs of intelligent manufacturing, and accurately identifies the target to be positioned, outputs specific positioning data and equipment identification, and completes the functions of the ultra-high-precision positioning system.
Smart Images
Figure CN119197516A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of intelligent manufacturing, and in particular relates to an ultra-high precision positioning method and system. Background Art
[0002] High-precision positioning refers to positioning with "sub-meter" accuracy. UWB, or ultra-wideband technology, is the most typical technology for achieving "sub-meter" high-precision positioning. Limited by the technical characteristics and application environment of UWB, the UWB positioning error in actual application scenarios is 30cm to 50cm.
[0003] In some smart manufacturing scenarios, production monitoring requires a positioning error as low as less than 10 cm, but the UWB-based high-precision positioning system cannot achieve such positioning accuracy.
[0004] The object recognition accuracy of LiDAR can reach less than 10cm and can be used for positioning applications. However, LiDAR can only know the location of an object. LiDAR cannot obtain the specific identification of the object through the communication process. Therefore, LiDAR cannot form a positioning system suitable for intelligent manufacturing.
[0005] Chinese patent CN107816990A discloses a positioning method and positioning device, which includes: obtaining M actual position information groups through a first positioning system, wherein the i-th actual position information group in the M actual position information groups includes N actual position information of the i-th object to be identified; obtaining N reference position information of the target object through a second positioning system; and determining that the j-th object to be identified among the M objects to be identified is the target object according to the M actual position information groups and the N reference position information of the target object. However, this method matches the positioning and identification of the target to be located by comparing the motion vector group, and cannot match the positioning and identification of the target to be located for static objects. Summary of the invention
[0006] The purpose of the present invention is to provide an ultra-high precision positioning method and system in order to overcome the defects of the above-mentioned prior art.
[0007] The purpose of the present invention can be achieved by the following technical solutions:
[0008] The present invention provides an ultra-high precision positioning method, comprising the following steps:
[0009] Step S1: The UWB base station obtains the position data of the laser radar and the preliminary position data of the target to be located according to the UWB tags of the laser radar and the target to be located;
[0010] Step S2: judging whether a target to be located is within the coverage of the laser radar through the laser radar position data, the preliminary position data of the target to be located and the three-dimensional visible area of the laser radar, and forming matching relationship data between the target to be located and the laser radar;
[0011] Step S3: The laser radar locates the target to be located within the coverage area to form relative position data of the target to be located;
[0012] Step S4: Calculate and obtain high-precision positioning data of the target to be positioned based on the position data of the laser radar and the relative position data of the target to be positioned;
[0013] Step S5: According to the preliminary position data of the target to be located, the matching relationship data between the target to be located and the laser radar, and the high-precision positioning data of the target to be located, the identifier of the target to be located is matched with the high-precision positioning data of the target to be located. The matching process includes: matching the device identifier of the laser radar in the matching relationship data between the target to be located and the laser radar with the high-precision positioning data of the target to be located, matching to the high-precision positioning data of multiple targets to be located, calculating the distance between the preliminary position data of the target to be located in the matching relationship data and the high-precision positioning data of multiple targets to be located, selecting the high-precision positioning data with the shortest distance, selecting the device identifier of the target to be located in the matching relationship data and the high-precision coordinates of the selected high-precision positioning data, matching the device identifier of the target to be located with the high-precision coordinates of the target to be located, traversing the matching relationship data, and matching all targets to be located.
[0014] Furthermore, when the UWB base station acquires the location data through the UWB tag, it also acquires the device identification of the device carrying the UWB tag.
[0015] Furthermore, the device identification includes device type, device name, and device number.
[0016] Furthermore, the installation position of the laser radar satisfies that the three-dimensional visible area of all laser radars covers all targets to be located.
[0017] Furthermore, the process of determining whether a target to be located is within the coverage of the laser radar in step S2 includes: based on the positioning of the laser radar Ly by the UWB base station and the three-dimensional visible area of the laser radar, the positioning of the UWB tag Tx of the target to be located by the UWB base station, determining whether the target to be located is within the coverage of the laser radar; if so, forming matching relationship data between the target to be located and the laser radar; if the same target to be located is within the coverage of different laser radars at the same time, the laser radar that is closer to the target to be located is preferentially matched.
[0018] Furthermore, the matching relationship data between the target to be located and the laser radar is R(Ly, Tx), where LY is the device identifier of the laser radar, Tx includes the device identifier of the target to be located and the preliminary position data of the target to be located, and the preliminary position data of the target to be located is the three-dimensional coordinate in the UWB base station coordinate system.
[0019] Furthermore, the relative position data of the target to be located in step S3 is a three-dimensional coordinate in a laser radar coordinate system.
[0020] Furthermore, the position data of the laser radar is a three-dimensional coordinate in the UWB base station coordinate system.
[0021] Furthermore, the calculation process of step S4 includes: the position data of the laser radar is (LY, Xj, Yj, Zj), the relative position data of the target to be located is (Xp, Yp, Zp), and it is determined whether the position data of the laser radar and the relative position data of the target to be located are in the same coordinate system. If so, the ultra-high precision positioning data of the target to be located is (LY, Xj+Xp, Yj+Yp, Zj+Zp), where LY is the device identifier of the laser radar, (Xj+Xp, Yj+Yp, Zj+ Zp) is the ultra-high precision coordinate of the target to be positioned in the UWB base station coordinate system. If not, the position data of the laser radar and the relative position data of the target to be positioned are converted into position data in the same coordinate system. The position data of the laser radar after conversion is (LY, X2j, Y2j, Z2j), and the relative position data of the target to be positioned is (X2p, Y2p, Z2p). Then the ultra-high precision positioning data of the target to be positioned is (LY, X2j+X2p, Y2j+Y2p, Z2j+Z2p).
[0022] The present invention also provides an ultra-high precision positioning system, including a UWB positioning module, a laser radar positioning module, and an ultra-high precision positioning calculation module. The UWB positioning module includes a UWB base station and a UWB tag set on a target to be located and a laser radar. The UWB tag periodically sends a UWB positioning pulse signal, and the UWB base station receives and measures the positioning pulse signal of the UWB positioning tag to obtain positioning data. The laser radar positioning module is used to generate ultra-high precision positioning data of the target to be located. The ultra-high precision positioning calculation module uses the position data of the laser radar, the matching relationship data between the target to be located and the laser radar, and the position data of the laser radar for locating the target to be located within the coverage range to form ultra-high precision positioning data of the target to be located and a specific device identification match.
[0023] Compared with the prior art, the present invention has the following advantages:
[0024] (1) The positioning data of the target to be positioned in the present invention is generated by laser radar measurement, and the accuracy can reach the centimeter level, meeting the ultra-high precision positioning requirements in the field of intelligent manufacturing.
[0025] (2) In the present invention, the positioning identifier of the target to be positioned is identified by the UWB tag, and the system can accurately know which specific UWB tag the positioning data points to, and accurately identify the target to be positioned.
[0026] (3) The method and device of the present invention finally output the specific identification and positioning data of the target to be located, thus completing the system function of ultra-high precision positioning.
[0027] (4) The present invention calculates the distance between the preliminary positioning data of the target to be located and the high-precision positioning data, and matches the identification of the target to be located with the high-precision positioning data, thereby solving the technical problem in the prior art that the positioning and identification of the target to be located must be matched by comparing the motion vector group, and the positioning and identification of the target to be located cannot be matched for static objects. BRIEF DESCRIPTION OF THE DRAWINGS
[0028] Figure 1 is a system diagram of the present invention;
[0029] Figure 2 A three-dimensional visible area diagram of the laser radar of the present invention;
[0030] Figure 3 is the lidar coverage map;
[0031] Figure 4 is the coordinate translation transformation diagram;
[0032] Figure 5 Rotate the spatial geometry diagram around a single coordinate axis. DETAILED DESCRIPTION
[0033] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are 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 should fall within the scope of protection of the present invention.
[0034] The present invention provides an ultra-high precision positioning method, characterized in that it comprises the following steps:
[0035] Step S1: The UWB base station obtains the position data of the laser radar and the position data of the target to be located according to the UWB tags of the laser radar and the target to be located;
[0036] Step S2: judging whether a target to be located is within the coverage of the laser radar through the position data of the laser radar, the position data of the target to be located and the three-dimensional visible area of the laser radar, and forming matching relationship data between the target to be located and the laser radar;
[0037] Step S3: The laser radar locates the target to be located within the coverage area to form relative position data of the target to be located;
[0038] Step S4: Calculate and obtain ultra-high precision positioning data of the target to be positioned based on the position data of the laser radar and the relative position data of the target to be positioned;
[0039] Step S5: According to the position data of the target to be located, the matching relationship data between the target to be located and the laser radar, and the ultra-high precision positioning data of the target to be located, the identification of the target to be located is matched with the ultra-high precision positioning data of the target to be located; when the UWB base station obtains the position data through the UWB tag, it also obtains the device identification of the device carrying the UWB tag.
[0040] The device identification includes the device type, device name, and device number. LiDAR has a certain three-dimensional visual area, such as Figure 2 As shown, taking Hesai AT128 as an example, the horizontal field of view is 120°, the vertical field of view is 25.4°, and the detection distance is 210m. The installation position of the laser radar satisfies that the three-dimensional visible area of all laser radars covers all targets to be located. Since laser radar is used to achieve higher-precision positioning, M laser radars need to cover the movable range of the positioning target, and the positioning target itself has position restrictions. Principles of position selection: full coverage or as few as possible, for example Figure 3 As shown: The rectangular area is the movable area for positioning the target. The laser radars are placed side by side and staggered.
[0041] Step S2 determines whether a target to be located is within the coverage of the laser radar. The process includes: based on the positioning of the laser radar Ly by the UWB base station and the three-dimensional visible area of the laser radar, the positioning of the UWB tag Tx of the target to be located by the UWB base station, and determining whether the target to be located is within the coverage of the laser radar. If so, matching relationship data between the target to be located and the laser radar is formed. If the same target to be located is within the coverage of different laser radars at the same time, the laser radar that is closer to the target to be located is prioritized.
[0042] The matching relationship data between the target to be located and the laser radar is R(Ly, Tx), where LY is the device identifier of the laser radar, and Tx includes the device identifier of the target to be located and the position data of the target to be located. The position data of the target to be located is the three-dimensional coordinates in the UWB base station coordinate system.
[0043] The relative position data of the target to be located in step S3 is the three-dimensional coordinates in the laser radar coordinate system.
[0044] The position data of the laser radar is the three-dimensional coordinates in the UWB base station coordinate system.
[0045] The calculation process of step S4 includes: the position data of the laser radar is (LY, Xj, Yj, Zj), the relative position data of the target to be located is (Xp, Yp, Zp), and it is determined whether the position data of the laser radar and the relative position data of the target to be located are in the same coordinate system. If so, the ultra-high precision positioning data of the target to be located is (LY, Xj+Xp, Yj+Yp, Zj+Zp), where LY is the device identifier of the laser radar, (Xj+Xp, Yj+Yp, Zj+Zp) is the ultra-high precision coordinates of the target to be positioned in the UWB base station coordinate system. If not, the position data of the laser radar and the relative position data of the target to be positioned are converted into position data in the same coordinate system. The position data of the laser radar after conversion is (LY, X2j, Y2j, Z2j), and the relative position data of the target to be positioned is (X2p, Y2p, Z2p). Then the ultra-high precision positioning data of the target to be positioned is (LY, X2j+X2p, Y2j+Y2p, Z2j+Z2p).
[0046] The principle of coordinate system conversion is as follows:
[0047] like Figure 4 Figure 5 As shown, the coordinate axis is moved from the first position OX, OY and OZ to the second position OX', O'Y and OZ', that is, O'X', O'Y and O2' are parallel to OX, OY and OZ respectively. This method is called the translation of the coordinate system. Assume that the coordinates of the new origin O' with respect to the old coordinate system are (a, b, c), and the coordinates of point P with respect to the old system and the new system are (x, y, z) and (x, y, z') respectively. Then according to Figure 4 The spatial geometric relationship of can be concluded:
[0048]
[0049] or
[0050]
[0051] Formula 1 and Formula 2 are called coordinate transformation formulas under micro-coordinate axis translation, or translation formulas for short.
[0052] In radar data processing systems, for the convenience of expression, the spatial coordinate position of the target is usually represented by a vector. Assume that a certain spatial coordinate system is defined as OX a Y a Z a , then the coordinate X of any point P in the coordinate systema Available vector formula: X a =[X xa X ya X za ]′, where X xa , X ya and X za They are respectively represented as the corresponding positions of point P on each coordinate axis. Equations 3 to 5 can be expressed as:
[0053] X b =L1X a
[0054] X b =L2X a
[0055] X b =L3X a
[0056] in
[0057]
[0058] It is called the basic rotation matrix around the Z axis, X axis, and Y axis. The rotation transformation relationship between any two coordinate systems can be obtained by synthesizing the basic rotation matrix.
[0059] If the coordinate system OX b Y b Z b The coordinate system OX a Y a Z a Counterclockwise rotation angle around the X axis, Y axis, and Z axis respectively Then we get the coordinate vector of point P in the new coordinate system as: b =[X ab X yb X zb ]′, then X a and X b The coordinate rotation transformation relationship is:
[0060] X b =L ab X a
[0061] in: It is called the transformation matrix from the a coordinate system to the b coordinate system. It is not difficult to prove that the coordinate transformation matrix L ba The following reversibility and orthogonality conditions are satisfied:
[0062]
[0063] The matching process of step S5 includes: matching the device identifier LY of the laser radar in the matching relationship data between the target to be located and the laser radar with LY in the ultra-high precision positioning data of the target to be located, matching the ultra-high precision positioning data of multiple targets to be located, calculating the distance between the position data of the target to be located in the matching relationship data and the ultra-high precision positioning data of multiple targets to be located, selecting the ultra-high precision positioning data with the smallest distance, selecting the device identifier of the target to be located in the matching relationship data and the ultra-high precision coordinates of the selected ultra-high precision positioning data, matching the device identifier of the target to be located with the ultra-high precision coordinates of the target to be located, traversing the matching relationship data, and matching all targets to be located.
[0064] The present invention also provides an ultra-high precision positioning system, including a UWB positioning module, a laser radar positioning module, and an ultra-high precision positioning calculation module. The UWB positioning module includes a UWB base station and a UWB tag set on a target to be located and a laser radar. The UWB tag periodically sends a UWB positioning pulse signal, and the UWB base station receives and measures the positioning pulse signal of the UWB positioning tag to obtain positioning data. The laser radar positioning module is used to generate ultra-high precision positioning data of the target to be located. The ultra-high precision positioning calculation module uses the position data of the laser radar, the matching relationship data between the target to be located and the laser radar, and the position data of the laser radar for locating the target to be located within the coverage range to form ultra-high precision positioning data of the target to be located and a specific device identification match.
[0065] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of various equivalent modifications or replacements within the technical scope disclosed by the present invention, and these modifications or replacements should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.
Claims
1. An ultra-high precision positioning method, characterized in that: The following steps are involved: Step S1: The UWB base station obtains the position data of the laser radar and the preliminary position data of the target to be located according to the UWB tags of the laser radar and the target to be located; Step S2: judging whether a target to be located is within the coverage of the laser radar through the laser radar position data, the preliminary position data of the target to be located and the three-dimensional visible area of the laser radar, and forming matching relationship data between the target to be located and the laser radar; Step S3: The laser radar locates the target to be located within the coverage area to form relative position data of the target to be located; Step S4: Calculate and obtain high-precision positioning data of the target to be positioned based on the position data of the laser radar and the relative position data of the target to be positioned; Step S5: According to the preliminary position data of the target to be located, the matching relationship data between the target to be located and the laser radar, and the high-precision positioning data of the target to be located, the identifier of the target to be located is matched with the high-precision positioning data of the target to be located. The matching process includes: matching the device identifier of the laser radar in the matching relationship data between the target to be located and the laser radar with the high-precision positioning data of the target to be located, matching to the high-precision positioning data of multiple targets to be located, calculating the distance between the preliminary position data of the target to be located in the matching relationship data and the high-precision positioning data of multiple targets to be located, selecting the high-precision positioning data with the shortest distance, selecting the device identifier of the target to be located in the matching relationship data and the high-precision coordinates of the selected high-precision positioning data, matching the device identifier of the target to be located with the high-precision coordinates of the target to be located, traversing the matching relationship data, and matching all targets to be located.
2. The ultra-high precision positioning method according to claim 1, characterized in that: When the UWB base station acquires the location data through the UWB tag, it also obtains the device identification of the device carrying the UWB tag.
3. The ultra-high precision positioning method according to claim 2, characterized in that: The device identification includes device type, device name, and device number.
4. The ultra-high precision positioning method according to claim 1, characterized in that: The installation position of the laser radar satisfies that the three-dimensional visual area of all laser radars covers all targets to be located.
5. The ultra-high precision positioning method according to claim 1, characterized in that: The process of step S2 for judging whether a target to be located is within the coverage range of the laser radar includes: based on the positioning of the laser radar Ly by the UWB base station and the three-dimensional visible area of the laser radar, the positioning of the UWB tag Tx of the target to be located by the UWB base station, judging whether the target to be located is within the coverage range of the laser radar, and if so, forming matching relationship data between the target to be located and the laser radar, if the same target to be located is within the coverage range of different laser radars at the same time, the laser radar that is closer to the target to be located is matched first.
6. The ultra-high precision positioning method according to claim 5, characterized in that: The matching relationship data between the target to be located and the laser radar is R(Ly, Tx), where LY is the device identifier of the laser radar, and Tx includes the device identifier of the target to be located and the preliminary position data of the target to be located. The preliminary position data of the target to be located is the three-dimensional coordinates in the UWB base station coordinate system.
7. The ultra-high precision positioning method according to claim 1, characterized in that: The relative position data of the target to be located in step S3 is the three-dimensional coordinates in the laser radar coordinate system.
8. The ultra-high precision positioning method according to claim 1, characterized in that: The position data of the laser radar is a three-dimensional coordinate in the UWB base station coordinate system.
9. The ultra-high precision positioning method according to claim 1, characterized in that: The calculation process of step S4 includes: the position data of the laser radar is (LY, Xj, Yj, Zj), the relative position data of the target to be located is (Xp, Yp, Zp), and it is determined whether the position data of the laser radar and the relative position data of the target to be located are in the same coordinate system. If so, the ultra-high precision positioning data of the target to be located is (LY, Xj+Xp, Yj+Yp, Zj+Zp), where LY is the device identifier of the laser radar, (Xj+Xp, Yj+Yp, Zj+Zp ) is the ultra-high precision coordinate of the target to be positioned in the UWB base station coordinate system. If not, the position data of the laser radar and the relative position data of the target to be positioned are converted into position data in the same coordinate system. The position data of the laser radar after conversion is (LY, X2j, Y2j, Z2j), and the relative position data of the target to be positioned is (X2p, Y2p, Z2p). The ultra-high precision positioning data of the target to be positioned is (LY, X2j+X2p, Y2j+Y2p, Z2j+Z2p).
10. An ultra-high precision positioning system, characterized in that: It includes a UWB positioning module, a laser radar positioning module, and an ultra-high precision positioning calculation module. The UWB positioning module includes a UWB base station and a UWB tag set on the target to be located and the laser radar. The UWB tag periodically sends a UWB positioning pulse signal. The UWB base station receives and measures the positioning pulse signal of the UWB positioning tag to obtain positioning data. The laser radar positioning module is used to generate high-precision positioning data of the target to be located. The high-precision positioning calculation module uses the position data of the laser radar, the matching relationship data between the target to be located and the laser radar, and the position data of the laser radar for locating the target to be located within the coverage range to form high-precision positioning data of the target to be located and a specific device identification match.