A servo-deployed beacon-assisted underground robot positioning method

Abstract

The application discloses a kind of following deployment beacon auxiliary underground robot positioning method, based on IMU measurement robot linear motion information and angular motion information, according to the initial pose of robot and pose recursion equation recursion robot pose;Combined with sensor observation, the error of acceleration measurement bias and angular rate measurement bias is estimated;Beacon robot motion is deployed on historical trajectory, its storage has the position coordinates of robot at this time, and beacon and robot real-time communication and ranging, based on the cumulative positioning error of robot being deployed beacon is corrected;The error state estimation after filtering is injected into nominal state estimation, and the accurate robot position estimation is obtained, and the estimation of the following filter cycle is reset after error state injection.By actively deploying beacon in the process of advancing underground robot, the distance observation between the deployed beacon and the robot is used to correct the cumulative positioning error of the robot and improve the positioning accuracy of the robot.

Description

Technical Field

[0001] This invention relates to the field of robot positioning technology, and more specifically to a method for positioning underground robots with beacon-assisted homing. Background Technology

[0002] Currently, various mobile robots are being extensively researched, with the expectation that they can replace humans in dangerous and complex environments such as caves and tunnels to perform tasks such as reconnaissance, exploration, rescue, and search. Accurately obtaining the robot's location information is essential for it to complete these tasks, and the most widely used method is to use the Global Navigation Satellite System (GNSS) for positioning.

[0003] However, in environments such as caves and tunnels, GNSS positioning fails due to satellite signal rejection. Robots typically use onboard sensors for measurements, combined with dead reckoning (DR) algorithms for positioning. However, dead reckoning is an incremental positioning relative to the initial position, and it has accumulated positioning errors. If these errors are not corrected over a long period, they will gradually increase, eventually causing the positioning results to diverge and become unusable.

[0004] Therefore, providing a beacon-assisted positioning method for underground robots that uses beacons to correct cumulative positioning errors and improve positioning accuracy is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0005] In view of this, the present invention provides a method for positioning underground robots with beacon-assisted positioning by active deployment. By actively deploying beacons during the movement of the underground robot, the cumulative positioning error of the robot is corrected by observing the distance between the deployed beacons and the robot, thereby improving the positioning accuracy of the robot.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] A method for localizing an underground robot with beacon-assisted homing deployment includes the following steps:

[0008] S1. Based on the IMU, measure the robot's linear motion and angular motion information, and deduce the robot's pose according to the robot's initial pose and pose recursion equation;

[0009] S2. Based on sensor observations, estimate the errors of acceleration measurement bias and angular rate measurement bias;

[0010] S3. The beacon is deployed on the historical trajectory as the robot moves. It stores the robot's position coordinates at this time, and the beacon communicates and measures distance with the robot in real time. The cumulative positioning error of the robot is corrected based on the deployed beacon.

[0011] S4. Inject the filtered error state estimate into the nominal state estimate to obtain an accurate robot position estimate. After the error state is injected, reset the estimate using the next filtering cycle.

[0012] Preferably, step S1 specifically includes:

[0013] Ignoring IMU measurement noise, and assuming that the accelerometer and gyroscope measurements have only one fixed bias, the robot state is then the nominal state. The recursive equation for the robot's discrete-time nominal pose is:

[0014]

[0015] v k+1 =v k +[R k (a mk -a bk )+g]Δt

[0016]

[0017] a b(k+1) =a bk

[0018] ω b(k+1) =ω bk

[0019] In the formula, the subscripts k and k+1 represent two adjacent moments, with the corresponding time interval being Δt, and p k v k Let p represent the robot's position and velocity in the k-time navigation coordinate system, respectively. k+1 v k+1 Let R represent the robot's position and velocity in the k+1 time navigation coordinate system, respectively. k Let a represent the rotation matrix from the body coordinate system to the navigation coordinate system at time k. mk a bk Represent the acceleration measurement value and its bias at time k, respectively. b(k+1) The bias is the acceleration measurement value at time k+1, where g represents the gravity at the robot's location, and q is the force of gravity. k q k+1 Let q represent the rotation quaternions from the body coordinate system to the navigation coordinate system at time k and time k+1, respectively. k {(ω mk -ω bk )Δt} is the axis-angle vector (ω) mk -ω bk The quaternion corresponding to Δt at time k, ω mk ω bk The angular velocity measurements at time k and their biases are given by ω. b(k+1)The angular velocity measurement at time k+1 is biased.

[0020] Preferably, step S2 specifically includes:

[0021] The error state variable to be estimated is selected as follows:

[0022] δX=[(δp) T (δv) T (δθ) T (δa b ) T (δω b ) T ] T

[0023] Where δp, δv, and δθ represent the robot's position error, velocity error, and attitude error, respectively, and δa b ,δω b These represent the errors in acceleration bias estimation and angular velocity bias estimation, respectively.

[0024] Establish the recursive equation for the discrete-time error state:

[0025] δp k+1 =δp k +δv k Δt

[0026]

[0027]

[0028]

[0029]

[0030] Where the subscripts k and k+1 represent two adjacent moments, with the corresponding time interval Δt and δp. k δp k+1 δv represents the robot position error at time k and time k+1, respectively. k Let δv be the value at time k, and R be the value at time k. k , Let a represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose, respectively. mk a bk These are the acceleration measurements and their biases at time k, δθ. k δθ k+1 The values ​​represent the robot's attitude error in the body coordinate system at time k and time k+1, respectively. Let ω be the disturbance vector for velocity error estimation. mk ω bkThese represent the measured angular rate and its bias, respectively. This represents the perturbation pulse vector for attitude error estimation. Let δa represent the pulsating pulse vectors for acceleration bias estimation and angular velocity bias estimation, respectively. bk ,δω bk Let δa be the error of the acceleration bias estimate and the angular velocity bias estimate at time k, respectively. b(k+1) ,δω b(k+1) These represent the errors in the acceleration bias estimation and angular velocity bias estimation at time k+1, respectively.

[0031] Preferably, step S3 specifically includes:

[0032] The location coordinates of the deployed beacon i are: The airborne beacon coordinates are the robot's position coordinates. Based on the force measurement between the two, the robot's state is estimated using the Kalman filter equation, and the robot's error state at time k is obtained. Obtain a prior estimate of the robot's error state at time k+1. and its corresponding covariance

[0033]

[0034]

[0035]

[0036]

[0037]

[0038] Where I is a 3×3 identity matrix, 0 is a 3×3 zero matrix, and Q... k Let σ be the covariance matrix of the disturbance pulse vector n. a 2 σ ω 2 These are velocity random walks and angle random walks, respectively. The power spectral density of the accelerometer and gyroscope with dynamic zero bias, respectively. k and G x For matrix notation, Let be the covariance corresponding to the posterior estimate of the robot's error state at time k;

[0039] The distance measurement between the deployed beacon and the robot is used as the observation, and the observation equation Z is... k for:

[0040] Z k =HδX+V

[0041]

[0042] H = [U 1×3 0 1×3 0 1×3 0 1×6 ]

[0043]

[0044] Where V represents the noise level in the distance measurement from the beacon to the robot at the time of capture. For measuring the actual distance from the deployed beacon to the robot, Let (x, y, z) be the calculated distance between the robot's nominal position and the deployed beacon. These represent the nominal position coordinates of the robot and the position coordinates of the deployed beacon, respectively. r1 represents the distance measurement between the deployed beacon and the robot. H and U are matrix symbols. 1×3 It is a 1×3 matrix, 0 1×3 It is a 1×3 dimensional zero vector, 0 1×6 It is a 1×6 dimensional zero vector;

[0045] Based on the Kalman filter equation, the posterior estimate of the robot's error state at time k+1 is... and its corresponding covariance for:

[0046]

[0047]

[0048]

[0049]

[0050] Where I is a 3×3 identity matrix. Z is a method for measuring the ranging error between the deployed beacon and the robot. k+1 The observation equation at time k+1, H k+1 This represents the value of the H matrix at time k+1.

[0051] Preferably, step S4 specifically includes:

[0052] The filtered error state estimate is injected into the nominal state estimate to obtain an accurate robot position estimate.

[0053]

[0054]

[0055]

[0056] Where, p k+1 v k+1 These represent the nominal position and nominal velocity estimates of the robot at time k+1, respectively. These are the posterior estimates of the robot's position error and velocity error at time k+1, respectively. k+1 Let be the quaternion corresponding to the robot's nominal pose at time k+1. The operator represents the posterior estimate of the attitude error at time k. The corresponding quaternion;

[0057] The error state is reset after injection and used for estimation in the next filtering cycle.

[0058]

[0059] As can be seen from the above technical solution, compared with the prior art, the present invention discloses a method for positioning underground robots with beacon-assisted positioning by active deployment. By actively deploying beacons during the movement of the underground robot, the cumulative positioning error of the robot is corrected by observing the distance between the deployed beacon and the robot, thereby improving the positioning accuracy of the robot. Attached Figure Description

[0060] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0061] Figure 1 The attached figure is a schematic diagram of the method flow structure provided by the present invention.

[0062] Figure 2 The attached diagram is a schematic diagram of the beacon-assisted underground robot positioning provided by the present invention. Detailed Implementation

[0063] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0064] This invention discloses a method for locating underground robots with beacon-assisted deployment, comprising the following steps:

[0065] S1. Based on the IMU, measure the robot's linear motion and angular motion information, and deduce the robot's pose according to the robot's initial pose and pose recursion equation;

[0066] S2. Based on sensor observations, estimate the errors of acceleration measurement bias and angular rate measurement bias;

[0067] S3. The beacon is deployed on the historical trajectory as the robot moves. It stores the robot's position coordinates at this time, and the beacon communicates and measures distance with the robot in real time. The cumulative positioning error of the robot is corrected based on the deployed beacon.

[0068] S4. Inject the filtered error state estimate into the nominal state estimate to obtain an accurate robot position estimate. After the error state is injected, reset the estimate using the next filtering cycle.

[0069] To further optimize the above technical solution, step S1 specifically includes:

[0070] Ignoring IMU measurement noise, and assuming that the accelerometer and gyroscope measurements have only one fixed bias, the robot state is then the nominal state. The recursive equation for the robot's discrete-time nominal pose is:

[0071]

[0072] v k+1 =v k +[R k (a mk -a bk )+g]Δt

[0073]

[0074] a b(k+1) =a bk

[0075] ω b(k+1) =ω bk

[0076] In the formula, the subscripts k and k+1 represent two adjacent moments, with the corresponding time interval being Δt, and p k v k Let p represent the robot's position and velocity in the k-time navigation coordinate system, respectively. k+1 v k+1 Let R represent the robot's position and velocity in the k+1 time navigation coordinate system, respectively. k Let a represent the rotation matrix from the body coordinate system to the navigation coordinate system at time k. mk a bk Represent the acceleration measurement value and its bias at time k, respectively. b(k+1)The bias is the acceleration measurement value at time k+1, where g represents the gravity at the robot's location, and q is the force of gravity. k q k+1 Let q represent the rotation quaternions from the body coordinate system to the navigation coordinate system at time k and time k+1, respectively. k {(ω mk -ω bk )Δt} is the axis-angle vector (ω) mk -ω bk The quaternion corresponding to Δt at time k, ω mk ω bk The angular velocity measurements at time k and their biases are given by ω. b(k+1) The angular velocity measurement at time k+1 is biased.

[0077] To further optimize the above technical solution, step S2 specifically includes:

[0078] The error state variable to be estimated is selected as follows:

[0079] δX=[(δp) T (δv) T (δθ) T (δa b ) T (δω b ) T ] T

[0080] Where δp, δv, and δθ represent the robot's position error, velocity error, and attitude error, respectively, and δa b ,δω b These represent the errors in acceleration bias estimation and angular velocity bias estimation, respectively.

[0081] Establish the recursive equation for the discrete-time error state:

[0082] δp k+1 =δp k +δv k Δt

[0083]

[0084]

[0085]

[0086]

[0087] Where the subscripts k and k+1 represent two adjacent moments, with the corresponding time interval Δt and δp. k δp k+1δv represents the robot position error at time k and time k+1, respectively. k Let δv be the value at time k, and R be the value at time k. k , Let a represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose, respectively. mk a bk These are the acceleration measurements and their biases at time k, δθ. k δθ k+1 The values ​​represent the robot's attitude error in the body coordinate system at time k and time k+1, respectively. Let ω be the disturbance vector for velocity error estimation. mk ω bk These represent the measured angular rate and its bias, respectively. This represents the perturbation pulse vector for attitude error estimation. Let δa represent the pulsating pulse vectors for acceleration bias estimation and angular velocity bias estimation, respectively. bk ,δω bk Let δa be the error of the acceleration bias estimate and the angular velocity bias estimate at time k, respectively. b(k+1) ,δω b(k+1) These represent the errors in the acceleration bias estimation and angular velocity bias estimation at time k+1, respectively.

[0088] To further optimize the above technical solution, step S3 specifically includes:

[0089] The location coordinates of the deployed beacon i are: The airborne beacon coordinates are the robot's position coordinates. Based on the force measurement between the two, the robot's state is estimated using the Kalman filter equation, and the robot's error state at time k is obtained. Obtain a prior estimate of the robot's error state at time k+1. and its corresponding covariance

[0090]

[0091]

[0092]

[0093]

[0094]

[0095] Where I is a 3×3 identity matrix, 0 is a 3×3 zero matrix, and Q... k Let σ be the covariance matrix of the disturbance pulse vector n. a 2 σω 2 These are velocity random walks and angle random walks, respectively. The power spectral density of the accelerometer and gyroscope with dynamic zero bias, respectively. k and G x For matrix notation, Let be the covariance corresponding to the posterior estimate of the robot's error state at time k;

[0096] The distance measurement between the deployed beacon and the robot is used as the observation, and the observation equation Z is... k for:

[0097] Z k =HδX+V

[0098]

[0099] H = [U 1×3 0 1×3 0 1×3 0 1×6 ]

[0100]

[0101] Where V represents the noise level in the distance measurement from the beacon to the robot at the time of capture. For measuring the actual distance from the deployed beacon to the robot, Let (x, y, z) be the calculated distance between the robot's nominal position and the deployed beacon. These represent the nominal position coordinates of the robot and the position coordinates of the deployed beacon, respectively. r1 represents the distance measurement between the deployed beacon and the robot. H and U are matrix symbols. 1×3 It is a 1×3 matrix, 0 1×3 It is a 1×3 dimensional zero vector, 0 1×6 It is a 1×6 dimensional zero vector;

[0102] Based on the Kalman filter equation, the posterior estimate of the robot's error state at time k+1 is... and its corresponding covariance for:

[0103]

[0104]

[0105]

[0106]

[0107] Where I is a 3×3 identity matrix. Z is a method for measuring the ranging error between the deployed beacon and the robot. k+1 The observation equation at time k+1, H k+1 This represents the value of the H matrix at time k+1.

[0108] To further optimize the above technical solution, step S4 specifically includes:

[0109] The filtered error state estimate is injected into the nominal state estimate to obtain an accurate robot position estimate.

[0110]

[0111]

[0112]

[0113] Where, p k+1 v k+1 These represent the nominal position and nominal velocity estimates of the robot at time k+1, respectively. These are the posterior estimates of the robot's position error and velocity error at time k+1, respectively. k+1 Let be the quaternion corresponding to the robot's nominal pose at time k+1. The operator represents the posterior estimate of the attitude error at time k. The corresponding quaternion;

[0114] The error state is reset after injection and used for estimation in the next filtering cycle.

[0115]

[0116] The beneficial effects of this application are: the underground robot actively deploys beacons during its advance, and by observing the distance between the deployed beacons and the robot, the robot's cumulative positioning error is corrected, thereby improving the robot's positioning accuracy.

[0117] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0118] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for slave deployment beacon assisted underground robot localization, the method comprising: The method comprises the following steps: S1, based on the IMU measurement robot line motion information and angular motion information, according to the initial pose of the robot and the pose recursive equation recursive robot pose; including: without considering the IMU measurement noise, assuming that the accelerometer and gyroscope measurement has only one fixed bias, at this time the robot state is the nominal state, then the nominal pose recursive equation of the robot in discrete time is: v k+1 = v k + [R k (a mk - a bk )+ g] Δt a b(k+1) = a bk ω b(k+1) = ω bk where subscripts k and k+1 denote two adjacent time instants with corresponding time interval Δt, p k , v k denote the robot's position and velocity in the navigation frame at time k, p k+1 , v k+1 denote the robot's position and velocity in the navigation frame at time k+1, R k denotes the rotation matrix from the body frame to the navigation frame at time k, a mk , a bk denote the acceleration measurement and its bias at time k, a b(k+1) is the bias of the acceleration measurement at time k+1, g denotes the gravity at the robot's location, q k , q k+1 denote the rotation quaternions from the body frame to the navigation frame at time k and k+1, q k {(ω mk -ω bk )Δt} is the quaternion corresponding to the axis-angle vector (ω mk -ω bk )Δt at time k, ω mk , ω bk are the angular rate measurement and its bias at time k, ω b(k+1) is the angular rate measurement bias at time k+1; S2, combined with sensor observation, the error of acceleration measurement bias and angular rate measurement bias is estimated; S3, the beacon random robot motion is deployed on the historical trajectory, which stores the position coordinates of the robot at this time, and the beacon communicates and measures the distance with the robot in real time, and the cumulative positioning error of the robot is corrected based on the deployed beacon; S4, the filtered error state estimation is injected into the nominal state estimation to obtain accurate robot position estimation, and the error state is reset after injection for estimation in the next filtering period.

2. The method of claim 1, wherein, The step S2 specifically comprises: The error state variable to be estimated is selected as: δx = [(δp) T (δv) T (δθ) T (δa b ) T (δω b ) T ] T wherein δp, δv, δθ represent the position error, velocity error and attitude error of the robot, respectively, and δa b , δω b represent the errors of the acceleration bias estimation and the angular velocity bias estimation, respectively. The recursive equation of the error state in discrete time is established: where subscripts k and k + 1 denote two adjacent time instants with corresponding time interval Δt, δp k , δp k+1 are the robot position errors at time instants k and k + 1, respectively, δv k is the value of δv at time instant k, R k , are the rotation matrix from the body frame to the navigation frame and its transpose, respectively, a mk , a bk are the acceleration measurement and its bias at time instant k, δθ k , Δθ k+1 are the attitude errors in the robot body frame at time instants k and k + 1, respectively, is the disturbance vector for velocity error estimation, ω mk , ω bk are the angular rate measurement and its bias, respectively, is the disturbance impulse vector for attitude error estimation, are the impulse vectors for acceleration bias estimation and angular velocity bias estimation, respectively, δa bk , δω bk are the errors in acceleration bias estimation and angular velocity bias estimation at time instant k, respectively, δa b(k+1) , δω b(k+1) are the errors in acceleration bias estimation and angular velocity bias estimation at time instant k + 1, respectively.

3. The method of claim 1, wherein, The step S3 specifically comprises: The position coordinates of the deployed beacon i are denoted as The on-board beacon coordinates, i.e. the position coordinates of the robot, based on the force measurements between the two, estimate the state of the robot through the Kalman filter equation, through the error state of the robot at time k Obtain the prior estimate of the error state of the robot at time k+1 And its corresponding covariance where I is a 3x3 identity matrix, 0 is a 3x3 zero matrix, Q k is the covariance matrix of the disturbance impulse vector n, σ a 2 , σ ω 2 are the velocity and angle random walks, respectively, are the power spectral densities of the dynamic biases of the accelerometer and gyroscope, respectively, F k and G x are the matrix symbols, is the covariance of the posterior estimate of the robot error state at time k. The distance measurement between the deployed beacon and the robot is taken as an observation, the observation equation Z k is: Z k = HδX + V H = [U 1×3 0 1×3 0 1×3 0 1×6 ] where V is the noise of the beacon-to-robot distance measurement when the robot is captured, is the actual beacon-to-robot distance measurement when the robot is deployed, is the computed distance between the robot nominal position and the deployed beacon, (x, y, z), are the robot nominal position coordinates and the deployed beacon position coordinates, respectively, r1is the distance measurement between the deployed beacon and the robot, H, U are matrix symbols, U 1×3 is a 1 x 3 dimensional matrix, 0 1×3 is a 1 x 3 dimensional 0 vector, 0 1×6 is a 1 x 6 dimensional 0 vector; According to the Kalman filter equation, the posteriori estimation of the error state of the robot at time k+1 and its corresponding covariance is: where I is a 3x3 identity matrix, Z is a method for ranging error between a deployed beacon and a robot, k+1 Hk+1 is the observation equation at time k+1, k+1 Hk+1 is the value of the H matrix at time k+1.

4. The method of claim 1, wherein, The step S4 specifically comprises: The filtered error state estimation is injected into the nominal state estimation to obtain accurate robot position estimation: where p k+1 , v k+1 are respectively the nominal position estimate and the nominal velocity estimate of the robot at time k + 1, are respectively the position error posterior estimate and the velocity error posterior estimate of the robot at time k + 1, q k+1 is the quaternion corresponding to the nominal pose of the robot at time k + 1, is the operator representing the pose error posterior estimate of the robot at time k corresponding quaternion; The error state is reset after injection, and is used for estimation in the next filtering period:

Images