Helmet posture tracking system and method using IMU to assist in capturing visual feature points

Abstract

The present invention relates to a helmet posture tracking system and method for IMU-assisted visual feature point capture; the system comprises: a camera unit, a marker unit, an IMU unit and a tracking processing unit; the marker unit comprises a plurality of groups of markers set at different positions of the helmet in a lit or extinguished state; the camera unit comprises a plurality of cameras set in a vehicle cabin; each camera shooting angle is oriented toward the helmet moving area in the cabin, so that at least one camera is aimed at a group of lit markers for shooting; the IMU unit comprises a helmet IMU and a vehicle IMU; the tracking processing unit uses the features of the lit markers in the image captured by the camera unit to perform visual posture measurement of the helmet; and establishes a Kalman filter for visual and inertial fusion, and uses IMU data to filter the visual posture measurement data; according to the filtering result, the position of the lit marker in the next frame of the captured image is predicted for fast matching of the next frame of visual posture measurement. The present invention satisfies the requirements of large-scale and high-precision visual measurement.

Description

Technical Field

[0001] The present invention relates to the technical field of posture tracking, and in particular to a helmet posture tracking system and method for capturing visual feature points with the assistance of an IMU. Background Art

[0002] At present, the relative position and posture tracking application of helmets in sports vehicles usually uses the infrared light group arranged on the helmet and the infrared camera arranged in the cockpit of the sports vehicle to complete the calculation of the position and posture of the helmet. In this application, the high-speed camera and the light group after accurate calibration are coordinated. After the camera obtains the real-time image, the image is processed by the vehicle computer to extract the light feature point information. After the feature point matching, the position and posture information of the helmet relative to the cockpit is solved by the PnP method of computer vision.

[0003] The basis of the PnP pose measurement method is accurate feature point matching. When the helmet moves slowly, the feature point matching process can be iterated based on the matching result of the previous frame of camera image as the initial value; but when the helmet moves too fast relative to the camera or the light group is switched, the position of the feature point changes greatly in the image, and it is impossible to use the previous frame of data for iterative judgment. If the next set of image data has arrived before the iteration of the previous frame of image is completed, the feature point matching cannot be completed at this time, causing the helmet tracking to fail. Accurate measurement of low dynamic conditions.

[0004] In order to solve the dynamics of helmet posture tracking, IMU is usually introduced as a supplementary means, and a Kalman filter is designed to use visual measurement results as observations to correct the integral drift of IMU. The delay between visual solution and IMU solution is basically a fixed value. When the vision is accurate, the filter can obtain good measurement results. However, when the range of movement of the helmet position is too large, there is a lack of visual measurement, resulting in excessive feature matching calculations and poor tracking performance. Summary of the invention

[0005] In view of the above analysis, the present invention aims to provide a helmet posture tracking system and method for IMU-assisted visual feature point capture to achieve head posture tracking and improve the dynamic performance of tracking.

[0006] The technical solution provided by the present invention is:

[0007] The present invention discloses a helmet posture tracking system for IMU-assisted visual feature point capture, comprising: a camera unit, a landmark unit, an IMU unit and a tracking processing unit;

[0008] The marking point unit includes a plurality of groups of marking points arranged at different positions of the helmet; each group of marking points is in a lit or unlit state;

[0009] The camera unit includes a plurality of cameras arranged at different positions in the vehicle cockpit; each camera has a shooting angle directed toward a range of a helmet moving area in the cockpit, so that at least one camera is aimed at a group of illuminated marker points for shooting;

[0010] IMU units, including helmet IMU and vehicle IMU, measure IMU data of helmet and vehicle respectively;

[0011] The tracking processing unit uses the features of the illuminated marker points in the image captured by the camera unit to perform visual attitude measurement of the helmet; and establishes a Kalman filter that integrates vision and inertia, and uses IMU data to filter the visual attitude measurement data; based on the filtering results, the position of the illuminated marker points in the next frame of the captured image is predicted for fast matching of the next frame of visual attitude measurement.

[0012] Furthermore, the camera unit, the landmark unit and the IMU unit are synchronously controlled; specifically including:

[0013] The cameras in the camera unit are numbered, and one of the cameras is used as the master camera to generate a synchronization signal Cam SYNC; the remaining cameras shoot synchronously after receiving the synchronization signal Cam SYNC, and send each frame of image with the camera number to the tracking processing unit;

[0014] The multiple groups of marker points of the marker point unit are numbered; the synchronization signal Cam SYNC controls the lighting of each group of marker points, and sends the number information of the lit marker points to the tracking processing unit;

[0015] The synchronization signal Cam SYNC is also sent to the IMU unit to control the synchronous measurement of the helmet IMU and the cockpit IMU.

[0016] Furthermore, the tracking processing unit includes an IMU differential module, a visual attitude measurement module, a Kalman filter and a position prediction module; wherein,

[0017] IMU differential module, used to perform differential calculation on the measurement data of the helmet IMU and the vehicle IMU to obtain the acceleration and angular velocity information of the helmet relative to the cockpit;

[0018] The visual attitude measurement module is used to match the feature points of the image in which a complete set of lit marker points appear in the camera field of view, and perform PnP solution after the feature points are matched, so as to obtain the visual attitude measurement data of the helmet relative to the cockpit and output it to the Kalman filter in real time; the visual attitude measurement data includes position and attitude data;

[0019] The Kalman filter is used to establish the state vector of the Kalman filter based on the acceleration and angular velocity information of the helmet relative to the vehicle, construct the propagation equation, and use the visual attitude data as the observation quantity to update the filter; the position and attitude information of the filtered helmet relative to the cockpit is used as the output;

[0020] The position prediction module is used to pre-integrate the filtered position and posture information of the helmet relative to the cockpit, combined with the new acceleration and angular velocity information of the helmet relative to the cockpit received from the IMU differential module, to infer the possible position of the landmark point in the next frame.

[0021] Furthermore, the visual attitude measurement module adopts a visual monocular working mode. In the visual monocular working mode,

[0022] When a complete set of lit marker points appears in a camera's field of view, the marker point number is confirmed and feature point matching is performed. After feature point matching, PnP solution is performed to obtain the position and posture measurement of the marker points, and the visual measurement results are output in real time for subsequent filter processing;

[0023] When the movement of the illuminated marker points exceeds the field of view of the shooting camera, the marker point unit lights up each group of marker points in turn according to the synchronization signal Cam SYNC; each camera continues to determine whether there is a complete set of marker points within the field of view of a camera. If so, the marker point switching is stopped, and the image of the camera that captured the group of marker points is used for feature point matching and PnP solution.

[0024] Furthermore, in the PnP solution process, a spatial orthogonal iterative algorithm is used for visual pose measurement.

[0025] Furthermore, the Kalman filter uses the relative acceleration and relative angular velocity obtained by the difference between the helmet IMU and the vehicle IMU to construct the state vector Construct the propagation equation; use the position and attitude measurement data of visual observation as the observation quantity to update the filter; take the position and attitude information of the filtered helmet relative to the cockpit as the output;

[0026] in, b is the position, velocity, and rotation quaternion of the helmet IMU in the cockpit IMU coordinate system; bω , b ba The zero bias of the angular velocity and acceleration measured by the helmet IMU; b vω , b va is the zero bias of the angular velocity and acceleration measured by the vehicle IMU; λ is the visual scale factor.

[0027] Furthermore, during the tracking process, when the camera that illuminates the marker point or photographs the marker point is switched, the fast matching process includes:

[0028] 1) According to the synchronization signal Cam SYNC, obtain the Kalman filter output of the light group or camera at the switching moment, and the posture data of the helmet relative to the cockpit;

[0029] 2) obtaining the spatial three-dimensional coordinates of each group of marker points in the cockpit according to the posture data of the helmet relative to the cockpit and the spatial positions of each group of marker points on the helmet;

[0030] 3) Based on the shooting angle of each camera, the three-dimensional spatial coordinates are projected into two-dimensional coordinates to calculate the two-dimensional coordinates of the marker points that can be photographed by each camera in the camera shooting picture; at the same time, according to the number of the marker point being lit determined by the synchronization signal Cam SYNC, the calculated two-dimensional coordinates of the lit marker point in each camera shooting picture are obtained;

[0031] 4) Obtain the actual two-dimensional coordinates of the lit marker points in the actual captured images of each camera at the switching moment, and calculate the center distance between the actual two-dimensional coordinates and the corresponding calculated two-dimensional coordinates; when the center distance calculated by a certain camera is less than the set threshold, the camera and the lit marker point are matched with feature points, and PnP solution is performed after the feature points are matched to obtain the visual attitude data of the helmet relative to the cockpit.

[0032] Furthermore, each group of marking points includes a plurality of luminous characteristic points; and the plurality of luminous characteristic points of each group of marking points are arranged on the helmet in a certain geometric configuration.

[0033] Furthermore, the arrangement of the luminous characteristic points in each group of marking points is in a tetrahedron or pyramid shape; wherein,

[0034] In the tetrahedral shape, the three feature points are located in the same feature plane, and the central feature point is higher than the plane;

[0035] In the pyramid shape, the four feature points are located on the same feature plane, and the central feature point is higher than the plane.

[0036] The present invention also discloses a helmet posture tracking method of a helmet posture tracking system using the IMU as described above to assist in capturing visual feature points, comprising:

[0037] Step S1, synchronously controlling the camera unit, the landmark unit and the IMU unit in the system;

[0038] Step S2, collecting the acceleration and angular velocity of the vehicle and the helmet according to the IMU unit, and performing inertial differential calculation to obtain the acceleration and angular velocity information of the helmet relative to the vehicle;

[0039] Step S3, performing feature matching according to the information of the illuminated marker points, and performing PnP solution after the feature point matching to obtain the position and posture measurement of the visually observed helmet relative to the cockpit;

[0040] Step S4: Based on the acceleration and angular velocity information of the helmet relative to the vehicle, establish the state vector of the Kalman filter, construct the propagation equation, use the visual attitude data as the observation quantity to update the filter; and use the filtered position and attitude information of the helmet relative to the cockpit as output;

[0041] Step S5: pre-integrate the filtered position and posture information of the helmet relative to the cockpit, combined with the new acceleration and angular velocity information of the helmet relative to the cockpit calculated by inertial difference, to infer the possible position of the marker point in the next frame.

[0042] The present invention can achieve at least one of the following beneficial effects:

[0043] The present invention proposes a helmet posture tracking system and method for IMU-assisted visual feature point capture, which simultaneously meets the requirements of large-scale and high-precision visual measurement through the layout of infrared feature points and the synchronous triggering of cameras.

[0044] After the IMU-assisted feature point matching method is introduced, the feature point position can be predicted while waiting for the next frame of image and the light group switching, thus reducing the computational complexity of feature point matching.

[0045] Compared with traditional relative posture measurement schemes, this method makes more effective use of equipment synchronization characteristics and is easy to deploy, making it suitable for engineering applications such as various passenger head-mounted display systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0047] Figure 1 It is a schematic block diagram of the composition of the helmet posture tracking system in an embodiment of the present invention;

[0048] Figure 2 A schematic diagram of a tetrahedral arrangement of luminous feature points within a group in an embodiment of the present invention;

[0049] Figure 3 A schematic diagram of a pyramid-shaped arrangement of luminous feature points within a group in an embodiment of the present invention;

[0050] Figure 4 This is a schematic diagram of a thin light diffuser covering the infrared LED light beads in an embodiment of the present invention;

[0051] Figure 5A schematic diagram of staggered arrangement of light-emitting characteristic points between groups in an embodiment of the present invention;

[0052] Figure 6 A schematic diagram of a synchronization method in an embodiment of the present invention;

[0053] Figure 7 A schematic diagram of the connection of the tracking processing unit in an embodiment of the present invention;

[0054] Figure 8 4 is a flow chart of a method for tracking a helmet posture in an embodiment of the present invention. DETAILED DESCRIPTION

[0055] The preferred embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used to illustrate the principles of the present invention together with the embodiments of the present invention.

[0056] This embodiment discloses a helmet posture tracking system that uses an IMU to assist in capturing visual feature points. Figure 1 As shown, it includes: a camera unit, a landmark unit, an IMU unit and a tracking processing unit;

[0057] The marking point unit includes a plurality of groups of marking points arranged at different positions of the helmet; each group of marking points is controlled to be in a lighted or extinguished state;

[0058] Specifically, each group of marking points is in a light-on or light-off state under the control of the marking point controller; the marking points are numbered as LED1, LED2, LED3, ..., LEDm;

[0059] The camera unit includes a plurality of cameras arranged at different positions in the vehicle cockpit; each camera has a shooting angle directed toward a range of a helmet moving area in the cockpit, so that at least one camera is aimed at a group of illuminated marker points for shooting;

[0060] The cameras are numbered as Cam1, Cam2, Cam3, ..., Camn; and the cameras may be high-speed cameras to ensure the sampling frequency of the images;

[0061] IMU units, including helmet IMU and vehicle IMU, measure IMU data of helmet and vehicle respectively;

[0062] The tracking processing unit uses the features of the illuminated marker points in the image captured by the camera unit to perform visual attitude measurement of the helmet; and establishes a Kalman filter that integrates vision and inertia, and uses IMU data to filter the visual attitude measurement data; based on the filtering results, the position of the illuminated marker points in the next frame of the captured image is predicted for fast matching of the next frame of visual attitude measurement.

[0063] Specifically, the multiple groups of marker points are set at different positions of the helmet and are in a lighted or extinguished state under the control of the marker point controller; the marker points are numbered as LED1, LED2, LED3, ..., LEDm;

[0064] The IMU unit includes a helmet IMU, a vehicle IMU, and an IMU controller;

[0065] The vehicle IMU is arranged in the vehicle cockpit and fixedly connected to the cockpit, and is used to measure the acceleration and angular velocity data of the moving vehicle;

[0066] Preferably, the vehicle IMU is arranged on one of the cameras on the cockpit;

[0067] The helmet IMU is arranged on the helmet and fixedly connected to the helmet, and is used to measure acceleration and angular velocity data of the helmet.

[0068] Specifically, among the multiple groups of marking points set at different positions of the helmet, each group of marking points includes multiple luminous feature points; and the multiple luminous feature points of each group of marking points are arranged on the helmet in a certain geometric configuration; the geometric configurations of the multiple groups of marking points can be the same or different.

[0069] Preferably, the luminous feature points in the marking points use infrared LED light beads, and the camera of the corresponding camera unit is an infrared camera.

[0070] The luminous feature points can also emit light in two ways: active luminescence or passive luminescence; the active way is self-luminescence, and the passive way is camera or external environment fill light.

[0071] like Figure 2 As shown, in a typical geometric configuration, the luminous feature points in the group are arranged in a tetrahedron shape, wherein three feature points are located in the same feature plane and the central feature point is higher than the plane.

[0072] like Figure 3 As shown, in another typical geometric configuration, the luminous feature points in the group are arranged in a pyramid shape, wherein four feature points are located in the same feature plane and the central feature point is higher than the plane.

[0073] And, in Figure 2 and Figure 3 In the two arrangements, the normal of each luminous feature point is perpendicular to the feature plane, which is convenient for camera observation.

[0074] In a preferred embodiment, Figure 4 As shown, a thin soft light sheet is covered over the infrared LED light-emitting lamp beads at the light-emitting feature points, and the light emission uniformity and visible range of the lamp beads are controlled by the thin soft light sheet.

[0075] Since the surface area of ​​the helmet is relatively small, more groups of marking points are arranged on the smaller helmet surface to improve the utilization rate of the helmet surface. Figure 5 In the preferred embodiment shown, multiple groups of infrared LED light beads are arranged in a staggered manner on the helmet.

[0076] In the preferred embodiment, Figure 6 As shown, in order to facilitate the data transmission and operation of the system, the camera unit, the landmark unit and the IMU unit are synchronously controlled; specifically, the following steps are performed:

[0077] 1) Number each camera in the camera unit, and use one of the cameras as the master camera to generate a synchronization signal Cam SYNC; the remaining cameras shoot synchronously after receiving the synchronization signal Cam SYNC, and send each frame of image with the camera number to the tracking processing unit;

[0078] In the specific example, the camera numbered Cam 1 is the main control camera, responsible for generating the synchronization signal Cam SYNC. The other cameras shoot synchronously after receiving Cam SYNC. The images shot synchronously by Cam 1, Cam 2, Cam 3...Cam n are sent to the tracking processing unit through Cam DATA. In order to reduce the amount of calculation and transmission speed of the tracking processing unit, the camera can also send only the extracted infrared feature points.

[0079] 2) Numbering multiple groups of marker points of the marker point unit; the synchronization signal Cam SYNC controls the lighting of each group of marker points, and sends the number information of the lit marker points to the tracking processing unit;

[0080] In a specific example, the synchronization signal Cam SYNC generated by the master camera is also sent to the marker controller LEDController for controlling the lighting of LEDs. Each camera can correspond to any group of LEDs, that is, Cam 1 can correspond to LED 1, LED 2, LED 3, ..., LED n, and similarly Cam 2, Cam 3, ..., Cam n. After controlling the lighting of the light group, the marker controller LED Controller also sends the number information of the lit light group to the tracking processing unit for subsequent calculations.

[0081] 2) The synchronization signal Cam SYNC is also sent to the IMU unit to control the synchronous measurement of the helmet IMU and the cockpit IMU.

[0082] In a specific embodiment, after receiving the synchronization signal Cam SYNC, the IMU unit can control the two IMUs to use the frequency of the synchronization signal Cam SYNC to control the IMU to sample. For example, if the camera sampling frequency is 120Hz, then the IMU can use a sampling frequency of 960Hz. After the collection is completed, the IMU Controller sends it to the tracking processing unit.

[0083] In addition, before starting the position and attitude measurement, the internal parameters of the cameras Cam 1, Cam 2, Cam3, ..., Cam n in the camera unit are calibrated respectively, and the external parameters are calibrated after each camera is installed; and the three-dimensional spatial position coordinates of each marker point in the marker point unit are calibrated.

[0084] Specifically, Figure 7 As shown, the tracking processing unit includes an IMU differential module, a visual attitude measurement module, a Kalman filter and a position prediction module; wherein,

[0085] IMU differential module, used to perform differential calculation on the measurement data of the helmet IMU and the vehicle IMU to obtain the acceleration and angular velocity information of the helmet relative to the cockpit;

[0086] The visual attitude measurement module is used to match the feature points of the image in which a complete set of landmark points appears in the camera field of view, and perform PnP solution after the feature points are matched, so as to obtain the visual attitude measurement data of the helmet relative to the cockpit and output it to the Kalman filter in real time; the visual attitude measurement data includes position and attitude data;

[0087] The Kalman filter is used to establish the state vector of the Kalman filter based on the acceleration and angular velocity information of the helmet relative to the vehicle, construct the propagation equation, and use the visual attitude data as the observation quantity to update the filter; the position and attitude information of the filtered helmet relative to the cockpit is used as the output;

[0088] The position prediction module is used to pre-integrate the filtered position and posture information of the helmet relative to the cockpit, combined with the new acceleration and angular velocity information of the helmet relative to the cockpit received from the IMU differential module, to infer the possible position of the landmark point in the next frame.

[0089] The visual attitude measurement module adopts a visual "monocular" working mode. In the visual monocular working mode,

[0090] When a complete set of group landmark points appears in the field of view of a camera, feature point matching is performed according to the group landmark point number, and PnP solution is performed after feature point matching to obtain position and attitude measurements, and the visual measurement results are output to the filter in real time for subsequent filter processing, the filter is updated, and the position and attitude information at the next moment is inferred.

[0091] The feature point matching may be achieved by performing image matching according to the number of luminous feature points included in the marker point group corresponding to the group marker point number and the geometric configuration of the luminous feature points.

[0092] When the feature point moves beyond the camera field of view, the marker point unit lights up each group of marker points in turn according to the synchronization signal Cam SYNC; each camera continues to determine whether there is a complete set of LED marker points in a camera field of view. If there is a complete set of group marker points, the marker point switching is stopped and the group of marker points is used for feature point matching and PnP solution.

[0093] In the visual attitude measurement module, a visual "multi-eye" working mode can also be adopted, and the visual "multi-eye" working mode is a "high-precision" working mode, wherein:

[0094] Multiple cameras can observe the same set of landmarks at the same time. When at least two cameras observe the same set of landmarks, they enter the "high-precision" working mode. In the "high-precision" mode, the multi-camera can directly obtain the optimized value based on the "3D reconstruction". The multi-channel visual measurement results are output to the filter in real time for subsequent filter processing;

[0095] Update the filter to estimate the position and attitude information at the next moment, and obtain more accurate position and attitude measurements.

[0096] The "multi-eye" working mode has higher accuracy, but it can only be used when multiple cameras can see the same group of landmarks at the same time. At the same time, this method takes up a lot of computer resources.

[0097] In this embodiment, the coordinate system of the helmet landmark point is defined as h, and the IMU coordinate system on the helmet is defined as b. The h system and the b system remain relatively fixed when the helmet moves. The camera coordinate system is defined as c, and the vehicle IMU coordinate system is defined as v. The c system and the v system remain relatively stationary when the helmet moves relative to the vehicle.

[0098] Specifically, in the PnP solution process, a spatial orthogonal iterative algorithm is used for visual pose measurement. The solution process includes:

[0099] 1) Establish a camera model;

[0100] Definition P i is a spatial coordinate point, and its three-dimensional coordinates are [X i ,Y i ,Z i ] T , in meters. Point P i The coordinate in the camera coordinate system is p i For [x i ,y i ,z i ] T. Space point P i To the point p in the camera coordinate system i There is an external parameter conversion relationship;

[0101]

[0102] in It is the 3×3 rotation matrix and 3×1 translation vector from the camera coordinate system to the helmet coordinate system. The physical meaning of each row of elements is the coordinates of the unit vector of the helmet coordinate system in the camera coordinate system, the translation vector The physical meaning of is the coordinate of the origin of the helmet coordinate system in the camera coordinate system.

[0103] Point p i In the normalized plane coordinates [u i ,v i ,1] T , the unit is pixel, the normalized plane coordinates and the camera coordinate system have an intrinsic transformation relationship

[0104]

[0105] Among them, f x 、f y 、c x 、c y is the camera intrinsic parameter, in pixels, and K is the camera intrinsic parameter matrix.

[0106] 2) Use spatial orthogonal iterative algorithm to perform visual pose measurement;

[0107] definition is the line of sight projection matrix, when V i When acting on a vector, the vector can be projected vertically onto p i superior.

[0108] Define point p i In V i The projection on is q i , then

[0109] q i =V i (RP i +t) (3)

[0110] Ideally, the object point, image point, and camera origin satisfy the spatial collinearity equation, that is, p i In V i The projection on should be p i itself

[0111] RP i +t=V i (RPi +t) (4)

[0112] The target space collinearity error obtained by deformation is:

[0113] e i =(IV i )(RP i +t) (5)

[0114] The sum of squares of spatial collinear errors is used as the objective function, and the optimal estimates of R and t are obtained by optimizing the objective function.

[0115]

[0116] The objective function can be obtained by partial derivative when the rotation R is given. Get the optimal solution of t with respect to R:

[0117]

[0118] Therefore, for a fixed R, the corresponding t can be obtained by the above formula. Next, find the optimal solution for R. For the estimated value of R at the kth iteration, R (k) , we can get the kth iteration t (k) , calculate the space point P i Projection estimate of

[0119]

[0120] k+1 times rotation matrix estimate R (k+1) It can be solved by finding the minimum value of the following function

[0121]

[0122] This formula can be regarded as a point set {P i} to the point set {q i The absolute orientation problem can be solved by singular value decomposition (SVD). The steps are as follows: and is the centroid of the point set, we have

[0123]

[0124] Define (1 / n)M as a point set {P i} and the point set {q i The covariance matrix of

[0125]

[0126] Then the R that minimizes E(R,t) is * With t * satisfy

[0127] R * = arg max R tr(R t M) (12)

[0128]

[0129] Perform SVD decomposition on M, that is, U T MV=Σ, the optimal solution at this time

[0130] R (k+1) =VU T (13) The algorithm has global convergence. For any initial rotation matrix R, repeating the above steps can converge to the optimal value. The optimal value of convergence is , through formula (7) we can get .

[0131] Specifically, the list of variables in the Kalman filter is as follows:

[0132]

[0133] In the Kalman filter,

[0134] The state vector is constructed using the relative acceleration and relative angular velocity obtained by the difference between the helmet IMU and the vehicle IMU. Construct propagation equations; use the position and attitude measurements of visual observations as observations to update the filter.

[0135] in, b is the position, velocity, and rotation quaternion of the helmet IMU in the cockpit IMU coordinate system; bω , b ba The zero bias of the angular velocity and acceleration measured by the helmet IMU; b vω , b va is the zero bias of the angular velocity and acceleration measured by the vehicle IMU; λ is the visual scale factor.

[0136] Noise-free state

[0137] Used to represent the true state X and the state without noise The state error vector It is expressed as:

[0138]

[0139] Among them, the details of each item are as follows:

[0140]

[0141]

[0142]

[0143]

[0144]

[0145]

[0146] in, The error state dynamics are small and can be derived from linear equations. Need to be obtained through linearization processing, where is a small angle approximation of the quaternion error.

[0147] After linearization, we get:

[0148]

[0149]

[0150] The system state error recursive relationship, that is, the state error equation is:

[0151]

[0152] F X is the state transfer matrix; F N is the noise transfer matrix;

[0153] U=[a h ,ω h ,a v ,ω v ] T ;

[0154] a h ,ω h 、a v , and ω v They are respectively the acceleration and angular velocity output of the helmet IMU and the acceleration and angular velocity output of the sports vehicle IMU;

[0155] N is the state noise vector,

[0156]

[0157]

[0158]

[0159]

[0160]

[0161] in, They are respectively the variance of helmet IMU acceleration noise, motion vehicle IMU acceleration noise, helmet IMU angular velocity noise, motion vehicle IMU angular velocity noise, helmet IMU acceleration zero bias noise, motion vehicle IMU acceleration zero bias noise, helmet IMU angular velocity zero bias noise and motion vehicle IMU angular velocity zero bias noise.

[0162] Further,

[0163] The state transition matrix

[0164] in, is the relative rotation matrix from the helmet to the moving vehicle without error; The relative rotation matrix from the moving vehicle to the helmet without error;

[0165]

[0166]

[0167] I is the identity matrix;

[0168] Noise transfer matrix

[0169] Then, the state covariance matrix of the system is

[0170] Based on the above process, the process of updating the error state covariance matrix of the Kalman filter includes:

[0171] 1) Obtain helmet IMU data;

[0172] 2) Update the state vector according to the helmet motion model

[0173] 3) Update the state transfer matrix F X , Update the covariance matrix F N NF N T ;

[0174] 4) Update the error state covariance matrix .

[0175] The observation equation of the Kalman filter for posture measurement applicable to this scheme is:

[0176]

[0177] The error vector Hp is the position measurement matrix; the error vector H q is the position measurement matrix; z p 、z q is the position vector and attitude vector of the visual observation of Kalman filter; The position vector and attitude vector estimated by Kalman filter.

[0178] During the observation process,

[0179] 1) Write and update the partial position measurement model z p ;

[0180] Among them, the position measurement model

[0181] In the formula, Represents the displacement of the landmark point relative to the camera, which is obtained by visual measurement after the internal parameter changes; is the transformation matrix from the vehicle coordinate system to the camera coordinate system, which can be obtained through calibration; Represents the displacement of the helmet coordinate system in the vehicle coordinate system; is the translation vector and rotation matrix between the helmet IMU and the vehicle coordinates, and is the state vector in the filter; is the external parameter of the helmet relative to the IMU, which can be obtained through calibration; n p To measure noise.

[0182] The error vector Expanded:

[0183]

[0184] After expansion, ignoring the second-order terms, we get:

[0185]

[0186] According to the observation equation Δz p =H p Δx, position measurement matrix H p Write as follows:

[0187]

[0188] In the formula, is the position observation, is the corresponding cross product matrix.

[0189] 2) Write and update the partial attitude measurement model z q ;

[0190] Among them, the posture measurement model

[0191] The error vector Expand

[0192]

[0193] According to the observation equation Δz q =H q Δx, rotation measurement matrix H q Write as follows:

[0194]

[0195] The process of updating the state covariance matrix and the state vector in this embodiment includes:

[0196] 1) Calculate observation residuals

[0197] 2) Calculate the update matrix S = HPH T +R;

[0198] 3) Calculate Kalman gain K = PH T S -1 ;

[0199] 4) Calculate the state correction

[0200] 5) Calculate the recursive result of the state covariance matrix P←(I d -KH)P(I d -KH) T +KRK T .

[0201] 6) Update the state With the original state vector After superposition, the updated state vector is obtained.

[0202] The position prediction module is used to pre-integrate the filtered position p and attitude q information of the helmet relative to the cockpit, combined with the new acceleration a and angular velocity ω information of the helmet relative to the cockpit received from the IMU differential module, to infer the possible position of the landmark point in the next frame.

[0203] The camera unit uses the possible position of the landmark point in the next frame output by the position prediction module to perform fast matching and output the visual pose measurement result.

[0204] Furthermore, during the tracking process, when the camera that illuminates the marker point or photographs the marker point is switched, the fast matching process includes:

[0205] 1) According to the synchronization signal Cam SYNC, obtain the Kalman filter output of the light group or camera at the switching moment, and the posture data of the helmet relative to the cockpit;

[0206] 2) obtaining the spatial three-dimensional coordinates of each group of marker points in the cockpit according to the posture data of the helmet relative to the cockpit and the spatial positions of each group of marker points on the helmet;

[0207] 3) Based on the shooting angle of each camera, the three-dimensional spatial coordinates are projected into two-dimensional coordinates to calculate the two-dimensional coordinates of the marker points that can be photographed by each camera in the camera shooting picture; at the same time, according to the number of the marker point being lit determined by the synchronization signal Cam SYNC, the calculated two-dimensional coordinates of the lit marker point in each camera shooting picture are obtained;

[0208] 4) Obtaining the actual two-dimensional coordinates of the lit marker points in the actual captured images of each camera at the switching moment, and calculating the center distance between the actual two-dimensional coordinates and the corresponding calculated two-dimensional coordinates; when the center distance calculated by a certain camera is less than a set threshold, the camera and the lit marker point are matched with feature points, and PnP solution is performed after the feature points are matched to obtain the visual attitude data of the helmet relative to the cockpit;

[0209] The set threshold is σΔt, where σ is the measurement allowable error coefficient, which is set according to an empirical value; and Δt is the interval time of the synchronization signal.

[0210] If there are multiple sets of correctly matched feature points, the feature point closest to the center of the picture and with the largest circumscribed circle area can be used as the visual measurement value to continue updating the filter (small amount of calculation); or multiple sets of visual measurement values ​​can be used as visual measurement values ​​to continue updating the filter (high accuracy).

[0211] In summary, the IMU-assisted helmet posture tracking system for capturing visual feature points in the embodiment of the present invention satisfies both large-scale and high-precision visual measurement through the infrared feature point layout and camera synchronous triggering. After introducing the IMU-assisted feature point matching method, the feature point position can be predicted while waiting for the next frame of the image and the light group switching, reducing the computational complexity of feature point matching; compared with the traditional relative posture measurement solution, this method more effectively utilizes the synchronization characteristics of the equipment, and is simple to arrange, and is suitable for various engineering applications such as passenger head-mounted display systems.

[0212] Embodiment 2

[0213] This embodiment discloses a helmet posture tracking method of the helmet posture tracking system using the IMU-assisted visual feature point capture described in the first embodiment. Figure 8 As shown, the following steps are included:

[0214] Step S1, synchronously controlling the camera unit, the landmark unit and the IMU unit in the system;

[0215] Step S2, collecting the acceleration and angular velocity of the vehicle and the helmet according to the IMU unit, and performing inertial differential calculation to obtain the acceleration and angular velocity information of the helmet relative to the vehicle;

[0216] Step S3, performing feature matching according to the information of the illuminated marker points, and performing PnP solution after the feature point matching to obtain the position and posture measurement of the visually observed helmet relative to the cockpit;

[0217] Step S4: Based on the acceleration and angular velocity information of the helmet relative to the vehicle, establish the state vector of the Kalman filter, construct the propagation equation, use the visual attitude data as the observation quantity to update the filter; and use the filtered position and attitude information of the helmet relative to the cockpit as output;

[0218] Step S5: pre-integrate the filtered position and posture information of the helmet relative to the cockpit, combined with the new acceleration and angular velocity information of the helmet relative to the cockpit calculated by inertial difference, to infer the possible position of the marker point in the next frame.

[0219] The specific technical details and beneficial effects of this embodiment are the same as those described in the previous embodiment. Please refer to the previous embodiment and will not be described in detail here.

[0220] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A helmet posture tracking system with IMU-assisted visual feature point capture, characterized in that: include: Camera unit, landmark unit, IMU unit and tracking processing unit; A marker point unit, comprising a plurality of groups of marker points arranged at different positions of the helmet; Each group of marking points is in a lit or unlit state; The camera unit includes a plurality of cameras arranged at different positions in the vehicle cockpit; each camera has a shooting angle directed toward a range of a helmet moving area in the cockpit, so that at least one camera is aimed at a group of illuminated marker points for shooting; IMU units, including helmet IMU and vehicle IMU, measure IMU data of helmet and vehicle respectively; The tracking processing unit uses the features of the illuminated marker points in the image captured by the camera unit to perform visual posture measurement of the helmet; A Kalman filter that integrates vision and inertia is established, and the visual attitude measurement data is filtered using IMU data; The position of the highlighted mark in the next frame of the image is predicted based on the filtering result, which is used for fast matching of the next frame of visual pose measurement. Each group of marking points includes a plurality of luminous characteristic points; the luminous characteristic points in each group of marking points are arranged in a tetrahedron or pyramid shape; wherein, In the tetrahedral shape, the three feature points are located in the same feature plane, and the central feature point is higher than the plane; In the pyramid shape, the four feature points are located on the same feature plane, and the central feature point is higher than the plane; In order to arrange more groups of marking points on the surface of the helmet and improve the utilization rate of the helmet surface, multiple groups of luminous feature points are arranged in a staggered manner on the helmet.

2. The helmet posture tracking system according to claim 1, characterized in that: The camera unit, the landmark unit and the IMU unit are synchronously controlled; specifically including: The cameras in the camera unit are numbered, and one of the cameras is used as the master camera to generate a synchronization signal Cam SYNC; the remaining cameras shoot synchronously after receiving the synchronization signal Cam SYNC, and send each frame of image with the camera number to the tracking processing unit; The multiple groups of marker points of the marker point unit are numbered; the synchronization signal Cam SYNC controls the lighting of each group of marker points, and sends the number information of the lit marker points to the tracking processing unit; The synchronization signal Cam SYNC is also sent to the IMU unit to control the synchronous measurement of the helmet IMU and the cockpit IMU.

3. The helmet posture tracking system according to claim 1, characterized in that: The tracking processing unit includes an IMU differential module, a visual attitude measurement module, a Kalman filter and a position prediction module; wherein, IMU differential module, used to perform differential calculation on the measurement data of the helmet IMU and the vehicle IMU to obtain the acceleration and angular velocity information of the helmet relative to the cockpit; The visual attitude measurement module is used to match the feature points of the image in which a complete set of lit marker points appear in the camera field of view, and perform PnP solution after the feature points are matched, so as to obtain the visual attitude measurement data of the helmet relative to the cockpit and output it to the Kalman filter in real time; the visual attitude measurement data includes position and attitude data; The Kalman filter is used to establish the state vector of the Kalman filter based on the acceleration and angular velocity information of the helmet relative to the vehicle, construct the propagation equation, and use the visual attitude data as the observation quantity to update the filter; the position and attitude information of the filtered helmet relative to the cockpit is used as the output; The position prediction module is used to pre-integrate the filtered position and attitude information of the helmet relative to the cockpit, combined with the new acceleration and angular velocity information of the helmet relative to the cockpit received from the IMU differential module, to infer the next frame position of the landmark point.

4. The helmet posture tracking system according to claim 3, characterized in that: The visual attitude measurement module adopts a visual monocular working mode. In the visual monocular working mode, When a complete set of lit marker points appears in a camera's field of view, the marker point number is confirmed and feature point matching is performed. After feature point matching, PnP solution is performed to obtain the position and posture measurement of the marker points, and the visual measurement results are output in real time for subsequent filter processing; When the movement of the illuminated marker points exceeds the field of view of the shooting camera, the marker point unit lights up each group of marker points in turn according to the synchronization signal Cam SYNC; each camera continues to determine whether there is a complete set of marker points within the field of view of a camera. If so, the marker point switching is stopped, and the image of the camera that captured the group of marker points is used for feature point matching and PnP solution.

5. The helmet posture tracking system according to claim 4, characterized in that: During the PnP solution process, a spatial orthogonal iterative algorithm is used for visual pose measurement.

6. The helmet posture tracking system according to claim 3, characterized in that: The Kalman filter uses the relative acceleration and relative angular velocity obtained by the difference between the helmet IMU and the vehicle IMU to construct the state vector Construct propagation equations; use the position and attitude measurement data of visual observations as observations to update the filter; The filtered position and attitude information of the helmet relative to the cockpit is used as output; in, The position, velocity, and rotation quaternion of the helmet IMU in the cockpit IMU coordinate system; Measure the zero bias of angular velocity and acceleration for the helmet IMU; Measure the zero bias of angular velocity and acceleration for the vehicle IMU; is the visual scale factor.

7. The helmet posture tracking system according to claim 6, characterized in that: During the tracking process, when the camera that illuminates the marker point or shoots the illuminated marker point switches, the fast matching process includes: 1) According to the synchronization signal Cam SYNC, obtain the Kalman filter output of the light group or camera at the switching moment, and the posture data of the helmet relative to the cockpit; 2) obtaining the spatial three-dimensional coordinates of each group of marker points in the cockpit according to the posture data of the helmet relative to the cockpit and the spatial positions of each group of marker points on the helmet; 3) Based on the shooting angle of each camera, the spatial three-dimensional coordinates are projected to the two-dimensional coordinates, and the two-dimensional coordinates of the marker points that can be photographed by each camera in the camera shooting picture are calculated; at the same time, according to the number of the marker point being lit determined by the synchronization signal Cam SYNC, the calculated two-dimensional coordinates of the lit marker point in the shooting picture of each camera are obtained; 4) Obtain the actual two-dimensional coordinates of the lit marker points in the actual captured images of each camera at the switching moment, and calculate the center distance between the actual two-dimensional coordinates and the corresponding calculated two-dimensional coordinates; when the center distance calculated by a certain camera is less than the set threshold, the camera and the lit marker point are matched with feature points, and PnP solution is performed after the feature points are matched to obtain the visual attitude data of the helmet relative to the cockpit.

8. A method for helmet posture tracking using a helmet posture tracking system using an IMU to assist in capturing visual feature points as described in any one of claims 1 to 7, characterized in that: include: Step S1, synchronously controlling the camera unit, the landmark unit and the IMU unit in the system; Step S2, collecting the acceleration and angular velocity of the vehicle and the helmet according to the IMU unit, and performing inertial differential calculation to obtain the acceleration and angular velocity information of the helmet relative to the vehicle; Step S3, performing feature matching according to the information of the illuminated marker points, and performing PnP solution after the feature point matching to obtain the position and posture measurement of the visually observed helmet relative to the cockpit; Step S4: Based on the acceleration and angular velocity information of the helmet relative to the vehicle, a state vector of the Kalman filter is established, a propagation equation is constructed, and the filter is updated using the visual attitude measurement data as an observation quantity; The filtered position and attitude information of the helmet relative to the cockpit is used as output; Step S5: pre-integrate the filtered position and posture information of the helmet relative to the cockpit, combined with the new acceleration and angular velocity information of the helmet relative to the cockpit calculated by inertial difference, to infer the next frame position of the marker point.

Images