CN105987697A - Right-angle bend Mecanum wheel type AGV (Automatic Guided Vehicle) navigation and positioning method and system - Google Patents

Abstract

The invention discloses a right-angle bend Mecanum wheel type AGV (Automatic Guided Vehicle) navigation and positioning method, comprising the following steps: firstly, establishing a global coordinate system, an AGV initial state coordinate system and a laser radar coordinate system; establishing a right-angle bend and AGV initial state description model under the global coordinate system according to laser radar line segment features; obtaining AGV rotary speed data through an encoder in an AGV; based on a kinematic model of the Mecanum wheel type AGV, realizing a dead-reckoning algorithm and calculating coordinates and gestures of an AGV mass center in the initial state coordinate system; and finally, calculating the description on an AGV outline and a steering state in the global coordinate system so as to realize positioning. According to the method provided by the invention, a solution is provided for navigating and positioning the Mecanum wheel type AGV under a right-angle bend in the aspect of engineering; and the method is simple and feasible and an inertial sensor does not need to be additionally arranged.

Description

Mecanum wheeled AGV navigation locating method and system under a kind of quarter bend

Technical field

The present invention relates to wheeled mobile robot independent navigation field, realize Mecanum wheeled AGV navigation particularly to one fixed Method for position, is suitable for location and the description of kinestate of the wheeled AGV of Mecanum under indoor quarter bend scene.

Background technology

Along with the continuous progress of robotics, the application of mobile robot is more and more extensive, and its important development direction is complete Autonomy-oriented, i.e. independent navigation.Independent navigation first has to solve the first big problem, i.e. " where am I (I where) ", is About the orientation problem of autonomous mobile robot, also it is to move robot to realize a primary problem of independent navigation, most important An one of function.

Orientation problem can be described as robot and pass through sensor senses environment, sets up rational mathematical model to describe mobile machine The working environment of people, and determine self exact position in the work environment, and kinestate.The location mode of robot takes Certainly in the sensor used, common alignment sensor has inertial sensor, video camera, laser radar, ultrasound wave, infrared Line, gyroscope etc..

Common alignment system has GPS geo-location system, but GPS can be used only in outdoor, and such a at quarter bend The posture information of AGV locally can not be described under scene.Secondly, relatively common also has inertial navigation alignment system, passes based on inertia Sensor realizes the method for dead reckoning.The inertial sensor such as such as speed or accelerometer, electronic compass, gyroscope realizes machine The reckoning of the pose of people.Owing to inertial sensor needs additional configuration, so cost is higher, and it is easily subject to magnetic interference, Projection accuracy is caused large effect.Traditional dead reckoning is based on internal sensor, can not describe the initial shape of AGV State.And coordinate information can only be obtained, AGV profile can not be reflected, between attitude information, and AGV and environment Relation.So under quarter bend, the scene of such a local and inapplicable.

Therefore, it is badly in need of one and is applicable to the wheeled AGV of Mecanum, under the scene of quarter bend local, low cost, practicable Navigation locating method.

Summary of the invention

In view of this, in order to solve appeal problem, the present invention, in the case of fully analyzing quarter bend characteristic, utilizes laser radar Set up quarter bend scene and the description of AGV original state.It is then based on Mecanum wheeled locomotion mechanism kinematics model, The encoder within AGV is utilized to realize the dead reckoning of AGV under quarter bend scene.And on this basis, AGV is described Outline coordinate information and and environment between relation, it is achieved comprehensively position.

It is an object of the invention to be achieved through the following technical solutions:

Mecanum wheeled AGV navigation locating method under a kind of quarter bend that the present invention provides, comprises the following steps:

S1: set up global coordinate system oxy, original state coordinate system o ' x ' y ' and laser radar coordinate system o " x " y "；According to laser thunder Reach line segment feature under global coordinate system in set up to quarter bend and AGV original state descriptive model；

S2: obtain AGV rotary speed data by the encoder within AGV；And it is real based on Mecanum wheeled AGV kinematics model Existing dead reckoning algorithm calculates the coordinate (x ' at original state coordinate system o ' x ' y ' middle AGV barycenter_k,y′_k,θ′_k)；And calculate in the overall situation AGV center-of-mass coordinate and attitude (x in coordinate system_k,y_k,θ_k)；

S3: calculate AGV outline and steering state in global coordinate system and describe.

Further, the foundation of described quarter bend and AGV original state descriptive model specifically comprises the following steps that

S11: at laser radar coordinate system o " x " y " in, by laser radar range data (d₁,d₂...d_m) it is converted into line segment (L₁,L₂,L₃)；

Wherein, (d₁,d₂...d_m) it is laser radar initial range data；L_iIt is expressed as laser radar range image under quarter bend Line segment feature, L_iThe parameter model (θ being described as straight line_i,p_i), θ_iRepresent initial point and do the vertical line of straight line, vertical line and x " axle Angle, p_iDistance for the straight line of initial point；

L₁,L₂,L₃Represent three line segments under quarter bend, (θ₁,p₁)、(θ₂,p₂)、(θ₃,p₃) represent the parameter of corresponding line segment respectively Model；

S12: in global coordinate system oxy, sets up the descriptive model of quarter bend according to below equation:

$\left\{\begin{array}{c}{w}_1=p_1+p_2\\ w_2=\frac{\left|k_2{x}_{12}-y_{12}+b_2\right|}{\sqrt{{k_2}^2+1}}\end{array}\right.;$

Wherein, the coordinate of F point is (w₁,-w₂), the coordinate of E point is (0,0), w₁、w₂For the width of passage, w₁For L₁,L₂ Between distance, w₂For F to L₂Between distance；K₂Represent line segment L₂Slope；b₂Represent line segment L₂Intercept； (x₁₂, y₁₂) represent line segment L₁End point；The coordinate of the descriptive model F point of quarter bend isThe coordinate of E point is (0,0)；

E, F point represents the coordinate of the flex point of quarter bend under world coordinates.

S13: in global coordinate system oxy, at the beginning of quarter bend and AGV in realizing under world coordinates according to following steps respectively Beginning state description；

If θ_s> 0, i.e. AGV is towards the right side, then have:

OrderM₂=l₁ cosθ_s；

$\{\begin{array}{c}{x}_0=w_1-M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\θ}}_0={\mathrm{\θ}}_s={\mathrm{\θ}}_1\end{array};$

If θ_s< 0, i.e. AGV is towards a left side, then have:

OrderM₂=l₁ cosθ_s；

$\{\begin{array}{c}{x}_0=M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_3\end{array};$

Wherein, the axis of AGV and L₁The extended line of line segment intersects at H, and angle is θ_s, (x₀,y₀,θ₀) represent that AGV barycenter O ' exists Initial coordinate in oxy coordinate system；

(θ_i,p_i) represent straight line L_iParameter model (θ_i,p_i)；L is shown as at quarter bend₁,L₂,L₃Article three, line segment；

l₁Represent the half of AGV length；

Further, coordinate and the attitude of described AGV barycenter in global coordinate system specifically comprises the following steps that

S21: set up the kinematics model of the wheeled AGV of Mecanum according to below equation；

Wherein, r is that Mecanum takes turns roller radius, w₁,w₂,w₃,w₄Rotating speed for AGV four-wheel；w₁、w₂、w₃、w₄For pressing According to the fixed cycle T collection value by encoder；

(v_x,v_y, w) representing in initial position co-ordinates system, the speed of AGV barycenter, the most corresponding x direction, y direction, w represents Attitude angle；

S22: calculate in the t=kT moment according to dead reckoning principle, calculates at original state coordinate system o ' x ' y ' middle AGV barycenter Coordinate (x '_k,y′_k,θ′_k) it is:

$\left\{\begin{array}{c}{x^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;x}}_i*{\mathrm{cos\&theta;}}_i+{\mathrm{\&Delta;y}}_i*{\mathrm{sin\&theta;}}_i\\ {y^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;y}}_i*{\mathrm{cos\&theta;}}_i-{\mathrm{\&Delta;x}}_i*{\mathrm{sin\&theta;}}_i\\ {{\mathrm{\&theta;}}^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;\&theta;}}_i\end{array}\right.;$

Wherein, (x '_k,y′_k,θ′_k) represent the coordinate at original state coordinate system o ' x ' y ' middle AGV barycenter.

(Δx_i,Δy_i,Δθ_i) represent within the i-th cycle, the side-play amount of AGV barycenter.

S23: the barycenter calculated in global coordinate system oxy with geocentric coordinate system o ' x ' y ' conversion relational expression according to global coordinate system oxy Coordinate (x_k,y_k,θ_k):

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\&theta;}}_k\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_s& {\mathrm{sin\&theta;}}_s& 0\\ -{\mathrm{sin\&theta;}}_s& {\mathrm{cos\&theta;}}_s& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x^{\&prime;}}_k\\ {y^{\&prime;}}_k\\ {{\mathrm{\&theta;}}^{\&prime;}}_k\end{array}\right]+\left[\begin{array}{c}{x}_0\\ y_0\\ {\mathrm{\&theta;}}_0\end{array}\right]$

Wherein, (x₀,y₀,θ₀) it is the initial coordinate of barycenter under global coordinate system oxy.

(x_k,y_k,θ_k) it is the coordinate of AGV barycenter under global coordinate system oxy.

Further, described specifically comprising the following steps that of describing of AGV outline and steering state in global coordinate system

S31: according to following rotation relationship calculating AGV outline ABCD coordinate under global coordinate system:

The coordinate of A point: (-l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of B point: (l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of C point: (l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)

The coordinate of D point: (-l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)；

Wherein, ABCD is AGV outline coordinate；(x_k,y_k,θ_k) it is center-of-mass coordinate under global coordinate system；

S32: according to below equation calculating AGV steering state in global coordinate system:

$\left\{\begin{array}{c}{\mathrm{dis}}_{A-L_2}=|l_2*\sin \left({\mathrm{\&theta;}}_k\right)+l_1*\cos \left({\mathrm{\&theta;}}_k\right)+y_k|\\ {\mathrm{dis}}_{BC-F}=\frac{\left|J_1{w}_1-J_2{w}_2+J_3\right|}{\sqrt{{J_1}^2+{J_2}^2}}\\ {\mathrm{dis}}_{D-L_3}=-l_2*\cos \left({\mathrm{\&theta;}}_k\right)-l_1*\sin \left({\mathrm{\&theta;}}_k\right)+x_k\end{array}\right.;$

J₁=2l₁*cos(θ_k), J₂=-2l₁*sin(θ_k)；

J₃=-J₂*(l₂*sin(θ_k)-l₁*cos(θ_k)+y_k)-(l₂*cos(θ_k)-l₁*sin(θ_k)+x_k)*J₁；

Wherein, the barrier point that AGV collides in steering procedure；Outline summit A and L₂Limit, side BC and flex point F, D point distance limit L₃；

AGV outline summit A and L₂The distance of axle is

The distance of bend flex point F distance profile BC is dis_BC-F；

D point distance L₃Axle is

Present invention also offers Mecanum wheeled AGV navigation positioning system under a kind of quarter bend, including establishment of coordinate system module, Channel pattern sets up module, module set up by AGV original state descriptive model, data acquisition module, overall situation center-of-mass coordinate calculate mould Block, overall situation outline computing module and overall situation steering state describe computing module；

Described establishment of coordinate system module, is used for setting up global coordinate system oxy, original state coordinate system o ' x ' y ' and laser radar coordinate It is o " x " y "；

Described channel pattern sets up module, sets up quarter bend model according to laser radar line segment feature under global coordinate system；

Module set up by described AGV original state descriptive model, builds according to laser radar line segment feature under global coordinate system Vertical AGV original state descriptive model；

Described data acquisition module, for obtaining AGV rotary speed data by the encoder within AGV；

Described overall situation center-of-mass coordinate computing module, for realizing dead reckoning algorithm based on Mecanum wheeled AGV kinematics model Calculate the coordinate (x of AGV barycenter in global coordinate system oxy_k,y_k,θ_k)；

Described overall situation outline computing module, for calculating AGV outline in global coordinate system；

Described overall situation steering state describes computing module, describes for calculating the steering state in global coordinate system.

Further, the foundation of described quarter bend and AGV original state descriptive model specifically comprises the following steps that

S11: at laser radar coordinate system o " x " y " in, by laser radar range data (d₁,d₂...d_m) it is converted into line segment (L₁,L₂,L₃)；

Wherein, L_iIt is expressed as the laser radar line segment feature of range image, L under quarter bend_iThe parameter model being described as straight line (θ_i,p_i), θ_iRepresent initial point and be the vertical line of straight line, vertical line and x " angle of axle, p_iDistance for the straight line of initial point；

S12: in global coordinate system oxy, calculates w according to below equation₂For F to L₂Between distance:

$\left\{\begin{array}{c}{w}_1=p_1+p_2\\ w_2=\frac{\left|k_2{x}_{12}-y_{12}+b_2\right|}{\sqrt{{k_2}^2+1}}\end{array}\right.;$

Wherein, the coordinate of F point is (w₁,-w₂), the coordinate of E point is (0,0), w₁、w₂For the width of passage, w₁For L₁,L₂ Between distance w₂For F to L₂Between distance；K₂Represent line segment L₂Slope；b₂Represent line segment L₂Intercept； (x₁₂, y₁₂) represent line segment L₁End point；The coordinate of the descriptive model F point of quarter bend isThe coordinate of E point is (0,0)；

E, F point represents the coordinate of the flex point of quarter bend under world coordinates.

S13: in global coordinate system oxy, at the beginning of quarter bend and AGV in realizing under world coordinates according to following steps respectively Beginning state description；

If θ_s> 0, i.e. AGV is towards the right side, then have:

OrderM₂=l₁ cosθ_s；

$\left\{\begin{array}{c}{x}_0=w_1-M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_1\end{array}\right.;$

If θ_s< 0, i.e. AGV is towards a left side, then have:

OrderM₂=l₁ cosθ_s；

$\left\{\begin{array}{c}{x}_0=M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_3\end{array}\right.;$

Wherein, the axis of AGV and L₁The extended line of line segment intersects at H, and angle is θ_s, AGV barycenter O ' is in oxy coordinate system Initial coordinate be (x₀,y₀,θ₀)；

(θ_i,p_i) represent straight line L_iParameter model (θ_i,p_i)；

l₁Represent the half of AGV length；

Further, coordinate and the attitude of described AGV barycenter in global coordinate system specifically comprises the following steps that

S21: set up the kinematics model of the wheeled AGV of Mecanum according to below equation；

Wherein, r is that Mecanum takes turns roller radius, w₁,w₂,w₃,w₄Rotating speed for AGV four-wheel；w₁、w₂、w₃、w₄For pressing According to the fixed cycle T collection value by encoder；

(v_x,v_y, w) represent in initial position co-ordinates system, the speed of AGV barycenter.

S22: calculate in the t=kT moment according to dead reckoning principle, calculates at original state coordinate system o ' x ' y ' middle AGV barycenter Coordinate (x '_k,y′_k,θ′_k) it is:

$\left\{\begin{array}{c}{x^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;x}}_i*{\mathrm{cos\&theta;}}_i+{\mathrm{\&Delta;y}}_i*{\mathrm{sin\&theta;}}_i\\ {y^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;y}}_i*{\mathrm{cos\&theta;}}_i-{\mathrm{\&Delta;x}}_i*{\mathrm{sin\&theta;}}_i\\ {{\mathrm{\&theta;}}^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;\&theta;}}_i\end{array}\right.;$

Wherein, (x '_k,y′_k,θ′_k) represent the coordinate at original state coordinate system o ' x ' y ' middle AGV barycenter.

(Δx_i,Δy_i,Δθ_i) represent within the i-th cycle, the side-play amount of AGV barycenter.

S23: the barycenter calculated in global coordinate system oxy with geocentric coordinate system o ' x ' y ' conversion relational expression according to global coordinate system oxy Coordinate (x_k,y_k,θ_k):

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\&theta;}}_k\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_s& {\mathrm{sin\&theta;}}_s& 0\\ -{\mathrm{sin\&theta;}}_s& {\mathrm{cos\&theta;}}_s& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x^{\&prime;}}_k\\ {y^{\&prime;}}_k\\ {{\mathrm{\&theta;}}^{\&prime;}}_k\end{array}\right]+\left[\begin{array}{c}{x}_0\\ y_0\\ {\mathrm{\&theta;}}_0\end{array}\right]$

Wherein, (x₀,y₀,θ₀) be the initial coordinate of barycenter under global coordinate system oxy, S13 can be calculated.

(x_k,y_k,θ_k) it is the coordinate of AGV barycenter under global coordinate system oxy.

Further, described specifically comprising the following steps that of describing of AGV outline and steering state in global coordinate system

S31: according to following rotation relationship calculating AGV outline ABCD coordinate under global coordinate system:

The coordinate of A point: (-l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of B point: (l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of C point: (l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)

The coordinate of D point: (-l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)；

Wherein, ABCD is AGV outline coordinate；(x_k,y_k,θ_k) it is center-of-mass coordinate under global coordinate system；

S32: according to below equation calculating AGV steering state in global coordinate system:

$\left\{\begin{array}{c}{\mathrm{dis}}_{A-L_2}=|l_2*\sin \left({\mathrm{\&theta;}}_k\right)+l_1*\cos \left({\mathrm{\&theta;}}_k\right)+y_k|\\ {\mathrm{dis}}_{BC-F}=\frac{\left|J_1{w}_1-J_2{w}_2+J_3\right|}{\sqrt{{J_1}^2+{J_2}^2}}\\ {\mathrm{dis}}_{D-L_3}=-l_2*\cos \left({\mathrm{\&theta;}}_k\right)-l_1*\sin \left({\mathrm{\&theta;}}_k\right)+x_k\end{array}\right.;$

J₁=2l₁*cos(θ_k), J₂=-2l₁*sin(θ_k)；

J₃=-J₂*(l₂*sin(θ_k)-l₁*cos(θ_k)+y_k)-(l₂*cos(θ_k)-l₁*sin(θ_k)+x_k)*J₁；

Wherein, the barrier point that AGV collides in steering procedure；Outline summit A and L₂Limit, side BC and flex point F, D point distance limit L₃；

AGV outline summit A and L₂The distance of axle is

The distance of bend flex point F distance profile BC is dis_BC-F；

D point distance L₃Axle is

Owing to have employed technique scheme, present invention have the advantage that:

The method that the present invention provides is to realize Mecanum wheeled AGV navigator fix under quarter bend in engineering to provide a kind of solution Certainly scheme, simple and be not required to additional configuration inertial sensor.Establish quarter bend scene and AGV based on laser radar The descriptive model of initial attitude.And realize dead reckoning by model realization based on Mecanum wheeled AGV kinematics model, build Found the relation when location of AGV, the description of outline and AGV being turned under whole bend scene and between environment.

Other advantages, target and the feature of the present invention will be illustrated to a certain extent in the following description, and at certain In kind of degree, will be apparent to those skilled in the art based on to investigating hereafter, or can be from this Bright practice is instructed.The target of the present invention and other advantages can be realized by description below and obtain.

Accompanying drawing explanation

The accompanying drawing of the present invention is described as follows.

Fig. 1 is algorithm flow chart.

Fig. 2 is the foundation of three coordinate systems.

Fig. 3 is for set up bend descriptive model based on laser radar line segment feature.

Fig. 4 is for set up AGV original state descriptive model based on laser radar line segment feature.

Fig. 5 is to set up outline and the description of steering state.

Detailed description of the invention

The invention will be further described with embodiment below in conjunction with the accompanying drawings.

Embodiment 1

As Figure 1-5, wherein, Fig. 1 is algorithm flow chart.Fig. 2 is the foundation of three coordinate systems.With quarter bend flex point E it is Zero, channel direction is that x-axis sets up oxy coordinate system.With AGV barycenter as zero, axis is axis of ordinates, sets up O ' x ' y ' coordinate system.With laser radar as zero, set up laser radar o " x " y " coordinate system.Fig. 3 is based on laser radar Line segment feature sets up bend descriptive model.Under quarter bend scene, at o " x " y " in coordinate system, through line segment segmentation and fitting a straight line Algorithm, can obtain being converted into line segment (L₁,L₂,L₃)。L_iParameter model (θ for straight line_i,p_i).Fig. 4 is based on laser radar Line segment feature sets up AGV original state descriptive model.In oxy global coordinate system, AGV axis and L₁The extended line phase of line segment Meeting at H, angle is θ_sIf the AGV barycenter O ' initial coordinate in oxy coordinate system is (x₀,y₀,θ₀).Fig. 5 is for setting up externally Profile and the description of steering state.The outline of AGV is ABCD, and under global coordinate system, center-of-mass coordinate is (x_k,y_k,θ_k), outward Profile summit A and L₂The distance of axle isThe distance of bend flex point F distance profile BC is dis_BC-F, D point distance L₃ Axle is

Mecanum wheeled AGV navigation locating method under the quarter bend that the present embodiment provides, utilizes laser radar to set up quarter bend Scene and the description of AGV original state, be then based on Mecanum wheeled locomotion mechanism kinematics model, utilize inside AGV Encoder realize the dead reckoning of AGV under quarter bend scene；By AGV outline coordinate information and and environment between Relational implementation comprehensively position.

The localization method that the present embodiment provides exists three coordinate systems, as it is shown in figure 1, with quarter bend flex point E as zero, Channel direction is that x-axis sets up oxy coordinate system.This coordinate system is referred to as global coordinate system, belongs to absolute coordinate system, is used for realizing the overall situation Location.With AGV barycenter as zero, axis is axis of ordinates, sets up o ' x ' y ' coordinate system, referred to as original state coordinate system. This coordinate system belongs to absolute coordinate system, is used for realizing dead reckoning.With laser radar as zero, set up laser radar o " x " y " Coordinate system, this coordinate system belongs to local coordinate system, for setting up quarter bend scene and the description of AGV initial attitude.

As in figure 2 it is shown, a length of 2l of AGV₁, a width of 2l₂, the narrow a width of w of quarter bend scene₁×w₂, unit cm.O ' x ' y ' coordinate System is θ with the angle of oxy coordinate system_s, o " x " y " has translated up l relative to o ' x ' y ' coordinate system₁。

Specifically comprise the following steps that

(1) set up quarter bend and AGV original state descriptive model based on laser radar line segment feature

Under quarter bend scene, as it is shown on figure 3, at o " x " y " in coordinate system, through line segment segmentation and Algorithm of fitting a straight line, can With by laser radar range data (d₁,d₂...d_m) extracted by line segment feature, extract line segment feature (L₁,L₂,L₃)。L_iFor straight line Parameter model (θ_i,p_i), wherein θ_iRepresent initial point and be the vertical line of straight line, vertical line and x " angle of axle, p_iStraight line for initial point Distance.Its slope intercept model is (k_i,b_i).Initial, the end coordinate of line segment are (x_i1,y_i1)、(x_in,y_in)。

As it is shown on figure 3, in oxy coordinate system, the coordinate of F point is (w₁,-w₂), the coordinate of E point is (0,0).w₁、w₂For The width of passage.w₁For L₁,L₂Between distance, w₂For F to L₂Between distance, then have:

$\left\{\begin{array}{c}{w}_1=p_1+p_2\\ w_2=\frac{\left|k_2{x}_{12}-y_{12}+b_2\right|}{\sqrt{{k_2}^2+1}}\end{array}\right.$

So, can get the coordinate of F point in global coordinate system isThe coordinate of E point is (0,0)。

As shown in Figure 4, in oxy global coordinate system, AGV axis and L₁The extended line of line segment intersects at H, and angle is θ_s, If the initial coordinate that AGV barycenter O ' is in oxy coordinate system is (x₀,y₀,θ₀)。

If θ_s> 0, i.e. AGV is towards the right side, as shown in FIG., then has:

OrderM₂=l₁ cosθ_s

$\left\{\begin{array}{c}{x}_0=w_1-M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_1\end{array}\right.$

If θ_s< 0, i.e. AGV is towards a left side, then have:

OrderM₂=l₁ cosθ_s

$\left\{\begin{array}{c}{x}_0=M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_3\end{array}\right.$

In sum: can under world coordinates in establish quarter bend and the description of AGV original state.

(2) dead reckoning based on kinematics model in global coordinate system

The kinematics model of the wheeled AGV of Mecanum

Wherein r is that Mecanum takes turns roller radius, w₁,w₂,w₃,w₄For the rotating speed of AGV four-wheel, based on the encoder within AGV Can obtain in real time.W is gathered by encoder with the cycle T that certain is fixing₁、w₂、w₃、w₄Value.In the t=kT moment, adopt The value of the rotating speed integrated is as w₁(k),w₂(k),w₃(k),w₄(k)。For AGV displacement under o ' x ' y ' coordinate system Amount, because T is sufficiently small, it is assumed that in cycle T, under original state coordinateFor steady state value, then Have

$\left[\begin{array}{c}{\mathrm{\&Delta;x}}_{o^{\&prime;}{x}^{\&prime;}{y}^{\&prime;}}\\ {\mathrm{\&Delta;y}}_{o^{\&prime;}{x}^{\&prime;}{y}^{\&prime;}}\\ {\mathrm{\&Delta;\&theta;}}_{o^{\&prime;}{x}^{\&prime;}{y}^{\&prime;}}\end{array}\right]=T*\left[\begin{array}{c}{v}_x\\ v_y\\ w\end{array}\right]=\frac{T*r}{4}\left[\begin{array}{cccc}1& 1& 1& 1\\ -1& 1& 1& -1\\ -\frac{1}{l_1+l_2}& \frac{1}{l_1+l_2}& -\frac{1}{l_1+l_2}& \frac{1}{l_1+l_2}\end{array}\right]\left[\begin{array}{c}{w}_1\left(k\right)\\ w_2\left(k\right)\\ w_3\left(k\right)\\ w_4\left(k\right)\end{array}\right]$

Can be in the t=kT moment according to dead reckoning principle, under o ' x ' y ' coordinate system, the coordinate (x ' of AGV barycenter_k,y′_k,θ′_k) it is:

$\left\{\begin{array}{c}{x^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;x}}_i*{\mathrm{cos\&theta;}}_i+{\mathrm{\&Delta;y}}_i*{\mathrm{sin\&theta;}}_i\\ {y^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;y}}_i*{\mathrm{cos\&theta;}}_i-{\mathrm{\&Delta;x}}_i*{\mathrm{sin\&theta;}}_i\\ {{\mathrm{\&theta;}}^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;\&theta;}}_i\end{array}\right.$

Thus recurrence relation model can obtain in real time in o ' x ' y ' coordinate system, the coordinate of AGV barycenter and attitude.By above it is known that Under coordinate system oxy, the initial coordinate of barycenter is (x₀,y₀,θ₀), can obtain according to oxy and o ' x ' y ' coordinate system conversion relational expression, little Car is the coordinate (x of barycenter under oxy coordinate system_k,y_k,θ_k) it is:

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\&theta;}}_k\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_s& {\mathrm{sin\&theta;}}_s& 0\\ -{\mathrm{sin\&theta;}}_s& {\mathrm{cos\&theta;}}_s& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x^{\&prime;}}_k\\ {y^{\&prime;}}_k\\ {{\mathrm{\&theta;}}^{\&prime;}}_k\end{array}\right]+\left[\begin{array}{c}{x}_0\\ y_0\\ {\mathrm{\&theta;}}_0\end{array}\right]$

Thus can obtain in real time in global coordinate system, in AGV navigation procedure, the coordinate of barycenter and attitude.

(3) to AGV outline and the description of steering state in global coordinate system

Under quarter bend so local scene, it is ensured that the property passed through, it is achieved the navigator fix of barycenter is inadequate, it is right to need to set up The description of whole outline and outline and environmental concerns.

As it is shown in figure 5, the outline of AGV is ABCD, under global coordinate system, center-of-mass coordinate is (x_k,y_k,θ_k), saved by upper one Reckoning can obtain.Then can obtain based on rotation relationship:

The coordinate of A point: (-l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of B point: (l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of C point: (l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)

The coordinate of D point: (-l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)

AGV is in steering procedure, and the place collided has at three, i.e. outline summit A and L₂Limit, side BC and flex point F, D point distance limit L₃.Make AGV outline summit A and L₂The distance of axle isBend flex point F distance profile BC away from From for dis_BC-F, D point distance L₃Axle isThen have:

Make J₁=2l₁*cos(θ_k), J₂=-2l₁*sin(θ_k)；

J₃=-J₂*(l₂*sin(θ_k)-l₁*cos(θ_k)+y_k)-(l₂*cos(θ_k)-l₁*sin(θ_k)+x_k)*J₁

$\left\{\begin{array}{c}{\mathrm{dis}}_{A-L_2}=|l_2*\sin \left({\mathrm{\&theta;}}_k\right)+l_1*\cos \left({\mathrm{\&theta;}}_k\right)+y_k|\\ {\mathrm{dis}}_{BC-F}=\frac{\left|J_1{w}_1-J_2{w}_2+J_3\right|}{\sqrt{{J_1}^2+{J_2}^2}}\\ {\mathrm{dis}}_{D-L_3}=-l_2*\cos \left({\mathrm{\&theta;}}_k\right)-l_1*\sin \left({\mathrm{\&theta;}}_k\right)+x_k\end{array}\right.$

Thus, may be implemented in the description to AGV outline and steering state in global coordinate system, it is achieved Camera calibration.

Embodiment 2

Mecanum wheeled AGV navigation locating method under the quarter bend that the present embodiment provides, specifically comprises the following steps that

The first step: set up based on laser radar line segment feature and quarter bend and AGV original state are described；

As it is shown on figure 3, under quarter bend scene, at laser radar coordinate system o " x " y " in, will pass after line segment feature extracts Sensor data are extracted into line segment information (L₁,L₂,L₃)。L_iParameter model (θ for straight line_i,p_i), its slope intercept model is (k_i,b_i).Initial, the end coordinate of line segment are (x_i1,y_i1)、(x_in,y_in)。

(1) description to quarter bend is set up based on laser radar line segment feature

Then can try to achieve channel pattern:

$\left\{\begin{array}{c}{w}_1=p_1+p_2\\ w_2=\frac{\left|k_2{x}_{12}-y_{12}+b_2\right|}{\sqrt{{k_2}^2+1}}\\ F:(p_1+p_2,-\frac{\left|k_2{x}_{12}-y_{12}+b_2\right|}{\sqrt{{k_2}^2+1}})\\ E:(0,0)\end{array}\right.$

(2) description to AGV original state is set up based on laser radar line segment feature

As shown in Figure 4, if the AGV barycenter O ' initial coordinate in oxy coordinate system is (x₀,y₀,θ₀), then can try to achieve AGV initial State:

If q₁> 0, then have:

M₂=l₁ cos q₁

$\left\{\begin{array}{c}{x}_0=w_1-M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_1\end{array}\right.$

If q₁< 0, then have:

M₂=l₁ cos q₃

$\left\{\begin{array}{c}{x}_0=M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_3\end{array}\right.$

Second step: dead reckoning based on kinematics model in global coordinate system

W is gathered by encoder with the cycle T that certain is fixing₁、w₂、w₃、w₄Value.In the t=kT moment, the rotating speed of collection Value is w₁(k),w₂(k),w₃(k),w₄K (), Mecanum wheel roller radius is r, then have AGV center-of-mass coordinate in o ' x ' y ' coordinate system For:

$\left\{\begin{array}{c}{x^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;x}}_i*{\mathrm{cos\&theta;}}_i+{\mathrm{\&Delta;y}}_i*{\mathrm{sin\&theta;}}_i\\ {y^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;y}}_i*{\mathrm{cos\&theta;}}_i-{\mathrm{\&Delta;x}}_i*{\mathrm{sin\&theta;}}_i\\ {{\mathrm{\&theta;}}^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;\&theta;}}_i\end{array}\right.$

Wherein

Thus can obtain the coordinate (x of AGV barycenter under global coordinate system_k,y_k,θ_k) it is:

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\&theta;}}_k\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_s& {\mathrm{sin\&theta;}}_s& 0\\ -{\mathrm{sin\&theta;}}_s& {\mathrm{cos\&theta;}}_s& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x^{\&prime;}}_k\\ {y^{\&prime;}}_k\\ {{\mathrm{\&theta;}}^{\&prime;}}_k\end{array}\right]+\left[\begin{array}{c}{x}_0\\ y_0\\ {\mathrm{\&theta;}}_0\end{array}\right]$

Wherein (x₀,y₀,θ₀) obtained by the first step.

3rd step: to AGV outline and the description of steering state in global coordinate system

As it is shown in figure 5, channel pattern w can be obtained by the first step₁、w₂, E, F coordinate, AGV original state (x₀,y₀,θ₀)。 Can be (x in center-of-mass coordinate under global coordinate system by second step_k,y_k,θ_k), so can be in the overall situation based on the conversion between coordinate axes In coordinate system:

The coordinate of A point: (-l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of B point: (l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of C point: (l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)

The coordinate of D point: (-l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)

Can obtain further, AGV outline summit A and L₂The distance of axleThe distance of bend flex point F distance profile BC dis_BC-F, D point distance L₃Axle

J₁=2l₁*cos(θ_k), J₂=-2l₁*sin(θ_k),

J₃=-J₂*(l₂*sin(θ_k)-l₁*cos(θ_k)+y_k)-(l₂*cos(θ_k)-l₁*sin(θ_k)+x_k)*J₁

$\left\{\begin{array}{c}{\mathrm{dis}}_{A-L_2}=|l_2*\sin \left({\mathrm{\&theta;}}_k\right)+l_1*\cos \left({\mathrm{\&theta;}}_k\right)+y_k|\\ {\mathrm{dis}}_{BC-F}=\frac{\left|J_1{w}_1-J_2{w}_2+J_3\right|}{\sqrt{{J_1}^2+{J_2}^2}}\\ {\mathrm{dis}}_{D-L_3}=-l_2*\cos \left({\mathrm{\&theta;}}_k\right)-l_1*\sin \left({\mathrm{\&theta;}}_k\right)+x_k\end{array}\right..$

Finally illustrating, above example is only in order to illustrate technical scheme and unrestricted, although with reference to preferably implementing The present invention has been described in detail by example, it will be understood by those within the art that, can enter technical scheme Row amendment or equivalent, without deviating from objective and the scope of the technical program, it all should contain the claim in the present invention In the middle of scope.

Claims (8)

1. Mecanum wheeled AGV navigation locating method under a quarter bend, it is characterised in that: comprise the following steps:

S1: set up global coordinate system oxy, original state coordinate system o ' x ' y ' and laser radar coordinate system o " x " y "；According to laser thunder Reach line segment feature under global coordinate system in set up to quarter bend and AGV original state descriptive model；

S2: obtain AGV rotary speed data by the encoder within AGV and realize based on Mecanum wheeled AGV kinematics model Dead reckoning algorithm calculates the coordinate (x ' at original state coordinate system o ' x ' y ' middle AGV barycenter_k,y′_k,θ′_k)；And then calculate in the overall situation AGV center-of-mass coordinate and attitude (x in coordinate system oxy_k,y_k,θ_k)；

S3: calculate AGV outline and steering state in global coordinate system and describe.

2. Mecanum wheeled AGV navigation locating method under quarter bend as claimed in claim 1, it is characterised in that: described directly Curved and AGV original state descriptive model the foundation in angle specifically comprises the following steps that

S11: at laser radar coordinate system o " x " y " in, by laser radar range data (d₁,d₂...d_m) extracted by line segment feature, Extract line segment (L₁,L₂,L₃)；

(d₁,d₂...d_m) it is laser radar initial range data；L_iIt is expressed as laser radar line segment of range image under quarter bend Feature, L_iThe parameter model (θ being described as straight line_i,p_i), θ_iRepresented initial point and be the vertical line of straight line, vertical line and x " angle of axle, p_iDistance for the straight line of initial point；

L₁,L₂,L₃Represent three line segments under quarter bend, (θ₁,p₁)、(θ₂,p₂)、(θ₃,p₃) represent the ginseng of its corresponding line segment respectively Digital-to-analogue type；

S12: in global coordinate system oxy, sets up the descriptive model of quarter bend according to below equation:

$\left\{\begin{array}{c}{w}_1=p_1+p_2\\ w_2=\frac{|k_2{x}_{12}-y_{12}+b_2|}{\sqrt{{k_2}^2+1}}\end{array}\right.;$

Wherein, the coordinate of F point is (w₁,-w₂), the coordinate of E point is (0,0), w₁、w₂For the width of passage, w₁For L₁,L₂ Between distance, w₂For F to L₂Between distance；K₂Represent line segment L₂Slope；b₂Represent line segment L₂Intercept； (x₁₂, y₁₂) represent line segment L₁End point；The coordinate of the descriptive model F point of quarter bend isThe coordinate of E point is (0,0)；

E, F point represents the coordinate of the flex point of quarter bend under world coordinates；

S13: in global coordinate system oxy, retouches AGV original state in realizing under world coordinates according to following steps respectively State；

If θ_s> 0, i.e. AGV is towards the right side, then have:

OrderM₂=l₁cosθ_s；

$\left\{\begin{array}{c}{x}_0=w_1-M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_1\end{array}\right.;$

If θ_s< 0, i.e. AGV is towards a left side, then have:

OrderM₂=l₁cosθ_s；

$\left\{\begin{array}{c}{x}_0=M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_3\end{array}\right.;$

Wherein, the axis of AGV and L₁The extended line of line segment intersects at H, and angle is θ_s, (x₀,y₀,θ₀) represent that AGV barycenter O ' exists Initial coordinate in oxy coordinate system；

(θ_i,p_i) represent straight line L_iParameter model (θ_i,p_i), under quarter bend, there is L₁,L₂,L₃Article three, line segment, (θ₁,p₁)、(θ₂,p₂)、(θ₃,p₃) its parameter model the most corresponding；

l₁Represent the half of AGV length.

3. Mecanum wheeled AGV navigation locating method under quarter bend as claimed in claim 1, it is characterised in that: described In global coordinate system, coordinate and the attitude of AGV barycenter specifically comprise the following steps that

S21: set up the kinematics model of the wheeled AGV of Mecanum according to below equation；

Wherein, r is that Mecanum takes turns roller radius, w₁,w₂,w₃,w₄Rotating speed for AGV four-wheel；w₁、w₂、w₃、w₄For pressing According to the fixed cycle T collection value by encoder；

(v_x,v_y, w) representing in initial position co-ordinates system, the speed of AGV barycenter, the most corresponding x direction, y direction, w represents Attitude angle；

S22: calculate in the t=kT moment according to dead reckoning principle, calculates at original state coordinate system o ' x ' y ' middle AGV barycenter Coordinate (x '_k,y′_k,θ′_k) recurrence formula be:

$\left\{\begin{array}{c}{x^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;x}}_i*{\mathrm{cos\&theta;}}_i+{\mathrm{\&Delta;y}}_i*{\mathrm{sin\&theta;}}_i\\ {y^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;y}}_i*{\mathrm{cos\&theta;}}_i-{\mathrm{\&Delta;x}}_i*{\mathrm{sin\&theta;}}_i\\ {{\mathrm{\&theta;}}^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;\&theta;}}_i\end{array}\right.;$

Wherein, (x '_k,y′_k,θ′_k) represent the coordinate at original state coordinate system o ' x ' y ' middle AGV barycenter；

(Δx_i,Δy_i,Δθ_i) represent within the i-th cycle, the side-play amount of AGV barycenter；

S23: the barycenter calculated in global coordinate system oxy with geocentric coordinate system o ' x ' y ' conversion relational expression according to global coordinate system oxy Coordinate (x_k,y_k,θ_k):

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\&theta;}}_k\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_s& {\mathrm{sin\&theta;}}_s& 0\\ -{\mathrm{sin\&theta;}}_s& {\mathrm{cos\&theta;}}_s& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x^{\&prime;}}_k\\ {y^{\&prime;}}_k\\ {{\mathrm{\&theta;}}^{\&prime;}}_k\end{array}\right]+\left[\begin{array}{c}{x}_0\\ y_0\\ {\mathrm{\&theta;}}_0\end{array}\right]$

Wherein, (x₀,y₀,θ₀) it is the initial coordinate of barycenter under global coordinate system oxy；

(x_k,y_k,θ_k) it is the coordinate of AGV barycenter under global coordinate system oxy.

4. Mecanum wheeled AGV navigation locating method under quarter bend as claimed in claim 1, it is characterised in that: described What in global coordinate system, AGV outline and steering state described specifically comprises the following steps that

S31: according to following rotation relationship calculating AGV outline ABCD coordinate under global coordinate system:

The coordinate of A point: (-l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of B point: (l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of C point: (l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)

The coordinate of D point: (-l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)；

Wherein, ABCD is AGV outline coordinate；(x_k,y_k,θ_k) it is center-of-mass coordinate under global coordinate system；

S32: according to below equation calculating AGV steering state in global coordinate system:

$\left\{\begin{array}{c}{\mathrm{dis}}_{A-L_2}=|l_2*\sin \left({\mathrm{\&theta;}}_k\right)+l_1*\cos \left({\mathrm{\&theta;}}_k\right)+y_k|\\ {\mathrm{dis}}_{BC-F}=\frac{\left|J_1{w}_1-J_2{w}_2+J_3\right|}{\sqrt{{J_1}^2+{J_2}^2}}\\ {\mathrm{dis}}_{D-L_3}=-l_2*\cos \left({\mathrm{\&theta;}}_k\right)-l_1*\sin \left({\mathrm{\&theta;}}_k\right)+x_k\end{array}\right.;$

J₁=2l₁*cos(θ_k), J₂=-2l₁*sin(θ_k)；

J₃=-J₂*(l₂*sin(θ_k)-l₁*cos(θ_k)+y_k)-(l₂*cos(θ_k)-l₁*sin(θ_k)+x_k)*J₁；

Wherein, the barrier point that AGV collides in steering procedure；Outline summit A and L₂Limit, side BC and flex point F, D point distance limit L₃；

AGV outline summit A and L₂The distance of axle is

The distance of bend flex point F distance profile BC is dis_BC-F；

D point distance L₃Axle is

5. Mecanum wheeled AGV navigation positioning system under a quarter bend, it is characterised in that: include establishment of coordinate system module, Channel pattern sets up module, module set up by AGV original state descriptive model, data acquisition module, overall situation center-of-mass coordinate calculate mould Block, overall situation outline computing module and overall situation steering state describe computing module；

Described establishment of coordinate system module, is used for setting up global coordinate system oxy, original state coordinate system o ' x ' y ' and laser radar coordinate It is o " x " y "；

Described channel pattern sets up module, sets up quarter bend model according to laser radar line segment feature under global coordinate system；

Module set up by described AGV original state descriptive model, builds according to laser radar line segment feature under global coordinate system Vertical AGV original state descriptive model；

Described data acquisition module, for obtaining the rotating speed of AGV four wheels by the encoder within AGV；

Described overall situation center-of-mass coordinate computing module, for realizing dead reckoning algorithm based on Mecanum wheeled AGV kinematics model Calculate the coordinate (x of AGV barycenter in global coordinate system oxy_k,y_k,θ_k)；

Described overall situation outline computing module, for calculating AGV outline in global coordinate system；

Described overall situation steering state describes computing module, describes for calculating the steering state in global coordinate system.

6. Mecanum wheeled AGV navigation positioning system under quarter bend as claimed in claim 5, it is characterised in that: described directly Curved and AGV original state descriptive model the foundation in angle specifically comprises the following steps that

S11: at laser radar coordinate system o " x " y " in, by laser radar range data (d₁,d₂...d_m) extracted by line segment feature, Extract line segment (L₁,L₂,L₃)；

Wherein, L_iIt is expressed as the laser radar line segment feature of range image, L under quarter bend_iThe parameter model being described as straight line (θ_i,p_i), θ_iRepresent initial point and be the vertical line of straight line, vertical line and x " angle of axle, p_iFor the distance of the straight line of initial point, (θ₁,p₁)、(θ₂,p₂)、(θ₃,p₃) it is respectively L₁,L₂,L₃Parameter model；

S12: in global coordinate system oxy, sets up the descriptive model of quarter bend according to below equation:

$\left\{\begin{array}{c}{w}_1=p_1+p_2\\ w_2=\frac{\left|k_2{x}_{12}-y_{12}+b_2\right|}{\sqrt{{k_2}^2+1}}\end{array}\right.;$

Wherein, the coordinate of F point is (w₁,-w₂), the coordinate of E point is (0,0), w₁、w₂For the width of passage, w₁For L₁,L₂ Between distance, w₂For F to L₂Between distance；K₂Represent line segment L₂Slope；b₂Represent line segment L₂Intercept； (x₁₂, y₁₂) represent line segment L₁End point；

The coordinate of the descriptive model F point of quarter bend isThe coordinate of E point is (0,0)；

S13: in global coordinate system oxy, at the beginning of quarter bend and AGV in realizing under world coordinates according to following steps respectively Beginning state description；

If θ_s> 0, i.e. AGV is towards the right side, then have:

OrderM₂=l₁cosθ_s；

$\left\{\begin{array}{c}{x}_0=w_1-M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_1\end{array}\right.;$

If θ_s< 0, i.e. AGV is towards a left side, then have:

OrderM₂=l₁cosθ_s；

$\left\{\begin{array}{c}{x}_0=M_1\\ y_0=-(p_2+M_2)\\ {\mathrm{\&theta;}}_0={\mathrm{\&theta;}}_s={\mathrm{\&theta;}}_3\end{array}\right.;$

Wherein, the axis of AGV and L₁The extended line of line segment intersects at H, and angle is θ_s, (x₀,y₀,θ₀) represent AGV barycenter O ' Initial coordinate in oxy coordinate system；

(θ_i,p_i) represent straight line L_iParameter model (θ_i,p_i)；

l₁Represent the half of AGV length.

7. Mecanum wheeled AGV navigation positioning system under quarter bend as claimed in claim 5, it is characterised in that: described In global coordinate system, coordinate and the attitude of AGV barycenter specifically comprise the following steps that

S21: set up the kinematics model of the wheeled AGV of Mecanum according to below equation；

Wherein, r is that Mecanum takes turns roller radius, w₁,w₂,w₃,w₄Rotating speed for AGV four-wheel；w₁、w₂、w₃、w₄For pressing According to the fixed cycle T collection value by encoder；

(v_xv_y,) and w represents in initial position co-ordinates system, the speed of AGV barycenter；

S22: calculate in the t=kT moment according to dead reckoning principle, calculates at original state coordinate system o ' x ' y ' middle AGV barycenter Coordinate (x '_k,y′_k,θ′_k) recurrence formula be:

$\left\{\begin{array}{c}{x^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;x}}_i*{\mathrm{cos\&theta;}}_i+{\mathrm{\&Delta;y}}_i*{\mathrm{sin\&theta;}}_i\\ {y^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;y}}_i*{\mathrm{cos\&theta;}}_i-{\mathrm{\&Delta;x}}_i*{\mathrm{sin\&theta;}}_i\\ {{\mathrm{\&theta;}}^{\&prime;}}_k=\underset{i=0}{\overset{i=k-1}{\&Sigma;}}{\mathrm{\&Delta;\&theta;}}_i\end{array}\right.;$

Wherein, (x '_k,y′_k,θ′_k) represent the coordinate at original state coordinate system o ' x ' y ' middle AGV barycenter；

(Δx_i,Δy_i,Δθ_i) represent within the i-th cycle, the side-play amount of AGV barycenter；

S23: the barycenter calculated in global coordinate system oxy with geocentric coordinate system o ' x ' y ' conversion relational expression according to global coordinate system oxy Coordinate (x_k,y_k,θ_k):

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\&theta;}}_k\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_s& {\mathrm{sin\&theta;}}_s& 0\\ -{\mathrm{sin\&theta;}}_s& {\mathrm{cos\&theta;}}_s& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x^{\&prime;}}_k\\ {y^{\&prime;}}_k\\ {{\mathrm{\&theta;}}^{\&prime;}}_k\end{array}\right]+\left[\begin{array}{c}{x}_0\\ y_0\\ {\mathrm{\&theta;}}_0\end{array}\right]$

Wherein, (x₀,y₀,θ₀) it is the initial coordinate of barycenter under global coordinate system oxy；

(x_k,y_k,θ_k) it is the coordinate of AGV barycenter under global coordinate system oxy.

8. Mecanum wheeled AGV navigation positioning system under quarter bend as claimed in claim 5, it is characterised in that: described What in global coordinate system, AGV outline and steering state described specifically comprises the following steps that

S31: according to following rotation relationship calculating AGV outline ABCD coordinate under global coordinate system:

The coordinate of A point: (-l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of B point: (l₂*cos(θ_k)+l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)+l₁*cos(θ_k)+y_k,θ_k)

The coordinate of C point: (l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,-l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)

The coordinate of D point: (-l₂*cos(θ_k)-l₁*sin(θ_k)+x_k,l₂*sin(θ_k)-l₁*cos(θ_k)+y_k,θ_k)；

Wherein, ABCD is AGV outline coordinate；(x_k,y_k,θ_k) it is center-of-mass coordinate under global coordinate system；

S32: according to below equation calculating AGV steering state in global coordinate system:

$\left\{\begin{array}{c}{\mathrm{dis}}_{A-L_2}=|l_2*\sin \left({\mathrm{\&theta;}}_k\right)+l_1*\cos \left({\mathrm{\&theta;}}_k\right)+y_k|\\ {\mathrm{dis}}_{BC-F}=\frac{\left|J_1{w}_1-J_2{w}_2+J_3\right|}{\sqrt{{J_1}^2+{J_2}^2}}\\ {\mathrm{dis}}_{D-L_3}=-l_2*\cos \left({\mathrm{\&theta;}}_k\right)-l_1*\sin \left({\mathrm{\&theta;}}_k\right)+x_k\end{array}\right.;$

J₁=2l₁*cos(θ_k), J₂=-2l₁*sin(θ_k)；

J₃=-J₂*(l₂*sin(θ_k)-l₁*cos(θ_k)+y_k)-(l₂*cos(θ_k)-l₁*sin(θ_k)+x_k)*J₁；

Wherein, the barrier point that AGV collides in steering procedure；Outline summit A and L₂Limit, side BC and flex point F, D point distance limit L₃；

AGV outline summit A and L₂The distance of axle is

The distance of bend flex point F distance profile BC is dis_BC-F；

D point distance L₃Axle is