A relative yaw angle self-detection method for head-mounted device with anti-magnetic interference

Abstract

The present invention relates to a head-mounted device's anti-magnetic interference relative yaw angle self-detection method, which belongs to the field of sensor fusion technology, and includes the following steps: S1, an operator wears a head-mounted device; S2, filtering the data measured by the optical flow ranging module 1 and the optical flow ranging module 2 to obtain the yaw angle; S3, establishing a constraint equation to suppress the translation of the head-mounted device and the change of the optical flow data caused by complex motion; S4, after the yaw angle obtained in step S2 is suppressed by step S3, it is fused and output with the yaw angle measured by the IMU; S5, the data does not change when the optical flow is absolutely stationary, and the yaw angle data drift caused by the magnetic field change when the IMU is stationary is suppressed. The present invention improves the accuracy of the data.

Description

Technical Field

[0001] The invention relates to a magnetic interference-resistant relative yaw angle self-detection method for a head-mounted device, belonging to the technical field of sensor fusion. Background Art

[0002] In the remote operation technology of engineering machinery, the remote attitude control of the follow-up gimbal requires the operator to wear a VR headset to collect the head attitude. When the operator is in a certain narrow environment, the IMU module of the headset is easily affected by environmental factors. In particular, the magnetic field interference of external equipment will cause the yaw angle of the Z axis to drift seriously, which will lead to errors in the data transmitted to the site. Patent 201810003966.9 proposes a method for determining attitude, which determines the first attitude through the IMU and the attitude of the most recent device. Next, the first magnetic vector of the IMU magnetometer searches for the second attitude from the stored attitude mapping table, and determines the current attitude of the device based on the first attitude and the second attitude. This method has relatively loose requirements on the surrounding magnetic field, but only obtains the attitude angle through the IMU, which has high requirements on the reliability of the device. Patent 202010276652.3 proposes a method of measuring three-dimensional angular motion using optical flow. It uses three or more optical flow sensors pointing in different planes to obtain the corresponding angular increment ratios on the x-axis, y-axis, and z-axis, and finally obtains the corresponding three-dimensional angular increments. This method does not take into account the impact of changes in optical flow data caused by the translation and complex rotation of the device, and its applicable scenarios are relatively single. This patent proposes a head-mounted device's anti-magnetic interference relative yaw angle self-detection method to suppress the Z-axis yaw angle deviation caused by magnetic interference, thereby improving the accuracy of head tracking control. Summary of the invention

[0003] The purpose of the present invention is to provide a magnetic interference-resistant relative yaw angle self-detection method for a head-mounted device, so as to overcome the yaw angle data drift caused by magnetic field interference and improve the accuracy of the data.

[0004] In order to achieve the above object, the technical solution adopted by the present invention is:

[0005] A method for self-detecting relative yaw angle of a head-mounted device against magnetic interference comprises the following steps:

[0006] S1. An operator wears a head-mounted device, on which an optical flow ranging module 1, an optical flow ranging module 2 and an optical flow ranging module 3 are installed. Each of the optical flow ranging module 1, the optical flow ranging module 2 and the optical flow ranging module 3 includes an optical flow module and a laser ranging module.

[0007] S2, filtering the data measured by the optical flow ranging module 1 and the optical flow ranging module 2 to obtain the yaw angle;

[0008] S3, establishing constraint equations to suppress the changes in optical flow data caused by the translation and complex motion of the head-mounted device;

[0009] S4, the yaw angle obtained in step S2 is suppressed in step S3 and then fused with the yaw angle measured by the IMU for output;

[0010] S5. The data of optical flow does not change when it is absolutely still, and the yaw angle data drift caused by the magnetic field change when the IMU is still is suppressed.

[0011] A further improvement of the technical solution of the present invention is that the head-mounted device includes an external helmet, a main control chip embedded in the external helmet, a 9-axis IMU module on the top of the external helmet, a light intensity sensor and three optical flow ranging modules, the three optical flow ranging modules are respectively installed on the right, front and upper sides of the external helmet, are orthogonal to each other and have coplanar centers, and are optical flow ranging module 1, optical flow ranging module 2 and optical flow ranging module 3 respectively.

[0012] A further improvement of the technical solution of the present invention is that the specific steps of step S2 are:

[0013] According to ΔT s The pixel changes in the X-axis direction of the optical flow ranging module 1 and the optical flow ranging module 2 during the time are used to obtain the angular velocity changes δω1 and δω2 in the X-axis direction. The average of the two is taken, and the angular velocity is integrated to obtain the angle change δθ i , accumulate the angle changes and then perform coordinate conversion, convert the angle obtained by the optical flow module in its own coordinate system to the geographic coordinate system, and obtain Yaw OF :

[0014]

[0015] A further improvement of the technical solution of the present invention is that the specific operation steps of step S3 are:

[0016] Assume that the coordinate system of the optical flow module is aligned with the coordinate system of the device body, the field of view angle of the optical flow module is θ, and the total number of pixels corresponding to the field of view plane is N P , then the optical flow module goes through the sampling time ΔT s The obtained pixel number change (dx, dy) and angular velocity (w x ,w y ) is:

[0017]

[0018] get

[0019]

[0020] Among them, NP It will change with the change of the distance L from the optical flow module to the object. The relationship between the two is obtained from the triangle similarity principle:

[0021]

[0022] Among them, N x is the number of pixels in the fixed field of view of the camera, and f is the focal length of the lens, which is determined by the parameters of the specific sensor; substituting into the equation, we get:

[0023]

[0024] The relationship between the pixel change of the optical flow module and the linear velocity is related to the sampling time ΔT s The relationship is:

[0025] k v =ΔT s

[0026] Thus, the specific coefficient K of the optical flow module is obtained w K v , establish a fully symmetrical coordinate system for the head-mounted device, and decompose the speed of the device rotating at a certain angle into v x and v y , according to the right-hand rule, we get the overall constraint equation:

[0027]

[0028] Where dx1, dy1, dx2, dy2, dx3, dy3 are the sampling times ΔT of optical flow ranging module 1, optical flow ranging module 2 and optical flow ranging module 3 respectively. s The obtained changes in the number of pixels in the X-axis and Y-axis directions, the angular velocities of the X-axis and Y-axis can be obtained from the IMU data, and the error is small and the credibility is high. Therefore, the equations are solved to obtain the three-axis velocity and the Z-axis angular velocity, and it is determined whether the change in optical flow data is caused by translation, and then constrained.

[0029] A further improvement of the technical solution of the present invention is that the specific steps of step S4 are:

[0030] ① When the magnetometer in the IMU measures that the external magnetic field strength does not exceed the set threshold K mag When the intensity of the magnetic field changes slightly, the yaw angle output by the optical flow module is used; when the light intensity sensor measures that the light intensity exceeds the set lower limit K of And the light intensity changes little, so the yaw angle output by IMU is used;

[0031] ② When the yaw angles output by IMU and optical flow are both reliable, the data are fused to reduce the error. The Kalman filter principle is used to establish the equation:

[0032] Z k =Z1+K k (Z2-Z1)

[0033] Let Z1 be the angle data measured by IMU and Z2 be the angle data measured by optical flow module. The purpose is to find K k Make the standard of the estimated value the least, specifically to find the minimum value of the variance of the estimated value, for K k Take the derivative, then set it to 0, find the extreme value, get the Kalman gain, then calculate the estimate, and finally update the variance of the estimate, and iterate.

[0034] A further improvement of the technical solution of the present invention is that the specific steps of step S5 are:

[0035] Yaw=K OF *Z1+Z2

[0036] When the device moves, the optical flow data changes, K OF If it is 1, the output yaw angle is the result of the fusion output of the IMU and the optical flow module. At this time, the attitude angle will change continuously with the movement of the device;

[0037] When the device stops moving, the optical flow data does not change, K OF is 0, so the data output by the optical flow can be used to determine whether the device is moving, thereby eliminating the drift of the yaw angle output by the IMU due to magnetic field interference.

[0038] Due to the adoption of the above technical solution, the technical effects achieved by the present invention are as follows:

[0039] The present invention uses the average value of data collected by two optical flow modules to output the Z-axis yaw angle angular velocity, integrates the angular velocity and filters it to obtain the yaw angle, and performs time-sharing multiplexing or fusion output with the yaw angle output by the IMU according to different on-site environments. At the same time, a constraint equation is established to suppress the optical flow data error caused by the translation and complex movement of the equipment. Finally, it is proposed to use the principle that the data does not change when the optical flow is absolutely still, so as to suppress the yaw angle data drift caused by the magnetic field change when the IMU is still, thereby improving the accuracy of the data. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 It is a schematic diagram of the process of the present invention;

[0041] Figure 2 It is a schematic diagram of the hardware structure of the head-mounted device;

[0042] Figure 3 It is the coordinate system diagram when the head mounted device is rotated by an angle α;

[0043] Figure 4It is the geometric relationship diagram of optical flow field of view;

[0044] Figure 5 It is a fully symmetrical coordinate system diagram of the head-mounted device. DETAILED DESCRIPTION

[0045] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. Examples of the embodiments are shown in the accompanying drawings, wherein the same or similar numbers throughout represent the same or similar elements or elements with the same or similar functions.

[0046] A method for self-detecting relative yaw angle of a head-mounted device against magnetic interference, such as Figure 1 As shown, the following steps are included:

[0047] S1. Operators wear head-mounted equipment, such as Figure 2 As shown, the head-mounted device includes: an external helmet, a main control chip STM32F103C8T6 embedded in the external helmet, a 9-axis IMU module JY901S on the top of the external helmet, a light intensity sensor GY-302 and three optical flow ranging modules, and the optical flow ranging module is integrated with the optical flow module PMW3901 and the laser ranging module VL53L0X. Among them, the three optical flow ranging modules are respectively installed on the right, front and upper sides of the external helmet, orthogonal to each other and coplanar in the center, and are optical flow ranging module 1, optical flow ranging module 2 and optical flow ranging module 3.

[0048] S2, filtering the data measured by the optical flow ranging module 1 and the optical flow ranging module 2 to obtain the yaw angle, the specific steps are as follows:

[0049] Since the original data of the optical flow module is ΔT in the camera lens s (Data collection cycle, related to the specific optical flow module) The number of pixels flowing through the time, it is easy to get ΔT s The number of pixels flowing through the lens in the X-axis direction of the internal optical flow ranging module 1 and the optical flow ranging module 2 1X_Sum and OF 2X_Sum Relationship with the change in angular velocity:

[0050]

[0051] Among them, δω1 and δω2 are ΔT s The angular velocity change in the X-axis direction of the internal optical flow ranging module 1 and the optical flow ranging module 2.

[0052] Take the average of the two and integrate the angular velocity to get the angle change δθ i

[0053]

[0054] Accumulate the angle change to get the angle Z in the head-mounted device coordinate system OF :

[0055]

[0056] Since the Euler angle given by the IMU is the value of the rotation of the geographic coordinate system, and the angular velocity obtained by the optical flow module rotates around the coordinate axis of its own coordinate system, the angle needs to be converted to finally obtain the yaw angle output by the optical flow module in the geographic coordinate system:

[0057]

[0058] Since the optical flow module integrates the angular velocity when calculating the yaw angle, there is an integration error and it cannot be trusted for a long time. Therefore, it is necessary to use Kalman filtering to filter the optical flow output data. The specific method is given:

[0059] Only focusing on the Z-axis yaw angle, the system state matrix is:

[0060]

[0061] Where θ represents the yaw angle, δω b (b stands for bias) represents the angular velocity deviation, where the angular velocity deviation specifically refers to the drift of the optical flow module data.

[0062] ① Establish the yaw angle measurement model equation:

[0063] Yaw OF =Yaw OF +(δω-δω b )*ΔT s

[0064] That is, angle estimation value = previous angle estimation value + (current angular velocity – angular velocity drift) * dt

[0065] Each drift δω b Same, use θ instead of Yaw OF

[0066] θ=θ+(δω-δω b )*ΔT s

[0067] We get the equation:

[0068]

[0069] ②State prediction equation:

[0070]

[0071] By comparison, we can get:

[0072]

[0073] ③ Covariance matrix prediction equation:

[0074]

[0075] Among them, P k is the covariance matrix

[0076]

[0077] Initially, the state can be considered to be determined, and the error covariance matrix is:

[0078]

[0079] Next is the process noise covariance matrix Q k :

[0080]

[0081] Assuming that the angle and angular velocity deviations are independent of each other, the covariance on the diagonal is 0, and the remaining is the angle variance Q θ and the variance of the angular velocity deviation These two values ​​are extremely critical parameters and need to be determined manually.

[0082] ④Calculate the residual:

[0083]

[0084] Here we calculate the actual observation value Z k and predicted values The difference between k It is the angle measured by the optical flow module, and H is the measurement matrix

[0085] H = [1 0]

[0086] ⑤Calculate Kalman gain:

[0087]

[0088] Map the covariance matrix to the observation space for calculation. R is the measurement noise. Note that R is not a matrix. The measurement noise is equal to the measurement variance. This value is also the third key parameter, which is provided by the sensor manufacturer. Here, R is used. mea To express.

[0089] ⑥State update equation:

[0090]

[0091] Represents the best estimate at this moment, which is equal to the prediction of the previous moment at this moment Add the Kalman gain K k Multiply by the residual It can be looped and used in the next state prediction process to achieve iteration.

[0092] ⑦Covariance matrix update equation:

[0093] P k,k =(IK k H)P k,k-1

[0094] The Kalman gain is an N*1 matrix, which needs to be converted into a 2*2 matrix for calculation and multiplied by the measurement matrix.

[0095] S3. Constraint equations are established to suppress the changes in optical flow data caused by the translation and complex movements of the head-mounted device.

[0096] Establish Figure 3 The coordinate system of the head mounted device shown is rotated at an angle α. Considering only the Z-axis rotation, the following equation can be obtained:

[0097]

[0098] Wherein, they are the linear velocity and angular velocity in the xoy plane, α is the direction of velocity movement, k v and k w is the coefficient of the relationship between the pixel change of the optical flow module and the linear velocity and angular velocity. The following gives k v and k w Method for determining:

[0099] like Figure 4 The optical flow field of view geometric relationship diagram shown in the figure assumes that the coordinate system of the optical flow module is aligned with the coordinate system of the device body, the field of view angle of the optical flow module is θ (radians), and the total number of pixels corresponding to the field of view plane is N P , then the optical flow module goes through the sampling time ΔT s The obtained pixel number change (dx, dy) and angular velocity (w x ,w y ) is:

[0100]

[0101] get

[0102]

[0103] And N PIt will change with the change of the distance L from the optical flow module to the object, Figure 3 The triangle similarity principle shows that the relationship between the two is:

[0104]

[0105] Among them, N x is the number of pixels in the fixed field of view of the camera, and f is the focal length of the lens, both of which are determined by the parameters of the specific sensor. Substituting in:

[0106]

[0107] The relationship between the pixel change of the optical flow module and the linear velocity is related to the sampling time ΔT s The relationship is:

[0108] k v =ΔT s

[0109] Then establish Figure 5 The fully symmetrical coordinate system of the head-mounted device shown in the figure decomposes the velocity in the α direction into v x and v y , according to the right-hand rule, we get the overall constraint equation:

[0110]

[0111] Disassemble to get

[0112]

[0113] Right now:

[0114]

[0115] Where dx1, dy1, dx2, dy2, dx3, dy3 are the sampling times ΔT of optical flow ranging module 1, optical flow ranging module 2 and optical flow ranging module 3 respectively. s The obtained pixel number changes in the X-axis and Y-axis directions, IMU can provide accurate angles and angular velocities of the X and Y axes, so substitute w x w y , using three optical flow modules to establish four sets of linear equations to solve v x v y v z w z , for w caused by translation and complex motion z Constrain changes.

[0116] S4, the yaw angle obtained in step S2 is suppressed in step S3 and then fused with the yaw angle measured by the IMU for output. The specific method includes:

[0117] ① When the magnetometer in the IMU measures that the external magnetic field strength does not exceed the set threshold K mag (It is based on whether the IMU data has serious drift, generally set to 0.6 Gauss) and when the magnetic field intensity changes slightly, the yaw angle measured by the IMU is credible, otherwise it is unreliable, and the yaw angle output by the optical flow module is used.

[0118] ②Install a light intensity sensor on the periphery of the head-mounted VR device. When the light intensity exceeds the set lower limit K of (generally set to 60lux) and the light intensity changes little, then the yaw angle output by the optical flow is credible, otherwise it is unreliable and the yaw angle output by the IMU continues to be used.

[0119] ③ When the yaw angles output by IMU and optical flow are both reliable, the data is fused to reduce the error. The Kalman filter principle is used to establish the equation:

[0120] Z k =Z1+K k (Z2-Z1)

[0121] Let Z1 be the angle data measured by IMU, and Z2 be the angle data measured by optical flow module. The purpose is to find K k Minimize the standard of the estimated value, that is, minimize the variance of the estimated value, that is, the variance of the estimated value:

[0122]

[0123] To find the minimum variance of the estimated value, we need to k Taking the derivative and then setting it to 0, we can find the extreme value and obtain the Kalman gain:

[0124]

[0125] First evaluate K k The Kalman gain is then calculated, and the estimated value is finally updated — the variance of the next estimated value, and the cycle iterates.

[0126] S5. The data of optical flow does not change when it is absolutely still. The yaw angle data drift caused by the magnetic field change when the IMU is still can be suppressed. Figure 1 .

[0127] Yaw=K OF *Z1+Z2

[0128] When the device moves, the optical flow data changes, K OF If it is 1, the output yaw angle is the result of the fusion output of the IMU and the optical flow module. At this time, the attitude angle will change continuously with the movement of the device.

[0129] When the device stops moving, the optical flow data does not change, K OF is 0, so the data output by the optical flow can be used to determine whether the device is moving, thereby eliminating the drift of the yaw angle output by the IMU due to magnetic field interference.

[0130] The present invention uses the average value of data collected by two optical flow modules to output the Z-axis yaw angle angular velocity, integrates the angular velocity and filters it to obtain the yaw angle, and performs time-sharing multiplexing or fusion output with the yaw angle output by the IMU according to different on-site environments. At the same time, a constraint equation is established to suppress the optical flow data error caused by the translation and complex movement of the equipment. Finally, it is proposed to use the principle that the data does not change when the optical flow is absolutely still, so as to suppress the yaw angle data drift caused by the magnetic field change when the IMU is still, thereby improving the accuracy of the data.

[0131] What is disclosed above is only a preferred embodiment of the present invention, and certainly cannot be used to limit the scope of rights of the present invention. Ordinary technicians in this field can understand that all or part of the processes of the above-mentioned embodiments and equivalent changes made according to the claims of the present invention are still within the scope of the invention.

Claims

1. A method for self-detecting relative yaw angle of a head-mounted device against magnetic interference, characterized in that: The following steps are involved: S1. An operator wears a head-mounted device, on which an optical flow ranging module 1, an optical flow ranging module 2 and an optical flow ranging module 3 are installed. Each of the optical flow ranging module 1, the optical flow ranging module 2 and the optical flow ranging module 3 includes an optical flow module and a laser ranging module. S2, filtering the data measured by the optical flow ranging module 1 and the optical flow ranging module 2 to obtain a yaw angle; S3, establishing constraint equations to suppress the changes in optical flow data caused by the translation and complex motion of the head-mounted device; S4, the yaw angle obtained in step S2 is suppressed in step S3 and then fused with the yaw angle measured by the IMU for output; S5. The data of optical flow does not change when it is absolutely still, and the yaw angle data drift caused by the magnetic field change when the IMU is still is suppressed.

2. The method for self-detecting relative yaw angle of a head-mounted device against magnetic interference according to claim 1, characterized in that: The head-mounted device includes an external helmet, a main control chip embedded in the external helmet, a 9-axis IMU module on the top of the external helmet, a light intensity sensor and three optical flow ranging modules. The three optical flow ranging modules are respectively installed on the right, front and upper sides of the external helmet, are orthogonal to each other and have coplanar centers, and are optical flow ranging module 1, optical flow ranging module 2 and optical flow ranging module 3.

3. The method for self-detecting relative yaw angle of a head-mounted device against magnetic interference according to claim 1, characterized in that: The specific steps of step S2 are: According to ΔT s The pixel changes in the X-axis direction of the optical flow ranging module 1 and the optical flow ranging module 2 during the time are used to obtain the angular velocity changes δω1 and δω2 in the X-axis direction. The average of the two is taken, and the angular velocity is integrated to obtain the angle change δθ i , accumulate the angle changes and then perform coordinate conversion, convert the angle obtained by the optical flow module in its own coordinate system to the geographic coordinate system, and obtain Yaw OF :

4. The method for self-detecting relative yaw angle of a head-mounted device against magnetic interference according to claim 1, characterized in that: The specific operation steps of step S3 are: Assume that the coordinate system of the optical flow module is aligned with the coordinate system of the device body, the field of view angle of the optical flow module is θ, and the total number of pixels corresponding to the field of view plane is N P , then the optical flow module goes through the sampling time ΔT s The obtained pixel number change (dx, dy) and angular velocity (w x ,w y ) is: get Among them, N P It will change with the change of the distance L from the optical flow module to the object. The relationship between the two is obtained from the triangle similarity principle: Among them, N x is the number of pixels in the fixed field of view of the camera, and f is the focal length of the lens, which is determined by the parameters of the specific sensor; substituting into: The relationship between the pixel change of the optical flow module and the linear velocity is related to the sampling time ΔT s The relationship is: k v =ΔT s Thus, the specific coefficient K of the optical flow module is obtained w K v , establish a fully symmetrical coordinate system for the head-mounted device, and decompose the speed of the device rotating at a certain angle into v x and v y , according to the right-hand rule, we get the overall constraint equation: Where dx1, dy1, dx2, dy2, dx3, dy3 are the sampling times ΔT of optical flow ranging module 1, optical flow ranging module 2 and optical flow ranging module 3 respectively. s The obtained changes in the number of pixels in the X-axis and Y-axis directions, the angular velocities of the X-axis and Y-axis can be obtained from the IMU data, and the error is small and the credibility is high. Therefore, the equations are solved to obtain the three-axis velocity and the Z-axis angular velocity, and it is determined whether the change in optical flow data is caused by translation, and then constrained.

5. The method for self-detecting relative yaw angle of a head mounted device against magnetic interference according to claim 1, characterized in that: The specific steps of step S4 are: ① When the magnetometer in the IMU measures that the external magnetic field strength does not exceed the set threshold K mag When the magnetic field intensity changes slightly, the yaw angle output by the optical flow module is used; When the light intensity sensor measures light intensity exceeding the set lower limit K of And the light intensity changes little, so the yaw angle output by IMU is used; ② When the yaw angles output by IMU and optical flow are both reliable, the data are fused to reduce the error. The Kalman filter principle is used to establish the equation: WITH k =Z1+K k (Z2-Z1) Let Z1 be the angle data measured by IMU and Z2 be the angle data measured by optical flow module. The purpose is to find K k Make the standard of the estimated value the least, specifically to find the minimum value of the variance of the estimated value, for K k Take the derivative, then set it to 0, find the extreme value, get the Kalman gain, then calculate the estimate, and finally update the variance of the estimate, and iterate.

6. The method for self-detecting relative yaw angle of a head-mounted device against magnetic interference according to claim 1, characterized in that: The specific steps of step S5 are: Yaw=K OF *Z1+Z2 When the device moves, the optical flow data changes, K OF If it is 1, the output yaw angle is the result of the fusion output of the IMU and the optical flow module. At this time, the attitude angle will change continuously with the movement of the device; When the device stops moving, the optical flow data does not change, K OF is 0, so the data output by the optical flow can be used to determine whether the device is moving, thereby eliminating the drift of the yaw angle output by the IMU due to magnetic field interference.

Images