Indoor machine people's position fixing and navigation system and method
The present invention relates to indoor machine people technical field, particularly relate to a kind of indoor machine people's location and navigational system and a kind of indoor machine people's location and air navigation aid.
The location and navigation technology of indoor mobile robot is a primary study focus in indoor mobile robot field.In correlation technique research, the location is that the mobile robot realizes the intelligent and autonomous fully key that moves with airmanship.To the research of location with airmanship, its purpose is to make the mobile robot under unmanned condition of intervening, to make its free routing along planning move and accomplish the task of appointment.
The location navigation of robot can adopt following method to realize usually.
1) based on road sign, residing position and target location are learnt through identification road sign in the image of taking by robot.In this mode,, disturbed also artificial easily the damage easily on every side by passing other machines people because road sign usually is positioned on the ground.In addition,, do not possess error correction property though simple road sign Flame Image Process is simple, though and complicated road sign has partial data error correction property, pattern is too complicated, computational complexity is high, and is also higher to the camera requirement, real-time is poor.At last, based on the method for road sign, can only let robot along the path movement of having laid, the path changing difficulty can not realize autonomy-oriented truly, is not easy to expand to the robot indoor positioning navigation under the overall situation.
2) adopting camera directly to take robot positions.In this method,, and under uncomplicated background, want to realize to detect accurately usually comparatively trouble to the high requirement that is equipped with of camera.
3) adopt laser or supersonic array, complex structure, cost are too high relatively.
4) read the electrode code-disc, motor is had relatively high expectations, bigger cumulative errors is arranged usually, and owing to be not the location of absolute position, at home environment and impracticable.
Summary of the invention
Based on this, be necessary to provide a kind of accurate positioning, path planning is flexible and lower-cost indoor machine people's position fixing and navigation system.
A kind of indoor machine people's location and air navigation aid also are provided in addition.
A kind of indoor machine people's location and navigational system; Comprise the robot and fixing location navigation device that can move freely; Said robot comprises: the signal transmitter; Be used for forming signal to fixing surface, said signal is corresponding one by one with robot residing position in its moving region in the position on said fixing surface; Wireless module is used to receive navigation signal; Motion-control module is used for generating the corresponding driving control signal according to the navigation signal that receives; Motion is used for according to said drive control signal operation to drive robot movement; Said location navigation device comprises: the signal detecting device is used to detect said signal, and confirms the coordinate of said signal; Positioning navigation module is used for the position of coordinate positioning machine people in its moving region according to said signal, and plans and revise mobile route according to the position of target location and robot, generates navigation signal; Wireless module is used to send said navigation signal.
Among embodiment, said signal transmitter is an infrared laser therein, and said signal detecting device is a thermal camera.
Among embodiment, the cursor of said infrared laser emission is designated as grating, hot spot or bar code therein.
Among embodiment, the quantity of said signal is more than two therein.
Among embodiment, the moving region of said robot is indoor ground therein, and said fixing surface is the ceiling relative with ground.
Therein among embodiment, said signal transmitter with signal from ground to the ceiling perpendicular projection.
Among embodiment, said motion comprises motor and by motor-driven wheel, said motor drives said wheel according to drive control signal with corresponding rotating speeds therein.
Among embodiment, said motion structure also comprises deflecting roller therein.
Among embodiment, said robot also comprises the ultrasonic detector that is used to survey the barrier on the motion path therein.
A kind of indoor machine people's location and air navigation aid; Be used for comprising the position fixing and navigation system of robot that can move freely and the location navigation device of fixing; By the location navigation device robot is navigated; Comprise: robot forms signal on fixing surface, and said signal is corresponding one by one with robot residing position in its moving region in the position on said fixing surface; The location navigation device detects said signal; And the coordinate of definite said signal; And according to the position of coordinate positioning machine people in its moving region of said signal, and according to the position planning of target location and robot with revise mobile route, generate and send navigation signal; Robot receives and moves to the target location according to said navigation signal.
Above-mentioned indoor machine people's location and air navigation aid and system, because the signal that adopts robot to launch positions and navigates, it can avoid adopting the variety of issue of landmark identification, for example mark is fragile, the location is inaccurate, path planning is dumb etc.In addition, signal transmitter and signal detecting device that the location is adopted can adopt low-cost device, make that the cost of total system is lower.
Description of drawings
Fig. 1 is the indoor machine people's of an embodiment location and a navigational system module map;
Fig. 2 is to be the plan structure synoptic diagram of robot;
Fig. 3 is to be the backsight structural representation of robot;
Fig. 4 is the signal emission and detects schematic diagram;
Change in location synoptic diagram when Fig. 5 is the interframe changes in coordinates;
Fig. 6 is that navigational parameter is analyzed synoptic diagram.
As shown in Figure 1, be the indoor machine people's of an embodiment location and navigational system module map.This location and navigational system 10 comprise the robot 100 and fixing location navigation device 200 that can move freely.Robot 100 moves freely in its moving region, and 200 pairs of robots 100 of location navigation device position and navigate.
Robot 100 comprises signal transmitter 101, wireless module 102, motion-control module 103 and motion 104.
Signal transmitter 101 is used for forming signal to fixing surface 300 emission light, during robot movement, drives signal transmitter 101 and moves, and signal also moves in the position on fixing surface 300 thereupon.Signal is corresponding one by one with robot 100 residing position in its moving region in the position on fixing surface 300, thereby through to signal definite in the position on the surface of fixing 300, can confirm the position of robot 100 in the moving region.Robot in indoor sport; Its moving region is exactly the ground in the room; Therefore ceiling that will be relative with ground is as the surface of signal projection, and can realize that to the ceiling perpendicular projection robot present position is corresponding one by one with the position of signal from ground very simply.Preferably, signal transmitter 101 is an infrared laser.The infrared light of infrared laser emission forms signal and is used for position probing, can not receive the restriction of lighting condition.In other embodiments, signal transmitter 101 can also be visible light emitter or other forms of invisible light transmitter, as long as the signal of its emission can be detected by corresponding signal detecting device.
Please refer to Fig. 2, is the plan structure synoptic diagram of robot.The quantity of the infrared laser of present embodiment is 2, separated by a distancely is arranged on robot 100 tops, and light direction is perpendicular to the ground.Two infrared lasers can form 2 signals on ceiling.The light pattern that the signal of infrared laser emission can be easy to discern for grating, hot spot or bar code etc.The quantity of signal also can be one in other embodiments, perhaps 3 and more than.
Wireless module 102 is used to receive navigation signal.Wireless module 102 is used to receive the navigation signal from location navigation device 200.Wireless module 102 can adopt the mixed-media network modules mixed-media that meets the various wireless communication agreement, for example WIFI module, infrared communication module, bluetooth communication etc.Preferably, wireless module 102 adopts the wireless serial communication module.
Motion-control module 103 is used for generating the corresponding driving control signal according to the navigation signal that receives.Navigation signal is the routing information of planning, and for example steering angle, travel distance etc. are provided by location navigation device 200.Navigation signal is formed for the drive control signal of drive movement mechanism 104, for example motor drive signal after motion-control module 103 is handled.This moment, motion-control module 103 can be selected single-chip microcomputer for use, utilized in advance program stored that routing information is handled.Navigation information also can be to obtain drive control data after having handled through location navigation device 200, and motion-control module 103 is directly used this drive control data drive movement mechanism.This moment, motion-control module 103 can adopt small-sized special-purpose drive controlling plate, to reduce the volume and weight of robot.
Motion 104 is used for moving to drive robot 100 according to said drive control signal operation.In conjunction with Fig. 1 and Fig. 3, motion 104 comprises motor 141 and by motor 141 wheel driven 142.This moment, drive control signal was exactly a motor control signal, specifically was the rotating speed of motor signal, and motor then turns round with corresponding rotating speeds according to this tach signal, with drive wheels 142.In other embodiments, motion structure 104 can also be other forms, for example motor-driven caterpillar tracks.As shown in Figure 3, two pairs of motors 141 and wheel 142 about motion 104 comprises.To about two wheels drive with different speed respectively, can let robot 100 turn to.
Further, motion structure 104 also comprises deflecting roller 143.Deflecting roller 143 is used for auxiliary robot 100 and turns to.
Further, robot 100 also comprises the ultrasonic detector 105 that is used to survey the barrier on the motion path.Ultrasonic detector 105 is constantly launched ultrasound wave on direct of travel; And detection of echoes signal; Time through the calculating echoed signal is returned judges whether the place ahead has barrier, and result of determination is sent to motion-control module 103, and 103 controlled motion mechanisms 104 turn to around barrier by motion-control module.
Location navigation device 200 comprises signal detecting device 201, positioning navigation module 202 and wireless module 203.
Signal detecting device 201 is used to detect said signal, and confirms the coordinate of said signal.Signal detecting device 201 is preferably thermal camera, in order to detect aforesaid infrared light mark.Thermal camera adopts CMOS wide-angle imaging head to add that infrared filter constitutes, and camera lens is carried out distortion correction, makes photographic images point position and real space point position (being the coordinate position on the ceiling) be linear corresponding relation.。When utilizing ceiling as the signal projection surface, location navigation device 200 fixes on the ground, and thermal camera is taken the video image of whole ceiling (perhaps covering most of ceiling of robot zone of action).When aforesaid signal transmitter 101 adopted other forms of light source, signal detecting device 201 also taked to detect the video camera or the other forms of vision facilities of this light source adaptably.
Positioning navigation module 202 is used for the position of coordinate positioning machine people in its moving region according to said signal, and plans and revise mobile route according to the position of target location and robot, generates navigation signal.The video flowing of positioning navigation module 202 thermal cameras picked-ups is as input, the position of signal in the analysis video frame.And, confirm the current direction of motion of robot, speed and rotational angular etc. according to analysis to successive frame.Further, based target position and robot current location planning and correction move to the path of target location.
Wireless module 203 is used for this navigation signal is sent to robot 100.
Therefore, positioning navigation module 202 comprises to the location of robot 100 with to the navigation of robot 100.
With robot architecture shown in Figure 2 is example, explains that localization method is following.
As shown in Figure 4, when mobile, emitted laser will form two flares at ceiling indoor in robot 100.Infrared camera photographs the ceiling picture, cuts apart through gray threshold is set, and extracts the position of two flares.The image coordinate of two flares is designated as (x 1, y 1), (x 2, y 2), according to projection formula:
Q=M -1Q, wherein, q = x y w , M = f x 0 c x 0 f y c y 0 0 1 , Q = X Y Z
Obtain the physical location (X of hot spot at ceiling 1, Y 1), (X 2, Y 2).In the top formula, the coordinate of q presentation graphs picture point, Q representation space point coordinate, M are the camera parameter matrix.
Here, because the infrared laser in the robot is vertically installed, hot spot is directly corresponding in indoor position at the physical location and the dolly of ceiling.Robot is at indoor real coordinate position (X, Y) available (X 1, Y 1), (X 2, Y 2) mid point represent:
( X , Y ) = ( X 1 + X 2 2 , Y 1 + Y 2 2 )
Be (X ＇, Y ＇) if the previous frame image detection obtains robot at indoor real coordinate position, then the travel direction angle α between two frames of robot is:
α = tan - 1 Y - Y ′ X - X ′
Because generating laser is that symmetry is installed on the robot left and right sides, and is as shown in Figure 5, the actual motion direction of robot and (X 1, Y 1), (X 2, Y 2) the connecting line direction vertical, for:
θ = Tan - 1 | X 2 - X 1 Y 2 - Y 1 | Perhaps
According to the continuity that robot moves between two frames, the actual direction of motion angle θ of robot current time gets and α angle that direction less than 90 °.
The speed V of robot current time motion and the angular velocity omega of rotation are approximately respectively:
V = 1 t × ( X - X ′ ) 2 + ( Y - Y ′ ) 2
ω = 1 t × ( θ - θ ′ )
Wherein, t is the mistiming of two frames, and θ ＇ is the detected actual direction of previous frame angle.
With robot architecture shown in Figure 2 is example, explains that air navigation aid is following.
As shown in Figure 6, the hypothetical target position coordinates is (X 0, Y 0), current direction of motion θ of robot and angle e to the direction β of target location are:
e = θ - β = θ - tan - 1 Y 0 - Y X 0 - X
Robot adopts following flow process avoiding barrier, and arrives the target location; When robot when move the target location, whether ultrasonic detector detects the place ahead has barrier, if having, moves along the barrier edge, up to all is clear ahead.
Wherein, when move control robot head for target position, adopting following control method, is controlled target to reduce drift angle e and to keep certain movement speed, by pid control algorithm, obtains the controlled target speed V of the two-wheeled of robot LAnd V R:
V L＝V 0+k×w
V R＝V 0-k×w
Wherein, P, D, I are respectively ratio, differential, integration control parameter, can regulate according to experience; W representes controlled quentity controlled variable; K is a conversion coefficient, by decisions such as the wheelspan of wheel, wheel radiuses; V 0Be the datum velocity of robot, at position far away, distance objective position, V 0Can be bigger, at nearer position, distance objective position, V 0Can suitably reduce.
The above embodiment has only expressed several kinds of embodiments of the present invention, and it describes comparatively concrete and detailed, but can not therefore be interpreted as the restriction to claim of the present invention.Should be pointed out that for the person of ordinary skill of the art under the prerequisite that does not break away from the present invention's design, can also make some distortion and improvement, these all belong to protection scope of the present invention.Therefore, the protection domain of patent of the present invention should be as the criterion with accompanying claims.