CN106871904A - A kind of mobile robot code-disc positioning correction method based on machine vision - Google Patents

Abstract

The invention discloses the present invention relates to belong to robotics, particularly a kind of mobile robot code-disc positioning correction method based on machine vision；The instantaneous discrete motion model of robot is set up by photoelectric coded disk, robot real-time coordinates are positioned；Then, two target dot images of the camera acquisition on robot body, set up robot vision measurement angle model；Subsequently, extended Kalman filter normatron device people's immediate movement variable quantity is set up, and statistical property using photoelectric coded disk obtains the average and variance of change in location；Then, with the method for difference come instead of the value of local derviation, the orientation angle observed quantity of impact point during calculating robot current location；Finally, set up Unscented kalman filtering device model carries out merging the amendment realized to photoelectric coded disk position error to data.

Description

A kind of mobile robot code-disc positioning correction method based on machine vision

Technical field

The present invention relates to belong to robotics, and in particular to localization for Mobile Robot technology and Kalman filtering Information fusion technology, particularly a kind of mobile robot code-disc positioning correction method based on machine vision.

Background technology

Autonomous positioning technology is the basis that mobile robot carries out navigation and motion control, is to improve robot autonomous ability Key technology.It is a kind of most common autonomous positioning mode by odometer positioning for mobile robot.But, with Be there are problems that in the traditional odometer localization method based on the photoelectric coded disk on robot wheel it is many, wherein Including：Robot geometric parameter is inaccurate, the error of code-disc step-by-step counting, wheel-slip cause physical location and calculating is not inconsistent And the error for producing, and track is rugged and rough.These errors can be accumulated, and the mistake for allowing can be exceeded after a period of time Differ from scope and cause positioning to fail.Therefore, there has been proposed visual odometry technology.The technology is by calculating front and rear two field pictures Optical flow field or part matching characteristic point three-dimensional coordinate, realize estimation to body movement parameter.

It is that the B nuclear power plant working robots of CN 101774170 and its control system belong to machine in Chinese patent grant number People and automation equipment field.The nuclear power plant working robot is crawler-type mobile manipulator, is put down by the movement of double track drives The manipulator composition of platform and its four-degree-of-freedom of carrying, can move, and have manual remote control and autonomous control inside nuclear power station Two kinds of control models, remote control is carried out using wirelessly or non-wirelessly mode to it.The control of described nuclear power plant working robot System is divided into upper monitoring and planning control system and robot control system two parts, and the two is with the use of control robot Running；But it does not solve still, in the case where impact point is lacked, to be adjusted by autonomous verification, is reduced with this The generation of error.

Chinese patent grant number CN102538781 discloses a kind of mobile robot based on machine vision and inertial navigation fusion Athletic posture method of estimation, its step is：Synchronous acquisition mobile robot binocular camera image and three axle inertial guidance datas；Before extraction Frame image features and match estimation athletic posture afterwards；The angle of pitch and roll angle are calculated using inertial navigation；Set up Kalman filter mould Type merges vision and inertial navigation Attitude estimation；According to estimate variance self-adaptative adjustment filter parameter；The accumulation boat position of attitude rectification Calculate.The present invention proposes real-time extension Kalman filter Attitude estimation model, is made using inertial navigation combination acceleration of gravity direction It is supplement, three direction Attitude estimations of visual odometry is decoupled, corrects the accumulated error of Attitude estimation；According to motion shape State adjusts filter parameter using fuzzy logic, realizes that the filtering of self adaptation is estimated, reduces the influence of acceleration noise, effectively Improve the positioning precision and robustness of visual odometry.

But, visual odometry also has certain scope of application.In general, visual odometry is required for substantial amounts of feature Point.But under some scenes, such as moonscape, due to there is the weak texture region of large area so that characteristic point is carried Take inherently one problem.In addition, the matching precision of characteristic point is also a problem.Substantial amounts of characteristic point may result in With including substantial amounts of noise spot in result.This can largely reduce the reliability of method.

Method designed by the present invention is equally to introduce visual information, but has not both needed substantial amounts of characteristic point, is also not required to The three-dimensional information of calculating characteristic point is removed, and only needs to calculate car body to the sight line angle between any several characteristic points, we Referred to as visual protractor；By the way that angle of the robot between identical two characteristic points that diverse location is seen does not become in the same time Change, be allowed to be blended with the location information of photoelectric coded disk using self adaptation Unscented kalman filtering technology, realize to photoelectricity The amendment of coding disk position error.

The content of the invention

In view of this, the purpose of the present invention is directed to the deficiencies in the prior art, there is provided a kind of movement based on machine vision Robot code-disc positioning correction method, setting photoelectric coded disk by way of vision measurement angle is combined, is modified, So as to cause the reduction of error maximum possible in the situation of a small amount of impact point.

To reach above-mentioned purpose, the present invention uses following technical scheme：

Mobile robot code-disc positioning correction method based on machine vision, it is characterised in that comprise the following steps：

S1：The instantaneous discrete motion model of robot is set up by photoelectric coded disk, robot real-time coordinates are positioned；

S2：By the two target dot images of camera acquisition on robot body, robot vision measurement angle is set up Degree model；

S3：Extended Kalman filter normatron device people's immediate movement variable quantity is set up, and using the system of photoelectric coded disk Meter characteristic obtains the average and variance of change in location；

S4：With the method for difference come instead of the value of local derviation, the observed quantity of impact point during calculating robot current location；

S5：Set up Unscented kalman filtering device model to merge the data that S1-S4 is obtained, obtained by photoelectric coded disk first To positional information substantially, impact point observed quantity is estimated by difference on this position, i.e. this position coordinates and target The angle of point, and the determination position of photoelectric coded disk is corrected by the angle of actual observation.

Further, the instantaneous discrete motion model method of robot is set up in the S1 as follows：

1) according to the pulse frequency of the photoelectric coded disk being arranged on robot revolver (L) and right wheel (R), left and right wheelses are calculated Linear velocity v^L、v^RFor：

2) and then show that the angular speed of robot is：

3) the discrete motion equation of robot is：

Two coordinate systems at driving wheel axis midpoint for defining robot initial position are world coordinate system, and robot is in k The coordinate at moment is (x_k,y_k).In formula：f_LAnd f_RIt is respectively the arteries and veins of the photoelectric coded disk for being located at robot left and right sides driving wheel Frequency is rushed, d is the diameter of driving wheel, and L is two wheelspans of driving wheel of robot, and n is driving wheel for the line number of photoelectric coded disk The umber of pulse of the photoelectric coded disk that turns around output, Δ t is time variable, when the driving wheel speed of both sides is differed, robot Movement locus be and moved in a circle centered on one of driving wheel.

Further, set up in the S2 robot vision measurement angle model method be：Set up two impact point A, B, measures the angle of the included angle A OB between impact point A, B and the photocentre O of camera lens, and the computing formula of ∠ AOB cosine is：

By two images of impact point A, B of hypothesis of camera acquisition on robot body, in formula, F is focal length of camera, and Sa, Sb are respectively the projection of impact point A, B in image plane, and So is photocentre O flat in image The projection in face.

Further, in the step 3 by extended Kalman filter be calculated robot kinematical equation and Visual observation equation is as follows respectively：

In formula, f (x_k,u_k) and h (x_k) it is respectively nonlinear motion model and measurement model of the robot at the k moment, w_k With v_kIt is orthogonal white Gaussian noise, x_k=[x, y, θ]^T,

The change u of the robot location obtained here by formula (3)_k=[Δ x_k,Δy_k]^T, and by the fusion meter of photoelectric coded disk Calculation obtains the average and variance of robot location's change.

Further, the observational equation z of the robot vision_k=h (x_k)+v_k, herein, z_k=[cos α 1_k,cosα 2_k,…,cosαn_k]^TThe vector being made up of the included angle cosine the vector of robot location's coordinate to each characteristic point, such as α in Fig. 2_AB1。

Real observational equation is：

But due to not knowing the coordinate of A, B point, thus this equation cannot be directly used to filtering.In spreading kalman filter The use of the purpose of this formula is the observed quantity z of the position estimated in the position of an estimation in ripple_k+1|kBy this amount With actual observation amount z_k+1Difference between (video camera direct measurement is obtained) corrects the position of estimation, makes what is obtained after amendment Position is under statistical significance closer to physical location.Meanwhile, because this equation is substantially nonlinear, in recursive process It also to be used for x_kLocal derviation, in the case of no feature point coordinates, this local derviation cannot obtain actual value, thus nothing Method is used to calculate.Replace the value of local derviation with the method for difference herein, then in x_kPoint does Taylor series expansion, and estimation exists x_k+1|kObserved quantity z during position_k+1|k.Specific formula is as follows：

Further, Unscented kalman filtering device uses standard Unscented kalman filtering device, information fusion side in step 5 Method is：Can just be entered using self adaptation Unscented kalman filtering algorithm with formula (9) and two information of initial point by formula (8) Row filtering is calculated.Design introduces the Unscented kalman filtering device of adaptation mechanism to further improve estimated accuracy.Self adaptation The detailed process of Unscented kalman filtering algorithm is described as follows.

1) calculation procedure of standard Unscented kalman filtering device

(1) original state is defined as follows (k=0):

In formula,It is the desired value of original state, P₀It is initial covariance.

The state of augmentation includes origin, and parameter, with process noise, is defined as

(2) k=1,2 is worked as ..., ∞

A () calculates sigma points, can obtain

(b) predict the step of be

Wherein, Q_kIt is the covariance matrix of process noise, weightsWithComputing formula be

In formula (14), n is the dimension of augmented state；Parameter alpha can control the size of the distributed areas of sigma points, especially when being When the non-linearization degree of system is stronger, the non-local effect in sampling period can be avoided by selecting a preferable decimal；β is One weights of non-negative, the higher order square information for confirming distribution.For a Gaussian prior model parameter β prioritizing selection It is β=2, in order to ensure the Positive of covariance matrix, the selection of covariance adjusting parameter is κ >=0.Remaining new parameter is defined It is as follows：

C () updates step

In formula, R_kIt is the covariance matrix of measurement noise.

Further, Unscented kalman filtering device uses standard Unscented kalman filtering device, information fusion side in step 5 Method is：In order to further improve estimated accuracy, in Unscented kalman filtering device estimation procedure, using a covariance matching Technology, introduces a noise covariance adaptively correcting mechanism.More precisely, the pose sequence based on mobile robot, examines Consider process noise covariance Q_kWith measurement noise covariance R_kART network.Therefore, Q_kWith R_kIt is estimated and updates repeatedly：

In formula,It is the measured pose of mobile robot, Ck is defined as foloows：

In formula,It is mobile robot in kth step appearance evaluated error, C_kIt is robot kth step The approximation of pose covariance matrix, L is the sliding window size matched with covariance.

The present invention blends the information of vision by adapting to Unscented kalman filtering with photoelectric code disk information, first code-disc Itself can obtain positional information substantially, and observed quantity-be exactly this position coordinates is estimated by difference on this position With the angle of impact point, the determination position of code-disc is then corrected by the angle of actual observation.

In addition, the present invention is directed to mobile robot the characteristics of rough ground photoelectric code disk positioning precision difference, using vision The method for carrying out angular surveying, and self adaptation Unscented kalman filtering method is utilized by the information and photoelectric code disk positioning result phase Fusion, and devise experiment and device；The method mainly has three advantages compared with traditional visual odometry technology：One is Without a large amount of characteristic points, so being applied to weak texture region as similar moonscape；Two be vision measurement value accurately and reliably. Its certainty of measurement is solely dependent upon the resolution ratio of image；Three be self adaptation Unscented kalman filtering device estimated accuracy be far above standard Kalman filter, extended Kalman filter and standard Unscented kalman filtering device；It is worth noting that, the method can not be thorough Bottom eliminates the accumulation of position error, but the designed method of the present invention is for improving wheeled mobile robot system accuracy Very simple is effective, in the case of reduces cost, under the mutual synergy by vision data and photoelectric code disk data, The error of positioning is calculated, is timely corrected, greatly reduce error accumulation.

Brief description of the drawings

Fig. 1 is visual angle measuring principle；

Fig. 2 carries out track correct schematic diagram for present invention application vision；

Fig. 3 is fundamental diagram of the present invention.

Specific embodiment

To make the purpose, technical scheme and advantage of the embodiment of the present invention clearer, below in conjunction with the embodiment of the present invention Accompanying drawing 1-3, the technical scheme to the embodiment of the present invention is clearly and completely described.Obviously, described embodiment is this A part of embodiment of invention, rather than whole embodiments.Based on described embodiments of the invention, the common skill in this area The every other embodiment that art personnel are obtained, belongs to the scope of protection of the invention.

Embodiment one

One, photoelectric coded disk positioning

Photoelectric coded disk positioning is realized by robot both sides code wheel reading and its difference of reading；Robot is used at the k moment (x_k,y_k) represent, x, y are the coordinate at the driving wheel axis midpoint of robot two；Bodywork reference frame with robot initial position is as generation Boundary's coordinate system, sets up the kinematical equation of robot.When both sides vehicle wheel rotational speed is differed, car just does circumference around a center Motion, if two-wheeled wheelspan is L, the line number of code-disc is n the umber of pulse of code-disc output (wheel turn around), and wheel diameter passes through for d The pulse frequency f of left and right code-disc_LAnd f_RThe linear velocity v of left and right wheel can be calculated^L、v^RFor：

The angular speed of robot is

The discrete motion equation of robot is

Two, robot vision measurement angle principle

Robot vision measurement angle is exactly in fact one video camera of installation on mobile robot body, as long as mark Point matching The pixel of accurate and camera is sufficiently high, it is possible to think that this angle value has enough precision.Angle is carried out using video camera The general principle of measurement is as shown in Figure 1；Wherein O is photocentre, and A, B are two impact points；So-called visual protractor, be exactly by A, 2 points of imagings on video camera of B, measure the angle of AOB.

If A, B are respectively S in the projection of the plane of delineation_a、S_b, photocentre O is projected as S the plane of delineation_o, f is focal length of camera, Then the computing formula of angle ∠ AOB cosine is：

Obviously, the accuracy of the angle cosine value is only relevant with image resolution ratio.Therefore angular surveying is carried out with video camera, simply It is reliable.

Three, filtering equations

Extended Kalman Filter requirement obtains kinematical equation and observational equation and their the error distributed intelligence of robot； Consider that the nonlinear motion equation and observational equation of robot are as follows：

In formula, f (x_k,u_k) and h (x_k) be respectively robot nonlinear motion model and measurement model, x_k=[x, y, θ]^T,w_kWith v_kIt is orthogonal white Gaussian noise.

The change u of the robot location obtained here by formula (3)_k=[Δ x_k,Δy_k]^T, and it is special by the statistics of photoelectric coded disk Property obtains the average and variance of change in location.

Four, robot vision metrical information

The observational equation z of robot vision_k=h (x_k)+v_k, herein, z_k=[cos α 1_k,cosα2_k,…,cosαn_k]^TBe by The vector that robot location's coordinate is constituted to the included angle cosine between the vector of each characteristic point, in Fig. 2With

Real observational equation is

But due to not knowing the coordinate of A, B point, thus this equation cannot be directly used to filtering in spreading kalman filter It is the observed quantity z estimated in the position of an estimation using the purpose of this formula in ripple_k+1|kBy this amount and reality Deflection observed quantity z_k+1Between difference correct the position of estimation, the orientation angle observed quantity z_k+1, i.e. video camera is direct Measurement is obtained, and makes the position obtained after amendment under statistical significance closer to physical location；Meanwhile, because this equation is obvious It is nonlinear, it is also used in recursive process for x_kLocal derviation, in the case of no feature point coordinates, this local derviation Actual value cannot be obtained, thus is not used to calculate.Replace the value of local derviation with the method for difference herein, then in x_kPoint First order Taylor series expansion is done, is estimated in x_k+1|kObserved quantity z during position_k+1|k.Specific formula is as follows：

Five, carry out information fusion with self adaptation Unscented kalman filtering device

Can just be calculated using self adaptation Unscented kalman filtering by formula (8) and formula (9) and two information of initial point Method is filtered calculating；Design introduces the Unscented kalman filtering device of adaptation mechanism to further improve estimated accuracy.From The detailed process for adapting to Unscented kalman filtering algorithm is described as follows.

1) calculation procedure of standard Unscented kalman filtering device

(1) original state is defined as follows (k=0):

In formula,It is the desired value of original state, P₀It is initial covariance.

The state of augmentation includes origin, and parameter, with process noise, is defined as

(2) k=1,2 is worked as ..., ∞

A () calculates sigma points, can obtain

(b) predict the step of be

Wherein, Q_kIt is the covariance matrix of process noise, weightsWithComputing formula be

In formula (14), n is the dimension of augmented state；Parameter alpha can control the size of the distributed areas of sigma points, especially When the non-linearization degree of system is stronger, the non local effect in sampling period can be avoided by selecting a preferable decimal Should；β is a weights for non-negative, the higher order square information for confirming distribution.It is excellent for a Gaussian prior model parameter β First selection is β=2, and in order to ensure the Positive of covariance matrix, the selection of covariance adjusting parameter is κ >=0；Remaining new ginseng Number is defined as follows：

C () updates step

In formula, R_kIt is the covariance matrix of measurement noise.

2) self adaptation Unscented kalman filtering device

In order to further improve estimated accuracy, in Unscented kalman filtering device estimation procedure, using a covariance Matching technique, introduces a noise covariance adaptively correcting mechanism.More precisely, the pose sequence based on mobile robot Row, it is considered to process noise covariance Q_kWith measurement noise covariance R_kART network；Therefore, Q_kWith R_kBe estimated with repeatedly Update：

In formula,It is the measured pose of mobile robot, C_kIt is defined as foloows：

In formula,It is mobile robot in kth step appearance evaluated error, C_kIt is robot kth step The approximation of pose covariance matrix, L is the sliding window size matched with covariance.

The present invention blends the information of vision by adapting to Unscented kalman filtering with photoelectric code disk information, first code-disc Itself can obtain positional information substantially, and observed quantity-be exactly this position coordinates is estimated by difference on this position With the angle of impact point, the determination position of code-disc is then corrected by the angle of actual observation.

In addition, the present invention is directed to mobile robot the characteristics of rough ground photoelectric code disk positioning precision difference, using vision The method for carrying out angular surveying, and self adaptation Unscented kalman filtering method is utilized by the information and photoelectric code disk positioning result phase Fusion, and devise experiment and device；The method mainly has three advantages compared with traditional visual odometry technology：One is Without a large amount of characteristic points, so being applied to weak texture region as similar moonscape；Two be vision measurement value accurately and reliably. Its certainty of measurement is solely dependent upon the resolution ratio of image；Three be self adaptation Unscented kalman filtering device estimated accuracy be far above standard Kalman filter, extended Kalman filter and standard Unscented kalman filtering device；It is worth noting that, the method can not be thorough Bottom eliminates the accumulation of position error, but the designed method of the present invention is for improving wheeled mobile robot system accuracy Very simple is effective, in the case of reduces cost, under the mutual synergy by vision data and photoelectric code disk data, The error of positioning is calculated, is timely corrected, greatly reduce error accumulation.

Finally illustrate, the above embodiments are merely illustrative of the technical solutions of the present invention and it is unrestricted, this area is common Other modifications or equivalent that technical staff is made to technical scheme, without departing from technical solution of the present invention Spirit and scope, all should cover in the middle of scope of the presently claimed invention.

Claims (7)

1. a kind of mobile robot code-disc positioning correction method based on machine vision, it is characterised in that comprise the following steps：

S1：The instantaneous discrete motion model of robot is set up by photoelectric coded disk, robot real-time coordinates are positioned；

S2：By the two target dot images of camera acquisition on robot body, robot vision measurement angle is set up Degree model；

S3：Extended Kalman filter normatron device people's immediate movement variable quantity is set up, and using the system of photoelectric coded disk Meter characteristic obtains the average and variance of change in location；

S4：With the method for difference come instead of the value of local derviation, the orientation angle observation of impact point during calculating robot current location Amount；

S5：Set up Unscented kalman filtering device model to merge the data that S1-S4 is obtained, obtained by photoelectric coded disk first To positional information substantially, impact point observed quantity is estimated by difference on this position, i.e. this position coordinates and target The angle of point, and the determination position of photoelectric coded disk is corrected by the angle of actual observation.

2. the mobile robot code-disc positioning correction method of machine vision is based on as claimed in claim 1, it is characterised in that institute To state that set up the instantaneous discrete motion model method of robot in S1 as follows：

1) according to the pulse frequency of the photoelectric coded disk being arranged on robot revolver and right wheel, the linear velocity of left and right wheelses is calculated v^L、v^RFor：

$\begin{array}{c}{v}_L=\frac{f_L}{n}\mathrm{\π}d\\ v_R=\frac{f_R}{n}\mathrm{\π}d\end{array}---\left(1\right)$

2) and then show that the angular speed of robot is：

$\mathrm{\ω}=\frac{v_R-v_L}{L}---\left(2\right)$

3) the discrete motion equation of robot is：

$\begin{array}{c}{x}_{k+1}=x_k+\frac{L(v_k^R+v_k^L)}{2(v_k^R-v_k^L)}({\mathrm{cos\θ}}_{k+1}-{\mathrm{cos\θ}}_k)\\ y_{k+1}=y_k+\frac{L(v_k^R+v_k^L)}{2(v_k^R-v_k^L)}({\mathrm{sin\θ}}_{k+1}-{\mathrm{sin\θ}}_k)\\ {\mathrm{\θ}}_{k+1}={\mathrm{\θ}}_k+\frac{v_k^R-v_k^L}{L}\mathrm{\Δ}t\end{array}---\left(3\right)$

Two coordinate systems at driving wheel axis midpoint for defining robot initial position are world coordinate system, and robot is at the k moment Coordinate be (x_k, y_k), in formula：f_LAnd f_RIt is respectively the pulse frequency of the photoelectric coded disk for being located at robot left and right sides driving wheel Rate, d is the diameter of driving wheel, and L is two wheelspans of driving wheel of robot, and n is to drive for the line number of incremental digital formula encoder The umber of pulse of photoelectric coded disk output of turning around is taken turns, Δ t is time variable, when the driving wheel speed of both sides is differed, machine The movement locus of people is and is moved in a circle centered on one of driving wheel.

3. the mobile robot code-disc positioning correction method of machine vision is based on as claimed in claim 1, it is characterised in that：Institute State and the method for robot vision measurement angle model is set up in S2 be：Two impact points A, B are set up, impact point A, B is measured and is taken the photograph The angle of the included angle A OB between the photocentre 0 of camera lens, the computing formula of ∠ AOB cosine is：

$\begin{array}{c}\cos \∠AOB=\frac{\left|\right|{\mathrm{OS}}_a|{|}^2+|\left|{\mathrm{OS}}_b\right|{|}^2-\left|\right|S_a{S}_b|{|}^2}{2\left|\right|{\mathrm{OS}}_a\left|\right|\·\left|\right|{\mathrm{OS}}_b\left|\right|}\\ =\frac{2f^2+\left|\right|S_o{S}_a|{|}^2+|\left|S_o{S}_b\right|{|}^2-\left|\right|S_a{S}_b|{|}^2}{2\sqrt{f^2+\left|\right|S_o{S}_a|{|}^2}\sqrt{f^2+\left|\right|S_o{S}_b|{|}^2}}\end{array}---\left(4\right)$

By two images of impact point A, B of hypothesis of camera acquisition on robot body, in formula, f is Focal length of camera, Sa, Sb are respectively the projection of impact point A, B in image plane, and So is photocentre 0 in image plane Projection.

4. the mobile robot code-disc positioning correction method of machine vision is based on as claimed in claim 1, it is characterised in that：Institute The kinematical equation and visual observation equation for being calculated robot by self adaptation Unscented kalman filtering device in S3 is stated to distinguish It is as follows：

$\begin{array}{c}{x}_{k+1}=f(x_k,u_k)+w_k\\ z_k=h\left(x_k\right)+v_k\end{array}---\left(5\right)$

In formula, f (x_k, u_k) and h (x_k) it is respectively nonlinear motion model and measurement model of the robot at the k moment, w_kWith v_k It is orthogonal white Gaussian noise, x_k=[x, y, θ]^T,

$\begin{array}{cc}E\left(w_k{w}_j^T\right)=Q_k{\mathrm{\δ}}_{kj},& Q_k\≥0\\ E\left(v_k{v}_j^T\right)=R_k{\mathrm{\δ}}_{kj},& R_k\≥0\end{array}---\left(6\right)$

The change u of the robot location obtained here by formula (3)_k=[Δ x_k, Δ y_k]^T, and by the fusion meter of photoelectric coded disk Calculation obtains the average and variance of robot location's change.

5. the mobile robot code-disc positioning correction method of machine vision is based on as claimed in claim 1, it is characterised in that：Institute State the observational equation z of robot vision_k=h (x_k)+v_k, herein, z_k=[cos α 1_k, cos α 2_k..., cos α n_k]^TIt is by machine Vector of people's position coordinates to the included angle cosine composition between the vector of each characteristic point；

Real observational equation is：

$\cos \&angle;{\mathrm{\&alpha;}}_{{\mathrm{AO}}_{1}B}=\frac{<O_{1}A,O_{1}B>}{\left|\right|O_{1}A\left|\right|\&CenterDot;\left|\right|O_{1}B\left|\right|},\cos \&angle;{\mathrm{\&alpha;}}_{{\mathrm{AO}}_{2}B}=\frac{<O_{2}A,O_{2}B>}{\left|\right|O_{2}A\left|\right|\&CenterDot;\left|\right|O_{2}B\left|\right|}---\left(7\right)$

Specific formula is as follows：

$\frac{\&part;z_k}{\&part;x_k}=\left[\begin{array}{cc}\frac{\cos \mathrm{\&alpha;}{1}_k-\cos \mathrm{\&alpha;}{1}_{k-1}}{x_k-x_{k-1}}& \frac{\cos \mathrm{\&alpha;}{1}_k-\cos \mathrm{\&alpha;}{1}_{k-1}}{y_k-y_{k-1}}\\ \frac{\cos \mathrm{\&alpha;}{2}_k-\cos \mathrm{\&alpha;}{2}_{k-1}}{x_k-x_{k-1}}& \frac{\cos \mathrm{\&alpha;}{2}_k-\cos \mathrm{\&alpha;}{2}_{k-1}}{y_k-y_{k-1}}\\ .& .\\ .& .\\ .& .\\ \frac{{\mathrm{cos\&alpha;n}}_k-{\mathrm{cos\&alpha;n}}_{k-1}}{x_k-x_{k-1}}& \frac{{\mathrm{cos\&alpha;n}}_k-{\mathrm{cos\&alpha;n}}_{k-1}}{y_k-y_{k-1}}\end{array}\right]---\left(8\right)$

$z_{k+1|k}=\left[\begin{array}{cc}\frac{\cos \mathrm{\&alpha;}{1}_k-\cos \mathrm{\&alpha;}{1}_{k-1}}{x_k-x_{k-1}}& \frac{\cos \mathrm{\&alpha;}{1}_k-\cos \mathrm{\&alpha;}{1}_{k-1}}{y_k-y_{k-1}}\\ \frac{\cos \mathrm{\&alpha;}{2}_k-\cos \mathrm{\&alpha;}{2}_{k-1}}{x_k-x_{k-1}}& \frac{\cos \mathrm{\&alpha;}{2}_k-\cos \mathrm{\&alpha;}{2}_{k-1}}{y_k-y_{k-1}}\\ .& .\\ .& .\\ .& .\\ \frac{{\mathrm{cos\&alpha;n}}_k-{\mathrm{cos\&alpha;n}}_{k-1}}{x_k-x_{k-1}}& \frac{{\mathrm{cos\&alpha;n}}_k-{\mathrm{cos\&alpha;n}}_{k-1}}{y_k-y_{k-1}}\end{array}\right]u_k---\left(9\right).$

6. the mobile robot code-disc positioning correction method of machine vision is based on as claimed in claim 1, it is characterised in that：S5 Middle Unscented kalman filtering device uses standard Unscented kalman filtering device, and information fusion method is：By formula (8) and formula (9) Just calculating can be filtered using self adaptation Unscented kalman filtering algorithm with two information of initial point, self adaptation is without mark card The detailed process of Kalman Filtering algorithm is described as follows；

1) calculation procedure of standard Unscented kalman filtering device

(1) original state is defined as follows (k=0)：

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{x}}_0=E\&lsqb;x_0\&rsqb;\\ P_0=E\&lsqb;(x_0-{\stackrel{\&OverBar;}{x}}_0){(x_0-{\stackrel{\&OverBar;}{x}}_0)}^T\&rsqb;\end{array}\right.---\left(10\right)$

In formula,It is the desired value of original state, P₀It is initial covariance；

The state of augmentation includes origin, and parameter, with process noise, is defined as

$\left\{\begin{array}{c}{\hat{x}}_0^a={\left[\begin{array}{ccc}{\stackrel{\&OverBar;}{x}}_0^T& 0& 0\end{array}\right]}^T\\ P_0^a=diag(P_0,Q_0,R_0)\end{array}\right.---\left(11\right)$

(2) k=1,2 ..., ∞ are worked as

A () calculates sigma points, can obtain

$X_k=\&lsqb;\begin{array}{ccc}{\hat{x}}_k^a& {\hat{x}}_k^a+\mathrm{\&gamma;}\sqrt{P_k^a}& {\hat{x}}_k^a-\mathrm{\&gamma;}\sqrt{P_k^a}\end{array}\&rsqb;---\left(12\right)$

(b) predict the step of be

$\left\{\begin{array}{c}{X}_{k+1,k}^{*}=f(X_k,u_k)\\ {\hat{x}}_{k+1,k}^a=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^m{X}_{k+1,k}^{*}\left(i\right)\\ P_{k,k+1}^a=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^c\&lsqb;X_{k+1,k}^{*}\left(i\right)-{\hat{x}}_{k+1,k}^a\&rsqb;{\&lsqb;X_{k+1,k}^{*}\left(i\right)-{\hat{x}}_{k+1,k}^a\&rsqb;}^T+Q_k\\ X_{k+1,k}=\&lsqb;\begin{array}{ccc}{\hat{x}}_{k+1,k}^a& {\hat{x}}_{k+1,k}^a+\mathrm{\&gamma;}\sqrt{P_{k,k+1}^a}& {\hat{x}}_{k+1,k}^a-\mathrm{\&gamma;}\sqrt{P_{k,k+1}^a}\end{array}\&rsqb;\\ z_{k+1,k}=h\left(X_{k+1,k}\right)\\ {\hat{z}}_{k+1,k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^m{Y}_{k+1,k}\left(i\right)\end{array}\right.---\left(13\right)$

Wherein, Q_kIt is the covariance matrix of process noise, weightsWithComputing formula be

$\left\{\begin{array}{c}{W}_i^m=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}},i=0\\ W_i^c=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;}),i=0\\ W_i^m=W_i^c=\frac{1}{2(n+\mathrm{\&lambda;})},i=1,2,\mathrm{...},2n\end{array}\right.---\left(14\right)$

In formula (14), n is the dimension of augmented state；Parameter alpha can control the size of the distributed areas of sigma points, and β is one non- Negative weights, are β=2 for a Gaussian prior model parameter β selection, in order to ensure the Positive of covariance matrix, The selection of covariance adjusting parameter is κ >=0, and remaining new parameter is defined as follows：

$\left\{\begin{array}{c}\mathrm{\&lambda;}={\mathrm{\&alpha;}}^2(n+\mathrm{\&kappa;})-n\\ \mathrm{\&gamma;}=\sqrt{n+\mathrm{\&lambda;}}\end{array}\right.---\left(15\right)$

C () updates step

$\begin{array}{c}{P}_{yy}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^c\&lsqb;z_{k+1,k}\left(i\right)-{\hat{z}}_{k+1,k}\&rsqb;{\&lsqb;z_{k+1,k}\left(i\right)-{\hat{z}}_{k+1,k}\&rsqb;}^T+R_k\\ P_{xy}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^c\&lsqb;X_{k+1,k}\left(i\right)-{\hat{x}}_{k+1,k}^a\&rsqb;{\&lsqb;z_{k+1,k}\left(i\right)-{\hat{z}}_{k+1,k}\&rsqb;}^T\\ K_k=P_{xy}{P}_{yy}^{-1},{\hat{x}}_{k+1}^a={\hat{x}}_{k+1,k}^a+K_k(z_{k+1}-{\hat{z}}_{k+1,k})\\ P_{k+1}^a=P_{k,k+1}^a-K_k{P}_{yy}{K}_k^T\end{array}---\left(16\right)$

In formula, R_kIt is the covariance matrix of measurement noise.

7. the mobile robot code-disc positioning correction method of machine vision is based on as claimed in claim 6, it is characterised in that：S5 Middle Unscented kalman filtering device uses self adaptation Unscented kalman filtering device, and information fusion method is：Q_kWith R_kBe estimated with repeatedly Update：

$\begin{array}{c}{Q}_k=K_k{C}_k{\left(K_k\right)}^T\\ R_k=C_k+\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^c\&lsqb;z_{k+1,k}\left(i\right)-{\hat{z}}_{k+1,k}\&rsqb;{\&lsqb;z_{k+1,k}\left(i\right)-{\hat{z}}_{k+1,k}\&rsqb;}^T\end{array}---\left(17\right)$

In formula,It is the measured pose of mobile robot, Ck is defined as foloows：

$C_k=\underset{i=k-L+1}{\overset{k}{\mathrm{\&Sigma;}}}{E}_i{\left(E_i\right)}^T---\left(18\right)$

In formula,It is mobile robot in kth step appearance evaluated error, C_kIt is robot kth step The approximation of pose covariance matrix, L is the sliding window size matched with covariance.