CN107941217A - A kind of robot localization method, electronic equipment, storage medium, device - Google Patents
A kind of robot localization method, electronic equipment, storage medium, device Download PDFInfo
- Publication number
- CN107941217A CN107941217A CN201710923970.2A CN201710923970A CN107941217A CN 107941217 A CN107941217 A CN 107941217A CN 201710923970 A CN201710923970 A CN 201710923970A CN 107941217 A CN107941217 A CN 107941217A
- Authority
- CN
- China
- Prior art keywords
- robot
- information
- pose
- kinematic constraint
- image information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Abstract
The present invention provides a kind of robot localization method, by obtaining movable information, estimates the current pose of robot according to movable information, odometer and gyroscope are provided kinematic constraint, optimized the current pose of robot using kinematic constraint, obtain the optimal trajectory of robot；The present invention relates to a kind of robotic positioning device；The invention further relates to electronic equipment and readable storage medium storing program for executing, for performing a kind of robot localization method；The present invention perceives external environment condition by binocular vision sensor, odometer and gyroscope perceive robot displacement, strengthen the robustness of vision positioning method, more preferable kinematic constraint is provided, position that cost is low, the accumulated error of pose estimation constantly corrected during robot longtime running, solve that environmental characteristic is similar or sparse and environment in there are under the complex scenes such as more dynamic object, the problem of mistake is calculated caused by error hiding, causes positioning failure.
Description
Technical field
The present invention relates to robot autonomous mobile technology field, more particularly to a kind of robot localization method, electronic equipment,
Storage medium, device.
Background technology
In recent years, robot technology, especially mobile robot technology are greatly affecting our work and life
It is living.Mobile robot it is self-positioning, refer to gather environmental information or displacement information by various kinds of sensors, pass through technology
Data fusion is determined the pose of the relatively a certain reference frame of mobile robot by means and prior information.In service and interior
The fields such as inspection, it is desirable to which robot can realize operation steady in a long-term indoors in environment, and can realize and accurately make by oneself
Position.Positioning method at this stage based on 3D laser radars can meet the demand, but the cost of 3D laser radars is higher, no
It is adapted to a wide range of popularization and application of robot.And visual sensor cost is relatively low, and the information content obtained is larger, by visual sensing
Device is used for robot self-localization, can largely reduce production cost, but existing vision positioning method is transported for a long time
There are accumulated error during row, and in environment texture information require it is higher, existing vision positioning method high dynamic,
Owe in texture scene, failure is positioned caused by it can not establish correct data correlation, and then influence the completion of robot task.
The content of the invention
For overcome the deficiencies in the prior art, it is an object of the present invention to providing a kind of robot localization method, lead to
Cross binocular vision sensor and perceive external environment condition, odometer and gyroscope perceive robot displacement, strengthen vision positioning side
The robustness of method, there is provided more preferable kinematic constraint, positioning cost is low, and pose is constantly corrected during robot longtime running
The accumulated error of estimation, solve that environmental characteristic is similar or sparse and environment in there are the complex scenes such as more dynamic object
Under, because the problem of correct data correlation causes positioning failure can not be established.
The second object of the present invention is to provide a kind of electronic equipment, it can perceive external rings by binocular vision sensor
Border, odometer and gyroscope perceive robot displacement, strengthen the robustness of vision positioning method, there is provided preferably move about
Beam, positioning cost is low, and the accumulated error of pose estimation is constantly corrected during robot longtime running, solves environment spy
Levy in similar or sparse and environment there are under the complex scenes such as more dynamic object, led because correct data correlation can not be established
The problem of causing positioning failure.
The third object of the present invention is to provide a kind of computer-readable recording medium, it can pass through binocular vision sensor
External environment condition is perceived, odometer and gyroscope perceive robot displacement, strengthen the robustness of vision positioning method, there is provided more
Good kinematic constraint, positioning cost is low, and the accumulated error of pose estimation, solution are constantly corrected during robot longtime running
Environmental characteristic of having determined is similar or sparse and environment in there are under the complex scenes such as more dynamic object, because that can not establish correctly
Data correlation causes the problem of positioning failure.
The fourth object of the present invention is to provide a kind of robotic positioning device, it can be perceived by binocular vision sensor
External environment condition, odometer and gyroscope perceive robot displacement, strengthen the robustness of vision positioning method, there is provided preferably
Kinematic constraint, positioning cost is low, and the accumulated error of pose estimation is constantly corrected during robot longtime running, is solved
Environmental characteristic is similar or sparse and environment in there are under the complex scenes such as more dynamic object, because correct data can not be established
Association causes the problem of positioning failure.
An object of the present invention is realized using following technical scheme：
A kind of robot localization method, comprises the following steps：
Movable information is obtained, obtains the movable information of robot, the movable information includes odometer information, gyroscope is believed
Breath, binocular image information, the binocular image information are the image information of binocular camera；
Estimate current pose, the current pose of robot is estimated according to binocular image information；
Kinematic constraint is calculated, kinematic constraint is calculated according to odometer information and gyroscope information；
Optimize current pose, the current pose is optimized according to the kinematic constraint, obtains the optimal trajectory of robot.
Further, the step estimates that current pose specifically includes following steps：
Estimated information is obtained, obtains the present frame binocular image information of robot, previous frame binocular image information, one upper
Appearance；
Pose change is calculated, position is calculated according to the present frame binocular image information and the previous frame binocular image information
Appearance variable quantity；
Current pose is calculated, according to the pose variable quantity and the current pose of the upper pose calculating robot.
Further, the step calculates kinematic constraint and includes calculating the first kinematic constraint, obtains the odometer information
With the relative motion of the gyroscope information computer device people.
Further, the first kinematic constraint of the calculating specifically includes following steps：
Data conversion, the mileage that the odometer information and the gyroscope information are converted to mobile platform is counted and
Gyro data；
Rate integrating, counts the mileage of the mobile platform and gyro data integrates respectively, obtains speed
Integration and angular speed integration；
Relative motion is calculated, separates the initial value component of the rate integrating and angular speed integration, pre-integration is obtained and surveys
Value, separates the noise component(s) of the pre-integration measured value, obtains relative motion measured value.
Further, the step calculates kinematic constraint and further includes the second kinematic constraint of calculating, obtains the rate integrating
With the dynamical equation of angular speed integration, covariance is solved according to the dynamical equation.
Further, the step rate integrating further includes correction angle velocity bias, corrects the angular speed integration
Angular speed offset.
Further, the correction angle velocity bias is specially the Jacobian matrix for obtaining angular speed, according to described refined
The angular speed integration is corrected than matrix, the relative motion measured value is corrected according to modified angular speed integration, is repaiied
Just with respect to motion measure.
Further, the current pose of the optimization order is specially according to the amendment relative motion measured value and the association
Variance optimizes the current pose, obtains the optimal trajectory of robot.
The second object of the present invention is realized using following technical scheme：
A kind of electronic equipment, the equipment include：Processor；
Memory；And program, wherein described program is stored in the memory, and is configured to by processor
Perform, described program includes being used to perform a kind of above-mentioned robot localization method.
The third object of the present invention is realized using following technical scheme：
A kind of computer-readable recording medium, is stored thereon with computer program, and the computer program is held by processor
A kind of above-mentioned robot localization method of row.
The fourth object of the present invention is realized using following technical scheme：
A kind of robotic positioning device, including：
Obtain information module：For obtaining the movable information of robot, the movable information includes odometer information, gyro
Instrument information, binocular image information, the binocular image information are the image information of binocular camera；
Motion estimation module：For estimating the current pose of robot according to the binocular image information；
Kinematic constraint module：For calculating kinematic constraint according to the odometer information and the gyroscope information；
Optimization module：For optimizing the current pose according to the kinematic constraint, the optimal trajectory of robot is obtained.
Further, the motion estimation module further includes：
Obtain estimated information unit：For obtaining present frame binocular image information, the previous frame binocular image letter of robot
Breath, a upper pose；
Calculate pose change unit：For according to the present frame binocular image information and previous frame binocular image letter
Breath calculates pose variable quantity；
Calculate current pose unit：For according to the current of the pose variable quantity and the upper pose calculating robot
Pose.
Further, the kinematic constraint module includes the first kinematic constraint module, the first kinematic constraint module bag
Include：
Date Conversion Unit：For the odometer information and the gyroscope information to be converted to the mileage of mobile platform
Count and gyro data；
Rate integrating unit：For being counted to the mileage of the mobile platform and gyro data integrates respectively,
Obtain rate integrating and angular speed integration；
Calculate relative motion unit：For separating the initial value component of the rate integrating and angular speed integration, obtain
Pre-integration measured value, separates the noise component(s) of the pre-integration measured value, obtains relative motion measured value.
Further, the kinematic constraint module includes the second kinematic constraint module, and the second kinematic constraint module is used
In the dynamical equation for obtaining the rate integrating and angular speed integration, covariance is solved according to the dynamical equation.
Further, the first kinematic constraint module further includes amending unit, and the amending unit is used to obtain angle speed
The Jacobian matrix of degree, corrects the angular speed according to the Jacobian matrix and integrates, integrated and corrected according to modified angular speed
The relative motion measured value, obtains and corrects relative motion measured value.
Compared with prior art, the beneficial effects of the present invention are：
The present invention provides a kind of robot localization method, by obtaining movable information, estimates robot according to movable information
Current pose, odometer and gyroscope provide kinematic constraint, using the current pose of kinematic constraint optimization robot, obtain machine
The optimal trajectory of people；The present invention relates to a kind of robotic positioning device；The invention further relates to electronic equipment and readable storage medium storing program for executing,
For performing a kind of robot localization method；The present invention perceives external environment condition, odometer and gyro by binocular vision sensor
Instrument perceives robot displacement, strengthens the robustness of vision positioning method, there is provided and more preferable kinematic constraint, positioning cost is low,
The accumulated error of pose estimation is constantly corrected during robot longtime running, it is similar or sparse to solve environmental characteristic
And cause asking for positioning failure because correct data correlation can not be established there are under the complex scenes such as more dynamic object in environment
Topic.
Described above is only the general introduction of technical solution of the present invention, in order to better understand the technological means of the present invention,
And can be practiced according to the content of specification, below with presently preferred embodiments of the present invention and coordinate attached drawing describe in detail as after.
The embodiment of the present invention is shown in detail by following embodiments and its attached drawing.
Brief description of the drawings
Attached drawing described herein is used for providing a further understanding of the present invention, forms the part of the application, this hair
Bright schematic description and description is used to explain the present invention, does not form inappropriate limitation of the present invention.In the accompanying drawings：
Fig. 1 is a kind of robot localization method flow diagram of the present invention；
Fig. 2 is a kind of robotic positioning device structure diagram of the present invention.
Embodiment
In the following, with reference to attached drawing and embodiment, the present invention is described further, it is necessary to which explanation is, not
Under the premise of afoul, new implementation can be formed between various embodiments described below or between each technical characteristic in any combination
Example.
A kind of robot localization method, as shown in Figure 1, comprising the following steps：
Obtain movable information, obtain the movable information of robot, movable information include odometer information, gyroscope information,
Binocular image information, binocular image information are the image information of binocular camera；
Estimate current pose, the current pose of robot is estimated according to binocular image information；The pose of robot is some
Moment robot position and orientation, usually by translational component TiWith rotational component RiRepresent, wherein：
Ti∈R3
Ri∈R3,Ri=-Ri T,Ri TRi=I
In one embodiment, using the movement of binocular image information estimation robot, matched according to visual adjacent two field picture
The difference of mode, estimates that the method for movement is divided into dense method and sparse method, wherein, dense method is the gray scale according to consecutive frame image
The gradient difference of difference or gray scale estimation robot motion, sparse method are to be transported by extracting the feature assessment robot of two field pictures
Dynamic, the present embodiment is using sparse method estimation robot motion, it is preferable that step estimates that current pose specifically includes following steps：
Estimated information is obtained, obtains the two field pictures information of robot, two field pictures information is specially present frame binocular figure
Picture information and previous frame binocular image information, obtain a upper pose for robot, such as i moment binocular image information, j moment binoculars
Image information, i moment robots pose；
Pose change is calculated, pose change is calculated according to present frame binocular image information and previous frame binocular image information
Amount；
Current pose is calculated, according to pose variable quantity and the current pose of upper pose calculating robot.
Kinematic constraint is calculated, kinematic constraint is calculated according to odometer information and gyroscope information；Preferably, step calculates fortune
Moving constraint includes calculating the first kinematic constraint, obtains odometer information and the relative motion of gyroscope information computer device people.
In one embodiment, using binocular camera coordinate system as robot coordinate system, because odometer information sample frequency and
The sample frequency of gyroscope information is higher than the frequency acquisition of binocular camera image, by odometer and the high frequency signals of gyroscope
For low frequency signal, odometer information and gyroscope information is provided kinematic constraint, it is preferable that calculate the first kinematic constraint include with
Lower step：
Data conversion, obtains odometer data sequence, mileage is counted by the wheel type encoder being installed on robot chassis
Include multiple wheeled odometer speed datas with timestamp according to sequence, control robot motion for a period of time, wheeled coding
Device gathers odometer information, and odometer information includes the speed of robot left driving wheel and the speed of right driving wheel, by being installed in
Electronic gyroscope in robot obtains gyro data sequence, obtain gyro data sequence include it is multiple with timestamp
Gyroscope angular velocity data, by the motion model of mobile platform by wheeled odometer speed data and gyroscope angular velocity data
The mileage for being converted to mobile platform counts and gyro data, and the foundation of motion model has had the theories of many maturations, this
Place does not add to repeat, and transfer process is as follows：
vm=v+nv
ωm=ω+bω+nω
Wherein, v is speed actual value, and ω is angular speed actual value, vmFor velocity measurement, ωmFor angular velocity measurement value,
vlFor left driving wheel rotating speed, vrFor right driving wheel rotating speed, nvFor the measurement noise of speed, nωFor the measurement noise of angular speed, bωFor
Fixed bias in gyroscope angular velocity measurement, bωChange over time, and it is 0 that variable quantity, which obeys average, and variance isHeight
This distribution, v are derived by odometer information and odometer motion model.
Rate integrating, counts the mileage of mobile platform and gyro data integrates respectively, obtains rate integrating
Integrated with angular speed, integral formula is as follows：
Wherein, RjRotation for robot at the j moment relative to world coordinate system, PjFor robot at the j moment relative to generation
The speed of boundary's coordinate system, RiRotation for robot at the i moment relative to world coordinate system, Exp () are to reflect rotating vector
Spin matrix is mapped to, because of bωChange is slow, with i moment bωApproximate j moment bω, the difference of i moment pose and j moment poses is by machine
The observation of two field pictures calculates before and after device people odometer is obtained or matched by iteration nearest neighbor algorithm.
The initial value component of relative motion, separating rate integration and angular speed integration is calculated, obtains pre-integration measured value, separation
The noise component(s) of pre-integration measured value, obtains relative motion measured value, the initial value point in the relative motion constraint that integration is obtained
Amount is separated, and relative motion formula is：
Wherein, Δ PijFor speed pre-integration measured value, the i.e. relative measurement without considering speed initial value, Δ RijFor angle speed
Spend pre-integration measured value, the i.e. relative measurement without considering angular speed initial value, separating rate pre-integration measured value and speed initial value
Relative measurement noise component(s), relative motion measured value is as follows：
Relative motion measured value Gaussian distributed：
Define ηijFor the noise component(s) of relative motion measured value, ΣijFor the covariance of noise vector, ηijIt is as follows：
Preferably, step calculates relative motion measured value and further includes correction angle velocity bias, correction angle rate integrating
Angular speed offset, it is preferable that correction angle velocity bias is specially the Jacobian matrix for obtaining angular speed, according to Jacobean matrix
Battle array correction angle rate integrating, integrates according to modified angular speed and corrects relative motion measured value, obtains and corrects relative motion measurement
Value, eliminates b in relative motion calculating processωCalculation error caused by drift, bωThe Jacobian matrix changed over time is as follows：
Jτ+δτ=(I+Fτδτ)Jτ
Wherein, J0=I, then it is as follows to correct pre-integration measured value：
Preferably, step calculates kinematic constraint and further includes the second kinematic constraint of calculating, acquisition speed integration and angular speed product
The dynamical equation divided, solves covariance, the dynamical equation that rate integrating and angular speed integrate is as follows according to dynamical equation：
According to above-mentioned dynamical equation, solving covariance is：
Pτ+δτ=(I+Fτδτ)Pτ(I+Fτδτ)T+(Gτδτ)Q(Gτδτ)T
Wherein, P0=O9×9, Q is sensor noise matrix, and the present invention proposes gyroscope and odometer relative motion and association side
The method that difference calculates, gyroscope and odometer relative motion and covariance are unrelated with the state before robot, work as in robot
In preceding pose optimization process, it is not necessary to kinematic constraint is recalculated after each iteration, and relative motion meets Gaussian Profile,
Add pose optimization process using relative motion as constraint, solve that environmental characteristic is similar or sparse and environment in there are more
Under the complex scenes such as dynamic object, because the problem of correct data correlation causes positioning failure can not be established.
Optimize current pose, current pose is optimized according to kinematic constraint, obtains the optimal trajectory of robot, it is preferable that step
Suddenly it is specially to optimize current pose according to amendment relative motion measured value and covariance to optimize current pose, obtains robot most
Excellent track, optimal trajectory include each framing bit appearance of robot after optimization, are solved most according to consecutive frame position orientation relation and observation model
Excellent track, specially solves following optimization problem：
Wherein, xiFor i moment robots pose, xjFor j moment robots pose, u is that the device at i moment to j moment controls
Amount, pkK-th of the observation coordinate observed for i moment robots, e (xi,xj, u) and e (xi,pk) it is error function, error function
It is as follows：
Wherein, Ωij、ΩpkTo measure corresponding information matrix, information matrix is the inverse matrix of covariance matrix, information square
Battle array is used for the reliability for measuring difference measurement, can be realized according to the prior art, not add to repeat herein.Based on consecutive frame
Movement relation and accurate observation model, the MAP estimation or maximum of robot trajectory are solved using nonlinear optimization method
Possibility predication, can obtain the precise motion track of robot, using accurate motion model information, it is long-term to reduce robot
The accumulated error of pose estimation is constantly corrected during operation, the optimal trajectory tried to achieve is closer to the actual rail passed by of robot
Mark, preferably basis, and when other sensors error calculated is larger is provided for map splicing, only considers to regard in tradition
On the basis of feeling re-projection error constraint, kinematic constraint is carried out with reference to the movable information that odometer and gyroscope provide, is solved most
Excellent track, makes that the robustness of vision positioning is stronger, and effect of optimization is more preferable, and stability and accuracy higher, solve environmental characteristic
There are under the complex scenes such as more dynamic object in similar or sparse and environment, cause because correct data correlation can not be established
The problem of positioning failure.
A kind of electronic equipment, the equipment include：Processor；Memory；And program, its Program are stored in memory
In, and be configured to be performed by processor, program includes being used to perform a kind of above-mentioned robot localization method；A kind of computer
Readable storage medium storing program for executing, is stored thereon with computer program, and computer program is executed by processor a kind of above-mentioned robot localization side
Method.
A kind of robotic positioning device, as shown in Fig. 2, including：
Obtain the movable information that information module obtains robot, location information includes odometer information, gyroscope information, double
Mesh image information, binocular image information are the image information of binocular camera；Motion estimation module is estimated according to binocular image information
The current pose of robot；Kinematic constraint module calculates kinematic constraint according to odometer information and gyroscope information；Optimization module
Current pose is optimized according to kinematic constraint, obtains the optimal trajectory of robot.
In one embodiment, it is preferable that motion estimation module further includes：Obtain estimated information unit, calculate pose change
Unit and the current pose unit of calculating, obtain estimated information unit and obtain the present frame binocular image information of robot, previous frame
Binocular image information, a upper pose；Pose change unit is calculated according to present frame binocular image information and previous frame binocular image
Information calculates pose variable quantity；Current pose unit is calculated according to pose variable quantity and the present bit of upper pose calculating robot
Appearance.
In one embodiment, it is preferable that kinematic constraint module includes the first kinematic constraint module, the first kinematic constraint module
Including：Date Conversion Unit, rate integrating unit, calculate relative motion unit, and Date Conversion Unit is by odometer information and top
The mileage that spiral shell instrument information is converted to mobile platform counts and gyro data；Odometer of the rate integrating unit to mobile platform
Data and gyro data are integrated respectively, obtain rate integrating and angular speed integration；Calculate relative motion unit separation speed
The initial value component of degree integration and angular speed integration, obtains pre-integration measured value, separates the noise component(s) of pre-integration measured value, obtains
Relative motion measured value.Preferably, the first kinematic constraint module further includes amending unit, and amending unit obtains the refined of angular speed can
Than matrix, according to Jacobian matrix correction angle rate integrating, integrated according to modified angular speed and correct relative motion measured value, obtained
Relative motion measured value must be corrected.
In one embodiment, it is preferable that kinematic constraint module includes the second kinematic constraint module, the second kinematic constraint module
Acquisition speed integrates and the dynamical equation of angular speed integration, and solves covariance according to dynamical equation.
The present invention provides a kind of robot localization method, by obtaining movable information, estimates robot according to movable information
Current pose, odometer and gyroscope provide kinematic constraint, using the current pose of kinematic constraint optimization robot, obtain machine
The optimal trajectory of people；The present invention relates to a kind of robotic positioning device；The invention further relates to electronic equipment and readable storage medium storing program for executing,
For performing a kind of robot localization method；The present invention perceives external environment condition, odometer and gyro by binocular vision sensor
Instrument perceives robot displacement, strengthens the robustness of vision positioning method, there is provided and more preferable kinematic constraint, positioning cost is low,
The accumulated error of pose estimation is constantly corrected during robot longtime running, it is similar or diluter to solve environmental characteristic
Dredge, there are under the complex scenes such as more dynamic object in environment, cause what positioning failed because correct data correlation can not be established
Problem.
More than, it is only presently preferred embodiments of the present invention, not makees limitation in any form to the present invention；All one's own professions
The those of ordinary skill of industry can swimmingly implement the present invention shown in by specification attached drawing and above；But all to be familiar with sheet special
The technical staff of industry without departing from the scope of the present invention, is made a little using disclosed above technology contents
The equivalent variations of variation, modification and evolution, are the equivalent embodiment of the present invention；Meanwhile all substantial technologicals according to the present invention
Variation, modification and evolution of any equivalent variations made to above example etc., still fall within technical scheme
Within protection domain.
Claims (15)
- A kind of 1. robot localization method, it is characterised in that comprise the following steps：Obtain movable information, obtain the movable information of robot, the movable information include odometer information, gyroscope information, Binocular image information, the binocular image information are the image information of binocular camera；Estimate current pose, the current pose of robot is estimated according to binocular image information；Kinematic constraint is calculated, kinematic constraint is calculated according to odometer information and gyroscope information；Optimize current pose, the current pose is optimized according to the kinematic constraint, obtains the optimal trajectory of robot.
- A kind of 2. robot localization method as claimed in claim 1, it is characterised in that：The step estimates that current pose is specific Comprise the following steps：Estimated information is obtained, obtains the present frame binocular image information, previous frame binocular image information, a upper pose of robot；Pose change is calculated, calculating pose according to the present frame binocular image information and the previous frame binocular image information becomes Change amount；Current pose is calculated, according to the pose variable quantity and the current pose of the upper pose calculating robot.
- A kind of 3. robot localization method as claimed in claim 1, it is characterised in that：The step, which calculates kinematic constraint, to be included The first kinematic constraint is calculated, obtains the relative motion of the odometer information and the gyroscope information computer device people.
- A kind of 4. robot localization method as claimed in claim 3, it is characterised in that：The first kinematic constraint of the calculating is specific Comprise the following steps：Data conversion, the mileage that the odometer information and the gyroscope information are converted to mobile platform is counted and gyro Instrument data；Rate integrating, counts the mileage of the mobile platform and gyro data integrates respectively, obtains rate integrating Integrated with angular speed；Relative motion is calculated, the initial value component of the rate integrating and angular speed integration is separated, obtains pre-integration measured value, The noise component(s) of the pre-integration measured value is separated, obtains relative motion measured value.
- A kind of 5. robot localization method as claimed in claim 4, it is characterised in that：The step calculates kinematic constraint and also wraps Include and calculate the second kinematic constraint, the dynamical equation of the rate integrating and angular speed integration is obtained, according to the dynamic side Journey solves covariance.
- A kind of 6. robot localization method as claimed in claim 5, it is characterised in that：The step rate integrating, which further includes, to be repaiied Angular offset, corrects the angular speed offset of the angular speed integration.
- A kind of 7. robot localization method as claimed in claim 6, it is characterised in that：The correction angle velocity bias is specific To obtain the Jacobian matrix of angular speed, the angular speed is corrected according to the Jacobian matrix and is integrated, according to modified angle speed Degree integration corrects the relative motion measured value, obtains and corrects relative motion measured value.
- A kind of 8. robot localization method as claimed in claim 7, it is characterised in that：The current pose of optimization order is specific To optimize the current pose according to the amendment relative motion measured value and the covariance, the optimal rail of robot is obtained Mark.
- 9. a kind of electronic equipment, it is characterised in that including：Processor；Memory；And program, wherein described program is stored in the memory, and is configured to be held by processor OK, described program includes being used for the method described in perform claim requirement 1-8 any one.
- 10. a kind of computer-readable recording medium, is stored thereon with computer program, it is characterised in that：The computer program It is executed by processor the method as described in claim 1-8 any one.
- A kind of 11. robotic positioning device, it is characterised in that including：Obtain information module：For obtaining the movable information of robot, the movable information includes odometer information, gyroscope is believed Breath, binocular image information, the binocular image information are the image information of binocular camera；Motion estimation module：For estimating the current pose of robot according to the binocular image information；Kinematic constraint module：For calculating kinematic constraint according to the odometer information and the gyroscope information；Optimization module：For optimizing the current pose according to the kinematic constraint, the optimal trajectory of robot is obtained.
- 12. a kind of robotic positioning device as claimed in claim 11, it is characterised in that the motion estimation module is also wrapped Include：Obtain estimated information unit：For obtain the present frame binocular image information of robot, previous frame binocular image information, One pose；Calculate pose change unit：Based on according to the present frame binocular image information and the previous frame binocular image information Calculate pose variable quantity；Calculate current pose unit：For the present bit according to the pose variable quantity and the upper pose calculating robot Appearance.
- 13. a kind of robotic positioning device as claimed in claim 11, it is characterised in that the kinematic constraint module includes the One kinematic constraint module, the first kinematic constraint module include：Date Conversion Unit：Mileage for the odometer information and the gyroscope information to be converted to mobile platform counts According to and gyro data；Rate integrating unit：For being counted to the mileage of the mobile platform and gyro data integrates respectively, obtain Rate integrating and angular speed integration；Calculate relative motion unit：For separating the initial value component of the rate integrating and angular speed integration, pre- product is obtained Divide measured value, separate the noise component(s) of the pre-integration measured value, obtain relative motion measured value.
- 14. a kind of robotic positioning device as claimed in claim 13, it is characterised in that the kinematic constraint module includes the Two kinematic constraint modules, the second kinematic constraint module are used for the dynamic for obtaining the rate integrating and angular speed integration Equation, covariance is solved according to the dynamical equation.
- A kind of 15. robotic positioning device as claimed in claim 13, it is characterised in that：The first kinematic constraint module is also Including amending unit, the amending unit is used for the Jacobian matrix for obtaining angular speed, and institute is corrected according to the Jacobian matrix Angular speed integration is stated, the relative motion measured value is corrected according to modified angular speed integration, obtains and corrects relative motion measurement Value.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710923970.2A CN107941217B (en) | 2017-09-30 | 2017-09-30 | Robot positioning method, electronic equipment, storage medium and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710923970.2A CN107941217B (en) | 2017-09-30 | 2017-09-30 | Robot positioning method, electronic equipment, storage medium and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107941217A true CN107941217A (en) | 2018-04-20 |
| CN107941217B CN107941217B (en) | 2020-05-22 |
Family
ID=61936012
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710923970.2A Active CN107941217B (en) | 2017-09-30 | 2017-09-30 | Robot positioning method, electronic equipment, storage medium and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107941217B (en) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108888188A (en) * | 2018-06-14 | 2018-11-27 | 深圳市沃特沃德股份有限公司 | Sweeping robot position calibration method and system |
| CN109579824A (en) * | 2018-10-31 | 2019-04-05 | 重庆邮电大学 | A kind of adaptive Kano Meng Te localization method incorporating two-dimensional barcode information |
| CN109737954A (en) * | 2018-12-06 | 2019-05-10 | 成都路行通信息技术有限公司 | A kind of inertial navigation method based on speed and angular speed |
| CN110160522A (en) * | 2019-04-16 | 2019-08-23 | 浙江大学 | A kind of position and orientation estimation method of the vision inertial navigation odometer based on sparse features method |
| CN110293563A (en) * | 2019-06-28 | 2019-10-01 | 炬星科技（深圳）有限公司 | Estimate method, equipment and the storage medium of robot pose |
| CN110411457A (en) * | 2019-08-27 | 2019-11-05 | 纵目科技（上海）股份有限公司 | Localization method, system, terminal and the storage medium merged with vision is perceived based on stroke |
| CN111091621A (en) * | 2019-12-11 | 2020-05-01 | 东南数字经济发展研究院 | Binocular vision synchronous positioning and composition method, device, equipment and storage medium |
| CN111256709A (en) * | 2020-02-18 | 2020-06-09 | 北京九曜智能科技有限公司 | Vehicle dead reckoning positioning method and device based on encoder and gyroscope |
| CN112212852A (en) * | 2019-07-12 | 2021-01-12 | 阿里巴巴集团控股有限公司 | Positioning method, mobile device and storage medium |
| CN112291701A (en) * | 2019-07-25 | 2021-01-29 | 科沃斯商用机器人有限公司 | Positioning verification method, positioning verification device, robot, external equipment and storage medium |
| CN112394190A (en) * | 2019-08-15 | 2021-02-23 | 纳恩博（北京）科技有限公司 | Method and device for determining angular velocity, storage medium, and electronic device |
| CN113538579A (en) * | 2021-07-14 | 2021-10-22 | 浙江大学 | Mobile robot positioning method based on unmanned aerial vehicle map and ground binocular information |
| CN113658260A (en) * | 2021-07-12 | 2021-11-16 | 南方科技大学 | Robot pose calculation method and system, robot and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| CN104501814A (en) * | 2014-12-12 | 2015-04-08 | 浙江大学 | Attitude and position estimation method based on vision and inertia information |
| CN106052683A (en) * | 2016-05-25 | 2016-10-26 | 速感科技（北京）有限公司 | Robot motion attitude estimating method |
| CN205898143U (en) * | 2016-07-20 | 2017-01-18 | 山东鲁能智能技术有限公司 | Robot navigation system based on machine vision and laser sensor fuse |
| EP3159123A1 (en) * | 2014-06-17 | 2017-04-26 | Yujin Robot Co., Ltd. | Device for controlling driving of mobile robot having wide-angle cameras mounted thereon, and method therefor |
| CN107193279A (en) * | 2017-05-09 | 2017-09-22 | 复旦大学 | Robot localization and map structuring system based on monocular vision and IMU information |
-
2017
- 2017-09-30 CN CN201710923970.2A patent/CN107941217B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| EP3159123A1 (en) * | 2014-06-17 | 2017-04-26 | Yujin Robot Co., Ltd. | Device for controlling driving of mobile robot having wide-angle cameras mounted thereon, and method therefor |
| CN104501814A (en) * | 2014-12-12 | 2015-04-08 | 浙江大学 | Attitude and position estimation method based on vision and inertia information |
| CN106052683A (en) * | 2016-05-25 | 2016-10-26 | 速感科技（北京）有限公司 | Robot motion attitude estimating method |
| CN205898143U (en) * | 2016-07-20 | 2017-01-18 | 山东鲁能智能技术有限公司 | Robot navigation system based on machine vision and laser sensor fuse |
| CN107193279A (en) * | 2017-05-09 | 2017-09-22 | 复旦大学 | Robot localization and map structuring system based on monocular vision and IMU information |
Non-Patent Citations (2)
| Title |
|---|
| CHRISTIAN FORSTER,ET ALL: "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", 《ROBOTICS: SCIENCE AND SYSTEMS XI，ROMA,ITALY，2015》 * |
| LI D,ECKENHOFF K,WU K,ET ALL: "Gyro-Aided Camera-Odometer Online Calibration and Localization", 《2017 AMERICAN CONTROL CONFERENCE(ACC),IEEE,2017》 * |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108888188A (en) * | 2018-06-14 | 2018-11-27 | 深圳市沃特沃德股份有限公司 | Sweeping robot position calibration method and system |
| CN109579824A (en) * | 2018-10-31 | 2019-04-05 | 重庆邮电大学 | A kind of adaptive Kano Meng Te localization method incorporating two-dimensional barcode information |
| CN109579824B (en) * | 2018-10-31 | 2022-12-27 | 重庆邮电大学 | Self-adaptive Monte Carnot positioning method integrating two-dimensional code information |
| CN109737954A (en) * | 2018-12-06 | 2019-05-10 | 成都路行通信息技术有限公司 | A kind of inertial navigation method based on speed and angular speed |
| CN110160522A (en) * | 2019-04-16 | 2019-08-23 | 浙江大学 | A kind of position and orientation estimation method of the vision inertial navigation odometer based on sparse features method |
| CN110293563A (en) * | 2019-06-28 | 2019-10-01 | 炬星科技（深圳）有限公司 | Estimate method, equipment and the storage medium of robot pose |
| CN110293563B (en) * | 2019-06-28 | 2022-07-26 | 炬星科技（深圳）有限公司 | Method, apparatus, and storage medium for estimating pose of robot |
| CN112212852A (en) * | 2019-07-12 | 2021-01-12 | 阿里巴巴集团控股有限公司 | Positioning method, mobile device and storage medium |
| CN112291701A (en) * | 2019-07-25 | 2021-01-29 | 科沃斯商用机器人有限公司 | Positioning verification method, positioning verification device, robot, external equipment and storage medium |
| CN112291701B (en) * | 2019-07-25 | 2023-02-17 | 科沃斯商用机器人有限公司 | Positioning verification method, positioning verification device, robot, external equipment and storage medium |
| CN112394190A (en) * | 2019-08-15 | 2021-02-23 | 纳恩博（北京）科技有限公司 | Method and device for determining angular velocity, storage medium, and electronic device |
| CN110411457A (en) * | 2019-08-27 | 2019-11-05 | 纵目科技（上海）股份有限公司 | Localization method, system, terminal and the storage medium merged with vision is perceived based on stroke |
| CN110411457B (en) * | 2019-08-27 | 2024-04-19 | 纵目科技（上海）股份有限公司 | Positioning method, system, terminal and storage medium based on stroke perception and vision fusion |
| CN111091621A (en) * | 2019-12-11 | 2020-05-01 | 东南数字经济发展研究院 | Binocular vision synchronous positioning and composition method, device, equipment and storage medium |
| CN111256709A (en) * | 2020-02-18 | 2020-06-09 | 北京九曜智能科技有限公司 | Vehicle dead reckoning positioning method and device based on encoder and gyroscope |
| CN113658260A (en) * | 2021-07-12 | 2021-11-16 | 南方科技大学 | Robot pose calculation method and system, robot and storage medium |
| CN113538579A (en) * | 2021-07-14 | 2021-10-22 | 浙江大学 | Mobile robot positioning method based on unmanned aerial vehicle map and ground binocular information |
| CN113538579B (en) * | 2021-07-14 | 2023-09-22 | 浙江大学 | Mobile robot positioning method based on unmanned aerial vehicle map and ground binocular information |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107941217B (en) | 2020-05-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107941217A (en) | A kind of robot localization method, electronic equipment, storage medium, device | |
| CN111024066B (en) | Unmanned aerial vehicle vision-inertia fusion indoor positioning method | |
| CN108253963B (en) | Robot active disturbance rejection positioning method and positioning system based on multi-sensor fusion | |
| CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
| CN107909614B (en) | Positioning method of inspection robot in GPS failure environment | |
| CN106989744A (en) | A kind of rotor wing unmanned aerial vehicle autonomic positioning method for merging onboard multi-sensor | |
| CN106123890A (en) | A kind of robot localization method of Fusion | |
| CN105606127A (en) | Calibration method for relative attitude of binocular stereo camera and inertial measurement unit | |
| CN111795686A (en) | Method for positioning and mapping mobile robot | |
| CN108170297B (en) | Real-time six-degree-of-freedom VR/AR/MR device positioning method | |
| CN112815939B (en) | Pose estimation method of mobile robot and computer readable storage medium | |
| CN109764880A (en) | The vision inertia ranging method and system of close coupling vehicle wheel encoder data | |
| CN107767425A (en) | A kind of mobile terminal AR methods based on monocular vio | |
| CN104715469A (en) | Data processing method and electronic device | |
| CN110533719B (en) | Augmented reality positioning method and device based on environment visual feature point identification technology | |
| CN113503873B (en) | Visual positioning method for multi-sensor fusion | |
| CN113034571B (en) | Object three-dimensional size measuring method based on vision-inertia | |
| CN111307146A (en) | Virtual reality wears display device positioning system based on binocular camera and IMU | |
| CN115371665A (en) | Mobile robot positioning method based on depth camera and inertia fusion | |
| CN114529576A (en) | RGBD and IMU hybrid tracking registration method based on sliding window optimization | |
| CN111998862A (en) | Dense binocular SLAM method based on BNN | |
| CN113218407A (en) | Map generation method and device based on fusion of VIO and satellite navigation system | |
| CN112179373A (en) | Measuring method of visual odometer and visual odometer | |
| CN115218906A (en) | Indoor SLAM-oriented visual inertial fusion positioning method and system | |
| CN108307174A (en) | A kind of depth image sensor precision improvement method and system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |