CN110455294A - Implementation method based on the multithreading distribution SLAM system under ROS environment - Google Patents

Implementation method based on the multithreading distribution SLAM system under ROS environment Download PDF

Info

Publication number
CN110455294A
CN110455294A CN201910804060.1A CN201910804060A CN110455294A CN 110455294 A CN110455294 A CN 110455294A CN 201910804060 A CN201910804060 A CN 201910804060A CN 110455294 A CN110455294 A CN 110455294A
Authority
CN
China
Prior art keywords
robot
road sign
subfilter
information
state
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
Application number
CN201910804060.1A
Other languages
Chinese (zh)
Inventor
裴福俊
宋豪男
王迪
刘宏康
崔治军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing University of Technology
Original Assignee
Beijing University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN201910804060.1A priority Critical patent/CN110455294A/en
Publication of CN110455294A publication Critical patent/CN110455294A/en
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems

Abstract

The invention discloses the implementation methods based on the multithreading distribution SLAM system under ROS environment, distributed SLAM system based on the multithreading under ROS environment is that entire state vector is divided into the estimation of robot pose and road sign estimation totally five dimension state using distributed structure, the design is handled using the distribution of system model, multiple effective road sign points are established into multiple threads, it forms to be parallel to each other and is separately formed the model structure of subfilter, then the pose estimated result of the robot of subfilter is merged in senior filter by main thread, robot location's estimation result is finally obtained by the fusion results of subfilter.It is compared using inventive algorithm and offline distributed algorithm finally by true experiment, it was demonstrated that the feasibility and validity of this algorithm, so as to complete to the realization based on the multithreading distribution SLAM system under ROS environment.

Description

Implementation method based on the multithreading distribution SLAM system under ROS environment
Technical field
Entire state vector is divided into the estimation of robot pose using distributed structure for present invention design and road sign is estimated Totally five dimension state is handled using the distribution of system model, multiple effective road sign points is established multiple threads, is formd mutually flat Row is separately formed the model structure of subfilter, and then the pose estimated result of the robot of subfilter is existed by main thread It is merged in senior filter, robot location's estimation result is finally obtained by the fusion results of subfilter, to realize Robot autonomous localization and build figure.It is loaded with laser to construct, odometer, the information of the multisensor of the sensors such as magnetic compass Data collection system and multithreading distribution EKF-SLAM system prototype based on ROS environment, and improve and optimize system Performance.
Background technique
It is basic for SLAM (Simultaneous Localization and Mapping) i.e. synchronous superposition Thought is: allowing robot to move since unknown position in circumstances not known, collects itself moving situation by self-sensor device And ambient environmental conditions carry out self-position estimation, while constructing increment type map.During SLAM, according to observation information into The real-time map estimation of row and update.Wherein, it is critical issue that multithreading is handled in real time.Data fusion in SLAM refers to utilization Computer program carries out information processing to the observation information of the robot sensor chronologically obtained under some way to it Complete the process of estimation task.Robot platform and the various sensors carried thereon are the hardware foundations of data fusion, more The collected data of sensor are the processing objects of data fusion, and the optimization of task and Integrated Real-time Processing are data fusions Core.The output result of system is by odometer, the information progress fused filtering generation of laser sensor and magnetic compass.
It is based on the distributed spreading kalman SLAM system that the multithreading under ROS environment is handled in real time online and divides offline Cloth SLAM system is corresponding, due in offline distributed frame, sensor data acquisition and subfilter and main filtering Device operation be not it is synchronous carry out, the not only problem small without multithreading bring operand, but not generating date and Shi Xing, also without the concern in robot moving process dynamic adjustment sensor between clock matches the problems such as.More than It is the deficiency of offline distribution SLAM system.
Summary of the invention
The present invention proposes a kind of based on ROS ring for the distributed treatment of existing SLAM system and the deficiency of real-time Multithreading distribution SLAM system under border.Entire state vector is divided into the estimation of robot pose and road sign point estimation totally five dimension State establishes multiple subfilters according to multiple effective road sign points, and each subfilter is simultaneously handled observation information, so The pose estimated result of the robot of subfilter is merged in senior filter afterwards, final robot pose is obtained and estimates Meter.To hereafter tell about respectively information data acquisition system based on multisensor and how by Fusion come Realize this distribution SLAM system.Finally by the experiment proves that present system validity.
Since the SLAM autonomous navigation system of distributed frame uses the processing method of Distributed Parallel Computing, so can To greatly reduce the calculation amount of SLAM algorithm, SLAM system survivability, and more easily implementation and application are improved, this It is also the core concept and essential purpose of research distributed frame SLAM system.
1. environment sensing laser sensor system
Robot under circumstances not known detects environment using laser sensor, realizes its positioning.Its essence is The local map being currently detected is matched with established global map, it is fixed to robot with the error for reducing track estimation The uncertainty of position.What the present invention used is the LMS111 laser radar range sensor of German SICK company production, laser thunder Up to sensor, run-down obtains the range data of 361 with barrier within the scope of 0 °~180 °, that is, indicates every 0.5 ° one Data.It can indicate that these range informations and angle-data, the optical center of the scanning of laser sensor are done using polar coordinate mode For pole, polar axis is then the main scanning direction (0 °) of laser sensor.Using (ρn,φn) (n=1 ..., 361) indicate laser The data set that the single pass of sensor arrives, ρnIt is the distance from scanning optical center to n-th of barrier, φnWhat is indicated is from sweeping to N-th of barrier direction to main scanning direction (0 °) angle, n indicate is the data scanned quantity.
The communication mode of LMS111 and host is the communication transport protocols based on TCP/IP, and that utilize is Socket end-to-end Communication mode.Laser radar is server end in this system, and host is client, that is, receiving end.Lead under Linux environment ServerSocket and ClientSocket are often used to realize that Socket is communicated.Laser sensor is come using ServerSocket Server S ocket is built, realizes and monitors other connection requests based on the host in TCP/IP transmission computer, that is, this system, And connection is established when receiving connection request to carry out the transmission of data.Basic step is as follows:
I.e. server-side passively connects before communicating, and client is actively to connect, then server end and client Connection need to could establish connection by three-way handshake.In entire communication process, the communication of server end and client has one A affirmation mechanism guarantees that data packet can smoothly reach an other side, but this is not absolutely, if intermediate link is asked Topic, also results in data-bag lost, confirms either party wrapped without receiving, the state that will always be at waiting and retransmitting is worked as In.
Server end establishes socket process:
Create socket- > bind- > listen- > recv (send) -> close
Client establishes socket process:
Create socket- > connect- > recv (send) -> close
After being connect by Ethernet with host system, socket communication connection, when acquiring data, small Main Engine is every 0.3 Second sends the hexadecimal acquisition instructions of laser, returned data packet after laser pick-off to instruction to Ethernet interface.Finally to data The hexadecimal range data scanned in packet carries out processing conversion, obtains apart from point cloud data.Due to being swept to laser setting Retouching range is 0 to 180 degree, so a packet range points cloud has 361 range data.
2. environment sensing magnetic compass transmitter system
The model that magnetic compass uses is HCM365B.It can provide high-precision course information, and course precision is 0.5 °.Its energy Course is resolved in real time, and the course by three axis accelerometer for inclination angle compensates, and makes it possible in extremely severe ring Accurate course information can be also provided in border.Magnetic compass is to measure spatial attitude angle using the intrinsic directive property in earth's magnetic field , it can measure to obtain the 3 d pose data of carrier: horizontal course, roll and pitching.System is the boat using magnetic compass It is added in observation model to as metrical information, and corrects odometer using the course information of magnetic compass in motion model The course itself being calculated.Since error constantly accumulates in odometer itself calculating process, the robot position being calculated Appearance has biggish error, and especially with the growth of time, error can be increasing.In the system model of building, in order to mention The constringency performance and improvement system estimation precision of high single subfilter, introduce system for the course information of magnetic compass as observed quantity In system model.
The communication mode of magnetic compass is RS-232 serial communication, every 0.3 second transmission once command, then returns to angle letter Breath.RS-232 serial communication is one of most common communication interface on computer, is an asynchronous transmission standard interface, is to calculate The standard that proximal end connects between machine interface and peripheral hardware or terminal.USB/RS-232 conversion can be used when with RS-232 serial ports Device, the data of RS-232 are transmitted by usb bus, i.e. the application program of host side is still to carry out serial ports volume to RS-232 Journey, and peripheral hardware or terminal be also communicated with RS-232, but from host to peripheral hardware between connection usb bus it is this Mode, therefore be also to be communicated with the data format of USB.
What the course of magnetic compass measurement indicated is geographical course, i.e. the angle of robot positive direction and geographical direct north, And the course of robot is the variable quantity for initial positive direction.Therefore when tectonic model, what is first handled is robot Variation pattern of the course in different coordinates, and the mode by demarcating carries out navigational calibration.
It is installed it is assumed that magnetic compass and robot have carried out accurate calibration, the course heading that magnetic compass measures is denoted as Φ, That is the angle of robot positive direction and geographical direct north, and the range of its angle change is 0-360 °.Robot itself Course is denoted as φ, the initial coordinate system X-axis forward direction with robot be it is identical, initial value is 0 °, the range of angle change is- 180 ° to 180 °.The robot course for finally being changed by coordinate system and being modified is denoted as Φ '.Therefore the variation relation between them Following Fig. 1.
A)-d) it is transformation relation in different quadrant Φ and Φ ', when what wherein dotted line coordinate system represented is robot initial Coordinate system, with coordinate system (Xr,Yr) indicate.Initial time robot forward direction and the angle of geographical direct north are indicated with θ.
3. speedometer sensor system
Pass through geometrical principle when robot motion, so that the motion model of the differential steering of wheeled robot is constructed, The wheel independent control of robot two sides makes them have different revolving speeds by two sidecar wheel of remote control control, thus makes it Both sides speed is different, and robot can be made to carry out the movement of various straight trips or turning by instruction in this way.
It there are two ways to calculating pose, is arc-shaped and linear type respectively by the odometer of encoder.Circular die Type has not only done corresponding calculating to robot displacement variation, has also done to the variation for being biased to angle compared with linear type model It calculates, keeps it closer with robot real trace.
Linear type model is to linearize the camber line of robot small range motion change, so that computational efficiency is more Fastly.In motion path in the higher situation of relatively easy and sample rate, performance is an advantage over circular arc type model.Due to linear type Model has the advantages that calculation amount is small, high-efficient, and the posture information that when robot motion generates will be calculated with linear type model It arrives.When the sampling interval is shorter, it is assumed that only generate straight-line displacement increment when robot motion, the increasing without generating angle Amount, can be substantially reduced the computation complexity of model in this way, so that computational efficiency is greatly enhanced, equation are as follows:
Wherein XtFor the state of t moment robot, ut-1It is the t-1 moment to the control amount of system, xtAnd ytFor t moment machine The position of people, ΦtFor the course angle of t moment robot.
The displacement that left and right wheels are passed through in the time interval of defined is calculated by odometer encoder pulse, and can To obtain the radian value turned over accordingly, thus the increment of robot relative displacement and angle is just obtained.Encoder is differentiated Rate σ are as follows:
Wherein, D is wheel diameter (unit: m), and P is encoder accuracy, and η is driving motor reduction ratio.Sampling interval determines Afterwards, in sampling time interval, two wheel displacements increment Delta d can be calculatedLWith Δ dR:
NLAnd NRThe pulse increment value that the encoder of respectively revolver and right wheel is exported in sampling interval duration.
Taking L (unit: m) is the spacing of robot left and right wheels, in the collected robot of sampling time Δ t inner encoder Displacement and angle variable quantity are as follows:
ΔDtThe distance that mobile robot is passed through when to move in t-1 to t time interval, obtains divided by sampling time Δ t Be speed vc, ΔΦtBetween the final position direction and initial position direction of mobile robot in t-1 to t moment time interval Angle.Be above based on 2-D laser radar sensor, magnetic compass, the multisensors such as odometer information data acquisition SLAM system.From hardware configuration, the information data acquisition system based on multisensor is devised, data fusion is realized Hardware foundation.Respectively to the laser radar of environment sensing sensor, the communication protocol and data of magnetic compass and odometer are acquired Method describes in detail, to complete the data acquisition of processing object, and different sensing datas is carried out clock matches, Mileage is counted, laser scanning data, compass heading data according to unified data format carry out data preservation, and same Moment collected different data is in subsequent algorithm, the output result of system to be by odometer, laser sensor and magnetic compass Information be filtered generation.
System must just have multiple thread synchronizations to run parallel each to the simultaneously and concurrently processing of realization distribution SLAM Subfilter realizes that United Dispatching, management and the fusion of subsystems are counted with main thread to realize that parallel synchronous is calculated It calculates.According to this analysis as a result, making Multi-core concurrent operation using POSIX Thread using under Linux system.This It is because all threads dwell on same memory headroom, POSIX thread does not need to carry out the big and complicated long distance of amount of overhead From calling.As long as all threads can read and modify existing data structure in program using simple synchronization mechanism. Without data via filec descriptor dump or are squeezed into tight narrow shared memory space.Kernel is without independent duplicating process Memory headroom.Which saves a large amount of CPU times, so that thread creation is than upper by ten to one hundred times fastly of new process creation.Because This point, can be largely using thread without too worrying bring CPU or low memory.It is very suitable to this distribution The simultaneously and concurrently process demand of SLAM system.
Safeguard one group of available thread by thread pool, master is managed with the role of main thread and safeguarded worker Thread, and shoulder the task of the distribution of external interface and work；And worker thread is merely responsible for data processing and calculates to appoint Business, and this task schedule mode is very flexible on cross-thread communication mode, and a set of money can be shared between different threads Source.The model is suitable for the parallel processing demand of distributed SLAM system, and main thread is as in scheduling, management and fusion calculation The heart, multiple worker threads calculate each subfilter system for synchronous.
In view of the data transportation requirements between each subfilter and senior filter, inter-thread communication mode uses IPC (inter process communication) inter-thread communication mode.Acquisition sensor can be managed simultaneously by enabling main thread Environmental observation information and worker thread can be managed to be responsible for data processing and calculating task and last Merge the information such as estimated result.
Distributed SLAM System State Model can regard a markoff process as, i.e., before t moment state and t-1 moment State is unrelated.The motion control information of robot is provided by odometer in system, as shown in Fig. 2, the state for robot shifts Process, Fig. 3 are actual robot coordinate system.
The state of robot can be described as: st=(x, y, φ)T, wherein x, y indicates coordinate value, φ indicate yaw angle (φ=0 indicates that direction x-axis is positive, and φ=pi/2 indicates that direction y-axis is positive).The state transition model of robot is as follows:
X in formulaL(t)=(xv(t)yv(t)φv(t))TIt is robot in the pose of t moment, Δ T is time variation amount, vc For robot movement speed, α is robot wheel steering angle, and L is two-wheeled spacing, and γ is that covariance is the white Gaussian that Q mean value is zero Noise.
The measurement of Extended Kalman filter updates the result is that the road sign point range information and magnetic sieve measured by laser sensor What the course information that disk measures was calculated, the observation model of robot, observed quantity z are provided for laser sensor and magnetic compass It is some distance and angle of road sign point relative to sensor respectively.Robotary information is associated with observation information, it can To obtain the observation model of road sign point:
In formula,It include the robot and road sign point spacing walk-off angle degree that sensor measures, X for observed quantityL =(xL yL)TFor road sign point coordinate, v is that covariance is the white Gaussian noise that R mean value is 0.mLFor ground map reference.In distribution Under frame, the model of each subfilter is as follows:
In formula, h (xr(t),mn) it is road sign point observation model, V (t) is that covariance is the white Gaussian noise that R mean value is 0.
Here corresponding SLAM state vector is divided into the estimation of robot pose in each subfilter constructed and road sign is estimated Meter totally five ties up state, by trolley course φvObservation, i.e. x is addedL=[xv,yv,φv,xL,yL]T, the state equation of system is as follows:
Wherein ω be covariance be R mean value be 0 white Gaussian noise Kalman filtering solve be Linear system model, and SLAM model belongs to nonlinear model, and the Jacobian matrix for being carried out observation model after linearization process is as follows:
Wherein
Δ x=[xν-xL] (13)
Δ y=[yν-yL] (14)
Zr, Zθ, ZφIt is observed quantity, distance respectively between the sensor robot measured and road sign point, angle and robot Course.
Then distributed model is utilized, model is rewritten as following form:
From model (7) and (16) as can be seen that SLAM problem is the state estimation problem of nonlinear and non-Gaussian, pass through first Nonlinear problem is converted into linear and estimated by spreading kalman algorithm, secondly in state model and observation model respectively The white Gaussian noise that mean value is zero is added, then establishes distributed multithreading subfilter structure, is handled in real time using multithreading Greatly optimize SLAM modeling and estimation procedure.
Tell about the realization of entire distribution SLAM system emphatically below:
The realization of the system is under Linux system, and experiment porch is little Qiang robot XQ-4Pro robot platform, such as Shown in Fig. 3.What system was used is trolley odometer, the sensors such as laser radar and magnetic compass.
Trolley generally Three-wheel type structure, before the turning of two driving wheel differential controls, subsequent universal wheel is as driven Wheel.Such structure can guarantee that the accuracy of its angle of turn, load-bearing capacity also greatly promote.Laser is carried above trolley The devices such as sensor and magnetic compass and screen.Small strong master controller is the mini computer of an i7 processor, includes 8G memory With 64G solid state hard disk.Such hardware configuration ensure that the powerful operational capability of trolley.Trolley movement speed maximum 0.8m/s, Motor carries encoder and is furnished with speed control, adaptive a variety of loads and ground road conditions.
Present online trolley data collecting mechanism: based under ROS environment, laser, odometer and magnetic compass three are passed The data of sensor are acquired, and guarantee that acquisition clock is identical, and frequency acquisition is 0.3 second.
The system realizes the control of robot platform and the acquisition of sensing data and processing.Small Main Engine booting Afterwards, laser sensor and the automatic connection of magnetic compass booting, standby, small cart handle control trolley starts to move, together When laser sensor and magnetic compass start carry out data acquisition and data transmission.And when different sensing datas is carried out Clock matching, mileage is counted, laser scanning data, compass heading data according to unified data format carry out data preservation, And in the SLAM algorithm that the collected different data of synchronization is handled in real time online for multithreading.
It is above exactly the introduction of system experimental platform, as shown in Figure 4.Realize the data acquisition and cooperation of multiple sensors It uses, lays a good foundation for the realization of following distributed SLAM system.
The basic conception of distributed SLAM system is by multiple independent parallel subfilters to the more of sensor measurement Thread is handled in real time, is realized the prediction and update of NextState and its covariance, is then merged in senior filter updated Information, output estimation value, flow chart are as shown in Figure 5.According to the actual demand of distribution SLAM system, using multithreading task Scheduling and inter-thread communication mechanism, its main feature is that: one group of available thread is safeguarded by thread pool, master is with main thread Role manages and safeguards worker thread, and shoulders the task of the distribution of external interface and work；And worker thread It is merely responsible for the data processing and calculating task of multiple sensors, and this task schedule mode is non-on cross-thread communication mode Often flexibly, a set of resource can be shared between different threads.POSIX thread library is encapsulated according to characteristics such as processor, operating systems The interface of one thread process.For linux system, it often acquiescently arrives a newly created thread scheduling It is executed in the core different from main thread, more balancedly balanced load is come with this.Therefore, which is suitable for distributed SLAM system Parallel processing demand, main thread is as scheduling, management and senior filter fusion calculation center, and multiple worker threads are for same Step calculates each subfilter system.
Therefore the advantage based on the above multithreading, present invention application multithreading realize holding parallel for each subfilter Row.The implementation of multithreading of the invention is to initially set up a main thread, that is, executes the thread of main () function.Secondly After the completion of the sensing data pretreatment of acquisition, when establishing multiple subfilters, a new thread is first created, is called Pthread_create () function, another thread of newly created thread creation, with the side of such new thread creation new thread Formula finally creates multiple subfilters.Although this mode, thread standard treats them as equal same levels.
After the completion of thread creation, multiple thread synchronization operations, the present invention realizes that cross-thread is same using Mutex function Step, and protect shared data simultaneously.After the completion of thread synchronization operation, multiple subfilters are weighted fusion, obtain most The position of whole trolley.During multithreading is realized, the present invention cleverly creates one when algorithm is realized comprising all The overall situation " dead thread list " for having stopped thread allowing a thread to become the cleaning thread of fixation specially to wait the thread and quilt of stoppings It is added in list.This fix cleaning thread come call pthread_join () function by the thread just stopped and from Oneself merges.A dedicated thread is so only used only and can be ingenious and effectively cleans up whole threads.Terminate thread There are many ways to: the present invention has invoked pthread_exit () function to terminate thread.The wound of entire thread is completed with this It builds, synchronous and termination.
The data fusion process of distributed SLAM navigation system is as follows:
Parallel, independent distributed subsystem, each distributed subsystem quilt are established using each road sign point observed Regard a worker thread as, master manages multiple worker threads as main thread simultaneously and carries out multiple subfilter meters It calculates, the estimated result of each subfilter is input in senior filter, each sub- filtering is determined according to each subfilter covariance Device weight shared in senior filter, and be normalized, finally utilize information fusion output final position estimation knot Fruit.Its distributed multithreading programming flowchart such as Fig. 6.Each subfilter pose estimation procedure are as follows:
Obtained result is as the following input for measuring and updating.Measurement in subfilter and senior filter updates are as follows:
After obtaining the result of subfilter update, fusion process is carried out according to covariance matrix, the fusion in senior filter Weighted mean method is used, to export final position:
Wherein ηiFor weighting coefficient, weighted mean method calculates the robot pose estimated information from different subfilters Weight, the result after being weighted and averaged is as fusion value.
Thus the data fusion and robot test platform for relying on SLAM distributed treatment propose a kind of based on ROS ring Multithreading distribution SLAM system under border.The system realize robot platform control and sensing data acquisition and Processing.After small Main Engine booting, laser sensor and the automatic connection of magnetic compass booting, standby, small cart handle control Trolley processed starts to move, while laser sensor and magnetic compass start to carry out the transmission of data acquisition and data.And it will be different Sensing data carry out clock matches, mileage is counted, laser scanning data, compass heading data are according to unified data Format carries out data preservation, and the collected different data of synchronization is used for the extension karr that multithreading is handled in real time online In graceful SLAM algorithm.
Detailed description of the invention
Fig. 1: robot course and compass heading transformation relation；
Fig. 2: the state migration procedure figure of robot；
Fig. 3: robot actual coordinates；
Fig. 4: moving trolley platform；
Fig. 5: experiment porch diagram；
Fig. 6: algorithm flow chart；
Fig. 7: distributed multithreading programming flowchart；
Fig. 8: 8 font experimental situations；
Fig. 9: 8 font experimental result picture of this algorithm；
Figure 10: 8 font experimental result picture of off-line algorithm；
Specific embodiment
Below in conjunction with drawings and examples, the present invention is described in detail.
The technical solution adopted by the present invention is the implementation method based on the multithreading distribution SLAM system under ROS environment, Specific step is as follows for the implementation method:
Step 1: building information data acquisition system.
According to the information collection and transmission mode between each sensor, building is loaded with laser, odometer, the sensing such as magnetic compass The information data acquisition system of the multisensor of device.After powering on, laser sensor and magnetic compass start to carry out data acquisition and The transmission of data, and different sensing datas is subjected to clock matches, mileage is counted, laser scanning data, compass boat Data preservation is carried out according to unified data format to data, for use in the distributed SLAM algorithm of multithreading later.
Step 2: the foundation of model.
Entire state vector is divided into the estimation of robot pose and road sign point estimation totally five dimension state, state vector are as follows: xL=[xv,yv,φv,xL,yL]T, xv,yv,φvIt is the state of trolley, xL,yLIt is the state of road sign point.Establish motion model and sight Survey model, the state description of robot are as follows: st=(x, y, φ)T, wherein x, y indicates coordinate value, φ indicate yaw angle wherein, φ=0 indicates that direction x-axis is positive, and φ=pi/2 indicates that direction y-axis is positive.The state transition model of robot is as follows:
X in formulaL(t)=(xv(t)yv(t)φv(t))TIt is robot in the pose of t moment, Δ T is time variation amount, vc For robot movement speed, α is robot wheel steering angle, and L is two-wheeled spacing, and γ is that covariance is the white Gaussian that Q mean value is zero Noise.
The measurement of Extended Kalman filter updates the result is that the road sign point range information and magnetic sieve measured by laser sensor What the course information that disk measures was calculated, the observation model of robot, observed quantity z are provided for laser sensor and magnetic compass It is some distance and angle of road sign point relative to sensor respectively.Robotary information is associated with observation information, it obtains Obtain the observation model of road sign point:
In formula,It include the robot and road sign point spacing walk-off angle degree that sensor measures, X for observed quantityL =(xL yL)TFor road sign point coordinate, v is that covariance is the white Gaussian noise that R mean value is 0.mLFor ground map reference.
Motion model and observation model be utilized respectively odometer output and laser sensor output it is corresponding to estimate Robot and road sign dot position information.
Step 3: map initialization.
Coordinate system is established, using the position at robot initial moment as coordinate origin, using due east and direct north as x-axis and y The positive direction of axis；Map initialization, robot is in initial position, and the road pointing information scanned using sensor is with establishing environment Figure, by robot pose and road sign point, totally five dimension states are stored together as global map.
Step 4: road sign point matches and establishes distributed subfilter.
Obtain the road sign three-point state information that measures of robot t-1 moment, and with having stored in the road in map before Pointing information matches, and for the road sign point not matched at the t-1 moment, adds it to map；For in t-1 moment energy The multiple road sign points enough matched establish subfilter to it respectively, for parallel multithread distributed filter under robot The estimation of one moment pose and road sign point estimation.
Step 5: state update is carried out by distributed multithreading expanded Kalman filtration algorithm.
The information of each road sign point and the robot course information of t moment are sent into the same subfilter, pass through distribution Multithreading expanded Kalman filtration algorithm carries out the state vector and covariance of each subfilter established in above-mentioned steps Processing, the iteration for carrying out state-transition matrix and corresponding covariance matrix respectively update, including predict process and measure to update two A aspect.
Prediction process:
First by the estimated state and covariance matrix of each subfilter at t-1 moment, in conjunction with the defeated of t moment odometer Data out substitute into state transition model, carry out a step recursion using one-step prediction equation (17) and (18), obtain robot Position.
Each subfilter pose estimation procedure are as follows:
It measures and updates:
By way of recursion during prediction, after obtaining the predicted value of robot location, laser sensing is then regathered The observation information of device and magnetic compass.In conjunction with predicted value and measured value, using obtained result as the following input for measuring and updating.Son Measurement in filter and senior filter updates are as follows:
It obtains the revised estimated value of present status and exports corresponding covariance matrix simultaneously for successive iterations.
By above-mentioned prediction process and update is measured, robot pose and road sign point are updated.
Step 6: calculating weight normalization and output result is merged to sub- filter data.
The estimated result of each subfilter is input in senior filter according in step 4, is assisted according to each subfilter Variance determines the weight of each subfilter state and covariance in senior filter, then obtains subfilter update according to formula As a result after, fusion process is carried out according to covariance matrix, the fusion in senior filter uses weighted mean method, so that output is final Position:
Wherein ηiFor weighting coefficient, weighted mean method calculates the robot pose estimated information from different subfilters Weight, the result after being weighted and averaged is as fusion value.
Weight is normalized using weighted mean method.Fusion meter is carried out to data all in subfilter by senior filter It calculates, the state estimation of final output t moment robotAnd varianceAnd estimationIt is stored in map；
Step 7: among algorithm involvement system, building the multithreading under the ROS environment based on multi-sensor collection system Distributed SLAM system.
The multithreading distribution SLAM system under the environment proposed by the present invention based on ROS is tested by actual scene Card.Testing platform used is the three wheeler oneself built, and sensor uses the laser sensor of LMS111, moving trolley platform Pictorial diagram such as Fig. 4.Experiment environment used is 8 font runways, and using trunk as road sign, point is in runway both sides, 8 font runways Experimental situation is as shown in Figure 8.By using this distribution SLAM system and offline distribution SLAM system comparison, it was demonstrated that this distribution The feasibility and validity of formula SLAM system.
Experimental data is drawn in Qt, and 8 font runway experimental traces figures are as shown in figure 9, mileage is counted such as figure solid line institute Show, it is seen that with the increase of odometer add up error, odometer track increasingly dissipates.Figure 10 is offline distribution SLAM system Trajectory diagram, experimental data draws in Matlab.The track in track and Figure 10 in Fig. 9 is based on ROS ring respectively The fusion knot of distributed SLAM system and corresponding offline distribution SLAM system that multithreading under border is handled in real time online Fruit, the unsmooth phenomenon occurred in comparison diagram 10 in x=14m and x=16m (figure Green is irised out), estimated result can be with Explicitly find out, the relatively offline distribution SLAM of distributed SLAM system handled in real time online based on the multithreading under ROS environment The smoother stabilization of system and estimated result Uniformity and Astringency is more preferable, deviation is smaller.It can be seen that the present invention is based under ROS environment Multithreading distribution SLAM system has preferably the interference of uncertain noise during the compensation of error and Robotic Dynamic adjustment Estimation performance, can achieve preferable estimation effect, so that preferably completing to the present invention is based on more under ROS environment The realization of thread distribution SLAM system.

Claims (1)

1. the implementation method based on the multithreading distribution SLAM system under ROS environment, it is characterised in that: the tool of the implementation method Steps are as follows for body,
Step 1: building information data acquisition system；
According to the information collection and transmission mode between each sensor, constructs and be loaded with laser, odometer, the sensors such as magnetic compass The information data acquisition system of multisensor；After powering on, laser sensor and magnetic compass start to carry out data acquisition and data Transmission, and different sensing data is subjected to clock matches, mileage is counted, laser scanning data, compass heading number Data preservation is carried out according to according to unified data format, for use in the distributed SLAM algorithm of multithreading later；
Step 2: the foundation of model；
Entire state vector is divided into the estimation of robot pose and road sign point estimation totally five dimension state, state vector are as follows: xL= [xv,yv,φv,xL,yL]T, xv,yv,φvIt is the state of trolley, xL,yLIt is the state of road sign point；Establish motion model and observation mould Type, the state description of robot are as follows: st=(x, y, φ)T, wherein x, y indicates coordinate value, φ indicate yaw angle wherein, φ= 0 indicates to be directed toward x-axis forward direction, and φ=pi/2 indicates that direction y-axis is positive；The state transition model of robot is as follows:
X in formulaL(t)=(xv(t) yv(t) φv(t))TIt is robot in the pose of t moment, Δ T is time variation amount, vcFor Robot movement speed, α are robot wheel steering angle, and L is two-wheeled spacing, and γ is that covariance is the Gauss white noise that Q mean value is zero Sound；
The measurement of Extended Kalman filter updates the result is that the road sign point range information and magnetic compass that are measured by laser sensor are surveyed What the course information obtained was calculated, the observation model of robot, observed quantity z difference are provided for laser sensor and magnetic compass It is some distance and angle of road sign point relative to sensor；Robotary information is associated with observation information, obtain road The observation model of punctuate:
In formula,It include the robot and road sign point spacing walk-off angle degree that sensor measures, X for observed quantityL=(xL yL)TFor road sign point coordinate, v is that covariance is the white Gaussian noise that R mean value is 0；mLFor ground map reference；
Motion model and observation model are utilized respectively the output of odometer and corresponding machine is estimated in the output of laser sensor People and road sign dot position information；
Step 3: map initialization；
Coordinate system is established, using the position at robot initial moment as coordinate origin, using due east and direct north as x-axis and y-axis Positive direction；Map initialization, in initial position, the road pointing information scanned using sensor establishes environmental map for robot, By robot pose and road sign point, totally five dimension states are stored together as global map；
Step 4: road sign point matches and establishes distributed subfilter；
Obtain the road sign three-point state information that measures of robot t-1 moment, and in the road sign point being had stored in map before Information matches, and for the road sign point not matched at the t-1 moment, adds it to map；For the t-1 moment can The multiple road sign points mixed establish subfilter to it respectively, for parallel multithread distributed filter to a period of time under robot Carve pose estimation and road sign point estimation；
Step 5: state update is carried out by distributed multithreading expanded Kalman filtration algorithm；
The information of each road sign point and the robot course information of t moment are sent into the same subfilter, by distributed multi-thread Journey expanded Kalman filtration algorithm handles the state vector and covariance of each subfilter established in above-mentioned steps, The iteration for carrying out state-transition matrix and corresponding covariance matrix respectively updates, including prediction process and measurement update two sides Face；
Prediction process:
First by the estimated state and covariance matrix of each subfilter at t-1 moment, in conjunction with the output number of t moment odometer According to, substitute into state transition model in, using one-step prediction equation carry out a step recursion, obtain robot location；
Each subfilter pose estimation procedure are as follows:
It measures and updates:
During prediction by way of recursion, after obtaining the predicted value of robot location, then regather laser sensor and The observation information of magnetic compass；In conjunction with predicted value and measured value, using obtained result as the following input for measuring and updating；Son filtering Measurement in device and senior filter updates are as follows:
It obtains the revised estimated value of present status and exports corresponding covariance matrix simultaneously for successive iterations；
By above-mentioned prediction process and update is measured, robot pose and road sign point are updated；
Step 6: calculating weight normalization and output result is merged to sub- filter data；
The estimated result of each subfilter is input in senior filter according in step 4, according to each subfilter covariance It determines the weight of each subfilter state and covariance in senior filter, the result of subfilter update is then obtained according to formula Afterwards, fusion process is carried out according to covariance matrix, the fusion in senior filter uses weighted mean method, to export final position It sets:
Wherein ηiFor weighting coefficient, the robot pose estimated information from different subfilters is calculated weight by weighted mean method, Result after being weighted and averaged is as fusion value；
Weight is normalized using weighted mean method；Fusion calculation is carried out to data all in subfilter by senior filter, most The state estimation of output t moment robot eventuallyAnd varianceAnd estimationIt is stored in map；
Step 7: among algorithm involvement system, building the multithreading distribution under the ROS environment based on multi-sensor collection system Formula SLAM system.
CN201910804060.1A 2019-08-28 2019-08-28 Implementation method based on the multithreading distribution SLAM system under ROS environment Pending CN110455294A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910804060.1A CN110455294A (en) 2019-08-28 2019-08-28 Implementation method based on the multithreading distribution SLAM system under ROS environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910804060.1A CN110455294A (en) 2019-08-28 2019-08-28 Implementation method based on the multithreading distribution SLAM system under ROS environment

Publications (1)

Publication Number Publication Date
CN110455294A true CN110455294A (en) 2019-11-15

Family

ID=68489664

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910804060.1A Pending CN110455294A (en) 2019-08-28 2019-08-28 Implementation method based on the multithreading distribution SLAM system under ROS environment

Country Status (1)

Country Link
CN (1) CN110455294A (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111078210A (en) * 2019-12-11 2020-04-28 中国科学院光电技术研究所 QT and Matlab multithreading hybrid programming software architecture for adaptive optical control system
CN111413691A (en) * 2020-03-10 2020-07-14 杭州电子科技大学 Semantic positioning and mapping method adopting distributed structure
CN112857390A (en) * 2021-01-14 2021-05-28 江苏智派战线智能科技有限公司 Calculation method applied to intelligent robot moving path
CN112925318A (en) * 2021-01-25 2021-06-08 西南交通大学 Calculation method applied to intelligent robot moving path

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7562154B2 (en) * 2003-06-30 2009-07-14 International Business Machines Corporation System and method for filtering stale messages resulting from membership changes in a distributed computing environment
CN103644903A (en) * 2013-09-17 2014-03-19 北京工业大学 Simultaneous localization and mapping method based on distributed edge unscented particle filter
CN104601142A (en) * 2013-10-31 2015-05-06 横河电机株式会社 Filtering method, filter and flicker test system
CN105606104A (en) * 2016-03-17 2016-05-25 北京工业大学 Robot autonomous navigation method based on heading-assisting distributed type SLAM (Simultaneous Localization and Mapping)
CN105737832A (en) * 2016-03-22 2016-07-06 北京工业大学 Distributed SLAM (simultaneous localization and mapping) method on basis of global optimal data fusion

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7562154B2 (en) * 2003-06-30 2009-07-14 International Business Machines Corporation System and method for filtering stale messages resulting from membership changes in a distributed computing environment
CN103644903A (en) * 2013-09-17 2014-03-19 北京工业大学 Simultaneous localization and mapping method based on distributed edge unscented particle filter
CN104601142A (en) * 2013-10-31 2015-05-06 横河电机株式会社 Filtering method, filter and flicker test system
CN105606104A (en) * 2016-03-17 2016-05-25 北京工业大学 Robot autonomous navigation method based on heading-assisting distributed type SLAM (Simultaneous Localization and Mapping)
CN105737832A (en) * 2016-03-22 2016-07-06 北京工业大学 Distributed SLAM (simultaneous localization and mapping) method on basis of global optimal data fusion

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
程雨航: "基于性能分析与优化的分布式SLAM算法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 *
齐林庆: "基于EKF-SLAM的AUV自主导航算法的多线程实现", 《现代电子技术》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111078210A (en) * 2019-12-11 2020-04-28 中国科学院光电技术研究所 QT and Matlab multithreading hybrid programming software architecture for adaptive optical control system
CN111413691A (en) * 2020-03-10 2020-07-14 杭州电子科技大学 Semantic positioning and mapping method adopting distributed structure
CN112857390A (en) * 2021-01-14 2021-05-28 江苏智派战线智能科技有限公司 Calculation method applied to intelligent robot moving path
CN112925318A (en) * 2021-01-25 2021-06-08 西南交通大学 Calculation method applied to intelligent robot moving path

Similar Documents

Publication Publication Date Title
CN110455294A (en) Implementation method based on the multithreading distribution SLAM system under ROS environment
EP3364153B1 (en) Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor
CN108051002B (en) Transport vehicle space positioning method and system based on inertial measurement auxiliary vision
CN108627153B (en) Rigid body motion tracking system based on inertial sensor and working method thereof
CN109885080B (en) Autonomous control system and autonomous control method
Wang et al. A simple and parallel algorithm for real-time robot localization by fusing monocular vision and odometry/AHRS sensors
CN105953796A (en) Stable motion tracking method and stable motion tracking device based on integration of simple camera and IMU (inertial measurement unit) of smart cellphone
Su et al. GR-LOAM: LiDAR-based sensor fusion SLAM for ground robots on complex terrain
CN108664024B (en) Motion planning and cooperative positioning method and device for unmanned vehicle network formation
CN107943042A (en) A kind of earth magnetism fingerprint database automated construction method and device
CN107478223A (en) A kind of human body attitude calculation method based on quaternary number and Kalman filtering
Marin et al. Event-based localization in ackermann steering limited resource mobile robots
CN108225370B (en) Data fusion and calculation method of motion attitude sensor
WO2018214227A1 (en) Unmanned vehicle real-time posture measurement method
CN112697138B (en) Bionic polarization synchronous positioning and composition method based on factor graph optimization
CN106504275B (en) A kind of real-time three-dimensional method for reconstructing of inertial positioning and point cloud registering coupling and complementing
CN110018691A (en) Small-sized multi-rotor unmanned aerial vehicle state of flight estimating system and method
CN103914068A (en) Service robot autonomous navigation method based on raster maps
CN108332759A (en) A kind of map constructing method and system based on 3D laser
Huang et al. An improved particle filter algorithm for geomagnetic indoor positioning
CN110470297A (en) A kind of attitude motion of space non-cooperative target and inertial parameter estimation method
Hu et al. A small and lightweight autonomous laser mapping system without GPS
CN110442014A (en) A kind of location-based mobile robot RFID servo method
Wang et al. Applying SLAM algorithm based on nonlinear optimized monocular vision and IMU in the positioning method of power inspection robot in complex environment
Zhang et al. Research on UAV attitude data fusion algorithm based on quaternion gradient descent

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

Application publication date: 20191115

RJ01 Rejection of invention patent application after publication