Freedom positioning system for robot
Technical field
The invention belongs to intelligent robot controller, be specifically related to a kind of combination freedom positioning system, be used for the autonomous positioning of underground environment intelligent robots such as colliery, tunnel based on inertial sensor and 3-dimensional digital compass.
Background technology
The robot location technology is to be used for determining that robot is in the real-time pose of environment, the gordian technique that guided robot arrives impact point.In real work, when robot will independently realize a certain expectation function, primary is to monitor in real time self pose, could come next step movement locus of planning robot according to the difference between current and the expection pose then, and the pose of real-time update oneself.
The location technology of robot has experienced the process that is transformed to the integrated positioning mode by single locator meams.The more locator meams of current application has GPS, images match, inertial navigation etc., and these modes have his own strong points.Independence and bearing accuracy are to estimate two important indicators of positioning system always, also do not have a kind of locator meams can have this two advantages simultaneously at present.Integrate the advantage of multiple location, designing novel integrated positioning system is the main path that improves location independence and precision.
Intelligent robot is used and is rescued field expansions such as detection and location to mine mapping, subterranean tunnel measurement and construction, personnel from traditional commercial Application, and positioning system has been proposed requirements at the higher level, as independence, and real-time etc.Existing location theory and technology can provide reliably and accurately robot posture information under artificial environment, but for the underground environment that has a large amount of unknown messages, there are a lot of limitation in existing localization method, even can't work.
Underground environment operation intelligent robot has proposed to explicitly call for to the independence of its positioning system.In underground destructuring environment, can't receive gps signal, can only utilize robot self internal sensor to obtain motion state, obtain self pose by data processing again.Inertial sensor does not need environmental information to get final product perception, robot measurement kinematic parameter, but fully autonomous all weather operations, fast, the instantaneous precision height of renewal speed.But inertial sensor exists error to accumulate soon, use the preceding shortcoming that needs initial alignment in time, and this has limited the application of inertial sensor in the long-time hi-Fix occasion of needs.The 3-dimensional digital compass is the sensor of a kind of base area signal magnetic field robot measurement 3 d pose, but it can not provide robot motion's acceleration information, signal output frequency low, be subjected to the environmental interference influence of magnetic field easily, the drawbacks limit that these are fatal the widespread use of 3-dimensional digital compass.
For solving the high-precision independent orientation problem of robot, the localization method that mainly adopts inertial navigation to combine traditionally with GPS.This method has the advantages that independence is strong, bearing accuracy is high.In actual use, when gps signal was effective, its locating effect was good.Yet in the environment of no gps signal, though utilize inertial navigation can realize autonomous positioning, its positioning error can sharply be accumulated in time.Image matching method also is a method commonly used in the intelligent robot location, but this mode need be stored matching image or map in advance, could determine the robot current location by images match then.These locator meamss are not suitable for the robot location of working in underground destructuring environment.Therefore, development is a difficult point problem of current research at the freedom positioning system for robot of underground environment, also is intelligent robot technology's an important development direction.
The autonomous positioning technology is that can intelligent robot be finished predict task smoothly and need possess one of key core technology, and there is the contradiction between independence and the precision in existing location technology, can not satisfy the underground environment job requirements.Therefore, characteristics at underground environment, the positioning system that the latest developments of combined with intelligent control and Robotics development, development have independence is an important directions of robot location and navigation technology development, also is an important means that improves the intelligent robot level.
Summary of the invention
The invention provides a kind of freedom positioning system for robot, solve the high-precision independent orientation problem of robot in the underground environment operation.
A kind of freedom positioning system for robot of the present invention comprises mounting platform, is arranged at sensor subsystem, data process subsystem and voltage transformation module on the mounting platform, and mounting platform is demarcated for sensor subsystem simultaneously required sampled point is provided; The real-time sensing robot motion of sensor subsystem status information is carried out denoising, filtering, fusion and calculating through data process subsystem, draws the pose and the velocity information of robot; Voltage transformation module is converted to required operating voltage with vehicle-mounted direct supply; It is characterized in that:
Described mounting platform top is the turntable of carrying sensor subsystem, and the turntable range of movement be ± 360 °, by motor drive module driving, kinematic error is controlled at ± 0.1 ° in; The fixed pedestal of mounting platform bottom is sealing cavity structure, inner installation data processing subsystem and motor drive module;
Described mounting platform has locking device, and an end of locking device is fixedlyed connected with the turntable bottom, and the locking device other end is by the fixed pedestal locking of separable screw rod and mounting platform bottom; When turntable need move on mounting platform, screw rod separated with mounting platform bottom fixed pedestal, the turntable rotation, and after sensor subsystem is demarcated and is finished, screw rod and the locking of mounting platform bottom fixed pedestal;
Described sensor subsystem comprises inertial sensor module and 3-dimensional digital compass, the inertial sensor module is taken into account by three-dimensional acceleration and is connected firmly on its three sensitive axes mutually orthogonal fibre optic gyroscope and form, and measures inertial sensor module references coordinate system with respect to earth inertial coordinates system motion angular velocity and acceleration; The 3 d pose of the relative geographic coordinate system of 3-dimensional digital lining 3-dimensional digital compass reference frame;
Described data process subsystem comprises central controller and is loaded into its inner data preprocessing module, data fusion module and reckoning module; Central controller has multichannel RS-232 serial ports and ethernet network interface, utilizes these interfaces to communicate with sensor subsystem and external unit respectively; Data preprocessing module adopts small wave converting method respectively to the data pre-service of fibre optic gyroscope, three-dimensional accelerometer and 3-dimensional digital compass, reduces data noise, separating vibration and impact; Data fusion module utilizes Kalman's algorithm to merge fibre optic gyroscope and the pretreated data of 3-dimensional digital compass process, obtains the 3 d pose information of mounting platform; The reckoning module adopts the relative positioning mode to calculate and update contruction platform real-time pose and velocity information according to the on-site longitude and latitude of mounting platform and gravitational field vector, initial velocity vector or each speed and pose vector constantly.
Described freedom positioning system for robot is characterized in that, the treatment step of described data preprocessing module is: (1) is carried out the many resolution wavelet of hyperchannel to the data of fibre optic gyroscope, three-dimensional accelerometer and 3-dimensional digital compass and is decomposed; (2), take to force denoising Processing to the high fdrequency component in the aforementioned decomposition data; (3) low frequency component that extracts in the aforementioned decomposition data carries out the reconstruct of many resolution wavelet of hyperchannel; (4) output is through pretreated data.
Described freedom positioning system for robot is characterized in that, the treatment step of described data fusion module is: (1) is to the data computation statistics parameter of fibre optic gyroscope and 3-dimensional digital compass; (2) real-time update Kalman filter parameter; (3) adaptive Kalman filter; (4) calculate the freedom positioning system attitude parameter, it is returned step (2).
Described freedom positioning system for robot, it is characterized in that, the treatment step of described reckoning module is: (1) initialization system parameter, comprise the initial point geographic coordinate system, robot initial angular velocity vector, vector acceleration, linear velocity vector, position vector, local longitude and latitude and gravitational field under the geographic coordinate system;
(2) set up the freedom positioning system for robot coordinate system;
(3) according to step (1), (2), freedom positioning system for robot is carried out initial alignment;
(4) calculated direction transformation matrix;
(5) upgrade with fusion according to previous moment robot pose and sensing data pre-service, calculate robot motion's state parameter under the current time geographic coordinate system, comprise geographic coordinate system roll off the production line vector acceleration, with respect to attitude vector, velocity vector and the motion vector of geographic coordinate system;
(6) robot autonomous locator data storage and renewal, locator data is transferred to external unit by communication interface, and the velocity vector after the renewal, motion vector, attitude vector enter step (4) and circulate.
Described freedom positioning system for robot is characterized in that: described motor drive module is made of the direct current generator that is fixed on the attached code-wheel on the bearing, and direct current generator is fixedly connected by transmission shaft and turntable bottom;
Described freedom positioning system for robot, it is characterized in that: described mounting platform main material is an aluminium alloy, the screw rod that described locking device and mounting platform use is copper screw rod, the integral body of mounting platform adopts the magnetic shield material to coat except that the 3-dimensional digital compass, to isolate the interference of electromagnetic field to the 3-dimensional digital compass.
The present invention utilizes inertial sensor and 3-dimensional digital compass array mode to realize autonomous positioning, cardinal principle and process are as follows: on the one hand, utilize the angular velocity vector of fibre optic gyroscope robot measurement with respect to earth inertial coordinates system, through direction matrixing and rejecting rotational-angular velocity of the earth vector, the calculating robot once can obtain the robot pose vector to time integral then with respect to the angular velocity vector of geographic coordinate system; The attitude vector of the direct robot measurement of 3-dimensional digital compass under geographic coordinate system; Adopt data fusion method calculating robot attitude vector then.On the other hand, the three-dimensional accelerometer robot measurement obtains the linear acceleration vector of robot under geographic coordinate system with respect to the linear acceleration vector of earth inertial coordinates system through the direction matrixing with after rejecting harmful vector acceleration; According to pose, the speed of previous moment robot, to the position of twice calculating robot's current time of time integral; Bring in constant renewal in the circulation of sensing data and double counting process, freedom positioning system can obtain the robot pose in real time.
In the sensing data processing procedure, owing to the uncertainty of sensor measurement, to the reasons such as disappearance that the accurate description of robot motion's process shortage, environmental information are described, sensing data is by a large amount of noise pollutions; The sum of errors noise that contains in the sensing data can be exaggerated when carrying out time integral, and vibration that produces during the robot motion and impact are the reactive component of sensor output data, and these factors have reduced the confidence level of measurement data.The 3-dimensional digital compass in the course of the work, ferrimagnet and interfere with electromagnetic field all produce the accuracy of 3-dimensional digital lining and have a strong impact in the environment, especially the interference of electromagnetic field is difficult to thoroughly eliminate to the interference of 3-dimensional digital compass.Therefore, must carry out various processing to sensing data.After the method that data process subsystem adopts wavelet transformation and Kalman filtering combination merges sensing data pre-service and sensing data, calculate the robot pose through reckoning, and, can obtain the robot posture information in real time according to the sensing data circulation aforementioned calculation process of bringing in constant renewal in; Voltage transformation module provides the operating voltage that meets each subsystem and module.
Freedom positioning system of the present invention is primarily aimed at and is applied to underground environment Work robot autonomous positioning, can be used for underground environment work intelligent robots such as colliery, tunnel, solution cavity (as robot, shield machine, underground engines etc.) autonomous positioning, finish tasks such as exploration, exploration and search at underground environment for intelligent robot effective posture information is provided, solved the autonomous positioning problem of intelligent robot, finished predict task for intelligent robot decision-making foundation is provided in the underground environment operation.
Description of drawings
Fig. 1 The general frame of the present invention;
Fig. 2 mounting platform structural representation of the present invention;
Fig. 3 data process subsystem schematic flow sheet;
Fig. 4 (a) data process subsystem data preprocessing module treatment scheme synoptic diagram;
Fig. 4 (b) data process subsystem data fusion module treatment scheme synoptic diagram;
Fig. 5 reckoning resume module schematic flow sheet;
Fig. 6 voltage transformation module schematic diagram.
Embodiment
Fig. 1 is a The general frame of the present invention, the present invention includes mounting platform, be arranged at sensor subsystem, data process subsystem and voltage transformation module on the mounting platform, the central controller 1 of data process subsystem has RS-232 interface and Ethernet interface, utilize these interfaces, freedom positioning system is real-time transmitted to designated equipment with the robot locating information; Central controller 1 inside is mounted with data preprocessing module 8, data fusion module 9 and reckoning module 10.Sensor subsystem 2 comprises inertial sensor module 3,3-dimensional digital compass 4, the inner integrated respectively internal processor of inertial sensor module 3 and 3-dimensional digital compass 4, internal processor is converted to the sensor measurement data and calculates the digital signal that function is directly transmitted and handled, and outputs to central controller 1 by the RS-232C interface.Inertial sensor module references coordinate system is made of the sensitive axes of fibre optic gyroscope and three-dimensional accelerometer, three sensitive axes of reference frame and inertial sensor module references coordinate system that the sensitive axes of 3-dimensional digital compass 4 constitutes are parallel all the time, and the freedom positioning system reference frame is set to overlap with inertial sensor module references coordinate system.Therefore, fibre optic gyroscope, three-dimensional accelerometer and 3-dimensional digital lining information are angular velocity vector, the linear acceleration vector sum attitude vector of robot in the freedom positioning system reference frame, realize the autonomous positioning function of robot then by data process subsystem.The motor drive module 5 of mounting platform receives the control signal of central controller 1, and is converted to the driving voltage of direct current generator 7, thereby realizes transducer calibration.Voltage transformation module 6 provides positioning system required operating voltage.
Mounting platform of the present invention comprises turntable, fixed pedestal and motor drive module with locking device as shown in Figure 2.Turntable constitutes enclosure space by cover plate 11 and semi-enclosed aluminum barrel 12, and 3-dimensional digital compass 4 is fixedly mounted on cover plate 11 tops.The semi-enclosed aluminum barrel 12 inner inertial sensor modules 3 of installing, the parallelism error between inertial sensor module 3 and 4 two kinds of sensor reference coordinate systems of the 3-dimensional digital compass respective shaft by make and installation be controlled at ± 0.5 °.Turntable can rotation in self axis is made ± 360 ° of scopes under motor-driven, and rotation error is controlled at ± and 0.1 °.Fixed pedestal is that pedestal cover plate 13 and base 14 form the sealing cavity structure, and inside is equipped with motor drive module 5 and central controller 1, and base 14 can connect firmly by bolt with robot body.Motor drive module 5 comprises direct current generator 7, motor hold-down support 15, the transmission shaft 16 of attached code-wheel, is fixedly connected by semi-enclosed aluminum barrel 12 bottoms of transmission shaft 16 with turntable then; Locking device 17 connects firmly with semi-enclosed aluminum barrel 12, realizes locking and release by screw and fixed pedestal then.It is nonferromugnetic material that mounting platform, the motor driven systems assembly except that direct current generator 7 and all connect firmly the bolt material.Except that the 3-dimensional digital compass, the outside all coated magnetic shielding materials of assemblies such as the turntable of mounting platform, fixed pedestal and direct current generator are to isolate inertial sensor, the direct current generator electromagnetic interference (EMI) to 3-dimensional digital compass and central controller.
As shown in Figure 3, the central controller of data process subsystem is a hardware foundation of realizing that sensing data is handled, its inside is mounted with data preprocessing module 8, data fusion module 9 and reckoning module 10, by the sensor raw data is carried out pre-service, fusion and reckoning, obtain the posture information of robot in motion process in real time.
The idiographic flow of data preprocessing module 8 is shown in Fig. 4 (a); The purpose of data preprocessing module is at freedom positioning system characteristics, environment for use and sensor accuracy level, error features, utilize algorithm to the sensing data pre-service, remove or weaken the influence of interference noise sensor accuracy, thus ready for improving the robot bearing accuracy.The pretreated main method of sensing data is many resolution wavelet of hyperchannel decomposition and reconstruction, promptly adopts many resolution wavelet to decompose, and rejects the high fdrequency component in the sensing data, only extracts final low frequency component reconstruct sensing data.After pre-service, sensing data has actual response environment and the time dependent characteristics of sensor, more can react the motion state of robot.Accelerometer data is after data preprocessing module, and its high frequency noise reduces greatly or rejects, and vibration that environment causes and impact Recombinant Interferon are effectively isolated.Fibre optic gyroscope and 3-dimensional digital compass preprocessed data merge through data fusion module, obtain the attitude vector of freedom positioning system for robot.
The idiographic flow of data fusion module 9 is shown in Fig. 4 (b); The core algorithm of data fusion module is an adaptive Kalman fused filtering algorithm, promptly according to sensor preprocessed data and statistical property thereof, utilizes the method estimation robot pose information of optimal estimation.After the sensing data pre-service and merging, the sensing data precision is well improved, and the precision of the freedom positioning system that calculates thus improves greatly.
As shown in Figure 5, the reckoning module 10 of freedom positioning system for robot is a real-time update and cycle calculations module, its major function is according to predefined robot initial pose, geographical environment (comprising longitude, latitude, gravitational field etc.), last one robot speed and posture information and sensor new data etc. more constantly, real-time computer device physiognomy is for the posture information of initial point, and continual renovation robot pose.The reckoning module adopts the relative positioning mode, output be the pose of robot with respect to initial point.The reckoning process of reckoning module is as follows:
(1) initialization system parameter comprises the initial point geographic coordinate system, robot initial angular velocity vector, vector acceleration, linear velocity vector, position vector, local longitude and latitude and gravitational field etc. under the geographic coordinate system;
(2) set up the freedom positioning system for robot coordinate system;
(3) according to (1), (2), freedom positioning system for robot is carried out initial alignment;
(4) calculated direction transformation matrix;
(5) upgrade with fusion according to previous moment robot pose and sensing data pre-service, calculate robot motion's state parameter under the current time geographic coordinate system, comprise geographic coordinate system roll off the production line vector acceleration, with respect to attitude vector, velocity vector and the motion vector of geographic coordinate system;
(6) robot autonomous locator data storage and renewal, locator data is transferred to external unit by communication interface, and the velocity vector after the renewal, motion vector, attitude vector enter step (4) and circulate.
As shown in Figure 6, voltage transformation module provides the voltage that meets the demands for the underground environment freedom positioning system for robot.But this freedom positioning system working voltage scope is+24V~+ the vehicle-mounted direct supply of 35V, required supply voltages such as inner each sensor of freedom positioning system, central controller and motor drive module are+5V ,+two kinds of 12V.Voltage transformation module is by standard coupling head CON2, switch S
1Link to each other with vehicle-mounted direct supply, through 7812 chip U
1With 7805 chip U
2Carry vehicle-mounted direct supply is converted to respectively+12V and+5V voltage, wherein among the figure GND for holding altogether.
The specific embodiment of this example:
The maximum physical dimension of freedom positioning system for robot profile is 250mm * 250mm * 320mm (length * wide * height).
Inertial sensor module: adopt the fast XWVG7200 that reaches the research and development of development in science and technology company limited of Beijing StarNet.
3-dimensional digital compass: the HMR3000 3-dimensional digital compass of U.S. Honeywell company.
Central controller: adopt the PCM-3341 embedded system of Advantech incorporated company, this system possesses the software and hardware condition of data processing, and has 4 RS-232 interface and 1 Ethernet interface.Numeral I/O module PC3730 provide A/D translation interface.
Voltage transformation module: adopt 7805 chips and 7812 chips for freedom positioning system provide+5V ,+12V voltage.
Motor drive module: adopt the direct current generator of subsidiary rotary encoder, the operating voltage of direct current generator is+12V.The A/D interface of the control signal from processor of motor is through the driving circuit drive motor.
Data process subsystem: utilize the method for many resolution wavelet conversion reconstruct sensing data low frequency amount, effectively cancelling noise and interference.Fibre optic gyroscope and 3-dimensional digital compass data are merged by Kalman's fused filtering algorithm after pre-service, obtain the attitude vector of freedom positioning system for robot, and as next attitude parameter of direction transformation matrix constantly.Accelerometer data enters the reckoning module behind pre-service and direction transformation matrix.And along with the renewal of sensing data, data process subsystem constantly circulates, real-time computer device people pose.