CN104848852A - System and method for locating annular sensing array - Google Patents

Abstract

The invention discloses a system and a method for locating an annular sensing array. The system comprises an annular sensing array module, a sensor signal acquisition module and a processing circuit module, wherein the sensor array of the annular sensing array module is in annular arrangement, a planar location system is arranged on a plane with grids, and a grid signal is detected through detection of the annular sensing array module; the location method is utilized, the processing circuit module is used for acquiring sensor data, the coordinate position and the angle are acquired through data processing, and the data are uploaded to an upper computer through certain one communication protocol; during movement, the location system can obtain the grid displacement and the rotating angle relative to the grids, and the accurate position of the location system can be acquired. According to the system and the method, the position and the attitude can be accurately acquired, no error accumulation is caused, the design is simple, the cost is low, the reliability is high, and the system and the method can be widely applied to the indoor and outdoor location navigation of autonomous mobile equipment.

Description

A kind of positioning system of annular sensor array and method

Art

The invention belongs to field of locating technology, relate to the field of locating technology of mobile robot and AGV etc. particularly.

Background technology

Autonomous mobile robot and, the application of automatically guiding trolley is more and more general, and location navigation guarantees its more mobile key freely worked, and at present, the many location navigation mode of domestic and international investigation and application has following several:

The first navigates in magnetic stripe mode, utilize a row or multi-row Magnetic Sensor to detect the relative position of magnetic stripe thus control or dolly all the time and then magnetic stripe move.Advantage is that cost is low, simplicity of design, and deficiency is that robot or dolly can only follow magnetic stripe to move, and cannot rely on Magnetic Sensor and obtain current location accurately.

The second is located based on ultra broadband; Based on ultra wideband location techniques, there is higher positioning precision, form primarily of receiver, reference label and active tag.Timing receiver receives the signal of transmitted tag, and filters out various noise, positions finally by TDOA time-delay distance finding.There is based on ultra wideband location techniques the advantages such as emissive power is low, penetration power strong, carrierfree, but its cost is too high, and the reasons such as electromagnetic interference influence reliability may be subject to, cannot large-scale application be suitable for.

The third is the indoor positioning based on inertial sensor, and it utilizes gyroscope and accelerometer by obtaining the coordinate angle of robot to the triple integral of acceleration, due to pure in the problem of error accumulation, cannot ensure for a long time accurately location.

4th kind is laser radar location, and laser radar can be good at carrying out indoor positioning, but involves great expense.5th kind is camera location, and camera carries out indoor status algorithm complexity, and reliability is low, and compare and be difficult to ripe utilization, cost is relative still higher.

If can develop one can obtain position and attitude accurately, do not produce deviation accumulation, simplicity of design, cost is low, and the location navigation sensor-based system that reliability is high and corresponding positioning navigation method will be very significant.

Summary of the invention

The object of the invention is to avoid the weak point existing for above-mentioned technology, provide a kind of and realize cost is low, positional accuracy is high Position Fixing Navigation System and method, this positioning system by annular sensor array module, collecting sensor signal module, processing circuit module.This positioning system is positioned in the plane of grid, by detecting the annular sensor array module signal crossing with line, and utilize localization method of the present invention, time mobile, this positioning system just can obtain self displacement relative to grid and the angle of rotation, thus obtains the exact position of positioning system.

Principle of the present invention is as follows: as shown in Figure 1, when positioning system moves, around the annular sensor array module sensor of a week just can checking network ruling whether in its lower section, if annular sensor array module is crossing with mesh lines, then detect that the respective sensor of mesh lines can change.And the displacement of this change and location-plate and angle change and have certain corresponding relation, simultaneously also relevant with the relative position of grid with location-plate, thus by founding mathematical models, we just can find this corresponding relation, and then can obtain the position and attitude of location-plate.

The positioning system of annular sensor array of the present invention comprises: annular sensor array module, collecting sensor signal module, processing circuit module.The sensor array circular array of annular sensor array module can be a ring or many rings, and the shape of ring can be closed rings that are circular or oval or other shapes.This positioning system is positioned in the plane of grid, the signal of grid is detected by detecting annular sensor array module, and utilize localization method of the present invention, time mobile, this positioning system just can obtain self displacement relative to grid and the angle of rotation, thus obtains the exact position of positioning system.Processing circuit module pick-up transducers data, obtain coordinate position and angle by data processing, and data are passed to host computer, for location navigation by certain communication protocol.

The positioning system of annular sensor array can be moved by three degree of freedom in the planes; The motion of X-direction, the motion of Y-direction respectively, the rotation angle θ of location-plate self.

The sensor array circular array of annular sensor array module can be a ring or many rings, and the shape of ring can be closed rings that are circular or oval or other shapes.

Generally be arranged as monocycle sensor array annular to arrange, terrestrial network ruling is square net.The width of the mesh lines of reference when spacing between adjacent sensor is less than location.The diameter of annular is greater than the length of side of the square net line of reference.

The square net of reference during location in above-mentioned, in practice, can be the grid that ready-made every floor forms, also can be the artificial grid marked on the ground, can also be the lattice that ceiling forms.

The positioning system of annular sensor array can also comprise wired or wireless communication module, it carries out the transmitting-receiving of data by wired or wireless mode, it is connected with micro-chip processor, and wireless communication module comprises Wifi wireless module, Zigbee wireless module, bluetooth radio module, 2.4G wireless module, GPRS wireless module.

The positioning system of foregoing two kinds of annular sensor arrays, they all can also comprise application module, and application module is the device that can realize certain application function, as mechanical arm, suction cleaner module.

The present invention also comprises a kind of localization method of positioning system of annular sensor array, and the method comprises the following steps:

S1. coordinate and the angle of each sensor in annular sensor array module is calculated according to the coordinate of the positioning system of annular sensor array and angle gauge; The coordinate in former and later two moment is done the motion vector that difference obtains the positioning system of annular sensor array, a rear moment coordinate and angle are unknown numbers, wait to be solved.

S2. set up matching error model, ask for the matching error of the sensor be positioned on mesh lines and total matching error of all sensors be positioned on mesh lines;

S3. motion vector when total matching error value gets minimum value is tried to achieve by iterative algorithm, and using the displacement of its displacement as location-plate;

So that in conjunction with Kalman filtering algorithm, and data fusion positioning can also be carried out in conjunction with other sensors, as inertial sensor accelerometer gyroscope etc. in step.

The geometric meaning of this algorithm is centered by lamp plate previous moment coordinate, move in a little scope, just can infer when moving to certain position and to detect which the sensor of line is, if sensor line being detected just and current detection to the sensor matching of line, displacement at this moment just uniquely determines, and now matching error E is minimum.

Advantage of the present invention is as follows:

1. to realize cost low for equipment, is beneficial to input daily use.The sensor that positioning system adopts can be photoelectric sensor or Magnetic Sensor, and compare the sensor of the location such as laser, radar, gyroscope, cost reduces greatly, is beneficial to input volume production.

2. the present invention can obtain position and attitude accurately, and do not produce deviation accumulation, simplicity of design, reliability is high, can widespread use autonomous mobile apparatus at the location navigation of indoor and outdoor.

Accompanying drawing explanation

Fig. 1 is the positioning system principle schematic in the embodiment of the present invention; Wherein, Fig. 1 marks corresponding title: 1-location-plate, 2-post exercise location-plate, 3-mesh lines, the grid of reference during 4-location, sensor crossing with mesh lines on 5-location-plate.

Fig. 2 is location-plate coordinate and ground coordinate schematic diagram in the embodiment of the present invention.

Fig. 3 is the location-plate described in embodiment.

Embodiment:

For the localization method of the positioning system making annular sensor array of the present invention is clearly understood, below in conjunction with specific embodiment, the present invention is described in more detail.

The positioning system embodiment of annular sensor array:

A kind of domestic sweeper device people, it comprises: driver part, location-plate, photoelectric sensor, signal acquisition module, microprocessor, wireless communication module, suction cleaner module.

Driver part, it has been come by a tripod robot, mainly according to the mode that controlled in wireless or owner set, carries out the motion of formula line by line in indoor.It can motion in X direction, Y-direction motion, can also rotate around self by rotation angle θ.

Location-plate, as shown in Figure 3, it is annular, its edge it photoelectric sensor is installed, it is fixing on the driving part.

Photoelectric sensor, it adopts phototriode, and quantity is between 20 to 150, and it arranges ringwise on location-plate, the length of side of the square net line of reference when the diameter of annular is greater than location, the width of the square net line of reference when the spacing between adjacent photoelectric sensor is less than location.

Signal acquisition module gathers photo-sensor signal will.

The square net of reference during location in above-mentioned, in practice, selects the grid that ready-made every floor forms.

Wireless communication module, selects Zigbee wireless module.

Suction cleaner module, for completing the absorption to dust and the process to rubbish, it is to be fixed on driver part.

The embodiment of the localization method of the positioning system of annular sensor array:

The method that each step is concrete can be:

The method of S1 is:

Setting up coordinate on ground is x-o-y, and location-plate is set up coordinate system x '-o '-y ', and on lamp plate, i-th photoelectric sensor is A at the middle coordinate of x '-o '-y ' _i' (x _i', y _i'), in x-o-y, coordinate is A _i(x _i, y _i), θ _jfor the angle that the plane that location-plate forms at x, y rotates, the pose of j moment location-plate under ground coordinate is o _j(x _oj, y _oj, θ _j), then

${\mathrm{OA}}_{\mathrm{ij}}=\left|\begin{array}{c}{x}_{\mathrm{ij}}\\ y_{\mathrm{ij}}\end{array}\right|=\left|\begin{array}{c}{x}_{\mathrm{oj}}\\ y_{\mathrm{oj}}\end{array}\right|+\left|\begin{array}{cc}\cos \left({\mathrm{\θ}}_j\right)& -\sin \left({\mathrm{\θ}}_j\right)\\ \sin \left({\mathrm{\θ}}_j\right)& \cos \left({\mathrm{\θ}}_j\right)\end{array}\right|\left|\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\end{array}\right|$

Be carved with during J+1

${\mathrm{OA}}_{\mathrm{ij}+1}=\left|\begin{array}{c}{x}_{\mathrm{ij}+1}\\ y_{\mathrm{ij}+1}\end{array}\right|=\left|\begin{array}{c}{x}_{\mathrm{oj}+1}\\ y_{\mathrm{oj}+1}\end{array}\right|+\left|\begin{array}{cc}\cos \left({\mathrm{\θ}}_{j+1}\right)& -\sin \left({\mathrm{\θ}}_{j+1}\right)\\ \sin \left({\mathrm{\θ}}_{j+1}\right)& \cos \left({\mathrm{\θ}}_{j+1}\right)\end{array}\right|\left|\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\end{array}\right|$

On J moment and j+1 moment lamp plate, the displacement of i-th photoelectric sensor is

${\mathrm{\Δa}}_i=\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\end{array}\right]={\mathrm{OA}}_{\mathrm{ij}+1}-{\mathrm{OA}}_{\mathrm{ij}}=\left[\begin{array}{c}{\mathrm{\Δx}}_{\mathrm{oj}}\\ {\mathrm{\Δy}}_{\mathrm{oj}}\end{array}\right]+\left|\begin{array}{cc}\cos ({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-\cos \left({\mathrm{\θ}}_j\right)& -\sin ({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)+\sin \left({\mathrm{\θ}}_j\right)\\ \sin ({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-\sin \left({\mathrm{\θ}}_j\right)& \cos ({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-\cos \left({\mathrm{\θ}}_j\right)\end{array}\right|\left|\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\end{array}\right|$ 。

The method of S2 is:

Suppose that i-th photoelectric sensor is engraved in above white line when jth+1, then the coordinate of its nearest white line intersection point is

$B_i=({\mathrm{xb}}_i,{\mathrm{yb}}_i)=\left(p\right[\frac{x_{\mathrm{ij}+1}}{p}],p[\frac{y_{\mathrm{ij}+1}}{p}\left]\right),$ Wherein, p is the number of photoelectric sensor,

Can obtain:

$A_i{B}_i=\left[\begin{array}{c}{\mathrm{\Δxb}}_i\\ {\mathrm{\Δyb}}_i\end{array}\right]=\left[\begin{array}{c}{\mathrm{xb}}_i\\ {\mathrm{yb}}_i\end{array}\right]-\left[\begin{array}{c}{x}_{\mathrm{ij}}\\ y_{\mathrm{ij}}\end{array}\right]$

If | Δ xb _i|≤| Δ yb _i| then $R_i=\left[\begin{array}{c}{\mathrm{\Δxb}}_i\\ 0\end{array}\right](R_i=\left[\begin{array}{c}1\\ 0\end{array}\right])$

If | Δ xb _i| > | Δ yb _i| then $R_i=\left[\begin{array}{c}0\\ {\mathrm{\Δyb}}_i\end{array}\right](R_i=\left[\begin{array}{c}0\\ {\mathrm{\Δyb}}_i\end{array}\right])$

Matching error

$e_i=\left\{\begin{array}{c}{\left[\right|\frac{R_i\·{\mathrm{\Δa}}_i}{\left|R_i\right|}-R_i|-\frac{d}{2}]}^2.....|\frac{R_i\·{\mathrm{\Δa}}_i}{\left|R_i\right|}-R_i|>\frac{d}{2}\\ 0.....|\frac{R_i\·{\mathrm{\Δa}}_i}{\left|R_i\right|}-R_i|\≤\frac{d}{2}\end{array}\right.$

The photoelectric sensor dropped on white line has n, then total matching error is: (if i photoelectric sensor is engraved in above white line when jth+1)

Namely can be expressed as:

E _＝Σe _i＝f(Δx _oj,Δy _oj,Δθ _j)

(Δ x when E gets minimum value _oj, Δ y _oj, Δ θ _j) be required by,

Now the coordinate attitude of location-plate is

$\left[\begin{array}{c}{x}_{\mathrm{oij}+1}\\ y_{\mathrm{oij}+1}\\ {\mathrm{\θ}}_{j+1}\end{array}\right]=\left[\begin{array}{c}{x}_{\mathrm{oij}}\\ y_{\mathrm{oij}}\\ {\mathrm{\θ}}_j\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δx}}_{\mathrm{oj}}\\ {\mathrm{\Δy}}_{\mathrm{oj}}\\ {\mathrm{\Δ\θ}}_j\end{array}\right].$

According to above-described embodiment, just the present invention can be realized well.What deserves to be explained is; under prerequisite based on above-mentioned design concept; for solving same technical matters; even if some making on architecture basics disclosed in this invention are without substantial change or polishing; the essence of the technical scheme adopted is still the same with the present invention, therefore it also should in protection scope of the present invention.

Claims (8)

1. a positioning system for annular sensor array, this system comprises: annular sensor array module, collecting sensor signal module, and processing circuit module, is characterized in that:

Annular sensor array module is circular layout in module housing by multiple sensing element can responding to grid, sensing element comprises photoelectric sensor, magnetic detecting element, sensing element is connected with collecting sensor signal module, annular arrangment of sensors array comprises a ring or many rings, and the shape of ring comprises circle, ellipse, closed ring; The width of the mesh lines of reference when spacing between adjacent sensing element is less than location, the diameter of annular is greater than the length of side of the square net line of reference;

Collecting sensor signal module is connected with processing circuit module, and it gathers the Grid Signal that annular sensor array module senses, it can be integrated on sensing element, also can be independent module.

2. the positioning system of annular sensor array according to claim 1, also comprise wired and wireless communication module, it is characterized in that, it carries out the transmitting-receiving of data by wired or wireless mode, it is connected with processing circuit module, and wireless communication module comprises Wifi wireless module, Zigbee wireless module, bluetooth radio module, 2.4G wireless module, GPRS wireless module.

3. the positioning system of annular sensor array according to claim 1 and 2, also comprises application module, and it is characterized in that described application module is the device that can realize certain application function, as mechanical arm, suction cleaner module, it is connected with processing circuit module.

4. a localization method for the positioning system of annular sensor array, the method comprising the steps of:

S1. coordinate and the angle of each sensor in annular sensor array module is calculated according to the coordinate of the positioning system of annular sensor array and angle gauge; The coordinate in former and later two moment is done the motion vector that difference obtains the positioning system of annular sensor array, a rear moment coordinate and angle are unknown numbers, wait to be solved;

S2. set up matching error model, ask for the matching error of the sensor be positioned on mesh lines and total matching error of all sensors be positioned on mesh lines;

S3. motion vector when total matching error value gets minimum value is tried to achieve by iterative algorithm, and using the displacement of its displacement as location-plate.

5. the positioning system localization method of annular sensor array according to claim 4, also comprises step S4, that is: the displacement of loop mapping plate be added with the initial position of location-plate and obtain the new position of location-plate.

6. the positioning system localization method of the annular sensor array according to claim 4 or 5, is characterized in that, also combine Kalman filtering algorithm in part steps.

7. the positioning system localization method of annular sensor array according to claim 4, it is characterized in that: the method for 1 is: setting up coordinate on ground is x-o-y, location-plate is set up coordinate system x '-o '-y ', and on lamp plate, i-th photoelectric sensor is A ' at the middle coordinate of x '-o '-y ' _i(x ' _i, y ' _i), in x-o-y, coordinate is A _i(x _i, y _i), θ _jfor the angle that the plane that location-plate forms at x, y rotates, the pose of j moment location-plate under ground coordinate is o _j(x _oj, y _oj, θ _j), then:

${\mathrm{OA}}_{\mathrm{ij}}=\left|\begin{array}{c}{x}_{\mathrm{ij}}\\ y_{\mathrm{ij}}\end{array}\right|=\left|\begin{array}{c}{x}_{\mathrm{oj}}\\ y_{\mathrm{oj}}\end{array}\right|+\left|\begin{array}{cc}\cos \left({\mathrm{\θ}}_j\right)& -\sin \left({\mathrm{\θ}}_j\right)\\ \sin \left({\mathrm{\θ}}_j\right)& \cos \left({\mathrm{\θ}}_j\right)\end{array}\right|\left|\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\end{array}\right|$

Positioning system has when moving to J+1 moment:

${\mathrm{OA}}_{\mathrm{ij}+1}=\left|\begin{array}{c}{x}_{\mathrm{ij}+1}\\ y_{\mathrm{ij}+1}\end{array}\right|=\left|\begin{array}{c}{x}_{\mathrm{oj}+1}\\ y_{\mathrm{oj}+1}\end{array}\right|+\left|\begin{array}{cc}\cos \left({\mathrm{\θ}}_{j+1}\right)& -\sin \left({\mathrm{\θ}}_{j+1}\right)\\ \sin \left({\mathrm{\θ}}_{j+1}\right)& \cos \left({\mathrm{\θ}}_{j+1}\right)\end{array}\right|\left|\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\end{array}\right|$

On J moment and j+1 moment lamp plate, the displacement of i-th photoelectric sensor is

${\mathrm{\Δa}}_i=\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\end{array}\right]={\mathrm{OA}}_{\mathrm{ij}+1}-{\mathrm{OA}}_{\mathrm{ij}}=\left[\begin{array}{c}{\mathrm{\Δx}}_{\mathrm{oj}}\\ {\mathrm{\Δy}}_{\mathrm{oj}}\end{array}\right]\left|\begin{array}{cc}\cos ({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-\cos \left({\mathrm{\θ}}_j\right)& -\sin ({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)+\sin \left({\mathrm{\θ}}_j\right)\\ \sin ({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-\sin \left({\mathrm{\θ}}_j\right)& \cos ({\mathrm{\θ}}_j+{\mathrm{\Δ\θ}}_j)-\cos \left({\mathrm{\θ}}_j\right)\end{array}\right|\left|\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\end{array}\right|.$

8. the positioning system localization method of annular sensor array according to claim 5, is characterized in that: the method for S2 is: suppose that i-th photoelectric sensor is engraved in above white line when jth+1, then the coordinate of its nearest grid intersection point is:

$B_i({\mathrm{xb}}_i,{\mathrm{yb}}_i)=\left(d\right[\frac{x_{\mathrm{ij}+1}}{d}],d[\frac{y_{\mathrm{ij}+1}}{d}\left]\right),$ Wherein, d is the length of side of square net,

Can obtain:

$A_i{B}_i=\left[\begin{array}{c}{\mathrm{\Δxb}}_i\\ {\mathrm{\Δyb}}_i\end{array}\right]=\left[\begin{array}{c}{\mathrm{xb}}_i\\ {\mathrm{yb}}_i\end{array}\right]-\left[\begin{array}{c}{x}_{\mathrm{ij}}\\ y_{\mathrm{ij}}\end{array}\right]$

If | Δ xb _i|≤| Δ yb _i| then $R_i=\left[\begin{array}{c}{\mathrm{\Δxb}}_i\\ 0\end{array}\right](R_i=\left[\begin{array}{c}1\\ 0\end{array}\right])$

If | Δ xb _i| > | Δ yb _i| then $R_i=\left[\begin{array}{c}0\\ {\mathrm{\Δyb}}_i\end{array}\right](R_i=\left[\begin{array}{c}0\\ {\mathrm{\Δyb}}_i\end{array}\right])$

Matching error

$e_i=\left\{\begin{array}{c}{\left[\right|\frac{R_i\·{\mathrm{\Δa}}_i}{\left|R_i\right|}-R_i|-\frac{d}{2}]}^2.....|\frac{R_i\·{\mathrm{\Δa}}_i}{\left|R_i\right|}-R_i|>\frac{d}{2}\\ 0.....|\frac{R_i\·{\mathrm{\Δa}}_i}{\left|R_i\right|}-R_i|\≤\frac{d}{2}\end{array}\right.$

The sensor dropped on mesh lines has n, then total matching error is: (if i sensor is engraved in above mesh lines when jth+1)

Namely can be expressed as:

E＝Σe _i＝f(Δx _oj,Δy _oj,Δθ _j)

(Δ x when E gets minimum value _oj, Δ y _oj, Δ θ _j) be required by,

Now the coordinate attitude of location-plate is

$\left[\begin{array}{c}{x}_{\mathrm{oij}+1}\\ y_{\mathrm{oij}+1}\\ {\mathrm{\θ}}_{j+1}\end{array}\right]=\left[\begin{array}{c}{x}_{\mathrm{oij}}\\ y_{\mathrm{oij}}\\ {\mathrm{\θ}}_j\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δx}}_{\mathrm{oj}}\\ {\mathrm{\Δy}}_{\mathrm{oj}}\\ {\mathrm{\Δ\θ}}_j\end{array}\right]$

Positioning system can be obtained in real time relative to the coordinate of grid and angle by the continuous recursion of algorithm.