A positioning method, system and application of a robot

Abstract

The present invention provides a positioning method, system and application for a robot, which relates to the technical field of positioning systems. Among them, the positioning method includes: obtaining the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time change amount between the current moment and the previous moment, the linear acceleration and the angular acceleration, and calculating the state pose at the current moment according to the pose state vector at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time change amount between the current moment and the previous moment, the linear acceleration and the angular acceleration; obtaining the total measurement noise at the current moment, and calculating the measurement pose at the current moment according to the total measurement noise at the current moment and the state pose at the current moment; calculating the optimal pose at the current moment according to the state pose at the current moment and the measurement pose at the current moment. The present invention combines tight coupling and loose coupling to fuse the pose estimation results of various sensors, improves the positioning accuracy of the robot, and has low cost.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning systems, and particularly to a positioning method, system and application of a robot in an oil environment. Background Art

[0002] At present, large vertical metal storage tanks are the main way for oil storage. During the oil storage and transportation process, impurities such as water, sand, soil, rust, heavy metal salts, etc. contained in the oil itself and infiltrated later will gradually deposit at the bottom of the tank, forming a viscous black colloidal oil sludge. The oil sludge will damage the quality of the oil, corrode the tank body and accessories, reduce the tank volume, and cause static electricity accidents. Therefore, it is necessary to conduct corrosion detection and cleaning of the storage tank on time.

[0003] Since manual cleaning and detection of storage tanks have disadvantages such as high cost, long cycle, low efficiency, poor effect, and difficulty in ensuring the safety of operators, the current relatively advanced method for cleaning and detecting storage tanks is to use robots for mechanical cleaning. In robot cleaning, in order to ensure the safe operation of the robot, avoid collision with the internal structure of the tank, and ensure the accuracy of cleaning and detection, robot positioning is one of the key technologies.

[0004] At present, the conventional positioning methods for robots in an oil environment include inertial navigation positioning, high-precision navigation positioning, and positioning based on the fusion of inertial navigation and Doppler velocimeters. Among them, inertial navigation positioning belongs to dead reckoning navigation, which has problems such as poor positioning accuracy, poor robustness, and serious cumulative errors; the positioning method based on the fusion of inertial navigation and Doppler velocimeters still has poor positioning accuracy and environmental anti-interference ability; high-precision navigation positioning generally refers to infrared positioning, which has high positioning accuracy, but its penetrability is extremely poor, and a complex layout is required to achieve the positioning effect, so its cost is also greatly increased. That is, the existing conventional positioning methods for robots in an oil environment cannot simultaneously meet the requirements of high positioning accuracy and low cost. Summary of the Invention

[0005] In view of the above technical problems, the present invention provides a positioning method, system and application of a robot, which realizes high-precision positioning of a robot based on multi-sensor fusion of an adaptive unscented Kalman filter algorithm, reduces the problem of cumulative inertial navigation errors by tightly coupling and fusing an encoder and a nine-axis inertial navigation unit, improves the positioning accuracy of the robot, and only requires the use of the most basic robot detection devices to obtain each data, without high-value precision equipment or complex layouts, and has a low cost. The technical solution is as follows:

[0006] The present invention specifically provides a positioning method for a single robot, and the method includes: obtaining the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time change amount between the current moment and the previous moment, the linear acceleration, and the angular acceleration, and calculating the state pose at the current moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time change amount between the current moment and the previous moment, the linear acceleration, and the angular acceleration; obtaining the total measurement noise at the current moment, and calculating the measurement pose at the current moment according to the total measurement noise at the current moment and the state pose at the current moment; calculating the optimal pose at the current moment according to the state pose at the current moment and the measurement pose at the current moment.

[0007] Further, the calculating the state pose at the current moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the linear acceleration, and the angular acceleration includes: calculating the pose change amount between the current moment and the previous moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, and the time change amount between the current moment and the previous moment; calculating the state pose at the current moment according to the pose change amount between the current moment and the previous moment, the state pose at the previous moment, the linear acceleration, the angular acceleration, and the time change amount between the current moment and the previous moment.

[0008] Further, the calculating the state pose at the current moment according to the pose change amount between the current moment and the previous moment, the state pose at the previous moment, the linear acceleration, the angular acceleration, and the time change amount between the current moment and the previous moment includes: calculating the state noise at the current moment according to the state pose at the previous moment, the linear acceleration, the angular acceleration, and the time change amount between the current moment and the previous moment; calculating the state pose at the current moment according to the state pose at the previous moment, the pose change amount between the current moment and the previous moment, and the state noise at the current moment.

[0009] Further, the obtaining the total measurement noise at the current moment includes: obtaining the measurement noise in the flat state, the roll attitude angle at the current moment, and the pitch angle at the current moment, calculating the first measurement noise at the current moment according to the measurement noise in the flat state, the roll attitude angle at the current moment, and the pitch angle at the current moment, and calculating the total measurement noise at the current moment according to the first measurement noise at the current moment.

[0010] Further, the obtaining of the total measurement noise at the current moment further includes: obtaining the initial environmental measurement noise, the first environmental feature number at the current moment, and the second environmental feature number at the current moment, calculating the second measurement noise at the current moment and the third measurement noise at the current moment according to the initial environmental measurement noise, the first environmental feature number at the current moment, and the second environmental feature number at the current moment, and calculating the total measurement noise at the current moment according to the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment.

[0011] Further, the calculating of the total measurement noise at the current moment according to the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment includes: presetting the initial fusion weights of the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment; adjusting the initial fusion weights according to the noise levels of the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment to obtain the final fusion weights; and calculating the total measurement noise at the current moment according to the final fusion weights.

[0012] The present invention also provides a positioning method for multiple robots. The multiple robots are divided into a first robot and a second robot. The method includes: the first robot obtains the relative measurement position information of the second robot relative to the first robot, and obtains the optimal pose of the first robot at the current moment according to the above method. The first robot sends the obtained relative measurement position information of the second robot relative to the first robot and the optimal pose of the first robot at the current moment to the second robot; the second robot obtains its own state positioning information, and calculates the optimal pose of the second robot at the current moment by combining the relative measurement position information of the second robot relative to the first robot and the optimal pose of the first robot at the current moment.

[0013] The present invention also provides a positioning system for a single robot, including: a prediction module, configured to collect data of a state sensor and obtain the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time variation between the current moment and the previous moment, the linear acceleration, and the angular acceleration; an observation module, configured to collect data of a measurement sensor and obtain the flat state measurement noise, the roll attitude angle at the current moment, the pitch angle at the current moment, the initial environmental measurement noise, the first environmental feature number at the current moment, and the second environmental feature number at the current moment; a filtering module, configured to perform an unscented Kalman filter transformation on the data obtained by the prediction module and the observation module; and a pose positioning module, configured to calculate the optimal pose of the robot at the current moment according to the data obtained by the filtering module, and perform robot pose positioning according to the optimal pose at the current moment.

[0014] The present invention also provides a positioning system for multiple robots, including a first robot and a second robot; the first robot includes an estimation module, an observation module, a filtering module, and a pose positioning module in the positioning system of the above single robot; the second robot includes: a self-positioning module for obtaining the yaw angle of the second robot; a receiving module for receiving the known pose information of the first robot; and a calculation module for calculating the optimal pose of the second robot at the current moment according to the known pose information and the yaw angle.

[0015] The present invention also provides an application of the positioning system for a single robot or multiple robots as described above in an oil environment.

[0016] Advantages of the present invention:

[0017] First of all, the present invention proposes a positioning method for robots, mainly referring to the high-precision positioning of robots based on the multi-sensor fusion of the adaptive unscented Kalman filter algorithm. The multi-sensor fusion positioning improves the positioning accuracy of the robot; the tight coupling fusion of the encoder and the nine-axis inertial navigation unit reduces the problem of inertial navigation error accumulation; the harsh state of the environment is fed back according to the perception data of the sensor, thereby effectively determining the process noise of the pose estimation; the distributed loose coupling adaptive unscented Kalman filter fusion greatly improves the anti-interference ability of the robot and the system robustness while improving the positioning accuracy of the robot. At the same time, the acquisition of each data in this method is mainly based on the odometer system model, only the most basic robot detection device is needed, without high-value precision equipment or complex layout, and the cost is low, which can be widely promoted and applied.

[0018] Secondly, the present invention proposes a positioning system for multiple robots. Using cooperative positioning, only the first robot needs to carry high-precision sensors and have high-precision positioning capabilities, and the second robot only needs to be equipped with a wireless communication module. The cooperative positioning based on the wireless radio frequency module effectively transmits the high positioning accuracy and system robustness of the first robot, improves the cooperative positioning accuracy of multiple robots, thereby improving the positioning efficiency and reducing the robot cost. Description of the Drawings

[0019] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments of the present invention.

[0020] Figure 1 is a flowchart of the positioning method for a single robot;

[0021] Figure 2 is a schematic diagram of the positioning method for multiple robots;

[0022] Figure 3It is a flowchart of a positioning system for multiple robots.

[0023] 1. First robot; 2. Second robot. Specific implementation manners

[0024] To make the objectives, technical solutions and advantages of the present invention clearer, the following will further describe in detail the implementation manners of the present invention with reference to the accompanying drawings. Examples of the embodiments are shown in the drawings, where the same or similar reference numerals throughout the drawings indicate the same or similar components or components with the same or similar functions. The embodiments described below by referring to the drawings are exemplary and are only used to explain the present invention and cannot be construed as a limitation to the present invention.

[0025] In the description of this specification, if terms such as "Embodiment 1", "this embodiment", "in one embodiment" and the like are described, it means that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic expressions of the above terms do not necessarily refer to the same embodiment or example; moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in an appropriate manner.

[0026] In the description of this specification, terms such as "connection", "installation", "fixation", "setting", "having" and the like are all understood in a broad sense. For example, "connection" may be a fixed connection, a detachable connection or an integral connection; it may be a mechanical connection or an electrical connection; it may be directly connected or indirectly connected through an intermediate medium, and it may be the communication inside two elements. For those of ordinary skill in the art, the specific meanings of the above terms in this application can be understood according to specific situations.

[0027] In the description of this specification, relative terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "including", "comprising" or any other variant thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements, but also includes other elements not expressly listed, or also includes elements inherent to such process, method, article or device. Without further limitation, the element defined by the statement "including one..." does not exclude the existence of additional identical elements in the process, method, article or device including the said element.

[0028] In one embodiment, a positioning method for a robot includes: obtaining the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time variation between the current moment and the previous moment, the linear acceleration, and the angular acceleration, and calculating the state pose at the current moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time variation between the current moment and the previous moment, the linear acceleration, and the angular acceleration; obtaining the total measurement noise at the current moment, and calculating the measured pose at the current moment according to the total measurement noise at the current moment and the state pose at the current moment; calculating the optimal pose at the current moment according to the state pose at the current moment and the measured pose at the current moment.

[0029] In the robot positioning method of this embodiment, the method of calculating the state pose at the current moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time variation between the current moment and the previous moment, the linear acceleration, and the angular acceleration makes use of a tightly coupled form to fuse and estimate the pose of the robot, obtaining the state pose vector of the robot.

[0030] Further, calculating the optimal pose at the current moment according to the state pose at the current moment and the measured pose at the current moment is to perform loose coupling fusion on the state pose at the current moment and the measured pose at the current moment by adopting a distributed fusion method to obtain the optimal pose at the current moment.

[0031] Thus, this embodiment adopts a combination of tight coupling and loose coupling to fuse the pose estimation results of each sensor. The proposed distributed fusion framework is convenient for the pose estimation fusion system to access new sensing units on the one hand, and is also convenient for adaptively adjusting the fusion weights of each sensor according to the environment in which the robot is located, thereby ensuring the positioning accuracy of the robot in a complex environment. At the same time, in the above method, the acquisition of each data is mainly based on the odometer system model. Therefore, only the most basic robot detection device is needed, without high-value precision equipment or complex layouts, and the cost is low, which can be widely promoted and applied.

[0032] In one embodiment, calculating the state pose at the current moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the linear acceleration, and the angular acceleration includes: calculating the pose variation between the current moment and the previous moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, and the time variation between the current moment and the previous moment; calculating the state pose at the current moment according to the pose variation between the current moment and the previous moment, the state pose at the previous moment, the linear acceleration, the angular acceleration, and the time variation between the current moment and the previous moment.

[0033] Further, calculate the pose at the current moment based on the pose change amount between the current moment and the previous moment, the pose at the previous moment, the linear acceleration, the angular acceleration, and the time change amount between the current moment and the previous moment, including: calculating the state noise at the current moment based on the pose at the previous moment, the linear acceleration, the angular acceleration, and the time change amount between the current moment and the previous moment; calculating the pose at the current moment based on the pose at the previous moment, the pose change amount between the current moment and the previous moment, and the state noise at the current moment.

[0034] In the above embodiment, the pose of the robot is tightly coupled by calculating the pose change amount between the current moment and the previous moment. By calculating the state noise at the current moment and using the state noise at the current moment to perform unscented Kalman filter transformation on the pose change amount between the current moment and the previous moment, the pose at the current moment is finally obtained, reducing the interference of the environment and improving the positioning accuracy of the robot.

[0035] In one embodiment, obtaining the total measurement noise at the current moment includes: obtaining the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment, and calculating the total measurement noise at the current moment based on the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment.

[0036] Among them, obtaining the first measurement noise at the current moment includes: obtaining the flat state measurement noise, the roll attitude angle at the current moment, and the pitch angle at the current moment, and calculating the first measurement noise at the current moment based on the flat state measurement noise, the roll attitude angle at the current moment, and the pitch angle at the current moment.

[0037] Obtaining the second measurement noise and the third measurement noise at the current moment includes: obtaining the initial environment measurement noise, the first environmental feature number at the current moment, and the second environmental feature number at the current moment, and calculating the second measurement noise and the third measurement noise at the current moment respectively based on the initial environment measurement noise, the first environmental feature number at the current moment, and the second environmental feature number at the current moment.

[0038] As Figure 1 shown, as a specific fusion scheme, the specific calculation process is as follows:

[0039] (1) Determine the pose at the current moment of the robot

[0040] First, determine the state vector of the robot. The robot mentioned in the present invention is a wall-clinging robot in an oil environment, and its pose relative to the wall surface only needs to be represented by three variables. The right side in the horizontal direction of the wall surface is regarded as the positive direction of the robot's position x, the upward direction in the longitudinal direction of the wall surface is regarded as the positive direction of the robot's y-axis, and the angle between the robot and the y-axis is regarded as the yaw angle θ of the robot. Then the pose state vector of the robot is: X = [p x py θ] T , an odometer model using a motor encoder and a nine-axis inertial unit is adopted to construct the state equation of the system. Then the equation of the state pose at the current moment is as follows:

[0041]

[0042] where, X k and X k-1 are the state poses of the system at the k-th moment and the (k - 1)-th moment respectively, that is, the current moment state pose and the previous moment state pose in the first embodiment. v k and ω k are the linear velocity and angular velocity of the robot at the k-th moment of the system. The linear velocity and angular velocity are obtained by fusing the calculated values of the encoder and the measured values of the nine-axis inertial unit through unscented Kalman filtering. Δt is the time change amount between the k-th moment and the (k - 1)-th moment of the system. μ a and μ ω are the linear acceleration and angular acceleration of the robot measured by the nine-axis inertial unit. In the above formula, the first term on the right side is the system pose change amount based on the angular velocity, linear velocity and time change amount at the k-th moment, and the second matrix on the right side is the noise term of the system state prediction.

[0043] (2) Determine the measured pose of the robot at the current moment

[0044] Taking the rotary rangefinder sensor and the binocular vision rangefinder sensor as the observation sensors, the pose measurement value of the robot can be obtained through the feature matching method by the rotary rangefinder sensor and the binocular vision, that is, the measurement vector Z = [p xzk p yzk θ zk T . Then the equation of the measured pose at the current moment is:

[0045] Z(k) = H·X(k) + V(k)

[0046] where, Z(k) is the measurement prediction value at the k-th moment, V(k) is the process noise generated during measurement, V(k) = [p xk p yk θ k T . Since the measurement vector of the sensor is the same as the state vector of the system, the measurement matrix H is the identity matrix, and the measurement matrices of the rotary rangefinder sensor and the binocular vision are the same.

[0047] (3) Determine the total measurement noise at the current moment

[0048] Since the odometer model is used to estimate the pose of the robot, the flatness of the wall surface will affect the pose accuracy of the robot. Through the roll attitude angle θ of the nine-axis inertial sensor roll ​​Variation and pitch angle θ pitch The flatness of the variation feedback wall surface is obtained, and the measurement noise V quantized by the motor encoder is obtained E V(k)=V E0 +F(θ roll ,θ pitch ), which is the first measurement noise at the current moment. V E0 is the measurement noise in the flat state of the motor encoder, and F(θ roll ,θ pitch ) is the measurement noise corresponding to a certain rolling attitude angle and pitch attitude angle

[0049] The rotational rangefinder sensor and binocular vision are prone to positioning failure or even failure in an environment with few features. In addition, the measurement accuracy of the two sensors will decrease linearly with the increase of the distance. Assume that the number of environmental features perceived by the rotational rangefinder sensor is n r , and the number of environmental features perceived by binocular vision is n c . The optimal ranging range of the sensor is d r , d c . Define the ratio of the number of environmental features of the two sensors as r rc =n c / n r . The measurement noise of the rotational rangefinder sensor can be obtained as V r (k)=V r0 +F(n r ,d r ,r rc ), which is the second measurement noise at the current moment; the measurement noise of binocular vision is V c (k)=V c0 +F(n c ,d c ,r rc ), which is the third measurement noise at the current moment. Among them, V r0 and V c0 are the initial environmental measurement noises of the rotational rangefinder sensor and binocular vision respectively

[0050] (4) Determine the optimal pose at the current moment

[0051] After determining the state pose equation, measurement pose equation, and total measurement noise equation of the robot at the current moment

[0052] Through the unscented Kalman filter algorithm, the current state pose of the robot and the state covariance matrix are updated according to the equation of the current state pose at the current moment based on the previous state of the robot and the linear and angular velocities at that moment. The current measurement pose and the measurement covariance matrix are updated by the equation of the current measurement pose at the current moment. Further, the gain matrix is solved to obtain the optimal pose and covariance matrix of the robot at the current moment. Thus, the current pose of the robot can be completely determined. The specific process is as follows:

[0053] ① Initialization.

[0054]

[0055] Among them, X0 is the initial state vector of the robot, is the mean of the initial pose of the robot, and P0 is the initial covariance matrix of the robot pose.

[0056] ② Time update. Calculate the Sigma sampling points under time update,

[0057]

[0058] Among them, χ represents the sigma point matrix, and each column in the matrix represents a set of sigma points. λ is the scaling factor, and P k-1 is the covariance matrix under the optimal state estimation of the system at time k-1, and n is the state dimension of the system.

[0059] Substitute the Sigma points and the linear and angular velocities of the robot at time k into the nonlinear state equation of the mobile robot X k∣k-1 (i) = f(χ k-1 (i)), that is, in the equation of the current state pose at the current moment in step (1), calculate and predict the state vector and covariance matrix P k∣k-1 .

[0060]

[0061] Among them, represents the weight of the i-th sigma point.

[0062] ③ Measurement update. Calculate the Sigma points and Sigma point weights under measurement update

[0063]

[0064] Among them, ξ represents the sampling sigma point matrix under the measurement equation.

[0065] Substitute the Sigma points into the equation of the current measurement pose of the mobile robot Z k∣k-1 (i) = f(ξk∣k-1 (i) That is, in Z(k) = H·X(k) + V(k), calculate and predict the predicted observation vector of the mobile robot at time k under sensor observation Observation covariance matrix S zz,k , and the covariance matrix T of the observation with respect to the system prediction xz,k .

[0066]

[0067] ④ Optimal pose estimation. Calculate the gain matrix K k , and further calculate the optimal pose estimation of the mobile robot system at time k and its corresponding covariance matrix P k .

[0068]

[0069] where Z k is the observed pose of the sensor at time k

[0070] In another embodiment, calculate the total measurement noise at the current time according to the first measurement noise at the current time, the second measurement noise at the current time, and the third measurement noise at the current time, including: preset the initial fusion weights of the first measurement noise at the current time, the second measurement noise at the current time, and the third measurement noise at the current time. Adjust the initial fusion weights according to the noise levels of the first measurement noise at the current time, the second measurement noise at the current time, and the third measurement noise at the current time to obtain the final fusion weights. Calculate the total measurement noise at the current time according to the final fusion weights

[0071] This embodiment combines the environmental perception information of each sensor. Considering the situation where the accuracy of each sensor may decrease or even fail in extreme cases, a method of adjusting the initial fusion weights according to the noise levels to obtain the final fusion weights is proposed. The measurement accuracy of the sensor changes with the environment and is even prone to positioning failure and invalidation in extreme cases. The noise level refers to the degree of decrease in the sensor accuracy, and the noise level is the largest when the sensor loses its positioning ability. That is, this embodiment indirectly reflects the actual feature situation of the environment and the lighting situation of the environment through the number of environmental features sensed by the sensor, determines the measurement noise corresponding to the sensor according to the environmental change, and then dynamically adjusts the covariance matrix of the process noise, and further adjusts the fusion weights of the sensors to ensure that each sensor can be effectively utilized

[0072] This application also proposes a positioning method for multiple robots, such as Figure 2As shown in the figure, multiple robots are divided into a first robot 1 and a second robot 2. The first robot 1 obtains the relative measurement position information of the second robot 2 relative to the first robot 1, and obtains the optimal pose of the first robot 1 at the current moment according to the method of any one of the foregoing embodiments. The first robot 1 sends the obtained relative measurement position information of the second robot 2 relative to the first robot 1 and the optimal pose of the first robot 1 at the current moment to the second robot 2; the second robot 2 obtains its own state positioning information, and combines the relative measurement position information of the second robot 2 relative to the first robot 1 and the optimal pose of the first robot 1 at the current moment to calculate the optimal pose of the second robot 2 at the current moment. Among them, the relative measurement position information of the second robot 2 relative to the first robot 1 refers to the distance and angle of the second robot 2 relative to the first robot 1.

[0073] This method only needs to obtain the high-precision positioning information of the first robot 1. The second robot 2 only needs to perform simple calculations using the high-precision positioning information of the first robot 1 and its own state positioning information to obtain the high-precision positioning information of the second robot 2, thereby improving the positioning efficiency and reducing the robot cost.

[0074] This application also proposes a positioning system for a single robot for implementing the above positioning method, specifically including:

[0075] A prediction module, which is used to collect data of a motor encoder and a nine-axis inertial unit, and obtain the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time change amount between the current moment and the previous moment, the linear acceleration, and the angular acceleration. The motor encoder and the nine-axis inertial unit are in a tightly coupled form to fuse and estimate the robot pose in a filtering module. The kinematic equation of the system is obtained based on the combined positioning of the two.

[0076] An observation module, which is used to collect data of a rotary ranging sensor and a binocular vision ranging sensor, and obtain the flat state measurement noise, the roll attitude angle at the current moment, the pitch angle at the current moment, the initial environment measurement noise, the first environmental feature number at the current moment, and the second environmental feature number at the current moment. The rotary ranging sensor and the binocular vision both obtain the robot pose estimation by means of feature matching. The two sensors are used as the observation equations for the system pose estimation, and the distributed fusion method and the pose estimation results of the motor encoder and the nine-axis inertial unit are loosely coupled and fused in the filtering module.

[0077] A filtering module, which is used to perform unscented Kalman filter transformation on the data obtained by the prediction module and the observation module. The observation module obtains the number of observation features of the rotary ranging sensor and the binocular vision and the distance range of the features, indirectly obtains whether the environment where the robot is located is feature-single, whether the light is sufficient, etc., and then adjusts the noise covariance of the sensor observation process in this module.

[0078] The pose positioning module is used to calculate the optimal pose of the robot at the current moment based on the data obtained by the filtering module, and perform robot pose positioning according to the optimal pose at the current moment. This module combines with the filtering module, based on the odometer system model, and adopts an adaptive unscented Kalman filter fusion algorithm to fuse the robot poses estimated based on the motor encoder, nine-axis inertial unit, rotational ranging sensor, and binocular vision through a combination of tight coupling and loose coupling, and finally obtains the optimal pose of the robot at the current moment, and performs robot pose positioning according to the optimal pose at the current moment.

[0079] The positioning system of a single robot provided in this embodiment realizes the fusion of the pose estimation results of each sensor in a combination of tight coupling and loose coupling by setting up a prediction module, an observation module, a filtering module, and a pose positioning module, and using the multi-sensor fusion-based robot high-precision positioning method based on the adaptive unscented Kalman filter algorithm in the foregoing embodiment. Among them, the proposed distributed fusion framework is convenient for the pose estimation fusion system to access new sensing units on the one hand, and is also convenient for adaptively adjusting the fusion weights of each sensor according to the environmental conditions of the robot on the other hand, so as to ensure the positioning accuracy of the robot in a complex environment. At the same time, by obtaining the number of observation features and the distance range of the features of the rotational ranging sensor and the binocular vision, the situation of whether the environment where the robot is located is featureless and whether the illumination is sufficient is indirectly obtained, and then the noise covariance of the sensor observation process is adjusted to reduce the interference of the environment and improve the positioning accuracy of the robot.

[0080] The specific implementation of this embodiment is as follows:

[0081] A depth gauge, a nine-axis inertial unit, a motor encoder, a rotational ranging sensor, and binocular vision are configured on the robot. For the robot, the unscented Kalman filter (UKF) is used to fuse the poses estimated by the multi-sensors carried by it to obtain the optimal estimate of the robot pose, thereby determining the robot pose. Among them, the depth gauge is used to feedback the depth information of the oil under the position where the robot is located.

[0082] Based on the odometer system model, through a combination of tight coupling and loose coupling, an adaptive unscented Kalman filter fusion algorithm is used to fuse the robot poses estimated based on the motor encoder, nine-axis inertial unit, rotational ranging sensor, and binocular vision. Among them, the motor encoder and the nine-axis inertial unit are fused in a tight coupling form to estimate the robot pose, and the kinematic equation of the system is obtained based on the combined positioning of the two. The rotational ranging sensor and the binocular vision both obtain the robot pose estimate by means of feature matching. The two sensors are used as the observation equations of the system pose estimate, and a distributed fusion method is used to perform loose coupling fusion with the pose estimation results of the motor encoder and the nine-axis inertial unit.

[0083] A noise model of the system is established based on the linear acceleration, angular acceleration, and roll angle of the inertial measurement unit. A measurement noise model is established based on the number of features and the observation distance observed by the rotating range sensor and binocular vision. The system will adjust the predicted noise and observation noise of the fusion according to the real-time environmental perception information of the sensor, thereby adjusting the covariance matrix of the prediction and observation, and further adjusting the fusion weights of each sensor, which not only improves the accuracy of the robot pose estimation but also enables the robot to have strong environmental anti-interference ability.

[0084] This application also proposes a positioning system for multiple robots, such as Figure 3 shown, including a first robot 1 and a second robot 2; the first robot 1 includes the prediction module, observation module, filtering module, and pose positioning module in the foregoing embodiment; the second robot 2 includes: a self-positioning module for obtaining the yaw angle of the second robot 2; a receiving module for receiving the known pose information of the first robot 1; a calculation module for calculating the optimal pose of the second robot 2 at the current moment according to the known pose information and the yaw angle. Among them, the calculation module is the pose positioning module in the above embodiment, and different algorithms are set to calculate the optimal pose of different robots at the current moment.

[0085] Specifically, the self-positioning module of the second robot 2 obtains the data of the motor encoder of the second robot 2 and obtains the self-yaw angle of the second robot 2. The receiving module of the second robot 2 receives the optimal pose information of the first robot 1 at the current moment obtained according to the positioning system of a single robot in the foregoing embodiment, and the distance and angle information of the second robot 2 relative to itself obtained by the rotating range sensor mounted on the first robot 1. The calculation module of the second robot 2 calculates the optimal pose of the second robot 2 at the current moment according to the optimal pose information of the first robot 1 at the current moment, the distance and angle information of the second robot 2 relative to the first robot 1, and the self-yaw angle of the second robot 2.

[0086] Specifically, multiple detection robots are divided into two categories, one is called the first robot 1, and the other is called the second robot 2. In the multi-robot system, there is only one first robot 1, and the first robot 1 is equipped with a depth meter, a nine-axis inertial unit, a motor encoder, a rotating range sensor, binocular vision, and a radio frequency module. The second robot 2 is mainly equipped with a motor encoder and a radio frequency module.

[0087] For the first robot 1, using the method in the above embodiment, through the combination of tight coupling and loose coupling, the adaptive unscented Kalman filter fusion algorithm is used to fuse the robot poses estimated based on the motor encoder, nine-axis inertial unit, rotating range sensor, and binocular vision.

[0088] While the first robot 1 scans the environmental features, it determines its distance and angle relative to the surrounding second robots 2 by rotating the range sensor and determines the numbers of the second robots 2. After the first robot 1 obtains its optimal pose estimate, the first robot 1 converts the position and sends the position data to the corresponding numbered second robots 2 through the radio frequency module. After receiving the sent position information of the first robot 1, the second robots 2 determine their own poses in combination with the yaw angle estimated by the coding, thus completing the cooperative positioning of the master and second robots 2.

[0089] The number of the second robots 2 can be one or more.

[0090] Through the above method, this embodiment well solves the problems of poor robot positioning accuracy and poor environmental anti-interference ability, and greatly improves the robustness and positioning accuracy of the robot positioning system. The main feature of the multi-robot cooperative positioning in which the first robot 1 and the second robots 2 cooperate is that only the first robot 1 needs to carry high-precision sensors and have high-precision positioning capabilities, and the other second robots 2 only need to be equipped with wireless communication modules, thereby improving the positioning efficiency and reducing the cost of the robots.

[0091] The present invention also provides an application of the positioning system of a single robot or a positioning system of multiple robots as described above in an oil environment. It solves the problems of poor robot positioning accuracy and poor environmental anti-interference ability in the oil environment, greatly improves the robustness and positioning accuracy of the robot positioning system, improves the positioning efficiency and reduces the cost of the robots.

[0092] In one embodiment, a positioning system of a robot includes at least one processor and a memory communicatively connected to the at least one processor; the processor is configured to execute the method of any one of the above embodiments by calling a computer program stored in the memory. The computer program is program code, and when the program code runs on the robot positioning device, the program code is used to cause the robot positioning device to execute the steps in the robot pose positioning method described in a part of the above embodiments of this specification.

[0093] The above description of the embodiments is to enable those of ordinary skill in the art to understand and apply the technology of this case. Obviously, those who are familiar with the technology in this field can easily make various modifications to these examples and apply the general principles described herein to other embodiments without creative labor. Therefore, this case is not limited to the above embodiments, and the following modifications should all be within the protection scope of this case: ① A new technical solution implemented based on the technical solution of the present invention and combined with the existing common knowledge, and the technical effect produced by this new technical solution does not exceed the technical effect of the present invention; ② An equivalent replacement of some features of the technical solution of the present invention using well-known technologies, and the technical effect produced is the same as the technical effect of the present invention; ③ An expansion based on the technical solution of the present invention, and the substantial content of the expanded technical solution does not exceed the technical solution of the present invention; ④ An equivalent transformation made using the content of the specification and drawings of the present invention, directly or indirectly applied to other related technical fields.

Claims

1. A positioning method for a single robot, characterized in that, The method includes: Obtaining the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time change amount between the current moment and the previous moment, the linear acceleration, and the angular acceleration, and calculating the state pose at the current moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time change amount between the current moment and the previous moment, the linear acceleration, and the angular acceleration; Obtaining the total measurement noise at the current moment, and calculating the measurement pose at the current moment according to the total measurement noise at the current moment and the state pose at the current moment; Calculating the optimal pose at the current moment according to the state pose at the current moment and the measurement pose at the current moment; The obtaining the total measurement noise at the current moment includes: obtaining the flat state measurement noise, the roll attitude angle at the current moment, and the pitch angle at the current moment, calculating the first measurement noise at the current moment according to the flat state measurement noise, the roll attitude angle at the current moment, and the pitch angle at the current moment, and calculating the total measurement noise at the current moment according to the first measurement noise at the current moment; The obtaining the total measurement noise at the current moment further includes: obtaining the initial environment measurement noise, the first environmental feature number at the current moment, and the second environmental feature number at the current moment, respectively calculating the second measurement noise at the current moment and the third measurement noise at the current moment according to the initial environment measurement noise, the first environmental feature number at the current moment, and the second environmental feature number at the current moment, and calculating the total measurement noise at the current moment according to the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment; The calculating the total measurement noise at the current moment according to the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment includes: presetting the initial fusion weights of the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment; adjusting the initial fusion weights according to the noise levels of the first measurement noise at the current moment, the second measurement noise at the current moment, and the third measurement noise at the current moment to obtain the final fusion weights; and calculating the total measurement noise at the current moment according to the final fusion weights.

2. The positioning method of the robot according to claim 1, characterized in that, The calculating the state pose at the current moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time change amount between the current moment and the previous moment, the linear acceleration, and the angular acceleration includes: Calculating the pose change amount between the current moment and the previous moment according to the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, and the time change amount between the current moment and the previous moment; Calculating the state pose at the current moment according to the pose change amount between the current moment and the previous moment, the state pose at the previous moment, the linear acceleration, the angular acceleration, and the time change amount between the current moment and the previous moment.

3. The positioning method of the robot according to claim 2, characterized in that The calculating the state pose at the current moment according to the pose change amount between the current moment and the previous moment, the state pose at the previous moment, the linear acceleration, the angular acceleration, and the time change amount between the current moment and the previous moment includes: Calculate the state noise at the current moment based on the state pose at the previous moment, the linear acceleration, the angular acceleration, and the time change amount between the current moment and the previous moment; Calculate the state pose at the current moment based on the state pose at the previous moment, the pose change amount between the current moment and the previous moment, and the state noise at the current moment.

4. A positioning method for multiple robots, where the multiple robots are divided into a first robot and a second robot, characterized in that, The method includes: The first robot obtains the relative measurement position information of the second robot relative to the first robot, and obtains the optimal pose of the first robot at the current moment according to the method described in any one of claims 1-3. The first robot sends the obtained relative measurement position information of the second robot relative to the first robot and the optimal pose of the first robot at the current moment to the second robot; The second robot obtains its own state positioning information, and calculates the optimal pose of the second robot at the current moment by combining the relative measurement position information of the second robot relative to the first robot and the optimal pose of the first robot at the current moment.

5. A positioning system for a single robot, characterized in that, Include: An estimation module for collecting data from state sensors and obtaining the state pose at the previous moment, the linear velocity at the current moment, the angular velocity at the current moment, the time change amount between the current moment and the previous moment, the linear acceleration, and the angular acceleration; An observation module for collecting data from measurement sensors and obtaining the flat state measurement noise, the roll attitude angle at the current moment, the pitch angle at the current moment, the initial environment measurement noise, the first environmental characteristic number at the current moment, and the second environmental characteristic number at the current moment; A filtering module for performing an unscented Kalman filter transformation on the data obtained by the estimation module and the observation module; A pose positioning module for calculating the optimal pose of the robot at the current moment based on the data obtained by the filtering module, and performing robot pose positioning according to the optimal pose at the current moment.

6. A positioning system for multiple robots, characterized in that, Include a first robot and a second robot; the first robot includes the estimation module, the observation module, the filtering module, and the pose positioning module described in claim 5; the second robot includes: A self-positioning module for obtaining the yaw angle of the second robot; A receiving module that receives the known pose information of the first robot; A calculation module that calculates the optimal pose of the second robot at the current moment according to the known pose information and the yaw angle.

7. Application of a positioning system for a single robot as described in claim 5 or a positioning system for multiple robots as described in claim 6 in an oil environment.

Images