CN107346025A

CN107346025A - A kind of double reflecting pole laser positionings and air navigation aid based on filtering

Info

Publication number: CN107346025A
Application number: CN201710572154.1A
Authority: CN
Inventors: 林志赟; 邢浩; 金智
Original assignee: Zhejiang Titanium Robot Ltd By Share Ltd
Current assignee: Zhejiang Titanium Robot Ltd By Share Ltd
Priority date: 2017-07-13
Filing date: 2017-07-13
Publication date: 2017-11-14
Anticipated expiration: 2037-07-13
Also published as: CN107346025B

Abstract

The invention discloses a kind of double reflecting pole laser positionings based on filtering and air navigation aid, it is different from the geometric triangulation positioning in conventional art, the present invention deeply excavates and takes full advantage of the plural domain information of reflected light, the technical bottleneck of three reflecting poles in traditional sense is broken, its service condition is to be detected simultaneously by the data of two reflecting poles, substantially increases the applicability of laser navigation technology；Calculation error caused by measurement error is filtered out by filtering algorithm, is greatly enhanced positioning and navigation accuracy；Finally, multiple reflecting poles may be detected simultaneously by although minimum requirements of the present invention is detects two reflecting poles, during practical application, by design data fusion treatment, the information of reflecting pole can be maximally utilized, so as to further improve precision.

Description

A kind of double reflecting pole laser positionings and air navigation aid based on filtering

Technical field

The invention belongs to intelligent storage logistics field, and in particular to a kind of double reflecting pole laser positionings based on filtering are with leading Boat method.

Background technology

Positioning and airmanship based on laser sensor are the key techniques in the fields such as industrial AGV, intelligent robot, Compared to traditional rail navigation mode, the technology has the advantages that positioning precision is high, flexible and changeable, suitable for complicated, high dynamic In industrial scene.The domestic and international existing laser positioning technology based on reflecting pole, employs geometric triangulation positioning principle, every when per Necessarily require at quarter to be detected simultaneously by the just achievable positioning of three reflecting poles.This quantitative requirement is actually in complex industrial scene It is difficult to meet, the practicality of laser positioning and airmanship is greatly constrained.

The content of the invention

In order to solve the above problems, the present invention provides a kind of new pattern laser positioning and air navigation aid, by extracting and utilizing Plural domain information in laser ranging information, the minimum requirements of three reflecting poles is reduced to two, efficiently solves reflecting pole The technical bottleneck of minimum quantity, substantially increase the applicability of laser navigation technology.

The purpose of the present invention is achieved through the following technical solutions：A kind of double reflecting pole laser positionings based on filtering With air navigation aid, this method includes：

(1) reflecting pole is arranged in industrial environment, presets reflecting pole world coordinates, generates reflecting pole list of coordinates；

(2) laser sensor of installation on a mobile platform radially launches laser to surrounding, and receives reflection laser；

(3) efficient beam of the screening from reflecting pole：By detect reflection laser intensity I and with preset strength threshold value σ Compare, judge that laser irradiation thing is reflecting pole or conventional environment object；

(4) reflecting pole quantity that current time is irradiated to and its relative coordinate of relative laser sensor are determined：According to anti- The continuity of irradiating light beam angle, determine whether same reflecting pole, or, according to the continuous of the reflected beams angle and distance Property, determine whether same reflecting pole；According to the reflected light data for belonging to same reflecting pole, with reference to reflective column radius number According to amendment, relative coordinate of the reflecting pole relative to laser sensor is obtained, and be stored in reflecting pole list；

(5) reflecting pole list is initialized, obtains the world coordinates of at least two reflecting poles：It is artificial to determine that initial position is corresponding Reflecting pole list at least two reflecting poles world coordinates；Or laser sensor obtains at least three in initial position The angle and distance that reflecting pole returns, the distance between reflecting pole two-by-two is calculated, it is anti-with being generated according to reflecting pole list of coordinates Light beam range information matches, and obtains the world coordinates of at least two reflecting poles；

(6) calculated in dynamic process and it is expected reflecting pole list：According to last moment to current time laser sensor position The prediction with angle is put, estimates relative distance and angle between laser sensor and all reflecting poles, and it is reflective to be stored in expectation Colonnade table；

(7) in dynamic process reflecting pole list matching：Current time reflecting pole list is calculated with it is expected reflective colonnade The difference of the difference of distance and angle corresponding to same reflecting pole in table, when the difference apart from its difference and angle is satisfied by predetermined threshold value When, the match is successful；

(8) the laser sensor pose based on double reflecting pole data calculates：Utilize the multifrequency of laser sensor measurement data Domain information, optional two in the reflecting pole that the match is successful：L-th and k-th, and calculate：

z_k=X_k+i*Y_k

z_l=X_l+i*Y_l

Wherein, subscript l and k represent l and k-th of reflecting pole respectively；α and ρ represents relative certainly in laser sensor respectively Under the polar coordinate system of body, the angle and distance of reflecting pole；X and Y is respectively reflecting pole in X and the component of Y-axis；Z is that reflecting pole exists Complex coordinates under world coordinate system；z_k,lFor the laser sensor world coordinates being calculated according to l and k-th of reflecting pole, θ_kIt is the angle according to the laser sensor that the data of k-th of reflecting pole are calculated under world coordinate system；

(9) laser sensor position filtering：Pose prediction is carried out according to mobile platform kinematics model first, then basis The pose of last moment prediction, and the laser sensor measurement data at current time is combined, the position at current time is filtered Ripple, so as to obtain the accurate position and posture of final mobile platform and laser sensor under world coordinate system.

Further, in the step (6), directly using the position and angle of last moment laser sensor as current The prediction at moment, or be predicted using filtering algorithm.

Further, before the step (9), in addition to the step of multiple optimizations：If laser sensor detects three Individual and the above reflecting pole data, data fusion can be carried out according to the pose that any two groups are calculated in multi-group data.

Further, in the step (9), pose prediction is carried out according to mobile platform kinematics model, formula is as follows：

θ_t|t-1=θ_t-1|t-1+Δθ_c

Wherein, input quantity is：Pose (the x of last moment_t-1|t-1,y_t-1|t-1,θ_t-1|t-1), the resolution ratio of motor encoder Resolution, speed reducing ratio speed_reduction_ratio, left and right turns encoder reading count_left this moment, Count_right, reading last_count_right, the last_count_right at a moment in left and right turns encoder, sampling week Phase Δ T, left and right wheels radius r, the angular velocity omega of left and right wheel_L、ω_R, linear velocity v of the mobile platform in last moment_t-1, it is mobile flat Steering angular speed omega of the platform in last moment_t-1, steering angle θ of the mobile platform under last moment world coordinate system_t-1, laser The distance d of two wheel subcenter points of sensor distance；Output quantity is：The increment of motion (Δ x, Δ y, Δ θ) at upper moment relatively, root According to the current time pose (x of last moment data prediction_t|t-1,y_t|t-1,θ_t|t-1)；

The pose predicted according to last moment, with reference to the pose result of calculation at current time, enters to the pose at current time Row filtering, it is specific as follows：If current time only detects 2 reflecting poles, l and k are designated as, using the formula of linear Kalman filter It is as follows：

Wherein, input quantity is：The position of current time predictionThe covariance matrix of last moment P_t-1|t-1, the covariance matrix Q of process noise, the covariance matrix R of measurement noise；Output quantity is：Laser sensor is in the world Coordinate under coordinate systemThe covariance matrix P of current time renewal_t|t；

If current time detects the reflecting pole of 3 and the above, it is filtered using the thought of multiple data fusions；Adopt It is as follows with the formula for passing through sequence Kalman filtering：

For any combination of two in M reflecting pole detecting, calculate：

z_k=X_k+i*Y_k

z_l=X_l+i*Y_l

After traveling through all combinations, obtain：

Beneficial effects of the present invention are as follows：Different from the geometric triangulation positioning in conventional art, the present invention deeply excavates simultaneously The plural domain information of reflected light is taken full advantage of, has broken the technical bottleneck of three reflecting poles in traditional sense, its service condition To be detected simultaneously by the data of two reflecting poles, the applicability of laser navigation technology is substantially increased；Pass through filtering algorithm Calculation error caused by measurement error is filtered out, is greatly enhanced positioning and navigation accuracy；Finally, although minimum requirements of the present invention To detect two reflecting poles, but multiple reflecting poles may be detected simultaneously by during practical application, at design data fusion Reason, the information of reflecting pole can be maximally utilized, so as to further improve precision.The present invention specific performance be：Position error< 1cm, angular error<0.5 °, location frequency>35Hz.

Brief description of the drawings

Fig. 1 is the hardware architecture diagram of the present invention；

Fig. 2 is reflecting pole ranging and data correction schematic diagram；

Fig. 3 is mobile platform kinematics model schematic diagram；

Fig. 4 is the inventive method overall flow figure.

Embodiment

Below in conjunction with the accompanying drawings, the present invention is further described by embodiment.

A kind of double reflecting pole laser positionings and air navigation aid, this method based on filtering provided by the invention include：

(2) laser sensor of installation on a mobile platform radially launches laser to surrounding, and receives reflection laser (laser sensor is arranged on the mobile platforms such as AGV, industrial vehicle)；

(4) reflecting pole quantity that current time is irradiated to and its relative coordinate of relative laser sensor are determined：According to anti- The continuity of irradiating light beam angle, determine whether same reflecting pole, or, according to the continuous of the reflected beams angle and distance Property, determine whether same reflecting pole；According to the reflectance data for belonging to same reflecting pole, enter line number with reference to reflective column radius R According to amendment, as shown in Fig. 2 obtaining relative coordinate of the reflecting pole relative to laser sensor, and reflecting pole list is stored in；

According to the continuity of the reflected beams angle and distance, same reflecting pole, a kind of possible realization are determined whether Mode is as follows：

For reflective strong point all in data tables：

If the sequence number step (i) of i-th of reflective strong point is equal to the sequence number step (i-1)+1 of the i-th -1 reflective strong point, and And difference range (step (i))-range (step (i-1)) of distance for measuring of i-th and i-1 laser strong point be less than it is default away from From threshold value range_threshold, then what light beam step (i) and step (i-1) was irradiated to is same reflecting pole, then will (step (i), range) assignment into current k-th of landmark sublist, and in the sublist beam data group number Number=number+1；

Otherwise, what is be irradiated to is a new reflecting pole, then current k-th of landmark sublist terminates；

If number=1, will (k ,) write-in measure_landmark_list, wherein α=(step-1) Resolution+min_angle, ρ=range+R；Min_angle is the minimum value of laser sensor scanning angle；

If number>=2, then extract two groups of data of the reflective point of maximum intensity in current landmark sublist (step₁,range₁) and (step₂,range₂), calculate：

If range₁<range₂

Otherwise

Meanwhile newly-built+1 landmark sublist of kth, group number are initialized as number=0；Will (step (i), Range) assignment is into new landmark sublist, number=number+1；Continue to read data；

Finally give be irradiated to reflecting pole list measure_landmark_list；

Corresponding false code is as follows：

For i=1:number_data

If step (i)=step (i-1)+1＆＆range (step (i))-range (step (i-1))<range_ threshold

What then light beam step (i) and step (i-1) were irradiated to is same reflecting pole

By (step (i), range) assignment into current k-th of landmark sublist

Number=number+1；

else

If number=1,

Then generals (k ,) write-in measure_landmark_list,

Wherein α=(step-1) resolution+min_angle, ρ=range+R；

else

Extract two groups of data (step of the reflective point of maximum intensity in current landmark sublist₁,range₁) and (step₂, range₂), calculate：

Ifrange₁<range₂

else

endif

Newly-built+1 landmark sublist of kth；

Number=0；

By (step (i), range) assignment into new landmark sublist,

Number=number+1

endif

endfor

Wherein number_data is the number of reflective strong point, and resolution is that the angle of adjacent twice laser beam (divides Resolution), range_threshold is the distance threshold for determining whether to be irradiated to same reflecting pole, and ρ represents in laser respectively For sensor with respect under the polar coordinate system of itself, the angle and distance of the reflecting pole, measure_landmark_list is to be irradiated to The list of reflecting pole.

Or：According only to the continuity of the reflected beams angle, same reflecting pole, a kind of possible realization are determined whether Mode is as follows：

For reflective strong point all in data tables：

If the sequence number step (i) of i-th of reflective strong point is equal to the sequence number step (i-1)+1 of the i-th -1 reflective strong point, that What light beam step (i) and step (i-1) was irradiated to is same reflecting pole, then by (step (i), range) assignment to currently In k-th of landmark sublist, and in the sublist beam data group number number=number+1；

If number=1, (k, α, ρ) is write into measure_landmark_list, wherein α=(step-1) Resolution+min_angle, ρ=range+R；Min_angle is the minimum value of laser sensor scanning angle；

If range₁<range₂

Otherwise

Finally give be irradiated to reflecting pole list measure_landmark_list；

(5) reflecting pole list is initialized, obtains the world coordinates of at least two reflecting poles：It is artificial to determine that initial position is corresponding Reflecting pole list at least two reflecting poles world coordinates；Or laser sensor obtains at least three in initial position The angle and distance that reflecting pole returns, the distance between reflecting pole two-by-two is calculated, it is anti-with being generated according to reflecting pole list of coordinates Light beam range information matches, and obtains the world coordinates of at least two reflecting poles；A kind of possible implementation is as follows：

Calculate the distance between any two reflecting pole

Calculate the distance between reflective strong point of any two

For all combination of two (l, k) of M reflecting pole being irradiated in measure_landmark_list lists： Find i, j ∈ 1 ..., N } so that | D_k,l-R_i,j| ＜ R_i,j·threshold；Then i is added to correspondence_list (k), j is added to correspondence_list (l)；

Frequency of occurrence highest value in correspondence_list (k) is found, it is reflective corresponding to as reflective strong point k Post c (k), if can not find such point, remove point i from measure_landmark_list, and make M=M-1；

If the reflective strong point number M included in measure_landmark_list<=2, then warning is handled.

(6) calculated in dynamic process and it is expected reflecting pole list：According to last moment to current time laser sensor position The prediction with angle is put, estimates relative distance and angle between laser sensor and all reflecting poles, and it is reflective to be stored in expectation Colonnade table；(directly using prediction of the position and angle of last moment laser sensor as current time, or using filtering Algorithm is predicted) a kind of possible implementation is as follows：

For i-th of reflecting pole, i takes 1 to arrive N, calculates：

Wherein function arctan2 (x, y) returns to origin to the azimuth of point (x, y), value for (- π, π].

If ρ_t|t-1Less than maximal distance threshold max_distance, and α_t|t-1More than laser sensor scanning angle Minimum value min_angle subtract angle threshold threshold_angle, α_t|t-1Less than the maximum of laser sensor scanning angle Value max_angle adds angle threshold threshold_angle, then willIt is added to desired reflective list In estimated_landmark_list；

Corresponding false code is as follows：

For i=1:N

If ρ_t|t-1＜ max_distance＆＆min_angle-threshold_angle ＜ α_t|t-1＜ max_angle+ threshold_angle

Then willIt is added to desired reflective list estimated_landmarks_list

End

(7) in dynamic process reflecting pole list matching：Current time reflecting pole list is calculated with it is expected reflective colonnade The difference of the difference of distance and angle corresponding to same reflecting pole in table, when the difference apart from its difference and angle is satisfied by predetermined threshold value When, the match is successful；A kind of possible implementation is as follows：

For measure_landmark_list all reflecting poles, { i, X are found_i,Y_i,ρ_t|t-1,α_t|t-1So that：

Calculate measurement point (ρ_k,α_k) and world coordinate systemError：

If dist<Threshold_distance, then c (k)=i

If can not find such point, remove point i from measure_landmark_list, and make M=M-1；If The reflective strong point number M included in measure_landmark_list<=2, then warning is handled.

z_k=X_k+i*Y_k

z_l=X_l+i*Y_l

(9) multiple optimizations：, can be according to multigroup number if laser sensor detects the reflecting pole data of three and the above The pose that any two groups are calculated in carries out data fusion.Optional scheme has：Choose two groups of most strong numbers of reflective light intensity According to result of calculation as the current pose of laser sensor, or in multi-group data combination of two carry out pose calculating, Ran Houqiu Take their average value, or weighted average (such as weight is related to intensity of reflected light).

(10) laser sensor position filtering：Pose prediction is carried out according to mobile platform kinematics model first, formula is such as Under：

θ_t|t-1=θ_t-1|t-1+Δθ_c

Then the pose predicted according to last moment, with reference to the pose result of calculation at current time, to the position at current time Put and be filtered；If current time only detects 2 reflecting poles, l and k, a kind of public affairs of possible linear Kalman filter are designated as Formula is as follows：

If current time detects the reflecting pole of 3 and the above, it is filtered using the thought of multiple data fusions；One The possible scheme of kind is to pass through sequence Kalman filtering, and false code is as follows：

For any combination of two in M reflecting pole detecting, calculate：

z_k=X_k+i*Y_k

z_l=X_l+i*Y_l

After traveling through all combinations, obtain：

Embodiment 1

As shown in figure 1, the hardware of the present invention, which is formed, mainly includes laser sensor, industrial computer, onboard control circuit and car Carry power supply；Described industrial computer, laser sensor are communicated with onboard control circuit by modes such as RS232/CAN/SPI； Vehicle power carries out direct current supply to laser sensor and onboard control circuit through DC/DC moulds, through DC/AC moulds to vehicle-mounted work Control machine carries out Alternating Current Power Supply.

Further, a number of reflecting pole is placed in suitable position in industrial environment (map), by taking AGV as an example, So that any positions of the AGV in map can successfully be detected the reflecting pole of 2 or more；Laser transmitter projects laser And detected reflectance signal.

Further, industrial computer calculates reflective intercolumniation, uses static matching during initialization, then uses Dynamic Matching To identify reflecting pole, and reflecting pole world coordinates information is obtained from reflecting pole list.

Further, the plural domain information in industrial computer extraction laser sensor ranging data, utilizes two reflecting poles Data carry out the calculating of AGV coordinate positions and posture；If the reflective column information of more than 3 is detected simultaneously by, by reflecting pole number According to combination of two is carried out, calculated respectively in the case where combining weekly, AGV position and appearance are then used as using the average value of result of calculation State.

Further, industrial computer establishes AGV motion models, and is accurately estimated using Kalman filtering on this basis AGV actual position and posture.The overall flow of the present invention is as shown in Figure 4.

Claims

1. a kind of double reflecting pole laser positionings and air navigation aid based on filtering, it is characterised in that this method includes：

(3) efficient beam of the screening from reflecting pole：By detecting reflection laser intensity I and compared with preset strength threshold value σ Compared with it is reflecting pole or conventional environment object to judge that laser irradiates thing；

(4) reflecting pole quantity that current time is irradiated to and its relative coordinate of relative laser sensor are determined：According to reflected light The continuity of beam angle degree, determine whether same reflecting pole, or, according to the continuity of the reflected beams angle and distance, sentence Whether disconnected is same reflecting pole；According to the reflected light data for belonging to same reflecting pole, carry out data with reference to reflective column radius and repair Just, relative coordinate of the reflecting pole relative to laser sensor is obtained, and is stored in reflecting pole list；

(5) reflecting pole list is initialized, obtains the world coordinates of at least two reflecting poles：It is artificial to determine corresponding to initial position instead The world coordinates of at least two reflecting poles in light beam list；Or laser sensor is reflective in initial position acquisition at least three The angle and distance that post returns, the distance between reflecting pole two-by-two is calculated, with the reflecting pole generated according to reflecting pole list of coordinates Range information matches, and obtains the world coordinates of at least two reflecting poles；

(6) calculated in dynamic process and it is expected reflecting pole list：According to last moment to current time laser sensor position and The prediction of angle, estimate relative distance and angle between laser sensor and all reflecting poles, and be stored in and it is expected reflective colonnade Table；

(7) in dynamic process reflecting pole list matching：Current time reflecting pole list is calculated with it is expected in reflecting pole list The difference of the difference of distance and angle corresponding to same reflecting pole, when the difference apart from its difference and angle is satisfied by predetermined threshold value, With success；

(8) the laser sensor pose based on double reflecting pole data calculates：Believed using the complex frequency domain of laser sensor measurement data Breath, optional two in the reflecting pole that the match is successful：L-th and k-th, and calculate：

<mrow> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>&rho;</mi> <mi>k</mi> </msub> </mfrac> <msup> <mi>e</mi> <mrow> <mo>-</mo> <mi>i</mi> <mo>*</mo> <msub> <mi>&alpha;</mi> <mi>k</mi> </msub> </mrow> </msup> </mrow>

<mrow> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>&rho;</mi> <mi>l</mi> </msub> </mfrac> <msup> <mi>e</mi> <mrow> <mi>i</mi> <mo>*</mo> <mrow> <mo>(</mo> <mi>&pi;</mi> <mo>-</mo> <msub> <mi>&alpha;</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </msup> </mrow>

z_k=X_k+i*Y_k

z_l=X_l+i*Y_l

<mrow> <msub> <mi>&theta;</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>arg</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> </mrow> <msub> <mi>&rho;</mi> <mi>k</mi> </msub> </mfrac> <mo>&CenterDot;</mo> <msup> <mi>e</mi> <mrow> <mi>i</mi> <mo>&CenterDot;</mo> <msub> <mi>&alpha;</mi> <mi>k</mi> </msub> </mrow> </msup> <mo>)</mo> </mrow> </mrow>

Wherein, subscript l and k represent l and k-th of reflecting pole respectively；α and ρ represent respectively laser sensor with respect to itself Under polar coordinate system, the angle and distance of reflecting pole；X and Y is respectively reflecting pole in X and the component of Y-axis；Z is reflecting pole in the world Complex coordinates under coordinate system；z_k,lFor the laser sensor world coordinates being calculated according to l and k-th of reflecting pole, θ_kIt is According to angle of the laser sensor that the data of k-th of reflecting pole are calculated under world coordinate system；

(9) laser sensor position filtering：Pose prediction is carried out according to mobile platform kinematics model first, then according to upper one The pose of moment prediction, and the laser sensor measurement data at current time is combined, the position at current time is filtered, from And obtain the accurate position and posture of final mobile platform and laser sensor under world coordinate system.

2. double reflecting pole laser positionings and air navigation aid according to claim 1 based on filtering, it is characterised in that described In step (6), directly using prediction of the position and angle of last moment laser sensor as current time, or using filter Ripple algorithm is predicted.

3. double reflecting pole laser positionings and air navigation aid according to claim 1 based on filtering, it is characterised in that described Before step (9), in addition to the step of multiple optimizations：If laser sensor detects the reflecting pole data of three and the above, Data fusion can be carried out according to the pose that any two groups are calculated in multi-group data.

4. double reflecting pole laser positionings and air navigation aid according to claim 1 based on filtering, it is characterised in that described In step (9), pose prediction is carried out according to mobile platform kinematics model, formula is as follows：

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&omega;</mi> <mi>L</mi> </msub> <mo>&CenterDot;</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>=</mo> <mn>2</mn> <mi>&pi;</mi> <mo>&CenterDot;</mo> <mi>r</mi> <mi>e</mi> <mi>s</mi> <mi>o</mi> <mi>l</mi> <mi>u</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> <mi>n</mi> <mo>&CenterDot;</mo> <mfrac> <mn>1</mn> <mrow> <mi>s</mi> <mi>p</mi> <mi>e</mi> <mi>e</mi> <mi>d</mi> <mo>_</mo> <mi>r</mi> <mi>e</mi> <mi>d</mi> <mi>u</mi> <mi>c</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> <mi>n</mi> <mo>_</mo> <mi>r</mi> <mi>a</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> </mrow> </mfrac> <mrow> <mo>(</mo> <mi>c</mi> <mi>o</mi> <mi>u</mi> <mi>n</mi> <mi>t</mi> <mo>_</mo> <mi>l</mi> <mi>e</mi> <mi>f</mi> <mi>t</mi> <mo>-</mo> <mi>l</mi> <mi>a</mi> <mi>s</mi> <mi>t</mi> <mo>_</mo> <mi>c</mi> <mi>o</mi> <mi>u</mi> <mi>n</mi> <mi>t</mi> <mo>_</mo> <mi>l</mi> <mi>e</mi> <mi>f</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&omega;</mi> <mi>R</mi> </msub> <mo>&CenterDot;</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>=</mo> <mn>2</mn> <mi>&pi;</mi> <mo>&CenterDot;</mo> <mi>r</mi> <mi>e</mi> <mi>s</mi> <mi>o</mi> <mi>l</mi> <mi>u</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> <mi>n</mi> <mo>&CenterDot;</mo> <mfrac> <mn>1</mn> <mrow> <mi>s</mi> <mi>p</mi> <mi>e</mi> <mi>e</mi> <mi>d</mi> <mo>_</mo> <mi>r</mi> <mi>e</mi> <mi>d</mi> <mi>u</mi> <mi>c</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> <mi>n</mi> <mo>_</mo> <mi>r</mi> <mi>a</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> </mrow> </mfrac> <mrow> <mo>(</mo> <mi>c</mi> <mi>o</mi> <mi>u</mi> <mi>n</mi> <mi>t</mi> <mo>_</mo> <mi>r</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mo>-</mo> <mi>l</mi> <mi>a</mi> <mi>s</mi> <mi>t</mi> <mo>_</mo> <mi>c</mi> <mi>o</mi> <mi>u</mi> <mi>n</mi> <mi>t</mi> <mo>_</mo> <mi>r</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>v</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>=</mo> <mfrac> <mi>r</mi> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <msub> <mi>&omega;</mi> <mi>R</mi> </msub> <mo>&CenterDot;</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>+</mo> <msub> <mi>&omega;</mi> <mi>L</mi> </msub> <mo>&CenterDot;</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&omega;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>=</mo> <mfrac> <mi>r</mi> <mi>L</mi> </mfrac> <mrow> <mo>(</mo> <msub> <mi>&omega;</mi> <mi>R</mi> </msub> <mo>&CenterDot;</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>-</mo> <msub> <mi>&omega;</mi> <mi>L</mi> </msub> <mo>&CenterDot;</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&Delta;x</mi> <mi>c</mi> </msub> <mo>=</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>&CenterDot;</mo> <msub> <mi>v</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mi>cos&theta;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&Delta;y</mi> <mi>c</mi> </msub> <mo>=</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>&CenterDot;</mo> <msub> <mi>v</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mi>sin&theta;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&Delta;&theta;</mi> <mi>c</mi> </msub> <mo>=</mo> <mi>&Delta;</mi> <mi>T</mi> <mo>&CenterDot;</mo> <msub> <mi>&omega;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>

θ_t|t-1=θ_t-1|t-1+Δθ_c

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>2</mn> <mi>&pi;</mi> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>></mo> <mi>&pi;</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <mo>-</mo> <mi>&pi;</mi> <mo><</mo> <msub> <mi>&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo><</mo> <mi>&pi;</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mn>2</mn> <mi>&pi;</mi> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo><</mo> <mo>-</mo> <mi>&pi;</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced>

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>&Delta;</mi> <mi>x</mi> <mo>=</mo> <msub> <mi>&Delta;x</mi> <mi>c</mi> </msub> <mo>+</mo> <mi>d</mi> <mo>*</mo> <msub> <mi>cos&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mi>d</mi> <mo>*</mo> <msub> <mi>cos&theta;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&Delta;</mi> <mi>y</mi> <mo>=</mo> <msub> <mi>&Delta;y</mi> <mi>c</mi> </msub> <mo>+</mo> <mi>d</mi> <mo>*</mo> <msub> <mi>sin&theta;</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mi>d</mi> <mo>*</mo> <msub> <mi>sin&theta;</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>

<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>&Delta;</mi> <mi>x</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>&Delta;</mi> <mi>y</mi> </mrow> </mtd> </mtr> </mtable> </mfenced>

Wherein, input quantity is：Pose (the x of last moment_t-1|t-1,y_t-1|t-1,θ_t-1|t-1), the resolution ratio of motor encoder Resolution, speed reducing ratio speed_reduction_ratio, left and right turns encoder reading count_left this moment, Count_right, reading last_count_right, the last_count_right at a moment in left and right turns encoder, sampling week Phase Δ T, left and right wheels radius r, the angular velocity omega of left and right wheel_L、ω_R, linear velocity v of the mobile platform in last moment_t-1, it is mobile flat Steering angular speed omega of the platform in last moment_t-1, mobile platform under last moment world coordinate system towards angle, θ_t-1, laser The distance d of two wheel subcenter points of sensor distance；Output quantity is：The increment of motion (Δ x, Δ y, Δ θ) at upper moment relatively, root According to the current time pose (x of last moment data prediction_t|t-1,y_t|t-1,θ_t|t-1)；

The pose predicted according to last moment, and the laser sensor measurement data at current time is combined, to the position at current time Appearance is filtered, specific as follows：If current time only detects 2 reflecting poles, l and k are designated as, using linear Kalman filter Formula is as follows：

<mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> 2

<mrow> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <msubsup> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> <mi>T</mi> </msubsup> <mo>&CenterDot;</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <msubsup> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <mi>R</mi> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>

<mrow> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>Re</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>z</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>Im</mi> <mrow> <mo>(</mo> <msub> <mi>w</mi> <mi>k</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>w</mi> <mi>l</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>z</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

<mrow> <msub> <mi>X</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&CenterDot;</mo> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow>

<mrow> <msub> <mi>P</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>

If current time detects the reflecting pole of 3 and the above, it is filtered using the thought of multiple data fusions；Using passing through The formula of sequence Kalman filtering is as follows：

For any combination of two in M reflecting pole detecting, calculate：

<mrow> <msub> <mi>&omega;</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>&rho;</mi> <mi>k</mi> </msub> </mfrac> <msup> <mi>e</mi> <mrow> <mo>-</mo> <mi>i</mi> <mo>*</mo> <msub> <mi>&alpha;</mi> <mi>k</mi> </msub> </mrow> </msup> </mrow>

<mrow> <msub> <mi>&omega;</mi> <mi>l</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>&rho;</mi> <mi>l</mi> </msub> </mfrac> <msup> <mi>e</mi> <mrow> <mi>i</mi> <mo>*</mo> <mrow> <mo>(</mo> <mi>&pi;</mi> <mo>-</mo> <msub> <mi>&alpha;</mi> <mi>l</mi> </msub> <mo>)</mo> </mrow> </mrow> </msup> </mrow>

z_k=X_k+i*Y_k

z_l=X_l+i*Y_l

<mrow> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&CenterDot;</mo> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>X</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow>

<mrow> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mi>H</mi> <mrow> <mi>t</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>l</mi> <mo>)</mo> </mrow> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>

After traveling through all combinations, obtain：

<mrow> <msub> <mi>P</mi> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>P</mi> <mo>~</mo> </mover> <mrow> <mi>t</mi> <mo>|</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>.</mo> </mrow> 4