Specific embodiment
The application is described in further detail with reference to the accompanying drawings and examples.It is understood that this place is retouched
The specific embodiment stated is used only for explaining related invention, rather than the restriction to the invention.It also should be noted that in order to
Convenient for description, is illustrated only in attached drawing and invent relevant part with related.
It should be noted that in the absence of conflict, the features in the embodiments and the embodiments of the present application can phase
Mutually combination.The application is described in detail below with reference to the accompanying drawings and in conjunction with the embodiments.
Referring to FIG. 1, it illustrates according to one embodiment of the air navigation aid of the automated guided vehicle of the application
Flow chart.This approach includes the following steps:
Step 101, it is based on position and attitude error model, determines the pose in current pose sampling time section automated guided vehicle
Error.
In the present embodiment, homing guidance can be transported at interval of preset duration (also referred to as pose sampling period)
The pose of vehicle is sampled, and determines the pose of the automated guided vehicle in each pose sampling time section.
In the present embodiment, the error of the pose sampling time section of automated guided vehicle can refer to the pose sampling time
The difference of the measured value and actual value of the pose of the automated guided vehicle of the finish time of section.Correspondingly, the pose sampling time
The pose of the automated guided vehicle of section can refer to the pose of the finish time automated guided vehicle of pose sampling time section.
In the present embodiment, can be based on the position and attitude error model of the automated guided vehicle pre-established, determination is being worked as
The position and attitude error of preceding pose sampling time section automated guided vehicle.
Referring to FIG. 2, it illustrates the schematic diagrames of the motion model of automated guided vehicle.
If the barycenter of automated guided vehicle is the M points on the central axes of chassis, spacing is L, radius of wheel between two wheels
For r, automated guided vehicle angular speed is ω, and the left and right wheel angles speed of linear velocity v, automated guided vehicle are respectively ωr,
ωl.(x y θ) indicates that the pose of automated guided vehicle, kinematical equation are as follows:
Wherein,
Formula (2-1) discretization can be obtained into following formula:
Wherein,
In the present embodiment, k can indicate that a pose sampling instant, k+1 can indicate the pose sampling instant that k is indicated
Next pose sampling instant, k and k+1 can be initial time and the finish time of pose sampling time section.
In formula (2-2), x (k), y (k) can indicate pose acquisition moment at a moment k automated guided vehicle barycenter M
The coordinate put, x (k+1), y (k+1) can indicate oneself of next pose acquisition moment at moment k+1 at pose acquisition moment at moment k
The coordinate of dynamic guide transport lorry barycenter M points.θ (k) can indicate the course of pose acquisition k moment, automated guided vehicle moment
Angle, v (k) can indicate that the linear velocity of pose acquisition k moment, M moment point, ω (k) can indicate the pose acquisition moment at moment k certainly
The angular speed of dynamic guide transport lorry, TsIt can indicate the pose sampling period.
In the present embodiment, the number of turns that can turn to motor counts, by the wheel of automated guided vehicle
Installation code-disc, the reduction ratio of the line number of code-disc, radius of wheel, speed reducer can extrapolate each pose sampling time section
Distance interior, that wheel is advanced.S may be usedlAnd SrThe left side wheel and right side wheels of expression automated guided vehicle are one respectively
The distance advanced in a pose sampling time section.
Formula (2-2) can be write as:
Formula (2-3) can indicate to acquire the pose of the automated guided vehicle at moment at moment k and next in pose
The incidence relation of the pose of the automated guided vehicle at a pose acquisition moment at moment k+1.But in automated guided vehicle in reality
During border works, lead to Wheel deflection degree difference, left and right sides vehicle since radius of wheel measures inaccurate, load difference
The distance between wheel measures the factors such as inaccuracy, can lead to the error of automated guided vehicle pose.It therefore, need to be to formula
(2-3) is modified, and obtains following formula:
Wherein,
The automated guided vehicle of code-disc measurement can be indicated in k
The measured value of the pose at moment, you can to indicate survey of the automated guided vehicle in the X-axis at k moment, the position of Y-axis and course angle
Magnitude.
((x (k) y (k) θ (k)) indicates automated guided vehicle in the attained pose at k moment, i.e. automated guided vehicle exists
The X-axis at k moment, the physical location of Y-axis and actual heading angle.
((δ x (k) δ y (k) δ θ (k)) can indicate the measurement error for the pose that k moment code-discs measure.
δSl(k), δ Sr(k) it can indicate respectively in a pose acquisition time section i.e. by the k moment as initial time and k+
1 moment as in the pose acquisition time section of finish time, the left side wheel of automated guided vehicle and the code-disc of right side wheels
Scale factor error, δ L can indicate the measurement error of the spacing of left side wheel and right side wheels.Since δ θ (k) are minimum
Amount, in the present embodiment, correlation values can be arranged as follows:
Cos (δ θ (k))=1, sin (δ θ (k))=δ θ (k), δ θ (k) δ Sl(k)=0, δ θ (k) δ Sr(k)=0,
L(k)>>δL(k)
L (k+1)=L (k), δ Sr(k+1)=δ Sr(k), δ Sl(k+1)=δ Sl(k)。
Following formula can be obtained in simultaneous formula (2-3), (2-4), (2-5):
The constant multiplier that the position and attitude error model for the automated guided vehicle that formula (2-6) indicates mainly considers code-disc misses
The measurement error of poor (mainly being caused by wheel radius error) and the distance of left side wheel and right side wheels.
The automated guided vehicle position and attitude error model that formula (2-6) indicates can describe the k moment and the k+1 moment leads automatically
Draw the incidence relation of the error of the pose of transport vehicle, i.e. the position and attitude error model of automated guided vehicle can be described because of code-disc
Each pose acquisition time section caused by the measurement error of the distance of scale factor error and left side wheel and right side wheels
It is interior, compared to the error of the pose of the increased automated guided vehicle of upper pose acquisition time section.
Since automated guided vehicle is in practical work process, the source of most important position and attitude error be due to loading not
Uniformly or ground is uneven, cause two-wheeled stress it is different caused by skid error.When wheel skids, lead to wheel
The distance of traveling is more than its practical distance turned over.S in formula (2-6)l(k), Sr(k) measured value when skidding is than reality
Actual value is big.
In some optional modes of the present embodiment, a uniaxial gyro may be used come caused by eliminating wheel-slip
The error for the distance that wheel is advanced.The slippery conditions that following manner judges automated guided vehicle may be used:Peace can be obtained
The angular speed of gyro output loaded on automated guided vehicle;Angular speed, zero drift based on gyro and pose sampling period,
Determine the deflection angle of the course angle of gyro to measure;It obtains between the measured value of distance, wheel that the wheel that code-disc measures advances
The measured value of the distance advanced based on wheel of measured value of distance, the distance between wheel measured value, determine that code-disc measures
Course angle deflection angle;When the deflection angle of the course angle of gyro to measure is more than the deflection angle for the course angle that code-disc measures
When, it may be determined that left side wheel is skidded;When the deflection angle of the course angle of gyro to measure is less than the course angle that code-disc measures
Deflection angle when, it may be determined that right side wheels are skidded.
Following formula expression may be used in the measurement model of gyro:
It can indicate that the deflection angle of the course angle of the automated guided vehicle of gyro to measure, Ω (k) can indicate
The angular speed of gyro exports, TsIt can indicate the pose sampling period.In practical applications due to gyro, zero drift is main
Error source, consider that the drift of gyro, the error model of gyro can be expressed as:
εb(k) zero drift of gyro can be indicated.
In some optional realization methods of the present embodiment, following manner may be used and determine vehicle caused by wheel-slip
Take turns the error for the distance advanced:It, can be by actual distance value between wheel and gyro to measure when left side wheel is skidded
The product of the tangent value of the differential seat angle of the deflection angle for the course angle that the deflection angle of course angle is measured with code-disc is beaten as wheel
The error for the distance that wheel is advanced caused by sliding.When right side wheels are skidded in wheel, can by between wheel actually away from
Opposite number from the deflection angle of value and the course angle of gyro to measure and the differential seat angle of the deflection angle of the course angle of code-disc measurement
Tangent value product as caused by wheel-slip wheel advance distance error.
Referring to FIG. 3, the schematic diagram of error caused by wheel-slip it illustrates automated guided vehicle.
Solid line can indicate that the initial position of wheel, course angle are 0 degree, and the wheel of automated guided vehicle both sides is with identical
Speed advance, in a pose sampling time section, when the wheel of side skids, the homing guidance of gyro to measure is transported
The differential seat angle of the deflection angle of the course angle for the automated guided vehicle that the deflection angle and code-disc of the course angle of vehicle measure is △
θ, due to TsVery little, △ θ also very littles, can be handled camber line near linear, the error for the distance that the wheel to skid is advanced
It can be expressed as:
Δ S (k)=Ltan (Δ θ (k)) (2-10)
Along the direction that automated guided vehicle advances, it is just to take the course angle rotated counterclockwise.Gyro can be passed through respectively
It measures the deflection angle of the course angle of automated guided vehicle and measures the inclined of the course angle of automated guided vehicle by code-disc
Gyration.
In a pose acquisition time section, i.e., the position by the k moment as initial time and k+1 moment as finish time
Appearance acquisition time section, the deflection angle of the course angle of the automated guided vehicle of gyro to measure and leading automatically by code-disc measurement
Drawing the deflection angle of the course angle of transport vehicle can divide table to be expressed as:
The automated guided vehicle that the deflection angle of the course angle of the automated guided vehicle of gyro to measure is measured with code-disc
The deviation of deflection angle of course angle can be expressed as:
In a pose sampling time section, when the deflection angle of the course angle of the automated guided vehicle of gyro to measure is big
When the deflection angle of the course angle for the automated guided vehicle that code-disc measures, it is believed that be that left side wheel is skidded.When
The deflection angle of the course angle of the automated guided vehicle of gyro to measure is less than the course for the automated guided vehicle that code-disc measures
When the deflection angle at angle, it is believed that be that right side wheels are skidded.
In the case of the error for the distance that wheel caused by considering wheel-slip is advanced, formula (2-6) can be carried out
Optimization, obtains following formula:
In formula (2-13) after optimization, the error for the distance for indicating that the wheel caused by wheel-slip is advanced is added
Item Δ Sr(k), Δ Sl(k).Course angle is chosen for the deflection angle of the course angle of the automated guided vehicle of gyro to measure.Profit
The automated guided vehicle position and attitude error model indicated with formula (2-13) can further eliminate the vehicle caused by wheel-slip
Take turns the error for the distance advanced.ΔSr(k), Δ Sl(k) value is as follows:
By the combined measurement of gyro and code-disc, formula (2-15) is arrived in conjunction with formula (2-10), is led automatically to establish
Draw the position and attitude error model of the pose (x, y, θ) of transport vehicle.The position and attitude error model of the automated guided vehicle can describe one
In a pose acquisition time section, the incidence relation of the error of the pose of automated guided vehicle, the i.e. automated guided vehicle
Position and attitude error model can describe the distance of the scale factor error because of code-disc, left side wheel and right side wheels measurement error,
Caused by the error for the distance that the wheel because of caused by wheel-slip is advanced in each pose acquisition time section, compared to upper one
The error of the pose of the increased automated guided vehicle of pose acquisition time section.It is missed using the pose of the automated guided vehicle
While differential mode type inhibits the measurement error of the distance of the scale factor error of code-disc, left side wheel and right side wheels, Ke Yijin
One step effectively inhibits error caused by wheel-slip in the case of the unbalanced ground inequality of the capacity of body.
In the present embodiment, the position and attitude error model that can utilize automated guided vehicle, determines that each pose is adopted
Increased position and attitude error in the sample period.It can be based on a upper sampling time section for pre-stored current pose sampling time section
Position and attitude error and current pose sampling time section in increased position and attitude error, obtain current pose sampling time section pose miss
Difference.It, can be according to advance by taking the initial time of current pose acquisition time section and finish time are k moment and k+1 moment as an example
The position and attitude error at k moment and the current increased position and attitude error of acquisition time section are stored, determines the position and attitude error at k+1 moment, so
Afterwards, the position and attitude error at k+1 moment is stored.
In some optional realization methods of the present embodiment, the position of automated guided vehicle can be obtained by Quick Response Code
Appearance;Determine the measurement error of Quick Response Code.For example, Quick Response Code square formation can be laid on the ground that automated guided vehicle travels.It is logical
The camera acquisition Quick Response Code being arranged in automated guided vehicle is crossed, the mark of the road sign point of Quick Response Code can be obtained by analysis
Know, and then obtain the automated guided vehicle measured by Quick Response Code in the position of X-axis, Y-axis, is obtained by analysis of two-dimensional code image
Course angle when passing through the Quick Response Code to vehicle.Pass through the homing guidance with automated guided vehicle that will be measured by Quick Response Code
Transport vehicle is in X-axis, the position of Y-axis and course angle.
It, can be based on the position and attitude error mould of automated guided vehicle in some optional realization methods of the present embodiment
Type, establishes system state equation, and system state equation includes:System state variables, systematic observation matrix, process noise matrix,
System state variables include:Position and attitude error, the angular error of gyro to measure, scale factor error, the left side wheel of code-disc measurement
Measurement error, the zero drift error of gyro with the spacing of right side wheels.Can by the measurement error of code-disc and Quick Response Code or
The measurement error of gyro and Quick Response Code is measured as systematic perspective, establishes systematic observation equation.It is then possible to be based on system mode side
Journey, systematic observation equation, optimal system state variables are estimated using Kalman filtering algorithm.To further eliminate automatic
The accumulated error of the position and attitude error model of guide transport lorry.It, can be with when further eliminating the accumulated error of position and attitude error model
Determine that system state variables, systematic state variable X can be expressed as first:
Wherein, δ x (k) can be with
Indicate x that code-disc measures to displacement error;δ y (k) can indicate y that code-disc measures to displacement error;It can indicate code
The angular error that disk measures,It can indicate the angular error of gyro to measure, δ Sr(k) code-disc of right side wheels can be indicated
Scale factor error, δ Sl(k) it can indicate that the scale factor error of the code-disc of left side wheel, δ L (k) can indicate between two-wheeled
Away from error, εb(k) the zero drift error of gyro can be indicated.After determining system state variables, system can be established
State equation, the state equation of system is as follows:
X (k+1)=AX (k)+Q (2-16), wherein
A can indicate that systematic observation matrix, Q can indicate systematic procedure noise matrix.
According to formula (2-12), the non-zero element that can obtain matrix A is:
A44=1 A48=Ts A55=1 A66=1 A77=1 A88=1
The output equation of the measured value of the pose of the automated guided vehicle measured by Quick Response Code can be expressed as:
Wherein,It can indicate the survey of the pose of the automated guided vehicle measured by Quick Response Code
Magnitude, (X (k), Y (k), θ (k)) can indicate the true pose of automated guided vehicle, (△ Xq(k), △ Yq(k),△θq
(k)) Quick Response Code measurement noise can be indicated.Quick Response Code measurement noise is mainly by the Measurement Resolution of Quick Response Code and installation error
It determines.
It can be measured using the measurement error of code-disc/gyro and Quick Response Code as systematic perspective in conjunction with formula (2-5), establish system
The observational equation of the observational equation of system, system can be expressed as:
Wherein, H is observing matrix, and R is to measure noise matrix.
Wherein,
Classics can be utilized in conjunction with the observational equation formula (2-18) of the state equation formula (2-16) and system of system
Kalman filter equation filters initial value such as formula (2-19) to (2-23) in settingAnd P0It is iterated later, you can estimation
Go out the optimum state X of system, you can to estimate the correlated error in position and attitude error model, and then obtain optimal homing guidance
The pose (x y θ) of transport vehicle.
State one-step prediction:
State estimation:
Filtering gain:Gk=Pk/k-1Hk T(HkPk/k-1Hk T+Rk)-1 (2-21)
Mean square error one-step prediction:Pk/k-1=APk-1AT+Q (2-22)
Estimate mean square error:Pk=(I-GkHk)Pk/k-1 (2-23)
Step 102, the position and attitude error based on current pose sampling time section leads the automatic of current pose sampling time section
The pose for drawing transport vehicle is corrected.
In the present embodiment, by step 101 be based on position and attitude error model, determine current pose sampling time section from
It, can be according to the pose of the automated guided vehicle of current pose sampling time section after the position and attitude error of dynamic guide transport lorry
Measured value and position and attitude error are corrected the pose of the automated guided vehicle of current pose sampling time section.With present bit
It, can be in a step 101 according in advance for the initial time of appearance acquisition time section and finish time are k moment and k+1 moment
The position and attitude error at k moment and the current increased position and attitude error of acquisition time section are stored, determines the position and attitude error at k+1 moment, so
Afterwards, the pose at the k+1 moment of automated guided vehicle can be corrected.
In the present embodiment, above-mentioned steps 101-102 can be executed in each pose sampling time section, in real time
The pose of automated guided vehicle is corrected.
Referring to FIG. 4, it illustrates according to one embodiment of the navigation system of the automated guided vehicle of the application
Structural schematic diagram.The navigation system of automated guided vehicle includes:Position and attitude error determination unit 401, pose correct unit 402.
Wherein, position and attitude error determination unit 401 is configured to the position and attitude error model based on automated guided vehicle, determines in present bit
The position and attitude error of appearance sampling time section automated guided vehicle, wherein position and attitude error model describes each pose sampling time section
The incidence relation of interior increased position and attitude error and position and attitude error related information, position and attitude error related information include:Homing guidance is transported
Wheel row caused by the measurement error of the distance between the scale factor error of the code-disc of the wheel of defeated vehicle, wheel, wheel-slip
Into distance error;Pose correction unit 402 is configured to based on current pose sampling time section automated guided vehicle
Position and attitude error is corrected the pose of the automated guided vehicle of current pose sampling time section.
In some optional realization methods of the present embodiment, the navigation system of automated guided vehicle further includes:It skids
Judging unit (not shown) is configured to obtain the angular speed for the gyro output for being installed on automated guided vehicle;Based on gyro
In angular speed, zero drift and the pose sampling period of output, determine the deflection angle of the course angle of gyro to measure;Code-disc is obtained to survey
The measured value of the distance between the measured value of the distance that the wheel of amount is advanced, wheel;The road advanced based on the wheel that code-disc measures
The measured value of the distance between the measured value of journey, wheel determines the deflection angle for the course angle that code-disc measures;When gyro to measure
When the deflection angle of course angle is more than the deflection angle for the course angle that code-disc measures, determine that left side wheel is skidded;Work as gyro
When the deflection angle of the course angle of measurement is less than the deflection angle for the course angle that code-disc measures, determine that right side wheels are skidded;
Skidding error determination unit (not shown), is configured to when left side wheel is skidded, by the actual distance value between wheel
The tangent value of the angle difference of the deflection angle of the course angle measured with code-disc with the deflection angle of the course angle of gyro to measure
The error for the distance that product is advanced as wheel caused by wheel-slip;It, will be between wheel when right side wheels are skidded
The angle difference of the deflection angle for the course angle that the deflection angle of actual distance value and the course angle of gyro to measure is measured with code-disc
Opposite number tangent value product as caused by wheel-slip wheel advance distance error;Quick Response Code metrical information obtains
Unit (not shown) is taken, Quick Response Code is configured to and obtains in the position of current pose sampling time section automated guided vehicle
Appearance;Determine the measurement error of the pose of the automated guided vehicle measured by Quick Response Code.
In some optional realization methods of the present embodiment, optimum state determination subelement (not shown) is configured to
Position and attitude error model based on automated guided vehicle, establishes system state equation, and system state equation includes:System mode becomes
Amount, systematic observation matrix, process noise matrix, system state variables include:The code-disc of position and attitude error, wheel that code-disc measures
The measurement error of the distance between scale factor error, wheel, the angular error of gyro to measure, the zero drift error of gyro;
The measurement error of code-disc and the measurement error of Quick Response Code are measured as systematic perspective or by the measurement error of gyro and Quick Response Code
Measurement error is measured as systematic perspective, establishes systematic observation equation;Based on system state equation, systematic observation equation, using card
Kalman Filtering algorithm estimates optimal system state variables.
Referring to FIG. 5, it illustrates the structural schematic diagrams according to one embodiment of the automated guided vehicle of the application.
As shown in figure 5, automated guided vehicle includes CPU501, memory 502, gyro 503, code-disc 504, CPU501,
Memory 502, gyro 503, code-disc 504 are connected with each other by bus 505.Gyro 503 can measure automated guided vehicle
The deflection angle of course angle.Code-disc 504 can install the left and right sides wheel for being separately mounted to automated guided vehicle.According to this
The air navigation aid of the automated guided vehicle of application may be implemented as computer program, for example, including in the computer program
The instruction of operation described in above-mentioned steps 101-102.Computer program can be stored in memory 502.Homing guidance is transported
The CPU 501 of defeated vehicle is by calling the computer program stored in memory 502, to determine the position of automated guided vehicle in real time
Appearance error, control automated guided vehicle are in real time corrected pose.
Present invention also provides a kind of computer-readable medium, which can be automated guided vehicle
Included by trolley;Can also be individualism, without in supplying automated guided vehicle trolley.The computer-readable medium
One or more program is carried, when one or more program is executed by automated guided vehicle so that homing guidance
Transport vehicle:Position and attitude error model based on automated guided vehicle is determined and is transported in current pose sampling time section homing guidance
The position and attitude error of vehicle, wherein position and attitude error model describes increased position and attitude error in each pose sampling time section and missed with pose
The incidence relation of poor related information, position and attitude error related information include:The scale of the code-disc of the wheel of automated guided vehicle because
The error for the distance that wheel is advanced caused by number errors, the measurement error of the distance between wheel, wheel-slip;Based on present bit
The position and attitude error of appearance sampling time section automated guided vehicle, to the position of the automated guided vehicle of current pose sampling time section
Appearance is corrected.
It should be noted that above computer readable medium can be computer-readable signal media or computer-readable
Storage medium either the two arbitrarily combines.Computer readable storage medium for example may be-but not limited to-
System, device or the device of electricity, magnetic, optical, electromagnetic, infrared ray or semiconductor, or the arbitrary above combination.It is computer-readable
The more specific example of storage medium can include but is not limited to:Electrical connection, portable computing with one or more conducting wires
Machine disk, hard disk, random access storage device (RAM), read-only memory (ROM), erasable programmable read only memory (EPROM
Or flash memory), optical fiber, portable compact disc read-only memory (CD-ROM), light storage device, magnetic memory device or above-mentioned
Any appropriate combination.In this application, computer readable storage medium can be any include or storage program it is tangible
Medium, the program can be commanded the either device use or in connection of execution system, device.And in this application,
Computer-readable signal media may include in a base band or as the data-signal that a carrier wave part is propagated, wherein carrying
Computer-readable program code.The data-signal of this propagation can be diversified forms, including but not limited to electromagnetic signal,
Optical signal or above-mentioned any appropriate combination.Computer-readable signal media can also be computer readable storage medium with
Outer any computer-readable medium, the computer-readable medium can be sent, propagated or transmitted for by instruction execution system
System, device either device use or program in connection.The program code for including on computer-readable medium can be with
It transmits with any suitable medium, including but not limited to:Wirelessly, electric wire, optical cable, RF etc. or above-mentioned any appropriate
Combination.
Above description is only the preferred embodiment of the application and the explanation to institute's application technology principle.People in the art
Member should be appreciated that invention scope involved in the application, however it is not limited to technology made of the specific combination of above-mentioned technical characteristic
Scheme, while should also cover in the case where not departing from inventive concept, it is carried out by above-mentioned technical characteristic or its equivalent feature arbitrary
Other technical solutions of combination and formation.Such as features described above has similar functions with (but not limited to) disclosed herein
Technical characteristic is replaced mutually and the technical solution that is formed.