Positioning method of intelligent inspection robot based on Beidou navigation system
By integrating Beidou module, encoder and inertial measurement unit into the intelligent patrol robot, combining extended Kalman filtering and Monte Carlo positioning algorithm, the problem of insufficient positioning accuracy of intelligent patrol robots in open and complex environments is solved, and the High precision and stable autonomous navigation.
Patent Information
- Application Number
- CN202111382617.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-11-20
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2041-11-20
AI Technical Summary
The existing intelligent inspection robot positioning method based on Beidou navigation system is insufficient in open and complex environments, especially in the case of large turning angles, fast driving speeds, interference between moving objects and transparent objects, and in wide open Estimated pose errors when there are fewer environmental features.
Using a positioning system integrating Beidou module, encoder, inertial measurement unit and laser camera, the extended Kalman filtering fusion algorithm and Monte Carlo positioning algorithm are combined with the data of the encoder and inertial measurement unit to be fused, and the extended Kalman filtering is used. The fusion pose is sampled as a particle reference, and the trajectory calculation is performed using the incremental information of the fusion intelligent patrol robot pose in the open area until the pose matching degree increases.
It realizes the high-precision autonomous navigation of intelligent patrol robots in open environments and positioning accuracy requirements in occlusion environments, meets the navigation needs in complex environments, and provides positioning solutions without cumulative errors, high reliability and stability.
Smart Images

Figure CN114488241B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of intelligent inspection of substations, and in particular relates to a positioning method for an intelligent inspection robot based on a Beidou navigation system. Background Art
[0002] In recent years, with the widespread application of my country's Beidou navigation system with completely independent intellectual property rights and the rapid development of artificial intelligence technology, intelligent inspection robots have gradually replaced manpower and become the main method of substation inspection with their powerful and reliable positioning navigation and intelligent recognition technology. In this process, more and more substations have adopted the intelligent inspection robot positioning method based on the Beidou navigation system.
[0003] There are many specific implementation methods for the intelligent inspection robot positioning method based on the Beidou navigation system, but generally speaking, they all include the following six basic steps:
[0004] Step 1: Determine the reference base station, usually by selecting a fixed point with precise coordinates in the substation as the reference base station;
[0005] Step 2: Calculate the measured coordinate value of the reference reference station by the Beidou receiving unit installed at the reference reference station, and then calculate the differential correction value by the precise coordinate value of the reference reference station and the measured coordinate value;
[0006] Step 3: Calculate the positioning position of the inspection robot according to the Beidou satellite signal, and correct the positioning position of the inspection robot by the differential correction amount transmitted by the reference base station, so as to obtain the real-time positioning position of the inspection robot;
[0007] Step 4: Establish a working coordinate system and determine the origin of the working coordinate system. Usually, the coordinate position of the inspection robot charging station is used as the origin of the working coordinate system.
[0008] Step 5: Calculate the real-time positioning position of the inspection robot and the conversion matrix from the positioning position of the reference base station to the working coordinate system;
[0009] Step six, convert the inspection path coordinate information into the inspection robot working coordinate system through the transformation matrix, calculate the optimal inspection path and perform inspection navigation.
[0010] The implementation effect of the above six basic steps depends on the corresponding algorithms. At present, the mainstream positioning solution for intelligent inspection robots is Lidar-scan, that is, the robot's real-time posture (position and attitude) is solved by registering the current frame point cloud data with the map data. The corresponding algorithms can be collectively referred to as laser registration algorithms, mainly including iterative closest point (ICP) algorithm and its variants, such as point-to-line iterative nearest neighbor algorithm (point-to-line ICP), point-to-plane iterative nearest neighbor algorithm (point-to-plane ICP), normal distribution transformation algorithm (normal distribution transformation, NDT), and feature matching algorithm. In addition, there are many algorithms based on deep learning to improve matching speed and stability, such as Monte Carlo localization algorithm (MonteCarlo localization, MCL) has become the most popular positioning solution due to its stability and re-positioning.
[0011] The above algorithm can give the optimal pose estimation when the surrounding environment features are rich and the intelligent inspection robot is moving smoothly. However, the above algorithm also has some defects. For example, the intelligent inspection robot may mismatch due to large turning angles, fast driving speeds, interference between moving objects and transparent objects during its movement. For example, in an open environment, there are fewer surrounding environment features, which may directly lead to errors in the estimated pose of the intelligent inspection robot. Summary of the invention
[0012] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and improve the positioning accuracy of the intelligent inspection robot in open environments and complex environments. The purpose of the invention is achieved through the following technical solutions:
[0013] A positioning method for an intelligent inspection robot based on a Beidou navigation system, wherein the positioning system used includes a central processing unit, a data conversion module, a database, a clock module, a gateway module, an information display module, a wired Internet module, a Beidou module, an encoder, an inertial measurement unit, and a laser camera arranged on a pan-tilt platform of the intelligent inspection robot;
[0014] The Beidou module, encoder, and inertial measurement unit are connected to the central processing unit through the data conversion module, the wired Internet module and information display module are connected to the central processing unit through the gateway module, and the database, clock module, and laser camera are directly connected to the central processing unit;
[0015] The positioning method includes six basic steps: determining the reference base station, calculating the differential correction amount, correcting the positioning position of the inspection robot, establishing a working coordinate system and determining the origin of the working coordinate system, calculating the real-time positioning position of the inspection robot and the conversion matrix from the positioning position of the reference base station to the working coordinate system, and calculating the optimal inspection path and performing inspection navigation. During this process, the central processing unit performs extended Kalman filtering fusion on the data transmitted by the Beidou module, encoder and inertial measurement unit, and combines the Monte Carlo positioning algorithm to sample the fused posture as a particle reference.
[0016] On the basis of the above technical solution, the present invention can add the following technical means to better achieve the purpose of the present invention:
[0017] The encoder photoelectric sensor is installed on the wheels of the intelligent inspection robot.
[0018] Furthermore, when the central processor performs extended Kalman filter fusion on the data transmitted by the Beidou module, the encoder and the inertial measurement unit, the data transmitted by the encoder is used as the observation quantity, and the data transmitted by the inertial measurement unit is used as the measurement quantity, and the first extended Kalman filter fusion is performed in the robot coordinate system formed by the data transmitted by the encoder.
[0019] Furthermore, Beidou positioning data is used as the observation quantity, and the first extended Kalman filter fusion data is used as the measurement quantity to perform the second extended Kalman filter fusion in the global coordinate system, thereby obtaining the fusion posture of the intelligent inspection robot in the global coordinate system.
[0020] Furthermore, the extended Kalman filter fusion pose data with Beidou positioning data as observation can be integrated into the Monte Carlo positioning algorithm through the following steps:
[0021] Step 1: adjust the importance parameters of all samples according to the Euclidean distance between all sample particles and the fusion pose;
[0022] Step 2: During the initial sampling and resampling process, samples are extracted based on the Gaussian distribution of the fused pose;
[0023] Step 3: Use the fused pose as a sample subset to replace particles with low importance in the sample set;
[0024] Step 4: Adjust the fusion strategy according to the particle matching degree. That is, when the posture matching degree is low, it means that there are few reference objects nearby and it belongs to an open area. In this case, the extended Kalman filter can be used to fuse the incremental information of the intelligent inspection robot's posture to perform track calculation until the particle filter estimates the posture to regain a higher matching degree, and then use the Monte Carlo positioning algorithm.
[0025] The main beneficial effects of the present invention are as follows:
[0026] The present invention utilizes sensors such as Beidou modules, encoders, and inertial measurement units, based on an extended Kalman filter fusion algorithm and combined with a Monte Carlo positioning algorithm, to provide a real-time posture solution of the intelligent inspection robot with no cumulative error, high reliability, and stability. This not only enables the intelligent inspection robot to complete precise autonomous navigation in an open environment, but also ensures the positioning accuracy requirements of the intelligent inspection robot in some obstructed environments, thereby meeting the navigation needs in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS
[0027] Figure 1 is a schematic structural diagram of a positioning system in one embodiment of the present invention;
[0028] Figure 2 A schematic diagram of an overall algorithm framework of an embodiment of the present invention;
[0029] Figure 3 This is a Beidou positioning differential flow chart in one embodiment of the present invention.
[0030] In the figure: CPU - central processing unit; Date - data conversion module; Odom - encoder; IMU - inertial measurement unit; BDS - Beidou module; Laca - laser camera; Time - hour hand module; Dbse - database; Gate - gateway module; Info - information display module; Inte - wired Internet module DETAILED DESCRIPTION
[0031] An embodiment of the present invention is described below with reference to the accompanying drawings.
[0032] like Figure 1 As shown, a positioning method for an intelligent inspection robot based on a Beidou navigation system, the positioning system used in the method includes a central processing unit, a data conversion module, a database, a clock module, a gateway module, an information display module, a wired Internet module, a Beidou module, an encoder, an inertial measurement unit, and a laser camera arranged on a pan-tilt platform of the intelligent inspection robot;
[0033] The data conversion module, database, clock module, wired Internet module, Beidou module, encoder, inertial measurement unit, and laser camera are respectively connected to the central processing unit for communication; the gateway module, information display module, and central processing unit are connected to each other for communication;
[0034] The positioning method includes six basic steps: determining a reference base station, calculating a differential correction amount, correcting the positioning position of the inspection robot, establishing a working coordinate system and determining the origin of the working coordinate system, calculating the real-time positioning position of the inspection robot and a conversion matrix from the positioning position of the reference base station to the working coordinate system, and calculating the optimal inspection path and performing inspection navigation. In this process, the central processing unit performs an extended Kalman filter (Extend Kalman Filte, EKF) fusion on the data transmitted by the Beidou module, the encoder and the inertial measurement unit, and combines the Monte Carlo (Monte Carlo, MCL) positioning algorithm to sample the fused posture as a particle reference.
[0035] like Figure 2 Combined with Figure 1 As shown, when the central processing unit performs extended Kalman filter fusion on the data transmitted by the Beidou module, the encoder and the inertial measurement unit, the data transmitted by the encoder is used as the observation quantity, and the data transmitted by the inertial measurement unit is used as the measurement quantity. The first extended Kalman filter fusion is performed in the robot coordinate system formed by the data transmitted by the encoder (the purpose is to ensure the continuity of the fused data). On this basis, the Beidou positioning data is used as the observation quantity, and the second extended Kalman filter fusion is performed in the global coordinate system with the first extended Kalman filter fusion data as the measurement quantity, thereby obtaining the fusion posture of the intelligent inspection robot in the global coordinate system.
[0036] like Figure 3 As shown, in order to ensure the accuracy requirements of Beidou data, this embodiment adopts differential positioning to improve Beidou positioning accuracy. Differential data is provided by the Beidou location service platform, that is, the differential data center. Its data is transmitted through HTTP and can be accessed through devices such as wireless data transmission terminals. Its protocol follows the Ntrip protocol, and the differential data communication format is RTCM3.0. The solved position information is obtained through the serial port in the form of a message, which complies with the NMEA message standard format. Since the Beidou module not only provides position information, but also provides heading information, it is necessary to read the positioning message and heading message separately. The solved positioning data is:
[0037] (x bds y bds ρ bds ) (1)
[0038] In formula (1), (x bds y bds ) is the position, ρ bds For the heading, in the local navigation strategy, there is no need to consider the elevation information, and the error of the altitude information of positioning is relatively large, so this embodiment filters out the altitude information, wherein the positioning data obeys the normal distribution.
[0039] The inertial measurement unit (IMU) can provide accurate transient information of the intelligent inspection robot, which is divided into three-axis acceleration information and three-axis angular velocity information. In addition, the inertial measurement unit is a self-reference measurement unit. In addition to its own measurement information, there is no other observation to provide redundant constraints. Therefore, the position and rotation information obtained by integration will have an increasing cumulative error due to the time scale, resulting in data unavailability. Therefore, its transient quantity or increment is generally used for fusion. Because the fusion algorithm is based on the robot coordinate system, it is necessary to exclude the measurement information of gravity acceleration. In addition, it needs to be calibrated, assuming that the data obeys a zero-mean normal distribution.
[0040] In order to make the data obtained and transmitted by the encoder more accurate, in this embodiment, the encoder photoelectric sensor is installed on the wheels of the intelligent inspection robot. Its function is to infer the position and track of the intelligent inspection robot based on the dynamic model of the intelligent inspection robot. The static error of the data obtained and transmitted by the encoder is much smaller than the dynamic error. The reason is that the position data information is greatly affected by the running speed of the intelligent inspection robot. In order to reflect the influence of speed, the data variance is regulated by speed-related factors. The basic strategy is that the variance is positively correlated with the speed:
[0041]
[0042] In formula (2), σ init is the initial variance of the encoder, v odom is the robot velocity, and h(g) is the linear equation about the velocity.
[0043] The above describes the device used in an embodiment of the present invention and its working principle and basic method in conjunction with the accompanying drawings. The following further describes the specific algorithms, combination methods and effect verification during the implementation of this embodiment.
[0044] 1. Extended Kalman Filter (EKF) Algorithm
[0045] In estimation theory, the extended Kalman filter is a recursive filter estimation algorithm derived from the hidden Markov model, which makes Gaussian assumptions on confidence and noise, and linearizes the motion model and observation model. For some mildly nonlinear non-Gaussian systems, the extended Kalman filter can give better results. Its nonlinear motion and observation models are as follows:
[0046] x k =f(x k-1 , v k )+w k (1)
[0047] yk =g(x k )+n k (2)
[0048] x k Assume that x k The confidence function of is a Gaussian distribution:
[0049]
[0050] y k is the observed quantity, v k is the control quantity, f(g) and g(g) are both nonlinear functions,
[0051] Assumption: w k and n k It's all white noise.
[0052] f(g), g(g) are differentiable.
[0053] Due to the nonlinear characteristics of f(g) and g(g), a closed-form solution cannot be calculated. Therefore, it is necessary to linearize the motion and observation models and perform a first-order Taylor expansion of f(g) and g(g):
[0054]
[0055] In formula (4):
[0056] Taylor expansion of the observation equation:
[0057]
[0058] In formula (5):
[0059] The extended Kalman filter algorithm can be roughly divided into two steps: prediction and update.
[0060] 1. Prediction
[0061]
[0062]
[0063] (II) Update
[0064]
[0065]
[0066]
[0067] In the above formula: is the update amount;
[0068] K k is called the Kalman gain matrix.
[0069] Prediction is to update the mean and variance based on the motion equation, and updating is to correct the mean and variance based on the observation information, thereby estimating the accurate posture data of the intelligent inspection robot.
[0070] Extending the classical recursive update equation of the Kalman filter, by updating the equation, this embodiment can be obtained by Calculate
[0071] 2. Monte Carlo (MCL) positioning algorithm
[0072] The Monte Carlo positioning algorithm, also known as the particle filter algorithm, is a nonlinear recursive Bayesian filtering algorithm that solves the posterior probability of posture through the statistical perspective of the law of large numbers. The Monte Carlo positioning algorithm is based on the given prior map information and perception state quantity. According to the state quantity, a group of random particles with weights are sampled to represent the posterior probability of posture estimation, and the current state quantity is calculated and corrected based on these particles and weights. Its advantage is that it is not limited by the model and Gaussian assumption. Its expression is:
[0073] Bel(x)≈{x (i) ,w (i)} i=1,...,n (1)
[0074] In formula (1), Bel(x) is a particle sample of the random state x, which includes position and rotation. The weight of each sample particle can be expressed by a non-negative parameter w (i) To determine, which is also called the importance factor of the particle, it represents the probability of the estimated pose of the intelligent inspection robot at this sample. The basic Monte Carlo recursive update is implemented by the following steps:
[0075] (1) Sampling
[0076] Particles are sampled based on the joint probability density of the Bayesian posterior estimate and the noise at the previous moment:
[0077]
[0078] In formula (2), m is the particle number.
[0079] (ii) Prediction
[0080] Substitute each prior particle and noise sample into the nonlinear motion model:
[0081]
[0082] The above predicted particles together approximately describe the probability density of the intelligent inspection robot’s position and posture.
[0083] (III) Correction of the posterior probability
[0084] Correcting the posterior probability is mainly divided into two steps:
[0085] Step 1: Assign a weight w to each particle based on the expected posterior and predicted posterior of each particle. k,m :
[0086]
[0087] In formula (4), η is the normalization coefficient.
[0088] Step 2: According to the assigned weight, importance resample each particle:
[0089]
[0090] 3. Extended Kalman Filter Fusion and Its Combination with Monte Carlo Positioning
[0091] In this embodiment, the extended Kalman filter fusion posture data with Beidou positioning data as the observation amount can be integrated into the Monte Carlo positioning algorithm through the following steps:
[0092] Step 1: adjust the importance parameters of all samples according to the Euclidean distance between all sample particles and the fusion pose;
[0093] Step 2: During the initial sampling and resampling process, samples are extracted based on the Gaussian distribution of the fused pose;
[0094] Step 3: Use the fused pose as a sample subset to replace particles with low importance in the sample set;
[0095] Step 4: Adjust the fusion strategy according to the particle matching degree. That is, when the posture matching degree is low, it means that there are few reference objects nearby and it belongs to an open area. In this case, the extended Kalman filter can be used to fuse the incremental information of the intelligent inspection robot's posture to perform track calculation until the particle filter estimates the posture to regain a higher matching degree, and then use the Monte Carlo positioning algorithm.
[0096] 4. Effect Verification
[0097] In order to verify the technical effect of this embodiment, the research and development team of the present invention conducted 10 experiments in structured environments (areas with rich surrounding environmental features), open areas and blocked areas. The selected intelligent inspection robot is the Keda Intelligent Inspection Robot developed by Keda Intelligent Co., Ltd. and equipped with a Beidou module. The 10 groups of experimental data obtained in each area were averaged, and the navigation and positioning accuracy test results under different environments are shown in Table 1.
[0098] Table 1 Navigation positioning accuracy test results in different environments
[0099]
[0100] It can be seen from the data in Table 1 that the present invention utilizes sensors such as Beidou modules, encoders, and inertial measurement units, based on the extended Kalman filter fusion algorithm, and combined with the Monte Carlo positioning algorithm, which can provide a posture solution with no cumulative error, high reliability and stability in real time. It not only enables the intelligent inspection robot to complete accurate autonomous navigation in an open environment, but also ensures the positioning accuracy requirements of the intelligent inspection robot in some obstructed environments, thereby meeting the navigation needs in complex environments.
Claims
1. A positioning method for an intelligent inspection robot based on the Beidou navigation system, wherein the positioning system used includes a central processing unit, a data conversion module, a database, a clock module, a gateway module, an information display module, a wired Internet module, a Beidou module, an encoder, an inertial measurement unit, and a laser camera arranged on the pan-tilt platform of the intelligent inspection robot; The Beidou module, encoder, and inertial measurement unit are connected to the central processing unit through the data conversion module, the wired Internet module and information display module are connected to the central processing unit through the gateway module, and the database, clock module, and laser camera are directly connected to the central processing unit; The positioning method includes six basic steps: determining the reference base station, calculating the differential correction amount, correcting the positioning position of the inspection robot, establishing a working coordinate system and determining the origin of the working coordinate system, calculating the real-time positioning position of the inspection robot and the conversion matrix from the positioning position of the reference base station to the working coordinate system, calculating the optimal inspection path and performing inspection navigation. Features: The central processor performs extended Kalman filter fusion on the data transmitted by the Beidou module, encoder and inertial measurement unit, and combines the Monte Carlo positioning algorithm to sample the fused pose as a particle reference; When the central processor performs extended Kalman filter fusion on the data transmitted by the Beidou module, the encoder and the inertial measurement unit, the data transmitted by the encoder is used as the observed quantity, the data transmitted by the inertial measurement unit is used as the measured quantity, and the first extended Kalman filter fusion is performed in the robot coordinate system formed by the data transmitted by the encoder; Beidou positioning data is used as the observation quantity, and the first extended Kalman filter fusion data is used as the measurement quantity to perform the second extended Kalman filter fusion in the global coordinate system, thereby obtaining the fusion posture of the intelligent inspection robot in the global coordinate system; The extended Kalman filter fusion pose data with Beidou positioning data as the observation is integrated into the Monte Carlo positioning algorithm through the following steps: Step 1: adjust the importance parameters of all samples according to the Euclidean distance between all sample particles and the fusion pose; Step 2: During the initial sampling and resampling process, samples are extracted based on the Gaussian distribution of the fused pose; Step 3: Use the fused pose as a sample subset to replace particles with low importance in the sample set; Step 4: Adjust the fusion strategy according to the particle matching degree. That is, when the posture matching degree is low, it means that there are few reference objects nearby and it belongs to an open area. In this case, the extended Kalman filter is used to fuse the incremental information of the intelligent inspection robot's posture to perform track calculation until the particle filter estimates the posture to regain a higher matching degree, and then use the Monte Carlo positioning algorithm.
2. The intelligent inspection robot positioning method based on the Beidou navigation system as claimed in claim 1, Features: The encoder is a photoelectric sensor and is installed on the wheels of the intelligent inspection robot.
