A 6-DOF relative pose estimation system based on multi-sensor fusion

Abstract

The present invention discloses a 6-DOF relative pose estimation system based on multi-sensor fusion, which is applied to robot B and includes an infrared LED module, an ultra-wideband (UWB) module, an infrared fisheye camera, an inertial measurement unit (IMU) module and a processor: the infrared LED module is used to encode its own ID through the duty cycle of LED flashing; the infrared fisheye camera is used to capture its own field of view image; the UWB module is used to provide the relative distance from the robot; the IMU module is used to obtain its own 3-axis acceleration, 3-axis angular velocity and gravity direction; the processor is used to use the field of view image to identify the ID and pixel coordinates of robot A, perform preliminary relative pose solution according to the pixel coordinates, relative distance and gravity direction, and filter the calculated preliminary relative pose based on the 3-axis acceleration and 3-axis angular velocity using an improved error state Kalman filter, thereby realizing relative pose estimation.

Description

Technical Field

[0001] The present application relates to the technical field of multi-robot relative pose estimation in the field of robotics, and in particular to a 6-DOF relative pose estimation system based on multi-sensor fusion. Background Art

[0002] In recent years, multi-robot systems (MRS) have attracted increasing attention in areas such as collaborative mapping, collaborative exploration, monitoring, and search and rescue. In an efficient multi-robot system, relative pose estimation is often the key to completing collaborative tasks. A fast, stable, and accurate relative pose estimation system can greatly improve the efficiency of collaborative work.

[0003] Currently, a common method for obtaining the relative pose between robots is to use a global positioning system, such as satellite GPS, motion capture systems, and ultra-wideband ranging systems, to obtain global odometry information for each robot, and then further calculate the relative pose between the robots. However, this system often requires pre-built equipment, making it difficult to apply in scenarios with unknown environments. Alternatively, each robot can use a real-time positioning and mapping system to obtain its own odometry information, and then determine the relative pose between the robots by matching the same environmental features. However, this method typically consumes a lot of computing power and will degrade in environments with fewer environmental features. Some other methods use labels such as AprilTags or LEDs to the robots and use camera recognition to determine the relative pose between the robots. However, these methods are severely constrained by the robots' field of view and the distance between them, and typically operate at relatively short distances.

[0004] Currently, the methods for solving the relative posture estimation problem between robots can be divided into methods based on some positioning systems (such as satellites, motion capture systems, real-time positioning and mapping systems, etc.), methods based on multi-frame relative distance observations, and some active tag-based methods, such as installing ApriTags or LED tags on robots.

[0005] Solutions based on global positioning systems, such as those based on ultra-wideband ranging base station models and motion capture systems, obtain the pose of each robot in the same reference frame and then, through a few simple transformations, determine the relative pose between the robots. However, this approach typically requires the installation of positioning sensors in the environment in advance, which is difficult to implement in many practical application scenarios. Solutions based on real-time positioning and mapping systems allow each robot to obtain odometry information in its own reference frame in real time. By identifying some common environmental or other features, the transformation relationship between the respective reference frames can be estimated, and the relative pose between the robots can be further calculated. In "Y. Cao and G. Beltrame, "Vir-slam: Visual, inertial, and ranging slam for single and multi-robot systems," Autonomous Robots, vol. 45, no. 6, pp. 905–917, 2021," Cao et al. proposed an effective relative pose estimation scheme by fusing visual inertial odometry and ultra-wideband ranging to measure the distance between the robot and fixed anchor points in the environment. In "Omni-swarm: A decentralized omnidirectional visual-inertial-UWB stateestimation system for aerial swarm," CoRR, vol. abs / 2103.04131, 2021, Xu et al. proposed a global pose graph optimization algorithm for relative pose estimation, leveraging information from an omnidirectional visual-inertial positioning system and ultra-wideband ranging between robots. However, these solutions rely on visual-inertial positioning systems, requiring significant computational resources and easily degrading in environments with few environmental features.

[0006] To reduce dependence on environmental features, Trawny et al. (N. Trawny, X. S. Zhou, K. X. Zhou, and SI Roumeliotis, “3D relative pose estimation from distance-only measurements,” in the 2007 IEEE / RSJ International Conference on Intelligent Robots and Systems, 2007, pp. 1071–1078) first proposed an efficient algebraic algorithm for solving the relative pose of a robot using 10 distance measurements without imposing any constraints on the robot's motion. Zhou et al. (X. S. Zhou and SI Roumeliotis, “Robot-to-robot relative pose estimation from range measurements,” IEEE Transactions on Robotics, vol. 24, no. 6, pp. 1379–1393, 2008) studied a 3-DOF robot and analyzed the number of possible relative pose solutions corresponding to different numbers of distance measurements. However, this relative pose estimation method based on distance measurements requires sufficient motion excitation in practical applications and cannot estimate the relative pose of relatively stationary robots.

[0007] Some studies have used active light-emitting diodes (LEDs) to estimate the relative pose between robots. For example, Faessler et al. (M.Faessler, E.Mueggler, K.Schwabe, and D.Scaramuzza, “A monocular poseestimation system based on infrared LEDs,” in 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014, pp. 907–913) used four infrared LEDs placed on the robot in a certain mounting structure and used the PnP algorithm to calculate the relative pose between the quadrotor robot and the ground robot. When it comes to multi-robot scenarios, active markers (D.Dias, R.Ventura, P.Lima, and A.Martinoli, “On-board visionbased 3D relative localization system for multiple quadrotors,” in 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016, pp.1181–1187) or active LED coding boards (X.Yan, H.Deng, and Q.Quan, “Active infrared coded target design and pose estimation for multiple objects,” in 2019 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp.6885–6890) are designed to encode the robot’s ID information by LED flashing frequency or LED arrangement sequence, and then combine with the PnP algorithm to complete the relative pose estimation. However, since the PnP algorithm requires the pixel coordinates of each LED on the image, these methods can only work within a limited distance to ensure that the light points of each LED do not overlap on the image plane. Summary of the Invention

[0008] In view of the shortcomings of the existing technology, the purpose of the embodiments of the present application is to provide a 6-DOF relative pose estimation system based on multi-sensor fusion.

[0009] According to a first aspect of an embodiment of the present application, a 6-DOF relative pose estimation system based on multi-sensor fusion is provided, which is applied to a robot B, an infrared LED module, a UWB module, an infrared fisheye camera, an IMU module, and a processor:

[0010] The infrared LED module is used to encode its own ID through the duty cycle of LED flashing;

[0011] The infrared fisheye camera is used to capture its own field of view image;

[0012] The UWB module is used to provide a relative distance from the robot;

[0013] The IMU module is used to obtain its own 3-axis acceleration, 3-axis angular velocity and gravity direction.

[0014] The processor is used to use the field of view image to identify the ID and pixel coordinates of robot A, perform preliminary relative pose calculation based on the pixel coordinates, relative distance and gravity direction, and use the three-axis acceleration and three-axis angular velocity to filter the calculated preliminary relative pose based on the improved error state Kalman filter, thereby realizing relative pose estimation.

[0015] Furthermore, the infrared LED module is a circular light board composed of a number of infrared LEDs.

[0016] Furthermore, in the processor, using the field of view image to identify the ID and pixel coordinates of the robot A includes:

[0017] Convert the field of view image into a binary image with a threshold, perform circle detection, and obtain the pixel coordinates of the center of the detection point, which are the pixel coordinates of robot A;

[0018] According to the distance constraint, the points detected in the next frame are associated with the points detected in the previous frames, and their duty cycle is calculated;

[0019] The ID of robot A is determined by comparing the calculated duty cycle with data in an ID library, where the IDs of all robots are stored.

[0020] Furthermore, in the processor, preliminary relative posture calculation is performed according to the pixel coordinates, relative distance and gravity direction, including:

[0021] According to the pixel coordinates combined with the fisheye camera model, the three-dimensional direction vector of robot A in the robot B coordinate system is obtained. B p uA ;

[0022] According to the direction vector B p uAand the relative distance d AB , calculate the relative position of robot A in the coordinate system of robot B;

[0023] Calculating the roll angle and pitch angle of the robot B according to the direction of gravity;

[0024] Using the rotation matrix of the roll angle and pitch angle and Get the intermediate reference coordinate system of the robot B coordinate system

[0025] The direction vector B p uA Transform to the intermediate reference coordinate system Downward direction vector

[0026] The direction vector Projection to the intermediate reference coordinate system On the XOY plane, the angle between the projection vector and the positive direction of the x-axis is obtained Combined with the angle received from the robot A Get the intermediate reference coordinate system of the robot A coordinate system The intermediate reference coordinate system between the robot's B coordinate system The relative side heading angle ψ between them;

[0027] Using the rotation matrix of the roll angle and pitch angle of robot B and The rotation matrix corresponding to the side heading angle ψ, combined with the rotation matrix of the roll angle and pitch angle received from the robot A and Get the rotation matrix of robot A relative to robot B

[0028] Furthermore, information is received from the robot A via WiFi.

[0029] Furthermore, in the processor, filtering the calculated preliminary relative pose based on an improved error state Kalman filter includes:

[0030] 1) Prediction model:

[0031] Get the position, velocity and rotation quaternion of the robot κ in the fixed reference frame w w p k , w v k , w q k , where k represents robot A or B;

[0032] The relative pose between robots A and B is calculated using the following formula:

[0033] p=R T { w q B}( w p A - w p B )

[0034] v=R T { w q B}( w v A - w v B )

[0035]

[0036] in express The corresponding rotation matrix, Represents quaternion or rotation angle, R T represents the inverse matrix of the rotation matrix R, B q A is the rotation matrix B R A The corresponding quaternion, q * represents the conjugate quaternion of the four-element q;

[0037] Using the relative position between robots A and B, the nominal state x and the actual state x of the system are calculated. t , error state δx:

[0038]

[0039] where δθ k is the local angle error corresponding to the robot k quaternion, The relationship between the true state, the nominal state and the error state is:

[0040]

[0041] p t =R T {δθ B}(p+δp)

[0042] v t =R T {δθ B}(v+δv)

[0043]

[0044] According to the measured value a of the robot's A and B3 axis acceleration mA , a mB , the measured value of the 3-axis angular velocity w mA , w mB and the measurement noise of the 3-axis acceleration a nA , a nB , the measurement noise of the three-axis angular velocity w nA , w nB , where the measurement noise of acceleration and angular velocity conforms to the Gaussian distribution with zero mean, and the random walk error of IMU zero bias is ignored. The recursive equation of the nominal state of the relative posture estimation system between robots A and B is obtained as follows:

[0045] p←R T {w mB Δt}(p+vΔt+0.5(R{q}a mA -a mB )Δt 2 )

[0046] v←R T {w mB Δt}(v+(R{q}a mA -a mB )Δt)

[0047]

[0048] Where ← represents a discrete time update and Δt represents a discrete time interval;

[0049] The difference equation of the error state is obtained as follows:

[0050] δx-f(x,δx,u m ,u n )=F x (x,u m )+F i u n

[0051] δp←R T {w mB Δt}(δp+δvΔt)

[0052] δv←R T {w mB Δt}(δv+αΔt)

[0053] δθ A ←R T {w mA Δt}δθ A -w nA Δt

[0054] δθB ←R T {w mB Δt}δθ B -w nB Δt

[0055] where α = -R{q}[a mA ]×δθ A +[a mB ]×δθ B -R{q}a n A+a nB , Represents a vector The corresponding cross product matrix, F x and F i is the error state δx and noise u n The corresponding Jacobian matrix is ​​calculated as follows:

[0056]

[0057] Then construct the formula for prediction update:

[0058]

[0059]

[0060] in Q i Indicates u n The corresponding covariance matrix;

[0061] 2) Measurement update:

[0062] The result of the preliminary relative pose solution is used as the observation value z of the relative pose,

[0063]

[0064] in is the rotation matrix The corresponding quaternion, observation value and true state satisfy the following observation equation:

[0065] z=h(x t )+ε

[0066]

[0067]

[0068]

[0069] in Estimate of the true state Able to pass Calculated, due to the error state estimation Therefore, there is The Jacobian matrix H of the observation equation between robots A and B is:

[0070]

[0071] The update equation of the improved error state Kalman filter is:

[0072] K=PH T (HPH T +V) -1

[0073]

[0074] P←(I-KH)P

[0075] 3) Adding and resetting error status:

[0076] The calculated error state is added to the nominal state, and the calculation formula is:

[0077]

[0078] After the error state is added to the nominal state, the error state is reset to 0 and the covariance matrix P is updated, where the error state reset equation is:

[0079]

[0080]

[0081]

[0082]

[0083]

[0084] According to the Jacobian matrix G corresponding to the error state reset equation, update the estimated value of the error state And the corresponding covariance matrix P:

[0085]

[0086] P←GPG T

[0087] in

[0088] Furthermore, if there are three or more robots in the scene, in the processor, after using the three-axis acceleration and three-axis angular velocity to filter the calculated preliminary relative pose based on the improved error state Kalman filter, it also includes using a pose graph optimization algorithm to further optimize the filtering result.

[0089] Furthermore, the pose graph optimization algorithm is used to further optimize the filtering results. The pose optimization problem is expressed as

[0090]

[0091] Among them, the relative pose between robot i and robot j output by the error state Kalman filter is The position of robot i relative to reference frame B is represented by X i =(R i , t i )∈SE(3), O represents the set of robots, L represents the edges of mutual observation between robots, ρ is the kernel function, is defined as follows:

[0092]

[0093] The technical solutions provided by the embodiments of the present application may have the following beneficial effects:

[0094] 1. This paper provides a complete system for relative pose estimation, capable of performing six-degree-of-freedom (3 rotational and 3 translational) relative pose estimation between two or more robots. In multiple experiments, the median translational error of this relative pose estimation system was approximately 0.125 meters, and the median rotational error was approximately 1.281°.

[0095] 2. This invention provides a hardware solution for relative pose estimation based on infrared LEDs, an infrared fisheye camera, an ultra-wideband ranging sensor, and an inertial navigation module. Previous work using infrared LEDs combined with the PnP algorithm for relative pose estimation required precise identification of each light in the image, limiting the distance between two robots to less than 7 meters. However, this system only requires identifying the combined light spot of all lights in the image, significantly extending the effective working distance to a maximum of 27.5 meters.

[0096] 3. The pose estimation algorithms implemented in this invention based on the hardware device include: a direct relative pose solution algorithm, an improved error-state Kalman filter algorithm, and a pose graph optimization algorithm based on this device. This set of algorithms enables the relative pose between robots to be determined within the hardware system we designed. Furthermore, the filtering and optimization algorithms mitigate the relative pose error caused by noise from the original hardware sensor measurements, improving the system's estimation accuracy.

[0097] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0098] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0099] Figure 1 This is a schematic diagram of a 6-DOF relative pose estimation system based on multi-sensor fusion according to an exemplary embodiment, wherein (a) is the hardware composition and (b) is the algorithm block diagram.

[0100] Figure 2 is a flowchart of the initial relative pose solution according to an exemplary embodiment, wherein (a) is the relative orientation solution of robots A and B, (b) is the representation of the two direction vectors in (a) in a gravity-aligned coordinate system, and (c) is the projection of the two direction vectors in (b) on the XOY plane.

[0101] Figure 3 3 is a schematic diagram of relative posture optimization of multiple robots according to an exemplary embodiment.

[0102] Figure 4 Figure 3 is a schematic diagram of an experimental scenario and trajectory for relative pose estimation between two UAVs according to an exemplary embodiment, where (a) is the UAV platform and experimental environment used in the experiment, and (b), (c), and (d) are the flight trajectories of UAV 1 calculated based on the estimation results of our system when UAV 0 autonomously executes different trajectories.

[0103] Figure 5 : This figure shows the experimental error of relative pose estimation of two UAVs according to an exemplary embodiment, where (a) is the error of the relative translation estimation result of this system, and (b) is the error of the relative rotation estimation result of this system.

[0104] Figure 6Figure 1 is a schematic diagram of system feature verification according to an exemplary embodiment, wherein (a) is a display of an experimental scenario for an outdoor long-distance experiment, and (b) is a comparison of the trajectory of the UAV obtained using RTK (true trajectory value) and the trajectory of the UAV calculated using the estimation results of this system (estimated trajectory value). DETAILED DESCRIPTION

[0105] Exemplary embodiments are described in detail herein, with examples illustrated in the accompanying drawings. When the following description refers to the drawings, identical numerals in different drawings represent identical or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments are not intended to represent all embodiments consistent with this application.

[0106] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. As used in this application and the appended claims, the singular forms "a," "an," "the," and "the" are intended to include the plural forms, unless the context clearly indicates otherwise. It should also be understood that the term "and / or" as used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items.

[0107] It should be understood that although the terms first, second, third, etc. may be used in this application to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from each other. For example, without departing from the scope of this application, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0108] The following description will be made using the system on robot B as an example, with robot B serving as the observer (reference system of relative posture).

[0109] Figure 1 FIG is a schematic diagram of a 6-DOF relative pose estimation system based on multi-sensor fusion according to an exemplary embodiment. Figure 1As shown in (a) and (b), the system may include an infrared LED module, a UWB module, an infrared fisheye camera, an IMU module and a processor: the infrared LED module is used to encode its own ID through the duty cycle of the LED flashing; the infrared fisheye camera is used to capture its own field of view image; the UWB module is used to provide the relative distance from the robot; the IMU module is used to obtain its own 3-axis acceleration, 3-axis angular velocity and gravity direction; the processor is used to use the field of view image to identify the ID and pixel coordinates of robot A, perform preliminary relative pose solution according to the pixel coordinates, relative distance and gravity direction, and use the 3-axis acceleration and 3-axis angular velocity to filter the calculated preliminary relative pose based on the improved error state Kalman filter, thereby realizing relative pose estimation.

[0110] As can be seen from the above embodiments, this application provides a complete system for relative pose estimation, which can perform relative pose estimation with six degrees of freedom (3 rotational degrees of freedom and 3 translational degrees of freedom) between two or more robots. It also provides a hardware solution for relative pose estimation based on infrared LEDs, infrared fisheye cameras, ultra-wideband ranging sensors, and inertial navigation modules. Previous work using infrared LEDs combined with the PnP algorithm for relative pose estimation required accurate identification of each light in the image, so the distance between two robots generally could not exceed 7 meters. However, this system only needs to identify the light spot composed of all lights in the image, greatly improving the effective working distance, with a maximum effective distance of 27.5 meters. The hardware device also implements related pose estimation algorithms: a direct relative pose solution algorithm, an improved error state Kalman filter algorithm, and a pose graph optimization algorithm based on this device. Through this set of algorithms, the relative pose between robots can be solved from the hardware system designed by us. Moreover, due to the existence of the filtering algorithm and the optimization algorithm, the error caused by the original hardware sensor measurement noise on the relative pose is suppressed to a certain extent, improving the estimation accuracy of the system.

[0111] Specifically, the infrared LED module is a circular light board composed of a number of infrared LEDs, the infrared fisheye camera is an infrared fisheye camera with a field of view of 185°, and the infrared fisheye camera is equipped with a filter corresponding to the wavelength band of the infrared LED.

[0112] In the processor, using the field of view image to identify the ID and pixel coordinates of the robot A may include the following steps:

[0113] S11: converting the field of view image into a binary image with a threshold, and performing circle detection to obtain the pixel coordinates of the detection point center, which are the pixel coordinates of robot A;

[0114] S12: According to the distance constraint, the point detected in the next frame is associated with the points detected in the previous frames, and their duty cycle is calculated;

[0115] S13: Determine the ID of robot A by comparing the calculated duty cycle with data in an ID library, wherein the ID library stores the IDs of all robots in the robot system.

[0116] In the processor, Figure 2 As shown, a preliminary relative pose solution is performed based on the pixel coordinates, relative distance, and gravity direction. This requires mutual direction measurement from the infrared camera, ultra-wideband ranging, and measurements from the robot's own IMU and the neighboring robot's IMU. We use a DW1000-based ultra-wideband module to provide mutual ranging measurements. A separate, low-cost 6-DOF IMU module is used to provide 3-axis acceleration and angular velocity, with intermediate data transmission using Wi-Fi. The process of preliminary relative pose solution may include:

[0117] S21: According to the pixel coordinates combined with the fisheye camera model, obtain the three-dimensional direction vector of robot A in the coordinate system of robot B B p uA ;

[0118] Specifically, robots A and B are first required to appear in each other's camera field of view, like Figure 2 As shown in (a), based on the pixel coordinates detected in the image of robot B and the fisheye camera model, we can obtain the three-dimensional direction vector of robot A in the B coordinate system B p uA .

[0119] S22: According to the direction vector B p uA and the relative distance d AB , calculate the relative position of robot A in the coordinate system of robot B;

[0120] Specifically, similarly to step S21, we can obtain the three-dimensional direction vector of robot B in the coordinate system of robot A: A p uB , according to the distance d between robots A and B provided by UWB AB , the relative position of robot A in the coordinate system of robot B can be expressed as:

[0121]

[0122] S23: Calculating the roll angle and pitch angle of the robot B according to the gravity direction;

[0123] Specifically, if Figure 2 As shown in (b), in order to solve the relative rotation relationship, we introduce two intermediate reference systems and We use ZYX to define the Euler angle. According to the gravity direction measured by the IMU of robot B, we can calculate the roll angle and pitch angle of robot B. The corresponding rotation matrix is ​​recorded as and

[0124] S24: Using the rotation matrix of the roll angle and pitch angle and Get the intermediate reference coordinate system of the robot B coordinate system

[0125] S25: The direction vector B p uA Transform to the intermediate reference coordinate system Downward direction vector

[0126] Specifically, B p uA exist is expressed as

[0127]

[0128] S26: The direction vector Projection to the intermediate reference coordinate system On the XOY plane, the angle between the projection vector and the positive direction of the x-axis is obtained Combined with the angle received from the robot A Get the intermediate reference coordinate system of the robot A coordinate system The intermediate reference coordinate system between the robot's B coordinate system The relative side heading angle ψ between them;

[0129] Specifically, if Figure 2 As shown in (c) in the figure, we will Projection to On the XOY plane, the angle between the projection vector and the positive direction of the x-axis is recorded as If we follow the same steps for robot A, we get Coordinate system and The relative yaw angle ψ can be obtained by the following formula:

[0130]

[0131] S27: Using the rotation matrix of the roll angle and pitch angle of robot B and The rotation matrix corresponding to the side heading angle ψ, combined with the rotation matrix of the roll angle and pitch angle received from the robot A and Get the rotation matrix of robot A relative to robot B

[0132] Specifically, the rotation matrix of robot A relative to robot B is It can be calculated by the following formula:

[0133]

[0134] Among them, R yaw {ψ} is the rotation matrix corresponding to the yaw angle ψ. Through the above process, we can get the relative rotation of robots A and B and pan In the same way, in the processor of robot A, taking robot A as an observer, we can get and

[0135] In the processor, the calculated preliminary relative pose is filtered based on an improved error state Kalman filter. Compared to the error state Kalman filter in the inertial frame, our filter needs to take into account the rotation and translation of the reference frame itself when designing. This step may include:

[0136] 1) Prediction model:

[0137] First, we need to introduce a fixed reference frame w and obtain the position, velocity and rotation quaternion of the robot κ in the fixed reference frame w: w p k , w v k , w q k , where k represents robot A or B;

[0138] The relative pose between robots A and B is calculated using the following formula:

[0139] p=R T { w q B}( w p A - w p B )

[0140] v=R T { w qB}( w v A - w v B )

[0141]

[0142] in express The corresponding rotation matrix, Represents quaternion or rotation angle, R T represents the inverse matrix of the rotation matrix R, B q A is the rotation matrix B R A The corresponding quaternion, q * represents the conjugate quaternion of the four-element q;

[0143] Using the relative position between robots A and B, the nominal state x and the actual state x of the system are calculated. t , error state δx:

[0144]

[0145] where δθ k is the local angle error corresponding to the robot k quaternion, The relationship between the true state, the nominal state and the error state is:

[0146]

[0147] p t =R T {δθ B}(p+δp)

[0148] v t =R T {δθ B}(v+δv)

[0149]

[0150] The IMU acceleration measurement values ​​a of robot A and robot B are mA , a mB and the measured value of angular velocity w mA , w mB As the input u of this error state Kalman filter m The noise u corresponding to the system input n The noise a caused by the IMU acceleration of robot A and robot B nA , a nB and the angular velocity noise wnA , w nB We assume that the noise term follows a Gaussian distribution with mean 0.

[0151]

[0152] We simplified the IMU model and assumed that the IMU zero bias is a fixed value and has been processed during the IMU calibration phase, so it was not considered during modeling. We can get the recursive equation of the nominal state as:

[0153] p←R T {w mB Δt}(p+vΔt+0.5(R{q}a mA -a mB )Δt 2 )

[0154] v←R T {w mB Δt}(v+(R{q}a mA -a mB )Δt)

[0155]

[0156] where ← represents a discrete-time update and Δt represents a discrete time interval.

[0157] The difference equation of the error state is obtained as follows:

[0158] δx←f(x,δx,u m ,u n )=F x (x,u m )+F i u n

[0159] δp←R T {w mB Δt}(δp+δvΔt)

[0160] δv←R T {w mB Δt}(δv+αΔt)

[0161] δθ A ←R T {w mA Δt}δθ A -w nA Δt

[0162] δθ B ←R T {w mB Δt}δθ B -wnB Δt

[0163] where α = -R{q}[a mA ]×δθ A +[a mB ]×δθ B -R{q}a nA +a nB , Represents a vector The corresponding cross product matrix, F x and F i is the error state δx and noise u n The corresponding Jacobian matrix is ​​calculated as follows:

[0164]

[0165] Then construct the formula for prediction update:

[0166]

[0167]

[0168] in Q i Indicates u n The corresponding covariance matrix;

[0169] 2) Measurement update:

[0170] The result of the preliminary relative pose solution is used as the observation value z of the relative pose,

[0171]

[0172] in is the rotation matrix The corresponding quaternion, observation value and true state satisfy the following observation equation:

[0173] z=h(x t )+ε

[0174]

[0175]

[0176]

[0177] in Estimate of the true state Able to pass Calculated, due to the error state estimation Therefore, there is The Jacobian matrix H of the observation equation is:

[0178]

[0179] The update equation of the improved error state Kalman filter is:

[0180] K=PH T (HPH T +V) -1

[0181]

[0182] P←(I-KH)P

[0183] 4) Adding and resetting error status:

[0184] The calculated error state is added to the nominal state, and the calculation formula is:

[0185]

[0186] This formula has been mentioned before and will not be repeated here.

[0187] After the error state is added to the nominal state, the error state is reset to 0 and the covariance matrix P is updated, where the error state reset equation is:

[0188]

[0189]

[0190]

[0191]

[0192]

[0193] According to the Jacobian matrix G corresponding to the error state reset equation, update the estimated value of the error state And the corresponding covariance matrix P:

[0194]

[0195] P←GPG T

[0196] in

[0197] It should be noted that, in the above process, for robot B (observer), the remaining robots in the system can all be the above-mentioned robot A.

[0198] like Figure 3As shown, if there are three or more robots in the scene, in the processor, after using the three-axis acceleration and three-axis angular velocity to filter the calculated preliminary relative pose based on the improved error state Kalman filter, it also includes using the pose graph optimization algorithm to further optimize the filtering result.

[0199] Specifically, the pose graph optimization algorithm is used to further optimize the filtering results. The pose optimization problem is expressed as

[0200]

[0201] Among them, the relative pose between robot i and robot j output by the error state Kalman filter is The position of robot i relative to reference frame B is represented by X i =(R i , t i )∈SE(3), O represents the set of robots, L represents the edges of mutual observation between robots, ρ is the kernel function, is defined as follows:

[0202]

[0203] At this point, the system has achieved the estimation of relative pose. The following describes the relative pose estimation effect of the system.

[0204] The innovation of this invention is to propose a relative pose estimation system that does not rely on global positioning and odometry information. It can estimate relative pose stably and accurately in dark and long-distance scenes. Our solution was tested on two drones and used the motion capture system of NOKOV as the ground truth. The experimental results are as follows: Figure 4 shown. Figure 4 Figure (a) shows the scenario of our experiment, in which we used a drone (UAV0) as the observer (reference frame). Figures (b), (c), and (d) respectively demonstrate the effects of relative pose estimation when the drones automatically execute different trajectories. The left side of each graph shows the actual trajectory of UAV0 obtained by the motion capture system, while the right side shows the trajectory of UAV1 obtained based on relative pose estimation. We designed the trajectories of UAV0 and UAV1 to have the same shape. This shows that our system can achieve relatively stable and accurate relative pose estimation results for various UAV flight trajectories.

[0205] Figure 5 Shown Figure 4 The error between the estimated relative pose and the true relative pose (obtained by the motion capture system) is calculated using the absolute trajectory error (ATE) method. The three experiments correspond to Figure 4 The errors in the three flights are shown in box plots. Figure 5 It can be seen from (a) and (b) that the median error of the relative position estimation is between 0.102m and 0.161m, and the median error of the relative rotation angle is between 1.046° and 1.517°.

[0206] In addition, if Figure 6 As shown, we also conduct experimental verification in long-distance scenarios. Figure 6 (a) and (b) show the operation of the system of the present invention in outdoor long-distance scenarios. The system of the present invention can still stably estimate the relative posture between the two robots when the two robots are 27.5 meters apart. Compared with the solution based on AprilTags and LED mentioned above, the effective working distance is greatly improved.

[0207] Those skilled in the art will readily conceive of other embodiments of the present application after considering the specification and practicing the contents disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present application that follow the general principles of this application and include common knowledge or customary techniques in the art that are not disclosed in this application.

[0208] It will be understood that the present application is not limited to the exact construction that has been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof.

Claims

1. A 6-DOF relative pose estimation system based on multi-sensor fusion, applied to robot B, characterized in that: Including infrared LED module, UWB module, infrared fisheye camera, IMU module and processor: The infrared LED module is used to encode its own ID through the duty cycle of LED flashing; The infrared fisheye camera is used to capture its own field of view image; The UWB module is used to provide a relative distance from the robot; The IMU module is used to obtain its own 3-axis acceleration, 3-axis angular velocity and gravity direction. The processor is configured to use the field of view image to identify the ID and pixel coordinates of the robot A, perform preliminary relative pose calculation based on the pixel coordinates, relative distance, and gravity direction, and filter the calculated preliminary relative pose based on an improved error state Kalman filter using the three-axis acceleration and three-axis angular velocity, thereby achieving relative pose estimation; The processor filters the calculated preliminary relative pose based on an improved error state Kalman filter, including: 1) Prediction model: Get the position, velocity and rotation quaternion of the robot κ in the fixed reference frame w w p k , w v k , w q k , where k represents robot A or B; The relative pose between robots A and B is calculated using the following formula: p=R T { w q B }( w p A - w p B ) v=R T { w q B }( w v A - w v B ) in express The corresponding rotation matrix, Represents quaternion or rotation angle, R T represents the inverse matrix of the rotation matrix R, B q A is the rotation matrix B R A The corresponding quaternion, q * represents the conjugate quaternion of the four-element q; 2) Measurement update: The result of the preliminary relative pose solution is used as the observation value z of the relative pose, in is the rotation matrix The corresponding quaternion, observation value and true state satisfy the following observation equation: z=h(x t )+ε in 2. The system according to claim 1, wherein: The infrared LED module is a circular light board composed of a number of infrared LEDs.

3. The system according to claim 1, wherein: In the processor, using the field of view image to identify the ID and pixel coordinates of the robot A includes: Convert the field of view image into a binary image with a threshold, perform circle detection, and obtain the pixel coordinates of the center of the detection point, which are the pixel coordinates of robot A; According to the distance constraint, the points detected in the next frame are associated with the points detected in the previous frames, and their duty cycle is calculated; The ID of robot A is determined by comparing the calculated duty cycle with data in an ID library, where the IDs of all robots are stored.

4. The system according to claim 1, wherein: In the processor, a preliminary relative pose solution is performed based on the pixel coordinates, relative distance, and gravity direction, including: According to the pixel coordinates combined with the fisheye camera model, the three-dimensional direction vector of robot A in the robot B coordinate system is obtained. B p uA ; According to the direction vector B p uA and the relative distance d AB , calculate the relative position of robot A in the coordinate system of robot B; Calculating the roll angle and pitch angle of the robot B according to the direction of gravity; Using the rotation matrix of the roll angle and pitch angle and Get the intermediate reference coordinate system of the robot B coordinate system The direction vector B p uA Transform to the intermediate reference coordinate system Downward direction vector The direction vector Projection to the intermediate reference coordinate system On the XOY plane, the angle between the projection vector and the positive direction of the x-axis is obtained Combined with the angle received from the robot A Get the intermediate reference coordinate system of the robot A coordinate system The intermediate reference coordinate system between the robot's B coordinate system The relative side heading angle ψ between them; Using the rotation matrix of the roll angle and pitch angle of robot B and The rotation matrix corresponding to the side heading angle ψ, combined with the rotation matrix of the roll angle and pitch angle received from the robot A and Get the rotation matrix of robot A relative to robot B 5. The system according to claim 4, characterized in that Receive information from the robot A via WiFi.

6. The system according to claim 1, wherein: In the processor, filtering the calculated preliminary relative pose based on an improved error state Kalman filter includes: 1) Prediction model: Get the position, velocity and rotation quaternion of the robot κ in the fixed reference frame w w p k , w v k , w q k , where k represents robot A or B; The relative pose between robots A and B is calculated using the following formula: p=R T { w q B }( w p A - w p B ) v=R T { w q B }( w v A - w v B ) in express The corresponding rotation matrix, Represents quaternion or rotation angle, R T represents the inverse matrix of the rotation matrix R, B q A is the rotation matrix B R A The corresponding quaternion, q * represents the conjugate quaternion of the four-element q; Using the relative position between robots A and B, the nominal state x and the actual state x of the system are calculated. t , error state δx: where δθ k is the local angle error corresponding to the robot k quaternion, The relationship between the true state, the nominal state and the error state is: p t =R T {δθ B }(p+δp) v t =R T {sth} B }(v+δv) According to the measured values ​​a of the 3-axis acceleration of robots A and B mA , a mB , the measured value of the 3-axis angular velocity w mA , w mB and the measurement noise of the 3-axis acceleration a nA , a nB , the measurement noise of the three-axis angular velocity w nA , w nB , where the measurement noise of acceleration and angular velocity conforms to the Gaussian distribution with zero mean, and the random walk error of IMU zero bias is ignored. The recursive equation of the nominal state of the relative posture estimation system between robots A and B is obtained as follows: p←R T {w mB Δt}(p+vΔt+0.5(R{q}a mA -a mB )Δt 2 ) v←R T {w mB Δt}(v+(R{q}a mA -a mB )Δt) Where ← represents a discrete time update and Δt represents a discrete time interval; The difference equation of the error state is obtained as follows: δx←f(x,δx,u m ,u n )=F x (x,u m )+F i u n δp←R T {w mB Δt}(δp+δvΔt) δv←R T {w mB Δt}(δv+αΔt) dth A ←R T {w mA Δt}δθ A -w nA Δt dth B ←R T {w mB Δt}δθ B -w nB Δt where α = -R{q}[a mA ] × δθ A +[a mB ] × δθ B -R{q}a nA +a nB , [θ] × Represents the cross product matrix corresponding to vector θ, F x and F i is the error state δx and noise u n The corresponding Jacobian matrix is ​​calculated as follows: Then construct the formula for prediction update: in Q i Indicates u n The corresponding covariance matrix; 2) Measurement update: The result of the preliminary relative pose solution is used as the observation value z of the relative pose, in is the rotation matrix The corresponding quaternion, observation value and true state satisfy the following observation equation: z=h(x t )+ε in Estimate of the true state Able to pass Calculated, due to the error state estimation Therefore, there is The Jacobian matrix H of the observation equation between robots A and B is: The update equation of the improved error state Kalman filter is: K=PH T (HPH T +V) -1 P←(I-KH)P 3) Adding and resetting error status: The calculated error state is added to the nominal state, and the calculation formula is: After the error state is added to the nominal state, the error state is reset to 0 and the covariance matrix P is updated, where the error state reset equation is: According to the Jacobian matrix G corresponding to the error state reset equation, update the estimated value of the error state And the corresponding covariance matrix P: P←GPG T in 7. The system according to claim 1, wherein: If there are three or more robots in the scene, in the processor, after using the three-axis acceleration and three-axis angular velocity to filter the calculated preliminary relative pose based on the improved error state Kalman filter, it also includes using the pose graph optimization algorithm to further optimize the filtering result.

8. The system according to claim 7, characterized in that The pose graph optimization algorithm is used to further optimize the filtering results. The pose optimization problem is expressed as Among them, the relative pose between robot i and robot j output by the error state Kalman filter is The position of robot i relative to reference frame B is represented by X i =(R i ,t i )∈SE(3), O represents the set of robots, L represents the edges of mutual observation between robots, ρ is the kernel function, is defined as follows:

Images