CN110345936A - The track data processing method and its processing system of telecontrol equipment - Google Patents
The track data processing method and its processing system of telecontrol equipment Download PDFInfo
- Publication number
- CN110345936A CN110345936A CN201910615659.0A CN201910615659A CN110345936A CN 110345936 A CN110345936 A CN 110345936A CN 201910615659 A CN201910615659 A CN 201910615659A CN 110345936 A CN110345936 A CN 110345936A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- telecontrol equipment
- track data
- frame point
- reference frame
- 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.)
- Granted
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The present invention provides the track data processing method and its processing system of a kind of telecontrol equipment, the acquisition methods include the successive two frames point cloud for obtaining telecontrol equipment external environment profile, and the relative translation amount of encoder and the angulation change amount of Inertial Measurement Unit between the two frames point cloud, wherein, the two frames point cloud is labeled as reference frame point cloud and present frame point cloud respectively；Initial transformation matrix is constructed using the translation knots modification and the angulation change amount；The present frame point cloud is carried out according to transformation, to obtain transition frames point cloud using the initial transformation matrix；Corresponding points are obtained to set using the reference frame point cloud and transition frames point cloud；The track data of the telecontrol equipment is obtained to set using the corresponding points.Using the present invention, accumulated error when telecontrol equipment running track calculates can be effectively reduced, improves the accuracy that telecontrol equipment running track calculates.
Description
Technical field
The present invention relates to technical field of robot control, in particular to the track data processing method of a kind of telecontrol equipment and
Its processing system.
Background technique
Autonomous mobile robot technology based on laser scanning has had more and more mature application.Obtained by laser scanning
Point cloud be a kind of description to robot external environment profile, the description substantially give robot from environment edge away from
From information.According to these range informations, the determination of self-position is may be implemented in machine, and the function of Real Time Obstacle Avoiding also may be implemented.
Therefore make full use of a cloud information be robot autonomous movement a basic capacity.
Point cloud matching is the synchronous positioning of robot and builds figure (Simultaneously Localization and
Mapping important component).In two successive frame point clouds, there is extremely similar shape.It is rigid due to environment profile
Volume property, what point cloud matching exported is the rotation amount and translational movement between two frame point clouds.If the position of a given wherein frame point cloud
Coordinate can then calculate the position of another frame point cloud.And so on, pass through the point Yun Xu of the carried laser scanning of robot
Column, the motion profile based on our available robots of point cloud matching algorithm.
Point cloud matching is substantially an optimization problem.This type of optimization problem is generally solved using iterative algorithm, is changed at this time
Initial value is strongly relied on for process, if initial value is given bad, iterative algorithm is easily trapped into locally optimal solution, this office
Portion's optimal solution is the source that robot trajectory calculates accumulated error.So if provide one relatively globally optimal solution just
Initial value is with regard to critically important.
On the other hand, in point cloud matching, error function is according to corresponding points to setting up, and corresponding points are to being root
It is got according to the calculating of closest approach rule.At this point, the search of closest approach will occupy the calculating time of the overwhelming majority.How to reduce recently
Point search range is a basic problem of point cloud matching.
Summary of the invention
In view of the foregoing deficiencies of prior art, the purpose of the present invention is to provide a kind of track datas of telecontrol equipment
Processing method and its processing system are run by reasonably selecting initial transformation matrix for solving telecontrol equipment in the prior art
Cumulative errors larger technical problem when track calculates.
In order to achieve the above objects and other related objects, the present invention provides a kind of track data processing side of telecontrol equipment
The track data processing method of method, the telecontrol equipment includes:
Obtain the phase of encoder between the successive two frames point cloud and the two frames point cloud of telecontrol equipment external environment profile
To the angulation change amount of translational movement and Inertial Measurement Unit, wherein respectively by the two frames point cloud labeled as reference frame point cloud and
Present frame point cloud；
Initial transformation matrix is constructed using the translation knots modification and the angulation change amount；
The present frame point cloud is converted using the initial transformation matrix, to obtain transition frames point cloud；
Corresponding points are obtained to set using the reference frame point cloud and transition frames point cloud；
The track data of the telecontrol equipment is obtained to set using the corresponding points.
In one embodiment, the described the step of track data of the telecontrol equipment is obtained to set using the corresponding points
Include:
It establishes about the corresponding points to corresponding points all in set to the objective function of error metrics；
Objective function described in Optimization Solution, to obtain target variable increment；
According to initial transformation matrix described in the target variable incremental update, to obtain the track number of the telecontrol equipment
According to.
In one embodiment, described that the objective function is solved using optimization algorithm, to obtain the step of target variable increment
Suddenly include:
The objective function described in target variable linear optimization, to obtain the equation group about the target variable；
The equation group of the target variable is solved, to obtain target variable increment.
In one embodiment, the track data processing method of the telecontrol equipment further include:
The step of whether track data meets termination condition judged；
If "No", for the track data that will acquire as initial transformation matrix, return is described to utilize the initial change
It changes matrix and the present frame point cloud be iterated the step of transition frames point cloud with obtaining, according to transformation until the track
Data meet termination condition, and motion trace data when meeting the termination condition as final data and is exported.
In one embodiment, for the track data that will acquire as initial transformation matrix, return is described to utilize institute
Initial transformation matrix is stated to carry out according to transformation the present frame point cloud, when being iterated the step of transition frames point cloud with obtaining,
It is obtained in the step of corresponding points are to set using the reference frame point cloud and transition frames point cloud, the pre-determined distance threshold value of use
It is smaller than the pre-determined distance threshold value of last time.
In one embodiment, the telecontrol equipment includes robot.
In one embodiment, described to obtain corresponding points to set using the reference frame point cloud and transition frames point cloud
Step includes:
The telecontrol equipment is oriented with reference to sight when obtaining the reference frame point cloud, for the transition frames point cloud
Each of point, described with reference to point range corresponding to this point in reference frame point cloud described in viewing proximity range searching；
The distance between each point and point range corresponding to this point in the reference frame point cloud are calculated, selected distance is minimum
And the two o'clock for meeting pre-determined distance threshold value establishes corresponding points pair；
The all the points in the transition frames point cloud are traversed, to establish the corresponding points to set.
In one embodiment, it is described to obtain the reference frame point cloud when telecontrol equipment be oriented with reference to sight,
It, and should in the reference frame point cloud with reference to described in viewing proximity range searching for each of transition frames point cloud point
Point corresponding point range the step of include:
It is oriented with the robot of reference frame point cloud position with reference to sight, in the transition frames point cloud
Each point, since it is described with reference to sight into reference frame point cloud described in the point by point search of both sides point range corresponding to this point.
In order to achieve the above objects and other related objects, the present invention also provides a kind of running track processing systems of telecontrol equipment
System, the running track processing system of the telecontrol equipment include:
Service unit；
Point cloud acquisition device, is set on telecontrol equipment, described cloud acquisition device is connect with the service unit, described
Point cloud acquisition device is used to obtain the external environment profile point cloud of the telecontrol equipment；
Encoder is set on the telecontrol equipment, and the encoder is connect with the service unit, and the encoder is used
In the relative translation amount for obtaining the telecontrol equipment；
Inertia test cell is set on the telecontrol equipment, and the inertia test cell is connect with the service unit,
The inertia test cell is used to obtain the angulation change amount of the telecontrol equipment；
Wherein, the service unit be used for according to the external environment profile point cloud of acquisition, the relative translation amount with
And the angulation change amount obtains track data.
In order to achieve the above objects and other related objects, the present invention also provides a kind of service unit, the service unit packets
It includes:
Communicator, is used for and PERCOM peripheral communication；
Memory, for storing computer program；
Processor connects the communicator and memory, executes for running the computer program above-mentioned any one
The track data processing method of the telecontrol equipment of item.
In order to achieve the above objects and other related objects, described the present invention also provides a kind of computer readable storage medium
Computer-readable recording medium storage has computer program；The computer program executes described in above-mentioned any one when running
The track data processing method of telecontrol equipment.
Using the track data processing method and its processing system of telecontrol equipment of the invention, by by the opposite of encoder
The angulation change amount of translational movement and Inertial Measurement Unit can effectively reduce telecontrol equipment running track as initial transformation matrix
Accumulated error when reckoning, improves the accuracy that telecontrol equipment running track calculates, obtained final rigid body translation (namely rail
Mark data) there is very high confidence level, to reduce the accumulated error of track reckoning, while arithmetic speed is also improved,
The landing application of autonomous mobile robot is had a very important significance；
In addition, in the present invention, with telecontrol equipment (such as robot) where reference frame point cloud be oriented with reference to sight into
When row corresponds to point search, the points range and pre-determined distance threshold value of search can be independently set, and use from reference sight
Start so reduce search calculation amount toward the point-by-point search strategy in both sides, improve arithmetic speed；
Also, the present invention is closed in the collection of corresponding points pair chooses squared distance function as error function (objective function), it is somebody's turn to do
Function has convexity and slickness, and subsequent linearisation can be facilitated to approach；
Also, in the present invention, update initial transformation matrix and act on present frame point cloud again, then carries out corresponding points to searching
Suo Shi so can gradually obtain true corresponding points pair by the way of reducing pre-determined distance threshold value；
Certainly, it implements any of the products of the present invention and does not necessarily require achieving all the advantages described above at the same time.
Detailed description of the invention
Fig. 1 is shown as the flow chart of the track data processing method of telecontrol equipment of the invention.
Fig. 2 is shown as the sub-process figure of step S40 in Fig. 1.
Fig. 3 is shown as the sub-process figure of step S50 in Fig. 1.
Fig. 4 is shown as the block diagram of the running track processing system of telecontrol equipment of the invention.
Fig. 5 is shown as the block diagram of service unit in the running track processing system of telecontrol equipment of the invention.
Fig. 6 is shown as the work flow diagram of the running track processing system of telecontrol equipment of the invention.
Specific embodiment
Illustrate embodiments of the present invention below by way of specific specific example, those skilled in the art can be by this specification
Other advantages and efficacy of the present invention can be easily understood for disclosed content.The present invention can also pass through in addition different specific realities
The mode of applying is embodied or practiced, the various details in this specification can also based on different viewpoints and application, without departing from
Various modifications or alterations are carried out under spirit of the invention.
Please refer to Fig. 1-6.It should be noted that only the invention is illustrated in a schematic way for diagram provided in the present embodiment
Basic conception, only shown in schema then with related component in the present invention rather than component count, shape when according to actual implementation
Shape and size are drawn, when actual implementation kenel, quantity and the ratio of each component can arbitrarily change for one kind, and its component cloth
Office's kenel may also be increasingly complex.
As shown in figures 1 to 6, the embodiment of the present invention discloses a kind of track data processing method of telecontrol equipment, the movement
The track data processing method of device at least includes the following steps: step S10, obtaining the successive of telecontrol equipment external environment profile
The relative translation amount (namely translation knots modification) and inertia measurement list of encoder 12 between two frame point clouds and the two frames point cloud
The angulation change amount of member 13, wherein the two frames point cloud is labeled as reference frame point cloud and present frame point cloud respectively；Step S20,
Using the translation knots modification and the angulation change amount as initial transformation matrix；Step S30, the initial transformation matrix is utilized
The present frame point cloud is converted, to obtain transition frames point cloud；Step S40, the reference frame point cloud and the mistake are utilized
It crosses frame point cloud and obtains corresponding points to set；Step S50, the track number of the telecontrol equipment is obtained to set using the corresponding points
According to.Using the track data processing method of the telecontrol equipment of the present embodiment, by reasonably selecting initial rigid body translation (initial transformation
Matrix), with the angulation change amount of the relative translation amount of encoder and Inertial Measurement Unit, telecontrol equipment movement can be effectively reduced
Cumulative errors during the reckoning of track.
It should be noted that in the present embodiment, the telecontrol equipment for example may include that can execute work automatically
Installations, such as robot, the robot is installed with it a cloud acquisition device 11, encoder 12, Inertial Measurement Unit
13 (Inertial Measurement Unit, IMU) and processing unit 14, the track data refer to the movement of robot
Track data.Certainly, in other embodiments, the telecontrol equipment, which is for example also possible to installation, a cloud acquisition device, coding
Other telecontrol equipments such as electric vehicle of device, Inertial Measurement Unit and processing unit 14.It should be noted that in other embodiments
In, the processing unit 14 be also possible to independently of the device of telecontrol equipment namely its can be not mounted on telecontrol equipment.
In step slo, the reference frame point cloud and present frame point cloud are the points by being installed on robot
What cloud point cloud acquisition device 11 was obtained.Described cloud acquisition device 11 can be obtained such as can be laser scanning device
The equipment of the point cloud (including many a points) of external environment profile, the point mysorethorn matter that laser scanning device obtains when robot is run
On give range information of the robot from environment edge, according to these range informations, self-position is may be implemented in robot
It determines, the function of Real Time Obstacle Avoiding also may be implemented, for convenient for subsequent explanation, by the reference frame point Yun Jiwei { Pi, it is described to work as
Previous frame point cloud { Qi}.In a specific embodiment, every frame point cloud of the acquisition scanned by laser scanning device for example may be used
To include 1081 points, each point can be indicated with rotation angle peace shifting amount.
In step slo, the Inertial Measurement Unit 13 is used to obtain the angulation change amount of telecontrol equipment, is measurement object
The device of triaxial attitude angle (or angular speed) and acceleration.Generally, an IMU contain three uniaxial accelerometers and
Three uniaxial gyros, accelerometer detection object founds the acceleration signal of three axis in carrier coordinate system unification and independence, and gyro detects
Angular velocity signal of the carrier relative to navigational coordinate system, the angular speed and acceleration of measurement object in three dimensions, and with this
Calculate the posture of object.It should be noted that in order to improve reliability more sensors can also be equipped with for each axis.
In step slo, the encoder 12 (encoder) is to work out signal (such as bit stream) or data, turn
Being changed to, which can be used to, communicates, the equipment of the signal form of transimission and storage.The encoder can be incremental encoder and absolutely
Formula encoder；Incremental encoder is will to be displaced to be converted into periodic electric signal, then this electric signal is transformed into counting arteries and veins
Punching indicates the size of displacement with the number of pulse.The corresponding determining digital code in each position of absolute type encoder, because
This its indicating value is only related to the starting and ending positions of the measurement, and is independent of the intermediate process of measurement.In the present embodiment, institute
On the output shaft that states encoder 12 for example and can be installed on the driving motor of the wheel of robot.
In step S20, what is exported due to encoder 12 and Inertial Measurement Unit 13 is high-frequency data stream, is being obtained
The angular amount that a large amount of encoder translational movements and Inertial Measurement Unit have been exported when two frame point clouds, needs to carry out this tittle at this time
Compound operation is to obtain the relative translation amount (x of encoder 12 between two frame point clouds0, y0) and the angle of Inertial Measurement Unit 13 change
Variable θ0, can just be used as initial rigid body translation (namely initial transformation matrix).
In step s 30, by the relative translation amount (x of encoder 120, y0) and Inertial Measurement Unit 13 angulation change amount
θ0The present frame point cloud is acted on as initial transformation matrix, namely using initial transformation matrix in the present frame point cloud
Each click-through row data transformation, to obtain transition frames point cloud, so that transition frames point cloud is sufficiently close together reference frame.
As shown in Fig. 2, in step s 40, it is described to be corresponded to using the reference frame point cloud and transition frames point cloud
Point also at least includes the following steps the step of set: telecontrol equipment when step S41, to obtain the reference frame point cloud
It is oriented with reference to sight, for each of transition frames point cloud point, described with reference to described in viewing proximity range searching
Point range corresponding to this point in reference frame point cloud；Step S42, it is corresponding with the point with the reference frame point cloud to calculate each point
The distance between point range, selected distance is minimum and meets the two o'clock of pre-determined distance threshold value and establishes corresponding points pair；Time step S43,
The all the points in the transition frames point cloud are gone through, to establish the corresponding points to set.
Further, in step S41, in order to improve search efficiency (computational efficiency), in the present embodiment, with the ginseng
The robot for examining frame point cloud position is oriented with reference to sight, for each of transition frames point cloud point, from described
Start the point range corresponding to this point into reference frame point cloud described in the point by point search of both sides with reference to sight.
In step S41, when obtaining the reference frame point cloud telecontrol equipment be oriented with reference to reference to sight into
When row corresponds to point search, need to be arranged points range and pre-determined distance threshold value.
In the present embodiment, the direction (current gaze) and the ginseng of the robot when present frame point cloud be will acquire
It examines angle between sight and is defined as sight angle, by angle between two laser harness of laser scanning device (point cloud acquisition device)
It is defined as harness clips degree, the points range can be determined according to the sight angle and the harness angle.In this implementation
In example, the range of the sight angle for example can be 0-20 °.
In one embodiment, such as sight sight angle can be added into a natural number divided by the quotient of the harness angle
Twice of (such as can be 0,1,2 ...) is used as the points range；As an example, the sight angle is, for example, 10 degree,
The harness angle is 0.25, then can the points range be 80 plus a natural number, such as the points range takes 80.?
In another embodiment, a specific value can be set as points range, such as can be 50.It should be noted that at it
In his embodiment, the setting of points range can be carried out according to the actual situation, however it is not limited to above two method.
In the present embodiment, the pre-determined distance threshold value is set according to the range ability of robot between two frame point clouds
It sets, such as can be configured according to the maximal translation amount of robot between two frame point clouds, maximal translation amount can be according to two frames
The maximum movement speed of time interval and robot between point cloud carries out calculating acquisition.It should be noted that in the present embodiment,
The running track data for obtaining robot generally require and carry out multiple iterative process, utilize the reference frame in first time
Point cloud and transition frames point cloud obtained in the step of corresponding points are to set, using the maximal translation of robot between two frame point clouds
Amount is used as pre-determined distance threshold value, and successively iteration, used pre-determined distance threshold value are described more default than last time for subsequent every progress
Distance threshold is small, it is to be understood that the reduction mode of front and back pre-determined distance threshold value twice can carry out spirit according to the actual situation
Setting living.As an example, every carry out an iteration, this used pre-determined distance threshold value for example can be using described in last time
The half of pre-determined distance threshold value, namely carry out by the way of halving the setting of pre-determined distance threshold value.
As an example, the operating rate of the robot for example can be 2m/s, the time interval example between two frame point clouds
It such as can be 30ms, the maximal translation amount that can calculate robot is 0.06m, also can be by the pre-determined distance threshold value of first time
Be set as 0.06m, when subsequent each iteration, distance threshold successively can be set to 0.03m, 0.015m, 0.075m ....
In step S42 and S43, by the way that the condition of pre-determined distance threshold value is added, it can exclude a part of undesirable
Corresponding points pair, the corresponding points of foundation can be denoted as { P' to seti, Q'i}。
As shown in figure 3, in step s 5, the track data of the telecontrol equipment is obtained to set using the corresponding points
Step at least includes the following steps: step S51, establishing about the corresponding points to corresponding points all in set to error metrics
Objective function；Step S52, the objective function is solved using optimization algorithm, to obtain target variable increment；Step S53, basis
Initial transformation matrix described in the target variable incremental update, to obtain the track data of the telecontrol equipment.
Specifically, in step s 51, for the corresponding points obtained in step S4 to set { P'i, Q'iAssigned error degree
Amount, and establish following error function (objective function):
Above formula indicates the value of R and t when the subsequent expression formula of arg min being made to reach minimum value, wherein in formula R be about
The spin matrix of angle, θ is rotated, t is translation vector (x, y), and N is corresponding points to set { P'i, Q'iIn corresponding points pair number.
It should be noted that in corresponding points to set { P'i, Q'iOn, what the present embodiment was chosen is squared distance function, which has
Convexity and slickness facilitate the linear optimization in subsequent step S52 to approach.It is understood that in other embodiments,
The functional form of other suitable forms can be chosen.
Specifically, in step S52 and step S53, the target letter in above-mentioned optimization problem is linearized about target variable
Number, obtains the system of linear equations as follows about target variable-rotation angle, θ and translational movement x, y:
In formula, P'ixAnd P'iyRespectively corresponding points are to set { P'i, Q'iIn i-th point in the flat of the direction translational movement x and y
Move component, Q'ixAnd Q'iyRespectively corresponding points are to set { P'i, Q'iIn the i-th point of translation minute in the direction translational movement x and y
Amount, N is corresponding points to set { P'i, Q'iIn corresponding points pair number.
Such as Gaussian elimination method can be used and can be calculated target variable increment (Δ x, Δ y, Δ θ), it updates at this time initial
Transformation matrix are as follows:
(x1,y1,θ1)=(x0,y0,θ0)+(Δx,Δy,Δθ)
Using updated initial transformation matrix as the track data of telecontrol equipment.
It should be noted that under normal circumstances, only by executing a S10-S50 the step of acquired track data simultaneously
It is impossible to meet the requirements of test accuracy, and error is larger, in order to be further reduced the accumulated error of track reckoning, improve sportswear
The accuracy of running track reckoning is set, the track data processing method of the telecontrol equipment further includes step S60, judges the rail
The step of whether mark data meet termination condition, if "No", the track data that will acquire is returned as initial transformation matrix
Return it is described using the initial transformation matrix to present frame point cloud progress according to transformation, the step of to obtain transition frames point cloud into
Row iteration (namely return step S30), the institute until the track data meets termination condition, when will meet the termination condition
Motion trace data is stated as final data and is exported.
Using the track data processing method of telecontrol equipment of the invention, by by the relative translation amount of encoder 12 and used
Property measuring unit 13 angulation change amount as initial transformation matrix, can effectively reduce when telecontrol equipment running track calculates
Accumulated error improves the accuracy that telecontrol equipment running track calculates, obtained final rigid body translation (namely track data)
With very high confidence level, to reduce the accumulated error of track reckoning, while arithmetic speed is also improved, for autonomous
The landing application of mobile robot (telecontrol equipment) has a very important significance
In the present embodiment, judging whether the motion trace data meets termination condition is by judging that the target becomes
Whether amount increment meets termination condition to carry out；If "Yes", using updated initial transformation matrix as rigid body translation square
Battle array is output to the storage of service unit 14, the rigid body translation matrix namely two frames point cloud (reference frame point cloud and present frame point cloud) it
Between rotation amount and translational movement, as the track data；If "No", the track data that will acquire is as initial transformation
Matrix, return is described to carry out according to transformation, to obtain transition frames point cloud the present frame point cloud using the initial transformation matrix
The step of (namely return step S30) be iterated, until the target variable increment meets termination condition, when the target becomes
It is when amount increment meets the termination condition, then using the track data of the telecontrol equipment of the secondary acquisition as final data and defeated
Out.
In the present embodiment, judge that the step of whether the target variable increment meets termination condition can both be located at step
Between S52 and step S53, it can also be located at after step S53.When judging whether the target variable increment meets termination condition
The step of between rapid S52 and step S53 when, if it is determined that result be "No", then follow the steps S53, according to the target become
Initial transformation matrix described in incremental update is measured, and using updated initial transformation matrix as initial transformation matrix, return step
S30 continues to execute step S30 and subsequent step, until the target variable increment meets the termination condition.When judging
It states that the step of whether target variable increment meets termination condition is located at after step S53, if it is determined that result is "No", then will update
Initial transformation matrix afterwards continues to execute step S30 and subsequent step as initial transformation matrix, return step S30, until
The target variable increment meets the termination condition.
It should be noted that return is described to utilize the initial transformation matrix to the present frame point cloud in step S60
It carries out according to transformation, when (return step 30) is iterated the step of to obtain transition frames point cloud, is utilizing the reference frame point cloud
It obtains obtaining by the way of reducing pre-determined distance threshold value described in the step of corresponding points are to set with the transition frames point cloud
Corresponding points are to set.Relevant portion description as detailed above, this will not be repeated here.
In step S60, the termination condition for example may include meeting preset condition or the number of iterations to change more than default
Generation number.Wherein, such as target variable increment whether sufficiently small (being less than a certain specified value) can be used or whether no longer change
It is changed into as the preset condition, the default the number of iterations, which can according to need, to be rationally arranged, as an example, described default
The number of iterations for example can be 20 times, it should be noted that preset condition and default the number of iterations can according to the actual situation into
The reasonable setting of row, is not limited with enumerating herein.
It should be noted that the record of the number of iterations can pass through the memory in the service unit 14 mentioned hereinafter
The computer program that stores in 143 is realized, can also be realized by setting special counter.When using special counting
When device, counter can be connected directly with the processor 141 of service unit 14, and the number of iterations of counter is sent to processor
141, for determining whether the number of iterations is more than default the number of iterations, the counter is also used as at the running track of telecontrol equipment
One element of reason system.
As Figure 4-Figure 6, the embodiment of the present invention is also provided at a kind of track data for realizing above-mentioned telecontrol equipment
Reason system, the processing system include service unit 14, and the point cloud point cloud acquisition device 11 being set in robot, coding
Device 12 and inertia test cell 14；Described cloud acquisition device 11 is connect with the service unit 14, for obtaining the fortune
The external environment profile point cloud of dynamic device；The encoder 12 is connect with the service unit 14, and the encoder 12 is for obtaining
Take the relative translation amount of the telecontrol equipment；The inertia test cell 14 is connect with the service unit 14, and the inertia is surveyed
Examination unit 14 is used to obtain the angulation change amount of the telecontrol equipment；Wherein, the service unit 14 is used for the institute according to acquisition
It states external environment profile point cloud, the relative translation amount and the angulation change amount and obtains track data.Fig. 6 shows this reality
The work flow diagram of the running track processing system of the telecontrol equipment of example is applied, detailed process relevant portion as detailed above is retouched
It states, this will not be repeated here.
In the present embodiment, the service unit 14 includes: communicator 142, is used for and PERCOM peripheral communication；Memory 143 is used
In storage computer program；Processor 141 connects the communicator 142 and memory 143, for running the computer journey
Sequence is to execute the track data processing method of the telecontrol equipment.
It should be noted that the computer program in above-mentioned memory 142 can be real by way of SFU software functional unit
Now and when sold or used as an independent product, it can store in a computer readable storage medium.Based on such
Understand, the technical solution of the present invention substantially part of the part that contributes to existing technology or the technical solution in other words
It can be expressed in the form of software products, which is stored in a storage medium, including some instructions
With so that a computer equipment (can be personal computer, electronic equipment or the network equipment etc.) executes the present invention respectively
The all or part of the steps of a embodiment method.The computer readable storage medium for example may include USB flash disk, mobile hard disk,
Read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic
The various media that can store program code such as dish or CD.
It should be noted that the track data processing method and acquisition system of telecontrol equipment through this embodiment obtained
Robot trajectory's data, if the position coordinates of a given wherein frame point cloud, can calculate the position of another frame point cloud.With this
Analogize, the point Yun Xulie obtained by the carried laser scanning device of robot, the motion profile of our available robots.
Therefore, the track data processing method of the telecontrol equipment of the present embodiment and acquisition system can be applied to the synchronous positioning of robot and build
Scheme (Simultaneously Localization and Mapping).
It should be noted that the track data processing method and acquisition system of the telecontrol equipment of the present embodiment, are based on the short time
The relative increment that inner encoder 12 and Inertial Measurement Unit 13 provide is accurately true, using this knots modification as point cloud
Initial transformation matrix with (frame Point Cloud Processing) can so fall into locally optimal solution to avoid optimization problem, then change
For formula solving optimization problem and update initial rigid body translation；By reference to robot where frame point cloud in iterative process each time
Corresponding points pair are searched for towards being used as with reference to sight, can so reduce search calculation amount；So obtained final rigid body becomes
(namely track data) is changed with very high confidence level, to reduce the accumulated error of track reckoning, is also improved simultaneously
Arithmetic speed has a very important significance the landing application of autonomous mobile robot.
In description herein, many specific details, the example of such as component and/or method, to provide to this are provided
Inventive embodiments are understood completely.However, those skilled in the art will appreciate that can be in no one or more details
In the case where or by other equipment, system, component, method, component, material, part etc. practice the embodiment of the present invention.
In other cases, the well known structure of not specifically illustrated or detailed description, material or operation, to avoid the embodiment of the present invention is made
Aspect fogs.
" one embodiment (one embodiment) ", " embodiment (an are mentioned throughout the manual
Embodiment) " or " specific embodiment (a specific embodiment) " means and the specific spy that describes in conjunction with the embodiments
Sign, structure or characteristic are included at least one embodiment of the present invention, and not necessarily in all embodiments.Thus,
Phrase " (in one embodiment) in one embodiment ", the " (in embodiment of different places in entire description
An embodiment) " or each presentation of " (in a specific embodiment) in a particular embodiment " be not necessarily
Refer to identical embodiment.In addition, a particular feature, structure, or characteristic of any specific embodiment of the invention can be by any suitable
Mode in conjunction with one or more other embodiments.It should be understood that other modifications of inventive embodiments described and illustrated herein
It may be according to a part instructed herein, and spirit and scope of the invention will be considered with modification.
It should also be understood that can also by more separate or more integrate in a manner of implement one or more of element shown in attached drawing,
Or it is even removed because of that cannot operate in some cases or because according to specific application can be useful and be mentioned
For.
In addition, unless otherwise explicitly indicated, any mark arrow in attached drawing should be considered only as illustratively, and not be
Limitation.In addition, unless otherwise specified, the term as used herein "or" is generally intended to indicate "and/or".In term because providing separation
Or combination ability be unclear and in the case where being foreseen, the combination of component or step also will be regarded as being specified.
It is used such as in description herein and entire chapter claims below, unless otherwise specified, " one (a) ", " one
A (an) " and " being somebody's turn to do (the) " include plural reference object.Equally, such as in description herein and entire chapter claims below
It is used, unless otherwise specified, " ... in (in) " the meaning include " ... in (in) " and " ... go up (on) ".
The foregoing description (including the content described in abstract of description) of illustrated embodiment of the present invention is not intended to detailed
Enumerate or limit the invention to precise forms disclosed herein.Although describing this hair merely for the purpose of illustration herein
Bright specific embodiment and example of the invention, but as those skilled in the art will recognize and appreciate that, it is various equivalent to repair
Changing can be within the spirit and scope of the present invention.As noted, it can be retouched according to the above-mentioned of embodiment of the present invention
It states and these modifications is carried out to the present invention, and these modifications will within the spirit and scope of the present invention.
System and method are described as on the whole herein to help to understand details of the invention.In addition, having given
Various details are gone out to provide the overall understanding of the embodiment of the present invention.However, those skilled in the relevant art will be appreciated that
It arrives, the embodiment of the present invention can be practiced in the case where none or multiple details, or utilize other dresses
It sets, system, accessory, method, component, material, part etc. are practiced.In other cases, and it is not particularly shown or retouch in detail
Known features, material and/or operation is stated to cause to obscure to avoid the various aspects to the embodiment of the present invention.
Thus, although the present invention is described in this paper with reference to its specific embodiment, modify free, various changes
It is intended in disclosed above with replacement, and it is to be understood that in some cases, without departing from the scope and spirit for proposing invention
Under the premise of, some features of the invention will be used in the case where no correspondence uses other features.Therefore, can be permitted
More modifications, so that specific environment or material adapt to essential scope and spirit of the invention.The present invention is not intended to limit under
Specific term used in the claims of face and/or specific real to execute disclosed in best mode of the invention as imagining
Example is applied, but the present invention will include any and all embodiments fallen within the scope of the accompanying claims and equivalent.Thus,
The scope of the present invention will be only determined by appended claims.
Claims (10)
1. a kind of track data processing method of telecontrol equipment characterized by comprising
Encoder is relatively flat between the successive two frames point cloud and the two frames point cloud of acquisition telecontrol equipment external environment profile
The angulation change amount of shifting amount and Inertial Measurement Unit, wherein respectively by the two frames point cloud labeled as reference frame point cloud and current
Frame point cloud；
Initial transformation matrix is constructed by the translation knots modification and the angulation change amount；
Data transformation is carried out to the present frame point cloud using the initial transformation matrix, to obtain transition frames point cloud；
Corresponding points are obtained to set using the reference frame point cloud and transition frames point cloud；
The track data of the telecontrol equipment is obtained to set using the corresponding points.
2. the track data processing method of telecontrol equipment according to claim 1, which is characterized in that described using described right
The step of obtaining the track data of the telecontrol equipment to set, which should be put, includes:
It establishes about the corresponding points to corresponding points all in set to the objective function of error metrics；
Objective function described in Optimization Solution, to obtain target variable increment；
According to initial transformation matrix described in the target variable incremental update, to obtain the track data of the telecontrol equipment.
3. the track data processing method of telecontrol equipment according to claim 2, which is characterized in that described to be calculated using optimization
Method solves the objective function, includes: the step of target variable increment to obtain
The objective function described in target variable linear optimization, to obtain the equation group about the target variable；
The equation group of the target variable is solved, to obtain target variable increment.
4. the track data processing method of telecontrol equipment according to claim 2, which is characterized in that the acquisition methods are also
Include:
The step of whether track data meets termination condition judged；
If "No", for the track data that will acquire as initial transformation matrix, return is described to utilize the initial transformation square
Battle array be iterated the step of transition frames point cloud to obtain, according to transformation until the track data to the present frame point cloud
Meet termination condition, motion trace data when meeting the termination condition as final data and is exported.
5. the track data processing method of telecontrol equipment according to claim 4, which is characterized in that the institute that will acquire
Track data is stated as initial transformation matrix, return it is described using the initial transformation matrix to each of described present frame point cloud
The transformation of row data is clicked through, when being iterated the step of transition frames point cloud to obtain, is utilizing the reference frame point cloud and the mistake
It crosses frame point cloud to obtain in the step of corresponding points are to set, the pre-determined distance threshold value of the pre-determined distance threshold value of use than last time
It is small.
6. the track data processing method of telecontrol equipment according to claim 1, which is characterized in that the telecontrol equipment packet
Include robot.
7. the track data processing method of telecontrol equipment described in -6 any one according to claim 1, which is characterized in that described
Obtain corresponding points using the reference frame point cloud and transition frames point cloud includes: to the step of set
The telecontrol equipment is oriented with reference to sight when obtaining the reference frame point cloud, in the transition frames point cloud
Each point, the point range corresponding to this point in the reference frame point cloud with reference to described in viewing proximity range searching；
The distance between each point and point range corresponding to this point in the reference frame point cloud are calculated, selected distance is minimum and full
The two o'clock of sufficient pre-determined distance threshold value establishes corresponding points pair；
The all the points in the transition frames point cloud are traversed, to establish the corresponding points to set.
8. the track data processing method of telecontrol equipment according to claim 7, which is characterized in that it is described with obtain described in
The telecontrol equipment is oriented with reference to sight when reference frame point cloud, for each of transition frames point cloud point, in institute
Stating the step of referring to point range corresponding to this point in reference frame point cloud described in viewing proximity range searching includes:
It is oriented with the robot of reference frame point cloud position with reference to sight, for each in the transition frames point cloud
It is a, since it is described with reference to sight into reference frame point cloud described in the point by point search of both sides point range corresponding to this point.
9. a kind of running track processing system of telecontrol equipment characterized by comprising
Service unit；
Point cloud acquisition device, is set on telecontrol equipment, described cloud acquisition device is connect with the service unit, described cloud
Acquisition device is used to obtain the external environment profile point cloud of the telecontrol equipment；
Encoder is set on the telecontrol equipment, and the encoder is connect with the service unit, and the encoder is for obtaining
Take the relative translation amount of the telecontrol equipment；
Inertia test cell is set on the telecontrol equipment, and the inertia test cell is connect with the service unit, described
Inertia test cell is used to obtain the angulation change amount of the telecontrol equipment；
Wherein, the service unit is used for the external environment profile point cloud, the relative translation amount and institute according to acquisition
It states angulation change amount and obtains track data.
10. a kind of service unit characterized by comprising
Communicator, is used for and PERCOM peripheral communication；
Memory, for storing computer program；
Processor connects the communicator and memory, for running the computer program to execute such as claim 1 to 8
Any one of described in telecontrol equipment track data processing method.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910615659.0A CN110345936B (en) | 2019-07-09 | 2019-07-09 | Track data processing method and processing system of motion device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910615659.0A CN110345936B (en) | 2019-07-09 | 2019-07-09 | Track data processing method and processing system of motion device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110345936A true CN110345936A (en) | 2019-10-18 |
| CN110345936B CN110345936B (en) | 2021-02-09 |
Family
ID=68178637
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910615659.0A Active CN110345936B (en) | 2019-07-09 | 2019-07-09 | Track data processing method and processing system of motion device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110345936B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110930444A (en) * | 2019-11-29 | 2020-03-27 | 上海有个机器人有限公司 | Point cloud matching method, medium, terminal and device based on bilateral optimization |
| CN110956666A (en) * | 2019-11-12 | 2020-04-03 | 深圳市瑞立视多媒体科技有限公司 | Motion data calibration method and device, terminal equipment and storage medium |
| CN110991526A (en) * | 2019-11-29 | 2020-04-10 | 上海有个机器人有限公司 | Non-iterative point cloud matching method, medium, terminal and device |
| CN113176782A (en) * | 2021-05-21 | 2021-07-27 | 福建盛海智能科技有限公司 | Autonomous path-changing tracking method and unmanned vehicle |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103914875A (en) * | 2014-04-17 | 2014-07-09 | 中国科学院深圳先进技术研究院 | Functional modeling method for indoor scene |
| CN104764457A (en) * | 2015-04-21 | 2015-07-08 | 北京理工大学 | Urban environment composition method for unmanned vehicles |
| CN105261060A (en) * | 2015-07-23 | 2016-01-20 | 东华大学 | Point cloud compression and inertial navigation based mobile context real-time three-dimensional reconstruction method |
| CN105783913A (en) * | 2016-03-08 | 2016-07-20 | 中山大学 | SLAM device integrating multiple vehicle-mounted sensors and control method of device |
| CN106406338A (en) * | 2016-04-14 | 2017-02-15 | 中山大学 | Omnidirectional mobile robot autonomous navigation apparatus and method based on laser range finder |
| CN106767827A (en) * | 2016-12-29 | 2017-05-31 | 浙江大学 | A kind of mobile robot point cloud map creating method based on laser data |
| KR20190064311A (en) * | 2017-11-30 | 2019-06-10 | 주식회사 모빌테크 | Method and apparatus for building map using LiDAR |
-
2019
- 2019-07-09 CN CN201910615659.0A patent/CN110345936B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103914875A (en) * | 2014-04-17 | 2014-07-09 | 中国科学院深圳先进技术研究院 | Functional modeling method for indoor scene |
| CN104764457A (en) * | 2015-04-21 | 2015-07-08 | 北京理工大学 | Urban environment composition method for unmanned vehicles |
| CN105261060A (en) * | 2015-07-23 | 2016-01-20 | 东华大学 | Point cloud compression and inertial navigation based mobile context real-time three-dimensional reconstruction method |
| CN105783913A (en) * | 2016-03-08 | 2016-07-20 | 中山大学 | SLAM device integrating multiple vehicle-mounted sensors and control method of device |
| CN106406338A (en) * | 2016-04-14 | 2017-02-15 | 中山大学 | Omnidirectional mobile robot autonomous navigation apparatus and method based on laser range finder |
| CN106767827A (en) * | 2016-12-29 | 2017-05-31 | 浙江大学 | A kind of mobile robot point cloud map creating method based on laser data |
| KR20190064311A (en) * | 2017-11-30 | 2019-06-10 | 주식회사 모빌테크 | Method and apparatus for building map using LiDAR |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110956666A (en) * | 2019-11-12 | 2020-04-03 | 深圳市瑞立视多媒体科技有限公司 | Motion data calibration method and device, terminal equipment and storage medium |
| CN110930444A (en) * | 2019-11-29 | 2020-03-27 | 上海有个机器人有限公司 | Point cloud matching method, medium, terminal and device based on bilateral optimization |
| CN110991526A (en) * | 2019-11-29 | 2020-04-10 | 上海有个机器人有限公司 | Non-iterative point cloud matching method, medium, terminal and device |
| CN110991526B (en) * | 2019-11-29 | 2023-11-28 | 上海有个机器人有限公司 | Non-iterative point cloud matching method, medium, terminal and device |
| CN110930444B (en) * | 2019-11-29 | 2023-12-12 | 上海有个机器人有限公司 | Point cloud matching method, medium, terminal and device based on bilateral optimization |
| CN113176782A (en) * | 2021-05-21 | 2021-07-27 | 福建盛海智能科技有限公司 | Autonomous path-changing tracking method and unmanned vehicle |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110345936B (en) | 2021-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110345936A (en) | The track data processing method and its processing system of telecontrol equipment | |
| CN111052183B (en) | Vision inertial odometer using event camera | |
| CN106056664B (en) | A kind of real-time three-dimensional scene reconstruction system and method based on inertia and deep vision | |
| CN109506642B (en) | Robot multi-camera visual inertia real-time positioning method and device | |
| CN105931275A (en) | Monocular and IMU fused stable motion tracking method and device based on mobile terminal | |
| CN108051002A (en) | Transport vehicle space-location method and system based on inertia measurement auxiliary vision | |
| CN111578937B (en) | Visual inertial odometer system capable of simultaneously optimizing external parameters | |
| CN109084732A (en) | Positioning and air navigation aid, device and processing equipment | |
| US20210183100A1 (en) | Data processing method and apparatus | |
| CN112815939B (en) | Pose estimation method of mobile robot and computer readable storage medium | |
| CN109676604A (en) | Robot non-plane motion localization method and its motion locating system | |
| CN112907678B (en) | Vehicle-mounted camera external parameter attitude dynamic estimation method and device and computer equipment | |
| Gemeiner et al. | Simultaneous motion and structure estimation by fusion of inertial and vision data | |
| CN110726406A (en) | Improved nonlinear optimization monocular inertial navigation SLAM method | |
| CN107941217A (en) | A kind of robot localization method, electronic equipment, storage medium, device | |
| CN109443348A (en) | It is a kind of based on the underground garage warehouse compartment tracking for looking around vision and inertial navigation fusion | |
| CN109141411B (en) | Positioning method, positioning device, mobile robot, and storage medium | |
| CN110942474B (en) | Robot target tracking method, device and storage medium | |
| CN107782304A (en) | The localization method and device of mobile robot, mobile robot and storage medium | |
| Nilsson et al. | Reliable vehicle pose estimation using vision and a single-track model | |
| CN103076023A (en) | Method and device for calculating step | |
| KR20190081334A (en) | Method for tracking moving trajectory based on complex positioning and apparatus thereof | |
| CN114593735A (en) | Pose prediction method and device | |
| CN112284381B (en) | Visual inertia real-time initialization alignment method and system | |
| CN108051001A (en) | A kind of robot movement control method, system and inertia sensing control device |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |