Visual inertia angle fusion positioning method capable of actively adjusting field of view

Abstract

The embodiment of the invention provides a visual inertia angle fusion positioning method and device for actively adjusting a field of view, a medium and equipment. The method comprises the following steps: calculating an external parameter rotation matrix of feature points in each frame of target image calibrated by a binocular camera relative to an inertia IMU according to current angle information; the method comprises the following steps: establishing a local coordinate system taking an initial position of a target image as an original point based on three-dimensional motion of the target image and parameters measured by an inertial IMU, and respectively calculating a corresponding visual projection error vector and an IMU pre-integration residual vector based on a feature point rotation matrix and an external parameter rotation matrix of the target image; constructing a factor graph according to the visual projection error vector and the IMU pre-integration residual vector, solving the state quantity in the factor graph based on a sliding window, and determining the position of the unmanned aerial vehicle and the high-frequency body pose of the unmanned aerial vehicle based on the state quantity; the orientation of the camera is automatically adjusted according to the distribution of the feature points in the field of view of the camera, and the positioning method for actively adjusting visual inertia angle fusion of the field of view is achieved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of simultaneous localization and mapping, and in particular to a vision-inertial angle fusion positioning method and device with active field of view adjustment, a medium and an apparatus. BACKGROUND

[0002] Simultaneous localization and mapping (SLAM) is a core technology for intelligent autonomous robots to complete task objectives independently, and has been widely researched in recent years and widely applied in many fields such as micro unmanned aerial vehicles, intelligent driving, virtual reality and augmented reality.

[0003] With the research and open source of all parties in recent years, the SLAM technology has developed from the initial positioning method relying only on vision or only on laser radar to the multi-sensor positioning method capable of overcoming the shortcomings of a single sensor. In the prior art, the positioning method fusing vision and inertial is the most mainstream method due to its low cost advantage. However, in the existing vision-inertial positioning method, the camera is usually fixed on the carrier, and when the carrier moves along the preset path, it may encounter a weak texture area such as a white wall, at which time almost no feature point can be found in the field of view of the camera for stable tracking. Although the traditional vision-inertial positioning method can still use the inertial measurement unit (IMU) to estimate the motion, it cannot correct the cumulative error of the IMU, and thus the positioning method of the prior art reduces the accuracy of simultaneous localization and mapping. SUMMARY

[0004] The main purpose of the present application is to provide a vision-inertial angle fusion positioning method and device with active field of view adjustment, a medium and an apparatus, which aims to determine the best orientation of the camera for positioning according to the distribution of feature points, and drive the servo to move the camera to the best orientation, reduce feature loss, and improve the robustness and accuracy of autonomous positioning in different scenes.

[0005] To achieve the above object, the application provides a positioning method of vision-inertial angle fusion with active adjustment of field of view, comprising: acquiring current angle information of each frame of target image shot by a camera relative to a rotation axis in real time by using an angle encoder, and extracting and tracking feature points of the target image shot by a binocular camera; calculating an external parameter rotation matrix of the feature points in each frame of target image calibrated by the binocular camera relative to an inertial IMU according to the current angle information; establishing a local coordinate system with an initial position of the target image as an origin based on three-dimensional motion of the target image and parameters measured by the inertial IMU, and calculating corresponding vision projection error vectors and IMU pre-integral residual error vectors of the feature points and the external parameter rotation matrix respectively in the local coordinate system based on the target image; constructing a factor graph according to the vision projection error vectors and the IMU pre-integral residual error vectors, and solving state quantities in the factor graph based on a sliding window; determining a UAV position based on the state quantities, and obtaining a high-frequency UAV body pose according to IMU interpolation of the UAV position; and adjusting the current angle value of the camera to a target view angle of the camera based on the distribution of the feature points of the target image.

[0006] Optionally, the current angle information comprises a rotation angle of each frame of target image; the calculation of the external parameter rotation matrix of the feature points in each frame of target image calibrated by the binocular camera relative to the inertial IMU according to the current angle information comprises: acquiring preset external parameter matrices of the feature points in preset images shot by the binocular camera relative to the inertial IMU under two groups of different preset rotation angles respectively; converting the two groups of preset external parameter matrices into corresponding Lie algebras after representing each rotation angle respectively; calculating a rotation ratio of each feature point based on a rotation axis angle of the target image and a rotation axis angle of the preset image; wherein the rotation axis angle of the current image refers to the angle of the current image relative to the rotation axis; determining two rotation amounts and two translation amounts of each feature point calibrated by each camera in the binocular camera according to the two groups of Lie algebras respectively; calculating left rotation amounts and right rotation amounts of each feature point according to the two rotation amounts of each camera weighted by the rotation ratio, and calculating left translation amounts and right translation amounts of each feature point according to the two translation amounts of each frame of image weighted by the rotation ratio; and determining the external parameter rotation matrix of each frame of target image relative to the inertial IMU according to the left rotation amounts and the right rotation amounts, and the left translation amounts and the right translation amounts of each feature point.

[0007] Optionally, the calculation of the rotation ratio of each feature point based on the rotation axis angle of the target image and the rotation axis angle of the preset image comprises: calculating left external parameter matrices and right external parameter matrices of the target image according to the rotation ratio and the left external parameter matrices and the right external parameter matrices of the preset image frame.

[0008] Optionally, the cameras include a left-eye camera and a right-eye camera, the two rotation amounts of each feature point calibrated by the left-eye camera include a left-eye first rotation amount and a left-eye second rotation amount, the two translation amounts of each feature point calibrated by the left-eye camera include a left-eye first translation amount and a left-eye second translation amount, the two rotation amounts of each feature point calibrated by the right-eye camera include a right-eye first rotation amount and a right-eye second rotation amount, the two translation amounts of each feature point calibrated by the right-eye camera include a right-eye first translation amount and a right-eye second translation amount, and the calculation of the left rotation amount and the right rotation amount of each feature point according to the two rotation amounts of each camera weighted by the rotation proportion and the calculation of the left translation amount and the right translation amount of each feature point according to the two translation amounts of each frame image weighted by the rotation proportion include: determining a first weighting factor according to a difference between 1 and the rotation proportion, determining a second weighting factor according to the rotation proportion, calculating the left rotation amount of each feature point calibrated by the binocular camera according to a sum of the left-eye first rotation amount of each feature point weighted by the first weighting factor and the left-eye second rotation amount of each feature point weighted by the second weighting factor, calculating the right rotation amount of each feature point calibrated by the binocular camera according to a sum of the right-eye first rotation amount weighted by the first weighting factor and the right-eye second rotation amount weighted by the second weighting factor, calculating the left translation amount of each feature point calibrated by the binocular camera according to a sum of the left-eye first translation amount weighted by the first weighting factor and the left-eye second translation amount weighted by the second weighting factor, and calculating the right translation amount of each feature point calibrated by the binocular camera according to a sum of the right-eye first translation amount weighted by the first weighting factor and the right-eye second translation amount weighted by the second weighting factor.

[0009] Optionally, the parameters determined based on the three-dimensional motion of the target image and the inertial IMU include establishing a local coordinate system with an initial position of the target image as an origin, which includes: obtaining relative pose data of the target relative to the initial position based on the three-dimensional motion of the target image; obtaining acceleration data of an inertial IMU accelerometer, bias data of a gyroscope, and gravity vector data of a gravimeter; and aligning the relative pose data, the acceleration data, the bias data of the gyroscope, and the gravity vector data of the gravimeter to obtain the local coordinate system with the initial position of the body as the origin.

[0010] Optionally, the camera is arranged on the unmanned aerial vehicle body, and the visual projection error vector is calculated based on the feature points of the target image and the extrinsic rotation matrix, including: determining a first subtrahend based on a first vector constructed based on the same feature points observed in different images; determining a projection function and an inverse projection function of each feature point in the image based on a camera model; determining a first transformation matrix and a first inverse transformation matrix according to an extrinsic transformation from the center of the unmanned aerial vehicle body to the center of the camera, and determining a second transformation matrix and a second inverse transformation matrix according to the pose of each frame of image captured by the camera of the unmanned aerial vehicle in the local coordinate system; determining a first product term according to the projection function, determining a second product term according to the product of the first inverse transformation matrix and the second inverse transformation matrix, the first transformation matrix, the second transformation matrix and the inverse projection function, determining a third product term according to each feature point and the first vector, and determining a first minuend according to the first product term, the second product term and the third product term successively multiplied; and calculating the visual projection error vector according to the difference between the first subtrahend and the first minuend.

[0011] Optionally, the process of calculating the IMU pre-integration residual error vector based on the feature points of the target image and the extrinsic rotation matrix includes: obtaining the relative position, the relative velocity and the relative rotation amount at the previous moment by pre-integrating the inertial IMU data; constructing the error of the position pre-integration amount, the error of the rotation integration amount, the error of the velocity integration amount, the error of the gyroscope zero offset and the error of the accelerometer zero offset based on the relative position, the relative velocity and the relative rotation amount, the gyroscope zero offset and the accelerometer zero offset respectively, and constructing the IMU pre-integration residual error vector based on the error of the position pre-integration amount, the error of the rotation integration amount, the error of the velocity integration amount, the error of the gyroscope zero offset and the error of the accelerometer zero offset; wherein a first minuend is determined based on the relative position change amount, a first numerical term after being multiplied by the coordinate system rotation matrix is taken as a first subtrahend, and the error of the position pre-integration amount is obtained according to the difference between the first subtrahend and the first minuend; the error of the rotation integration amount is determined based on the imaginary part of the weighted quaternion term, the quaternion term being determined based on the difference between the relative attitude transformation at adjacent moments and the product of two rotation quaternions of the body coordinate system to the world coordinate system at adjacent moments and the quaternion of 1; a second subtrahend is determined based on a second numerical term after being multiplied by the error of the velocity integration amount of the coordinate system rotation matrix, a second minuend is determined based on the data integration amount at adjacent moments, and the error of the velocity integration amount is determined according to the difference between the second subtrahend and the second minuend; the error of the accelerometer zero offset is obtained based on the difference value of the accelerometer zero offset at adjacent moments; and the error of the gyroscope zero offset is obtained based on the difference value of the gyroscope zero offset at adjacent moments.

[0012] Optionally, the adjusting the current angle value of the camera to the target view angle based on the feature point distribution of the target image comprises: dividing each frame of image into a plurality of equal-width regions to correspond to a plurality of horizontal scanning regions; counting the number of feature points in each horizontal scanning region and calculating a vertical distribution curve of the feature points in each horizontal scanning region, wherein the vertical distribution curve is determined by the sum of the products of each feature point and an indicator function, and the indicator function is used to determine whether a coordinate point belongs to the horizontal scanning region; counting the brightness in each horizontal scanning region, selecting the brightness region with the highest comprehensive score based on a preset brightness selection expression; processing the center coordinates of the brightness region based on a preset pitch angle condition expression to obtain a target pitch angle adjustment angle; and adjusting the servo driver of the binocular camera according to the target pitch angle adjustment angle and the current angle, so that the binocular camera turns to the target angle.

[0013] To achieve the above object, the present application further provides a positioning device for actively adjusting the field of view of visual-inertial angle fusion, comprising: an input module for acquiring current angle information of each frame of target image shot by a camera relative to a rotation axis in real time by using an angle encoder, extracting and tracking feature points of a target image shot by a binocular camera; calculating an external parameter rotation matrix of the feature points in each frame of target image calibrated by the binocular camera relative to an inertial IMU according to the current angle information; a fusion processing module for establishing a local coordinate system with an initial position of the target image as the origin based on three-dimensional motion of the target image and parameters determined by the inertial IMU, calculating a corresponding visual projection error vector and an IMU pre-integral residual error vector based on the feature points and the external parameter rotation matrix of the target image in the local coordinate system, constructing a factor graph according to the visual projection error vector and the IMU pre-integral residual error vector, and solving state quantities in the factor graph based on a sliding window, determining a UAV position based on the state quantities, and obtaining a high-frequency UAV body pose according to the IMU interpolation of the UAV position; and an adjustment module for adjusting the current angle value of the camera to the target view angle based on the feature point distribution of the target image.

[0014] To achieve the above object, the present application further provides an electronic device, comprising: at least one processor, a memory and an input-output unit; wherein the memory is used to store a computer program, and the processor is used to call the computer program stored in the memory to execute the positioning method for actively adjusting the field of view of visual-inertial angle fusion provided by any of the preceding embodiments.

[0015] The positioning method, device, medium and equipment of active field of view adjustment visual inertial angle fusion provided by the embodiment of the application, by using the angle encoder to obtain the current angle information of each frame of target image shot by the camera relative to the rotation axis in real time, the feature points of the target image shot by the binocular camera are extracted and tracked; the current angle information is used to calculate the external parameter rotation matrix of the feature points in each frame of target image calibrated by the binocular camera relative to the inertial IMU; based on the three-dimensional motion of the target image and the parameters measured by the inertial IMU, a local coordinate system with the initial position of the target image as the origin is established, in the local coordinate system, the corresponding visual projection error vector and IMU pre-integration residual error vector are calculated based on the target image respectively, the factor graph is constructed according to the visual projection error vector and the IMU pre-integration residual error vector, and the state quantity in the factor graph is solved based on the sliding window, the UAV position is determined based on the state quantity, and the high-frequency UAV body pose is obtained according to the IMU interpolation of the UAV position; the current angle value of the camera is adjusted to the target view angle of the camera based on the feature point distribution of the target image, the best orientation of the camera is calculated by analyzing the feature point distribution of each frame of image in the field of view, and the best orientation is sent to the servo driver to adjust the camera to the best orientation, so as to ensure the observation quality of the feature points and improve the robustness of the positioning method in the dynamic scene. In addition, the angle encoder is used to obtain the angle of the camera relative to the rotation axis in real time, the external parameter of each frame of image corresponding to the camera relative to the IMU is calculated according to the angle, and the independent external parameter of each frame of image is used in the joint optimization process, so that the autonomous positioning under the condition of non-rigid fixation of the camera and the IMU can be realized, that is, the camera can be randomly transformed during positioning, the camera and the IMU need to be rigidly fixed in the traditional visual inertial odometer, and then the positioning accuracy is reduced due to the decline of the feature point quality of the camera under part of the motion attitude. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1 The flowchart provided by an embodiment of the positioning method of active field of view adjustment visual inertial angle fusion of the application; Figure 2 The flowchart provided by an embodiment of the positioning method of active field of view adjustment visual inertial angle fusion of the application; Figure 3 The binocular camera diagram provided by an embodiment of the positioning method of active field of view adjustment visual inertial angle fusion of the application; Figure 4 The rotation diagram provided by an embodiment of the positioning method of active field of view adjustment visual inertial angle fusion of the application; Figure 5 The structural block diagram provided by an embodiment of the positioning device of active field of view adjustment visual inertial angle fusion of the application.

[0017] The implementation, functional features and advantages of the present application will be further described with reference to the embodiments and the accompanying drawings. DETAILED DESCRIPTION

[0018] It should be understood that the specific embodiments described herein are merely exemplary and do not limit the application.

[0019] The data input module is used for acquiring the data of IMU, binocular camera and angle encoder, wherein the IMU data contains time stamp, three-axis acceleration, three-axis angular velocity and three-axis angular acceleration, the binocular camera data contains time stamp, left eye image and right eye image, and the angle encoder contains time stamp and angle of the camera relative to the rotating shaft, meanwhile, the closest angle according to the time stamp is matched for each frame of image, the corresponding external parameter is calculated, and finally these data are added to the processing queue; the fusion processing module contains two stages of initialization and fusion processing, the three-dimensional motion reconstruction only by vision is performed in the initialization stage to obtain the initial relative pose of the carrier, the joint initialization of vision and IMU is performed to determine the bias of the IMU accelerometer and gyroscope and the alignment of the gravity vector, the coordinate system is established, in the fusion processing stage, the vision feature points, the external parameter corresponding to each frame of image and the IMU pre-integration result are fused, the low-frequency but high-precision body pose is output through the nonlinear optimization, and the high-frequency body pose is obtained through the IMU interpolation; the dynamic adjustment module analyzes the distribution of the feature points in the field of view of each frame of image, calculates the best orientation of the camera, and sends the best orientation to the servo driver to adjust the camera to the best orientation, so as to ensure the observation quality of the feature points and improve the robustness of the positioning method in the dynamic scene.

[0020] REFERENCE Figure 1 and Figure 2 , Figure 1 The flow chart of the positioning method of the active adjustment of the field of view of the visual inertial angle fusion provided by the first embodiment of the present application is shown in FIG. 2. Figure 2 The idea of the present application is described as follows: firstly, the information of the camera, IMU and angle encoder is preprocessed, the feature extraction and tracking of the camera image are performed, the inertial IMU data is pre-integrated, and the external parameter of the camera relative to the IMU of the frame of image is calculated according to the angle obtained by the encoder; then, the three-dimensional motion reconstruction only by vision is performed using the image information, and the joint initialization with the IMU information is performed; the corresponding external parameter is marked for each frame of image and the back-end optimization is performed; the best orientation of the camera is estimated according to the distribution of the feature points in the field of view of the image, and the servo driver is driven to make the camera face the best orientation.

[0021] Figure 3 The schematic diagram of the device of the present application is shown in FIG. 3, wherein 1 is a binocular camera, 2 is a camera rotating bracket, 3 is a motor, 4 is an encoder and servo driver, 5 is a fixed support, the motor can drive the camera to rotate up and down (for example, the camera rotates 90 degrees up and down), the encoder and the servo driver are used to drive the camera to rotate to the best orientation, and the fixed support is used to fix the camera.Figure 4 As shown in FIG. 1, the encoder can output the rotation angle of the camera around the rotation axis in real time, and the camera image, the encoder, and the servo driver are connected to the computer through a cable, so that the image and the rotation angle can be transmitted to the computer in real time, and the servo driver can obtain the target rotation angle sent by the computer. Figure 1 , Figure 1 The flowchart of the positioning method of the visual-inertial angle fusion with the field of view actively adjusted according to the first embodiment of the present application can be executed by a processor, which can be arranged in a host or a server. The positioning method of the visual-inertial angle fusion with the field of view actively adjusted can include the following steps. S10, real-time acquisition of current angle information of each frame of target image shot by a camera relative to a rotation axis by using an angle encoder, feature point extraction and tracking of the target image shot by a binocular camera, and calculation of an external parameter rotation matrix of the feature point in each frame of target image calibrated by the binocular camera relative to an inertial measurement unit (IMU) according to the current angle information.

[0022] In an embodiment of the present application, the specific execution process of step S10 can include the following steps. S101, the current angle information includes a rotation angle of each frame of target image.

[0023] S102, the calculation of the external parameter rotation matrix of the feature point in each frame of target image calibrated by the binocular camera relative to the IMU according to the current angle information can include the following steps. S103, acquisition of preset external parameter matrices of feature points in preset images shot by the binocular camera under two groups of different preset rotation angles respectively.

[0024] S104, conversion of the two groups of preset external parameter matrices into corresponding Lie algebras after the two groups of preset external parameter matrices are represented by the respective rotation angles, and calculation of a rotation ratio of each feature point based on a rotation axis angle of the target image and a rotation axis angle of the preset image.

[0025] In an embodiment of the present application, the process of calculating the rotation ratio of each feature point based on the rotation axis angle of the target image and the rotation axis angle of the preset image can include the following steps. Calculation of the rotation ratio according to the rotation axis angle of the current image frame and the minimum rotation axis angle and the maximum rotation axis angle of the preset image frame.

[0026] It should be noted that the external parameter of the camera relative to the IMU can be further calculated by calculating the rotation ratio. The visual projection error vector and the IMU pre-integration residual error vector can be calculated according to the external parameter of the camera relative to the IMU.

[0027] Specifically, the processor reads the timestamp of the image frame after obtaining each image frame, and finds the angle value closest to the same moment from the historical data frame of the encoder as the angle of the camera relative to the rotation axis at the moment of image shooting. Next, the processor uses the two sets of different rotation angles calibrated in advance to determine the external parameter matrix of the two sets of IMUs. In addition, the processor can calculate the external parameter matrix of the current image according to the current angle. Specifically, the processor can determine the maximum axis angle and the minimum axis angle according to the respective three-degree-of-freedom rotation matrix in the two sets of external parameter matrices, and determine the numerator corresponding to the target image axis angle according to the rotation ratio of the frame image. Based on the numerator corresponding to the target image axis angle, and based on the maximum axis angle and the minimum axis angle value, the corresponding denominator is determined. Based on the ratio of the numerator and the denominator, the axis angle of the current image is determined, and then the left eye external parameter matrix and the right eye external parameter matrix of the current image are determined. It can be understood that the processor can determine the axis angle of the current image according to the rotation matrix in the two sets of external parameters, and then convert the axis angle of the current image into Lie algebra :

[0028] wherein, is the rotation angle, that is, the axis angle, is the unit rotation axis.

[0029] Then the processor can calculate the rotation ratio t : .

[0030] S105, respectively determine two rotation amounts and two translation amounts of each feature point calibrated by each camera in the binocular camera according to the two sets of Lie algebras.

[0031] S106, calculate the left rotation amount and the right rotation amount of each feature point according to the two rotation amounts of each camera weighted by the rotation ratio, and calculate the left translation amount and the right translation amount of each feature point according to the two translation amounts of each frame image weighted by the rotation ratio.

[0032] S107, determine the external parameter rotation matrix of the current frame image relative to the inertial IMU according to the left rotation amount and the right rotation amount, and the left translation amount and the right translation amount of each feature point.

[0033] In an embodiment of the present application, each camera includes a left eye camera and a right eye camera, the two rotation amounts of each feature point calibrated by the left eye camera include a left eye first rotation amount and a left eye second rotation amount, and the two translation amounts of each feature point calibrated by the left eye camera include a left eye first translation amount and a left eye second translation amount. The two rotation amounts of each feature point calibrated by the right eye camera include a right eye first rotation amount and a right eye second rotation amount, and the two translation amounts of each feature point calibrated by the right eye camera include a right eye first translation amount and a right eye second translation amount.

[0034] The execution process of calculating the left rotation amount and the right rotation amount of each feature point according to the two rotation amounts of each camera weighted by the rotation proportion, and calculating the left translation amount and the right translation amount of each feature point according to the two translation amounts of each frame image weighted by the rotation proportion can include the following: The left rotation amount of each feature point in binocular camera calibration is calculated by summing the first rotation amount of the left eye weighted by the first weighting factor determined according to the difference between 1 and the rotation proportion, and the second rotation amount of the left eye weighted by the second weighting factor determined according to the rotation proportion.

[0035] The right rotation amount of each feature point in binocular camera calibration is calculated by summing the first rotation amount of the right eye weighted by the first weighting factor, and the second rotation amount of the right eye weighted by the second weighting factor.

[0036] The left translation amount of each feature point in binocular camera calibration is calculated by summing the first translation amount of the left eye weighted by the first weighting factor, and the second translation amount of the left eye weighted by the second weighting factor.

[0037] The right translation amount of each feature point in binocular camera calibration is calculated by summing the first translation amount of the right eye weighted by the first weighting factor, and the second translation amount of the right eye weighted by the second weighting factor.

[0038] After the rotation proportion is calculated, next, the processor can calculate the Lie algebra and the translation vector:

[0039] wherein, , the first rotation amount of the left eye and the second rotation amount of the left eye in the two rotation amounts of the left target calibration, , the first rotation amount of the right eye and the second rotation amount of the right eye in the two rotation amounts of the right target calibration, , the first translation amount of the left eye and the second translation amount of the left eye in the two translation amounts of the left target calibration, , the first translation amount of the right eye and the second translation amount of the right eye in the two translation amounts of the right target calibration, t and 1- t are the second weighting factor and the first weighting factor, respectively.

[0040] Finally, the calculated Lie algebra is converted into a rotation matrix :

[0041] wherein, is the Lie algebra matrix exponential mapping.

[0042] The specific calculation process can include the following: First, the 3D vector is converted into an anti-symmetric matrix .

[0043] Wherein:

[0044] Then, the matrix is subjected to an exponential operation:

[0045] Wherein, is the unit matrix.

[0046] S20, based on the three-dimensional motion of the target image and the parameters measured by the inertial IMU, a local coordinate system with the initial position of the target image as the origin is established, in the local coordinate system, based on the target image, the feature points and the external parameter rotation matrix are calculated respectively Corresponding visual projection error vector and IMU pre-integration residual error vector, construct factor graph according to the visual projection error vector and the IMU pre-integration residual error vector, and solve the state quantity in the factor graph based on the sliding window, determine the UAV position based on the state quantity, and obtain the high-frequency body pose of the UAV according to the IMU interpolation of the UAV position.

[0047] In an embodiment of the present application, the process of establishing a local coordinate system with the initial position of the target image as the origin based on the three-dimensional motion of the target image and the parameters measured by the inertial IMU can include the following: Obtain the relative pose data of the target relative to the initial position based on the three-dimensional motion of the target image.

[0048] Obtain the acceleration data of the inertial IMU accelerometer, the bias data of the gyroscope, and the gravity vector data of the gravimeter.

[0049] Align the relative pose data, the acceleration data, the bias data of the gyroscope, and the gravity vector data of the gravimeter to obtain a local coordinate system with the initial position of the body as the origin.

[0050] That is, the processor establishes a local coordinate system with the initial position of the target image as the origin based on the three-dimensional motion of the target image and the parameters measured by the inertial IMU, which can be: the processor initializes, uses only visual three-dimensional motion reconstruction to obtain the initial relative pose of the carrier relative to the initial position, performs visual IMU joint initialization to obtain the alignment of the bias of the IMU accelerometer and the gyroscope and the gravity vector, and establishes a local coordinate system with the initial position of the body as the origin.

[0051] It should be noted that the processor can calculate the visual re-projection error, the IMU pre-integration residual, and construct a factor graph based on the visual re-projection error and the IMU pre-integration residual as factors respectively. Then, the processor constructs a sliding window to all state quantities in the factor graph is optimized and solved, wherein, is the number of frames in the sliding window, is the number of all feature points in the sliding window, , , , are the position, velocity and attitude of the carrier respectively, , are the biases of the accelerometer and the gyroscope, is the extrinsic parameter corresponding to each frame state quantity, and the specific form is , describes the rotation relationship from the camera coordinate system to the body coordinate system, describes the vector from the origin of the camera coordinate system to the origin of the body coordinate system, is the inverse depth of the feature point, and the processor can solve the pose of the machine position through the state quantity, wherein the machine position refers to the machine position of the unmanned aerial vehicle, and then the processor can perform IMU difference on the machine position to obtain the high-frequency body position.

[0052] Wherein, the camera is arranged on the unmanned aerial vehicle body, and the process of calculating the IMU pre-integration residual vector based on the target image, the feature point and the extrinsic rotation matrix can include the following: The relative position, relative velocity and relative rotation quantity of the previous moment obtained by pre-integrating the inertial IMU data.

[0053] Based on the relative position, the relative velocity, the relative rotation quantity, the gyroscope zero bias and the accelerometer zero bias, the errors of the position pre-integration quantity, the rotation integral quantity, the velocity integral quantity, the gyroscope zero bias and the accelerometer zero bias are constructed respectively, and the IMU pre-integration residual vector is constructed based on the errors of the position pre-integration quantity, the rotation integral quantity, the velocity integral quantity, the gyroscope zero bias and the accelerometer zero bias.

[0054] Wherein, the first subtractive term is determined based on the relative position change quantity, the first numerical term after the coordinate system rotation matrix is multiplied as the first minuend, and the difference between the first minuend and the first subtractive term is obtained. The first numerical term is obtained by subtracting the displacement of the adjacent time from the difference between the two first coordinate values of the adjacent time.

[0055] The error of the rotation integral is determined based on the imaginary part of the weighted quaternion term. The quaternion term is determined based on the difference between the relative attitude transformation at adjacent time points and the product of the two rotation quaternions from the body coordinate system to the world coordinate system at adjacent time points and the quaternion product of 1.

[0056] The second minuend is determined by left-multiplying the error of the velocity integral of the coordinate system rotation matrix by the second numerical term. The second subtrahend is determined by the data integrals at adjacent time points. The error of the velocity integral is determined by the difference between the second minuend and the second subtrahend.

[0057] The error of the accelerometer zero bias is obtained based on the difference between the zero bias values ​​of the accelerometer at adjacent time points.

[0058] The gyroscope's bias error is obtained based on the difference in the gyroscope's bias at adjacent time points.

[0059] For example, the camera factor is established by considering the l-th feature point first observed in the image. P ,Will P From the first time I saw it i Transform the camera coordinate system to the first camera coordinate system. j In each camera coordinate system, the observation residual in subsequent images is defined as:

[0060] in For the first The coordinates of the feature points observed in the camera-normalized camera coordinate system in the j-th frame are:

[0061] Is it an estimate of the first l The road sign at the 1st j Projected coordinates in the normalized camera coordinate system of the frame camera:

[0062] in For the first The feature point at the th ... i The coordinates observed in the normalized camera coordinate system of the frame camera:

[0063] , For unit ball and Any two orthogonal bases of intersecting tangent planes. It is the first l The inverse depth of feature points in the i-th frame, It is the actual depth value at that point. It is the firsti The external parameters from the camera to the machine body in the frame. It is the vector pointing from the origin of the camera coordinate system to the origin of the body coordinate system at frame i. It is the first j The rotation matrix from the frame's body to the camera. These are vectors pointing from the origin of the camera coordinate system to the origin of the body coordinate system at frame j. These vectors are generated by the camera extrinsic parameters for each frame. It is derived from the separation and conversion in the middle. It is the rotation matrix from the body coordinate system to the local coordinate system in the i-th frame. These are the coordinates of the origin of the body coordinate system in the local frame at frame i. These are the coordinates of the origin of the body coordinate system in the local frame at frame j. From the world coordinate system to the 1st j The rotation matrix of the frame body coordinate system. It is the vector from the origin of the body coordinate system in frame j to the origin of the camera coordinate system in frame j. It is the rotation matrix from the body coordinate system to the camera coordinate system. It is the first l The feature point at the th ... j Pixel coordinates in the frame camera coordinate system It is the first i The feature point at the th ... j Pixel coordinates in the frame camera coordinate system and These are the projection and back-projection functions, which are constructed from the camera model.

[0064] In one embodiment of this application, the execution process of calculating the IMU pre-integration residual vector based on feature points and extrinsic parameter rotation matrices of the target image may include the following: The first to third elements of the second minuend term of the IMU pre-integration residual vector are constructed based on the relative position, relative velocity, and relative rotation obtained from the pre-integration of the inertial IMU data at the previous moment, and the fourth to fifth elements of the second minuend term are set to zero.

[0065] The first element of the second subtraction term is determined based on the difference between the current observation value of the inertial IMU and other observation values.

[0066] The multiplicand is determined by the sum of the difference in velocity between adjacent moments and the integral of the gravitational acceleration.

[0067] The multiplication term is determined by the inverse of the rotation matrix at the previous moment, and the second element of the second subtraction term is determined by the product of the multiplication term and the multiplicand.

[0068] The third element of the second subtraction term is determined by multiplying the inverse of the rotation matrix at the previous moment with the rotation matrix at the current moment.

[0069] The fourth element of the second subtractive term is determined according to the difference between the acceleration data obtained by the accelerometer at adjacent time points.

[0070] The fifth element of the second subtractive term is determined according to the difference between the visual inertial angle fusion positioning method of the active adjustment field of view at adjacent time points.

[0071] The IMU pre-integration residual vector factor is determined by the difference between the second minuend and the second subtractive term.

[0072] In the specific implementation process, the processor assumes that the additional noise in the accelerometer and gyroscope measurements is Gaussian white noise, and the time-varying accelerometer and gyroscope bias is modeled as a random walk process, the derivative of which is Gaussian white noise. Since the inertial IMU acquires data at a higher frequency than other sensors, there are usually multiple IMU measurement data between two frames. Therefore, the present application pre-integrates the inertial IMU measurement on the manifold, and the covariance matrix is transmitted.

[0073] The processor can pre-integrate the inertial IMU measurement to generate relative positions k , relative velocities k and relative rotations between two time points and +1. In addition, the pre-integration also transmits the covariance matrices of the relative position, relative velocity and relative rotation, as well as the covariance matrix of the bias. The IMU residual can be defined as:

[0074] Error representing the position pre-integration quantity, Error representing the rotation integral quantity, Error representing the velocity integral quantity, Error representing the accelerometer zero bias, Error representing the gyroscope zero bias, is the coordinate system rotation matrix from the world coordinate system to the body coordinate system, is the first coordinate value of the body coordinate system under the local coordinate system, and refer to different time points, is the position of the origin of the body coordinate system under the local coordinate system, refers to the gravity vector under the local coordinate system, and is set as 0, 0, -9.8 ] ^T， is the time interval from k time point to k +1 time point, is obtained by IMU pre-integration, and is the time interval from kTo k the relative position change amount at the time t+1, is the relative position change amount from k to k the relative attitude transformation at the time t+1, and is k and k the rotation quaternion of the body coordinate system to the world coordinate system at the time t+1, is the imaginary part of the quaternion item x , y , z component, and is k the velocity of the body in the local coordinate system at the time t+1 and k time, is the relative velocity change amount from k time to k the time t+1 obtained by IMU pre-integration, is the accelerometer zero offset, which is a three-dimensional vector describing the system error measured by the accelerometer, is the gyroscope zero offset, which is a three-dimensional vector describing the system error measured by the gyroscope.

[0075] The two factors are added to the optimization term to establish an optimization model:

[0076] wherein, represents the set of all sensor measurement information.

[0077] Then, the processor adds the two factors to the optimization term, optimizes the state variable by the Levenberg-Marquardt method, minimizes the residual error to obtain the globally optimal pose estimation, and obtains the pose information of the carrier.

[0078] S30, adjust the current angle value of the camera to the target view angle of the camera based on the feature point distribution of the target image.

[0079] In an embodiment of the present application, the process of adjusting the current angle value of the camera to the target view angle of the camera based on the feature point distribution of the target image can include the following: Divide each frame of image into a plurality of equal-width regions to correspond to constructing a plurality of horizontal scanning regions.

[0080] Statistical feature points in each horizontal scanning region, and calculate the vertical distribution curve of the feature points in each horizontal scanning region, wherein the vertical distribution curve is determined by the sum of the product of each feature point and an indicator function, and the indicator function is used to judge whether the coordinate point belongs to the horizontal scanning region.

[0081] The luminance in each horizontal scanning area is counted, and the luminance area with the highest comprehensive score is selected based on a preset luminance selection expression.

[0082] The center coordinates of the luminance area are processed based on a preset pitch angle condition expression to obtain a target pitch angle adjustment angle.

[0083] According to the target pitch angle adjustment angle and the current angle, the servo driver of the binocular camera is adjusted to make the binocular camera turn to the target angle.

[0084] The processor can adjust the viewing angle of the camera according to the feature point distribution and the viewing angle selection strategy. For example, the processor first divides the image area, divides a frame of image picture into n equal-width regions along the vertical direction, i.e. n horizontal scanning areas, numbered , the width of each region is:

[0085] wherein h is the pixel height of the image.

[0086] Then the processor can count the number of feature points, count the number of feature points in each region , and calculate the vertical distribution curve :

[0087] wherein is an indicator function, if the feature point coordinates belong to , the value is 1, otherwise 0.

[0088] Next, the processor can count the luminance, calculate the average luminance of each region .

[0089] Then, the processor can select the candidate area, require to exclude noise interference, and require to exclude dark and bright areas. In the candidate area, the expression of the region with the highest comprehensive score is selected:

[0090] wherein and are the maximum values of the number of feature points and the luminance of all regions, and is a weight coefficient, used to balance the reference degree of feature points and brightness.

[0091] Finally, the processor can calculate the target angle adjustment amount, and according to the center coordinates of the target image , calculate the target pitch angle adjustment amount The expression is:

[0092] wherein, is the image height, is a proportion coefficient, a negative value represents a downward rotation, a positive value represents an upward rotation, the angle of the previous frame is , and the target angle of the current frame is The final result is transmitted to the servo driver to drive the camera to the target angle.

[0093] Referring to Figure 5 , on the basis of the above method embodiment, the application further provides a positioning device for actively adjusting the field of view of visual-inertial angle fusion, which can include an input module 201, a fusion processing module 202, and an adjustment module 203: the input module 201 is used to acquire the current angle information of each frame of target image relative to the rotation axis photographed by the camera in real time by using an angle encoder, and to extract and track the feature points of the target image photographed by the binocular camera. According to the current angle information, the feature points in each frame of target image calibrated by the binocular camera are calculated relative to the external parameter rotation matrix of the inertial IMU. The fusion processing module 202 is used to establish a local coordinate system with the initial position of the target image as the origin based on the three-dimensional motion of the target image and the parameters determined by the inertial IMU. In the local coordinate system, the corresponding visual projection error vector and IMU pre-integration residual error vector are calculated based on the feature points and the external parameter rotation matrix of the target image, respectively. The factor graph is constructed according to the visual projection error vector and the IMU pre-integration residual error vector, and the state quantity in the factor graph is solved based on the sliding window. The UAV position is determined based on the state quantity, and the high-frequency UAV body pose is obtained according to the IMU interpolation of the UAV position. The adjustment module 203 is used to adjust the current angle value of the camera to the target view angle based on the feature point distribution of the target image.

[0094] The application further provides an electronic device, characterized in that the electronic device includes at least one processor, a memory, and an input-output unit. The memory is used to store a computer program, and the processor is used to call the computer program stored in the memory to execute the positioning method for actively adjusting the field of view of visual-inertial angle fusion.

[0095] The above merely preferred embodiments of the present application and are not intended to limit the patent scope of the present application, any equivalent structure or equivalent process transformation made by using the content of the present application specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A positioning method of actively adjusting visual inertial angle fusion of field of view, characterized in that, The application relates to a method for determining the position of a UAV based on a target image. The method comprises the following steps: Real-time acquisition of current angle information of each frame of target image shot by a camera relative to a rotating shaft by using an angle encoder, extraction and tracking of feature points of the target image shot by a binocular camera, calculation of an external parameter rotation matrix of the feature points in each frame of target image calibrated by the binocular camera relative to an inertial IMU according to the current angle information; Based on the three-dimensional motion of the target image and the parameters measured by the inertial IMU, a local coordinate system with the initial position of the target image as the origin is established, and in the local coordinate system, the corresponding visual projection error vector and IMU pre-integration residual error vector are calculated based on the target image and the feature points and the external parameter rotation matrix respectively, a factor graph is constructed according to the visual projection error vector and the IMU pre-integration residual error vector, and the state quantity in the factor graph is solved based on a sliding window, the UAV position is determined based on the state quantity, and the high-frequency UAV body pose is obtained according to the IMU interpolation of the UAV position; 2. The method of claim 1, wherein the visual inertial angle fusion is adjusted on-the-fly. Adjustment of the current angle value of the camera to the target visual angle of the camera based on the feature point distribution of the target image. The current angle information comprises the rotating angle of each frame of target image; The calculation of the external parameter rotation matrix of the feature points in each frame of target image calibrated by the binocular camera relative to the inertial IMU according to the current angle information comprises the following steps: Respective acquisition of preset external parameter matrices of feature points in preset images shot by the binocular camera under two groups of different rotating angles relative to the inertial IMU; Conversion of the two groups of preset external parameter matrices into corresponding Lie algebras after the respective rotating angles are represented, and calculation of the rotating proportion of each feature point based on the rotating shaft angle of the target image and the rotating shaft angle of the preset image; The rotating shaft angle of the current image refers to the angle of the current image relative to the rotating shaft; Respective determination of two rotating quantities and two translation quantities of each feature point calibrated by each camera in the binocular camera according to the two groups of Lie algebras; Calculation of the left rotating quantity and the right rotating quantity of each feature point according to the two rotating quantities of each camera weighted by the rotating proportion, and calculation of the left translation quantity and the right translation quantity of each feature point according to the two translation quantities of each frame of image weighted by the rotating proportion; 3. The method of claim 2, wherein the visual inertial angle fusion is adjusted on-the-fly. Determination of the external parameter rotation matrix of each frame of image relative to the inertial IMU according to the left rotating quantity, the right rotating quantity, the left translation quantity and the right translation quantity of each feature point. The calculation of the rotating proportion of each feature point based on the rotating shaft angle of the target image and the rotating shaft angle of the preset image comprises the following steps:

4. The method of claim 2, wherein the visual inertial angle fusion is adjusted on-the-fly. Calculation of the rotating proportion according to the rotating shaft angle of the current image frame and the minimum rotating shaft angle and the maximum rotating shaft angle of the preset image frame. The cameras comprise a left camera and a right camera, the two rotating quantities of each feature point calibrated by the left camera comprise a left first rotating quantity and a left second rotating quantity, and the two translation quantities of each feature point calibrated by the left camera comprise a left first translation quantity and a left second translation quantity; the two rotating quantities of each feature point calibrated by the right camera comprise a right first rotating quantity and a right second rotating quantity, and the two translation quantities of each feature point calibrated by the right camera comprise a right first translation quantity and a right second translation quantity. The left rotation amount and the right rotation amount of each feature point are calculated according to the two rotation amounts of each camera weighted by the rotation proportion, and the left translation amount and the right translation amount of each feature point are calculated according to the two translation amounts of each frame image weighted by the rotation proportion, including: A first weighting factor is determined according to the difference between 1 and the rotation proportion, a second weighting factor is determined according to the rotation proportion, a left eye first rotation amount of each feature point weighted by the first weighting factor, and a left eye second rotation amount of each feature point weighted by the second weighting factor are summed to calculate the left rotation amount of each feature point in binocular camera calibration; A right eye first rotation amount of each feature point weighted by the first weighting factor, and a right eye second rotation amount of each feature point weighted by the second weighting factor are summed to calculate the right rotation amount of each feature point in binocular camera calibration; And a left eye first translation amount of each feature point weighted by the first weighting factor, and a left eye second translation amount of each feature point weighted by the second weighting factor are summed to calculate the left translation amount of each feature point in binocular camera calibration; A right eye first translation amount of each feature point weighted by the first weighting factor, and a right eye second translation amount of each feature point weighted by the second weighting factor are summed to calculate the right translation amount of each feature point in binocular camera calibration.

5. The method of claim 1, wherein, The local coordinate system with the initial position of the target image as the origin is established based on the three-dimensional motion of the target image and the parameters determined by the inertial IMU, including: Obtaining relative pose data of the target relative to the initial position based on the three-dimensional motion of the target image; Obtaining acceleration data of the inertial IMU accelerometer, bias data of the gyroscope, and gravity vector data of the gravimeter; Aligning the relative pose data, the acceleration data, the bias data of the gyroscope, and the gravity vector data of the gravimeter to obtain a local coordinate system with the initial position of the body as the origin.

6. The method of claim 1, wherein, The camera is arranged on the unmanned aerial vehicle body, and the visual projection error vector is calculated based on the feature points and the extrinsic rotation matrix of the target image, including: Determining a first subtracted term based on a first vector constructed based on the same feature points observed in different images; Determining a projection function and an inverse projection function of each feature point in the image based on the camera model; Determining a first transformation matrix and a first inverse transformation matrix based on the extrinsic transformation from the center of the unmanned aerial vehicle body to the center of the camera, Determining a second transformation matrix and a second inverse transformation matrix based on the pose of each frame image captured by the camera of the unmanned aerial vehicle in the local coordinate system; Determining a first product term based on the projection function, a second product term based on the product of the first inverse transformation matrix and the second inverse transformation matrix, the first transformation matrix, the second transformation matrix, and the inverse projection function, and a third product term based on each feature point and the first vector, Determining a first subtracted term based on the first product term, the second product term, and the third product term which are sequentially multiplied; Calculating the visual projection error vector based on the difference between the first subtracted term and the first subtracted term.

7. The method of claim 1, wherein, The process of calculating the IMU pre-integration residual error vector based on the feature points and the extrinsic rotation matrix of the target image, including: The relative position, the relative velocity, and the relative rotation amount at the previous moment obtained by pre-integrating the inertial IMU data; The errors of the position pre-integral quantity, the rotation integral quantity, the velocity integral quantity, the gyroscope zero offset and the accelerometer zero offset are respectively constructed based on the relative position, the relative velocity, the relative rotation quantity, the gyroscope zero offset and the accelerometer zero offset, and the IMU pre-integral residual error vector is constructed based on the errors of the position pre-integral quantity, the rotation integral quantity, the velocity integral quantity, the gyroscope zero offset and the accelerometer zero offset; The first subtractive term is determined based on the relative position change quantity, the first numerical term after being multiplied by the coordinate system rotation matrix is taken as the first minuend, and the error of the position pre-integral quantity is obtained according to the difference between the first minuend and the first subtractive term, wherein the first numerical term is obtained by subtracting the displacement at the adjacent time from the difference between the two first coordinate values at the adjacent time; The error of the rotation integral quantity is determined based on the imaginary part of the weighted quaternion term, wherein the quaternion term is determined based on the difference between the relative attitude transformation at the adjacent time and the product of the two rotation quaternions of the body coordinate system to the world coordinate system at the adjacent time and the quaternion of 1; The second minuend is determined based on the second numerical term after being multiplied by the error of the velocity integral quantity of the coordinate system rotation matrix, the second subtractive term is determined based on the data integral quantity at the adjacent time, and the error of the velocity integral quantity is determined according to the difference between the second minuend and the second subtractive term; The error of the accelerometer zero offset is obtained based on the difference between the accelerometer zero offsets at the adjacent time. The error of the gyroscope zero offset is obtained based on the difference between the gyroscope zero offsets at the adjacent time.

8. The method of claim 1, wherein, The method comprises the following steps: The image is divided into a plurality of equal-width regions to correspondingly construct a plurality of horizontal scanning regions; The number of feature points in each horizontal scanning region is counted, and the vertical distribution curve of the feature points in each horizontal scanning region is calculated, wherein the vertical distribution curve is determined by the sum of the products of the feature points and the indicator function, and the indicator function is used to determine whether the coordinate point belongs to the horizontal scanning region; The brightness of each horizontal scanning region is counted, and the brightness region with the highest comprehensive score is selected based on the preset brightness selection expression; The center coordinates of the brightness region are processed based on the preset pitch angle condition expression to obtain a target pitch angle adjustment angle; The servo driver of the binocular camera is adjusted according to the target pitch angle adjustment angle and the current angle, so that the binocular camera is turned to the target angle.

9. A positioning device that actively adjusts visual inertial angle fusion of field of view, characterized in that, The method comprises the following steps: The input module is used to acquire the current angle information of each frame of target image relative to the rotation axis by using the angle encoder in real time, extract and track the feature points of the target image shot by the binocular camera, and calculate the external parameter rotation matrix of the feature points in each frame of target image shot by the binocular camera relative to the inertial IMU according to the current angle information. The fusion processing module is configured to establish a local coordinate system with an initial position of the target image as an origin based on three-dimensional motion of the target image and parameters determined by the inertial IMU, to calculate a corresponding visual projection error vector and an IMU pre-integration residual error vector based on the target image under the local coordinate system, to construct a factor graph according to the visual projection error vector and the IMU pre-integration residual error vector, to solve state quantities in the factor graph based on a sliding window, to determine a UAV body position based on the state quantities, and to obtain a high-frequency UAV body pose according to IMU interpolation of the UAV body position. The adjusting module is configured to adjust a current angle value of the camera to a target view angle of the camera based on a feature point distribution of the target image.

10. An electronic device, comprising: The electronic device includes: at least one processor, a memory, and an input / output unit; wherein the memory is configured to store a computer program, and the processor is configured to invoke the computer program stored in the memory to execute the positioning method of the active adjustment of the field of view according to the visual-inertial angle fusion of claim 1-8.

Images