CN110058594A - The localization for Mobile Robot navigation system and method for multisensor based on teaching - Google Patents
The localization for Mobile Robot navigation system and method for multisensor based on teaching Download PDFInfo
- Publication number
- CN110058594A CN110058594A CN201910348071.3A CN201910348071A CN110058594A CN 110058594 A CN110058594 A CN 110058594A CN 201910348071 A CN201910348071 A CN 201910348071A CN 110058594 A CN110058594 A CN 110058594A
- Authority
- CN
- China
- Prior art keywords
- navigation
- connect
- module
- personal computer
- industrial personal
- 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
- 238000000034 method Methods 0.000 title claims abstract description 21
- 230000004807 localization Effects 0.000 title claims abstract description 18
- 230000033001 locomotion Effects 0.000 claims abstract description 37
- 230000000694 effects Effects 0.000 claims abstract description 5
- 238000012544 monitoring process Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000009466 transformation Effects 0.000 claims description 9
- 238000011161 development Methods 0.000 claims description 8
- 230000005484 gravity Effects 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 6
- 238000013459 approach Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 239000003638 chemical reducing agent Substances 0.000 claims description 3
- 238000004891 communication Methods 0.000 claims description 3
- 238000003780 insertion Methods 0.000 claims 1
- 230000037431 insertion Effects 0.000 claims 1
- 230000005540 biological transmission Effects 0.000 abstract description 2
- 230000007613 environmental effect Effects 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 7
- 230000004927 fusion Effects 0.000 description 6
- 241001269238 Data Species 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000037361 pathway Effects 0.000 description 4
- QSHDDOUJBYECFT-UHFFFAOYSA-N mercury Chemical compound [Hg] QSHDDOUJBYECFT-UHFFFAOYSA-N 0.000 description 3
- 229910052753 mercury Inorganic materials 0.000 description 3
- 238000009987 spinning Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000008901 benefit Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 210000004556 brain Anatomy 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000000465 moulding Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000004321 preservation Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a kind of localization for Mobile Robot navigation system of multisensor based on teaching and methods, including upper layer navigation module, Lower layer motion control module, handle, computer；The handle provides the enabled effect of moving of car control；The computer is for monitoring and debugging machine people；The Lower layer motion control module is for realizing the motion control of trolley platform, the reading and transmission of display trolley status information and encoder data, the upper layer navigation module is for acquiring point cloud data and speed data, the present invention is constituted rationally, succinctly, method is convenient, the navigation of indoor AGV can be rapidly carried out with the present invention, layman is easy to get started, easy to operate, easy to use.The method computational efficiency of its system is high, and can be with the path of autonomous learning and traversal arbitrary shape, while according to three-dimensional laser data and can also satisfy the need velocity magnitude by the location information that odometer information provides and direction is modified.
Description
Technical field
The invention belongs to industrial automation, in particular to the mobile robot of a kind of multisensor based on teaching is fixed
Position navigation system and method.
Background technique
With the raising of human cost and the development of science and technology, robot (especially industrial robot) is industrial automatic
Change field is more and more widely used.Self-navigation carrier (Automated Guided Vehicle hereinafter referred to as AGV)
It is the important link in modern intelligent logistics system referred to as transfer robot, however the AGV guidance technology of current mainstream is not
It can be made entirely autonomous from truly realizing at work.
Navigation is the most important mark of AGV technology development as one of core technology.Conventional navigation mode is with electromagnetic navigation
It is representative with tape navigation, using physical pathway, environment representation is simple, is not necessarily to automatic map building technology, and when positioning only needs
The deviation between car body and physical pathway is detected by sensor.With the development of science and technology laser sensor, imaging sensor by
Step is applied in AGV airmanship.Due to high with positioning accuracy, without being transformed to ground, the high advantage of routing flexibility,
AGV airmanship, that is, laser navigation based on laser radar is studied extensively and application of succeeding.With conventional navigation mode
It compares, it is more that laser navigation is related to face, and technology is complicated.Firstly, physical pathway is not present in laser navigation, therefore without image of Buddha conventional navigation
Mode directly detects the deviation between car body and physical pathway like that.Therefore in laser navigation, AGV is real-time by laser radar
Environmental data is acquired, by comparing the processing result of environmental data with the environmental map established in advance obtained from ground
Pose estimation in figure, this pose estimation are that the direct deviation calculating in contrast locating conventional navigation mode wants complicated.Secondly, swashing
Map used in light guide, which has, significantly estimates information, needs to carry out accurate Drawing, the environmental map precision shadow created
Ring AGV positioning accuracy.
In circumstances not known, due to wall etc. in investigative range and the measurement accuracy limitation of environment sensing sensor, environment
Object to sensor detect the problems such as blocking, robot can not pass through one-shot measurement create global context map.Therefore, machine
People can only obtain enough environment sensing data in constantly environment heuristic process, could complete the creation of global context map
Work.For the robot in the environment local map of each position creation, robot each position is only known exactly which
Pose the local map could be converted to global map, i.e. the creation of map depends on the pose of robot.However it is real
Situation is, what the pose of robot was obtained often by environmental map, i.e., the pose of robot depends on environmental map.Pose
This complementary relationship between map estimation proposes new project to navigation of the robot under circumstances not known, claims
Ask for " simultaneous localization and mapping " (Simultaneous Localization and Mapping hereinafter referred to as SLAM)
Topic.Therefore the final purpose of SLAM is the consistent environmental map of creation, if can be realized correct closed loop is method self performance
A kind of embodiment；And the map creating method of position error is eliminated based on closed loop, it is missed by pose estimated by flying track conjecture
In the biggish situation of difference, constraint condition can only be established by householder method, i.e., under the premise of establishing correct closed loop, then by excellent
Change method corrects robot pose and creation environmental map.Therefore such realize of slam system needs to construct globally consistent ground
Therefore figure brings higher calculating cost.
Summary of the invention
For overcome the deficiencies in the prior art, the invention proposes a kind of moving machines based on the multisensor based on teaching
Device people Position Fixing Navigation System and method.
The technical scheme adopted by the invention is as follows: a kind of localization for Mobile Robot based on the multisensor based on teaching is led
Boat system includes: upper layer navigation module, Lower layer motion control module, handle and computer；
The handle is connect with Lower layer motion control module, and the Lower layer motion control module and upper layer navigation module connect
It connects, the upper layer navigation module is connect with computer；
The upper layer navigation module includes router, odometer, three-dimensional laser, on-vehicle battery and boost module；
The Lower layer motion control module includes: that industrial personal computer, TTL turn USB module, router, self-navigation carrier
AGV, photoelectric encoder, DC speed-reducing and driver, embedded board and power supply；
The power supply is connect with self-navigation carrier AGV, the photoelectric encoder and DC speed-reducing and drive
Dynamic device is placed in steering wheel trolley, the photoelectric encoder is connect with odometer, the DC speed-reducing and driver with
Embedded board connection, the embedded board turn USB module with TTL and connect, and the TTL turns USB module and industrial personal computer
Connection；The handle is connect with industrial personal computer；
The on-vehicle battery is connect with three-dimensional laser, odometer, router and boost module respectively, the boost module
It is also connect with industrial personal computer, the three-dimensional laser is also connect with router, and the odometer is also connect with router, the router
It is connect respectively with industrial personal computer, computer；
The handle provides the enabled effect of moving of car control；
The computer is for monitoring and debugging machine people；
The TTL turns USB module for realizing the communication of embedded board and industrial personal computer；
The boost module is for realizing the normal voltage for converting the voltage of on-vehicle battery to industrial personal computer；
The power supply provides electric energy for self-navigation carrier AGV；
The industrial personal computer is communicated by router and three-dimensional laser, obtains three-dimensional laser three-dimensional point cloud generated
And the pose of calculating robot；
The photoelectric encoder is for calculating the circle number that self-navigation carrier AGV wheel turns over；
The DC speed-reducing and driver is the power drive unit of self-navigation carrier AGV；
The embedded development version is the basic motion control module of self-navigation carrier AGV；
The odometer calculates the mileage information of robot according to the data of photoelectric encoder；
A kind of localization for Mobile Robot air navigation aid of the multisensor based on teaching, using above-mentioned a kind of based on teaching
Multisensor localization for Mobile Robot navigation system realize, comprising the following steps:
Step 1, in teaching phase, speed command is sent to by industrial personal computer by handle control, industrial personal computer sends instruction
Turn USB module to TTL, TTL turns USB module and movement instruction is sent to embedded board STM32, embedded board again
The instruction received is sent to direct current speed reducer and driver to control robot motion by STM32；In the mistake of robot motion
Cheng Zhong, photoelectric encoder acquire velocity information to odometer, three-dimensional laser collection point cloud data information, odometer and three-dimensional laser
The information of acquisition is sent to industrial personal computer and computer by router, also will record point cloud data simultaneously；After the completion of teaching, stop
It only records, point cloud data will record in the database；
Step 2, in duplication stages, the order in teaching process is played back again by lever knob；
The point cloud that step 3, self-navigation carrier read machine people currently obtain, and the point that it is saved with teaching phase
Cloud is compared, and calculates the transformation of the three-dimensional point cloud from the three-dimensional point cloud of reading to record, to calculate the position of robot
Appearance；
The pose Solve problems of robot are converted into mathematical problem,
Q=[qR|qT]T
Wherein NpFor points, xiAnd piFor matched point pair, quaternary number be rotation a kind of expression-form, rotate herein
Vector qRIt is expressed with quaternary number, qτIt is translation vector, qRAnd qτCollectively constitute world coordinates transformation vector q, R (qR) it is rotating vector
qRCorresponding spin matrix, f (q) are to solve a q to match to obtain xiWith piBetween apart from minimum function.
The point coordinate in point cloud data target point set P that step 3.1 is obtained according to current robot is stored in teaching phase
Point cloud point collection curved surface S on search for corresponding closest approach point set and form reference set X；
Step 3.2 calculates target point set P and the center of gravity with reference to point set X: wherein μp、μxThe respectively center of gravity of point set P and X, Np
And NxThe respectively points of point set P and X；
Step 3.3 constructs covariance matrix by point set P and X
Step 3.4 constructs 4 × 4 symmetrical matrixes by covariance matrix:
Wherein, I3It is 3x3 unit matrix,
tr(∑P, X) it is matrix ∑P, XMark,Matrix A=∑P, X+∑P, X T, AijThe i-th row of representing matrix A,
The element of jth column；
Step 3.5 calculates Q (∑P, X) characteristic value and feature vector, the corresponding feature vector [q of maximum eigenvalue0 q1
q2 q3]TAs best rotating vectorWherein q0、q1、q2、q3For quaternary number；
Step 3.6 calculates best translation vector
Wherein
Step 3.7 obtainsAcquire least mean-square error dmin=f (q)；
The pose that the world coordinates being calculated transformation vector is converted to robot is sent embedded development by step 4
It is complete using the error for the corresponding pose that the current pose of position pid algorithm Compensating Robot and teaching phase store in plate STM32
At the navigation and positioning of mobile robot；
Advantageous effects of the invention: present system theory combines reality, constitutes rationally, succinctly, and method is convenient, fortune
The navigation of indoor AGV can be rapidly carried out with the present invention, layman is easy to get started, easy to operate, easy to use.
This method computational efficiency is high, is not necessarily to Calibrate camera, and can be with the path of autonomous learning and traversal arbitrary shape.
In teaching phase, robot is driven by a human operator, robot can store its speed, laser scanning figure and in
Journey meter information.In independent navigation, this method is not necessarily to explicit robot localization in two dimension even three-dimensional space, only needs to play back
In the speed of teaching phase study, while can also satisfy the need according to three-dimensional laser data and by the location information that odometer information generates
Velocity magnitude and direction are modified.Therefore, robot can be repeatedly along expected path automatic Pilot without artificial
Intervene.
Detailed description of the invention
Fig. 1 is that the present invention is based on the localization for Mobile Robot of the Multi-sensor Fusion of teaching and navigation system general construction frame
Figure；
Fig. 2 is that the present invention is based on the localization for Mobile Robot of the Multi-sensor Fusion of teaching and navigation system three-dimensional point cloud are raw
At schematic diagram；
Fig. 3 is that the present invention is based on the localization for Mobile Robot of the Multi-sensor Fusion of teaching and navigation system software frame to show
It is intended to；
Fig. 4 is to connect to show with navigation system hardware the present invention is based on the localization for Mobile Robot of the Multi-sensor Fusion of teaching
It is intended to；
Fig. 5 is the method flow diagram of the specific embodiment of the invention；
Fig. 6 is that the present invention is based on the localization for Mobile Robot of the Multi-sensor Fusion of teaching and the positioning of navigation system software to calculate
Core algorithm ICP algorithm flow chart in method；
In figure, ENCA1, ENCB1, ENCC1, ENCD1, which are four road PWM as DC speed-reducing and driver, must control letter
Number.
Specific embodiment
With reference to the accompanying drawings and examples, explanation that the present invention will be further explained.
The system summary structure frame of the present embodiment as shown in Figure 1, embedded board model STM32, the type of industrial personal computer
Number to account for beautiful gk400, the model that TTL turns USB module is CH340G, and the model of three-dimensional laser is VLP-16, the type of on-vehicle battery
Number be YSN-1211000, model MKX-DC12V~19V of boost module, the model Kai Meiwei 12V100A of on-vehicle battery,
The model SKYRC IMAX B6 of power supply, the model MERCURY MW325R of router, the model Sony of handle
PS3.In the present embodiment, the connection relationship of each component are as follows:
A kind of localization for Mobile Robot navigation system of the multisensor based on teaching: upper layer navigation module, Lower layer motion
Control module, handle and computer；
Handle is connect with lower layer's control module, and lower layer's control module is connect with upper layer navigation module, upper layer navigation module with
Computer connection；
Lower layer motion control module is for realizing the motion control of trolley platform, display trolley status information and encoder
The reading and transmission of data, including industrial personal computer, TTL turn USB module, self-navigation carrier AGV, photoelectric encoder, direct current and subtract
Speed motor and driver, embedded board and power supply；
Upper layer navigation module is for acquiring point cloud data comprising including router, odometer, three-dimensional laser, vehicle mounted electric
Pond and boost module；
Power supply is connect with self-navigation carrier AGV, and photoelectric encoder and DC speed-reducing and driver are arranged
In steering wheel trolley, the connection of photoelectric encoder and odometer, DC speed-reducing and driver are connect with embedded board,
Embedded board turns USB module with TTL and connect, and TTL turns USB module and connect with industrial personal computer；Handle is connect with industrial personal computer；
On-vehicle battery is connect with three-dimensional laser, odometer, router and boost module respectively, boost module also with industry control
Machine connection, three-dimensional laser also connect with router, and odometer is also connect with router, router respectively with industrial personal computer, computer
Connection；Boost module MKX-DC12V~19V, three-dimensional laser VLP-16, router MERCURY MW325R and odometer are straight
On-vehicle battery Kai Meiwei 12V100A is met in succession by its power supply.
Handle provides the enabled effect of moving of car control；
Computer is for monitoring and debugging machine people；
TTL turns USB module for realizing the communication of embedded board and industrial personal computer；
Boost module is for realizing the normal voltage for converting the voltage of on-vehicle battery to industrial personal computer；
Power supply provides electric energy for AGV trolley；
Industrial personal computer is communicated by router and three-dimensional laser, is obtained three-dimensional laser three-dimensional point cloud generated and is calculated
The pose of robot；
Photoelectric encoder is for calculating the circle number that AGV wheel turns over；
The power drive unit that DC speed-reducing and driver are AGV；
Embedded development version is the basic motion control module of AGV；
Odometer calculates the mileage information of robot according to the data of photoelectric encoder；
Industrial personal computer is communicated by router and three-dimensional laser row, after obtaining three-dimensional laser three-dimensional point cloud generated and running
Continuous algorithm routine；
Fig. 4 is the hardware connection diagram of lower layer's navigation elements, and it is whole for carrying the beautiful gk400 industrial personal computer that accounts for of linux system
The brain of a device navigation elements, it on the one hand pass through TTL turn USB module CH340G give STM32 send control instruction, can
Enough front, rear, left and right or spinning motion, on the other hand by router read mileage counts data with laser radar, such as
Fig. 2 three-dimensional point cloud generates shown in schematic diagram, and positioning and navigation then are realized in the fusion of both sensing datas by algorithm.
Laser radar is fixed by screws in above radar support frame, and radar support frame passes through on screw and universal wheel trolley
Layer platform connection；The supply lines of laser radar connects a layer on-vehicle battery Kai Meiwei 12V100A, and data line connects upper level router
MERCURYMW325R (connection on-vehicle battery Kai Meiwei 12V100A power supply)；The port on-vehicle battery 12V connects lower layer's boost module
MKX-DC12V~19V, rear industrial personal computer of transferring account for beautiful gk400；LAN mouthfuls of two of router MERCURY MW325R, one connects three
Laser VLP-16 is tieed up, another connects industrial personal computer and accounts for beautiful gk400；Pass through the wired connection of router, it is ensured that laser radar with
Stability, safety and the real-time that data are transmitted between industrial personal computer, the teaching software to run on industrial personal computer is handled in real time to swash
A large amount of three dimensional point clouds that optical radar is sent provide effective support.
Store Navigator in industrial personal computer, embedded board STM32 stores motion control program, Navigator with
Motion control program is to be write based on ROS operating system with C Plus Plus；Contain the navigation module and basic motion on upper layer
Control module two parts realize the entire run of whole system, and the program of navigation module is run in industrial personal computer at the middle and upper levels, bottom
Layer motion-control module is run on embedded board STM32；
The software frame of this example as shown in figure 3, in teaching phase, with handle drives robot in front, rear, left and right and
Mobile free routing on the basis of spinning motion, and by path point coordinate, that is, robot pose and three-dimensional laser point cloud data and
Rate control instruction is stored in the database on industrial personal computer, and in duplication stages, the navigation module searching database on upper layer will be counted
According to the path point coordinate reference path saved in library, the speed of storage is as reference velocity, by the point cloud of the preservation in database
Data determine the current pose of robot and the robot pose meter with storage compared with the point cloud data that robot currently obtains
It calculates a deviation to be added in reference velocity instruction as compensation, is then sent to basic motion control module；Basic motion control
Molding block all executes in STM32, including motor motion control program and data read transmitting/receiving program, wherein the number with industrial personal computer
SCIP2.0 agreement is based on according to transmitting/receiving program to write；Lower layer motion control module receives the rate control instruction of upper layer navigation module,
By the characteristic of universal wheel, front, rear, left and right and the spinning motion of AGV are realized；Four independent wheel photoelectric encoder values
Upper layer navigation module is transmitted to by the tcp/ip communication program to be resolved, and obtains the odometer information of trolley.Upper layer navigation
Module is divided into teaching and is repeated two stages based on the ROS operating system under Linux environment.
As shown in figure 5, based on the teaching air navigation aid that laser and odometer position, specific steps are as follows:
Step 1, in teaching phase, speed command is sent to by industrial personal computer by handle control, industrial personal computer sends instruction
Turn USB module to TTL, TTL turns USB module and movement instruction is sent to embedded board STM32, embedded board again
The instruction received is sent to direct current speed reducer and driver to control robot motion by STM32；In the mistake of robot motion
Cheng Zhong, photoelectric encoder acquire velocity information to odometer, three-dimensional laser collection point cloud data information, odometer and three-dimensional laser
The information of acquisition is sent to industrial personal computer and computer by router, also will record point cloud data simultaneously；After the completion of teaching, stop
It only records, point cloud data will record in the database；
Step 2, in duplication stages, the order in teaching process will be played back again by lever knob；
Step 3, operation location algorithm module, the point cloud that self-navigation carrier read machine people currently obtains, and by its
The point cloud saved with teaching phase is compared, and calculates the transformation of the three-dimensional point cloud from the three-dimensional point cloud of reading to record, from
And calculate the pose of robot；
The pose Solve problems of robot are converted into mathematical problem,
Q=[qR|qT]T
Wherein NpFor points, xiAnd piFor matched point pair, quaternary number be rotation a kind of expression-form, rotate herein
Vector qRIt is expressed with quaternary number, qτIt is translation vector, qRAnd qτCollectively constitute world coordinates transformation vector q, R (qR) it is rotating vector
qRCorresponding spin matrix, f (q) are to solve a q to match to obtain xiWith piBetween apart from minimum function.
Step 3.1 is being taught as shown in fig. 6, according to the point coordinate in the point cloud data target point set P of current robot acquisition
Corresponding closest approach point set, which is searched for, on the point cloud point collection curved surface S of stage storage forms reference set X；
Step 3.2 calculates target point set P and the center of gravity with reference to point set X: wherein μp、μxThe respectively center of gravity of point set P and X, Np
And NxThe respectively points of point set P and X；
Step 3.3 constructs covariance matrix by point set P and X
Step 3.4 constructs 4 × 4 symmetrical matrixes by covariance matrix:
Wherein, I3It is 3x3 unit matrix,
tr(∑FX) it is matrix ∑P, XMark,Matrix A=∑P, X+∑P, X T, AijThe i-th row of representing matrix A,
The element of jth column；
Step 3.5 calculates Q (∑F, X) characteristic value and feature vector, the corresponding feature vector [q of maximum eigenvalue0 q1
q2 q3]TAs best rotating vector qR=[q0q1q2q3]T；Wherein q0、q1、q2、q3For quaternary number；
Step 3.6 calculates best translation vector
Wherein
Step 3.7 obtains q=[qR|qT]r, acquire least mean-square error dmin=f (q)；
The pose that the world coordinates being calculated transformation vector is converted to robot is subsequently sent to industrial personal computer by step 4,
Industrial personal computer will control the error for the corresponding pose that the current pose of position pid algorithm Compensating Robot and teaching phase store, completion
The navigation and positioning of mobile robot；
The core algorithm of this system is based on iteration closest approach (Interative Closet Points, abbreviation ICP) point
Cloud matching algorithm.
Three-dimensional point cloud matching is a very important intermediate steps, it is fixed in resurfacing, Three-dimension object recognition, camera
There is extremely important application in the problems such as position.For three-dimensional point cloud matching problem, researcher proposes many solutions, but
Be it is most widely used, influence maximum or ICP algorithm, it is the three-dimensional point cloud matching algorithm based on pure geometrical model,
Due to its power and pinpoint accuracy, become the mainstream algorithm in point cloud matching soon.
Wherein motor motion control program is as follows:
1. retrieving the current pose q of robot from databasek_expectWith 3 dimension point cloud datas, by 3 dimension point cloud datas and
The 3 dimension point cloud datas that robot currently obtains are sent to location algorithm module and obtain the pose relative to database point cloud of robot
qk_calc, the input deviation of PID position ring is calculated, wherein k indicates the kth moment,
Δqk=qk_expect-qk_calc；
2. the output of the formula calculating position ring according to PID, wherein upk, KppAnd KpiRespectively indicate the defeated of PID position ring
Out, the ratio and integral coefficient of PID,
3. utilizing the current desired speed v for retrieving robot in the databasek_expectMachine is obtained with by odometer
The current true velocity v of peoplek_truthAnd the output of position PID loop calculates its deviation deltavk, wherein k indicates current time, and α indicates position
The output of ring is set in deviation deltavkIn weight,
Δvk=α upk+vk_expect-vk_truth,
4. the PID of calculating speed ring is exported, wherein uvk, Kvp, Kvd, KviIt respectively indicates speed ring PID to export, proportionality coefficient,
Differential coefficient and integral coefficient,
5. final output speed is calculated, wherein final output speed VkIt indicates,
Vk=vk_exect+uvk。
Claims (3)
1. a kind of localization for Mobile Robot navigation system of the multisensor based on teaching, which is characterized in that navigate including upper layer
Module, Lower layer motion control module, handle and computer；
The handle is connect with Lower layer motion control module, and the Lower layer motion control module is connect with upper layer navigation module, institute
Upper layer navigation module is stated to connect with computer；
The upper layer navigation module includes router, odometer, three-dimensional laser, on-vehicle battery and boost module；
The on-vehicle battery is connect with three-dimensional laser, odometer, router and boost module respectively, the boost module also with
Industrial personal computer connection, the three-dimensional laser are also connect with router, and the odometer is also connect with router, the router difference
It is connect with industrial personal computer, computer；
The Lower layer motion control module includes: that industrial personal computer, TTL turn USB module, router, self-navigation carrier AGV, light
Photoelectric coder, DC speed-reducing and driver, embedded board and power supply；
The power supply is connect with self-navigation carrier AGV, the photoelectric encoder and DC speed-reducing and driver
It is placed in steering wheel trolley, the photoelectric encoder is connect with odometer, the DC speed-reducing and driver and insertion
The connection of formula development board, the embedded board turn USB module with TTL and connect, and the TTL turns USB module and connect with industrial personal computer；
The handle is connect with industrial personal computer；
The on-vehicle battery is connect with three-dimensional laser, odometer, router and boost module respectively, the boost module also with
Industrial personal computer connection, the three-dimensional laser are also connect with router, and the odometer is also connect with router, the router difference
It is connect with industrial personal computer, computer；
The handle provides the enabled effect of moving of car control；
The computer is for monitoring and debugging machine people；
The TTL turns USB module for realizing the communication of embedded board and industrial personal computer；
The boost module is for realizing the normal voltage for converting the voltage of on-vehicle battery to industrial personal computer；
The power supply provides electric energy for self-navigation carrier AGV；
The industrial personal computer is communicated by router and three-dimensional laser, is obtained three-dimensional laser three-dimensional point cloud generated and is counted
Calculate the pose of robot；
The photoelectric encoder is for calculating the circle number that self-navigation carrier AGV wheel turns over；
The DC speed-reducing and driver is the power drive unit of self-navigation carrier AGV；
The embedded development version is the basic motion control module of self-navigation carrier AGV；
The odometer calculates the mileage information of robot according to the data of photoelectric encoder.
2. a kind of localization for Mobile Robot air navigation aid of the multisensor based on teaching, using one kind described in claim 1
The localization for Mobile Robot navigation system of multisensor based on teaching is realized, which comprises the following steps:
Step 1, in teaching phase, controlled by handle and speed command be sent to industrial personal computer, industrial personal computer sends an instruction to TTL
Turn USB module, TTL turns USB module and movement instruction is sent to embedded board STM32 again, and embedded board STM32 will
The instruction received is sent to direct current speed reducer and driver to control robot motion；During robot motion, light
Photoelectric coder acquires velocity information to odometer, three-dimensional laser collection point cloud data information, then by odometer and three-dimensional laser
The information of acquisition is sent to industrial personal computer and computer by router, also will record point cloud data simultaneously；After the completion of teaching, stop
It records, point cloud data will record in the database；
Step 2, in duplication stages, the order in teaching process is played back again by lever knob；
The point cloud that step 3, self-navigation carrier read machine people currently obtain, and the point cloud that it is saved with teaching phase into
Row compares, and calculates the transformation of the three-dimensional point cloud from the three-dimensional point cloud of reading to record, to calculate the pose of robot；
Q=[qR|qT]T
Wherein NpFor points, xiAnd piFor matched point pair, quaternary number be rotation a kind of expression-form, rotating vector q hereinR
It is expressed with quaternary number, qTIt is translation vector, qRAnd qTCollectively constitute world coordinates transformation vector q, R (qR) it is rotating vector qRIt is corresponding
Spin matrix, f (q) be solve a q to match to obtain xiWith piBetween apart from minimum function；
The point coordinate in point cloud data target point set P that step 3.1 is obtained according to current robot, in the point of teaching phase storage
Corresponding closest approach point set is searched on cloud point collection curved surface S forms reference set X；
Step 3.2 calculates target point set P and the center of gravity with reference to point set X: wherein μp、μxThe respectively center of gravity of point set P and X, NpAnd Nx
The respectively points of point set P and X；
Step 3.3 constructs covariance matrix by point set P and X
Step 3.4 constructs 4 × 4 symmetrical matrixes by covariance matrix:
Wherein, l3It is 3x3 unit matrix, tr
(∑F, x) it is matrix ∑P, XMark,Matrix A=∑P, X+∑P, X T,AijThe i-th row of representing matrix A,
The element of jth column；
Step 3.5 calculates Q (∑P, X) characteristic value and feature vector, the corresponding feature vector [q of maximum eigenvalue0 q1 q2
q3]TAs best rotating vectorWherein q0、q1、q2、q3For quaternary number；
Step 3.6 calculates best translation vector
Wherein
Step 3.7 obtainsAcquire least mean-square error dmin=f (q)；
The pose that the world coordinates being calculated transformation vector is converted to robot is sent embedded board by step 4
In STM32, using the error for the corresponding pose that the current pose of position pid algorithm Compensating Robot and teaching phase store, complete
The navigation and positioning of mobile robot.
3. a kind of teaching navigation system positioned based on laser and odometer according to claim 1, which is characterized in that institute
The algorithm routine stated is based on ROS operating system, is write using C Plus Plus.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910348071.3A CN110058594A (en) | 2019-04-28 | 2019-04-28 | The localization for Mobile Robot navigation system and method for multisensor based on teaching |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910348071.3A CN110058594A (en) | 2019-04-28 | 2019-04-28 | The localization for Mobile Robot navigation system and method for multisensor based on teaching |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110058594A true CN110058594A (en) | 2019-07-26 |
Family
ID=67319577
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910348071.3A Pending CN110058594A (en) | 2019-04-28 | 2019-04-28 | The localization for Mobile Robot navigation system and method for multisensor based on teaching |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110058594A (en) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110234029A (en) * | 2019-07-31 | 2019-09-13 | 上海商汤视觉科技有限公司 | The play handling method and device of multi-sensor data |
| CN110568848A (en) * | 2019-09-10 | 2019-12-13 | 东风商用车有限公司 | teaching automatic driving operation system of sweeper |
| CN112179346A (en) * | 2020-09-15 | 2021-01-05 | 国营芜湖机械厂 | Indoor navigation system of unmanned trolley and use method thereof |
| CN113946156A (en) * | 2021-12-20 | 2022-01-18 | 广州朗国电子科技股份有限公司 | Motion path teaching control method and control system of wheeled robot |
| CN114148836A (en) * | 2021-11-08 | 2022-03-08 | 中国科学院自动化研究所 | Robot autonomous ladder taking method and device |
| US20220353492A1 (en) * | 2019-06-25 | 2022-11-03 | Sony Group Corporation | Information processing device and method |
| US11636612B2 (en) | 2020-09-25 | 2023-04-25 | Industrial Technology Research Institute | Automated guided vehicle navigation device and method thereof |
| CN114265375B (en) * | 2021-11-25 | 2024-06-04 | 云南昆船智能装备有限公司 | System and method for storing and taking goods of flat truck by AGV |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100013153A1 (en) * | 2003-02-26 | 2010-01-21 | Silverbrook Research Pty Ltd | Game System With Robotic Game Pieces |
| CN104359464A (en) * | 2014-11-02 | 2015-02-18 | 天津理工大学 | Mobile robot positioning method based on stereoscopic vision |
| CN104657981A (en) * | 2015-01-07 | 2015-05-27 | 大连理工大学 | Dynamic compensation method for three-dimensional laser distance metering data of mobile robot in moving process |
| CN108317953A (en) * | 2018-01-19 | 2018-07-24 | 东北电力大学 | A kind of binocular vision target surface 3D detection methods and system based on unmanned plane |
| CN108919810A (en) * | 2018-07-26 | 2018-11-30 | 东北大学 | The localization for Mobile Robot and navigation system of view-based access control model teaching |
| CN108917759A (en) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | Mobile robot pose correct algorithm based on multi-level map match |
| CN109358340A (en) * | 2018-08-27 | 2019-02-19 | 广州大学 | A kind of AGV indoor map construction method and system based on laser radar |
-
2019
- 2019-04-28 CN CN201910348071.3A patent/CN110058594A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100013153A1 (en) * | 2003-02-26 | 2010-01-21 | Silverbrook Research Pty Ltd | Game System With Robotic Game Pieces |
| CN104359464A (en) * | 2014-11-02 | 2015-02-18 | 天津理工大学 | Mobile robot positioning method based on stereoscopic vision |
| CN104657981A (en) * | 2015-01-07 | 2015-05-27 | 大连理工大学 | Dynamic compensation method for three-dimensional laser distance metering data of mobile robot in moving process |
| CN108317953A (en) * | 2018-01-19 | 2018-07-24 | 东北电力大学 | A kind of binocular vision target surface 3D detection methods and system based on unmanned plane |
| CN108917759A (en) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | Mobile robot pose correct algorithm based on multi-level map match |
| CN108919810A (en) * | 2018-07-26 | 2018-11-30 | 东北大学 | The localization for Mobile Robot and navigation system of view-based access control model teaching |
| CN109358340A (en) * | 2018-08-27 | 2019-02-19 | 广州大学 | A kind of AGV indoor map construction method and system based on laser radar |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220353492A1 (en) * | 2019-06-25 | 2022-11-03 | Sony Group Corporation | Information processing device and method |
| US11991348B2 (en) * | 2019-06-25 | 2024-05-21 | Sony Group Corporation | Information processing device and method |
| CN110234029A (en) * | 2019-07-31 | 2019-09-13 | 上海商汤视觉科技有限公司 | The play handling method and device of multi-sensor data |
| CN110234029B (en) * | 2019-07-31 | 2021-12-17 | 上海商汤临港智能科技有限公司 | Playing processing method, device, equipment and storage medium of multi-sensor data |
| CN110568848A (en) * | 2019-09-10 | 2019-12-13 | 东风商用车有限公司 | teaching automatic driving operation system of sweeper |
| CN110568848B (en) * | 2019-09-10 | 2022-09-23 | 东风商用车有限公司 | Teaching automatic driving operation system of sweeper |
| CN112179346A (en) * | 2020-09-15 | 2021-01-05 | 国营芜湖机械厂 | Indoor navigation system of unmanned trolley and use method thereof |
| CN112179346B (en) * | 2020-09-15 | 2024-02-27 | 国营芜湖机械厂 | Indoor navigation system of unmanned trolley and application method thereof |
| US11636612B2 (en) | 2020-09-25 | 2023-04-25 | Industrial Technology Research Institute | Automated guided vehicle navigation device and method thereof |
| CN114148836A (en) * | 2021-11-08 | 2022-03-08 | 中国科学院自动化研究所 | Robot autonomous ladder taking method and device |
| CN114265375B (en) * | 2021-11-25 | 2024-06-04 | 云南昆船智能装备有限公司 | System and method for storing and taking goods of flat truck by AGV |
| CN113946156A (en) * | 2021-12-20 | 2022-01-18 | 广州朗国电子科技股份有限公司 | Motion path teaching control method and control system of wheeled robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110058594A (en) | The localization for Mobile Robot navigation system and method for multisensor based on teaching | |
| Royer et al. | Outdoor autonomous navigation using monocular vision | |
| CN109599945A (en) | A kind of autonomous crusing robot cruising inspection system of wisdom power plant and method | |
| CN112484725A (en) | Intelligent automobile high-precision positioning and space-time situation safety method based on multi-sensor fusion | |
| Pfaff et al. | Towards mapping of cities | |
| CN104914865A (en) | Transformer station inspection tour robot positioning navigation system and method | |
| CN109460052A (en) | A kind of control method for spelling group aircraft | |
| CN112734841B (en) | Method for realizing positioning by using wheel type odometer-IMU and monocular camera | |
| CN113189613B (en) | Robot positioning method based on particle filtering | |
| CN108919810A (en) | The localization for Mobile Robot and navigation system of view-based access control model teaching | |
| CN110039536A (en) | The auto-navigation robot system and image matching method of indoor map construction and positioning | |
| CN108759822A (en) | A kind of mobile robot 3D positioning systems | |
| CN113093756A (en) | Indoor navigation robot based on laser SLAM under raspberry group platform | |
| CN118020038A (en) | Two-wheeled self-balancing robot | |
| Basiuk et al. | Mobile Robot Position Determining Using Odometry Method | |
| CN114879699A (en) | Autonomous navigation system and method for buried pipeline field inspection robot | |
| Lamon et al. | Mapping with an autonomous car | |
| Stachniss et al. | How to learn accurate grid maps with a humanoid | |
| Su et al. | GR-SLAM: Vision-based sensor fusion SLAM for ground robots on complex terrain | |
| Chi et al. | Low-latency Visual-based High-Quality 3D Reconstruction using Point Cloud Optimization | |
| Zhu et al. | Indoor Localization Method of Mobile Educational Robot Based on Visual Sensor | |
| CN109542094A (en) | Mobile robot visual point stabilization without desired image | |
| CN114003041A (en) | Multi-unmanned vehicle cooperative detection system | |
| Xu et al. | Research on orb-slam autonomous navigation algorithm | |
| CN113341968A (en) | Accurate parking system and method for multi-axis flat car |
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: 20190726 |