Single-coordinate positioning method and device based on UWB and nine-axis gyroscope

Abstract

The invention provides a single-coordinate positioning method and device based on UWB and a nine-axis gyroscope. A positioning base point with reference coordinates of a preset coordinate system is arranged in a positioning space. The method comprises the following steps: firstly, accurately acquiring distance information between a positioning base point and a positioned point by using a UWB technology, and calculating attitude information of the positioned point in a preset coordinate system according to key parameters of a gyroscope of the positioned point; and locking the spherical surface of the positioned point by means of the distance information, fusing the attitude information to determine the relative coordinate of the positioned point on the spherical surface relative to the positioning base point, and finally superposing the relative coordinate and the reference coordinate to obtain the absolute coordinate. According to the method, UWB distance measurement and nine-axis gyroscope navigation are ingeniously fused, and more than four base stations required by traditional three-dimensional positioning are reduced to one base station. The method greatly reduces the cost and construction difficulty, plays an important role in the fields of safety monitoring and the like, can efficiently and accurately position a target, meets the multi-scene positioning requirements, powerfully pushes the development of a positioning technology, and provides reliable positioning support for related applications.

Description

Technical Field

[0001] The present invention belongs to the technical field of positioning, and in particular relates to a single-coordinate positioning method and device based on UWB and a nine-axis gyroscope. Background Art

[0002] Among modern positioning technologies, UWB positioning has attracted much attention for its high precision and high resolution. However, traditional UWB positioning solutions have obvious limitations. When performing plane positioning, at least three UWB base stations are required to provide relative distance information to determine the terminal location; and for three-dimensional spatial positioning, at least four or more UWB base stations are required to participate. This is because a single UWB base station can only obtain the distance information between it and the terminal and cannot independently complete the positioning solution.

[0003] In addition, the communication distance of UWB is generally limited to a hundred meters, and in indoor environments, obstacles such as walls and floors will greatly attenuate UWB signals. Therefore, in order to achieve comprehensive positioning coverage of a large range or complex indoor environment, a large number of UWB base stations must be deployed. This not only leads to a sharp increase in equipment costs, including the purchase, maintenance and subsequent upgrade costs of base station hardware equipment, but also involves a large amount of manpower and material investment in the deployment process, such as the site selection, installation, wiring and debugging of base stations, which makes the construction extremely difficult.

[0004] At the same time, with the increasing diversification and refinement of positioning needs, especially in the field of security monitoring, such as tracking the activity trajectory of people in large buildings and real-time location monitoring of important equipment in industrial plants, the high cost and high construction difficulty of traditional UWB positioning solutions limit their wider application and promotion. Therefore, a new positioning technology is urgently needed to overcome these problems, reduce costs and simplify the construction process while ensuring positioning accuracy. Summary of the invention

[0005] In view of this, the purpose of the present invention is to provide a single-coordinate positioning method and device based on UWB and a nine-axis gyroscope. By combining UWB ranging with nine-axis gyroscope inertial navigation, the high cost and high construction difficulty problems caused by the need for multiple base stations in traditional UWB positioning can be effectively solved, and accurate, efficient and economical positioning functions can be achieved to meet the needs of target positioning in fields such as security monitoring.

[0006] In order to achieve the above object, the technical solution provided by the present invention is as follows:

[0007] In a first aspect, the present invention provides a single-coordinate positioning method based on UWB and a nine-axis gyroscope, wherein a positioning base point is arranged in a positioning space, and the positioning base point has a reference coordinate in a preset coordinate system; the single-coordinate positioning method comprises the following steps:

[0008] The distance information between the positioning base point and the positioned point is obtained through UWB technology; the positioned point is a point in the positioning space;

[0009] Obtain the posture information of the located point in the preset coordinate system; the posture information of the located point is calculated based on the key parameters of the gyroscope of the located point;

[0010] Use the distance information to determine the spherical surface where the located point is located, and then combine the posture information to determine the relative coordinates of the located point on the spherical surface relative to the positioning base point;

[0011] The relative coordinates are superimposed on the reference coordinates to obtain the absolute coordinates of the located point in the preset coordinate system.

[0012] Furthermore, if the preset coordinate system is the geodetic coordinate system, the relative coordinates are determined according to the following formula:

[0013]

[0014] In the formula, (Xt, Yt, Zt) on the left side of the equal sign is the relative coordinate after iterative update at time t; R is the distance between the positioning base point and the positioned point, that is, the radius of the sphere; (Xt, Yt, Zt) on the right side of the equal sign is the relative coordinate before iterative update at time t, (Xse, Yse, Zse) is the displacement of the positioned point in the preset coordinate system, which is determined according to the posture information of the positioned point in the preset coordinate system; the initial value of the relative coordinate is 0.

[0015] Furthermore, the posture information of the located point in the preset coordinate system includes the posture angle and acceleration;

[0016] The displacement of the located point in the preset coordinate system is determined according to the following process:

[0017] Based on the acceleration in the attitude information, the acceleration value is calculated by iteratively accumulating the acceleration;

[0018] Iteratively calculate the displacement value by accumulating the acceleration value;

[0019] Using Euler rotation, combined with the attitude angle in the attitude information, the displacement value is mapped to the geodetic coordinate system, and the result is:

[0020] Xse=Xs×(cosZ×cosX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+

[0021] Zs×(sinZ×cosX-cosZ×sinY×sinX)

[0022] Yse=Xs×(-cosZ×sinX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+

[0023] Zs×(-sinZ×sinX-cosZ×sinY×cosX)

[0024] Zse=Xs×(-sinZ×cosY)+Ys×(sinY)+Zs×(cosZ×cosY)

[0025] Where (Xse, Yse, Zse) is the displacement value mapped to the earth coordinate system, (Xs, Ys, Zs) is the displacement value obtained by iterative calculation, and (X, Y, Z) is the attitude angle.

[0026] Furthermore, the key parameters of the gyroscope include:

[0027] Three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction;

[0028] Based on three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction, the posture information of the positioned point is calculated using the quaternion method.

[0029] Furthermore, the quaternion method is used to calculate the posture information of the positioned point, including:

[0030] Initialize quaternion q1=1, q2=q3=q4=0;

[0031] The three-axis acceleration and the three-axis magnetic flux direction are normalized and the three-axis angular velocity is converted into radians to obtain the processed acceleration (Xsa, Ysa, Zsa), magnetic flux angle (Xsm, Ysm, Zsm) and radians (Xsg, Ysg, Zsg);

[0032] The normalized three-axis magnetic flux direction is converted from the local coordinate system to the earth coordinate system to obtain:

[0033]

[0034] Where, (Xe, Ye) is the magnetic flux angle in the geodetic coordinate system; (Xsm, Ysm, Zsm) is the three-axis magnetic flux direction after normalization;

[0035] Based on the magnetic flux angle in the geodetic coordinate system, the X-axis of the navigation coordinate system is pointed to due north, and we get:

[0036]

[0037]

[0038] Where, (Xd, Zd) is the first intermediate variable;

[0039] The corrected error is calculated based on the first intermediate variable, and the following is obtained:

[0040]

[0041] Where, (S1, S2, S3, S4) is the correction error;

[0042] Normalize the correction error;

[0043] The second intermediate variable is calculated based on the normalized corrected error, and the result is:

[0044] Q t1 =0.5(-q2×Xg-q3×Yg-q4×Zg)-βS1

[0045] Q t2 =0.5(q1×Xg+q3×Zg-q4×Yg)-βS2

[0046] Q t3 =0.5(q1×Yg+q2×Zg+q4×Xg)-βS3

[0047] Q t4 =0.5(q1×Zg+q2×Yg+q3×Xg)-βS4

[0048] In the formula, (Q t1 ,Q t2 ,Q t3 ,Q t4 ) is the second intermediate variable, β is the compensation factor; the quaternion is iterated using the second intermediate variable as follows:

[0049] q1=Q t1 ×dt

[0050] q2=Q t2 ×dt

[0051] q3=Q t3 ×dt

[0052] q4=Q t4 ×dt

[0053] Normalize the iterated quaternion;

[0054] The attitude angle is calculated based on the normalized quaternion as follows:

[0055] X=-arctan(2×(q2×q4-q1×q3))×180 / π

[0056]

[0057] Where, (X, Y, Z) is the attitude angle;

[0058] The calculated attitude angle, three-axis acceleration and three-axis angular velocity are used as attitude information.

[0059] In a second aspect, the present invention further provides a single-coordinate positioning device based on UWB and a nine-axis gyroscope, wherein a positioning base point is arranged in the positioning space, and the positioning base point has a reference coordinate in a preset coordinate system;

[0060] The single coordinate positioning device includes:

[0061] Positioning base stations and positioning terminals;

[0062] The positioning base station is deployed at the positioning base point and is equipped with a UWB module;

[0063] The positioning terminal is deployed at the location to be positioned and is equipped with a UWB module and a gyroscope;

[0064] The positioning base station is used to obtain the distance information between the positioning base point and the positioned point through UWB technology; the positioned point is a point in the positioning space;

[0065] The positioning terminal is used to obtain the posture information of the positioned point in a preset coordinate system; the posture information of the positioned point is calculated based on the key parameters of the gyroscope of the positioned point;

[0066] The positioning base station is also used to use the distance information to determine the spherical surface on which the located point is located, and then combine the posture information to determine the relative coordinates of the located point on the spherical surface relative to the positioning base point; it is also used to superimpose the relative coordinates with the reference coordinates to obtain the absolute coordinates of the located point in the preset coordinate system.

[0067] Furthermore, if the preset coordinate system is the geodetic coordinate system, the relative coordinates are determined according to the following formula:

[0068]

[0069] In the formula, (Xt, Yt, Zt) on the left side of the equal sign is the relative coordinate after iterative update at time t; R is the distance between the positioning base point and the positioned point, that is, the radius of the sphere; (Xt, Yt, Zt) on the right side of the equal sign is the relative coordinate before iterative update at time t, (Xse, Yse, Zse) is the displacement of the positioned point in the preset coordinate system, which is determined according to the posture information of the positioned point in the preset coordinate system; the initial value of the relative coordinate is 0.

[0070] Furthermore, the posture information of the located point in the preset coordinate system includes the posture angle and acceleration;

[0071] The displacement of the located point in the preset coordinate system is determined according to the following process:

[0072] Based on the acceleration in the attitude information, the acceleration value is calculated by iteratively accumulating the acceleration;

[0073] Iteratively calculate the displacement value by accumulating the acceleration value;

[0074] Using Euler rotation, combined with the attitude angle in the attitude information, the displacement value is mapped to the geodetic coordinate system, and the result is:

[0075] Xse=Xs×(cosZ×cosX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+

[0076] Zs×(sinZ×cosX-cosZ×sinY×sinX)

[0077] Yse=Xs×(-cosZ×sinX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+

[0078] Zs×(-sinZ×sinX-cosZ×sinY×cosX)

[0079] Zse=Xs×(-sinZ×cosY)+Ys×(sinY)+

[0080] Zs×(cosZ×cosY)

[0081] Where (Xse, Yse, Zse) is the displacement value mapped to the earth coordinate system, (Xs, Ys, Zs) is the displacement value obtained by iterative calculation, and (X, Y, Z) is the attitude angle.

[0082] Furthermore, the key parameters of the gyroscope include:

[0083] Three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction;

[0084] Based on three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction, the posture information of the positioned point is calculated using the quaternion method.

[0085] Furthermore, the quaternion method is used to calculate the posture information of the positioned point, including:

[0086] Initialize quaternion q1=1, q2=q3=q4=0;

[0087] The three-axis acceleration and the three-axis magnetic flux direction are normalized and the three-axis angular velocity is converted into radians to obtain the processed acceleration (Xsa, Ysa, Zsa), magnetic flux angle (Xsm, Ysm, Zsm) and radians (Xsg, Ysg, Zsg);

[0088] The normalized three-axis magnetic flux direction is converted from the local coordinate system to the earth coordinate system to obtain:

[0089]

[0090]

[0091] Where, (Xe, Ye) is the magnetic flux angle in the geodetic coordinate system; (Xsm, Ysm, Zsm) is the three-axis magnetic flux direction after normalization;

[0092] Based on the magnetic flux angle in the geodetic coordinate system, the X-axis of the navigation coordinate system is pointed to due north, and we get:

[0093]

[0094]

[0095] Where, (Xd, Zd) is the first intermediate variable;

[0096] The corrected error is calculated based on the first intermediate variable, and the result is:

[0097]

[0098] Where, (S1, S2, S3, S4) is the correction error;

[0099] Normalize the correction error;

[0100] The second intermediate variable is calculated based on the normalized corrected error, and the result is:

[0101] Q t1 =0.5(-q2×Xg-q3×Yg-q4×Zg)-βS1

[0102] Q t2 =0.5(q1×Xg+q3×Zg-q4×Yg)-βS2

[0103] Q t3 =0.5(q1×Yg+q2×Zg+q4×Xg)-βS3

[0104] Q t4 =0.5(q1×Zg+q2×Yg+q3×Xg)-βS4

[0105] In the formula, (Q t1 ,Q t2 ,Q t3 ,Q t4 ) is the second intermediate variable, β is the compensation factor;

[0106] The quaternion is iterated using the second intermediate variable as follows:

[0107] q1=Q t1 ×dt

[0108] q2=Q t2 ×dt

[0109] q3=Q t3 ×dt

[0110] q4=Q t4 ×dt

[0111] Normalize the iterated quaternion;

[0112] The attitude angle is calculated based on the normalized quaternion as follows:

[0113] X=-arctan(2×(q2×q4-q1×q3))×180 / π

[0114]

[0115] Where, (X, Y, Z) is the attitude angle;

[0116] The calculated attitude angle, three-axis acceleration and three-axis angular velocity are used as attitude information.

[0117] In summary, the present invention provides a single-coordinate positioning method and device based on UWB and a nine-axis gyroscope, wherein a positioning base point is arranged in the positioning space, and the positioning base point has a reference coordinate in a preset coordinate system; the method includes obtaining the distance information between the positioning base point and the positioned point through UWB technology; the positioned point is a point in the positioning space; obtaining the posture information of the positioned point in the preset coordinate system; the posture information of the positioned point is calculated based on the key parameters of the gyroscope of the positioned point; using the distance information to determine the sphere where the positioned point is located, and then combining the posture information to determine the relative coordinates of the positioned point on the sphere relative to the positioning base point; superimposing the relative coordinates with the reference coordinates to obtain the absolute coordinates of the positioned point in the preset coordinate system. The present invention combines UWB ranging with nine-axis gyroscope inertial navigation to reduce more than four base stations required in three-dimensional positioning to one base station, thereby effectively solving the high cost and high construction difficulty caused by the need for multiple base stations in traditional UWB positioning, realizing accurate, efficient and economical positioning functions, and meeting the needs of target positioning in fields such as security monitoring. BRIEF DESCRIPTION OF THE DRAWINGS

[0118] In order to more clearly illustrate the embodiments of the present invention 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, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0119] Figure 1 It is a flow chart of a single coordinate positioning method based on UWB and nine-axis gyroscope;

[0120] Figure 2 Flow chart for attitude angle calculation;

[0121] Figure 3 This is the flow chart of absolute coordinate solution;

[0122] Figure 4 The block diagram of a single coordinate positioning device based on UWB and a nine-axis gyroscope is shown;

[0123] Figure 5 A block diagram of the composition of a computer device. DETAILED DESCRIPTION

[0124] In order to make the purpose, features and advantages of the present invention more obvious and easy to understand, the technical solutions 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 embodiments described below 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 are within the scope of protection of the present invention.

[0125] See also Figure 1 This embodiment provides a single coordinate positioning method based on UWB and a nine-axis gyroscope. The method first requires setting up a positioning base point in the positioning space, and the positioning base point has a reference coordinate in a preset coordinate system.

[0126] The main purpose of setting a positioning base point in the positioning space is to serve as a known reference point to provide a benchmark for determining the positions of other points to be positioned (such as mobile devices, target objects, etc.) in the space. By exchanging information with the points to be positioned (such as signal transmission, data exchange, etc.) and combining relevant measurement and calculation methods, the position information of the points to be positioned relative to the base station can be inferred, and then its specific coordinates in the entire positioning space can be determined. The preset coordinate system is a spatial reference system pre-set in the positioning system, which is used to uniformly describe the positions of all points in the positioning space.

[0127] The single coordinate positioning method includes the following steps:

[0128] S1: The distance information between the positioning base point and the positioned point is obtained through UWB technology; the positioned point is a point in the positioning space.

[0129] Ultra-wideband (UWB) technology is a wireless communication technology based on extremely short pulses to transmit data, and it has high-precision ranging capabilities. In this embodiment, UWB technology is used to transmit and receive signals between the positioning base point and the located point arranged in the positioning space. Based on the time of flight (ToF) principle of UWB signals or other related ranging mechanisms, the distance information between the positioning base point and the located point can be accurately measured. For example, by sending a UWB pulse signal and recording the time it takes from the base station to the located point and then back to the base station, the distance between the two can be calculated based on known conditions such as the speed of light. This distance information will serve as an important basic parameter for subsequent positioning calculations.

[0130] S2: Obtaining the posture information of the located point in a preset coordinate system; the posture information of the located point is calculated based on the key parameters of the gyroscope of the located point.

[0131] The attitude information of the located point is calculated based on the key parameters of the nine-axis gyroscope equipped at the point. The nine-axis gyroscope can measure information such as three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction.

[0132] S3: Use the distance information to determine the spherical surface where the located point is located, and then combine the posture information to determine the relative coordinates of the located point on the spherical surface relative to the positioning base point.

[0133] It is known that the distance information between the positioning base point and the positioned point obtained by UWB technology (set as R), according to the principle of spatial geometry, the positioned point must be located on a spherical surface with the positioning base point as the center and a radius as . This limits the possible position range of the positioned point from the distance dimension and determines a rough geometric space in which it is located - the spherical surface. After obtaining the attitude information of the positioned point, the attitude information is combined with the above-mentioned spherical position information through a series of complex mathematical calculations. For example, it may involve mapping the displacement of the terminal in its own coordinate system to the displacement in the geodetic coordinate system through operations such as Euler rotation (converted in combination with the attitude angle), and then calculating the relative coordinates of the positioned point on the spherical surface relative to the positioning base point based on the spatial geometric relationship, using the determined spherical surface and the converted displacement information.

[0134] S4: Superimpose the relative coordinates and the reference coordinates to obtain the absolute coordinates of the located point in the preset coordinate system.

[0135] After the relative coordinates of the located point with respect to the positioning base point have been calculated, these relative coordinates are superimposed with the reference coordinates of the positioning base point itself in the preset coordinate system.

[0136] This embodiment provides a single-coordinate positioning method based on UWB and a nine-axis gyroscope, which innovatively integrates ultra-wideband (UWB) technology with the measurement data of the nine-axis gyroscope for positioning. UWB technology provides high-precision distance measurement capabilities, which can accurately determine the distance relationship between the located point and the positioning base point, thereby defining the spherical position of the located point. The nine-axis gyroscope can provide rich attitude information, including the rotation attitude and tilt angle of the object. Through the precise calculation and processing of these attitude information, the specific position of the located point on the spherical surface relative to the positioning base point can be further determined. This method of combining the advantages of two different types of sensor technologies reduces the more than four base stations required in traditional three-dimensional positioning to one base station, thereby effectively solving the high cost and high construction difficulty caused by the need for multiple base stations for traditional UWB positioning, and realizing accurate, efficient and economical positioning functions, meeting the needs of target positioning in fields such as security monitoring.

[0137] As mentioned above, when calculating the relative coordinates of the located point on the spherical surface relative to the positioning base point, its attitude information is required. The attitude information includes attitude angle, acceleration, angular velocity, etc. In one embodiment, the key parameters of the gyroscope include: three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction; based on the three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction, the attitude information of the located point is calculated using the quaternion method.

[0138] In a further embodiment, a method for calculating the attitude angle using the quaternion method is provided, and the process is as follows: Figure 2 As shown, the following steps are included:

[0139] (1) Read the information in the gyroscope, three-axis acceleration (Xa, Ya, Za); three-axis angular velocity (Xg, Yg, Zg); three-axis magnetic flux direction (Xm, Ym, Zm).

[0140] (2) Calculate the terminal attitude using the quaternion method.

[0141] 1) Set the initial values ​​of iteration parameters q1=1, q2=q3=q4=0;

[0142] 2) Unitize input values;

[0143] Unitize acceleration:

[0144] Mold_A=sqrt(Xa*Xa+Ya*Ya+Za*Za);

[0145] Xsa = Xa / Mold_A;

[0146] Ysa=Ya / Mold_A;

[0147] Zsa = Za / Mold_A;

[0148] Magnetic flux unitization:

[0149] Mold_M=sqrt(Xm*Xm+Ym*Ym+Zm*Zm);

[0150] Xsm = Xm / Mold_m;

[0151] Ysm = Ym / Mold_m;

[0152] Zsm=Zm / Mold_m;

[0153] Convert degrees to radians:

[0154] Xsg = Xg*Pi / 180;

[0155] Ysg = Yg*Pi / 180;

[0156] Zsg=Zg*Pi / 180;

[0157] 3) Convert the normalized three-axis magnetic flux direction from the local coordinate system to the earth coordinate system to obtain:

[0158] Xe=Xsm*q1*q1–2*Ysm*q1*q4+2*Zsm*q1*q3+Xsm*q2*q2+

[0159] 2*Ysm*q2*q3+2*Zsm*q2*q4–Xsm*q3*q3-Xsm*q4*q4;

[0160] Ye=2*Xsm*q1*q4+Ysm*q1*q1-2*Zsm*q1*q2+2*Xsm*q2*q3-Ysm*q2*q2

[0161] +Ysm*q3*q3+2*Zsm*q3*q4-Ysm*q4*q4;

[0162] Its mathematical expression is as follows:

[0163]

[0164] Where, (Xe, Ye) is the magnetic flux angle in the geodetic coordinate system; (Xsm, Ysm, Zsm) is the three-axis magnetic flux direction after normalization;

[0165] 4) Based on the magnetic flux angle in the geodetic coordinate system, point the X-axis of the navigation coordinate system to due north, and we get:

[0166] Xd = sqrt(Xe*Xe+Ye*Ye);

[0167] Zd=-2*Xsm*q1*q3+2*Ysm*q1*q2+Zsm*q1*q1+2*Xsm*q2*q4–

[0168] Zsm*q2*q2+2*Ysm*q3*q4–Zsm*q3*q3+Zsm*q4*q4;

[0169] Its mathematical expression is as follows:

[0170]

[0171]

[0172] Where, (Xd, Zd) is the first intermediate variable;

[0173] 5) Calculate the correction error based on the first intermediate variable to obtain:

[0174] S1=-2*q3*(2*q2*q4–2*q1*q3-Xa)+2*q2*(2*q1*q2+2*q3*q4–

[0175] Ya)–Zd*q3*(Xd*(0.5-q3*q3-q4*q4)+Zd*(q2*q4-q1*q3)–Xsm)+

[0176] (-Xd*q4+Zd*q2)*(Xd*(q2*q3–q1*q4)+Zd*(q1*q2+q3*q4)–Ysm)+

[0177] Xd*q3*(Xd*(q1*q3+q2*q4)+Zd*(0.5–q2*q2–q3*q3)–Zsm);

[0178] S2=2*q4*(2*q2*q4-2*q1*q3-Xa)+2*q1*(2*q1*q2+2*q3*q4–Ya)-4*q2* (1-2*q2*q2-2*q3*q3-Za)+Zd*q4*(Xd*(0.5-q3*q3-q4*q4)+Zd*(q2*q4

[0179] –q1*q3)–Xsm)+(Xd*q3+Zd*q1)*(Xd*(q2*q3–q1*q4)+

[0180] Zd*(q1*q2 + q3*q4) – Ysm) + (Xd*q4 – 2Zd*q2)*(Xd*(q1*q3 + q2*q4) +

[0181] Zd*(0.5 – q2*q2 – q3*q3) – Zsm);

[0182] S3 = -2*q1*(2*q2*q4 – 2*q1*q3 – Xa) + 2*q4*(2*q1*q2 + 2*q3*q4 –

[0183] Ya) – 4*q3*(1 - 2*q2*q2 – 2*q3*q3 – Za) + (-2Xd*q3 –

[0184] Zd*q1)*(Xd*(0.5 - q3*q3 – q4*q4) + Zd*(q2*q4 – q1*q3) – Xsm) +

[0185] (Xd*q2 + Zd*q4)*(Xd*(q2*q3 – q1*q4) + Zd*(q1*q2 + q3*q4) – Ysm) + (Xd*q1 – 2Zd*q3)*(Xd*(q1*q3 + q2*q4) + Zd*(0.5 – q2*q2 – q3*q3) – Zsm;

[0186] S4 = 2*q2*(2*q2*q4 – 2*q1*q3 – Xa) + 2*q3*(2*q1*q2 + 2*q3*q4 – Ya) + (2*Xd*q4 + Zd*q2)*(Xd*(0.5 – q3*q3 – q4*q4) + Zd*(q2*q4 –

[0187] q1*q3) – Xsm) + (Xd*q1 + Zd*q3)*(Xd*(q2*q3 – q1*q4) + Zd*(q1*q2 +

[0188] q3*q4) – Ysm) + Xd*q2*(Xd*(q1*q3 + q2*q4) + Zd*(0.5 – q2*q2 – q3*q3) – Zsm);

[0189] Its mathematical expression is as follows:

[0190]

[0191]

[0192] Wherein, (S1, S2, S3, S4) are correction errors;

[0193] 6) Unit normalization of the error vector:

[0194] Mold_e=sqrt(S1*S1+S2*S2+S3*S3+S4*S4);

[0195] S1 = S1 / Mold_e;

[0196] S2 = S2 / Mold_e;

[0197] S3 = S3 / Mold_e;

[0198] S4 = S4 / Mold_e;

[0199] 7) Calculate the second intermediate variable based on the normalized corrected error, and obtain: Qt1 = 0.5*(-q2*Xg–q3*Yg–q4*Zg)–beta*S1;

[0200] Qt2=0.5*(q1*Xg+q3*Zg–q4*Yg)–beta*S2;

[0201] Qt3=0.5*(q1*Yg+q2*Zg+q4*Xg)–beta*S3;

[0202] Qt4=0.5*(q1*Zg+q2*Yg+q3*Xg)–beta*S4;

[0203] Its mathematical expression is as follows:

[0204] Q t1 =0.5(-q2×Xg-q3×Yg-q4×Zg)-βS1

[0205] Q t2 =0.5(q1×Xg+q3×Zg-q4×Yg)-βS2

[0206] Q t3 =0.5(q1×Yg+q2×Zg+q4×Xg)-βS3

[0207] Q t4 =0.5(q1×Zg+q2×Yg+q3×Xg)-βS4

[0208] In the formula, (Q t1 ,Q t2 ,Q t3 ,Q t4 ) is the second intermediate variable, β is the compensation factor;

[0209] 8) Iterate over four elements:

[0210] q1+=Qt1*dt;

[0211] q2+=Qt2*dt;

[0212] q3+=Qt3*dt;

[0213] q4+=Qt4*dt;

[0214] Its mathematical expression is as follows:

[0215] q1=Q t1 ×dt

[0216] q2=Q t2 ×dt

[0217] q3=Q t3 ×dt

[0218] q4=Q t4 ×dt

[0219] 9) Normalize the iterated quaternion:

[0220] Mold=sqrt(q1*q1+q2*q2+q3*q3+q4*q4)

[0221] q1=q1 / Mold;

[0222] q2=q2 / Mold;

[0223] q3=q3 / Mold;

[0224] q4=q4 / Mold;

[0225] 10) Calculate the attitude angle based on the normalized quaternion as follows: X = -arcsin(2*(q2*q4-q1*q3))*180 / Pi;

[0226] Y=arctan(2*(q1*q2+q3*q4) / (q1*q1-q2*q2-q3*q3+q4*q4))*180 / Pi;Z=arctan(2*(q2*q3+q1*q4) / (q1*q1+q2*q2-q3*q3-q4*q4))*180 / Pi;The mathematical expressions are as follows:

[0227] X=-arctan(2×(q2×q4-q1×q3))×180 / π

[0228]

[0229] Where, (X, Y, Z) is the attitude angle;

[0230] 11) The calculated attitude angle, three-axis acceleration and three-axis angular velocity are used as attitude information.

[0231] In one embodiment, the relative coordinates between the located point and the positioning base point need to be determined based on the displacement of the located point in a preset coordinate system. The positioning base point receives the posture information sent by the located point, including posture angle (X, Y, Z), acceleration (Xa, Ya, Za), angular velocity (Xg, Yg, Zg) and distance R.

[0232] The calculation process of the displacement is as follows:

[0233] (1) Calculate the velocity by iteratively adding the acceleration:

[0234] Xv=Xv+Xa;

[0235] Yv=Yv+Ya;

[0236] Zv=Zv+Za;

[0237] (2) Calculate the displacement iteratively by accumulating acceleration values:

[0238] Xs=Xs+Xv;

[0239] Ys=Ys+Yv;

[0240] Zs=Zs+Zv;

[0241] (3) Using Euler rotation, the displacement value is mapped to the geodetic coordinate system in combination with the attitude angle in the attitude information, and the result is:

[0242] Xse=Xs*(cosZ*cosX+sinZ*sinY*sinX)+Ys*(cosY*cosX)+Zs(sinZ*cosX-cos Z*sinY*sinX);

[0243] Yse=Xs*(-cosZ*sinX+sinZ*sinY*sinX)+Ys*(cosY*cosX)+Zx*(-sinZ*sinX-cosZ*sinY*cosX);

[0244] Zse=Xs*(-sinZ*cosY)+Ys*(sinY)+Zs*(cosZ*cosY);

[0245] Its mathematical expression is as follows:

[0246] Xse=Xs×(cosZ×cosX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+

[0247] Zs×(sinZ×cosX-cosZ×sinY×sinX)

[0248] Yse=Xs×(-cosZ×sinX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+

[0249] Zs×(-sinZ×sinX-cosZ×sinY×cosX)

[0250] Zse=Xs×(-sinZ×cosY)+Ys×(sinY)+Zs×(cosZ×cosY)

[0251] Where (Xse, Yse, Zse) is the displacement value mapped to the earth coordinate system, (Xs, Ys, Zs) is the displacement value obtained by iterative calculation, and (X, Y, Z) is the attitude angle.

[0252] In a further embodiment, if the preset coordinate system is a geodetic coordinate system, the relative coordinates are determined according to the following formula:

[0253] Xt=sqrt(R*R-(Yt+Yse)*(Yt+Yse)-(Zt+Zse)*(Zt+Zse)-Xse);

[0254] Yt=sqrt(R*R-(Xt+Xse)*(Xt+Xse)-(Zt+Zse)*(Zt+Zse)-Yse);

[0255] Zt=sqrt(R*R-(Xt+Xse)*(Xt+Xse)-(Yt+Yse)*(Yt+Yse)-Zse);

[0256] Its mathematical expression is as follows:

[0257]

[0258] In the formula, (Xt, Yt, Zt) on the left side of the equal sign is the relative coordinate after iterative update at time t; R is the distance between the positioning base point and the positioned point, that is, the radius of the sphere; (Xt, Yt, Zt) on the right side of the equal sign is the relative coordinate before iterative update at time t, (Xse, Yse, Zse) is the displacement of the positioned point in the preset coordinate system, which is determined according to the posture information of the positioned point in the preset coordinate system; the initial value of the relative coordinate is 0.

[0259] In a further embodiment, the absolute coordinates are determined according to the following process:

[0260] (1) Set the initial values ​​of iteration Xt=0; Yt=0; Zt=0;

[0261] (2) Calculate the displacement of the located point in the preset coordinate system;

[0262] (3) Calculate relative coordinates;

[0263] (4) Superimpose the base station coordinates to obtain the terminal absolute coordinates:

[0264] Xreal=Xbase+Xt;

[0265] Yreal=Ybase+Yt;

[0266] Zreal=Zbase+Zt;

[0267] Figure 3 This is a calculation process of absolute coordinates proposed in combination with the above embodiments.

[0268] Based on the same inventive concept, the embodiment of the present application also provides a single-coordinate positioning device based on UWB and a nine-axis gyroscope for implementing the single-coordinate positioning method based on UWB and a nine-axis gyroscope involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in the embodiment of the single-coordinate positioning device based on UWB and a nine-axis gyroscope provided below can be referred to the limitations of the single-coordinate positioning method based on UWB and a nine-axis gyroscope above, and will not be repeated here.

[0269] like Figure 4 As shown, this embodiment provides a single-coordinate positioning device based on UWB and a nine-axis gyroscope, and a positioning base point is arranged in the positioning space, and the positioning base point has a reference coordinate in a preset coordinate system;

[0270] The single coordinate positioning device includes:

[0271] Positioning base stations and positioning terminals;

[0272] The positioning base station is deployed at the positioning base point and is equipped with a UWB module;

[0273] The positioning terminal is deployed at the location to be positioned and is equipped with a UWB module and a gyroscope;

[0274] The positioning base station is used to obtain the distance information between the positioning base point and the positioned point through UWB technology; the positioned point is a point in the positioning space;

[0275] The positioning terminal is used to obtain the posture information of the positioned point in the preset coordinate system; the posture information of the positioned point is calculated based on the key parameters of the gyroscope of the positioned point;

[0276] The positioning base station is also used to use the distance information to determine the spherical surface on which the located point is located, and then combine the posture information to determine the relative coordinates of the located point on the spherical surface relative to the positioning base point; it is also used to superimpose the relative coordinates with the reference coordinates to obtain the absolute coordinates of the located point in the preset coordinate system.

[0277] The device of this embodiment can be designed to include a positioning base station and a plurality of positioning terminals, and the positioning base points are arranged at fixed positions with known coordinates as positioning references. The positioning terminals are carried by the user or vehicle to be positioned.

[0278] The positioning base point contains a UWB module, a group of processors and a communication module. The UWB module realizes the ranging of the positioning terminal and the communication with the positioning terminal; the processor is used to calculate the location information of the positioning terminal; the communication module can be an Ethernet module or a 4G module, which is used to send the location information of the positioning terminal to the platform.

[0279] The positioning terminal includes a UWB module, a nine-axis gyroscope, a battery, and a controller. The UWB module is used to achieve ranging and communication with the base station. The nine-axis gyroscope is used to measure the acceleration, angular acceleration and magnetic flux of the positioning terminal. The battery is used to power the system. The controller is used to realize the attitude solution of the positioning terminal.

[0280] When the positioning base point is deployed, its coordinates are known. After obtaining the distance and posture information of the positioning terminal through the UWB module, its relative coordinates relative to the positioning base point are calculated, and then superimposed with the coordinates of the base station itself to obtain the absolute coordinates of the positioning terminal.

[0281] Furthermore, if the preset coordinate system is the geodetic coordinate system, the relative coordinates are determined according to the following formula:

[0282]

[0283] In the formula, (Xt, Yt, Zt) on the left side of the equal sign is the relative coordinate after iterative update at time t; R is the distance between the positioning base point and the positioned point, that is, the radius of the sphere; (Xt, Yt, Zt) on the right side of the equal sign is the relative coordinate before iterative update at time t, (Xse, Yse, Zse) is the displacement of the positioned point in the preset coordinate system, which is determined according to the posture information of the positioned point in the preset coordinate system; the initial value of the relative coordinate is 0.

[0284] Furthermore, the posture information of the located point in the preset coordinate system includes the posture angle and acceleration;

[0285] The displacement of the located point in the preset coordinate system is determined according to the following process:

[0286] Based on the acceleration in the attitude information, the acceleration value is calculated by iteratively accumulating the acceleration;

[0287] Iteratively calculate the displacement value by accumulating the acceleration value;

[0288] Using Euler rotation, combined with the attitude angle in the attitude information, the displacement value is mapped to the geodetic coordinate system, and the result is:

[0289] Xse=Xs×(cosZ×cosX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+

[0290] Zs×(sinZ×cosX-cosZ×sinY×sinX)

[0291] Yse=Xs×(-cosZ×sinX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+

[0292] Zs×(-sinZ×sinX-cosZ×sinY×cosX)

[0293] Zse=Xs×(-sinZ×cosY)+Ys×(sinY)+

[0294] Zs×(cosZ×cosY)

[0295] Where (Xse, Yse, Zse) is the displacement value mapped to the earth coordinate system, (Xs, Ys, Zs) is the displacement value obtained by iterative calculation, and (X, Y, Z) is the attitude angle.

[0296] Furthermore, the key parameters of the gyroscope include:

[0297] Three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction;

[0298] Based on three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction, the posture information of the positioned point is calculated using the quaternion method.

[0299] Furthermore, the quaternion method is used to calculate the posture information of the positioned point, including:

[0300] Initialize quaternion q1=1, q2=q3=q4=0;

[0301] The three-axis acceleration and the three-axis magnetic flux direction are normalized and the three-axis angular velocity is converted into radians to obtain the processed acceleration (Xsa, Ysa, Zsa), magnetic flux angle (Xsm, Ysm, Zsm) and radians (Xsg, Ysg, Zsg);

[0302] The normalized three-axis magnetic flux direction is converted from the local coordinate system to the earth coordinate system to obtain:

[0303]

[0304] Where, (Xe, Ye) is the magnetic flux angle in the geodetic coordinate system; (Xsm, Ysm, Zsm) is the three-axis magnetic flux direction after normalization;

[0305] Based on the magnetic flux angle in the geodetic coordinate system, the X-axis of the navigation coordinate system is pointed to due north, and we get:

[0306]

[0307]

[0308] Where, (Xd, Zd) is the first intermediate variable;

[0309] The corrected error is calculated based on the first intermediate variable, and the result is:

[0310]

[0311]

[0312] Where, (S1, S2, S3, S4) is the correction error;

[0313] Normalize the correction error;

[0314] The second intermediate variable is calculated based on the normalized corrected error, and the result is:

[0315] Q t1 =0.5(-q2×Xg-q3×Yg-q4×Zg)-βS1

[0316] Q t2 =0.5(q1×Xg+q3×Zg-q4×Yg)-βS2

[0317] Q t3 =0.5(q1×Yg+q2×Zg+q4×Xg)-βS3

[0318] Q t4 =0.5(q1×Zg+q2×Yg+q3×Xg)-βS4

[0319] In the formula, (Q t1 ,Q t2 ,Q t3 ,Q t4 ) is the second intermediate variable, β is the compensation factor;

[0320] The quaternion is iterated using the second intermediate variable as follows:

[0321] q1=Q t1 ×dt

[0322] q2=Q t2 ×dt

[0323] q3=Q t3 ×dt

[0324] q4=Q t4 ×dt

[0325] Normalize the iterated quaternion;

[0326] The attitude angle is calculated based on the normalized quaternion as follows:

[0327] X=-arctan(2×(q2×q4-q1×q3))×180 / π

[0328]

[0329] Where, (X, Y, Z) is the attitude angle;

[0330] The calculated attitude angle, three-axis acceleration and three-axis angular velocity are used as attitude information.

[0331] Those skilled in the art can clearly understand that, for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the system can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

[0332] Reference Figure 5 An embodiment of the present invention further provides a computer device, comprising: a memory and a processor and a computer program stored in the memory. When the computer program is executed on the processor, a single-coordinate positioning method based on UWB and a nine-axis gyroscope as described in any one of the above methods is implemented.

[0333] The computer device may be a desktop computer, a notebook, a PDA, a cloud server or other computing device. The computer device may include, but is not limited to, a processor and a memory. Those skilled in the art will understand that Figure 5 It is only an example of a computer device and does not constitute a limitation of the computer device. It may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, it may also include input and output devices, network access devices, etc.

[0334] The processor may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.

[0335] In some embodiments, the memory may be an internal storage unit of the computer device, such as a hard disk or memory of the computer device. In other embodiments, the memory may also be an external storage device of the computer device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. equipped on the computer device. Further, the memory may also include both an internal storage unit of the computer device and an external storage device. The memory is used to store an operating system, an application program, a boot loader (BootLoader), data, and other programs, such as the program code of the computer program, etc. The memory may also be used to temporarily store data that has been output or is to be output.

[0336] An embodiment of the present invention further provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the single-coordinate positioning method based on UWB and a nine-axis gyroscope as described in any one of the above methods is implemented.

[0337] In this embodiment, if the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present application implements all or part of the processes in the above-mentioned embodiment method, which can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, the steps of the above-mentioned various method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium can at least include: any entity or device that can carry the computer program code to the camera / terminal device, recording medium, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, RandomAccess Memory), electric carrier signal, telecommunication signal and software distribution medium. For example, a USB flash drive, a mobile hard disk, a magnetic disk or an optical disk. In some jurisdictions, according to legislation and patent practice, computer-readable media cannot be electric carrier signals and telecommunication signals.

[0338] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0339] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0340] In the embodiments disclosed in the present application, it should be understood that the disclosed devices / terminal equipment and methods can be implemented in other ways. For example, the device / terminal equipment embodiments described above are only schematic. For example, the division of the modules or units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0341] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A single coordinate positioning method based on UWB and nine-axis gyroscope, characterized in that: A positioning base point is arranged in the positioning space, and the positioning base point has a reference coordinate in a preset coordinate system; the single-coordinate positioning method comprises the following steps: Acquire the distance information between the positioning base point and the positioned point by using UWB technology; the positioned point is a point in the positioning space; Acquire the posture information of the located point in the preset coordinate system; the posture information of the located point is calculated based on the key parameters of the gyroscope of the located point; Determine the spherical surface where the located point is located by using the distance information, and then determine the relative coordinates of the located point on the spherical surface relative to the positioning base point in combination with the posture information; The relative coordinates are superimposed on the reference coordinates to obtain the absolute coordinates of the located point in the preset coordinate system.

2. The single-coordinate positioning method based on UWB and nine-axis gyroscope according to claim 1, characterized in that: If the preset coordinate system is a geodetic coordinate system, the relative coordinates are determined according to the following formula: In the formula, (Xt, Yt, Zt) on the left side of the equal sign is the relative coordinate after iterative update at time t; R is the distance between the positioning base point and the positioned point, that is, the radius of the sphere; (Xt, Yt, Zt) on the right side of the equal sign is the relative coordinate before iterative update at time t, (Xse, Yse, Zse) is the displacement of the positioned point in the preset coordinate system, which is determined according to the posture information of the positioned point in the preset coordinate system; the initial value of the relative coordinate is 0.

3. The single-coordinate positioning method based on UWB and nine-axis gyroscope according to claim 2, characterized in that: The posture information of the located point in the preset coordinate system, including posture angle and acceleration; The displacement of the located point in the preset coordinate system is determined according to the following process: Based on the acceleration in the posture information, iteratively calculating the acceleration value by accumulating the acceleration; Iteratively calculating the displacement value by accumulating the acceleration values; The displacement value is mapped to the earth coordinate system by using Euler rotation and combining the attitude angle in the attitude information to obtain: Xse=Xs×(cosZ×cosX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+ Zs×(sinZ×cosX-cosZ×sinY×sinX) Yse=Xs×(-cosZ×sinX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+ Zs×(-sinZ×sinX-cosZ×sinY×cosX) Zse=Xs×(-sinZ×cosY)+Ys×(sinY)+Zs×(cosZ×cosY) Wherein, (Xse, Yse, Zse) is the displacement value mapped to the earth coordinate system, (Xs, Ys, Zs) is the displacement value obtained by iterative calculation, and (X, Y, Z) is the attitude angle.

4. The single coordinate positioning method based on UWB and nine-axis gyroscope according to claim 1 or 3, characterized in that: The key parameters of the gyroscope include: Three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction; Based on the three-axis acceleration, the three-axis angular velocity and the three-axis magnetic flux direction, the posture information of the located point is calculated using the quaternion method.

5. The single coordinate positioning method based on UWB and nine-axis gyroscope according to claim 4, characterized in that: The posture information of the located point is calculated by using the quaternion method, including: Initialize quaternion q1=1, q2=q3=q4=0; The three-axis acceleration and the three-axis magnetic flux direction are normalized and the three-axis angular velocity is converted into radians to obtain processed acceleration (Xsa, Ysa, Zsa), magnetic flux angle (Xsm, Ysm, Zsm) and radians (Xsg, Ysg, Zsg); The normalized three-axis magnetic flux directions are converted from the local coordinate system to the earth coordinate system to obtain: Wherein, (Xe, Ye) is the magnetic flux angle in the geodetic coordinate system; (Xsm, Ysm, Zsm) is the three-axis magnetic flux direction after unitization; Based on the magnetic flux angle in the geodetic coordinate system, the X-axis of the navigation coordinate system is pointed to due north, and we get: Where, (Xd, Zd) is the first intermediate variable; The correction error is calculated based on the first intermediate variable to obtain: Wherein, (S1, S2, S3, S4) is the correction error; Unitizing the correction error; The second intermediate variable is calculated based on the normalized correction error to obtain: Q t1 =0.5(-q2×Xg-q3×Yg-q4×Zg)-βS1 Q t2 =0.5(q1×Xg+q3×Zg-q4×Yg)-βS2 Q t3 =0.5(q1×Yg+q2×Zg+q4×Xg)-βS3 Q t4 =0.5(q1×Zg+q2×Yg+q3×Xg)-βS4 In the formula, (Q t1 ,Q t2 ,Q t3 ,Q t4 ) is the second intermediate variable, and β is the compensation factor; The quaternion is iterated using the second intermediate variable as follows: q1=Q t1 ×dt q2=Q t2 ×dt q3=Q t3 ×day q4=Q t4 ×dt Normalizing the iterated quaternion; The attitude angle is calculated based on the normalized quaternion as follows: X=-arctan(2×(q2×q4-q1×q3))×180 / π Wherein, (X, Y, Z) is the attitude angle; The calculated attitude angle, the three-axis acceleration, and the three-axis angular velocity are used as the attitude information.

6. A single coordinate positioning device based on UWB and a nine-axis gyroscope, characterized in that: A positioning base point is arranged in the positioning space, and the positioning base point has a reference coordinate in a preset coordinate system; The single coordinate positioning device comprises: Positioning base stations and positioning terminals; The positioning base station is arranged at the positioning base point and is equipped with a UWB module; The positioning terminal is arranged at the positioned point and is equipped with a UWB module and a gyroscope; The positioning base station is used to obtain the distance information between the positioning base point and the positioned point through UWB technology; the positioned point is a point in the positioning space; The positioning terminal is used to obtain the posture information of the positioned point in the preset coordinate system; the posture information of the positioned point is calculated based on the key parameters of the gyroscope of the positioned point; The positioning base station is also used to use the distance information to determine the spherical surface on which the positioned point is located, and then combine the posture information to determine the relative coordinates of the positioned point on the spherical surface relative to the positioning base point; and is also used to superimpose the relative coordinates with the reference coordinates to obtain the absolute coordinates of the positioned point in the preset coordinate system.

7. The single-coordinate positioning device based on UWB and nine-axis gyroscope according to claim 1, characterized in that: If the preset coordinate system is a geodetic coordinate system, the relative coordinates are determined according to the following formula: In the formula, (Xt, Yt, Zt) on the left side of the equal sign is the relative coordinate after iterative update at time t; R is the distance between the positioning base point and the positioned point, that is, the radius of the sphere; (Xt, Yt, Zt) on the right side of the equal sign is the relative coordinate before iterative update at time t, (Xse, Yse, Zse) is the displacement of the positioned point in the preset coordinate system, which is determined according to the posture information of the positioned point in the preset coordinate system; the initial value of the relative coordinate is 0.

8. The single-coordinate positioning device based on UWB and nine-axis gyroscope according to claim 7, characterized in that: The posture information of the located point in the preset coordinate system, including posture angle and acceleration; The displacement of the located point in the preset coordinate system is determined according to the following process: Based on the acceleration in the posture information, iteratively calculating the acceleration value by accumulating the acceleration; Iteratively calculating the displacement value by accumulating the acceleration values; The displacement value is mapped to the earth coordinate system by using Euler rotation and combining the attitude angle in the attitude information to obtain: Xse=Xs×(cosZ×cosX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+ Zs×(sinZ×cosX-cosZ×sinY×sinX) Yse=Xs×(-cosZ×sinX+sinZ×sinY×sinX)+Ys×(cosY×cosX)+ Zs×(-sinZ×sinX-cosZ×sinY×cosX) Zse=Xs×(-sinZ×cosY)+Ys×(sinY)+ Zs×(cosZ×cosY) Wherein, (Xse, Yse, Zse) is the displacement value mapped to the earth coordinate system, (Xs, Ys, Zs) is the displacement value obtained by iterative calculation, and (X, Y, Z) is the attitude angle.

9. The single-coordinate positioning device based on UWB and a nine-axis gyroscope according to claim 6 or 8, characterized in that: The key parameters of the gyroscope include: Three-axis acceleration, three-axis angular velocity and three-axis magnetic flux direction; Based on the three-axis acceleration, the three-axis angular velocity and the three-axis magnetic flux direction, the posture information of the located point is calculated using the quaternion method.

10. The single-coordinate positioning device based on UWB and nine-axis gyroscope according to claim 9, characterized in that: The posture information of the located point is calculated by using the quaternion method, including: Initialize quaternion q1=1, q2=q3=q4=0; The three-axis acceleration and the three-axis magnetic flux direction are normalized and the three-axis angular velocity is converted into radians to obtain processed acceleration (Xsa, Ysa, Zsa), magnetic flux angle (Xsm, Ysm, Zsm) and radians (Xsg, Ysg, Zsg); The normalized three-axis magnetic flux directions are converted from the local coordinate system to the earth coordinate system to obtain: Wherein, (Xe, Ye) is the magnetic flux angle in the geodetic coordinate system; (Xsm, Ysm, Zsm) is the three-axis magnetic flux direction after unitization; Based on the magnetic flux angle in the geodetic coordinate system, the X-axis of the navigation coordinate system is pointed to due north, and we get: Where, (Xd, Zd) is the first intermediate variable; The correction error is calculated based on the first intermediate variable to obtain: Wherein, (S1, S2, S3, S4) is the correction error; Unitizing the correction error; The second intermediate variable is calculated based on the normalized correction error to obtain: Q t1 =0.5(-q2×Xg-q3×Yg-q4×Zg)-βS1 Q t2 =0.5(q1×Xg+q3×Zg-q4×Yg)-βS2 Q t3 =0.5(q1×Yg+q2×Zg+q4×Xg)-βS3 Q t4 =0.5(q1×Zg+q2×Yg+q3×Xg)-βS4 In the formula, (Q t1 ,Q t2 ,Q t3 ,Q t4 ) is the second intermediate variable, β is the compensation factor; the quaternion is iterated using the second intermediate variable as follows: q1=Q t1 ×dt q2=Q t2 ×dt q3=Q t3 ×day q4=Q t4 ×dt Normalizing the iterated quaternion; The attitude angle is calculated based on the normalized quaternion as follows: X=-arctan(2×(q2×q4-q1×q3))×180 / π Wherein, (X, Y, Z) is the attitude angle; The calculated attitude angle, the three-axis acceleration, and the three-axis angular velocity are used as the attitude information.

Images