Summary of the invention
A technical matters to be solved by this invention is the defective that remedies above-mentioned prior art, and a kind of sweeping robot that adopts the smart mobile phone navigation is provided.
Another technical matters to be solved by this invention is the defective that remedies above-mentioned prior art, and a kind of navigation cleaning method that adopts the sweeping robot of smart mobile phone navigation is provided.
The sweeping robot technical matters of employing smart mobile phone navigation of the present invention is solved by the following technical programs.
The sweeping robot of this employing smart mobile phone navigation, comprise the sweeping robot module, described sweeping robot module comprises the second bluetooth communication, robot control submodule, keep away barrier sensor and road wheel control module, the described barrier sensor of keeping away comprises for the obstacle on every side of collection sweeping robot or the infrared wall inspection sensor of wall, be used for to gather sweeping robot around ground environment examine sensor infraredly, be used for gathering the virtual wall detecting sensor of restrictive signal on every side of sweeping robot, and fill detecting sensor for the seat that recharges a signal around the collection sweeping robot.
Described the second bluetooth communication is used for externally transmitting and receive data.
Described robot control submodule comprises the control chip that has embedded predetermined robot path planning's program, robotary data acquisition program and robot motion's control program, the control sweeping robot carries out work by predetermined path, and changes the motion state of sweeping robot according to the outer signals that receives.
Described road wheel control module comprises two driving wheels and a universal wheel, is used for sweeping robot and moves in the perform region.
The characteristics of the sweeping robot of this employing smart mobile phone navigation are:
Be provided with the smart mobile phone module at the sweeping robot body upper, described smart mobile phone module comprises smart mobile phone camera, image processing submodule, camera calibration submodule, extended Kalman filter-locate and map building (Extended Kalman Filter-Simultaneous Localization And Map Building synchronously, initialism is EKF-SLAM) submodule, map safeguard and path planning submodule and the first bluetooth communication, and sensor submodule, sensor data acquisition submodule.Described image is processed submodule, the camera calibration submodule, EKF-SLAM submodule and map maintenance and path planning submodule pass through respectively application programming interface (Application Programming Interface separately, initialism is API) communicate, to realize in the sweeping robot walking process, utilize the information of smart mobile phone camera and sweeping robot internal sensor, by based on the EKF-SLAM submodule, carry out the navigator fix computing, and according to the data of the sensor submodule collection of leading positioning result and described sweeping robot module or the information that provides, locate accurately, creation environment map and set up the indoor environment barrier and cover map, automatically the sweep-out pattern of planning sweeping robot is controlled next step motion of sweeping robot road wheel.
Described smart mobile phone camera is used for obtaining image information from the external world.
Described sensor data acquisition submodule comprises for the analog signal conversion with the sensor collection being analog to digital conversion (Analog To Digital Converter, initialism the is A/D) module of digital signal.
Described image is processed submodule and is used for the image that obtains from the external world is carried out feature extraction and exports correlated results.
Described camera calibration submodule is set up mapping relations for the image that will collect and the object of three dimensions.
The image information that described EKF-SLAM submodule is caught according to described smart mobile phone camera and the sweeping robot moving parameter information of described inertial sensor collection position, the creation environment map is also set up indoor barrier according to described sweeping robot ambient condition information of keeping away the collection of barrier sensor and is covered map.
Described map is safeguarded and the sweep-out pattern of path planning submodule according to the cartographic information planning sweeping robot of described EKF-SLAM submodule acquisition, and is passed through the action of the first bluetooth communication, the second bluetooth communication control sweeping robot.
Described the second bluetooth communication forms identically with described the first bluetooth communication in the described smart mobile phone module, carries out duplex communication by bluetooth, and utilizes the action of Bluetooth communication control sweeping robot.
The sweeping robot technical matters of employing smart mobile phone navigation of the present invention is solved by following further technical scheme.
Described sensor submodule comprises inertial sensor: gyroscope and accelerometer, described gyroscope are used for gathering the sweeping robot kinematic parameter: angular velocity, described accelerometer are used for gathering the sweeping robot kinematic parameter: linear acceleration.
The navigation cleaning method technical matters of the sweeping robot of employing smart mobile phone navigation of the present invention is solved by the following technical programs.
The characteristics of the navigation cleaning method of the sweeping robot of this employing smart mobile phone navigation are:
Adopt the sweeping robot of smart mobile phone navigation to work in the navigation cleaning modes based on default calibrating template, described default calibrating template is that pattern is chequered with black and white tessellated calibrating template, and following steps are arranged successively:
1) adopt described calibrating template to demarcate smart mobile phone camera intrinsic parameter;
2) setting up Bluetooth communication connects;
3) start the EKF-SLAM submodule, accurately locate, create the three-dimensional environment map;
4) set up the indoor environment barrier and cover map;
5) automatically plan the sweep-out pattern of sweeping robot;
6) next step motion of control sweeping robot road wheel.
The navigation cleaning method technical matters of the sweeping robot of employing smart mobile phone navigation of the present invention is solved by following further technical scheme.
Described step 1) demarcate smart mobile phone camera intrinsic parameter, comprise step by step following:
11) the smart mobile phone camera is aimed at described calibrating template;
12) start the camera calibration submodule, guaranteeing that described calibrating template all is arranged under the prerequisite in the camera image visual field, with both direction at least described calibrating template is taken pictures, gather the scaling board image;
13) the camera calibration submodule is set up corresponding relation with the point set that detects in the scaling board image that collects and the three-dimensional point set of actual scaling board, adopts the plane reference algorithm automatically to calculate Intrinsic Matrix (fu, fv that the smart mobile phone camera is optimized, u0, v0, k1, k2, p1, p2), and be stored in the smart mobile phone Intrinsic Matrix (fu, fv that described smart mobile phone camera is optimized, u0, v0, k1, k2, p1, p2) wherein:
Fu: horizontal focal length;
Fv: vertical focal length;
U0: principal point horizontal ordinate;
V0: principal point ordinate;
K1: camera lens is a distortion parameter radially;
K2: camera lens radial second distortion parameter;
P1: the tangential distortion parameter of camera lens;
P2: the tangential secondary distortion parameter of camera lens;
In system's operational process, same smart mobile phone needn't be demarcated again.
Described step 2) set up Bluetooth communication and connect, comprise step by step following:
21) start described the first bluetooth communication in the smart mobile phone module, and set up Bluetooth wireless communication after the second bluetooth communication on the sweeping robot fuselage is carried out Bluetooth pairing and be connected;
22) instruction that the control sweeping robot that is sent by smart mobile phone by sweeping robot identification moves;
If command instruction is identified as query statement, the described robot control submodule that has embedded the control chip of predetermined robotary data acquisition program is just encoded each state constantly of sweeping robot internal sensor, and joining day stamp, the return data status identifier, form the sensor states vector, again the sensor states vector is sent to smart mobile phone by the first bluetooth communication, the second bluetooth communication;
If instruction is identified as steering order, the described robot control submodule that has embedded the control chip of predetermined robot motion's control program is decoded to the steering order that the smart mobile phone that receives sends, and be converted to the operation control command of sweeping robot, the control sweeping robot carries out work by predetermined path, and change the motion state of sweeping robot according to the outer signals that receives, comprise forward-reverse, rotation specified angle;
If control successfully, then send the control link order by the first bluetooth communication, the second bluetooth communication to smart mobile phone, comprise in the instruction whether operating successfully identifying and control the link order sign;
If control is success, then postponing to resend steering order behind the setting-up time by smart mobile phone, repeat again step by step 22), described delay setting-up time is 2 seconds~4 seconds.
Described step 3) start the EKF-SLAM submodule, accurately locate, create the three-dimensional environment map, comprise the smart mobile phone camera is kept straight up, be fixed in the sweeping robot top, and step by step following:
31) EKF-SLAM submodule initialization
Judge whether to exist the map that has created;
Create map if exist, then changed EKF-SLAM algorithm reorientation operation over to, adopted the reorientation algorithm to obtain current smart mobile phone camera in the position and the attitude that create in the map, comprised following substep:
311) angle point that extracts in the current camera image and the map feature that has created in the map are carried out global registration, obtain the preliminary corresponding relation of two dimensional character and three-dimensional point;
312) utilization is based on direct linear transformation's's (Direct Linear Transformation, initialism are DLT) perspective N point algorithm (Perspective N Points, initialism are PNP), cooperate random sampling consistance (Random Sample Consensus, initialism are RANSAC) algorithm, according to the camera intrinsic parameter (fu that has demarcated, fv, u0, v0, k1, k2, p1, p2), position and the attitude of the smart mobile phone camera that acquisition is optimum;
If successfully obtain position and the attitude of optimum smart mobile phone camera, then carry out step by step 33), a step EKF-SLAM algorithm iteration;
If successfully do not obtain position and the attitude of optimum smart mobile phone camera, then carry out step by step 32);
If there is no created map, then directly carried out step by step 32);
32) map initialization
The bluetooth of utilizing smart mobile phone and sweeping robot to set up is connected, the distance that control sweeping robot rectilinear motion is set, to two two field pictures of motion front camera and rear camera two diverse locations acquisitions, carry out extraction and the coupling of the image angle point of the overall situation, utilize matching result and the ceiling environment hypothesis of plane mechanism, calculate relative position and the attitude R of two two field pictures and camera
0, t
0, R wherein
0That one 3 * 3 rotation matrix represents the three-dimensional rotation between initial two two field pictures and the camera, t
0That one 3 * 1 column vector represents the D translation between initial two two field pictures and the camera, and utilize trigonometric ratio to generate initial three-dimensional map feature, utilize bundle adjustment (Bundle Adjustment, initialism is BA) algorithm is optimized, and obtains the map feature point of final initial three-dimensional map and position and the attitude of the sweeping robot in two two field pictures;
The distance that described sweeping robot rectilinear motion is set is 25cm~35cm;
33) inertial sensor that reads in the sensor submodule of sweeping robot gathers the kinematic parameter of sweeping robot as the controlled quentity controlled variable of EKF-SLAM submodule, carry out the one-step prediction of system state, described system state comprises robot location, attitude, and the position of all three-dimensional map unique points;
34) video image that arrives for the smart mobile phone camera collection utilizes step by step 33) the sweeping robot state of prediction, obtain the characteristics of image estimation range, carry out that Corner Feature extracts and the coupling of map feature;
Corner Feature extracts and the coupling of map feature if successfully carry out, and then carries out step by step 35);
Corner Feature extracts and the coupling of map feature if successfully do not carry out, and the data correlation failure namely occurs, repeats step by step 31 again) reorientation of carrying out sweeping robot operates;
The coupling of described map feature is to adopt grey level histogram least mean-square error (Minimum Mean Squared Error, initialism the is MMSE) algorithm based on principal direction to mate;
35) will be step by step 34) carry out that Corner Feature extracts and the result of the coupling of map feature in conjunction with step by step 13) Intrinsic Matrix (fu, fv, the u0 of the smart mobile phone camera optimization that automatically calculates, v0, k1, k2, p1, p2), obtain observation and upgrade matrix, carry out EKF and upgrade, upgrade step by step 33) the sweeping robot state of prediction, upgrade simultaneously the map feature point, obtain current position and the attitude of sweeping robot;
36) judge whether to exist newly-built map feature need to carry out Feature Conversion;
If the standard deviation sigma of the ρ reciprocal of the unique point degree of depth under a certain camera coordinate system
ρMore than or equal to the threshold tau of setting, then enter step by step 37);
If the standard deviation sigma of the ρ reciprocal of the unique point degree of depth under a certain camera coordinate system
ρLess than the threshold tau of setting, need to carry out Feature Conversion, map feature is converted to the three-dimensional world coordinate by anti-depth representing represents, the selection of described threshold tau will guarantee that ρ drops on τ and σ with 99% probability
ρIn the general curved surface such as grade of determining;
Conversion formula is as follows:
Wherein:
The tri-vector of camera position;
x
W: the x axial coordinate parameter in the tri-vector of camera position;
y
W: the y axial coordinate parameter in the tri-vector of camera position;
z
W: the z axial coordinate parameter in the tri-vector of camera position;
The tri-vector of camera position during the map feature initialization;
r
x: the x axial coordinate parameter during the map feature initialization in the tri-vector of camera position;
r
y: the y axial coordinate parameter during the map feature initialization in the tri-vector of camera position;
r
z: the z axial coordinate parameter during the map feature initialization in the tri-vector of camera position;
θ: the deflection of the initial depth direction of map feature point;
The angle of pitch of the initial depth direction of map feature point;
ρ: the inverse of the unique point degree of depth under the camera coordinate system;
37) whether judge current visible features point less than setting range according to the PNP algorithm, described setting range is 6~15 visible points;
If there are 6~15 unique points of known three-dimensional position and correspondence image position, then the position of smart mobile phone camera and attitude have unique solution;
If current visible features point is less than setting range, newly-increased unique point then carries out step by step 38);
If current visible features point, is then finished a step EKF-SLAM algorithm iteration more than or equal to setting range, carry out step by step 39):
38) newly-built unique point
For current camera image, at the image-region without correct coupling map feature, the detected image angle point, and carry out the initialization of anti-depth map feature;
Described detected image angle point adopts Fast Corner Detection (Features from Accelerated Segment Test, abbreviation FAST) algorithm carries out, the description of Corner Feature adopts
radius 10 pixels to describe with the polar coordinates gray average histogram of principal direction, and the anti-degree of depth initiation parameter of map feature is 6 dimensional vectors:
Wherein:
[r
x, r
y, r
z]
T: the tri-vector of camera current location;
r
x: the x axial coordinate parameter in the tri-vector of camera current location;
r
y: the y axial coordinate parameter in the tri-vector of camera current location;
r
z: the z axial coordinate parameter in the tri-vector of camera current location;
θ: the deflection of the initial depth direction of map feature point;
The angle of pitch of the initial depth direction of map feature point;
ρ is the inverse of newly-built map feature estimation of Depth value;
39) a step EKF-SLAM algorithm iteration is finished, and obtains accurate positioning result and the three-dimensional environment map of robot, judges whether to stop cleaning operation;
If the termination cleaning operation then finishes the navigation operation;
If do not stop cleaning operation, then enter step 4).
Described step 4) sets up the indoor environment barrier and cover map, that map is safeguarded and the path planning submodule utilizes bluetooth communication to read the sensor information of current sweeping robot, with current sweeping robot positioning result, and the ambient condition information of keeping away the sweeping robot of barrier sensor collection is sent to map maintenance and path planning submodule, renewal floor level barrier covering map.
Described step 5) automatically planning the sweep-out pattern of sweeping robot, is to utilize step 4) the floor level barrier that upgrades covers map, according to sweeping robot course and overlay area, next step course of planning sweeping robot.
Described step 6) next step motion of control sweeping robot road wheel, to the smart mobile phone sending controling instruction by the first bluetooth communication, the second bluetooth communication, the control sweeping robot is to clear and not purging zone motion, below repeats step by step 33) and subsequent step.
The present invention's beneficial effect compared with prior art is:
The present invention has avoided directly installing navigator fix and sensor hardware device additional at sweeping robot, utilize smart mobile phone inside camera, comprise that accelerometer and gyrostatic inertial sensor carry out the navigator fix of sweeping robot, obtain the real-time precise position information of sweeping robot and the three-dimensional map of surrounding environment, and the floor level barrier covers map, and each step action by Bluetooth communication control sweeping robot, have and implement simply, hardware is changed little, accurate positioning, the cleaning area coverage is large, the advantage that sweeping efficiency is high.
Embodiment
Below in conjunction with embodiment and contrast accompanying drawing the present invention will be described.
A kind of sweeping robot of the employing smart mobile phone navigation shown in Fig. 1~8, comprise sweeping robot module 2, sweeping robot module 2 comprises the second bluetooth communication 11, robot control submodule 12, keep away barrier sensor and road wheel control module 17, wherein keeping away the barrier sensor comprises for the obstacle on every side of collection sweeping robot or the infrared wall inspection sensor 13 of wall, be used for to gather sweeping robot around ground environment examine sensor 14 infraredly, be used for gathering the virtual wall detecting sensor 15 of restrictive signal on every side of sweeping robot, and fill detecting sensor 16 for the seat that recharges a signal around the collection sweeping robot.
The second bluetooth communication 11 is used for externally transmitting and receive data.
Robot control submodule 12 comprises the control chip that has embedded predetermined robot path planning's program, robotary data acquisition program and robot motion's control program, the control sweeping robot carries out work by predetermined path, and changes the motion state of sweeping robot according to the outer signals that receives.
Road wheel control module 17 comprises two driving wheels and a universal wheel, is used for sweeping robot and moves in the perform region.
Be provided with smart mobile phone module 1 at the sweeping robot body upper, smart mobile phone module 1 comprises smart mobile phone camera 4, image processing submodule 7, camera calibration submodule 3, extended Kalman filter-locate and map building (Extended Kalman Filter-Simultaneous Localization And Map Building synchronously, initialism is EKF-SLAM) submodule 8, map safeguard and path planning submodule 9, the first bluetooth communication 10, and sensor submodule, sensor data acquisition submodule.Wherein the sensor submodule comprises inertial sensor: gyroscope 5 and accelerometer 6.Image is processed submodule 7, camera calibration submodule 3, EKF-SLAM submodule 8 and map safeguard that with path planning submodule 9 API by separately communicates respectively, to realize in the sweeping robot walking process, utilize the information of smart mobile phone camera 4 and sweeping robot internal sensor, by based on EKF-SLAM submodule 8, carry out the navigator fix computing, and according to the data of the sensor submodule collection of leading positioning result and sweeping robot module or the information that provides, locate accurately, creation environment map and set up the indoor environment barrier and cover map, automatically the sweep-out pattern of planning sweeping robot is controlled next step motion of sweeping robot road wheel.
Smart mobile phone camera 4 is used for obtaining image information from the external world.
Gyroscope 5 is used for gathering the sweeping robot kinematic parameter: angular velocity.
Accelerometer 6 is used for gathering the sweeping robot kinematic parameter: linear acceleration.
The sensor data acquisition submodule comprises for the analog signal conversion with the sensor collection being the A/D module of digital signal.
Image is processed submodule 7 and is used for the image that obtains from the external world is carried out feature extraction and exports correlated results.
Camera calibration submodule 3 is set up mapping relations for the image that will collect and the object of three dimensions.
The image information that EKF-SLAM submodule 8 is caught according to smart mobile phone camera 4 and the sweeping robot moving parameter information of inertial sensor collection position, the creation environment map is also set up indoor barrier according to the sweeping robot ambient condition information of keeping away the collection of barrier sensor and is covered map.
Map is safeguarded and the sweep-out pattern of path planning submodule 9 according to the cartographic information planning sweeping robot of EKF-SLAM submodule 8 acquisitions, and is passed through the action of the first bluetooth communication 10, the second bluetooth communication 11 control sweeping robots.
The second bluetooth communication 10 is identical with the first bluetooth communication 11 compositions, carries out duplex communication by bluetooth, and utilizes the action of Bluetooth communication control sweeping robot.
The sweep-out pattern of this embodiment comprises such as Fig. 4~intelligent obstacle formula, spiral shown in Figure 8 and extends out formula, sweeps formula, broken line essence along the wall essence and sweep formula and line barrier cleaning formula.
It is chequered with black and white tessellated calibrating template that the navigation cleaning method is based on default pattern, and following steps are arranged successively:
1) adopt calibrating template to demarcate smart mobile phone camera intrinsic parameter;
Comprise step by step following:
11) the smart mobile phone camera is aimed at calibrating template;
12) start the camera calibration submodule, guaranteeing that calibrating template all is arranged under the prerequisite in the camera image visual field, with both direction at least calibrating template is taken pictures, gather the scaling board image;
13) the camera calibration submodule is set up corresponding relation with the point set that detects in the scaling board image that collects and the three-dimensional point set of actual scaling board, adopts the plane reference algorithm automatically to calculate Intrinsic Matrix (fu, fv that the smart mobile phone camera is optimized, u0, v0, k1, k2, p1, p2), and be stored in the smart mobile phone Intrinsic Matrix (fu, fv that the smart mobile phone camera is optimized, u0, v0, k1, k2, p1, p2) wherein:
Fu: horizontal focal length;
Fv: vertical focal length;
U0: principal point horizontal ordinate;
V0: principal point ordinate;
K1: camera lens is a distortion parameter radially;
K2: camera lens radial second distortion parameter;
P1: the tangential distortion parameter of camera lens;
P2: the tangential secondary distortion parameter of camera lens;
In system's operational process, same smart mobile phone needn't be demarcated again.
2) setting up Bluetooth communication connects;
Comprise step by step following:
21) start the first bluetooth communication in the smart mobile phone module, and set up Bluetooth wireless communication after the second bluetooth communication on the sweeping robot fuselage is carried out Bluetooth pairing and be connected;
22) instruction that the control sweeping robot that is sent by smart mobile phone by sweeping robot identification moves;
If command instruction is identified as query statement, the robot control submodule that has embedded the control chip of predetermined robotary data acquisition program is just encoded each state constantly of sweeping robot internal sensor, and joining day stamp, the return data status identifier, form the sensor states vector, again the sensor states vector is sent to smart mobile phone by the first bluetooth communication, the second bluetooth communication;
If instruction is identified as steering order, the robot control submodule that has embedded the control chip of predetermined robot motion's control program is decoded to the steering order that the smart mobile phone that receives sends, and be converted to the operation control command of sweeping robot, the control sweeping robot carries out work by predetermined path, and change the motion state of sweeping robot according to the outer signals that receives, comprise forward-reverse, rotation specified angle;
If control successfully, then send the control link order by the first bluetooth communication, the second bluetooth communication to smart mobile phone, comprise in the instruction whether operating successfully identifying and control the link order sign;
If control is success, then resend steering order after 2 seconds~4 seconds by smart mobile phone postponing setting-up time, repeat again step by step 22);
3) start the EKF-SLAM submodule, accurately locate, create the three-dimensional environment map;
Comprise the smart mobile phone camera is kept straight up, be fixed in the sweeping robot top, and step by step following:
31) EKF-SLAM submodule initialization
Judge whether to exist the map that has created;
Create map if exist, then changed EKF-SLAM algorithm reorientation operation over to, adopted the reorientation algorithm to obtain current smart mobile phone camera in the position and the attitude that create in the map, comprised following substep:
311) angle point that extracts in the current camera image and the map feature that has created in the map are carried out global registration, obtain the preliminary corresponding relation of two dimensional character and three-dimensional point;
312) utilize the PNP algorithm based on DLT to cooperate the RANSAC algorithm, according to the camera intrinsic parameter of having demarcated (fu, fv, u0, v0, k1, k2, p1, p2), obtain position and the attitude of optimum smart mobile phone camera;
If successfully obtain position and the attitude of optimum smart mobile phone camera, then carry out step by step 33), a step EKF-SLAM algorithm iteration;
If successfully do not obtain position and the attitude of optimum smart mobile phone camera, then carry out step by step 32);
If there is no created map, then directly carried out step by step 32);
32) map initialization
The bluetooth of utilizing smart mobile phone and sweeping robot to set up is connected, control sweeping robot rectilinear motion set apart from 25cm~35cm, to two two field pictures of motion front camera and rear camera two diverse locations acquisitions, carry out extraction and the coupling of the image angle point of the overall situation, utilize matching result and the ceiling environment hypothesis of plane mechanism, calculate relative position and the attitude R of two two field pictures and camera
0, t
0, R wherein
0That one 3 * 3 rotation matrix represents the three-dimensional rotation between initial two two field pictures and the camera, t
0That one 3 * 1 column vector represents the D translation between initial two two field pictures and the camera, and utilize trigonometric ratio to generate initial three-dimensional map feature, utilize the BA algorithm to be optimized, obtain the map feature point of final initial three-dimensional map and position and the attitude of the sweeping robot in two two field pictures;
33) inertial sensor that reads in the sensor submodule of sweeping robot gathers the kinematic parameter of sweeping robot as the controlled quentity controlled variable of EKF-SLAM submodule, carry out the one-step prediction of system state, described system state comprises robot location, attitude, and the position of all three-dimensional map unique points;
34) video image that arrives for the smart mobile phone camera collection, utilize step by step 33) the sweeping robot state of prediction, obtain the characteristics of image estimation range, carry out Corner Feature and extract and adopt the coupling of carrying out map feature based on the grey level histogram MMSE algorithm of principal direction;
Corner Feature extracts and the coupling of map feature if successfully carry out, and then carries out step by step 35);
Corner Feature extracts and the coupling of map feature if successfully do not carry out, and the data correlation failure namely occurs, repeats step by step 31 again) reorientation of carrying out sweeping robot operates;
35) will be step by step 34) carry out that Corner Feature extracts and the result of the coupling of map feature in conjunction with step by step 13) Intrinsic Matrix (fu, fv, the u0 of the smart mobile phone camera optimization that automatically calculates, v0, k1, k2, p1, p2), obtain observation and upgrade matrix, carry out EKF and upgrade, upgrade step by step 33) the sweeping robot state of prediction, upgrade simultaneously the map feature point, obtain current position and the attitude of sweeping robot;
36) judge whether to exist newly-built map feature need to carry out Feature Conversion;
If the standard deviation sigma of the ρ reciprocal of the unique point degree of depth under a certain camera coordinate system
ρMore than or equal to the threshold tau of setting, then enter step by step 37);
If the standard deviation sigma of the ρ reciprocal of the unique point degree of depth under a certain camera coordinate system
ρLess than the threshold tau of setting, need to carry out Feature Conversion, map feature is converted to the three-dimensional world coordinate by anti-depth representing represents, the selection of described threshold tau will guarantee that ρ drops on τ and σ with 99% probability
ρIn the general curved surface such as grade of determining,
Conversion formula is as follows:
Wherein:
The tri-vector of camera position;
x
W: the x axial coordinate parameter in the tri-vector of camera position;
y
W: the y axial coordinate parameter in the tri-vector of camera position;
z
W: the z axial coordinate parameter in the tri-vector of camera position;
The tri-vector of camera position during the map feature initialization;
r
x: the x axial coordinate parameter during the map feature initialization in the tri-vector of camera position;
r
y: the y axial coordinate parameter during the map feature initialization in the tri-vector of camera position;
r
z: the z axial coordinate parameter during the map feature initialization in the tri-vector of camera position;
θ: the deflection of the initial depth direction of map feature point;
The angle of pitch of the initial depth direction of map feature point;
ρ: the inverse of the unique point degree of depth under the camera coordinate system;
37) judge that according to the PNP algorithm whether current visible features point is less than 6~15 visible points of setting range;
If there are 6~15 unique points of known three-dimensional position and correspondence image position, then the position of smart mobile phone camera and attitude have unique solution;
If current visible features point is less than setting range, newly-increased unique point then carries out step by step 38);
If current visible features point, is then finished a step EKF-SLAM algorithm iteration more than or equal to setting range, carry out step by step 39):
38) newly-built unique point
For current camera image, at the image-region without correct coupling map feature, adopt Fast Corner Detection (Features from Accelerated Segment Test is called for short FAST) algorithm detected image angle point, and carry out the initialization of anti-depth map feature;
The description of Corner Feature adopts
radius 10 pixels to describe with the polar coordinates gray average histogram of principal direction, and the anti-degree of depth initiation parameter of map feature is 6 dimensional vectors:
Wherein:
[r
x, r
y, r
z]
T: the tri-vector of camera current location;
r
x: the x axial coordinate parameter in the tri-vector of camera current location;
r
y: the y axial coordinate parameter in the tri-vector of camera current location;
r
z: the z axial coordinate parameter in the tri-vector of camera current location;
θ: the deflection of the initial depth direction of map feature point;
The angle of pitch of the initial depth direction of map feature point;
ρ is the inverse of newly-built map feature estimation of Depth value;
39) a step EKF-SLAM algorithm iteration is finished, and obtains accurate positioning result and the three-dimensional environment map of robot, judges whether to stop cleaning operation;
If the termination cleaning operation then finishes the navigation operation;
If do not stop cleaning operation, then enter step 4);
4) set up the indoor environment barrier and cover map;
Map is safeguarded and the path planning submodule utilizes bluetooth communication to read the sensor information of current sweeping robot, with current sweeping robot positioning result, and the ambient condition information of keeping away the sweeping robot of barrier sensor collection is sent to map maintenance and path planning submodule, renewal floor level barrier covering map;
5) automatically plan the sweep-out pattern of sweeping robot;
Utilizing step 4) the floor level barrier that upgrades covers map, according to sweeping robot course and overlay area, next step course of planning sweeping robot;
6) next step motion of control sweeping robot road wheel.
To the smart mobile phone sending controling instruction, the control sweeping robot is to clear and not purging zone motion by the first bluetooth communication, the second bluetooth communication, below repeats step by step 33) and subsequent step.
Above content is in conjunction with concrete preferred implementation further description made for the present invention, can not assert that implementation of the present invention is confined to these explanations.For the general technical staff of the technical field of the invention; make without departing from the inventive concept of the premise such as dried fruit and be equal to alternative or obvious modification; and performance or purposes are identical, all should be considered as belonging to the scope of patent protection that the present invention is determined by claims of submitting to.