CN103105852B - Displacement calculates method and apparatus and immediately locates and map constructing method and equipment - Google Patents

Abstract

Do you disclose a kind of synchronous superposition (Simultaneous? Localization? and? Mapping, SLAM) method and apparatus.Described displacement method comprises the steps: the acceleration α, the orientation vector β that repeatedly obtain described mobile object (as robot or handheld device) based on Fixed Time Interval δ t; At time t, according to the data measured and the instantaneous velocity V ' (t-Δ t) estimated before, obtain the displacement D (t) of this object during the time interval Δ t from time t-Δ t to time t and this mobile object orientation vector β (t) at time t, wherein Δ t=n δ t, n ∈ N; Based on the data gathered by the three-dimensional camera that this mobile object is installed and orientation vector β (t) estimated displacement, and filtering is carried out to obtain Δ D ' (t) for described displacement D (t), and then complete the structure of three-dimensional map; Instantaneous velocity V ' (t) is upgraded according to Δ D ' (t) that filtering obtains, and by described instantaneous velocity V ' (t) for synchronous shift and map structuring calculate next time.

Description

Displacement calculates method and apparatus and immediately locates and map constructing method and equipment

Technical field

The present invention relates to displacement calculate method and apparatus and immediately locate and map constructing method and equipment.More particularly, relate to the displacement that can improve precision and processing speed calculate method and apparatus and immediately locate and map constructing method and equipment.

Background technology

Instant location and map structuring (SimultaneousLocalizationandMapping, SLAM) are the popular research topics at present in robot localization.SLAM refers to robot under the uncertain condition of self-position, in complete graphics communication, create map, utilizes map independently locate and navigate simultaneously.SLAM problem can be described as: robot is mobile from a unknown position in circumstances not known, carries out self poisoning, build increment type map simultaneously in moving process according to location estimation and sensing data.Up to the present, many progress are also achieved.

Due to high computation complexity, the process that SLAM is normally very consuming time.Output accuracy and processing speed (frame is per second, FPS) can be improved by adopting other devices (e.g., camera and mileometer).

Such as, according in the SLAM of prior art, any mileometer can not be adopted and only adopt monocular camera or stereoscopic camera to follow the tracks of road sign (landmark), and estimating displacement every a time step.The shortcoming of this method is: first, because high calculated load makes such process be very consuming time; In addition, precision is not high yet; Finally, the method also can be subject to the restriction of bias light, such as, when bias light is darker, and poor effect.

And for example, according in the SLAM of prior art, the mileometer of motor machine can be used to estimate displacement.But the shortcoming of this method is: first, the design of motor machine and production are very complicated and expensive; Secondly, along with the use of the mileometer of motor machine and aging, stability reduces; 3rd, the mileometer of this motor machine is only applicable to two-dimensional measurement, can not be used for three-dimensional measurement; Finally, the maintenance cost of the mileometer of motor machine is very high and very complicated.

Summary of the invention

In view of above situation, expect to provide a kind of method and the equipment based on the method, it can by adopting inertial navigation system (InertialNavigationSystem, INS) improve in circumstances not known for the performance that the SLAM of mobile object (e.g., robot or handheld device) applies.

According to an aspect of the embodiment of the present invention, providing a kind of method for calculating the displacement resembled for a pair in the circumstances not known of space in instant location with map structuring (SLAM), comprising the steps:

Acceleration α, the orientation vector β of described object is repeatedly obtained based on Fixed Time Interval δ t;

At time t, according to the data measured and the instantaneous velocity V ' (t-Δ t) estimated before, obtain the displacement D (t) of this object during the time interval Δ t from time t-Δ t to time t and this object orientation vector β (t) at time t, wherein Δ t=n δ t, n ∈ N;

Based on the ambient image of being taken by the image acquisition units that this object is installed and orientation vector β (t), filtering is carried out to obtain Δ D ' (t) for described displacement D (t);

Upgrade instantaneous velocity V ' (t) according to Δ D ' (t) that filtering obtains, and described instantaneous velocity V ' (t) is calculated for displacement next time.

Preferably, according in the method for the embodiment of the present invention, according to following formulae discovery displacement D (t):

$\mathrm{\ΔD}\left(t\right)=\underset{j=0}{\overset{n-1}{\mathrm{\Σ}}}(\underset{i=0}{\overset{j}{\mathrm{\Σ}}}{\mathrm{\α}}_i\·\mathrm{\δt})\·\mathrm{\δt}+V^{\′}(t-\mathrm{\Δt})\·\mathrm{\Δt},$ Wherein $V^{\′}\left(t\right)=\frac{\mathrm{\Δ}{D}^{\′}\left(t\right)}{\mathrm{\Δt}},$ And the initial value of V ' (t) is set to 0.

Preferably, according in the method for the embodiment of the present invention, based on the ambient image of being taken by the image acquisition units that this object is installed and orientation vector β (t), filtering is carried out for described displacement D (t) and comprising to obtain Δ D ' (t):

The ambient image of first taking based on described image acquisition units and this object at orientation vector β (t) of time t, by the displacement D during SLAM method interval of delta t computing time _sLAM(t); Then by Δ D _sLAMt () and Δ D (t) are weighted on average, to obtain Δ D ' (t).

In addition, according to another aspect of the embodiment of the present invention, provide a kind of instant location and map structuring SLAM method, wherein calculate the displacement of an object in the circumstances not known of space by foregoing method.

In addition, according to a further aspect of the invention, provide a kind of equipment that be applied to instant location and map structuring SLAM, that calculate the displacement of an object in the circumstances not known of space, comprising:

Inertial navigation system INS sensor, for repeatedly measuring acceleration α, the orientation vector β of this object based on Fixed Time Interval δ t;

INS processor, for at time t, according to by the data of INS sensor measurement and the instantaneous velocity estimated before, calculate the displacement D (t) of this object during the time interval Δ t from time t-Δ t to time t and this object attitude β (t) at time t, wherein Δ t=n δ t, n ∈ N;

Image acquisition units, is arranged on on described object, for the image of the viewing angles surrounding environment of this object; SLAM application and wave filter, for the ambient image of taking based on described image acquisition units and orientation vector β (t), carry out filtering to obtain Δ D ' (t) for the displacement D (t) calculated by INS processor;

INS acts on behalf of, apply and wave filter for Δ D (t) calculated by described INS processor and β (t) are sent to described SLAM, and be sent to INS processor by by Δ D ' (t) that described SLAM applies and wave filter exports;

Wherein, instantaneous velocity V ' (t) upgraded estimated by described INS processor according to Δ D ' (t) obtained from described SLAM application and wave filter, and calculates for displacement next time.

Preferably, according in the equipment of the embodiment of the present invention, according to following formulae discovery displacement D (t):

$\mathrm{\ΔD}\left(t\right)=\underset{j=0}{\overset{n-1}{\mathrm{\Σ}}}(\underset{i=0}{\overset{j}{\mathrm{\Σ}}}{\mathrm{\α}}_i\·\mathrm{\δt})\·\mathrm{\δt}+V^{\′}(t-\mathrm{\Δt})\·\mathrm{\Δt},$ Wherein $V^{\′}\left(t\right)=\frac{\mathrm{\Δ}{D}^{\′}\left(t\right)}{\mathrm{\Δt}},$ And the initial value of V ' (t) is set to 0.

Preferably, according in the method for the embodiment of the present invention, the ambient image that first described SLAM application and wave filter are taken based on described image acquisition units and this object at orientation vector β (t) of time t, by the displacement D during SLAM method interval of delta t computing time _sLAMt (), then by Δ D _sLAMt () and Δ D (t) are weighted on average, to obtain Δ D ' (t).

Preferably, according in the method for the embodiment of the present invention, described inertial navigation system INS sensor comprises gyroscope, magnetometer and accelerometer.

In addition, according to another aspect of the embodiment of the present invention, provide a kind of instant location and map structuring SLAM equipment, comprise the foregoing equipment calculating the displacement of an object in the circumstances not known of space.

By the displacement computing method according to the embodiment of the present invention as above, displacement computing equipment, instant location is with map constructing method and immediately locate and map construction device, can obtain following advantage:

First, owing to have employed INS system, and in the process of displacement D (t) being carried out to filtering, directly adopt orientation vector β (t) obtained by INS system, therefore compared with existing SLAM, very high performance can be obtained at data precision with in the processing time.Secondly, owing to not adopting the mileometer of motor machine to estimate displacement, therefore very high system stability can be obtained.In addition, because image acquisition units can be stereoscopic camera, measure therefore, it is possible to realize 3D.In addition, owing to not adopting the mileometer of motor machine, because this reducing maintenance cost and safeguarding complexity.

Accompanying drawing explanation

Fig. 1 is the process flow diagram of diagram according to the process of the displacement computing method of the embodiment of the present invention;

Fig. 2 is the block diagram of diagram according to the configuration of the displacement computing equipment of the embodiment of the present invention.

Embodiment

Below with reference to accompanying drawings of the present invention each is preferred embodiment described.There is provided the description referring to accompanying drawing, to help the understanding to the example embodiment of the present invention limited by claim and equivalent thereof.It comprises the various details helping to understand, but they can only be counted as exemplary.Therefore, those skilled in the art will recognize that, can make various changes and modifications embodiment described herein, and do not depart from the scope of the present invention and spirit.And, in order to make instructions clearly succinct, will the detailed description to well known function and structure be omitted.

First, in instant location with map structuring (SLAM), the method resembling (such as, robot or the handheld device) displacement in the circumstances not known of space for a pair is calculated according to being applied to of the embodiment of the present invention with reference to Fig. 1 description.

Fig. 1 is the process flow diagram of diagram according to the process of the displacement computing method of the embodiment of the present invention.As shown in Figure 1, the method comprises the steps:

First, in step S101, repeatedly obtain acceleration α, the orientation vector β of described object based on Fixed Time Interval δ t.

Then, in step S102, at time t, according to the data measured in step S101 and before estimate instantaneous velocity V ' (t-Δ t), obtain the displacement D (t) of this object during the time interval Δ t from time t-Δ t to time t and this object orientation vector β (t) at time t, wherein Δ t=n δ t, n ∈ N.

Specifically, during the time interval Δ t from time t-Δ t to time t, receive n the vector of α altogether, { α _i| i=(0,1,2 ..., n-1) }.By the instantaneous velocity V ' (t-Δ t) estimated before utilization, the displacement D (t) of object during asking second order integro to assign to calculate Δ t about t for α.Concrete formula is as follows:

$\mathrm{\ΔD}\left(t\right)=\underset{j=0}{\overset{n-1}{\mathrm{\Σ}}}(\underset{i=0}{\overset{j}{\mathrm{\Σ}}}{\mathrm{\α}}_i\·\mathrm{\δt})\·\mathrm{\δt}+V^{\′}(t-\mathrm{\Δt})\·\mathrm{\Δt}...\left(1\right)$

It should be noted that the calculating of displacement D (t) is the process of a loop iteration.The initial value of V ' (t) is set to 0.

Then, in step S103, based on the ambient image of being taken by the image acquisition units that this object is installed and orientation vector β (t), filtering is carried out to obtain Δ D ' (t) for described displacement D (t).

Specifically, the ambient image of taking based on described image acquisition units and this object at orientation vector β (t) of time t, by the displacement D during SLAM method interval of delta t computing time _sLAM(t).Owing to be it is known to those skilled in the art that by the process of SLAM method displacement calculating, and emphasis of the present invention is not SLAM, but improving one's methods on SLAM basis, therefore for brevity, omit the description for it here.Then, the displacement D by will in this way calculate _sLAMt () asks weighted mean with the displacement D (t) calculated in step S102, can obtain the displacement D ' (t) reappraised.

It is pointed out that present inventor found through experiments, the precision of orientation vector β (t) obtained in step s 102 can be satisfactory, but the precision of displacement D (t) needs to be improved further.Therefore, inventive point of the present invention is the displacement D that will be calculated by SLAM method _sLAM(t) and be weighted on average by the displacement D (t) calculated according to the measurement data of INS sensor, thus the displacement D ' (t) of approaching to reality data more can be obtained.In addition, due to as previously mentioned, the precision of orientation vector β (t) obtained in step s 102 can be satisfactory, therefore by SLAM method displacement calculating Δ D _sLAMdirectly adopt this orientation vector β (t) in the process of (t), thus processing speed can be improved further.

Usually, EKF (Kalmanfilter) or particle filter (particlefilter) can be used to determine average weighted coefficient.

Finally, in step S104, according to following formula (2), upgrade instantaneous velocity V ' (t) with Δ D ' (t) obtained by filtering.

$V^{\′}\left(t\right)=\frac{\mathrm{\Δ}{D}^{\′}\left(t\right)}{\mathrm{\Δt}}...\left(2\right)$

Then described instantaneous velocity V ' (t) is calculated for displacement next time.

Hereinbefore, describing the displacement computing method according to the embodiment of the present invention by referring to Fig. 1, by being applied to instant location and map structuring, output accuracy and processing speed can be improved.Hereinafter, with reference to Fig. 2 describe according to the embodiment of the present invention be applied to instant location and map structuring SLAM, the equipment that calculates the displacement of an object in the circumstances not known of space.

Fig. 2 shows the displacement computing equipment according to the embodiment of the present invention.As shown in Figure 2, this equipment 200 comprises: INS sensor 201, INS processor 202, INS act on behalf of (INSAgent) 203, SLAM application & wave filter 204 and image acquisition units 205.

INS sensor 201 is for repeatedly measuring acceleration α, the orientation vector β of this object based on fixing sensing time interval δ t, it is connected with INS processor 202 by such as USB interface.Wherein, INS sensor 201 comprises gyroscope, magnetometer and accelerometer further.

The request that SLAM application & wave filter 204 is estimated to INS agency 203 transmission immediate movement (Δ D) with dynamic interval Δ t=n δ t (n ∈ N).Wherein, for each request, n is different.Here, SLAM application & wave filter 204 is connected by TCP/IP and INS agency 203.Here, INS agency 203 is equivalent to a transmission interface, for the swapping data at SLAM application & wave filter 204 and INS processor.

Then, during the time interval from time t-Δ t to time t, INS sensor has measured n the vector of α, { α _i| i=(0,1,2 ..., n-1) }.

INS processor 202 according to by the data of INS sensor measurement and the instantaneous velocity estimated before, calculates the displacement D (t) of this object during the time interval Δ t from time t-Δ t to time t and this object attitude β (t) at time t by above-mentioned formula (1).

Displacement D (t) during the time interval Δ t calculated and this object are sent to SLAM application & wave filter 204 at the attitude β (t) of time t via INS agency 203 by INS processor 202.

Image acquisition units 205 is arranged on on described object, for the image of the viewing angles surrounding environment of this object.It is pointed out that image acquisition units 205 here can be monocular camera, also can be stereoscopic camera, even also can be the three-dimensional imaging unit combined by two or more monocular camera.

By obtaining Δ D (t) and β (t) that INS processors 202 calculate via INS agency 203, the ambient image that SLAM application & wave filter 204 is taken based on described image acquisition units 205, carries out filtering to obtain Δ D ' (t) for the displacement D (t) calculated by INS processor 202.That is, as previously mentioned, the ambient image of taking based on described image acquisition units and this object at orientation vector β (t) of time t, by the displacement D during SLAM method interval of delta t computing time _sLAM(t).Then, the displacement D by will in this way calculate _sLAMt displacement D (t) that () and INS processor 202 calculate asks weighted mean, can reappraise position and the displacement D ' (t) of object.

As previously mentioned, orientation vector β (t) that SLAM application & wave filter 204 directly adopts INS processor 202 to calculate, can reduce matching range and improve processing speed.In addition, the displacement D will calculated by SLAM method _sLAMt displacement D (t) that () and INS processor 202 calculate is weighted on average, can obtain the displacement D ' (t) of approaching to reality data more.

Then, the displacement D ' (t) obtained after filtering is sent to INS processor 202 via INS agency 203 by SLAM application & wave filter 204.INS processor 202, according to foregoing formula (2), calculates instantaneous velocity V ' (t) upgraded, and is calculated for displacement next time by V ' (t).

Hereinbefore, the displacement computing equipment for SLAM according to the embodiment of the present invention is described with reference to Fig. 2.

In addition, it is pointed out that those skilled in the art should be understood that, the present invention also expects a kind of instant location of protection and map constructing method, wherein calculates the displacement of an object in the circumstances not known of space by above described method.Then, by the displacement that calculates and attitude, carry out the self poisoning of object, build increment type map simultaneously.

Similarly, the present invention also expects a kind of instant location of protection and map construction device, comprise by above described, for the equipment of the displacement of calculating object in the circumstances not known of space.

By the displacement computing method according to the embodiment of the present invention as above, displacement computing equipment, instant location is with map constructing method and immediately locate and map construction device, can data precision be improved and shorten the processing time, system stability can also be improved, realize 3D to measure, and reduce maintenance cost and safeguard complexity.

It should be noted that, in this manual, term " comprises ", " comprising " or its any other variant are intended to contain comprising of nonexcludability, thus make to comprise the process of a series of key element, method, article or equipment and not only comprise those key elements, but also comprise other key elements clearly do not listed, or also comprise by the intrinsic key element of this process, method, article or equipment.When not more restrictions, the key element limited by statement " comprising ... ", and be not precluded within process, method, article or the equipment comprising described key element and also there is other identical element.

Finally, also it should be noted that, above-mentioned a series of process not only comprises with the order described here temporally process that performs of sequence, and comprises process that is parallel or that perform respectively instead of in chronological order.

Through the above description of the embodiments, those skilled in the art can be well understood to the mode that the present invention can add required hardware platform by software and realize, and can certainly all be implemented by software.Based on such understanding, what technical scheme of the present invention contributed to background technology can embody with the form of software product in whole or in part, this computer software product can be stored in storage medium, as ROM/RAM, magnetic disc, CD etc., comprising some instructions in order to make a computer equipment (can be personal computer, server, or the network equipment etc.) perform the method described in some part of each embodiment of the present invention or embodiment.

Above to invention has been detailed introduction, applying specific case herein and setting forth principle of the present invention and embodiment, the explanation of above embodiment just understands method of the present invention and core concept thereof for helping; Meanwhile, for one of ordinary skill in the art, according to thought of the present invention, all will change in specific embodiments and applications, in sum, this description should not be construed as limitation of the present invention.

Claims (9)

1., for immediately locating and calculate in map structuring SLAM a method for the displacement resembled for a pair in the circumstances not known of space, comprise the steps:

Acceleration α, the orientation vector β of described object is repeatedly obtained based on Fixed Time Interval δ t;

At time t, according to the data measured and the instantaneous velocity V ' (t-Δ t) estimated before, obtain the displacement D (t) of this object during the time interval Δ t from time t-Δ t to time t and this object orientation vector β (t) at time t, wherein Δ t=n δ t, n ∈ N;

Based on the ambient image of being taken by the image acquisition units that this object is installed and orientation vector β (t), filtering is carried out to obtain Δ D ' (t) for described displacement D (t);

Upgrade instantaneous velocity V ' (t) according to Δ D ' (t) that filtering obtains, and described instantaneous velocity V ' (t) is calculated for displacement next time.

2. method according to claim 1, wherein according to following formulae discovery displacement D (t):

$\mathrm{\Δ}D\left(t\right)=\underset{j=0}{\overset{n-1}{\Σ}}(\underset{i=0}{\overset{j}{\Σ}}{\mathrm{\α}}_i\·\mathrm{\δ}t)\·\mathrm{\δ}t+V^{\′}(t-\mathrm{\Δ}t)\·\mathrm{\Δ}t,$ Wherein $V^{\′}\left(t\right)=\frac{{\mathrm{\ΔD}}^{\′}\left(t\right)}{\mathrm{\Δ}t},$ And the initial value of V ' (t) is set to 0.

3. method according to claim 1, wherein based on the ambient image of being taken by the image acquisition units that this object is installed and orientation vector β (t), filtering is carried out for described displacement D (t) and comprising to obtain Δ D ' (t):

The ambient image of first taking based on described image acquisition units and this object at orientation vector β (t) of time t, by the displacement D during SLAM method interval of delta t computing time _sLAM(t);

Then by Δ D _sLAMt () and Δ D (t) are weighted on average, to obtain Δ D ' (t).

4. instant location and a map structuring SLAM method, wherein calculate the displacement of an object in the circumstances not known of space by method according to claim 1.

5. an equipment that be applied to instant location and map structuring SLAM, that calculate the displacement of an object in the circumstances not known of space, comprising:

Inertial navigation system INS sensor, for repeatedly measuring acceleration α, the orientation vector β of this object based on Fixed Time Interval δ t;

INS processor, for at time t, according to by the data of INS sensor measurement and the instantaneous velocity estimated before, calculate the displacement D (t) of this object during the time interval Δ t from time t-Δ t to time t and this object attitude β (t) at time t, wherein Δ t=n δ t, n ∈ N;

Image acquisition units, is arranged on described object, for the image of the viewing angles surrounding environment of this object;

SLAM application and wave filter, for the ambient image of taking based on described image acquisition units and orientation vector β (t), carry out filtering to obtain Δ D ' (t) for the displacement D (t) calculated by INS processor;

INS acts on behalf of, apply and wave filter for Δ D (t) calculated by described INS processor and β (t) are sent to described SLAM, and be sent to INS processor by by Δ D ' (t) that described SLAM applies and wave filter exports;

Wherein, instantaneous velocity V ' (t) upgraded estimated by described INS processor according to Δ D ' (t) obtained from described SLAM application and wave filter, and calculates for displacement next time.

6. equipment according to claim 5, wherein according to following formulae discovery displacement D (t):

$\mathrm{\Δ}D\left(t\right)=\underset{j=0}{\overset{n-1}{\Σ}}(\underset{i=0}{\overset{j}{\Σ}}{\mathrm{\α}}_i\·\mathrm{\δ}t)\·\mathrm{\δ}t+V(t-\mathrm{\Δ}t)\·\mathrm{\Δ}t,$ Wherein $V^{\′}\left(t\right)=\frac{{\mathrm{\ΔD}}^{\′}\left(t\right)}{\mathrm{\Δ}t},$ And the initial value of V ' (t) is set to 0.

7. equipment according to claim 5, the ambient image that first wherein said SLAM application and wave filter are taken based on described image acquisition units and this object at orientation vector β (t) of time t, by the displacement D during SLAM method interval of delta t computing time _sLAMt (), then by Δ D _sLAMt () and Δ D (t) are weighted on average, to obtain Δ D ' (t).

8. equipment according to claim 5, wherein said inertial navigation system INS sensor comprises gyroscope, magnetometer and accelerometer.

9. instant location and a map structuring SLAM equipment, comprise the equipment calculating the displacement of an object in the circumstances not known of space as claimed in claim 5.