Intelligent vehicle positioning method based on feature point calibration

Abstract

An intelligent vehicle positioning method based on feature point calibration is provided. The intelligent vehicle positioning method includes: determining whether an intelligent vehicle is located in a blind area or a non-blind area; when the intelligent vehicle is located in a GNSS non-blind area, combining GNSS signals, odometer data and inertial measurement unit data, acquiring a current pose of the intelligent vehicle through a Kalman filtering, scanning a surrounding environment of the intelligent vehicle by using a laser radar, extracting corner points and arc features and performing processing to obtain the corner points and circle centers as the feature points, calculating global coordinates and weights of the feature points and storing the global coordinates and the weights of the feature points in a current feature point list.

Claims

1. An intelligent vehicle positioning method based on a feature point calibration, comprising: determining a global navigation satellite system (GNSS) signal quality according to a number of locked-on GNSS satellites, and determining whether an intelligent vehicle is in a GNSS blind area or a GNSS non-blind area according to the GNSS signal quality; when the intelligent vehicle is located in the GNSS non-blind area, combining GNSS signals, odometer data and inertial measurement unit data, acquiring a current pose of the intelligent vehicle through a Kalman filtering, meanwhile scanning a surrounding environment of the intelligent vehicle by using a laser radar, extracting corner points and arc features and performing processing to obtain the corner points and circle centers as feature points, calculating global coordinates and weights of the feature points and storing the global coordinates and the weights of the feature points in a current feature point list; adding the feature points in the current feature point list into an existing feature point list, and adding feature points meeting conditions in the existing feature point list into a trusted list; and when the intelligent vehicle is located in the GNSS blind area, acquiring feature points of a surrounding trusted list as an observed quantity by using the laser radar, fusing the odometer data and the inertial measurement unit data, and acquiring the current pose of the intelligent vehicle by a particle filtering method and achieving positioning seamless switching of the intelligent vehicle entering the GNSS blind area from the GNSS non-blind area; when the intelligent vehicle is in the GNSS non-blind area, the step of combining the GNSS signals, the odometer data and the inertial measurement unit data, acquiring the current pose of the intelligent vehicle through the Kalman filtering specifically comprises: step 1, sub-step 1 comprises calculating a displacement change and a heading angle change of the intelligent vehicle by photoelectric encoders on a left rear wheel and a right rear wheel of the intelligent vehicle through the following steps: calculating position increments Δd.sub.L and Δd.sub.R of the left rear wheel and the right rear wheel in a unit sampling time T.sub.S using the mathematical expression as follows: $\Delta d_L=\frac{\Delta N_L*\pi *D}{P*T_s*m}$ $\Delta d_R=\frac{\Delta N_R*\pi *D}{P*T_s*m}$ where ΔN.sub.L and ΔN.sub.R are respectively pulse increments in the unit sampling time of the left rear wheel and the right rear wheel, D is an equivalent diameter of a wheel, P is a total number of gratings of an optical code disc, and m is a deceleration rate of the photoelectric encoder; calculating the displacement change ΔD.sub.k and the heading angle change Δθ.sub.k.sup.WO of the intelligent vehicle from a moment t.sub.k-1 to a moment t.sub.k using the mathematical expression as follows: $\Delta D_k=\frac{\Delta d_R+\Delta d_L}{2},\Delta {\theta }_k^{\mathrm{WO}}=\frac{\Delta d_R-\Delta d_L}{W}$ where W is an axial length of the left rear wheel and the right rear wheel; calculating a speed at the moment t.sub.k using the mathematical expression as follows: $V_k=\frac{\Delta D_k}{T_s};$ step 1, sub-step 2 comprises acquiring a heading angular acceleration ω.sub.k directly from an inertial measurement unit (IMU) at the moment t.sub.k, and further integrating the heading angular acceleration ω.sub.k at each moment to obtain a second heading angle change Δθ.sub.k.sup.IMU, where a heading angle is θ.sub.k.sup.IMU=θ.sub.k-1+Δθ.sub.k.sup.IMU; step 1, sub-step 3 comprises performing a coordinate conversion on GNSS information collected by the intelligent vehicle to obtain a position (x.sub.k.sup.GNSS, y.sub.k.sup.GNSS) of the intelligent vehicle at the moment t.sub.k, step 1, sub-step 1 comprises fusing the GNSS information by a Kalman filter; defining a state of the intelligent vehicle at the moment t.sub.k-1 as: X.sub.k-1=[x.sub.k-1, y.sub.k-1, V.sub.k-1, θ.sub.k-1ω.sub.k-1].sup.T, defining motion and observation models respectively as: ${\hat{X}}_k=f\left(X_{k-1}\right)+c_k,c_k~N\left(0,Q\right),{\hat{z}}_k=h\left({\hat{X}}_k\right)+v_k,v_k~N\left(0,R\right);$ and defining a state transition function of a movement process as: $f_{\left(X_{k-1}\right)}=\left[\begin{array}{c}{x}_{k-1}+\frac{\Delta D_k}{\Delta {\theta }_k^{\mathrm{WO}}}\left(\sin \left({\theta }_{k-1}+\Delta {\theta }_k^{\mathrm{WO}}\right)-\sin {\theta }_{k-1}\right)\\ y_{k-1}-\frac{\Delta D_k}{\Delta {\theta }_k^{\mathrm{WO}}}\left(\cos \left({\theta }_{k-1}+\Delta {\theta }_k^{\mathrm{WO}}\right)-\cos {\theta }_{k-1}\right)\\ V_k\\ {\theta }_{k-1}+\Delta {\theta }_k^{\mathrm{WO}}\\ {\omega }_k\end{array}\right];$ where {circumflex over (X)}.sub.k=[{circumflex over (x)}.sub.k, ŷ.sub.k, {circumflex over (V)}.sub.k, {circumflex over (θ)}.sub.k, {circumflex over (ω)}.sub.k] is a predicted state of the intelligent vehicle at the moment t.sub.k, h({circumflex over (X)}.sub.k) is an observation model transfer function, {circumflex over (z)}.sub.k is the observed quantity with a noise added, c.sub.k is a system process noise with a covariance of Q, and v.sub.k is a sensor measurement noise with a covariance of R; acquiring the current pose of the intelligent vehicle based on the motion and observation models; the Kalman filter is predicted and updated as follows: predicting a state quantity: {circumflex over (X)}.sub.(k|k-1)=A{tilde over (X)}.sub.k-1; predicting an error covariance matrix: P.sub.(k|k-1)=AP.sub.k-1A.sup.T+Q; updating an optimal estimated state quantity: {tilde over (X)}.sub.(k)={circumflex over (X)}.sub.(k|k-1)+K.sub.k[z.sub.k−H{circumflex over (X)}.sub.(k|k-1)]; updating a Kalman gain: $K_k=\frac{P_{\left(k|k-1\right)}{H}^T}{\left[{\mathrm{HP}}_{\left(k|k-1\right)}{H}^T+R\right]};$ and updating the error covariance matrix: P.sub.k=[I−K.sub.k H]P.sub.(k|k-1); where {circumflex over (X)}.sub.(k|k-1) is a predicted pose state quantity at the moment t.sub.k, A is a state transition matrix, {tilde over (X)}.sub.k-1 is an optimal pose state quantity at the moment t.sub.k-1, P.sub.k-1 is an updated error covariance matrix at the moment t.sub.k-1, K.sub.k is a Kalman gain at the moment t.sub.k, H is a measurement matrix, and the observed quantity is z.sub.k=[x.sub.k.sup.GNSS, y.sub.k.sup.GNSS, V.sub.k, θ.sub.k.sup.IMU, ω.sub.k]; and {tilde over (X)}.sub.k is the optimal pose state quantity at the moment t.sub.k; the step of scanning the surrounding environment of the intelligent vehicle by using the laser radar, extracting the corner points and the arc features and performing processing to obtain the corner points and the circle centers as the feature points, calculating the global coordinates and the weights of the feature points and storing the global coordinates and the weights of the feature points in the current feature point list specifically comprises: removing individual noise points in point cloud data acquired by the laser radar according to a pre-designed point cloud density threshold; and then segmenting a point cloud into different point cloud clusters according to a Euclidean distance threshold between each point; extracting the arc features from the different point cloud clusters based on a Hough transform mechanism, and with corresponding circle centers of the arc features as the feature points, using coordinates of the circle centers as coordinates (x.sub.L, y.sub.L) of the feature points in a local coordinate system of a vehicle body, and calculating the weights of the feature points; detecting the corner points from the point cloud clusters as the feature points, using coordinates of the corner points as the coordinates (x.sub.L, y.sub.L) of the feature points in the local coordinate system of the vehicle body, and calculating the weights of the feature points; converting the coordinates (x.sub.L, y.sub.L) of the feature points in the local coordinate system of the vehicle body to coordinates (x.sub.G, y.sub.G) in a global coordinate system, and storing the coordinates (x.sub.G, y.sub.G) together with the weights in the current feature point list; the step of extracting the arc features from the point cloud clusters based on the Hough transform mechanism, and with the corresponding circle centers of the arc features as the feature points, using the coordinates of the circle centers as the coordinates (x.sub.L, y.sub.L) of the feature points in the local coordinate system of the vehicle body, and calculating weights of the feature points specifically comprises: for a certain point cloud cluster, each point casting a vote on a certain circle parameter through a Hough transform, wherein the certain circle parameter is expressed by a formula: (x−a).sup.2+(y−b).sup.2=r.sup.2; adding a first constraint and a second constraint, wherein the first constraint is that a number of votes is greater than a first threshold; and the second constraint is that a diameter of a composite circle is greater than a second threshold; and using a circle parameter meeting the first constraint and the second constraint as an observable arc feature; determining center coordinates and a weight according to an obtained arc feature: defining a vote number weight w.sub.1.sup.r=1/N, where N is the number of votes obtained for a circle feature, and an arc weight w.sub.2.sup.r=α/2π, where α is a central angle corresponding to an arc formed by scanning points and is expressed by a formula α=arcsin (L/2r), and L is a chord length obtained by connecting two ends of the arc, and if a distance from a point in the arc to a chord is greater than r, a corresponding central angle is greater than π, and the corresponding central angle is α=2π−arcsin (L/2r); using a circle center corresponding to the arc as a feature point with the coordinates (x.sub.L, y.sub.L) in the local coordinate system of the vehicle body, and the weight is w.sub.1.sup.r×w.sub.2.sup.r; the step of detecting the corner points from the point cloud clusters as the feature points, using the coordinates of the corner points as the coordinates (x.sub.L, y.sub.L) of the feature points in the local coordinate system of the vehicle body, and calculating weights of the feature points specifically comprises: detecting breakpoints in each point cloud cluster: if a distance between one point and the next point is greater than a breakpoint detection threshold, then considering the one point and the next point as the breakpoints; further segmenting the point cloud cluster by the breakpoints; connecting a starting point and an end point of a segment of the point cloud cluster Si to obtain a straight line l, calculating a distance from each point in the point cloud cluster to the straight line l, and finding a point K.sub.p with a largest distance, and if the largest distance d.sub.p from the point K.sub.p to the straight line l is greater than a set threshold d.sub.max, considering the point K.sub.p as a corner point, and segmenting the point cloud cluster S.sub.i into two sub-point cloud clusters using the point K.sub.p as a dividing point, and then executing a corner point determination process on two segmented point cloud clusters respectively, until the corner points in all point cloud clusters are found; meanwhile, adding a third constraint and a fourth constraint, wherein the third constraint is that distances from the corner point to end points of the straight line l and the straight line l on both sides are greater than a certain threshold; and the fourth constraint is that numbers of scanning points of the straight line l and the straight line l on both sides are greater than a set value; and removing the corner points without meeting the third constraint and the fourth constraint; fitting the two sub-point cloud clusters on both sides of an obtained corner point into two straight lines l.sub.1 and l.sub.2 by using a least square method, wherein a fitting process of one of the straight line l.sub.1 and the straight line l.sub.2 is as follows: designing a cost function: $J\left(\alpha ,\rho \right)=\min \underset{s=1}{\overset{S}{.Math.}}\left(\rho -{\alpha }_s\cos \alpha -b_s\sin \alpha \right);$ where ρ is a vertical distance from an origin of the local coordinate system of the vehicle body to the straight line, α is an included angle between an x axis and a normal of the straight line, and (a.sub.s, b.sub.s) is coordinates of an s.sup.th laser radar scanning point in the local coordinate system of the vehicle body; and a parameter ε=var(Δρ.sub.1, Δρ.sub.2, . . . , Δρ.sub.S) is used to measure an error of a straight line feature parameter, where Δρ.sub.s represents a vertical distance from a scanning point in the straight line to the straight line, and 1≤s≤S; and a weight of the straight line is $\frac{1}{\mathrm{.Math.}};$ finding an intersection point of the straight line l.sub.1 and the straight line l.sub.2 by using the straight line feature parameters, and using coordinates of the intersection point as the coordinates (x.sub.L, y.sub.L) of the feature point in the local coordinate system of the vehicle body; calculating a weight w.sub.1.sup.c×w.sub.2.sup.c of the feature point, where w.sub.1.sup.cw.sub.2.sup.c are weights of the straight lines l.sub.1 and l.sub.2, respectively; the step of adding the feature points in the current feature point list into the existing feature point list specifically comprises: calculating a weighted average of all recorded coordinates of any column in the existing feature point list as quasi-coordinates X.sub.s of the feature point in a column: $X_s=\underset{f=1}{\overset{F}{.Math.}}{w}_f{Z}_f;$ where a subscript f represents a recorded coordinate serial number of the feature point, Z.sub.f is recorded coordinates, and w.sub.f is a corresponding weight; taking the feature point from the current feature point list and matching the feature point with the existing feature point list, if a distance between the feature point and the quasi-coordinates in a column of the existing feature point list is less than a third threshold, considering a matching as successful, and adding coordinate and weight information of the feature point to the column; and if the matching is unsuccessful, considering the feature point as a new feature point, creating a new column in the existing feature point list, and saving the new feature point; the step of adding the feature points meeting conditions in the existing feature point list into the trusted list specifically comprises: step 2, sub-step 1 comprises, for a certain feature point in the existing feature point list, defining a total number of observations N.sub.A as a total number of scans of the surrounding environment since the laser radar scanned the certain feature point, defining an effective number of the observations N.sub.V as a total number of successful matches of the certain feature point, defining an effective observation rate as N.sub.V/N.sub.A×100%, and if the feature point meets conditions where N.sub.V is greater than a fourth threshold and the effective observation rate is greater than a fifth threshold, preparing to add the feature point to the trusted list, defining a number of optimal estimation points N=N.sub.V×β, where β is an optimal estimation rate, and re-calculating the quasi-coordinates after new recorded coordinates are added using the mathematical equation as follows: $X_s=\underset{f=1}{\overset{F+1}{.Math.}}{w}_f{Z}_f;$ taking nearest N points from X.sub.s and calculating a weighted average again to obtain modified coordinates: $X_F=\underset{n=1}{\overset{N}{.Math.}}{w}_n{Z}_n;$ calculating a weight w.sub.F=1/ε.sub.V, where ε.sub.V is a distribution variance of the optimal estimation point, and meanwhile amplifying ε.sub.V, and removing feature point coordinates outside an amplified distribution range; if w.sub.F is greater than a sixth threshold, adding the modified coordinates X.sub.F and the weight w.sub.F of the feature point to the trusted list.

2. The intelligent vehicle positioning method of claim 1, wherein when the intelligent vehicle is located in the GNSS blind area, the step of acquiring the surrounding trusted feature points as the observed quantity by using the laser radar, fusing the odometer data and the inertial measurement unit data, and acquiring the current pose of the intelligent vehicle by the particle filtering method specifically comprises: step 3, sub-step 1 comprises using an optimal pose {tilde over (X)}.sub.0=(x.sub.0, y.sub.0, θ.sub.0) at a moment before the intelligent vehicle enters the GNSS blind area as an initial distribution of a particle swarm, constructing a probability density function by {tilde over (X)}.sub.0 and an error covariance matrix P.sub.0 as a proposal distribution {tilde over (q)}, randomly selecting M initial particles, creating a Gaussian distribution for the M initial particles around (x.sub.0, y.sub.0, θ.sub.0) in the proposal distribution {tilde over (q)}, then expressing a state of an i.sup.th particle at the moment t.sub.k as X.sub.k.sup.i=[x.sub.k.sup.i, y.sub.k.sup.i, θ.sub.k.sup.i].sup.T, and calculating a weight of the i.sup.th particleas w.sub.0.sup.i=1/M; step 3, sub-step 2 comprises performing a particle prediction process to obtain displacement and heading direction of the intelligent vehicle using the mathematical expression as follows: at the moment t.sub.k, using an equation of a motion X.sub.k.sup.i=g(X.sub.k-1.sup.i)+d.sub.k-1, d.sub.k-1˜N(0, Q.sub.1), wherein a state transition function is: $g_{\left(X_{k-1}^i\right)}=\left[\begin{array}{c}{x}_{k-1}^i+\frac{\Delta D_k}{\Delta {\theta }_k^{\mathrm{WO}}}\left(\sin \left({\theta }_{k-1}^i+\Delta {\theta }_k^{\mathrm{WO}}\right)-\sin {\theta }_{k-1}^i\right)\\ y_{k-1}^i-\frac{\Delta D_k}{\Delta {\theta }_k^{\mathrm{WO}}}\left(\cos \left({\theta }_{k-1}^i+\Delta {\theta }_k^{\mathrm{WO}}\right)-\cos {\theta }_{k-1}^i\right)\\ {\theta }_{k-1}^i+\Delta {\theta }_k^{\mathrm{IMU}}\end{array}\right];$ where d.sub.k-1 in the formula is the system process noise of a covariance matrix Q.sub.1; step 3, sub-step 3 comprises acquiring current feature points of the surrounding environment of the intelligent vehicle as the observed quantity by using the laser radar, meanwhile performing the matching on the feature points from the trusted list, calculating a predicted observed quantity of matched feature points for the M initial particles, and calculating a difference between the predicted observed quantity and the observed quantity; step 3, sub-step 4 comprises updating weights of the M initial particles according to the difference between the predicted observed quantity and the observed quantity using the mathematical expression as follows: $w_k^i=w_{k-1}^i\underset{n=1}{\overset{N}{.Math.}}\frac{1}{2\pi {\sigma }_x{\sigma }_y}{e}^{-\frac{1}{2}\left({\left(\frac{\Delta {\stackrel{\_}{x}}_n^i}{{\sigma }_x}\right)}^2+{\left(\frac{\Delta {\stackrel{\_}{y}}_n^i}{{\sigma }_y}\right)}^2\right)}.Math.w_F^n;$ where N is the number of the feature points matching a current surrounding environment of the intelligent vehicle from the trusted list, σ.sub.x and σ.sub.y represent measurement variances of the laser radar, and w.sub.F.sup.n represent the weights of the feature points, and Δx.sub.n.sup.i and Δy.sub.n.sup.i are errors of the predicted observed quantity and the observed quantity of an n.sup.th feature point for the i.sup.th particle, and 1≤n≤N, steps 3, sub-step 5 comprises normalizing the weights using the mathematical expression as follows: ${\tilde{w}}_k^i=\frac{w_k^i}{\underset{i=1}{\overset{M}{.Math.}}{w}_k^i};$ step 3, sub-step 6 comprises calculating a number of effective particles weights using the mathematical expression as follows: ${\tilde{N}}_{\mathrm{eff}}=\frac{1}{\underset{i=1}{\overset{M}{.Math.}}{\tilde{w}}_k^i};$ if Ñ.sub.eff is less than the threshold, performing a resampling process, wherein a low-variance resampling is used for a sampling, and particles with larger weights are retained in the resampling process, and normalizing all particle weights to 1/M after resampling is completed; otherwise, proceeding to step 3, sub-step 7; step 3, sub-step 7 comprises performing state estimation output at the moment t.sub.k, by calculating a state estimation value $X_k=\underset{i=1}{\overset{M}{.Math.}}{\tilde{w}}_k^i{X}_k^i$ to obtain the current pose of the intelligent vehicle.

3. The intelligent vehicle positioning method of claim 2, wherein the step of acquiring the current feature points of the surrounding environment of the intelligent vehicle as the observed quantity by using the laser radar, meanwhile performing the matching on the feature points from the trusted list, calculating the predicted observed quantity of the matched feature points for the M initial particles, and calculating the difference between the predicted observed quantity and the observed quantity specifically comprises; based on the predicted state X.sub.k.sup.i=[x.sub.k.sup.i, y.sub.k.sup.i, θ.sub.k.sup.i].sup.T of the i.sup.th particle obtained after step 3, sub-step 2 at the moment t.sub.k, establishing the local coordinate system of the i.sub.th particle by using a predicted position (x.sub.k.sup.i, y.sub.k.sup.i) of the i.sup.th particle as an origin, a forward direction of the intelligent vehicle as an X direction, a direction perpendicular to the X axis and oriented to a left of the vehicle body as a Y axis direction, and a vertically upward direction as a Z axis; calculating x, y axial distances Δx.sub.j.sup.i and Δy.sub.j.sup.i of the feature points in the trusted list and the M initial particles in the global coordinate system one by one, where a subscript represents a j.sup.th feature point, and a superscript represents the i.sup.th particle; further calculating a Euclidean distance ΔO.sub.j.sup.i=√{square root over ((Δx.sub.j.sup.i).sup.2+(Δy.sub.j.sup.i).sup.2)}, and if the Euclidean distance is less than a high-precision detection range of the laser radar, determining the j.sup.th feature point is within the high-precision detection range, storing the feature point in a comparison list, and re-assigning a sequence number n to represent a sequence of the feature point, obtaining a total of N feature points meeting a requirement within the high-precision detection range of the laser radar; and converting a distance in the global coordinate system corresponding to the n.sup.th feature point to a distance in the local coordinate system of the i.sup.th particle: $\{\begin{array}{c}\Delta x_n^{i^{\prime }}=\Delta x_n^i\cos {\theta }_i^k+\Delta y_n^i\sin {\theta }_i^k;\\ \Delta x_n^{i^{\prime }}=\Delta x_n^i\cos {\theta }_i^k+\Delta y_n^i\sin {\theta }_i^k;\end{array}\hspace{1em}$ where Δx.sub.n.sup.i′ and Δy.sub.n.sup.i′ are the predicted observed quantity of the n.sup.th feature point for the i.sup.th particle; at the moment t.sub.k, scanning the surrounding environment by the laser radar, detecting the circle centers and the corner points and calculating the feature point coordinates (x.sub.q, y.sub.q) in the local coordinate system of the vehicle body, and storing the feature point coordinates in the current feature point list, where q represents a serial number and (x.sub.q, y.sub.q) is the observed quantity of the feature point, and a total number of the current feature points is q.sub.max; calculating, one by one, differences Δx.sub.nq.sup.i and Δy.sub.nq.sup.i between the observed quantity corresponding to the feature point coordinates in the current feature point list and the predicted observed quantity of an n.sup.th calibration point for the i.sup.th particle in the comparison list; $\{\begin{array}{c}\Delta {\stackrel{\_}{x}}_{\mathrm{nq}}^i=\Delta x_n^{i^{\prime }}-x_q\\ \Delta {\stackrel{\_}{y}}_{\mathrm{nq}}^i=\Delta y_n^{i^{\prime }}-y_q\end{array};$ forming simultaneous equations by a nearest neighbor method: calculating a minimum value $\min \left(\sqrt{{\left(\Delta {\stackrel{\_}{x}}_{\mathrm{nq}}^i\right)}^2+{\left(\Delta {\stackrel{\_}{y}}_{\mathrm{nq}}^i\right)}^2},q=1,2\mathrm{.Math.}{q}_{\max }\right)$ of the feature point n in the comparison list, calculating a corresponding serial number q.sub.0 of the minimum value, forming the simultaneous equations of n and q.sub.0, with a corresponding error Δx.sub.nq.sub.0.sub.i and Δy.sub.nq.sub.0.sub.i, denoted by Δx.sub.n.sup.i and Δy.sub.n.sup.i, traversing the comparison list and executing the process, to accomplish feature point recognition and matching.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is a flow diagram of an intelligent vehicle positioning method based on feature point calibration of the present invention;

(2) FIG. 2 is a schematic diagram illustrating that an intelligent vehicle observes and calibrates surrounding environment features by a laser radar in a good GNSS signal state;

(3) FIG. 3 is a flow diagram of repeated calibrations and corrections of feature points;

(4) FIG. 4 is schematic diagram of calculating coordinates of a trusted calibrated point based on coordinates of an observed point;

(5) FIG. 5 is a process diagram of fusion positioning through Kalman filtering by multiple sensors;

(6) FIG. 6 is a schematic diagram of positioning by observing a trusted calibrated point after the intelligent vehicle enters a tree-lined road; and

(7) FIG. 7 is a process diagram of fusion positioning through Kalman filtering by multiple sensors in a GNSS blind area.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(8) The present invention discloses an intelligent vehicle positioning method based on feature point calibration, to maintain high positioning accuracy for a long time even in urban roads with densely distributed buildings, tree-lined roads, tunnels and other environments with poor GNSS signals. When the GNSS signal quality is good, vehicle-mounted odometer and IMU data are collected, and fused with a GNSS locating position by Kalman filtering to obtain a relatively accurate current position. At the same time, a laser radar continuously scans and observes a surrounding environment, extracts corner point and arc feature information in the environment, and fits the same into corner point and circle center coordinates. An intelligent vehicle continuously scans and calibrates feature points while traveling, to finally obtain stable and accurate coordinate values of the feature points, and converting the coordinate values to values in a global coordinate system for storage. When the GNSS signal quality becomes worse to seriously affect the positioning effect, the positioning method is changed into a particle filter method in which odometer and IMU data are used as prediction information, and the laser radar updates observed quantities of feature calibration points to obtain a relatively accurate vehicle locating position.

(9) An IMU is an inertial measurement unit, which is a device for measuring a three-axis attitude angle (or angular speed) and an acceleration of an object. A gyroscope and an accelerometer are main elements of the IMU. An odometer includes photoelectric encoders on a left rear wheel and a right rear wheel respectively.

(10) The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings.

(11) As shown in FIG. 1, the present invention proposes an intelligent vehicle positioning method based on feature point calibration, including the following steps:

(12) Step 1) As shown in FIG. 2, an intelligent vehicle normally travels on a road surface, and enters a tree-lined road after a period of time. In the case of good GNSS signal quality, during normal traveling of the intelligent vehicle on the road, curbs, trunks of trees on both sides and other static objects with obvious features are observed by using a laser radar, and corner points and arc features are extracted and processed to obtain corner points and circle centers as feature point coordinates. After multiple observations and calibrations, the coordinates are added to a trusted list. After coordinate conversion, the coordinates are stored in a global coordinate system (geodetic coordinate system) for use. When the vehicle enters a GNSS blind area, they are used as absolute coordinate points to provide a guiding observed quantity for the laser radar. The specific process is as follows:

(13) (1.1) A preprocessing process of laser radar data. Original point cloud data obtained during traveling must be processed in advance, which can greatly simplify subsequent work. The specific process is as follows:

(14) (1.1.1) Removal of noise points. Individual noise points in a point cloud are removed according to a pre-designed point cloud density threshold.

(15) (1.1.2) Clustering of scanning points. The point cloud is segmented into different point cloud clusters according to a Euclidean distance threshold between each point.

(16) (1.2) A feature point determination and coordinate calculation process, specifically including the following steps:

(17) (1.2.1) First, arc features are determined and processed. An arc feature determination and circle center coordinate calculation process is as follows:

(18) (1.2.1.1) A circle feature is extracted based on a Hough transform mechanism. For a certain point cloud cluster, each point casts a vote on a certain circle parameter through Hough transform, which is expressed by a formula: (x−a).sup.2+(y−b).sup.2=R.sup.2. Constraints are added: a. the number of votes is greater than a threshold; and b. the diameter of a composite circle is greater than a threshold. A circle parameter that meets both the above two constraints is used as an observable arc feature.

(19) (1.2.1.2) Center coordinates and weight are determined according to the obtained arc feature. a vote number weigh is defined: w.sub.1.sup.r=1/N, where N is the number of votes obtained for the circle feature, and an arc weight w.sub.2.sup.r=α/2π, where α is a central angle corresponding to an arc formed by scanning points and is expressed by a formula α=arcsin(L/2R), and L is a chord length obtained by connecting two ends of the arc, and if a distance from a point in the arc to the chord is greater than R, the corresponding central angle is greater than π, and in this case α=2π−arcsin (L/2R) Finally, the circle center coordinate weight is obtained: w.sup.r=w.sub.1.sup.r×w.sub.2.sup.r.

(20) (1.2.2) A determination and calculation process of a corner point feature as one type of feature point is as follows:

(21) (1.2.2.1) Breakpoint detection. In a point cloud cluster, if a distance between one point and the next point is greater than a breakpoint detection threshold, then the two points are considered as breakpoints.

(22) (1.2.2.2) Corner point determination. After the point cloud cluster further segmented by the breakpoints is obtained, a starting point and an end point of a segment of point cloud cluster S.sub.i are connected to obtain a straight line l, a distance from each point in the point cloud cluster to the straight line l is calculated, and a point K.sub.j with the largest distance is found, and if the distance d.sub.j from the point to l is greater than a set threshold d.sub.max, the point is considered as a corner point, and the point cloud cluster S.sub.i is segmented into two sub-point cloud clusters using the point as a dividing point, and then the above-mentioned corner point determination process is executed on the two segmented point cloud clusters respectively, until corner points in all point cloud clusters are found. At the same time, two constraints are added: a. distances from the corner point to end points of straight lines on both sides are greater than a certain threshold; and b. the numbers of scanning points of the straight lines on both sides are greater than a set value. Corner points that do not meet the above two constraints are removed.

(23) (1.2.2.3) Straight line feature parameter Estimation. The sub-point cloud clusters on both sides of the corner point obtained in the previous step are fit into straight lines and by using a least square method, wherein a cost function in a fitting process is:
J(α,ρ)=min Σ(ρ−x.sub.i cos α−y.sub.i sin α).sup.2

(24) In the formula, ρ is a vertical distance from an origin of a local coordinate system of the vehicle body to the straight line, α is an included angle between an x axis of the coordinate system and a normal of the straight line, and (x.sub.i, y.sub.i) is coordinates of the ith scanning point in the local coordinate system. A parameter ε=var(Δρ.sub.1, Δρ.sub.2, . . . , Δρ.sub.N) is used to measure an error of a straight line feature parameter, where Δρ.sub.m represents a vertical distance from a scanning point in the straight line to the straight line, and a weight of the straight line is defined as

(25) 0$w=\frac{1}{\mathrm{.Math.}}$
for subsequent calculation of a weight of a corner point.

(26) (1.2.2.4) Fitting and intersection finding to obtain corner point coordinates and weight. After straight line feature parameters of the straight lines are obtained, an intersection point of the straight lines is found to obtain coordinates of the corner point in the local coordinate system. A weight of the coordinates of the corner point is obtained: w.sup.c=w.sub.1.sup.c×w.sub.2.sup.c, where w.sub.1.sup.c, w.sub.2.sup.c are respectively weights of the straight lines on two sides thereof.

(27) (1.2.3) After the coordinates (x.sub.L, y.sub.L) of the feature points in the local coordinate system of the vehicle body are obtained in the previous step, coordinate conversion is performed to obtain coordinates (x.sub.G, y.sub.G) in a global coordinate system (geodetic coordinate system), and the coordinates together with the weights are stored in the current feature point list; a conversion formula is:

(28) $\hspace{1em}\{\begin{array}{c}{x}_G=x_0+x_L\cos \theta -y_L\sin \theta \\ y_G=y_0+x_L\sin \theta +y_L\cos \theta \end{array}$

(29) where (x.sub.0, y.sub.0) is coordinates of the intelligent vehicle in the global coordinate system, obtained by positioning using a GNSS in combination with multiple sensors.

(30) (1.3) Repeated calibrations and corrections of feature points

(31) The feature points are recorded in a trusted list and an existing feature point list. Each column of the existing feature point list records one feature point, including multiple recorded coordinates and weights. The trusted list stores corrected coordinates and weights of the feature points.

(32) As shown in FIG. 3, specific steps are as follows:

(33) (1.3.1) A feature point observation range is determined according to the type of laser radar used, and a feature point determination and calibration process can be performed only in a relatively high detection precision range.

(34) (1.3.2) A weighted average of all recorded coordinates of any column in the existing feature point list is calculated as quasi-coordinates X.sub.s of a feature point in the column:

(35) $X_s=\underset{f=1}{\overset{F}{.Math.}}{w}_f{Z}_f$

(36) where a subscript f represents a recorded coordinate serial number of the feature point, Z.sub.f is recorded coordinates, and w.sub.f is a corresponding weight.

(37) A feature point is taken from the current feature point list and matched with the existing feature point list. If a distance between the feature point and quasi-coordinates in a column of the existing feature point list is less than a threshold, the matching is considered as successful, and coordinate and weight information of the feature point is added to the column, and step (1.3.3) is performed. If the matching is unsuccessful, the feature point is considered as a new feature point, a new column is created in the existing feature point list, and the new feature point is saved.

(38) (1.3.3) A total number of observations N.sub.A is defined as a total number of scans of the surrounding environment since the laser radar scanned a certain feature point, an effective number of observations N.sub.V is defined as a total number of successful matches of the certain feature, and an effective observation rate is defined as N.sub.V/N.sub.A×100%. If the feature point meets conditions that N.sub.V is greater than a threshold and the effective observation rate is greater than a threshold, preparation is made to add the feature point to the trusted list.

(39) (1.3.4) The number of optimal estimation points is defined as N=N.sub.V×β, where, β in the formula is an optimal estimation rate, and the quasi-coordinates after new recorded coordinates are added are re-calculated.

(40) $X_s=\underset{f=1}{\overset{F+1}{.Math.}}{w}_f{Z}_f$

(41) The nearest N points from Xs are taken and a weighted average is calculated again to obtain modified coordinates:

(42) $X_F=\underset{n=1}{\overset{N}{.Math.}}{w}_f{Z}_f$

(43) a weight is w.sub.F=1/ε.sub.V, where ε.sub.V is a distribution variance of the optimal estimation point, and at the same time ε.sub.V is amplified, and feature point coordinates outside an amplified distribution range are removed.

(44) (1.3.5) After a repeated calibration and modification process, if a certain feature w.sub.F is observed to be greater than a threshold, the modified coordinates X.sub.F and weight w.sub.F of the feature point are added to the trusted list, and positions and weights thereof are continuously updated according to steps (1.3.3) and (1.3.4) to be used as a trusted feature point when the GNSS signal quality becomes worse, as shown in FIG. 4.

(45) Step 2) GNSS information, odometer data and IMU data are combined, and a current pose of the intelligent vehicle in the global coordinate system is acquired by Kalman filtering, as shown in FIG. 5. A specific process is as follows.

(46) (2.1) A local coordinate system of the vehicle body is established by using the center of the vehicle body when the intelligent vehicle is started as an origin O.sub.G, a forward direction of the intelligent vehicle as an X.sub.G axis direction, a direction perpendicular to the X.sub.G axis and oriented to the left of the vehicle body as a Y.sub.G axis direction, and a vertically upward direction as a Z.sub.G axis direction. In this method, a coordinate system of the mounted odometer and IMU inertial unit has been converted to the local coordinate system of the vehicle body in advance, and a time clock of each sensor has been calibrated, and only movement of the intelligent vehicle in the horizontal plane is considered without pitching and rolling motion. Thus, the Z.sub.G axis coordinate is always 0 and only the yaw angle changes. Kalman filtering fusion is performed on the odometer, IMU and GNSS information to obtain the current pose (x, y, θ) of the intelligent vehicle in the global coordinate system, where θ is a heading angle, i.e. an included angle between a current heading direction of the vehicle and a positive direction of the X axis.

(47) (2.2) Displacement and heading angle changes of the intelligent vehicle are calculated by photoelectric encoders on a left rear wheel and a right rear wheel of the intelligent vehicle, a specific process of which is as follows:

(48) (2.2.1) At a moment t.sub.0, a pose (x.sub.0, y.sub.0, θ.sub.0) of the intelligent vehicle in the coordinate system is (0, 0, 0).

(49) (2.2.2) Position increments Δd.sub.L and Δd.sub.R of the left rear wheel and the right rear wheel in unit sampling time T.sub.S are calculated:

(50) $\Delta d_L=\frac{\Delta N_L*\pi *D}{P*T_S*m}\Delta d_R=\frac{\Delta N_R*\pi *D}{P*T_S*m}$

(51) where ΔN.sub.L and ΔN.sub.R are respectively pulse increments in the sampling time of the left rear wheel and the right rear wheel, D is an equivalent diameter of the wheel, P is a total number of gratings of an optical code disc, and m is a deceleration rate of the encoder;

(52) (2.2.3) A displacement change ΔD.sub.k and a heading angle change Δθ.sub.k.sup.WO of the intelligent vehicle from a moment t.sub.k-1 to a moment t.sub.k are calculated:

(53) $\Delta D_k=\frac{\Delta d_R+\Delta d_L}{2},\Delta {\theta }_k^{\mathrm{WO}}=\frac{\Delta d_R-\Delta d_L}{W}$

(54) In the formula, W is an axial length of the two rear wheels; and the superscript WO represents a data source: odometer;

(55) (2.2.4) A speed at the moment t.sub.k is calculated:

(56) $V_k=\frac{\Delta D_k}{T_s}$

(57) (2.3) A heading angular acceleration ω.sub.k may be directly obtained from the IMU at the moment t.sub.k, and the angular acceleration ω.sub.k at each moment is further integrated to obtain a heading angle change Δθ.sub.k.sup.IMU, where the heading angle θ.sub.k.sup.IMU=θ.sub.k-1+Δθ.sub.k.sup.IMU.

(58) (2.4) The GNSS information collected by the intelligent vehicle is converted into coordinates in the global coordinate system, Gaussian projection is performed at an original longitude and latitude (ϕ, λ) in a WGS-84 coordinate system, and the coordinates are converted to an earth-centered, earth-fixed coordinate system and then converted to the local coordinate system of the vehicle body to obtain a vehicle position(x.sub.k.sup.GNSS, y.sub.k.sup.GNSS) at the moment t.sub.k.

(59) (2.5) The above information is fused by a Kalman filter; a state of the intelligent vehicle at the moment t.sub.k-1 is defined as: X.sub.k-1=[x.sub.k-1, y.sub.k-1, V.sub.k-1, θ.sub.k-1, ω.sub.k-1].sup.T, motion and observation models are respectively:

(60) ${\hat{X}}_k=f\left(X_{k-1}\right)+c_k,c_k~N\left(0,Q\right),{\hat{z}}_k=h\left({\hat{X}}_k\right)+v_k,v_k~N\left(0,R\right)$

(61) and a state transition function of a movement process is:

(62) $f_{\left(X_{k-1}\right)}=\left[\begin{array}{c}{x}_{k-1}+\frac{\Delta D_k}{\Delta {\theta }_k^{\mathrm{WO}}}\left(\sin \left({\theta }_{k-1}+\Delta {\theta }_k^{\mathrm{WO}}\right)-\sin {\theta }_{k-1}\right)\\ y_{k-1}-\frac{\Delta D_k}{\Delta {\theta }_k^{\mathrm{WO}}}\left(\cos \left({\theta }_{k-1}+\Delta {\theta }_k^{\mathrm{WO}}\right)-\cos {\theta }_{k-1}\right)\\ V_k\\ {\theta }_{k-1}+\Delta {\theta }_k^{\mathrm{WO}}\\ {\omega }_k\end{array}\right]$

(63) where {circumflex over (X)}.sub.k=[{circumflex over (x)}.sub.k, ŷ.sub.k, {circumflex over (V)}.sub.k, {circumflex over (θ)}.sub.k, {circumflex over (ω)}.sub.k] is a predicted state of the intelligent vehicle at the moment t.sub.k, h({circumflex over (X)}.sub.k) is an observation model transfer function, and {circumflex over (z)}.sub.k is an observed quantity with a noise added; c.sub.k is a system process noise with a covariance of Q, and v.sub.k is a sensor measurement noise with a covariance of R; both noise matrices are Gaussian distribution white noises; an initial value of the system process noise c.sub.k may be set according to empirical values, and the sensor measurement noise v.sub.k may be adaptively adjusted according to the quality of the odometer, IMU, and GNSS equipment.

(64) A filter prediction and update process is as follows:

(65) a state quantity is predicted: {circumflex over (X)}.sub.(k|k-1)=A{tilde over (X)}.sub.k-1;

(66) an error covariance matrix is predicted: P.sub.(k|k-1)=AP.sub.k-1A.sup.T+Q;

(67) an optimal estimated state quantity is updated: {tilde over (X)}.sub.(k)={circumflex over (X)}.sub.(k|k-1)+K.sub.k[z.sub.k−H{circumflex over (X)}.sub.(k|k-1)];

(68) a Kalman gain is updated:

(69) 0$K_k=\frac{P_{\left(k.Math.k-1\right)}{H}^T}{\left[H{P}_{\left(k.Math.k-1\right)}{H}^T+R\right]};$
and

(70) the error covariance matrix is updated: P.sub.k=[I−K.sub.k H]P.sub.(k|k-1)

(71) where {circumflex over (X)}.sub.(k|k-1) is a predicted pose state quantity at the moment t.sub.k, A is a state transition matrix, {tilde over (X)}.sub.k-1 is an optimal pose state quantity at the moment t.sub.k-1, P.sub.k-1 is an updated error covariance matrix at the moment t.sub.k-1, K.sub.k is a Kalman gain at the moment t.sub.k, H is a measurement matrix, and an observed quantity is z.sub.k=[x.sub.k.sup.GNSS, y.sub.k.sup.GNSS, V.sub.k, θ.sub.k.sup.IMU, ω.sub.k]; and {tilde over (X)}.sub.k is an optimal pose state quantity at the moment t.sub.k. During traveling the intelligent vehicle, the above prediction and update process is repeated to accomplish the Kalman filtering process, and finally obtain a more accurate current position of the vehicle.

(72) Step 3) When the intelligent vehicle enters a tree-lined road, the number of locked-on GNSS satellites decreases and the GNSS signals are unstable. The GNSS signal quality is determined based on the number of locked-on GNSS satellites to determine to the timing of positioning mode changeover. When the number of locked-on satellites is greater than 14, the GNSS is used for positioning through Kalman filtering in combination with multiple sensors. When the number of locked-on satellites is less than 14, it is considered that the intelligent vehicle enters an area with poor GNSS signal quality and the GNSS signals are unavailable, so it switches to particle filter positioning by using the laser radar in combination with multiple sensors.

(73) (4) As shown in FIG. 6, after the intelligent vehicle enters the tree-lined road, the GNSS signal quality becomes worse. It is considered that the intelligent vehicle enters a blind area of the GNSS. Trusted environment feature points are observed by using the laser radar to provide a guiding observed quantity, and odometer and IMU information is fused, and a simply deduced reckoning positioning result is corrected by the particle filtering method to obtain a current pose of the intelligent vehicle. A specific process is as follows:

(74) (4.1) Particle swarm initialization.

(75) A local coordinate system of the vehicle body is established like in step (2.1) to achieve seamless transition of positioning. Using an optimal pose {tilde over (X)}.sub.0=(x.sub.0, y.sub.0, θ.sub.0) at a moment before the intelligent vehicle enters the GNSS blind area as an initial distribution of a particle swarm, a probability density function is constructed as a proposal distribution {tilde over (q)} by {tilde over (X)}.sub.0 and an error covariance matrix P.sub.0, and Mc initial particles are randomly selected, a Gaussian distribution is created for the particles around (x.sub.0, y.sub.0, θ.sub.0) in the proposal distribution, {tilde over (q)}, then the state of the ith particle at the moment t.sub.k is expressed as X.sub.k.sup.i=[x.sub.k.sup.i, y.sub.k.sup.i, θ.sub.k.sup.i].sup.T, and a weight thereof is w.sub.0.sup.i=1/M.

(76) (4.2) A particle prediction process.

(77) At the moment t.sub.k, a pose distribution of the particles at the moment t.sub.k is calculated according to control information u.sub.k of the intelligent vehicle, that is, particle displacement and heading direction estimates are obtained according to odometer and IMU information. The odometer information is acquired like in step (2.2), to obtain ΔD and Δθ.sub.k.sup.WO. The angular acceleration in the IMU is further integrated to obtain Δθ.sub.k.sup.IMU. The equation of motion is X.sub.k.sup.i=g(X.sub.k-1.sup.i)+d.sub.k-1, d.sub.k-1˜N(0, Q.sub.1), where the state transfer function is:

(78) $g_{\left(X_{k-1}^i\right)}=\left[\begin{array}{c}{x}_{k-1}^i+\frac{\Delta D_k}{\Delta {\theta }_k^{\mathrm{WO}}}\left(\sin \left({\theta }_{k-1}^i+\Delta {\theta }_k^{\mathrm{WO}}\right)-\sin {\theta }_{k-1}^i\right)\\ y_{k-1}^i-\frac{\Delta D_k}{\Delta {\theta }_k^{\mathrm{WO}}}\left(\cos \left({\theta }_{k-1}^i+\Delta {\theta }_k^{\mathrm{WO}}\right)-\cos {\theta }_{k-1}^i\right)\\ {\theta }_{k-1}^i+\Delta {\theta }_k^{\mathrm{IMU}}\end{array}\right]$

(79) In the formula, d.sub.k-1 is a system process noise with a covariance of Q.sub.1.

(80) (4.3) Feature point recognition and matching.

(81) Based on a predicted state X.sub.k.sup.i=[x.sub.k.sup.i, y.sub.k.sup.i, θ.sub.k.sup.i].sup.T of the ith particle obtained after step 3-2) at the moment t.sub.k, establishing a local coordinate system of the ith particle by using a predicted position (x.sub.k.sup.i, y.sub.k.sup.i) of the ith particle as an origin, a forward direction of the intelligent vehicle as an X direction, a direction perpendicular to the X axis and oriented to the left of the vehicle body as a Y axis direction, and a vertically upward direction as a Z axis.

(82) x, y axial distances Δx.sub.j.sup.i and Δy.sub.j.sup.i of the feature points in the trusted list and the particles in the global coordinate system are calculated one by one, where the subscript represents the jth feature point, and the superscript represents the ith particle; a Euclidean distance ΔO.sub.j.sup.i=√{square root over ((Δx.sub.j.sup.i).sup.2+(Δy.sub.j.sup.i).sup.2)} is further calculated, and if the distance is less than a high-precision detection range of the laser radar, the jth feature point is within the detection range, the feature point is stored in a comparison list, and a sequence number n is re-assigned to represent a sequence thereof, thus obtaining a total of N feature points that meet the requirement; and a distance in the global coordinate system corresponding to the nth feature point is converted to a distance in the local coordinate system of the ith particle:

(83) $\{\begin{array}{c}\Delta x_n^{i^{\prime }}=\Delta x_n^i\cos {\theta }_i^k+\Delta y_n^i\sin {\theta }_i^k;\\ \Delta y_n^{i^{\prime }}=\Delta x_n^i\sin {\theta }_i^k+\Delta y_n^i\cos {\theta }_i^k;\end{array}\hspace{1em}$

(84) where Δx.sub.n.sup.i′ and Δy.sub.n.sup.i′ are a predicted observed quantity of the nth feature point for the ih particle.

(85) At the current moment t.sub.k, the surrounding environment is scanned by the laser radar, circle centers and corner points are detected, and feature point coordinates (x.sub.q, y.sub.q) in the local coordinate system of the vehicle body are calculated and stored in the current feature point list, where q represents a serial number and (x.sub.q, x.sub.q) is an observed quantity thereof, and the total number of current feature points is q.sub.max.

(86) Differences Δx.sub.nq.sup.i and Δy.sub.nq.sup.i between the observed quantity corresponding to the feature point coordinates in the current feature point list and the predicted observed quantity of the nth calibration point for the ith particle in the comparison list are calculated one by one; and

(87) $\{\begin{array}{c}\Delta {\stackrel{\_}{x}}_{\mathrm{nq}}^i=\Delta x_n^{i^{\prime }}-x_q\\ \Delta {\stackrel{\_}{y}}_{\mathrm{nq}}^i=\Delta y_n^{i^{\prime }}-y_q\end{array}$

(88) Finally, simultaneous equations are formed by a nearest neighbor method: a minimum value

(89) $\min \left(\sqrt{{\left(\Delta {\stackrel{\_}{x}}_{\mathrm{nq}}^i\right)}^2+{\left(\Delta {\stackrel{\_}{y}}_{\mathrm{nq}}^i\right)}^2},q=1,2\mathrm{.Math.}{q}_{\max }\right)$
of the feature point n in the comparison list is calculated, a corresponding serial number q.sub.0 of the minimum value is calculated, thus forming simultaneous equations of n and q.sub.0, with a corresponding error Δx.sub.nq.sub.0.sup.i and Δy.sub.nq.sub.0.sup.i, denoted by Δx.sub.n.sup.i and Δy.sub.n.sup.i, the comparison list is traversed and the process is executed to accomplish feature point recognition and matching.

(90) (4.4) Weight update.

(91) Weights of the particles are updated according to the difference between the observed quantity and the predicted observed quantity:

(92) $w_k^i=w_{k-1}^i\underset{n=1}{\overset{N}{.Math.}}\frac{1}{2\pi {\sigma }_x{\sigma }_y}{e}^{-\frac{1}{2}\left({\left(\frac{\Delta {\stackrel{\_}{x}}_n^i}{{\sigma }_x}\right)}^2+{\left(\frac{\Delta {\stackrel{\_}{y}}_n^i}{{\sigma }_y}\right)}^2\right)}.Math.w_F^n$

(93) where N represents the number of feature points matching the current surrounding environment of the intelligent vehicle from the trusted list, σ.sub.x and σ.sub.y represent measurement variances of the laser radar, and w.sub.F.sup.n represent weights of the feature points.

(94) (4.5) Weight normalization, which is performed by a formula

(95) ${\tilde{w}}_k^i=\frac{w_k^i}{\underset{i=1}{\overset{M}{.Math.}}{w}_k^i}.$

(96) (4.6) Resampling. The number of effective particles

(97) ${\tilde{N}}_{\mathrm{eff}}=\frac{1}{\underset{i=1}{\overset{M}{.Math.}}{\tilde{w}}_k^i}$
is calculated. If N.sub.eff is less than a threshold, a resampling process is performed, in which a low-variance resampling is used for sampling, and particles with larger weights are more likely to be retained, and all particle weights are normalized to 1/M after resampling is completed. Otherwise, step (4.7) is executed.

(98) (4.7) State estimation output.

(99) At the moment t.sub.k, a system state estimation value is

(100) $X_k=\underset{i=1}{\overset{M}{.Math.}}{\tilde{w}}_k^i{X}_k^i,$
to obtain a current locating position of the vehicle. Step (4.2) to this step are repeated to accomplish a particle filtering and positioning process.

(101) Finally, it should be noted that the above embodiments are only used for describing instead of limiting the technical solutions of the present invention. Although the present invention is described in detail with reference to the embodiments, persons of ordinary skill in the art should understand that modifications or equivalent substitutions of the technical solutions of the present invention should be encompassed within the scope of the claims of the present invention so long as they do not depart from the spirit and scope of the technical solutions of the present invention.