CN107861501A - Underground sewage treatment works intelligent robot automatic positioning navigation system - Google Patents
Underground sewage treatment works intelligent robot automatic positioning navigation system Download PDFInfo
- Publication number
- CN107861501A CN107861501A CN201710988719.4A CN201710988719A CN107861501A CN 107861501 A CN107861501 A CN 107861501A CN 201710988719 A CN201710988719 A CN 201710988719A CN 107861501 A CN107861501 A CN 107861501A
- Authority
- CN
- China
- Prior art keywords
- intelligent robot
- moment
- positioning navigation
- coordinate
- time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/0088—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- H—ELECTRICITY
- H02—GENERATION; CONVERSION OR DISTRIBUTION OF ELECTRIC POWER
- H02P—CONTROL OR REGULATION OF ELECTRIC MOTORS, ELECTRIC GENERATORS OR DYNAMO-ELECTRIC CONVERTERS; CONTROLLING TRANSFORMERS, REACTORS OR CHOKE COILS
- H02P29/00—Arrangements for regulating or controlling electric motors, appropriate for both AC and DC motors
Abstract
Underground sewage treatment works intelligent robot automatic positioning navigation system belongs to intelligent robot multi sensor combination positioning and navigation field.By the realization of functional module, hardware platform is built with reference to laser sensor and vision sensor, forms the intelligent robot automatic positioning navigation system of complete set, realizes the autonomous positioning Navigation Control of underground sewage treatment works intelligent robot.Underground sewage treatment works intelligent robot automatic positioning navigation system is applied to intelligent robot inspection process, the accurate control to intelligent robot is realized by the accurate control to servomotor.Autonomous positioning Navigation Control can not be realized to being fluctuated with illumination condition for conventional laser navigation and vision guided navigation, barrier dynamic change, the problem of underground inspection process stabilization for the features such as environmental aspect is unstable and accurate control, the intelligent control to intelligent robot location navigation is realized, improves the accuracy and stability of autonomous positioning navigation.
Description
Technical Field
According to the invention, the autonomous positioning navigation system of the intelligent robot of the underground sewage treatment plant is designed, and a hardware platform is built by combining a laser detector and a visual sensor to form a set of complete autonomous positioning navigation system of the intelligent robot, so that autonomous positioning navigation control of the intelligent robot of the underground sewage treatment plant is realized. The autonomous positioning navigation system of the intelligent robot of the underground sewage treatment plant is applied to the inspection process of the intelligent robot, and the intelligent robot is accurately controlled by accurately controlling the servo motor. An intelligent robot autonomous positioning navigation control of an underground sewage treatment plant belongs to the field of intelligent robot multi-sensor combined positioning navigation.
Background
With the development of economy, the acceleration of urbanization process, the high scarcity of large and medium-sized urban land resources and the increasingly prominent environmental pollution problem, the underground sewage treatment plant has the advantages of saving land resources, reducing pipe network investment, reducing vision, noise and gas pollution, stable operation, constant temperature and the like, and becomes the development and construction trend of novel sewage treatment plants in the global scope.
The underground sewage treatment plant adopts a totally closed design, the generated toxic and harmful gas has leakage risk, and the noise generated in the sewage treatment process can influence workers. Therefore, the intelligent inspection of the running state of the underground sewage treatment plant is a premise for realizing safe, stable and efficient running of the sewage treatment plant, and is an effective way for monitoring the running condition of the sewage treatment process in real time and accurately controlling the sewage treatment on line. With the increasing maturity of mobile robot technology development, the application of the industry to robots is also more and more extensive. The research is based on the autonomous positioning navigation system of the intelligent robot of the underground sewage treatment plant, and the information complementation and fusion of the multiple sensors of the intelligent robot improve the perception capability of the robot to the surrounding environment, acquire comprehensive and accurate information, form a set of complete autonomous positioning navigation system, ensure the safe, stable and efficient operation of the sewage treatment process, and become more and more urgent needs for the healthy operation of the underground sewage treatment plant.
The autonomous positioning navigation system is mainly used for controlling the positioning and navigation of the intelligent robot of the underground sewage treatment plant by adjusting the advancing direction, the speed, the position and the like of a servo motor of the intelligent robot, so that the uniqueness and the limitation of a single detector of the robot are eliminated, different sensor information is combined together, a plurality of information sources are used for mutual supplement, and a multifunctional system with higher redundancy and accuracy is formed. Compared with the traditional positioning navigation system with a single sensor, the autonomous positioning navigation system has considerable advantages in the aspects of stability, control precision and anti-interference capability.
The invention relates to design and research of an intelligent robot autonomous positioning navigation system of an underground sewage treatment plant, the positioning navigation system builds hardware platforms such as data acquisition, data transmission, intelligent control and the like, and data acquisition, data transmission and control signal issuing and execution are realized through a communication network. Through the development and integration of each functional module, an autonomous positioning navigation system is formed, the stability and the reliability of control are improved, and the positioning navigation precision of the intelligent robot of the underground sewage treatment plant is ensured.
Disclosure of Invention
1. An intelligent robot autonomous positioning navigation system of an underground sewage treatment plant,
the method is characterized by comprising the following steps:
(1) The hardware comprises a detection instrument, a data acquisition module, a data processing and storage module, a power management module, a wireless communication module, a central processor module and a drive module, and is specifically realized as follows:
the detection instrument comprises a laser, a laser detector, a measurement circuit and a visual sensor; the detection instrument and the data processing and storing module are in information communication through a communication interface, the data processing and storing module is connected with the central processing module, meanwhile, the power management module and the wireless transmission module are connected with the central processing unit, and the central processing unit sends a control signal to the servo motor of the actuating mechanism through the driving module;
adjusting and controlling the coordinate position in the autonomous positioning navigation, wherein the rotation speed and the direction of a servo motor are used as control quantity, and the coordinate position is controlled quantity; the method is characterized by comprising the following steps:
(1)designing an autonomous positioning navigation model based on a UKF filtering algorithm, wherein the input of the autonomous positioning navigation model is u k ＝[u k 1 ，u k 2 ]，(u k 1 ，u k 2 ) Is the coordinate of the intelligent robot at the moment k, u k 1 And u k 2 Respectively are the horizontal and vertical coordinates of the intelligent robot at the moment k; the output of the positioning navigation model is o k+1 ＝[o k+1 1 ，o k+1 2 ]Wherein (o) k+1 1 ，o k+1 2 ) Is the output coordinate of the intelligent robot at the moment k +1, o k+1 1 And o k+1 2 Respectively are the horizontal and vertical coordinates of the intelligent robot at the moment k + 1; the calculation method is as follows:
(1) the output of the autonomous positioning navigation model is represented as follows:
u k+1 ＝A k u k +w k (1)
o k+1 ＝B k+1 u k+1 +v k+1 (2)
wherein u is k Is the coordinate of the intelligent robot at the moment k, u k+1 Coordinate of the intelligent robot at time k +1, o k+1 Is the output coordinate of the intelligent robot at the time of k +1, A k Is the state transition matrix from time k to time k +1, B k For the predicted output and input coordinates at time k, B k+1 Transition matrix for predicted output and input coordinates at time k +1, w k Is the process noise at time k, v k+1 The noise is measured at the moment of k +1, and the two are zero mean white noise which are mutually uncorrelated;
(2) defining a cost function J of a control system k
Wherein o is k Is the output coordinate of the intelligent robot at the moment k,o k is o k Optimal prediction of (2); j. the design is a square k The error value of the coordinate at the moment k is obtained;
(3) updating coordinates of an autonomous positioning navigation model
u k ＝u k +C k (o k -o k ) (5)
Wherein the content of the first and second substances,u k is u k Optimal prediction of (C) k As a gain array, D k Is a weight matrix, P k Is u k The statistical property matrix of (a) is,is P k The inverse of the matrix of (a) is,P k is P k T is the transpose of the matrix; the related variable update formula is as follows:
wherein E is k And F k-1 Are respectively w k And v k-1 The covariance matrix of (a);as the weight coefficient,to find the weight coefficients for the first order statistical properties,for solving the weight coefficient of the second-order statistical characteristic, the calculation formula is as follows:
wherein t, i and j are state vectors, n is the number of the state vectors, and λ = a 2 n, a =0.5 determines the degree of spread of the constellation points, and β is u k U is the mean value of the coordinates at time k.
(4) Judging the size of the target function of the positioning navigation system at the current moment, if J k If the value is more than or equal to 0.01, repeating the step (3); if J k &If it is 0.01, go to step (1) to calculate the output o of autonomous positioning navigation k ；
(2) Designing an autonomous positioning navigation controller based on a UKF filtering algorithm, and inputting the autonomous positioning navigation controllerIs the state vector of the system at time k, (x) k 1 ，y k 1 ) Is the coordinate in the geodetic coordinate system of the intelligent robot, (x) k 2 ，y k 2 ) As coordinates in the geodetic coordinate system of the target point, where x k 1 And x k 2 As abscissa, y, of the corresponding geodetic coordinate system k 1 And y k 2 As ordinate of the corresponding geodetic coordinate system, m k 1 Is the velocity component, v, of the intelligent robot at the moment k in the horizontal coordinate direction k 1 The velocity component of the intelligent robot in the ordinate direction at the moment k,the course angle of the intelligent robot at the moment k is obtained; positioning navigation controller outputIs the measurement vector of the k-time system, wherein (x) k+1 1 ，y k+1 1 ) Acquiring coordinates of the intelligent robot in a geodetic coordinate system for the laser sensor at the moment k +1, (x) k+1 2 ，y k+1 2 ) Acquiring the coordinates of a target point in a geodetic coordinate system for a vision sensor at the moment k +1, m k+1 1 Acquiring the speed component v of the intelligent robot in the ordinate direction for the laser sensor k+1 1 The speed component of the intelligent robot in the ordinate direction is obtained for the laser sensor,the course angle of the intelligent robot is obtained for the laser sensor, and the calculation mode is as follows:
(1) the output of the autonomous position navigation controller is represented as follows:
x k+1 ＝H k x k +w k (11)
z k+1 ＝L k+1 x k+1 +v k+1 (12)
wherein x is k Is the state vector of the system at time k, z k+1 Is the measurement vector of the k +1 time system, H k Is a state transition matrix from the time k to the time k +1, | delta k | < 1 is a state transition matrix parameter value, L k A transition matrix, w, of the measurement vector and the state vector at time k k And v k Respectively representing process noise and measurement noise at the time k, wherein the process noise and the measurement noise are zero mean white noise which are mutually irrelevant;
(3) definition controlSystematic cost function J k
Wherein z is k Is the measurement vector of the system at the time k,z k is z k Optimal prediction of (1), J k The error value of the measurement vector at the moment k;
(3) updating state vectors of autonomous positioning navigation controllers
x k ＝x k +R k (z k -z k ) (16)
Wherein the content of the first and second substances,x k is x k Is optimally predicted, R k Is a Kalman filter gain array, M k For the filter gain coefficient matrix, N k Is x k The variance matrix of (a) is calculated,N k is N k T is the transpose of the matrix; e k And F k-1 Are respectively w k And v k-1 The covariance matrix of (a);as the weight coefficient,to find the weight coefficients for the first order statistical properties,for solving the weight coefficient of the second-order statistical characteristic, the calculation formula is as follows:
wherein t, i and j are state vectors, n is the number of the state vectors, and λ = a 2 n, a =0.5 determines the degree of scattering of the constellation points, β being x k X is the mean value of the state vectors at time k.
(4) Judging the size of the target function of the positioning navigation system at the current moment, if J k If the value is more than or equal to 0.01, repeating the step (3); if J k &If the value is 0.01, the step (1) is carried out to calculate the output z of the autonomous positioning navigation k ；
(3) Using the solved z k Control of the servo motor, z k The rotation speed and direction at the moment k are the control quantity, and the output of the control system is the actual coordinate, speed and heading.
The invention is mainly characterized in that:
(1) The invention aims at the problem that the routing inspection process of the intelligent robot in the underground sewage treatment plant is a process with the characteristics of fluctuation of illumination conditions, dynamic change of obstacles, unstable environmental conditions and the like, the positioning navigation of the intelligent robot needs to be controlled within a reasonable range, and the routing inspection course is difficult to be stably and efficiently controlled under the condition according to the traditional positioning navigation control method in the prior art; according to the combinability and accuracy of multi-sensor fusion, an autonomous positioning navigation controller is designed, the online control of positioning navigation is realized, and the method has the characteristics of good stability, good real-time performance, high control precision and the like;
(2) The invention designs the autonomous positioning navigation controller, the control method better solves the problem that the nonlinear system is difficult to control, and the real-time accurate control of the positioning navigation is realized; the intelligent robot solves the problem of safe and stable inspection of the intelligent robot under the complex environment condition of the underground sewage treatment plant, and has the characteristics of safety, reliability and stability;
drawings
FIG. 1 is a block diagram of a control system of the present invention
FIG. 2 is a diagram of a control system model of the present invention
Detailed Description
The invention obtains the autonomous positioning navigation system of the intelligent robot of the underground sewage treatment plant, and realizes the accurate control of the positioning navigation of the intelligent robot: the system is used for controlling, so that the problem that the intelligent robot is difficult to accurately control is solved, and the accuracy of positioning navigation control is improved;
the invention adopts the following technical scheme and implementation steps:
the design of the autonomous positioning navigation control system mainly designs a hardware system of the control system by providing an online control decision based on a UKF algorithm, and a structure diagram of the control system is given as shown in FIG. 1, and the system comprises a plurality of main functional modules including a detection instrument, a data acquisition module, a data processing and storage module, a power management module, a wireless communication module, a central processor module and a driving module, and is specifically realized as follows:
the robot instrument comprises a laser, a laser detector, a measuring circuit and a visual sensor; the detection instrument and the data processing and storage module are in information communication through a communication interface, the data processing and storage module is connected with the central processing module, meanwhile, the power management module and the wireless transmission module are connected with the central processing unit, and after the central processing unit gives out a corresponding control strategy on line, a control signal is sent to the servo motor of the actuating mechanism through the driving module;
adjusting and controlling the coordinate position in the autonomous positioning navigation, wherein the rotation speed and the direction of a servo motor are used as control quantity, and the coordinate position is controlled quantity;
(1) Designing an autonomous positioning navigation model based on a UKF filtering algorithm, wherein the input of the autonomous positioning navigation model is u k ＝[u k 1 ，u k 2 ]，(u k 1 ，u k 2 ) Is the coordinate of the intelligent robot at the moment k, u k 1 And u k 2 Respectively are the horizontal and vertical coordinates of the intelligent robot at the moment k; the output of the positioning navigation model is o k+1 ＝[o k+1 1 ，o k+1 2 ]Wherein (o) k+1 1 ，o k+1 2 ) Is the output coordinate of the intelligent robot at the moment k +1, o k+1 1 And o k+1 2 Respectively are the horizontal and vertical coordinates of the intelligent robot at the moment k + 1; the calculation method is as follows:
(1) the output of the autonomous positioning navigation model is represented as follows:
u k+1 ＝A k u k +w k (1)
o k+1 ＝B k+1 u k+1 +v k+1 (2)
wherein u is k Is the coordinate of the intelligent robot at the moment k, u k+1 Coordinate of the intelligent robot at time k +1, o k+1 Is the output coordinate of the intelligent robot at the moment k +1, A k Is the state transition matrix from time k to time k +1, B k For the predicted output and input coordinates at time k, B k+1 Transition matrix for predicted output and input coordinates at time k +1, w k Is the process noise at time k, v k+1 Measured noise at time k +1, bothZero-mean white noise which is uncorrelated with each other;
(2) defining a cost function J of a control system k
Wherein o is k Is the output coordinate of the intelligent robot at the moment k,o k is o k Optimal prediction of (2); j is a unit of k The error value of the coordinate at the moment k is obtained;
(3) updating coordinates of an autonomous positioning navigation model
u k ＝u k +C k (o k -o k ) (5)
Wherein the content of the first and second substances,u k is u k Optimal prediction of (C) k As a gain array, D k Is a weight matrix, P k Is u k The statistical property matrix of (a) is,is P k The inverse of the matrix of (a) is,P k is P k T is the transpose of the matrix; the related variable update formula is as follows:
wherein, E k And F k-1 Are respectively w k And v k-1 The covariance matrix of (a);as the weight coefficient,to find the weight coefficients for the first order statistical properties,for solving the weight coefficient of the second-order statistical characteristic, the calculation formula is as follows:
wherein t, i and j are state vectors, n is the number of the state vectors, and λ = a 2 n, a =0.5 determines the degree of spread of the constellation points, and β is u k U is the mean value of the coordinates at time k.
(4) Judging the size of the target function of the positioning navigation system at the current moment, if J k If the temperature is more than or equal to 0.01, repeating the step (3); if J is k &If the position is not less than 0.01, the step (1) is carried out to calculate the output o of the autonomous positioning navigation k ；
(2) Designing an autonomous positioning navigation controller based on a UKF filtering algorithm, and inputting the autonomous positioning navigation controllerIs the state vector of the system at time k, (x) k 1 ，y k 1 ) Is the coordinate in the geodetic coordinate system of the intelligent robot, (x) k 2 ，y k 2 ) As coordinates in the geodetic coordinate system of the target point, where x k 1 And x k 2 As the abscissa, y, of the corresponding geodetic coordinate system k 1 And y k 2 As ordinate of the corresponding geodetic coordinate system, m k 1 For k time intelligenceVelocity component, v, of the robot in the direction of the abscissa k 1 The velocity component of the intelligent robot in the ordinate direction at the moment k,the course angle of the intelligent robot at the moment k is obtained; positioning navigation controller outputIs the measurement vector of the k-time system, wherein (x) k+1 1 ，y k+1 1 ) Acquiring coordinates of the intelligent robot in a geodetic coordinate system for the laser sensor at the moment k +1, (x) k+1 2 ，y k+1 2 ) Acquiring the coordinates of a target point in a geodetic coordinate system for a vision sensor at the moment k +1, m k+1 1 Acquiring the speed component v of the intelligent robot in the ordinate direction for the laser sensor k+1 1 The speed component of the intelligent robot in the ordinate direction is acquired for the laser sensor,the course angle of the intelligent robot is obtained for the laser sensor, and the calculation mode is as follows:
(1) the output of the autonomous position navigation controller is represented as follows:
x k+1 ＝H k x k +w k (11)
z k+1 ＝L k+1 x k+1 +v k+1 (12)
wherein x is k Is the state vector of the system at time k, z k+1 Is the measurement vector of the k +1 time system, H k Is a state transition matrix from the time k to the time k +1, | delta k | < 1 is a state transition matrix parameter value, L k A transition matrix, w, of the measurement vector and the state vector at time k k And v k Respectively representing process noise and measurement noise at the time k, wherein the process noise and the measurement noise are zero mean white noise which are mutually irrelevant;
(3) defining a cost function J of a control system k
Wherein z is k Is the measurement vector of the system at the time k,z k is z k Optimal prediction of (1), J k An error value of a measurement vector at the time k;
(3) updating state vectors of autonomous positioning navigation controllers
x k ＝x k +R k (z k -z k ) (16)
Wherein the content of the first and second substances,x k is x k Is optimally predicted, R k Is a Kalman filter gain array, M k For the filter gain coefficient matrix, N k Is x k The variance matrix of (a) is calculated,N k is N k T is the transpose of the matrix; e k And F k-1 Are respectively w k And v k-1 The covariance matrix of (a);as the weight coefficient,to find the weight coefficients for the first order statistical properties,for solving the weight coefficient of the second-order statistical characteristic, the calculation formula is as follows:
wherein t, i and j are state vectors, n is the number of the state vectors, and λ = a 2 n, a =0.5 determines the degree of scattering of the constellation points, β being x k X is the mean value of the state vectors at time k.
(4) Judging the size of the target function of the positioning navigation system at the current moment, if J k If the temperature is more than or equal to 0.01, repeating the step (3); if J k &If lt, 0.01, go to step (1) to calculate the output z of autonomous positioning navigation k ；
(3) Using solved z k Control of the servo motor, z k The output of the control system is the actual coordinate, speed and heading, which are the rotating speed and direction at the moment k, namely the control quantity.
Claims (1)
1. The intelligent robot autonomous positioning navigation system of the underground sewage treatment plant is characterized in that:
the hardware comprises a detection instrument, a data acquisition module, a data processing and storage module, a power management module, a wireless communication module, a central processor module and a plurality of functional modules of a driving module, and the hardware is specifically realized as follows:
the detection instrument comprises a laser, a laser detector, a measurement circuit and a visual sensor; the detection instrument is in information communication with the data processing and storage module through a communication interface, the data processing and storage module is connected with the central processing module, meanwhile, the power management module and the wireless transmission module are connected with the central processing unit, and the central processing unit sends a control signal to the servo motor of the actuating mechanism through the driving module;
adjusting and controlling the coordinate position in the autonomous positioning navigation, wherein the rotation speed and the direction of a servo motor are used as control quantity, and the coordinate position is controlled quantity;
the method comprises the following steps:
(1) Designing an autonomous positioning navigation model based on a UKF filtering algorithm, wherein the input of the autonomous positioning navigation model is u k ＝[u k 1 ，u k 2 ]，(u k 1 ，u k 2 ) Is the coordinate of the intelligent robot at the moment k, u k 1 And u k 2 Respectively are the horizontal and vertical coordinates of the intelligent robot at the moment k; the output of the positioning navigation model is o k+1 ＝[o k+1 1 ，o k+1 2 ]Wherein (o) k+1 1 ，o k+1 2 ) Is the output coordinate of the intelligent robot at the moment k +1, o k+1 1 And o k+1 2 Respectively are the horizontal and vertical coordinates of the intelligent robot at the moment k + 1; the calculation method is as follows:
(1) the output of the autonomous positioning navigation model is represented as follows:
u k+1 ＝A k u k +w k (1)
o k+1 ＝B k+1 u k+1 +v k+1 (2)
wherein u is k Is the coordinate of the intelligent robot at the moment k, u k+1 Coordinate of the intelligent robot at time k +1, o k+1 Is the output coordinate of the intelligent robot at the moment k +1, A k Is the state transition matrix from time k to time k +1, B k For the transfer matrix of the predicted output and input coordinates at time k, B k+1 Transition matrix for predicted output and input coordinates at time k +1, w k Is the process noise at time k, v k+1 The noise is the measurement noise at the moment of k +1, and the two are zero mean white noise which are mutually irrelevant;
(2) defining a cost function J of a control system k
Wherein o is k Is the output coordinate of the intelligent robot at the moment k,o k is o k Optimal prediction of (2); j is a unit of k The error value of the coordinate at the moment k is obtained;
(3) updating coordinates of an autonomous positioning navigation model
u k ＝u k +C k (o k -o k ) (5)
Wherein the content of the first and second substances,u k is u k Optimal prediction of (C) k As a gain array, D k As a weight matrix, P k Is u k Statistical property matrix of, P k -1 Is P k The inverse of the matrix of (a) is,P k is P k T is the transpose of the matrix; the related variable update formula is as follows:
wherein, E k And F k-1 Are respectively w k And v k-1 The covariance matrix of (a); eta k i As a weight coefficient, gamma i m For the calculation of the weight coefficient in the first order statistical properties, gamma i c In order to obtain the weight coefficient of the second-order statistical characteristic, the calculation formula is as follows:
wherein t, i and j are state vectors, n is the number of the state vectors, and λ = a 2 n, a =0.5 determines the degree of spread of the constellation points, and β is u k U is the mean value of the coordinates at the moment k;
(4) judging the size of the target function of the positioning navigation system at the current moment, if J k If the value is more than or equal to 0.01, repeating the step (3); if J is k &If it is 0.01, go to step (1) to calculate the output o of autonomous positioning navigation k ；
(2) Designing an autonomous positioning navigation controller based on a UKF filtering algorithm, and inputting the autonomous positioning navigation controller Is the state vector of the system at time k, (x) k 1 ，y k 1 ) Is the coordinate in the geodetic coordinate system of the intelligent robot (x) k 2 ，y k 2 ) As coordinates in the geodetic coordinate system of the target point, where x k 1 And x k 2 As the abscissa, y, of the corresponding geodetic coordinate system k 1 And y k 2 As ordinate of the corresponding geodetic coordinate system, m k 1 Is the velocity component, v, of the intelligent robot at the moment k in the horizontal coordinate direction k 1 The velocity component of the intelligent robot in the ordinate direction at the moment k,the course angle of the intelligent robot at the moment k is obtained; positioning navigation controller outputIs the measurement vector of the k-time system, wherein (x) k+1 1 ，y k+1 1 ) Acquiring coordinates of the intelligent robot in a geodetic coordinate system for the laser sensor at the moment k +1, (x) k+1 2 ，y k+1 2 ) Acquiring the coordinate of a target point in a geodetic coordinate system for the vision sensor at the moment k +1, m k+1 1 Acquiring the speed component v of the intelligent robot in the ordinate direction for the laser sensor k+1 1 The speed component of the intelligent robot in the ordinate direction is obtained for the laser sensor,the course angle of the intelligent robot is obtained for the laser sensor, and the calculation mode is as follows:
(1) the output of the autonomous position navigation controller is represented as follows:
x k+1 ＝H k x k +w k (11)
z k+1 ＝L k+1 x k+1 +v k+1 (12)
wherein x is k Is the state vector of the system at time k, z k+1 Is the measurement vector of the k +1 time system, H k Is a state transition matrix from the time k to the time k +1, | delta k | < 1 is a state transition matrix parameter value, L k A transition matrix, w, of the measurement vector and the state vector at time k k And v k Respectively representing process noise and measurement noise at the moment k, wherein the process noise and the measurement noise are zero mean white noise which are mutually irrelevant;
(3) defining a cost function J of a control system k
Wherein z is k Is the measurement vector of the system at the time k,z k is z k Optimal prediction of (1), J k The error value of the measurement vector at the moment k;
(3) updating state vectors of autonomous positioning navigation controllers
x k ＝x k +R k (z k -z k ) (16)
Wherein the content of the first and second substances,x k is x k Is optimally predicted, R k Is a Kalman filter gain array, M k For the filter gain coefficient matrix, N k Is x k The variance matrix of (a) is calculated,N k is N k T is the transpose of the matrix; e k And F k-1 Are respectively w k And v k-1 The covariance matrix of (a); eta k i As a weight coefficient, gamma i m To find the weight coefficient in the first order statistical properties, gamma i c In order to obtain the weight coefficient of the second-order statistical characteristic, the calculation formula is as follows:
wherein t, i and j are state vectors, n is the number of the state vectors, and λ = a 2 n, a =0.5 determines the degree of scattering of the constellation points, β being x k X is the mean value of the state vector at the moment k;
(4) judging the size of the target function of the positioning navigation system at the current moment, if J k If the value is more than or equal to 0.01, repeating the step (3); if J k &If the value is 0.01, the step (1) is carried out to calculate the output z of the autonomous positioning navigation k ；
(3) Using solved z k Control of the servo motor, z k The rotation speed and direction at the moment k are the control quantity, and the output of the control system is the actual coordinate, speed and heading.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710988719.4A CN107861501A (en) | 2017-10-22 | 2017-10-22 | Underground sewage treatment works intelligent robot automatic positioning navigation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710988719.4A CN107861501A (en) | 2017-10-22 | 2017-10-22 | Underground sewage treatment works intelligent robot automatic positioning navigation system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107861501A true CN107861501A (en) | 2018-03-30 |
Family
ID=61696732
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710988719.4A Pending CN107861501A (en) | 2017-10-22 | 2017-10-22 | Underground sewage treatment works intelligent robot automatic positioning navigation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107861501A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109506657A (en) * | 2018-12-18 | 2019-03-22 | 盐城汇金科技信息咨询服务有限公司 | A kind of automatic deviation correction track and correction localization method based on AGV |
| CN111624875A (en) * | 2019-02-27 | 2020-09-04 | 北京京东尚科信息技术有限公司 | Visual servo control method and device and unmanned equipment |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102288176A (en) * | 2011-07-07 | 2011-12-21 | 中国矿业大学(北京) | Coal mine disaster relief robot navigation system based on information integration and method |
| KR20140070722A (en) * | 2012-11-26 | 2014-06-11 | 한국전자통신연구원 | Integration apparatus for multi-rate system and method thereof |
| CN104932522A (en) * | 2015-05-27 | 2015-09-23 | 深圳市大疆创新科技有限公司 | Autonomous landing method and system for aircraft |
| CN105371840A (en) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | Method for combined navigation of inertia/visual odometer/laser radar |
| CN106093951A (en) * | 2016-06-06 | 2016-11-09 | 清华大学 | Object tracking methods based on array of ultrasonic sensors |
| CN106708066A (en) * | 2015-12-20 | 2017-05-24 | 中国电子科技集团公司第二十研究所 | Autonomous landing method of unmanned aerial vehicle based on vision/inertial navigation |
| CN107193283A (en) * | 2017-07-27 | 2017-09-22 | 青岛诺动机器人有限公司 | The mobile robot and its operating method of a kind of independent navigation |
| CN107192387A (en) * | 2017-05-23 | 2017-09-22 | 北京理工大学 | A kind of combined positioning method based on Unscented kalman filtering |
-
2017
- 2017-10-22 CN CN201710988719.4A patent/CN107861501A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102288176A (en) * | 2011-07-07 | 2011-12-21 | 中国矿业大学(北京) | Coal mine disaster relief robot navigation system based on information integration and method |
| KR20140070722A (en) * | 2012-11-26 | 2014-06-11 | 한국전자통신연구원 | Integration apparatus for multi-rate system and method thereof |
| CN104932522A (en) * | 2015-05-27 | 2015-09-23 | 深圳市大疆创新科技有限公司 | Autonomous landing method and system for aircraft |
| CN105371840A (en) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | Method for combined navigation of inertia/visual odometer/laser radar |
| CN106708066A (en) * | 2015-12-20 | 2017-05-24 | 中国电子科技集团公司第二十研究所 | Autonomous landing method of unmanned aerial vehicle based on vision/inertial navigation |
| CN106093951A (en) * | 2016-06-06 | 2016-11-09 | 清华大学 | Object tracking methods based on array of ultrasonic sensors |
| CN107192387A (en) * | 2017-05-23 | 2017-09-22 | 北京理工大学 | A kind of combined positioning method based on Unscented kalman filtering |
| CN107193283A (en) * | 2017-07-27 | 2017-09-22 | 青岛诺动机器人有限公司 | The mobile robot and its operating method of a kind of independent navigation |
Non-Patent Citations (4)
| Title |
|---|
| PAUL VASILYEV,SEAN PEARSON,: "Inertial and time-of-arrival ranging sensor fusion", 《GAIT & POSTURE》 * |
| 冯肖维: "基于多传感器信息融合的移动机器人位姿计算方法研究", 《中国博士学位论文全文数据库 信息科技辑》 * |
| 谭玉新: "基于工业互联网的煤矿井下机器人导航与无线视频监控技术研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅰ辑》 * |
| 龙慧 等: "迭代卡尔曼滤波在机器人定位中的应用", 《现代电子技术》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109506657A (en) * | 2018-12-18 | 2019-03-22 | 盐城汇金科技信息咨询服务有限公司 | A kind of automatic deviation correction track and correction localization method based on AGV |
| CN111624875A (en) * | 2019-02-27 | 2020-09-04 | 北京京东尚科信息技术有限公司 | Visual servo control method and device and unmanned equipment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113189977B (en) | Intelligent navigation path planning system and method for robot | |
| CN110988894B (en) | Port environment-oriented multi-source data fusion unmanned automobile real-time positioning method | |
| CN104964683B (en) | A kind of closed-loop corrected method of indoor environment map building | |
| CN108759822B (en) | Mobile robot 3D positioning system | |
| CN113359710A (en) | LOS theory-based agricultural machinery path tracking method | |
| CN110895146A (en) | Synchronous positioning and map construction method for mobile robot | |
| CN107861501A (en) | Underground sewage treatment works intelligent robot automatic positioning navigation system | |
| CN112214014A (en) | Automatic driving control method and system for agricultural machinery | |
| CN111811503B (en) | Unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional code | |
| Yong et al. | An autonomous navigation strategy based on improved hector slam with dynamic weighted a* algorithm | |
| Che et al. | A wall-following navigation method for autonomous driving based on lidar in tunnel scenes | |
| CN112560571A (en) | Intelligent autonomous visual navigation method based on convolutional neural network | |
| Li et al. | Development of the automatic navigation system for combine harvester based on GNSS | |
| Tang et al. | SLAM with improved schmidt orthogonal unscented Kalman filter | |
| Zhou et al. | Localization of coal mine rescue robots based on multi-sensor fusion | |
| Bakirci et al. | Improving position-time trajectory accuracy in vehicle stop-and-go scenarios by using a mobile robot as a testbed | |
| Du et al. | Research on navigation system of AMR based on ROS | |
| Qi et al. | Research on SLAM of mobile robot based on fusion of laser and vision | |
| Chabok et al. | Sensor Data Fusion for Positioning of Agriculture Mobile Robot using Dempster-Shafer Method | |
| Dong | Path Planning Algorithm Based on Visual Image Feature Extraction for Mobile Robots | |
| Zhang et al. | The research for a kind of information fusion model based on BP neural network with multi position sources and big data selection | |
| Akkaya et al. | An ANN based NARX GPS/DR system for mobile robot positioning and obstacle avoidance | |
| CN113093743B (en) | Navigation control method based on virtual radar model and deep neural network | |
| CN114932562B (en) | Underground cable tunnel inspection robot based on laser radar and implementation method | |
| Li et al. | Three-dimensional positioning of unmanned vehicle in field based on unscented Kalman Filter |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180330 |