A positioning method of visual inertial angle fusion with field of view actively adjusted

Abstract

The positioning method, device, medium and equipment for actively adjusting the field of view of visual inertial angle fusion are provided, the feature point relative inertial IMU external parameter rotation matrix in each frame target image of binocular camera calibration is calculated according to 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, the corresponding visual projection error vector and the 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, and the unmanned aerial vehicle position and the high-frequency unmanned aerial vehicle body pose are determined based on the state quantity; according to the distribution of the feature points in the camera field of view, the camera direction is automatically adjusted, and the positioning method for actively adjusting the field of view of visual inertial angle fusion is solved.

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 a 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 a corresponding vision projection error vector and an IMU pre-integration residual error vector based on the target image under the local coordinate system, respectively; constructing a factor graph according to the vision projection error vector and the IMU pre-integration 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 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; and 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 an 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 a left rotation amount and a right rotation amount of each feature point according to the two rotation amounts of each camera weighted by the rotation ratio, and calculating a left translation amount and a right translation amount 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 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.

[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:

[0008] calculating a left external parameter matrix and a right external parameter matrix of the target image according to the rotation ratio and the left external parameter matrix and the right external parameter matrix of the preset image frame.

[0009] 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.

[0010] 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.

[0011] 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.

[0012] 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.

[0013] 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.

[0014] 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.

[0015] 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.

[0016] The positioning method, device, medium and equipment for actively adjusting the field of view of visual inertial angle fusion provided by the embodiment of the application, through real-time acquisition of current angle information of each frame of target image shot by the camera relative to the rotation axis by using an angle encoder, feature point extraction and tracking of the target image shot by the binocular camera are performed; the current angle information is used to calculate the external parameter rotation matrix of the feature point 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 determined 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 the 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, the best orientation is sent to the servo driver to adjust the camera to the best orientation, the observation quality of the feature point is ensured, and the robustness of the positioning method in the dynamic scene is improved, in addition, the angle encoder is used to real-time acquire the angle of the camera relative to the rotation axis, the external parameter of the camera relative to the IMU corresponding to each frame of image 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 problem caused by the decline of the feature point quality in the camera under part of the motion attitude is solved. BRIEF DESCRIPTION OF DRAWINGS

[0017] Figure 1 The flowchart provided by the embodiment of the active field of view adjustment visual inertial angle fusion positioning method of the application;

[0018] Figure 2 The invention idea diagram provided by the embodiment of the active field of view adjustment visual inertial angle fusion positioning method of the application;

[0019] Figure 3 The binocular camera diagram provided by the embodiment of the active field of view adjustment visual inertial angle fusion positioning method of the application;

[0020] Figure 4 The rotation diagram provided by the embodiment of the active field of view adjustment visual inertial angle fusion positioning method of the application;

[0021] Figure 5The structural block diagram of an embodiment of a positioning device of active field of view adjustment visual-inertial angular fusion of the application is provided.

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

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

[0024] The data input module is used to acquire 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 rotation axis, meanwhile, the closest angle according to the time stamp is matched for each frame of image, and 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 of only 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 visual 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 of each frame of image in the field of view, calculates the best orientation of the camera, and sends the best orientation to the servo driver to adjust the turning of 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.

[0025] Referring to Figure 1 and Figure 2 , Figure 1 The flowchart of the positioning method of active field of view adjustment visual-inertial angular fusion of the first embodiment of the application is provided, Figure 2 The idea of the 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 pre-integration of the inertial IMU data is performed, 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 of only 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 of the image in the field of view, and the servo driver is driven to make the camera face the best orientation.

[0026] Figure 3The device schematic diagram of the application is described, wherein 1 is a binocular camera, 2 is a camera rotating bracket, 3 is a motor, 4 is an encoder and a servo driver, 5 is a fixed support, the motor can drive the camera to rotate up and down (as shown in Figure 4 The encoder can output the rotation angle of the camera around the rotation shaft in real time, the camera picture, the encoder, the servo driver are connected with the computer through a cable, the picture 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 active adjustment field of view visual inertial angle fusion provided by the first embodiment of the application, which can be executed by a processor, can be arranged in a host or a server. The positioning method of the active adjustment field of view visual inertial angle fusion can include the following steps:

[0027] S10, real-time acquisition of current angle information of each frame of target image shot by the camera relative to the rotation shaft is realized by using an angle encoder, feature point extraction and tracking of the target image shot by the binocular camera are performed, and a feature point relative inertial IMU external parameter rotation matrix in each frame of target image calibrated by the binocular camera is calculated according to the current angle information.

[0028] In an embodiment of the application, the specific execution process of step S10 can include the following steps:

[0029] S101, the current angle information includes the rotation angle of each frame of target image.

[0030] S102, the calculation of the feature point relative inertial IMU external parameter rotation matrix in each frame of target image calibrated by the binocular camera according to the current angle information can include the following steps:

[0031] S103, the preset external parameter matrix of the feature point relative inertial IMU in the preset image shot by the binocular camera under two groups of different preset rotation angles is respectively acquired.

[0032] S104, after the two groups of preset external parameter matrices are respectively expressed by each rotation angle, the corresponding Lie algebra is converted, and the rotation scale of each feature point is calculated based on the rotation shaft angle of the target image and the rotation shaft angle of the preset image.

[0033] In an embodiment of the application, the process of calculating the rotation scale of each feature point based on the rotation shaft angle of the target image and the rotation shaft angle of the preset image can include the following steps:

[0034] According to the rotation shaft angle of the current image frame and the minimum rotation shaft angle and the maximum rotation shaft angle of the preset image frame, the rotation scale is calculated.

[0035] It should be noted that the rotation ratio can be calculated to further calculate the external parameter of the camera relative to the inertial IMU. According to the external parameter of the camera relative to the inertial IMU, the visual projection error vector and the IMU pre-integration residual error vector can be calculated.

[0036] Specifically, the processor reads the timestamp of the image frame after obtaining each image frame, and finds the angle value closest to the same time from the historical data frame of the encoder as the angle of the camera relative to the rotation axis at the image shooting time according to the image timestamp. Next, the processor uses the two sets of camera determined by 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, and 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 :

[0037]

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

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

[0040] S105, according to the two sets of Lie algebra respectively, determining two rotation amounts and two translation amounts of each feature point of each camera calibration in the binocular camera.

[0041] S106, 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 ratio, 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 ratio.

[0042] S107, determining 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 of each feature point, and the left translation amount and the right translation amount.

[0043] In an embodiment of the present application, 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, 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.

[0044] 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:

[0045] The first weighting factor is determined according to the difference between 1 and the rotation proportion, the second weighting factor is determined according to the rotation proportion, 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 are summed to calculate the left rotation amount of each feature point calibrated by the binocular camera.

[0046] 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 are summed to calculate the right rotation amount of each feature point calibrated by the binocular camera.

[0047] 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 are summed to calculate the left translation amount of each feature point calibrated by the binocular camera.

[0048] 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 are summed to calculate the right translation amount of each feature point calibrated by the binocular camera.

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

[0050]

[0051] wherein, , the left-eye first rotation amount and the left-eye second rotation amount in the two rotation amounts calibrated by the left-eye camera, , the right-eye first rotation amount and the right-eye second rotation amount in the two rotation amounts calibrated by the right-eye camera, , the left-eye first translation amount and the left-eye second translation amount in the two translation amounts calibrated by the left-eye camera, , A first translation amount and a second translation amount of the two translation amounts of the right eye relative to the target, t and 1- t are respectively a second weighting factor and a first weighting factor.

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

[0053]

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

[0055] The specific calculation process can include the following:

[0056] First, the 3-dimensional vector is converted into an anti-symmetric matrix .

[0057] wherein:

[0058]

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

[0060]

[0061] wherein, is an identity matrix.

[0062] 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 the 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 UAV body pose according to the IMU interpolation of the UAV position.

[0063] 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:

[0064] The three-dimensional motion of the target image obtains the relative pose data of the target relative to the initial position.

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

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

[0067] 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. The process can be: the processor performs initialization, obtains the initial relative pose of the carrier relative to the initial position using only visual three-dimensional motion reconstruction, performs visual IMU joint initialization to obtain the alignment of the biases of the IMU accelerometer and gyroscope and the gravity vector, and establishes a local coordinate system with the initial position of the body as the origin.

[0068] It should be noted that the processor can calculate the visual re-projection error and the IMU pre-integration residual, and construct a factor graph based on the calculated visual re-projection error and the IMU pre-integration residual as factors. Then, the processor constructs a sliding window to all state quantities in the factor graph and optimizes and solves, wherein, is the number of frames in the sliding window, is the number of all feature points in the sliding window, , , , is the position, velocity and attitude of the carrier, , is the bias of the accelerometer and gyroscope, is the external parameter corresponding to each frame of state quantity, and the specific form is wherein 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 camera through the state quantity, wherein the camera refers to the camera pose of the unmanned aerial vehicle, and then the processor can perform IMU difference on the camera pose to obtain the high-frequency body pose.

[0069] Wherein, the camera is arranged on the body of the unmanned aerial vehicle, and the process of calculating the IMU pre-integration residual vector based on the target image includes the following:

[0070] The relative position, relative velocity and relative rotation quantity of the previous moment obtained by pre-integrating the inertial IMU data.

[0071] Based on the relative position, the relative speed, the relative rotation amount, the gyroscope zero offset, the accelerometer zero offset respectively correspond to construct the error of the position pre-integral quantity, the error of the rotation integral quantity, the error of the speed integral quantity, the error of the gyroscope zero offset and the error of the accelerometer zero offset, and based on the error of the position pre-integral quantity, the error of the rotation integral quantity, the error of the speed integral quantity, the error of the gyroscope zero offset and the error of the accelerometer zero offset, the IMU pre-integral residual error vector is constructed.

[0072] Wherein, the first subtractive term is determined based on the relative position change amount, the first numerical term 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. The first numerical term is based on the difference between the two first coordinate values at adjacent time, and the displacement at adjacent time is subtracted.

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

[0074] The second numerical term multiplied by the error of the coordinate system rotation matrix velocity integral quantity is taken as the second minuend, the second subtractive term is determined based on the data integral quantity at adjacent time, and the error of the speed integral quantity is determined according to the difference between the second minuend and the second subtractive term.

[0075] Based on the difference value of the accelerometer zero offset at adjacent time, the error of the accelerometer zero offset is obtained.

[0076] Based on the difference value of the gyroscope zero offset at adjacent time, the error of the gyroscope zero offset is obtained.

[0077] Exemplarily, the camera factor is established: considering the first observed lth feature point in the image P , P From the first time it is seen in the jth camera coordinate system to the kth camera coordinate system, its observation residual error in the subsequent image is defined as: i j

[0078]

[0079] Wherein is the coordinate of the jth feature point observed in the jth frame of the camera normalized camera coordinate system:

[0080]

[0081] is the estimated jth landmark point in the kth frame of the camera coordinate system: l j ​​​​Projected coordinates in the normalized camera coordinate system of the frame camera:

[0082]

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

[0084]

[0085] , For unit ball and Any two orthogonal bases of intersecting tangent planes. It is the first l The inverse depth of a feature point in the i-th frame, It is the actual depth value at that point. It is the first i 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.

[0086] In an embodiment of the present application, the execution process of the process of calculating the feature point and the external parameter rotation matrix based on the target image to obtain the IMU pre-integration residual vector can include the following execution process shown in the following:

[0087] The first to third elements of the second subtrahend of the IMU pre-integration residual vector are constructed based on the relative position, the relative velocity and the relative rotation obtained by pre-integrating the inertial IMU data at the previous time, and the fourth to fifth elements of the second subtrahend are set to zero.

[0088] The first element of the second subtrahend is determined according to the difference between the inertial IMU current observation value and other observation values.

[0089] The multiplier is determined according to the sum of the product of the difference between the adjacent time velocities and the integral of the gravitational acceleration.

[0090] The multiplier is determined according to the inverse of the rotation matrix at the previous time, and the second element of the second subtrahend is determined according to the product of the multiplier and the multiplier.

[0091] The third element of the second subtrahend is determined according to the product of the inverse of the rotation matrix at the previous time and the rotation matrix at the current time.

[0092] The fourth element of the second subtrahend is determined according to the difference between the adjacent time acceleration data obtained by the accelerometer.

[0093] The fifth element of the second subtrahend is determined according to the difference between the adjacent time active adjustment field of view of the visual inertial angle fusion positioning method.

[0094] The IMU pre-integration residual vector factor is determined by the difference between the second subtrahend and the second subtrahend.

[0095] In the specific execution process, the processor assumes that the additional noise in the accelerometer and gyroscope measurement is Gaussian white noise, and the time-varying accelerometer and gyroscope bias is modeled as a random walk process, and the derivative 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.

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

[0097]

[0098] The error of the pre-integral quantity represents the position. Represents the error of the rotational integral. The error representing the integral of velocity, This represents the error representing the zero bias of the accelerometer. This represents the error of the gyroscope's zero bias. It is the coordinate system rotation matrix from the world coordinate system to the body coordinate system. It is the first coordinate value of the body coordinate system in the local coordinate system. and Refers to different times, It is the position of the origin of the body coordinate system in the local coordinate system. The gravity vector in the local coordinate system is denoted as [ 0, 0, -9.8 ] ^T， From k Time's up k The time interval at +1, It was obtained through IMU pre-integration, from k arrive k The change in relative position at time +1 From k arrive k Relative attitude change at time +1 and yes k and k At time +1, the rotation quaternion from the body coordinate system to the world coordinate system. It extracts the imaginary part of the quaternion terms. x , y , z Quantity, and yes k +1 time and k The velocity of the machine body in the local coordinate system at any given moment. Obtained through IMU pre-integration k Time's up k The change in relative velocity at time +1 This is the zero bias of the accelerometer, a three-dimensional vector that describes the systematic error in accelerometer measurements. It is the zero bias of the gyroscope, a three-dimensional vector that describes the systematic error of the gyroscope measurement.

[0099] Add these two factors to the optimization terms to establish an optimization model:

[0100]

[0101] in, representing all a set of sensor measurement information.

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

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

[0104] In an embodiment of the present application, the process of adjusting the current angle value of the camera to the target view angle based on the feature point distribution of the target image can include the following:

[0105] Each frame of image is divided into a plurality of equal-width regions to correspond to the construction of a plurality of horizontal scanning regions.

[0106] 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 each feature point and an indicator function, and the indicator function is used to determine whether the coordinate point belongs to the horizontal scanning region.

[0107] The brightness of each horizontal scanning region is counted, and the brightness region with the highest comprehensive score is selected based on a preset brightness selection expression.

[0108] The center coordinates of the brightness region are processed based on a preset pitch angle condition expression to obtain a target pitch angle adjustment angle.

[0109] 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.

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

[0111]

[0112] wherein, h is the pixel height of the image.

[0113] 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 :

[0114]

[0115] wherein, is an indicator function, which equals 1 if the feature point coordinate belongs to , otherwise 0.

[0116] Next, the processor can perform brightness statistics, and calculate the average brightness for each region .

[0117] Then, the processor can perform candidate region screening, requiring to exclude noise interference, and requiring to exclude dark and bright regions. In the candidate region, the expression of the region with the highest comprehensive score is:

[0118]

[0119] wherein, and are the maximum values of the feature point number and the brightness of all regions, and are weight coefficients for balancing the reference degree of the feature points and the brightness.

[0120] Finally, the processor can calculate the target angle adjustment amount, and calculate the target pitch angle adjustment amount according to the center coordinate of the target region. The expression of is:

[0121]

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

[0123] Referring to Figure 5On the basis of the method embodiments, the application further provides a positioning device for active adjustment of a visual inertial angle fusion field of view, which can comprise an input module 201, a fusion processing module 202 and an adjustment module 203. The input module 201 is configured to acquire current angle information of each frame of target image shot by a camera relative to a rotating shaft in real time by using an angle encoder, and extract and track feature points of the target image shot by a binocular camera. A feature point rotation matrix relative to an inertial IMU in each frame of target image shot by the binocular camera is calculated according to the current angle information. The fusion processing module 202 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. In the local coordinate system, a corresponding visual projection error vector and an IMU pre-integral residual error vector are calculated based on the feature points and the feature point rotation matrix of the target image, respectively. A factor graph is constructed according to the visual projection error vector and the IMU pre-integral residual error vector, and state quantities in the factor graph are solved based on a sliding window. A UAV position is determined based on the state quantities, and a high-frequency UAV body pose is obtained according to the IMU interpolation of the UAV position. The adjustment module 203 is configured to adjust the current angle value of the camera to a target visual angle based on the distribution of the feature points of the target image.

[0124] The application further provides an electronic device, which comprises at least one processor, a memory and an input-output unit. The memory is configured to store a computer program, and the processor is configured to call the computer program stored in the memory to execute the positioning method for active adjustment of a visual inertial angle fusion field of view.

[0125] The above is only a preferred embodiment of the application, and does not limit the patent scope of the application. Any equivalent structure or equivalent process transformation, or direct or indirect application in other related technical fields, which is based on the content of the specification and drawings, is also included in the patent protection scope of the 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