A passive optical motion capture method

Abstract

The present invention discloses a passive optical motion capture method, which relates to the technical field of motion capture and includes: S1, filtering by using an adaptive unscented Kalman algorithm for dynamically adjusting observation noise; S2, performing feature reconstruction as an observable quantity for state estimation of a target to be measured based on a feature reconstruction method for a fixed target and a feature reconstruction method based on a monocular EPnP algorithm; S3, performing information fusion between multiple groups of binocular systems in the form of matrix-weighted unscented Kalman filtering based on a loose coupling scheme. In view of the problem that outliers on a two-dimensional image are not thoroughly removed during target extraction in a complex background, the present invention proposes a motion capture method using prior volume constraint in three-dimensional space, and a motion capture method based on adaptive unscented Kalman filtering and target feature reconstruction. For a multi-camera system, information fusion can be performed based on loose coupling. By using binocular cameras and retroreflective targets, high-precision six-degree-of-freedom pose measurement in a complex background can be achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of motion capture, and particularly to a passive optical motion capture method. Background Art

[0002] The passive optical motion capture method is the most effective means for a mobile robot to perform high-precision pose measurement. This method uses retroreflective target balls as marking points. Based on computer graphics, the camera captures the inter-frame changes of the marking points to record the motion status of the object to be measured, and obtains the high-precision position and pose of the mobile robot. However, under complex conditions, the passive optical motion capture method has limitations. Mainly affected by complex backgrounds and occlusion conditions, the accuracy of its pose measurement results decreases: under a certain natural light illumination, the measurement site forms a complex background, and the interference of target adhesion and a large number of reflected light points leads to extraction errors and matching errors of the marking points; during the measurement process, the target balls are affected by occlusion conditions such as static occlusion, mutual occlusion, and self-occlusion, causing some marking points to be unable to be observed by the camera, resulting in a reduction in the number of marking points participating in pose estimation and matching errors.

[0003] Solving the problem of motion capture of a mobile robot under occlusion conditions is essentially tracking under occlusion conditions. Existing methods include: target tracking algorithms based on effective feature information, state estimation information, and stable spatio-temporal information.

[0004] (1) Target tracking methods based on effective feature information. Include optical flow motion feature method, manually designed appearance feature method, depth feature method, etc. For example, using a soft graph matching model to perform pose reconstruction for occlusion to estimate the missing marker positions, but this method is computationally complex and requires design and training for templates. Among them, target tracking algorithms based on deep learning have developed rapidly in recent years and often can obtain higher tracking accuracy, but there is still room for improvement in terms of accuracy, speed, and robustness.

[0005] (2) Target tracking methods based on state estimation information. Include two classic methods of Kalman filtering and particle filtering, which can achieve anti-occlusion through prediction. For example, using offline training to estimate the average displacement and angle of the previous few frames for severe occlusion, using Kalman filtering based on a constant velocity model to predict and reconstruct the three-dimensional coordinates of each missing marker point, but the motion process modeling of this method is relatively simple and cannot handle complex six-degree-of-freedom motions; or using adaptive unscented Kalman filtering for distributed fusion, this method uses a planar target and a monocular subsystem, but the applicable motion range is too small.

[0006] (3) Target tracking method based on stable spatio-temporal information. In the method implemented in the present invention, time, space, and spatio-temporal context are mainly utilized to improve the tracking stability in occlusion scenarios. However, this method improves the tracking stability to a large extent and cannot guarantee the improvement of the pose estimation accuracy in occlusion situations.

[0007] Therefore, those skilled in the art are committed to developing a passive optical motion capture method applicable to complex conditions to solve the problem of the decline in the system pose estimation accuracy caused by complex backgrounds and occlusion conditions. Summary of the Invention

[0008] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is the insufficient anti-occlusion accuracy of the unscented Kalman filter method and the decline in the system pose estimation accuracy caused by complex backgrounds and occlusion conditions.

[0009] To achieve the above object, the present invention provides a passive optical motion capture method, including the following steps:

[0010] S1. Filter using an adaptive unscented Kalman algorithm with dynamically adjusted observation noise;

[0011] S2. Based on the feature reconstruction method of a fixed target and the feature reconstruction method based on the monocular EPnP algorithm, perform feature reconstruction as the observation quantity for the state estimation of the target to be measured;

[0012] S3. Based on a loose coupling scheme, use the unscented Kalman filter form with matrix weighting for information fusion between multiple binocular systems.

[0013] In a preferred embodiment of the present invention, the adaptive unscented Kalman filter algorithm in S1 includes:

[0014] System initialization and generation of Sigma points;

[0015] Calculate the weights corresponding to each Sigma point;

[0016] Calculate the state mean and covariance of the one-step prediction;

[0017] Calculate the mean and variance of the predicted observation values;

[0018] Update to obtain the state mean and covariance at time k;

[0019] Update the observation noise R k .

[0020] In another preferred embodiment of the present invention, the weights corresponding to each Sigma point are:

[0021]

[0022]

[0023]

[0024] Among them, and are the mean weight and covariance weight corresponding to the initial Sigma points, W i m and W i c are the mean weight and covariance weight corresponding to the i-th Sigma point. α is taken as 0.001, β is taken as 2, and λ is the scaling factor.

[0025] In another preferred embodiment of the present invention, the feature reconstruction method based on the monocular EPnP algorithm in S2 includes:

[0026] For the feature point P i ω to be reconstructed, there is:

[0027]

[0028] Among them, is the two-dimensional pixel coordinate of the target ball, is the transformation matrix of the fixed target in the world coordinate system:

[0029]

[0030] Among them, is the translation vector of the current target coordinate system relative to the world coordinate system, is the rotation matrix of the previous target coordinate system relative to the world coordinate system;

[0031] The is obtained through the retroreflective target ball not affected by occlusion, and the remaining missing points are reconstructed based on the fixed target coordinate system to complete the feature information and update the state estimation problem as the observation quantity.

[0032] In another preferred embodiment of the present invention, the is calculated as follows:

[0033] For the initial three-dimensional world coordinate P 0 of each target ball, there is the centroid p 0 of the initial three-dimensional world coordinate:

[0034]

[0035] Define at this moment that the centroid of the P 0 is:

[0036]

[0037] Through binocular depth estimation, the three-dimensional world coordinates P of at least three target balls at a certain moment are obtained b :

[0038]

[0039] where n is a positive integer greater than or equal to 3. At the same time, at this moment, the centroid of the said P b is defined as:

[0040]

[0041] Due to occlusion, currently only the three-dimensional world coordinates of n target balls are known, and n≥3. Then the centroid-removed coordinates of the i-th point corresponding to the i-th target ball are respectively:

[0042]

[0043]

[0044] where, is the centroid-removed coordinate of the i-th point at a certain moment, is the initial three-dimensional world centroid-removed coordinate of the i-th point, p 0 is the initial three-dimensional world coordinate centroid, is the initial three-dimensional world coordinate of the i-th point, p b is the centroid of the target ball at a certain moment, is the coordinate of the i-th target ball at a certain moment.

[0045] According to the following formula to optimize the problem, calculate the rotation matrix R of the target relative to the initial state * :

[0046]

[0047] Use SVD decomposition to solve for the optimal R, that is, the said is:

[0048]

[0049]

[0050] where, U and V are obtained from the SVD decomposition of W:

[0051] W = UΣV T

[0052] Σ is a diagonal matrix composed of singular values;

[0053] After obtaining the rotation matrix, the said is:

[0054]

[0055] In the feature reconstruction of the occluded part of the target ball, the fixed position relationship between the observable target ball and the whole on the target can be used for feature reconstruction.

[0056] In another preferred embodiment of the present invention, in the feature reconstruction method based on the monocular EPnP algorithm in S2, the two-dimensional pixel coordinates P of the target ball observed in the monocular camera are utilized i b and the fixed target coordinate system to solve the transformation matrix of the fixed target in the camera coordinate system The transformation matrix of the camera coordinate system relative to the world coordinate system is obtained through binocular extrinsic calibration. Then, the three-dimensional world coordinates P' of the target ball reconstructed by the PnP method are: i w as follows:

[0057]

[0058] By using the known pixel coordinates of the unoccluded target ball and the relative position relationship of each target ball in the target ball coordinate system, the transformation matrix of the fixed target in the camera coordinate system is calculated and solved so as to perform the feature reconstruction of the missing target ball.

[0059] In another preferred embodiment of the present invention, the three-dimensional coordinates of the known feature points are represented by the weighted sum of using four virtual control points Let the three-dimensional world coordinates of the known feature points be:

[0060]

[0061] where a ij is the weight. Then, in the camera coordinate system, the three-dimensional coordinates of the known feature points can also be represented by the weighted sum of the virtual control points as follows:

[0062]

[0063]

[0064] According to the projection model of the camera, the i-th pixel coordinate observed of the known feature point and the virtual control point have the following relationship:

[0065]

[0066] can be transformed into two linear equations:

[0067]

[0068]

[0069] The system of equations can be obtained by combining the 2n linear equations of all n feature points:

[0070] Mx=0

[0071] Where M is a 2n×12 matrix, is the coordinate of a virtual control point in the camera coordinate system, which is a 12-dimensional vector and has:

[0072]

[0073] Among them, v i is the right singular vector of matrix M, N is the N zero eigenvalues ​​of matrix M. T The eigenvector of M is obtained by i , and since the spatial position of the virtual control point remains unchanged, the null space error of N is calculated to be 1, 2, 3, 4, and the β corresponding to the smallest dimension is selected i , thereby obtaining x, that is, the camera coordinate system coordinate of the virtual control point, and then calculating Center of gravity And the matrix A:

[0074]

[0075]

[0076] calculate Center of gravity And matrix B:

[0077]

[0078]

[0079] Let H = B T A, calculate the SVD decomposition of H:

[0080] H=UΣV T

[0081] The rotation component R of the pose transformation is calculated from this:

[0082] R=UV T

[0083] Use R to calculate the translation component t in the pose:

[0084]

[0085] In another preferred embodiment of the present invention, the information fusion method between multiple binocular systems in S3 includes:

[0086] Select a binocular subsystem with the smallest covariance matrix in the multi-camera to determine the system state equation;

[0087] Use the adaptive unscented Kalman filter to calculate the state estimation mean x of the M binocular subsystems at time k k and covariance matrix P. Arrange the covariance matrices according to the magnitude of the trace, and define the system with the smallest trace of the covariance matrix as the reference subsystem;

[0088] At the current moment, calculate the adaptive unscented Kalman filter for all subsystems. After calculating the reference subsystem using the predicted covariance matrix, fuse the prediction results of other subsystems with the reference subsystem to achieve loose coupling of the multi-camera.

[0089] In another preferred embodiment of the present invention, the following calculations are performed using the reference subsystem:

[0090] Select 2n + 1 Sigma points:

[0091]

[0092] where represents the state estimation value of the reference subsystem at time k - 1, is its corresponding state error covariance matrix;

[0093] Transform the Sigma points through the state transition matrix:

[0094]

[0095] Obtain the state prediction value of the reference subsystem:

[0096]

[0097] Calculate the state prediction error corresponding to each Sigma point:

[0098]

[0099] Weight according to the weights corresponding to the Sigma points to calculate the prediction error covariance of the reference subsystem:

[0100]

[0101] Weight the state estimations and covariances of the remaining binocular subsystems, and calculate the covariance of the multi-camera loose coupling system according to the matrix weighted fusion result:

[0102]

[0103] Among them, i represents the serial number of the local subsystem, M is the total number of local camera subsystems in the multi-vision sensor pose estimation system, and P i is the state error covariance of the i-th subsystem, and P s is the total state prediction error covariance of the loosely coupled system;

[0104] Calculate the mean value of the state estimation of the system after loose coupling:

[0105]

[0106] The present invention also proposes a passive optical motion capture system, which adopts the above-mentioned passive optical motion capture method.

[0107] Advantages of the present invention:

[0108] 1. Aiming at the problem that outliers on the two-dimensional image are not thoroughly removed during target extraction in a complex background, the present invention proposes a motion capture method using prior volume constraints in three-dimensional space. By using binocular cameras and retroreflective targets, high-precision six-degree-of-freedom pose measurement in a complex background is achieved.

[0109] 2. Aiming at the problem of insufficient anti-occlusion accuracy of the existing unscented Kalman filter method, the present invention proposes a motion capture method based on adaptive unscented Kalman filter and target feature reconstruction. This method can perform information fusion based on loose coupling for a multi-camera system, and realizes complete and continuous high-precision six-degree-of-freedom pose measurement under occlusion conditions.

[0110] 3. The present invention establishes for the first time a dataset for challenging factors such as few / weak textures, motion blur, light changes, and complex dynamics. This dataset provides high-precision pose ground truth at the millimeter level based on motion capture.

[0111] The following will further illustrate the concept, specific structure and technical effects of the present invention with reference to the accompanying drawings, so as to fully understand the purpose, features and effects of the present invention. Brief Description of the Drawings

[0112] Figure 1 It is a schematic diagram of the passive optical motion capture method of a preferred embodiment of the present invention. Detailed Embodiments

[0113] The following introduces multiple preferred embodiments of the present invention with reference to the accompanying drawings of the specification, so that its technical content is clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the protection scope of the present invention is not limited to the embodiments mentioned in the text.

[0114] In the accompanying drawings, components with the same structure are denoted by the same numerical labels, and components with similar structures or functions everywhere are denoted by similar numerical labels. The dimensions and thicknesses of each component shown in the drawings are arbitrarily shown, and the present invention does not limit the dimensions and thicknesses of each component. To make the illustration clearer, the thicknesses of some components are appropriately exaggerated in the drawings.

[0115] As Figure 1 shown, an embodiment of the present invention provides a passive optical motion capture method applicable to complex conditions, including the following steps:

[0116] S1. Use an adaptive unscented Kalman filter that dynamically adjusts the observation noise;

[0117] S2. Based on the feature reconstruction method of a fixed target and the feature reconstruction method based on the monocular EPnP algorithm, perform feature reconstruction as the observed quantity for the state estimation of the target to be measured;

[0118] S3. Based on a loose coupling scheme, use the unscented Kalman filter form with matrix weighting for information fusion between multiple groups of binocular systems.

[0119] When performing motion capture on the six-degree-of-freedom motion of a mobile robot, in order to provide the pose truth value of the entire motion process, the continuity, integrity, and accuracy of the measurement results are equally important. However, due to the relatively complex trajectory of the six-degree-of-freedom motion and the presence of certain scene components in the actual complex conditions used, when the measured robot performs a specific position or specific angle rotation motion, the target balls pasted on it may be blocked by the foreground formed by the measured robot itself or other objects in the scene, resulting in missing marker points, which in turn affects the accuracy of pose estimation and even estimation failure. When the mobile robot performs six-degree-of-freedom motion, it is extremely easy to cause a decrease in the accuracy of pose estimation, abnormal changes in the trajectory measurement results, and even trajectory breakage and missing.

[0120] First, the tracking problem under severe occlusion is analyzed below. Common filtering methods for solving the state estimation problem under occlusion conditions are introduced respectively, and the characteristics and deficiencies of related algorithms are analyzed. Then, the passive binocular optical motion capture method based on state estimation and feature reconstruction proposed in this embodiment is introduced. For severe occlusion situations, the binocular system has limitations, and this embodiment extends the corresponding method to a multiocular system, and then the effect of this method is verified through experiments.

[0121] During the process of performing motion capture on a mobile robot in a complex environment, tables, chairs, decorations, and other stationary or moving objects will appear in the environment where the robot is located, forming occlusion conditions. The possible occlusion situations during its motion capture process are classified.

[0122] According to the relationship between the target to be measured and the occluder, it can be divided into three categories: the target to be measured is occluded by static objects in the environment, that is, static occlusion; the target to be measured is occluded by other moving objects in the environment, that is, dynamic occlusion or mutual occlusion; the target to be measured is occluded by the six-degree-of-freedom motion of itself, that is, self-occlusion.

[0123] According to the degree of occlusion of the target to be measured, it can be divided into two categories: partial occlusion and severe occlusion.

[0124] According to the length of time the target to be measured is occluded during the positioning process, it can be divided into: short-term occlusion and long-term occlusion.

[0125] In the working scenario of the motion capture system in this embodiment, there will be problems of static occlusion caused by statically placed scene components, mutual occlusion caused by other moving objects such as people, and self-occlusion caused by the target ball and the robot itself.

[0126] In addition, according to the number of occluded target balls, it is divided into partial occlusion and severe occlusion; according to the occlusion time, it is divided into short-term occlusion and long-term occlusion.

[0127] The occlusion condition causes the missing of target ball extraction, that is, the missing markers problem, which makes the inter-frame target ball index disordered and the number of extracted marker points missing. Due to the incomplete state information of the target to be measured, the accuracy of the pose estimation of the mobile robot decreases, the integrity of the overall trajectory decreases, and even the target is lost, which will have a great impact on the tracking effect of the system. To sum up, how to perform six-degree-of-freedom motion capture on a mobile robot performing complex motions under occlusion becomes a difficult point affecting the trajectory integrity and pose estimation accuracy of the system.

[0128] In this embodiment, a fixed retroreflective target is used as a marker point to perform six-degree-of-freedom high-precision pose estimation on a single mobile robot. Therefore, the appearance feature of the target ball as a feature point is clear, and the motion capture problem pays more attention to the accuracy of pose estimation and the integrity of the trajectory, rather than indicators such as tracking stability and operation speed. To sum up, for the occlusion situation, this embodiment uses the adaptive unscented Kalman filter method in the state estimation information. First, according to the binocular subsystem, a feature reconstruction and adaptive unscented Kalman filter state estimation algorithm is proposed; then, the missing situation of the marker points of the binocular subsystem under occlusion is evaluated. For the severe occlusion situation that cannot be solved by the binocular subsystem, a multi-camera is used for information fusion based on loose coupling, effectively solving the problem of motion capture of the mobile robot under occlusion.

[0129] Furthermore, the essence of the occlusion problem in the motion capture system is that the number of marker points used as features is insufficient, resulting in an increase in system noise and a decrease in pose estimation accuracy. This optimal estimation using imperfect sensors is a state estimation problem. According to Bayes' rule, the measurement data at the current moment and the prior model of the system can be used to reconstruct the state of the system. Since the six-degree-of-freedom motion has the characteristics of Gaussian distribution and Markovian assumption, in this embodiment, a filtering-based method is used to estimate the state of complex moving targets. The particle filter (PF) represents the posterior probability density of the target state with discrete particles of different weights to solve the target tracking problem. However, due to problems such as particle degeneracy and large computational complexity, when the target to be measured performs relatively fast and complex motions, the tracking effect of the PF algorithm is poor. Therefore, for the six-degree-of-freedom motion capture of mobile robots, in this embodiment, Kalman filter-related methods are used to solve the state estimation problem.

[0130] Kalman Filtering:

[0131] For linear Gaussian systems, the Kalman Filtering (KF) is the most widely used filter model. For the following linear discrete system, the state estimation equations are listed, and the state equation and the observation equation are respectively:

[0132]

[0133] where k represents each discrete moment, x k is the state quantity at moment k, z k is the system observation quantity at moment k, A k is the state transition matrix from k - 1 to k, H k is the sensor observation matrix at moment k, w k and v k are the process noise and the observation noise respectively, and both satisfy the following zero-mean Gaussian distribution:

[0134] w k ~N(0, R k ), v k ~N(0, Q k )(2)

[0135] where Q is the process noise covariance and R is the observation noise covariance.

[0136] For the state estimation problem at moment k, two key pieces of information need to be focused on, namely the best estimate at moment k, which is also the mean of the Gaussian distribution and the covariance P k . Therefore, the classic five-step equations of the Kalman filter are expressed as follows:

[0137] Based on the Markov assumption, use the state estimate at time k-1 to make a one-step prediction and obtain

[0138]

[0139] Subsequently, calculate the one-step prediction covariance P k|k-1 :

[0140] P k|k-1 =A k ·P k-1|k-1 +Q k (4)

[0141] Calculate the Kalman gain K k , K k changes continuously at different times. If K k is smaller, the estimated value is more reliable. Conversely, the observed value is more reliable. K k is calculated as shown in the following equation:

[0142]

[0143] Define the difference between the actual sensor observation value at time k and the calculated predicted observation value as the innovation. We can obtain:

[0144]

[0145] Use the Kalman gain and the innovation to update the state value obtained from the one-step prediction. The optimal estimate at time k after the update is:

[0146]

[0147] The covariance at time k after the update is:

[0148] P k|k =P k|k-1 -K k ·H k ·P k|k-1 (8)

[0149] As a classic state estimation method, Kalman filtering can achieve unbiased optimal estimation for linear Gaussian systems, but it is difficult to converge for nonlinear systems. However, since in this embodiment, the pose estimation of the robot to be measured in complex six-degree-of-freedom motion is required, the measured values of the system are the three-dimensional coordinates of each marked point, and the state variables are the position, angle, velocity, acceleration, etc. of the target to be measured. Therefore, the state equation or the observation equation is a nonlinear function, and obviously it cannot form a linear system. Therefore, it is necessary to improve for the nonlinear problem.

[0150] Extended Kalman filtering:

[0151] The Extended Kalman Filter (EKF) is widely used in the state estimation problem of nonlinear systems. Its basic idea is to perform a first-order Taylor expansion of the nonlinear functions in the state equation and the observation equation near the reference point, and only retain the first-order terms to convert the nonlinear functions into linear functions. Subsequently, the Kalman filter is used for derivation according to the linear system.

[0152] The state estimation equation of a nonlinear system can be expressed as:

[0153]

[0154] Among them, the state transition function f(x) and the measurement function h(x) are nonlinear. Therefore, for the Extended Kalman Filter, at time k, the state equation and the observation equation are linearized at and P k-1 . For the state equation, the first-order partial derivative with respect to the one-step predicted value of the state is obtained, and the state transition matrix can be written as:

[0155]

[0156] For the observation equation, the first-order partial derivative with respect to the one-step predicted value of the state is obtained, and the observation matrix can be written as:

[0157]

[0158] Referring to the five-step equations of the Kalman filter, the prediction and update processes are expressed as follows:

[0159] Consider the nonlinear function f(x) to calculate the one-step predicted value:

[0160]

[0161] Use the state transition matrix obtained above to calculate the one-step predicted covariance:

[0162]

[0163] The form of the Kalman gain is the same as that in the Kalman filter:

[0164]

[0165] Use the observation matrix obtained after linearization to calculate the innovation:

[0166]

[0167] Therefore, the updated optimal estimate and covariance can be obtained:

[0168]

[0169] Pk|k = P k|k-1 - K k H k ·P k|k-1 (17)

[0170] Although the extended Kalman filter can better solve the state estimation problem of nonlinear systems, there are still the following limitations:

[0171] (1) Only one Taylor expansion is performed at . When far from the operating point, the first-order Taylor expansion may not be able to approximate the entire function, resulting in a certain nonlinear error.

[0172] (2) It is necessary to calculate the Jacobian matrix for the nonlinear state transition function and observation function. When the dimensions of the state variables and observation variables are large, the calculation is cumbersome and the model is complex.

[0173] Unscented Kalman Filter:

[0174] Since the six-degree-of-freedom motion of the mobile robot is a nonlinear system, and in order to avoid the influence of large nonlinear errors and the complex calculation of the Jacobian matrix, this embodiment uses the Unscented Kalman Filter (UKF) for state estimation.

[0175] Different from the EKF, the main idea of the UKF is to approximate the probability density distribution of the nonlinear function. Instead of linearizing the nonlinear function, it directly obtains the mean and covariance of the model to be predicted through the Unscented Transform (UT). Since approximating the probability distribution of the nonlinear function is easier than approximating the nonlinear function itself, without the need to solve the Jacobian matrix and without ignoring the higher-order terms, reducing the nonlinear error, the UKF often has higher accuracy and faster operation speed than the EKF.

[0176] In the UKF, its core idea - the UT transform is as follows. For a nonlinear system:

[0177] y = f(x) (18)

[0178] Given the mean of the n-dimensional state vector x x and the covariance P i , use the UT transform to construct 2n + 1 Sigma points x i , and construct the corresponding weights W i for each x to calculate the mean y of y and the covariance P

[0179]

[0180] where λ is the scaling factor, and the calculation formula is as follows:

[0181] λ = α 2 (n + κ) - n (20)

[0182] α determines the distribution state of the surrounding Sigma points. Adjusting α can reduce the influence of high-order terms, and it is generally a small positive number. In this embodiment, it is set to 0.001. κ determines the distance between the Sigma sampling points and the mean value, and according to the empirical formula, it can be set as κ = n.

[0183] The mean weight W of each Sigma point i m and the covariance weight W i c can be constructed as:

[0184]

[0185] where β is also the state distribution parameter, and in this embodiment, β = 2 is taken.

[0186] Substitute x i directly into the non-linear function to obtain the distribution of y i :

[0187] y i = f(x i ) (22)

[0188] Since the state estimation problem focuses on the mean and covariance of the state variables, the mean and covariance of y are obtained using the following formula:

[0189]

[0190] For UKF, the known state vector x is the state variable of the target to be measured at time k, and the y obtained by the UT transformation is the predicted observation value. Therefore, the state estimation of the non-linear system can be performed using the classical five-step equations of Kalman filter and the UT transformation.

[0191] Furthermore, a motion capture method based on state estimation and feature reconstruction.

[0192] Since the mobile robot is a rigid body moving with six degrees of freedom in three-dimensional space, the occlusion situation of the target ball during its movement is relatively complex. Therefore, in this embodiment, based on the classical unscented Kalman filter, the Sage-Husa method is used to adaptively update the observation noise, perform state estimation on it, and perform feature reconstruction according to the fixed retroreflective target, using all effective features. For the limitations of the binocular system, it is extended to a multi-camera system for information fusion based on loose coupling to solve the problem of motion capture of mobile robots under occlusion conditions.

[0193] Specifically, six-degree-of-freedom motion state estimation based on adaptive unscented Kalman filter.

[0194] Considering that for the pose estimation system, using the second-order acceleration model has better estimation results. Therefore, for the six-degree-of-freedom motion of the mobile robot to be measured, the state estimation equation of this embodiment is modeled as:

[0195]

[0196] where, x k is a six-dimensional state vector covering the position x of the object in the world coordinate system p =[x y z]; the angle x of the object in the world coordinate system a =[α β γ], represented by Euler angles; and the velocity of the object angular velocity acceleration of the object and angular acceleration A k is the state transition matrix, and there is:

[0197]

[0198] At the same time, there is:

[0199]

[0200] Since this motion capture system is for periodic sampling and shooting, Δt is a fixed value, that is, the sampling time of picture shooting.

[0201] z k is the observed value. Considering that in the complex background above, there are a large number of outliers in the pixel coordinates of the marker points to be removed, and after a series of processes such as contour extraction, RANSAC outlier removal, etc. in the binocular motion capture system, the three-dimensional world coordinates of each marker point have been obtained. Therefore, for the scalability of the system, the true observed value of the sensor in this embodiment is defined as the three-dimensional coordinates of each target ball in the fixed target. Therefore, for the target formed by seven target balls, there is z k =[z k1 z k2 zk3 …z k7 T, where z ki is in homogeneous coordinate form, and z k = [x i y i z i 1].

[0202] h(x k ) is a non - linear measurement function used to transform the state variables into the state space corresponding to the observed variables. For h(x k ), we have:

[0203]

[0204] where, P i b = [x ib y ib z ib 1] is the three - dimensional homogeneous coordinate of the i - th target ball in the base coordinate system. is the transformation matrix of the base coordinate system relative to the world coordinate system, which can be calculated from the position and angle of the object in the world coordinate system in the state vector:

[0205]

[0206] where is the rotation matrix calculated using the angle of the object in the world coordinate system in the state variables, is the translation vector calculated using the angle of the object in the world coordinate system in the state variables

[0207] In the specific calculation, this embodiment defines the state space of the system as the world coordinate system, and the representation method is defined as the Tait–Bryan angles commonly used in the SLAM problem. The rotation order is z - y - x, and the rotation method is external rotation. The state variable X(α) representing the angle is the Euler angle of the reference frame. As a static Euler angle, there is no gimbal lock problem. Therefore, the rotation matrix can be expressed as:

[0208]

[0209] where the rotation components of each axis are:

[0210]

[0211] However, in the six-degree-of-freedom complex motion, the system noise should be unknown and time-varying. If it is directly replaced by a constant, the estimation accuracy is relatively low. The proposed Sage-Husa adaptive Kalman filter realizes the adaptive state estimation in a complex noise environment by estimating the process noise Q and the observation noise R in real time. However, in some cases, the Sage-Husa Kalman filter cannot estimate Q and R simultaneously, and in the high-frame-rate sampling and shooting, the six-dimensional state vector fits the six-degree-of-freedom motion of the robot to be measured more accurately. On the contrary, due to the complex occlusion conditions, the observation noise R is constantly changing for different occlusion situations, and it is necessary to estimate and adjust R to improve the filtering performance when the system model is uncertain. Therefore, in this embodiment, the process noise is fixed, and the observation noise R is adaptively adjusted with reference to the Sage-Husa method. The algorithm flow of the adaptive unscented Kalman filter is designed as follows:

[0212] The system is initialized and Sigma points are generated, and the initial value x of the system state vector is set 0 , the initial covariance P 0 , the process noise Q, and the initial value R of the observation noise 0 For the state value at time k-1 and the covariance P k-1|k-1 , 2n + 1 Sigma points are obtained, where n is the dimension of the state vector.

[0213] From x k , n = 18, and there are:

[0214]

[0215] Among them, x k-1|k-1,i is the i-th Sigma point,

[0216] Then the weights corresponding to each Sigma point are:

[0217]

[0218] Among them, W i m and W i c are the mean weight and covariance weight corresponding to the i-th Sigma point. λ is obtained from Equation (20), α is taken as 0.001, and β is taken as 2.

[0219] (1) Calculate the state mean and covariance of the one-step prediction

[0220] Predict the Sigma points at time k through the state transition matrix:

[0221] x k|k-1,i = A k ·x k-1|k-1,i(33)

[0222] The one-step predicted value of the system state at time k is obtained from the predicted values and weights of i Sigma points

[0223]

[0224] Subsequently, calculate the one-step prediction covariance:

[0225]

[0226] Then the predicted Sigma point samples at this time are:

[0227]

[0228] (2) Calculate the mean and variance of the predicted observations

[0229] The predicted i-th Sigma point is transformed through the observation function to:

[0230] z k|k-1,i =h(x k|k-1,i ) (37)

[0231] The predicted observation mean of the system at time k is obtained from the predicted values and weights of i Sigma points

[0232]

[0233] Meanwhile, the predicted observation error S k|k-1 is obtained:

[0234]

[0235] (3) Update to obtain the state mean and covariance at time k

[0236] Calculate the Kalman gain K k as:

[0237]

[0238] After obtaining the one-step predicted state mean x and covariance P, and the predicted observation mean x and covariance P through (2) and (3), with the observation value z k at time k and the Kalman gain K k , update to obtain the mean and covariance P k|k at time k:

[0239]

[0240] (4) Update the observation noise R k

[0241] Update R by referring to the Sage-Husa method k :

[0242]

[0243] where d k-1 is:

[0244]

[0245] The forgetting factor b is usually 0.9 - 1. Since the observation noise of this system often changes, b = 0.945 is taken in this paper. When updating the observation noise, the process noise is not changed, and the filtering process continues to be executed from (2) at time k+1.

[0246] Specifically, the method for reconstructing the target features of a binocular camera:

[0247] As the minimum system of the motion capture system, the binocular camera may not be able to completely observe the fixed retroreflective target used in this system under complex occlusion conditions, and there may be a situation where only some target balls are observed by two or one of the cameras.

[0248] At this time, the three-dimensional coordinates of all the marked points cannot be directly obtained through epipolar matching and binocular depth estimation. If the target with missing part of the information is directly used for state estimation, the matrix dimensions of the observation equation do not match and a large number of lost effective features will reduce the accuracy of the system. To solve this missing markers problem, the industry uses a matching model and probabilistic inference of point topology to find the correspondence between two-dimensional points and three-dimensional dense surface points to be measured. Considering that the mobile robot to be measured undergoes a rigid pose transformation in this system and the retroreflective targets providing observation information as marked points are distributed in a fixed hemispherical surface, the following two methods for reconstructing the target features based on binocular cameras are proposed in this embodiment:

[0249] (1) Feature reconstruction method based on a fixed target

[0250] For the feature point P to be reconstructed i ω , there is:

[0251]

[0252] where is the transformation matrix of the fixed target in the world coordinate system:

[0253]

[0254] If the transformation matrix is obtained from the retroreflective target balls not affected by occlusion, the remaining missing points can be feature-reconstructed based on the fixed target coordinate system, thereby complementing the feature information and using it as an observable for updating the state estimation problem.

[0255] Transformation of the target coordinate system relative to the world coordinate system The calculation is as follows:

[0256] Through binocular depth estimation, the three-dimensional world coordinates P of at least three target balls are obtained b :

[0257]

[0258] Define its centroid as:

[0259]

[0260] For the initial three-dimensional world coordinates P of each target ball 0 and the centroid p b , there is:

[0261]

[0262]

[0263] Due to occlusion, currently only the three-dimensional world coordinates of n points are known, and n ≥ 3. Then the de-centered coordinates of the i-th point are respectively:

[0264]

[0265] According to the following optimization problem, calculate the rotation matrix R of the target as a whole relative to the initial state * :

[0266]

[0267] Use SVD decomposition to solve for the optimal R, that is, obtain the rotation matrix of the current target coordinate system relative to the world coordinate system

[0268]

[0269] Among them, U and V are obtained from the SVD decomposition of W:

[0270] W = UΣV T (53)

[0271] Σ is a diagonal matrix composed of singular values.

[0272] After obtaining the rotation matrix, the translation vector of the current target coordinate system relative to the world coordinate system can be obtained

[0273]

[0274] In the feature reconstruction of the occluded part of the target ball, the fixed position relationship between the observable target balls and the whole on the target can be used for feature reconstruction.

[0275] (2) Feature reconstruction method based on monocular EPnP algorithm

[0276] Under severe occlusion conditions, if only one camera in the binocular system can observe some target balls and the three-dimensional coordinates of at least three points in the world coordinate system cannot be obtained through binocular depth estimation, the fixed target cannot be directly used for feature reconstruction. At this time, the PnP (Perspective-n-Point) method needs to be used, and the two-dimensional pixel coordinates P of the target balls observed in the monocular camera i b and the fixed target coordinate system are used to solve the transformation matrix of the fixed target in the camera coordinate system The transformation matrix of the camera coordinate system relative to the world coordinate system is obtained through binocular extrinsic calibration. Then, the three-dimensional world coordinates P of the target balls reconstructed by the PnP method i w ' are:

[0277]

[0278] For the solution of the transformation matrix in the PnP method, the algorithm with the fewest feature points is the P3P algorithm. However, due to the complex six-degree-of-freedom motion, at least one more point needs to be added for verification. Similarly, when using at least four points, the EPnP has better accuracy. Therefore, in this embodiment, under the principle of using as few feature points as possible and with as high accuracy as possible, the EPnP algorithm is used for the monocular camera. Through the known pixel coordinates of the unoccluded target balls and the relative position relationship of each target ball in the target ball coordinate system, the transformation matrix of the fixed target in the camera coordinate system is calculated and solved so as to perform feature reconstruction of the missing target balls.

[0279] As a non-iterative PnP algorithm, EPnP represents the three-dimensional coordinates of the known feature points using the weighted sum of four virtual control points . Let the three-dimensional world coordinates of the known feature points be:

[0280]

[0281] where a ij is the weight. Then, in the camera coordinate system, the three-dimensional coordinates of the known feature points can also be represented by the weighted sum of the virtual control points :

[0282]

[0283] According to the camera projection model, the i-th pixel coordinates observed by the known feature points and the virtual control point The following relationship exists:

[0284]

[0285] It can be transformed into two linear equations:

[0286]

[0287]

[0288] The system of equations can be obtained by combining the 2n linear equations of all n feature points:

[0289] Mx=0 (61)

[0290] Where M is a 2n×12 matrix, is the coordinate of a virtual control point in the camera coordinate system, which is a 12-dimensional vector and has:

[0291]

[0292] Among them, v i is the right singular vector of matrix M, and N is the N zero eigenvalues ​​of matrix M. Calculate M T The eigenvector of M is obtained by i , and since the spatial position of the virtual control point remains unchanged, the null space error of N is calculated to be 1, 2, 3, 4, and the β corresponding to the smallest dimension is selected i , thus obtaining x, which is the camera coordinate system coordinate of the virtual control point. Then calculate Center of gravity And the matrix A:

[0293]

[0294] calculate Center of gravity And matrix B:

[0295]

[0296] Let H = B T A, calculate the SVD decomposition of H:

[0297] H=UΣV T (65)

[0298] The rotation component R of the pose transformation is calculated from this:

[0299] R=UV T (66)

[0300] Use R to calculate the translation component t in the pose:

[0301]

[0302] Therefore, this embodiment uses the EPnP algorithm to obtain the transformation matrix of the target coordinate system using the pixel coordinates of the marker points observed in the monocular camera Thus, the world coordinates of each missing feature after reconstruction are obtained.

[0303] Specifically, based on loosely coupled multi-camera information fusion:

[0304] As the smallest unit of the optical motion capture system, the binocular camera can realize the six-degree-of-freedom pose estimation of the mobile robot, but it has certain limitations under occlusion and poor robustness. In the case of severe occlusion, the binocular camera cannot provide observation values ​​and can only rely on the state model for prediction, with large errors; in the case of long-term occlusion, the binocular camera, as a single input, has certain cumulative errors. In addition, the field of view of the binocular camera system is fixed and has limitations, resulting in a limited working range. Therefore, on the basis of the binocular optical motion capture system, this embodiment increases the number of cameras to form a multi-eye system. The multi-eye optical motion capture system consists of multiple groups of binocular cameras. After fusing the information of the multiple cameras, it provides higher accuracy and a larger measurement range under occlusion conditions, thereby obtaining accurate and complete pose true values ​​throughout the movement process.

[0305] It is extended to a multi-purpose optical motion capture system, and its essence is the information fusion of multiple sensors of the same source. In the field of positioning and navigation, the methods for information fusion of multiple sensors include tight coupling scheme, loose coupling scheme and split combination scheme, among which the split combination fusion method is less used and has poor effect. The tight coupling scheme takes all the states of the targets to be measured by multiple sensors as the state variables of the system, and obtains the global optimal pose estimation after expanding the dimension of the observation information, which can make full use of the sensor data to obtain more accurate pose estimation results. However, this scheme is complex to implement and difficult to adapt to complex environments. The loose coupling scheme refers to modularizing multiple sensors, each module is independent of each other, first performing pose estimation separately, and then fusing their respective results through other methods to improve the accuracy of pose estimation. Estimating the targets to be measured in each module as multiple independent state quantities can reduce the dimension of the system state quantity and reduce the complexity of the system. Moreover, when a sensor fails to work, the pose estimation result can still be obtained, and it is easy to expand more sensors for information fusion.

[0306] Due to the high robustness and strong scalability of the loose coupling scheme, in the motion capture of mobile robots under occlusion conditions, in this embodiment, based on the loose coupling scheme, matrix-weighted unscented Kalman filtering is used for information fusion between multiple binocular systems to improve the accuracy and measurement range under occlusion conditions.

[0307] First, select a set of binocular subsystems from the multi-camera to determine the system state equation. In Kalman filtering, the covariance matrix represents the uncertainty of different error variables. Therefore, the binocular subsystem with the smallest covariance matrix has the smallest uncertainty in the state quantity, and it should be selected to determine the state equation. Use the adaptive unscented Kalman filter to calculate the state estimation mean x of the M binocular subsystems at time k k Arrange the covariance matrices according to the size of the trace. Then, the system with the smallest trace of the covariance matrix is defined as the reference subsystem, and the reference subsystem is used for the following calculations:

[0308] First, select 2n + 1 Sigma points:

[0309]

[0310] where represents the state estimation value of the reference subsystem at time k - 1, is its corresponding state error covariance matrix.

[0311] Then transform the Sigma points through the state transition matrix:

[0312]

[0313] Further obtain the state prediction value of the reference subsystem:

[0314]

[0315] Then calculate the state prediction error corresponding to each Sigma point:

[0316]

[0317] Weight according to the weights corresponding to the Sigma points, and calculate the prediction error covariance of the reference subsystem:

[0318]

[0319] Weight the state estimations and covariances of the remaining binocular subsystems, and calculate the covariance of the multi-camera loose coupling system according to the matrix-weighted fusion result:

[0320]

[0321] Where i represents the serial number of the local subsystem, M is the total number of local camera subsystems in the multi-vision sensor pose estimation system, and P i is the state error covariance of the ith subsystem, P s is the total state prediction error covariance of the loosely coupled system. Calculate the state estimation mean of the loosely coupled system:

[0322]

[0323] At the current moment, all subsystems calculate the adaptive unscented Kalman filter, and after using the predicted covariance matrix to calculate the benchmark subsystem, the prediction results of other subsystems are fused with the benchmark subsystem to achieve loose coupling of multi-cameras and improve system accuracy and robustness.

[0324] Furthermore, this embodiment provides an experimental test

[0325] As an important parameter of feature reconstruction results, the number of obscured target balls seriously affects the effect of the algorithm. Therefore, in order to determine the degree of occlusion, this embodiment makes the target to be measured move with six degrees of freedom, obscures different numbers of target balls, and uses fixed target and monocular EPnP algorithms to reconstruct features respectively. The absolute position error results of the number of unobstructed target balls under the two feature reconstruction methods are calculated as shown in the following table:

[0326] The number of unobstructed target balls and the absolute position error after feature reconstruction

[0327]

[0328]

[0329] The results show that:

[0330] (1) The error of feature reconstruction is negatively correlated with the number of unoccluded target balls and decreases as their number increases.

[0331] (2) With the same number of unobstructed target balls, the feature reconstruction accuracy based on fixed targets is higher than that based on the monocular EPnP algorithm. Therefore, for feature reconstruction, the method based on fixed targets should be used first. When binocular depth estimation is not possible, the monocular EPnP algorithm should be considered for feature reconstruction.

[0332] (3) For a fixed target consisting of 7 spheres, when the number of unobstructed spheres is less than 5, the errors of both feature reconstruction methods increase significantly. If the number of unobstructed spheres is less than 3, no observation value can be provided.

[0333] Therefore, the present embodiment defines the unobstructed target ratio threshold λ for distinguishing partial obstruction from severe obstruction:

[0334]

[0335] Among them, n a is the number of unoccluded target balls, and n b is the total number of target balls on the target.

[0336] From the above and related conclusions, for the threshold λ, there is:

[0337]

[0338] Define the unoccluded target ratio as n. If n is 100%, there is no occlusion; if the unoccluded target ratio is λ ≤ n < 100%, the system has partial occlusion; if the unoccluded target ratio n is 0 ≤ n < λ, the system has severe occlusion. For a target composed of 7 target balls with the rightmost two target balls occluded, the unoccluded target ratio at this time is 71.4%. Since 71.4% > 70%, the system is in a state of partial occlusion.

[0339] To further verify the algorithm effect of this embodiment, an experimental system is designed. Taking the multi-view motion capture system extended in this embodiment as an example of a four-camera system, the four-view loose-coupling algorithm is built and verified. One group of binoculars can be used to form a binocular subsystem for verifying the binocular algorithm. The above device is built within an Optitrack motion capture system. There are 20 Prime 41 cameras with tens of millions of pixels within the effective measurement range, which can achieve six-degree-of-freedom pose measurement with a positioning accuracy of 0.1 mm. Taking the output result of the Optitrack motion capture system as the true value of pose estimation, let the mobile platform carrying the fixed target perform three moving modes: reciprocating motion, triangular motion, and circular arc motion within a small range to verify the binocular motion capture algorithm under occlusion conditions; and drive freely within the effective measurement range to verify the multi-view motion capture algorithm under occlusion conditions.

[0340] Furthermore, test results of the binocular motion capture method under partial occlusion conditions

[0341] To verify the state estimation algorithm based on adaptive unscented Kalman filter proposed in this embodiment, let the mobile platform perform reciprocating motion within a small range and dynamically change the occlusion conditions, and compare the classic unscented Kalman filter algorithm with the adaptive Kalman filter algorithm of this embodiment.

[0342] This embodiment sets different occlusion conditions at different times to verify the dynamic update effect of the adaptive Kalman filter on the observation noise, as shown in the following table:

[0343]

[0344] As can be seen from the results, for the adaptive unscented Kalman filtering method for updating the observation error proposed in this embodiment, in the case of continuously changing occlusion during the reciprocating motion of the target to be measured, the obtained pose estimation results are closer to the true values and have better measurement accuracy. Moreover, the fewer the number of occluded target balls and the fewer the number of occluded cameras, the higher the system accuracy.

[0345] As shown in the following table, for the absolute position error APE and absolute angle error RPE indicators, in the case of continuously changing occlusion, the method proposed in this embodiment has smaller errors and higher accuracy. This is because the continuously changing occlusion situation causes the observation noise to change. If it is set to a fixed value, the observation results cannot be accurately reflected. Thus, it can be proved that compared with the classical unscented Kalman filtering algorithm, the adaptive unscented Kalman filtering algorithm proposed in this embodiment for updating the observation noise has higher accuracy.

[0346] Output results of two state estimation methods

[0347]

[0348] As shown in the following table, in triangular motion and circular arc motion, the absolute position error APE and absolute angle error RPE are used to quantitatively compare the accuracies of different feature reconstruction methods. It can be seen that in both motion modes, since binocular depth estimation provides an absolute scale and the fixed target provides prior information to determine the position distribution relationship of the target balls, the feature reconstruction method based on the fixed target is closest to the trajectory true value and has the highest accuracy. The pose estimation accuracy of the monocular camera for feature reconstruction based on the EPnP algorithm is slightly worse, and due to the error of the external parameter calibration, the result of using the right monocular camera for feature reconstruction is slightly worse than that of the left monocular camera. Since directly using known feature points for pose estimation seriously lacks the state information of the target to be measured, only centimeter-level accuracy can be achieved.

[0349] Comparison of absolute pose errors in triangular movement under different pose estimation methods

[0350]

[0351] Therefore, for different motion modes, the feature reconstruction method based on the fixed target should be preferred. If both binoculars are occluded and known feature points cannot be directly obtained through depth estimation, then the monocular EPnP algorithm can be used for feature reconstruction. And no matter which feature reconstruction algorithm is used, its accuracy is much better than the algorithm that directly uses known points for state estimation, which proves the effectiveness of the algorithm in this embodiment.

[0352] Furthermore, test results of the multi-camera motion capture method under severe occlusion conditions:

[0353] Under occlusion conditions, although the binocular camera can solve the partial occlusion problem of the six-degree-of-freedom motion of the mobile robot to be tested, it is difficult to maintain high accuracy in the case of continuous long-term occlusion or severe occlusion due to the principle of the binocular camera. Therefore, this embodiment takes a four-eye camera as an example to verify the solution to the severe occlusion problem by loosely coupled multi-eye camera information fusion.

[0354] In this embodiment, the binocular subsystem 1 and the binocular subsystem 2 formed by the four-eye camera are severely occluded for 10 seconds respectively. The target to be measured is allowed to move freely. During this period, the binocular subsystem 1 is occluded and the number of unoccluded target balls is set to 4, which lasts for 10 seconds. During this period, the binocular subsystem 2 is not occluded; then the subsystem 2 is occluded and the number of unoccluded target balls is set to 4, which also lasts for 10 seconds. During this period, the occlusion of the binocular subsystem 1 is released. The results show that for severe occlusion, the calculation accuracy is extremely poor when the unoccluded target balls are directly used, and the binocular subsystem 1 cannot obtain a high-precision trajectory.

[0355] The absolute position errors of different system working modes are shown in the following table. Severe occlusion will greatly affect the accuracy of the binocular subsystem, but after information fusion, the measurement accuracy of the multi-eye motion capture system has been greatly improved, which proves the advancement and effectiveness of the method proposed in this embodiment.

[0356] The absolute position errors of different system working modes are shown in the following table. Severe occlusion will greatly affect the accuracy of the binocular subsystem, but after information fusion, the measurement accuracy of the multi-eye motion capture system has been greatly improved, which proves the advancement and effectiveness of the method proposed in this embodiment.

[0357] Comparison of absolute posture errors of different system working modes

[0358]

[0359] The multi-camera information fusion method based on loose coupling proposed in this embodiment can measure the absolute position error of each point on the trajectory under severe occlusion conditions. It can be seen that the multi-camera fusion algorithm based on loose coupling proposed in this embodiment has better accuracy than the binocular subsystem under severe occlusion conditions, and can achieve millimeter-level precision measurement. Therefore, under occlusion conditions, the method of this embodiment uses as few cameras as possible to achieve millimeter-level accuracy, meet the available standards, and meet the requirements of most scenarios.

[0360] The preferred specific embodiments of the present invention have been described in detail above. It should be understood that those of ordinary skill in the art can make many modifications and variations based on the concept of the present invention without creative work. Therefore, all technical solutions that can be obtained by those skilled in the art in this technical field based on the concept of the present invention through logical analysis, reasoning or limited experiments on the basis of the prior art shall fall within the protection scope determined by the claims.

Claims

1. A passive optical motion capture method, It is characterized in that The steps include: S1, adaptive unscented Kalman algorithm filtering using dynamically adjusted observation noise; S2, a feature reconstruction method based on a fixed target and a feature reconstruction method based on a monocular EPnP algorithm, which performs feature reconstruction as an observation quantity to estimate the state of the target to be measured; S3, based on the loose coupling scheme, uses the matrix-weighted unscented Kalman filter form to perform information fusion between multiple binocular systems; The feature reconstruction method based on the monocular EPnP algorithm in S2 includes: For the feature point P to be reconstructed i ω , there is: Among them, is the two-dimensional pixel coordinate of the feature point corresponding to the center of the target ball, is the transformation matrix of the fixed target in the world coordinate system: Among them, is the translation vector of the current target coordinate system relative to the world coordinate system, is the rotation matrix of the previous target coordinate system relative to the world coordinate system; The is obtained by the retroreflective target ball that is not affected by occlusion, and the remaining missing points are feature-reconstructed based on the fixed target coordinate system, so as to complete the feature information and update the state estimation problem as the observation quantity.

2. The passive optical motion capture method according to claim 1, It is characterized in that The adaptive unscented Kalman filter algorithm in S1 includes: The system is initialized and Sigma points are generated; Calculate the opposite weight of each Sigma point; Calculate the state mean and covariance of the one-step forecast; Calculate the mean and variance of the predicted observations; Update the state mean and covariance at time k; Update the observation noise R k .

3. The passive optical motion capture method according to claim 2, It is characterized in that The weights of each Sigma point are: Among them, and are the mean weight and covariance weight corresponding to the initial Sigma points, W i m and W i c are the mean weight and covariance weight corresponding to the i-th Sigma point. α is taken as 0.001, β is taken as 2, and λ is the scaling factor.

4. The passive optical motion capture method according to claim 1, It is characterized in that The said The calculation is as follows: For the initial three-dimensional world coordinates P of each target ball 0 , there is a centroid p of the initial three-dimensional world coordinates 0 : Define this moment, the centroid of the P 0 is: Through binocular depth estimation, the three-dimensional world coordinates P of at least three target balls at a certain moment are obtained b : where n is a positive integer greater than or equal to 3, and at the same time, at this moment, the centroid of the said P b is defined as: Due to the existence of occlusion, currently only the 3D world coordinates of n target balls are known, and n ≥ 3, then the centroid coordinates of the ith point corresponding to the ith target ball are: Among them, is the centroid-removed coordinate of the i-th point at a certain moment, is the initial three-dimensional world coordinate of the i-th point with the centroid removed, p 0 is the centroid of the initial three-dimensional world coordinate, is the initial three-dimensional world coordinate of the i-th point, p b is the centroid of the target ball at a certain moment, is the coordinate of the i-th target ball at a certain moment, Optimize the problem according to the following formula and calculate the rotation matrix R of the overall target relative to the initial state * : Solve for the optimal R using SVD decomposition, and then obtain the as follows: Among them, U and V are decomposed by SVD of W: W = UΣV T Σ is a diagonal matrix composed of singular values; After obtaining the rotation matrix, the following can be obtained is: In the feature reconstruction of the occluded part of the target ball, the fixed position relationship of the observable target ball and the whole on the target can be used for feature reconstruction.

5. The passive optical motion capture method according to claim 4, It is characterized in that In the feature reconstruction method based on the monocular EPnP algorithm in S2, the two-dimensional pixel coordinates P of the target ball observed in the monocular camera are used i b and the fixed target coordinate system to solve the transformation matrix of the fixed target in the camera coordinate system The transformation matrix of the camera coordinate system relative to the world coordinate system is obtained through binocular extrinsic calibration. Then, the three-dimensional world coordinates P i w ' of the target ball reconstructed by the PnP method are as follows: Calculate and solve the transformation matrix of the fixed target in the camera coordinate system based on the known pixel coordinates of the unoccluded target balls and the relative position relationships of the target balls in the target ball coordinate system Thereby, perform feature reconstruction of the missing target balls 6. The passive optical motion capture method according to claim 5, It is characterized in that The three-dimensional coordinates of the known feature points based on the monocular EpnP algorithm are represented by the weighted sum of using four virtual control points Let the three-dimensional world coordinates of the known feature points be: where a ij is the weight, and in the camera coordinate system, the three-dimensional coordinates of the known feature points can also be represented by the weighted sum of the virtual control points : According to the projection model of the camera, there is the following relationship between the i-th pixel coordinate observed by the known feature point and the virtual control point as follows: It can be transformed into two linear equations: The system of equations can be obtained by combining the 2n linear equations of all n feature points: Mx=0 where M is a 2n×12 matrix, which is the coordinate of a virtual control point in the camera coordinate system, a 12-dimensional vector, and there is: Among them, v i is the right singular vector of matrix M, and N is used to calculate M for the N zero eigenvalues of matrix M T The eigenvector of M to obtain v i , and since the spatial position of the virtual control point remains unchanged, calculate the null space error when N takes 1, 2, 3, 4, and select the β corresponding to the smallest dimension i , so as to obtain x, that is, the camera coordinate system coordinates of the virtual control point, and then calculate the centroid of and matrix A: Calculate the centroid of and matrix B: Let H = B T A. Calculating the SVD decomposition of H gives: H = UΣV T The rotation component R of the pose transformation is calculated from this: R = UV T Use R to calculate the translation component t in the pose:

7. The passive optical motion capture method according to claim 1, It is characterized in that The information fusion method between multiple binocular systems in S3 includes: Select a group of binocular subsystems with the smallest covariance matrix in the multi-camera to determine the system state equation; Use the adaptive unscented Kalman filter to calculate the mean state estimate x of the M binocular subsystems at time k k and the covariance matrix P. Arrange the covariance matrices according to the magnitude of the trace, and define the system with the smallest trace of the covariance matrix as the reference subsystem; At the current moment, all subsystems calculate the adaptive unscented Kalman filter, and after using the predicted covariance matrix to calculate the benchmark subsystem, the prediction results of other subsystems are fused with the benchmark subsystem to achieve loose coupling of multi-cameras.

8. The passive optical motion capture method according to claim 7, It is characterized in that The following calculations are performed using the reference subsystem: Select 2n+1 Sigma points: Among them, represents the state estimation value of the reference subsystem at time k-1, and is its corresponding state error covariance matrix; Transform the Sigma point through the state transfer matrix: Obtain the state prediction value of the benchmark subsystem: Calculate and obtain the state prediction error corresponding to each Sigma point: According to the corresponding weights of the Sigma points, the prediction error covariance of the benchmark subsystem is calculated: The state estimates and covariances of the remaining binocular subsystems are weighted, and the covariance of the multi-eye loosely coupled system is calculated based on the matrix weighted fusion results: where \(i\) represents the serial number of the local subsystem, \(M\) is the total number of local camera subsystems in the multi-vision sensor pose estimation system, \(P\) i is the state error covariance of the \(i\)-th subsystem, \(P\) s is the total state prediction error covariance of the loose coupling system; Calculate the mean value of the state estimation of the loosely coupled system:

9. A passive optical motion capture system, characterized in that it adopts the passive optical motion capture method described in any one of claims 1 to 8.

Images