Course angle measuring method and electrical equipment

Abstract

The invention discloses a course angle measuring method and electrical equipment. The course angle measuring method comprises the following steps: selecting a vertical plane for fixing the UWB base station module; a UWB tag module is fixed on a rigid body with a course angle to be measured, and the UWB tag module is integrated with an inertial measurement unit; establishing a world coordinate system according to the position O1 of the UWB base station module, and establishing a label coordinate system according to the position O2 of the UWB label module; the UWB base station module carries out distance measurement and angle measurement on the UWB tag module to obtain a coordinate vector # imgabs0 # of the UWB tag module in a world coordinate system, the UWB tag module carries out distance measurement and angle measurement on the UWB base station module to obtain a coordinate reverse vector # imgabs1 # of the UWB base station module in a tag coordinate system, and an orthogonal vector of a gravity vector and a coordinate vector # imgabs2 # of the UWB base station module is calculated; and an orthogonal vector of the gravity vector and a coordinate vector # imgabs3 # of the UWB tag module. Obtaining a rotation matrix from the world coordinate system to the label coordinate system based on the two orthogonal vectors; obtaining a course angle based on the rotation matrix. The method is low in cost and accurate.

Description

Technical Field

[0001] The present invention relates to the technical field of heading angle measurement, and in particular to a method for measuring a heading angle with low cost and high accuracy. Background Art

[0002] At present, the measurement of the heading angle of a rigid body usually relies on the integral calculation of the Inertial Measurement Unit (IMU). However, the use of the IMU integration method alone has the following technical problems: on the one hand, the IMU itself cannot directly obtain the reference direction relative to the north of the earth; on the other hand, since the error will continue to accumulate during the integral calculation process, the deviation of the heading angle increases over time, causing the measurement results to gradually become invalid and unable to meet the actual application requirements.

[0003] To make up for this deficiency, some solutions use 9-axis IMU to obtain the direction of the earth's magnetic field through geomagnetic sensors to calculate the reference north. However, this method also faces significant challenges: first, geomagnetic sensors are easily affected by other magnetic field interference in the environment, resulting in reduced measurement accuracy and making the calculation result of the heading angle unreliable; second, this method can only provide a fixed reference direction of the earth's north, and users cannot flexibly define the reference north according to actual needs, thus limiting the applicability of the system.

[0004] Therefore, how to provide a method with low cost and capable of accurately measuring heading angle is a technical problem to be solved. Summary of the invention

[0005] In order to solve the technical problems in the prior art of large heading angle measurement error, high cost and inability to customize the reference direction, the present invention proposes a heading angle measurement method and electrical equipment.

[0006] The heading angle measurement method proposed by the present invention comprises:

[0007] Select a fixed vertical surface to fix the UWB base station module;

[0008] The UWB tag module is fixed on a rigid body whose heading angle is to be measured, and the UWB tag module is integrated with an inertial measurement unit;

[0009] The world coordinate system is established with the UWB base station module position O1, and the tag coordinate system is established with the UWB tag module position O2;

[0010] The UWB base station module measures the distance and angle of the UWB tag module to obtain the coordinate vector of the UWB tag module in the world coordinate system.

[0011] The UWB tag module measures the distance and angle of the UWB base station module, and obtains the coordinate reverse vector of the UWB base station module in the tag coordinate system.

[0012] Calculate the gravity vector and the coordinate vector of the UWB base station module The orthogonal vectors, as well as the gravity vector and the coordinate vector of the UWB tag module Orthogonal vectors of ;

[0013] Get the rotation matrix from the world coordinate system to the label coordinate system based on two orthogonal vectors;

[0014] Get the heading angle of the rigid body based on the rotation matrix.

[0015] Furthermore, the coordinate vector use or Indicates that ρ is the modulus, θ is the horizontal angle, is the pitch angle.

[0016] Furthermore, the gravity vector and the coordinate vector of the UWB base station module Orthogonal vectors The gravity vector and the coordinate vector of the UWB tag module Orthogonal vectors They are the acceleration vectors of gravity on the three coordinate axes measured by the inertial measurement unit in the tag coordinate system.

[0017] Further, in the calculation Previously, the vector modulus values ​​were filtered and selected to meet the conditions The vector modulus is used to calculate V min =9,V max =11.

[0018] Furthermore, when the inertial measurement unit has three axes or more, the method further includes the step of correcting the heading angle:

[0019] Define the covariance matrix P0, measurement noise matrix Q, and process noise matrix R respectively;

[0020] At a certain frequency Read the data of the gyroscope of the inertial measurement unit to obtain the acceleration (ω x ,ω y ,ω z );

[0021] Calculate the rotation angle (ω) of the inertial measurement unit in the three directions of the tag coordinate systemx Δt,ω y Δt,ω z Δt);

[0022] According to the rotation angle of the inertial measurement unit in the three directions of the tag coordinate system, the predicted values ​​of the rotation angles yaw, pitch, and roll of the three directions of the tag coordinate system relative to the world coordinate system are calculated;

[0023] Calculate covariance and Kalman gain;

[0024] Correct yaw, pitch, and roll based on covariance and Kalman gain, and calculate the rotation matrix through yaw, pitch, and roll to obtain the corrected heading angle;

[0025] Fix the covariance matrix and return to the above step of reading the gyroscope to continuously correct the heading angle.

[0026] The electrical device provided by the present invention comprises a control module, which receives signals sent by a UWB base station module and a UWB tag module and calculates a heading angle of the electrical device.

[0027] Furthermore, the electrical equipment includes an electric toothbrush, a directional remote controller, and a follow-up vehicle.

[0028] The present invention fuses UWB with IMU to make the heading angle measurement more accurate and reliable, and the implementation cost is low. At the same time, the reference north direction can be set and obtained more flexibly through UWB technology. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The present invention is described in detail below with reference to the embodiments and accompanying drawings, wherein:

[0030] Figure 1 is a flow chart of an embodiment of the present invention.

[0031] Figure 2 2 is a schematic diagram of a heading angle according to an embodiment of the present invention.

[0032] Figure 3 It is a coordinate schematic diagram of an embodiment of the present invention. DETAILED DESCRIPTION

[0033] In order to make the technical problems, technical solutions and beneficial effects to be solved by the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0034] Thus, a feature indicated in this specification will be used to illustrate one of the features of an embodiment of the present invention, rather than implying that each embodiment of the present invention must have the described feature. In addition, it should be noted that this specification describes many features. Although some features can be combined together to illustrate possible system designs, these features can also be used in other combinations that are not explicitly described. Thus, unless otherwise stated, the described combinations are not intended to be limiting.

[0035] like Figure 1 , Figure 3 As shown, the method for measuring the heading angle proposed by the present invention includes the following steps.

[0036] Select a fixed vertical surface for fixing the UWB base station module; for example, select a fixed wall as the UWB base station module, or select a vertically hung dressing mirror as the UWB base station module.

[0037] The UWB tag module is fixed on a rigid body whose heading angle is to be measured, and the UWB tag module is integrated with an inertial measurement unit;

[0038] The world coordinate system is established with the UWB base station module position O1, that is, the position of the UWB base station module is the origin of the world coordinate system, and the tag coordinate system is established with the UWB tag module position O2, that is, the position of the UWB tag module is the origin of the tag coordinate system.

[0039] The UWB base station module measures the distance and angle of the UWB tag module to obtain the coordinate vector of the UWB tag module in the world coordinate system.

[0040] The UWB tag module measures the distance and angle of the UWB base station module and obtains the coordinate vector of the UWB base station module in the tag coordinate system.

[0041] Coordinate vector use or Indicates that ρ is the modulus, θ is the horizontal angle, is the pitch angle. Figure 2 As shown, assuming that the UWB tag module is located on the front side of the vertical plane, the direction perpendicular to the vertical plane and toward the back of the vertical plane is defined as the north direction, the horizontal angle is the angle rotated horizontally relative to the north direction, the pitch angle is the angle with the horizontal plane, and the heading angle is the angle of the rigid body where the UWB tag module is located relative to the north direction.

[0042] Calculate the gravity vector and the coordinate vector of the UWB base station module The orthogonal vectors, as well as the gravity vector and the coordinate vector of the UWB tag module Orthogonal vectors of ;

[0043] Get the rotation matrix from the world coordinate system to the label coordinate system based on two orthogonal vectors;

[0044] Get the heading angle of the rigid body based on the rotation matrix.

[0045] In the above technical solution, the gravity vector and the coordinate vector of the UWB base station module Orthogonal vectors The gravity vector and the coordinate vector of the UWB tag module Orthogonal vectors They are the acceleration vectors of gravity on the three coordinate axes measured by the inertial measurement unit in the tag coordinate system.

[0046] according to It can be calculated In the coordinate system O1, the coordinates are (x, y, z), and the heading angle is the angle of the complex number x+yi. The range of the heading angle is (-π, π], with the reference north angle as π. The coordinates are (-1,0,z).

[0047] In a further embodiment, in calculating Previously, the vector modulus values ​​were filtered and selected to meet the conditions The vector modulus is used to calculate V min =9,V max =11.

[0048] When the inertial measurement unit has three axes or more, at a certain frequency Read the data of the gyroscope of the inertial measurement unit to obtain the acceleration (ω x ,ω y ,ω z );

[0049] Calculate the rotation angle (ω) of the inertial measurement unit in the three directions of the tag coordinate system x Δt,ω y Δt,ω z Δt);

[0050] The rotation of the tag coordinate system relative to the world coordinate system can be represented by three angles: yaw, pitch, and roll. The covariance matrix P0, the measurement noise matrix Q, and the process noise matrix R are defined respectively. In one embodiment, these matrices can take the following values. These values ​​are only empirical values ​​of an embodiment and are not used to limit the protection scope of the present invention. Those skilled in the art can adjust the initial values ​​of these matrices as needed.

[0051]

[0052] Then the following calculation process is iterated with frequency f:

[0053] A) Calculate the predicted values ​​of yaw, pitch, and roll:

[0054]

[0055] B) Calculate the covariance and Kalman gain:

[0056]

[0057] C) Correction of yaw, pitch, roll, where Y acc It is the coordinate rotation angle calculated by the accelerometer and UWB. The rotation matrix is ​​calculated by yaw, pitch, and roll to get the heading angle:

[0058]

[0059] D) Modified covariance matrix, I is the identity matrix, the diagonal elements are 1, and the rest are 0:

[0060]

[0061] Then return to step A to perform the next iteration of yaw, pitch, and roll.

[0062] The present invention also protects electrical equipment, which includes a control module, which receives signals sent by a UWB base station module and a UWB tag module and calculates a heading angle of the electrical equipment.

[0063] The electrical equipment of the present invention includes but is not limited to an electric toothbrush, a directional remote controller, a follow-up car, etc.

[0064] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A heading angle measurement method, characterized in that: include: Select a fixed vertical surface to fix the UWB base station module; The UWB tag module is fixed on a rigid body whose heading angle is to be measured, and the UWB tag module is integrated with an inertial measurement unit; The world coordinate system is established with the UWB base station module position O1, and the tag coordinate system is established with the UWB tag module position O2; The UWB base station module measures the distance and angle of the UWB tag module to obtain the coordinate vector of the UWB tag module in the world coordinate system. The UWB tag module measures the distance and angle of the UWB base station module, and obtains the coordinate reverse vector of the UWB base station module in the tag coordinate system. Calculate the gravity vector and the coordinate vector of the UWB base station module The orthogonal vectors, as well as the gravity vector and the coordinate vector of the UWB tag module Orthogonal vectors of ; Get the rotation matrix from the world coordinate system to the label coordinate system based on two orthogonal vectors; Get the heading angle of the rigid body based on the rotation matrix.

2. The method for measuring a heading angle according to claim 1, wherein: The coordinate vector It is expressed by (ρcosφcosθ, ρcosφsinθ, ρsinφ) or (ρ, θ, φ), where ρ is the modulus, θ is the horizontal angle, and φ is the pitch angle.

3. The method for measuring heading angle according to claim 1, characterized in that: The gravity vector and the coordinate vector of the UWB base station module Orthogonal vectors The gravity vector and the coordinate vector of the UWB tag module Orthogonal vectors They are the acceleration vectors of gravity on the three coordinate axes measured by the inertial measurement unit in the tag coordinate system.

4. The method for measuring a heading angle according to claim 3, wherein: In calculation Previously, the vector modulus values ​​were filtered and selected to meet the conditions The vector modulus is used to calculate V min =9,V max =11.

5. The method for measuring heading angle according to claim 3, characterized in that: When the inertial measurement unit has three axes or more, the following steps are also included to correct the heading angle: Define the covariance matrix P0, measurement noise matrix Q, and process noise matrix R respectively; At a certain frequency Read the data of the gyroscope of the inertial measurement unit to obtain the acceleration (ω x ,ω y ,ω z ); Calculate the rotation angle (ω) of the inertial measurement unit in the three directions of the tag coordinate system x Δt,ω y Δt,ω z Δt); According to the rotation angle of the inertial measurement unit in the three directions of the tag coordinate system, the predicted values ​​of the rotation angles yaw, pitch, and roll of the three directions of the tag coordinate system relative to the world coordinate system are calculated; Calculate covariance and Kalman gain; Correct yaw, pitch, and roll based on covariance and Kalman gain, and calculate the rotation matrix through yaw, pitch, and roll to obtain the corrected heading angle; Fix the covariance matrix and return to the above step of reading the gyroscope to continuously correct the heading angle.

6. An electrical device, comprising a control module, characterized in that: The control module receives signals sent by the UWB base station module and the UWB tag module, and calculates the heading angle of the electrical device.

7. The electrical device according to claim 6, characterized in that The electrical equipment includes an electric toothbrush, a directional remote controller, and a follow-up vehicle.

Images