Summary of the invention
A technical problem to be solved by this invention is the defect making up above-mentioned prior art, provides a kind of sweeping robot adopting smart mobile phone to navigate.
Another technical problem to be solved by this invention is the defect making up above-mentioned prior art, provides a kind of navigation cleaning method of the sweeping robot adopting smart mobile phone to navigate.
The sweeping robot technical problem 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 sweeping robot module, described sweeping robot module comprises the second bluetooth communication, robot controlling submodule, keep away barrier sensor and road wheel control module, the described infrared wall inspection sensor kept away barrier sensor and comprise surrounding obstacles for gathering sweeping robot or wall, sensor is examined for gathering the infrared of surrounding's ground environment of sweeping robot, for gathering the virtual wall detecting sensor of surrounding's restrictive signal of sweeping robot, and the seat that the surrounding for gathering sweeping robot recharges a signal fills detecting sensor.
Described second bluetooth communication is used for externally transmitting and receive data.
Described robot controlling submodule comprises the control chip that embedded in predetermined robot path planning's program, robotary data acquisition program and robot manufacturing cell, control sweeping robot and carry out work by predetermined path, and change the motion state of sweeping robot according to the outer signals received.
Described road wheel control module comprises two driving wheels and a universal wheel, moves in working region for sweeping robot.
The feature of the sweeping robot of this employing smart mobile phone navigation is:
Smart mobile phone module is provided with at sweeping robot body upper, described smart mobile phone module comprises smart mobile phone camera, image procossing submodule, camera calibration submodule, extended Kalman filter-synchronous location and map building (Extended Kalman Filter-Simultaneous LocalizationAnd Map Building, initialism is EKF-SLAM) submodule, map safeguard and path planning submodule and the first bluetooth communication, and sense signals module, sensor data acquisition submodule.Described image procossing submodule, camera calibration submodule, EKF-SLAM submodule and map are safeguarded with path planning submodule respectively by respective application programming interface (ApplicationProgramming Interface, initialism is API) communicate, to realize in sweeping robot walking process, utilize the information of smart mobile phone camera and sweeping robot internal sensor, by based on EKF-SLAM submodule, carry out navigator fix computing, and according to leading the data of sense signals module acquires of positioning result and described sweeping robot module or the information that provides, locate accurately, creation environment map and set up indoor environment barrier cover map, the sweep-out pattern of automatic planning sweeping robot, control 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 analog-to-digital conversion (Analog To Digital Converter, initialism the is A/D) module for the analog signal of sensor collection being converted to data signal.
Described image procossing submodule is used for carrying out feature extraction to the image obtained from the external world and exporting correlated results.
Described camera calibration submodule is used for the object in the image collected and three dimensions to set up mapping relations.
The sweeping robot moving parameter information of the image information that described EKF-SLAM submodule is caught according to described smart mobile phone camera and described inertial sensor collection positions, creation environment map, the sweeping robot ambient condition information of also keeping away the collection of barrier sensor described in basis sets up indoor barrier covering map.
The sweep-out pattern planning sweeping robot with path planning submodule according to the cartographic information that described EKF-SLAM submodule obtains safeguarded by described map, and controls the action of sweeping robot by the first bluetooth communication, the second bluetooth communication.
Described second bluetooth communication, forms identical with described first bluetooth communication in described smart mobile phone module, carries out duplex communication by bluetooth, and utilizes Bluetooth communication to control the action of sweeping robot.
The sweeping robot technical problem of employing smart mobile phone navigation of the present invention is solved by following further technical scheme.
Described sense signals module comprises inertial sensor: gyroscope and accelerometer, and described gyroscope is for gathering sweeping robot kinematic parameter: angular speed, and described accelerometer is for gathering sweeping robot kinematic parameter: linear acceleration.
The navigation cleaning method technical problem of the sweeping robot of employing smart mobile phone navigation of the present invention is solved by the following technical programs.
The feature of the navigation cleaning method of the sweeping robot of this employing smart mobile phone navigation is:
Adopt the sweeping robot of smart mobile phone navigation to work in navigation cleaning modes based on the calibrating template preset, described default calibrating template is pattern is chequered with black and white tessellated calibrating template, has following steps successively:
1) described calibrating template is adopted to demarcate smart mobile phone camera internal reference number;
2) set up Bluetooth communication to connect;
3) start EKF-SLAM submodule, accurately locate, create three-dimensional environment map;
4) set up indoor environment barrier and cover map;
5) sweep-out pattern of sweeping robot is automatically planned;
6) next step motion of sweeping robot road wheel is controlled.
The navigation cleaning method technical problem 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 internal reference number, comprise step by step following:
11) smart mobile phone camera is aimed at described calibrating template;
12) start camera calibration submodule, under the described calibrating template of guarantee is all arranged in the prerequisite in the camera image visual field, with at least both direction, described calibrating template is taken pictures, gather scaling board image;
13) the three-dimensional point set of the point set detected in the scaling board image collected and actual scaling board is set up corresponding relation by camera calibration submodule, adopts plane reference algorithm automatically to calculate Intrinsic Matrix (fu, the fv of the optimization of smart mobile phone camera, u0, v0, k1, k2, p1, p2), and be stored in 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 abscissa;
V0: principal point ordinate;
K1: the radial distortion parameter of camera lens;
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 operation, same smart mobile phone need not be demarcated again.
Described step 2) set up Bluetooth communication connection, comprise step by step following:
21) start described first bluetooth communication in smart mobile phone module, set up Bluetooth wireless communication after carrying out Bluetooth pairing with the second bluetooth communication on sweeping robot fuselage and be connected;
22) instruction of the control sweeping robot motion sent by smart mobile phone by sweeping robot identification;
If command instruction is identified as query statement, the state in each moment of sweeping robot internal sensor is just encoded by the described robot controlling submodule that embedded in the control chip of predetermined robotary data acquisition program, and joining day stamp, return data status identifier, composition sensor states vector, then sensor states vector is sent to smart mobile phone by the first bluetooth communication, the second bluetooth communication;
If instruction is identified as control instruction, the described robot controlling submodule that embedded in the control chip of predetermined robot manufacturing cell is decoded to the control instruction that the smart mobile phone received sends, and be converted to the operation control command of sweeping robot, control sweeping robot and carry out work by predetermined path, and the motion state of sweeping robot is changed according to the outer signals received, comprise forward-reverse, rotate specified angle;
If control successfully, then send control return instruction by the first bluetooth communication, the second bluetooth communication to smart mobile phone, instruction comprises whether successful operation identifies and control return instruction mark;
If control unsuccessful, then after delay setting-up time, resend control instruction by smart mobile phone, then repeat step by step 22), described delay setting-up time is 2 seconds ~ 4 seconds.
Described step 3) start EKF-SLAM submodule, accurately locate, create three-dimensional environment map, comprise and smart mobile phone camera is kept straight up, be fixed on sweeping robot top, and below step by step:
31) EKF-SLAM submodule initializes
Judge whether to there is the map created;
If exist and create map, then proceed to EKF-SLAM algorithm reorientation operation, adopt reorientation algorithm to obtain current smart mobile phone camera in the position created in map and attitude, comprise following sub-step:
311) global registration is carried out to the angle point extracted in current camera image and the map feature created in map, obtain the preliminary corresponding relation of two dimensional character and three-dimensional point;
312) utilize based on direct linear transformation (Direct Linear Transformation, initialism is DLT) perspective N point algorithm (Perspective N Points, initialism is PNP), coordinate random sampling uniformity (Random Sample Consensus, initialism is RANSAC) algorithm, according to the camera internal reference number (fu demarcated, 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;
Obtain position and the attitude of optimum smart mobile phone camera if unsuccessful, then carry out step by step 32);
If there is no create map, then directly carry out step by step 32);
32) map initialization
The bluetooth utilizing smart mobile phone and sweeping robot to set up is connected, control the distance of sweeping robot rectilinear motion setting, to two two field pictures that motion front camera and rear camera obtains at two diverse locations, 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, wherein R
0the three-dimensional rotation that the spin matrix of 3 × 3 represents between initial two two field pictures and camera, t
0it is the D translation that the column vector of 3 × 1 represents between initial two two field pictures and 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 position and the attitude of the sweeping robot in the map feature point of final initial three-dimensional map and two two field pictures;
The distance of described sweeping robot rectilinear motion setting is 25cm ~ 35cm;
33) inertial sensor read in the sense signals module of sweeping robot gathers the controlled quentity controlled variable of kinematic parameter as EKF-SLAM submodule of sweeping robot, carry out the one-step prediction of system mode, described system mode comprises robot location, attitude, and the position of all three-dimensional map characteristic points;
34) for the video image that smart mobile phone camera collection arrives, step by step 33 are utilized) the sweeping robot state predicted, obtain characteristics of image estimation range, carry out Corner Feature and extract and the coupling of map feature;
Extract and the coupling of map feature if successfully carry out Corner Feature, then carry out step by step 35);
Carry out Corner Feature to extract and the coupling of map feature if unsuccessful, namely occur data correlation failure, then repeat step by step 31) reorientation of carrying out sweeping robot operates;
The coupling of described map feature adopts grey level histogram least mean-square error (Minimum Mean Squared Error, initialism the is MMSE) algorithm based on principal direction to mate;
35) by step by step 34) carry out Corner Feature and to extract and the result of coupling of map feature combines step by step 13) Intrinsic Matrix (fu, fv, the u0 of the optimization of smart mobile phone camera that automatically calculate, v0, k1, k2, p1, p2), obtain observation and upgrade matrix, carry out EKF renewal, upgrade step by step 33) the sweeping robot state predicted, upgrade map feature point simultaneously, obtain the current position of sweeping robot and attitude;
36) judge whether that there is newly-built map feature needs to carry out Feature Conversion;
If the standard deviation sigma of the ρ reciprocal of the characteristic point degree of depth under a certain camera coordinate system
ρbe 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 characteristic point degree of depth under a certain camera coordinate system
ρbe less than the threshold tau of setting, need to carry out Feature Conversion, map feature is converted to three-dimensional world coordinate by anti-depth representing and represents, the selection of described threshold tau to ensure ρ with 99% probability drop on τ and σ
ρwithin the general curved surface such as grade determined;
Conversion formula is as follows:
Wherein:
The three-dimensional vector of camera position;
X
w: the x-axis coordinate parameters in the three-dimensional vector of camera position;
Y
w: the y-axis coordinate parameters in the three-dimensional vector of camera position;
Z
w: the z-axis coordinate parameters in the three-dimensional vector of camera position;
The three-dimensional vector of camera position when map feature initializes;
R
x: map feature initialize time camera position three-dimensional vector in x-axis coordinate parameters;
R
y: map feature initialize time camera position three-dimensional vector in y-axis coordinate parameters;
R
z: map feature initialize time camera position three-dimensional vector in z-axis coordinate parameters;
θ: the deflection in the ID direction of map feature point;
the angle of pitch in the ID direction of map feature point;
ρ: the inverse of the characteristic point degree of depth under camera coordinate system;
37) judge whether current visible features point is less than setting range according to PNP algorithm, described setting range is 6 ~ 15 visible points;
If there are 6 ~ 15 characteristic 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, then newly-increased characteristic point, carries out step by step 38);
If current visible features point is more than or equal to setting range, then complete a step EKF-SLAM algorithm iteration, carry out step by step 39):
38) newly-built characteristic point
For current camera image, at the image-region without correct coupling map feature, 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 AcceleratedSegment Test, being called for short FAST) algorithm carries out, the description of Corner Feature adopts radius 10 pixel 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 three-dimensional vector of camera current location;
R
x: the x-axis coordinate parameters in the three-dimensional vector of camera current location;
R
y: the y-axis coordinate parameters in the three-dimensional vector of camera current location;
R
z: the z-axis coordinate parameters in the three-dimensional vector of camera current location;
θ: the deflection in the ID direction of map feature point;
the angle of pitch in the ID direction of map feature point;
ρ is the inverse of newly-built map feature estimation of Depth value;
39) a step EKF-SLAM algorithm iteration completes, and obtains accurate positioning result and the three-dimensional environment map of robot, judges whether to stop cleaning operation;
If termination cleaning operation, then terminate guidance operation;
If do not stop cleaning operation, then enter step 4).
Described step 4) set up indoor environment barrier covering map, that map is safeguarded and path planning submodule utilizes bluetooth communication to read the sensor information of current sweeping robot, by current sweeping robot positioning result, and the ambient condition information of the sweeping robot keeping away the collection of barrier sensor is sent to map maintenance and path planning submodule, upgrades floor level barrier and covers map.
Described step 5) sweep-out pattern of automatic planning sweeping robot, be 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) control next step motion of sweeping robot road wheel, to smart mobile phone sending controling instruction by the first bluetooth communication, the second bluetooth communication, control sweeping robot to clear and the motion of non-purging zone, below repeat step by step 33) and subsequent step.
The present invention's beneficial effect is compared with prior art:
Present invention, avoiding and directly on sweeping robot, install navigator fix and sensor hardware device additional, utilize the camera of smart mobile phone inside, comprise the navigator fix that accelerometer and gyrostatic inertial sensor carry out sweeping robot, obtain the three-dimensional map of the real-time precise position information of sweeping robot and surrounding environment, and floor level barrier covers map, and each step action of sweeping robot is controlled by Bluetooth communication, have and implement simply, hardware modifications is little, accurate positioning, cleaning area coverage is large, the advantage that sweeping efficiency is high.
Detailed description of the invention
Contrast accompanying drawing below in conjunction with detailed description of the invention the present invention will be described.
A kind of sweeping robot of the employing smart mobile phone navigation as shown in Fig. 1 ~ 8, comprise sweeping robot module 2, sweeping robot module 2 comprises the second bluetooth communication 11, robot controlling submodule 12, keep away barrier sensor and road wheel control module 17, wherein keep away the infrared wall inspection sensor 13 that barrier sensor comprises surrounding obstacles for gathering sweeping robot or wall, sensor 14 is examined for gathering the infrared of surrounding's ground environment of sweeping robot, for gathering the virtual wall detecting sensor 15 of surrounding's restrictive signal of sweeping robot, and the seat that the surrounding for gathering sweeping robot recharges a signal fills detecting sensor 16.
Second bluetooth communication 11 is for externally transmitting and receive data.
Robot controlling submodule 12 comprises the control chip that embedded in predetermined robot path planning's program, robotary data acquisition program and robot manufacturing cell, control sweeping robot and carry out work by predetermined path, and change the motion state of sweeping robot according to the outer signals received.
Road wheel control module 17 comprises two driving wheels and a universal wheel, moves in working region for sweeping robot.
Smart mobile phone module 1 is provided with at sweeping robot body upper, smart mobile phone module 1 comprises smart mobile phone camera 4, image procossing submodule 7, camera calibration submodule 3, extended Kalman filter-synchronous location and map building (Extended Kalman Filter-SimultaneousLocalization And Map Building, initialism is EKF-SLAM) submodule 8, map safeguard and path planning submodule 9, first bluetooth communication 10, and sense signals module, sensor data acquisition submodule.Wherein sense signals module comprises inertial sensor: gyroscope 5 and accelerometer 6.Image procossing submodule 7, camera calibration submodule 3, EKF-SLAM submodule 8 is safeguarded with map and is communicated respectively by respective API with path planning submodule 9, to realize in 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 navigator fix computing, and according to leading the data of sense signals module acquires of positioning result and sweeping robot module or the information that provides, locate accurately, creation environment map and set up indoor environment barrier cover map, the sweep-out pattern of automatic planning sweeping robot, control next step motion of sweeping robot road wheel.
Smart mobile phone camera 4 is for obtaining image information from the external world.
Gyroscope 5 is for gathering sweeping robot kinematic parameter: angular speed.
Accelerometer 6 is for gathering sweeping robot kinematic parameter: linear acceleration.
Sensor data acquisition submodule comprises the A/D module for the analog signal of sensor collection being converted to data signal.
Image procossing submodule 7 is for carrying out feature extraction to the image obtained from the external world and exporting correlated results.
Camera calibration submodule 3 is for setting up mapping relations by the object in the image collected and three dimensions.
The sweeping robot moving parameter information of the image information that EKF-SLAM submodule 8 is caught according to smart mobile phone camera 4 and inertial sensor collection positions, creation environment map, also sets up indoor barrier according to the sweeping robot ambient condition information of keeping away the collection of barrier sensor and covers map.
The sweep-out pattern planning sweeping robot with path planning submodule 9 according to the cartographic information that EKF-SLAM submodule 8 obtains safeguarded by map, and controlled the action of sweeping robot by the first bluetooth communication 10, second bluetooth communication 11.
Second bluetooth communication 10 forms identical with the first bluetooth communication 11, carries out duplex communication by bluetooth, and utilizes Bluetooth communication to control the action of sweeping robot.
The sweep-out pattern of this detailed description of the invention comprises intelligent obstacle formula as shown in Fig. 4 ~ Fig. 8, spiral extends out formula, sweep formula along wall essence, broken line essence sweeps formula and line barrier cleaning formula.
Navigation cleaning method be based on preset pattern be chequered with black and white tessellated calibrating template, have following steps successively:
1) calibrating template is adopted to demarcate smart mobile phone camera internal reference number;
Comprise step by step following:
11) smart mobile phone camera is aimed at calibrating template;
12) start camera calibration submodule, under guarantee calibrating template is all arranged in the prerequisite in the camera image visual field, with at least both direction, calibrating template is taken pictures, gather scaling board image;
13) the three-dimensional point set of the point set detected in the scaling board image collected and actual scaling board is set up corresponding relation by camera calibration submodule, adopts plane reference algorithm automatically to calculate Intrinsic Matrix (fu, the fv of the optimization of smart mobile phone camera, u0, v0, k1, k2, p1, p2), and be stored in smart mobile phone, Intrinsic Matrix (fu, fv that smart mobile phone camera is optimized, u0, v0, k1, k2, p1, p2) wherein:
Fu: horizontal focal length;
Fv: vertical focal length;
U0: principal point abscissa;
V0: principal point ordinate;
K1: the radial distortion parameter of camera lens;
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 operation, same smart mobile phone need not be demarcated again.
2) set up Bluetooth communication to connect;
Comprise step by step following:
21) start the first bluetooth communication in smart mobile phone module, set up Bluetooth wireless communication after carrying out Bluetooth pairing with the second bluetooth communication on sweeping robot fuselage and be connected;
22) instruction of the control sweeping robot motion sent by smart mobile phone by sweeping robot identification;
If command instruction is identified as query statement, the state in each moment of sweeping robot internal sensor is just encoded by the robot controlling submodule that embedded in the control chip of predetermined robotary data acquisition program, and joining day stamp, return data status identifier, composition sensor states vector, then sensor states vector is sent to smart mobile phone by the first bluetooth communication, the second bluetooth communication;
If instruction is identified as control instruction, the robot controlling submodule that embedded in the control chip of predetermined robot manufacturing cell is decoded to the control instruction that the smart mobile phone received sends, and be converted to the operation control command of sweeping robot, control sweeping robot and carry out work by predetermined path, and the motion state of sweeping robot is changed according to the outer signals received, comprise forward-reverse, rotate specified angle;
If control successfully, then send control return instruction by the first bluetooth communication, the second bluetooth communication to smart mobile phone, instruction comprises whether successful operation identifies and control return instruction mark;
If control unsuccessful, then resend control instruction at delay setting-up time after 2 seconds ~ 4 seconds by smart mobile phone, then repeat step by step 22);
3) start EKF-SLAM submodule, accurately locate, create three-dimensional environment map;
Comprise and smart mobile phone camera is kept straight up, be fixed on sweeping robot top, and below step by step:
31) EKF-SLAM submodule initializes
Judge whether to there is the map created;
If exist and create map, then proceed to EKF-SLAM algorithm reorientation operation, adopt reorientation algorithm to obtain current smart mobile phone camera in the position created in map and attitude, comprise following sub-step:
311) global registration is carried out to the angle point extracted in current camera image and the map feature created in map, obtain the preliminary corresponding relation of two dimensional character and three-dimensional point;
312) utilize the PNP algorithm based on DLT to coordinate RANSAC algorithm, according to the camera internal reference number (fu, fv, u0, v0, k1, k2, p1, p2) demarcated, 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;
Obtain position and the attitude of optimum smart mobile phone camera if unsuccessful, then carry out step by step 32);
If there is no create map, then directly carry out step by step 32);
32) map initialization
The bluetooth utilizing smart mobile phone and sweeping robot to set up is connected, control the distance 25cm ~ 35cm of sweeping robot rectilinear motion setting, to two two field pictures that motion front camera and rear camera obtains at two diverse locations, 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, wherein R
0the three-dimensional rotation that the spin matrix of 3 × 3 represents between initial two two field pictures and camera, t
0it is the D translation that the column vector of 3 × 1 represents between initial two two field pictures and camera, and utilize trigonometric ratio to generate initial three-dimensional map feature, utilize BA algorithm to be optimized, obtain position and the attitude of the sweeping robot in the map feature point of final initial three-dimensional map and two two field pictures;
33) inertial sensor read in the sense signals module of sweeping robot gathers the controlled quentity controlled variable of kinematic parameter as EKF-SLAM submodule of sweeping robot, carry out the one-step prediction of system mode, described system mode comprises robot location, attitude, and the position of all three-dimensional map characteristic points;
34) for the video image that smart mobile phone camera collection arrives, utilize step by step 33) the sweeping robot state predicted, obtain characteristics of image estimation range, carry out Corner Feature extraction and adopt the grey level histogram MMSE algorithm based on principal direction to carry out the coupling of map feature;
Extract and the coupling of map feature if successfully carry out Corner Feature, then carry out step by step 35);
Carry out Corner Feature to extract and the coupling of map feature if unsuccessful, namely occur data correlation failure, then repeat step by step 31) reorientation of carrying out sweeping robot operates;
35) by step by step 34) carry out Corner Feature and to extract and the result of coupling of map feature combines step by step 13) Intrinsic Matrix (fu, fv, the u0 of the optimization of smart mobile phone camera that automatically calculate, v0, k1, k2, p1, p2), obtain observation and upgrade matrix, carry out EKF renewal, upgrade step by step 33) the sweeping robot state predicted, upgrade map feature point simultaneously, obtain the current position of sweeping robot and attitude;
36) judge whether that there is newly-built map feature needs to carry out Feature Conversion;
If the standard deviation sigma of the ρ reciprocal of the characteristic point degree of depth under a certain camera coordinate system
ρbe 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 characteristic point degree of depth under a certain camera coordinate system
ρbe less than the threshold tau of setting, need to carry out Feature Conversion, map feature is converted to three-dimensional world coordinate by anti-depth representing and represents, the selection of described threshold tau to ensure ρ with 99% probability drop on τ and σ
ρwithin the general curved surface such as grade determined,
Conversion formula is as follows:
Wherein:
The three-dimensional vector of camera position;
X
w: the x-axis coordinate parameters in the three-dimensional vector of camera position;
Y
w: the y-axis coordinate parameters in the three-dimensional vector of camera position;
Z
w: the z-axis coordinate parameters in the three-dimensional vector of camera position;
The three-dimensional vector of camera position when map feature initializes;
R
x: map feature initialize time camera position three-dimensional vector in x-axis coordinate parameters;
R
y: map feature initialize time camera position three-dimensional vector in y-axis coordinate parameters;
R
z: map feature initialize time camera position three-dimensional vector in z-axis coordinate parameters;
θ: the deflection in the ID direction of map feature point;
the angle of pitch in the ID direction of map feature point;
ρ: the inverse of the characteristic point degree of depth under camera coordinate system;
37) judge whether current visible features point is less than setting range 6 ~ 15 visible points according to PNP algorithm;
If there are 6 ~ 15 characteristic 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, then newly-increased characteristic point, carries out step by step 38);
If current visible features point is more than or equal to setting range, then complete a step EKF-SLAM algorithm iteration, carry out step by step 39):
38) newly-built characteristic 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 pixel 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 three-dimensional vector of camera current location;
R
x: the x-axis coordinate parameters in the three-dimensional vector of camera current location;
R
y: the y-axis coordinate parameters in the three-dimensional vector of camera current location;
R
z: the z-axis coordinate parameters in the three-dimensional vector of camera current location;
θ: the deflection in the ID direction of map feature point;
the angle of pitch in the ID direction of map feature point;
ρ is the inverse of newly-built map feature estimation of Depth value;
39) a step EKF-SLAM algorithm iteration completes, and obtains accurate positioning result and the three-dimensional environment map of robot, judges whether to stop cleaning operation;
If termination cleaning operation, then terminate guidance operation;
If do not stop cleaning operation, then enter step 4);
4) set up indoor environment barrier and cover map;
Map is safeguarded and path planning submodule utilizes bluetooth communication to read the sensor information of current sweeping robot, by current sweeping robot positioning result, and the ambient condition information of the sweeping robot keeping away the collection of barrier sensor is sent to map maintenance and path planning submodule, upgrades floor level barrier and covers map;
5) sweep-out pattern of sweeping robot is automatically planned;
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 sweeping robot road wheel is controlled.
By the first bluetooth communication, the second bluetooth communication to smart mobile phone sending controling instruction, control sweeping robot to clear and the motion of non-purging zone, below repeat step by step 33) and subsequent step.
Above content is in conjunction with concrete preferred embodiment further description made for the present invention, can not assert that specific embodiment of the invention is confined to these explanations.For general technical staff of the technical field of the invention; make without departing from the inventive concept of the premise and substituting or obvious modification as dried fruit is equivalent; and performance or purposes identical, all should be considered as belonging to the scope of patent protection that the present invention is determined by submitted to claims.