Method for detecting surface gradient of unstructured road based on lidar

1. A method for detecting a surface gradient of an unstructured road based on Lidar, comprising the following steps:

(1) obtaining an environmental point cloud of a target region by using a Lidar, and filtering out a point cloud of a non-surface region to obtain a point cloud of a surface region, wherein the target region comprises the surface region and the non-surface region;

(2) calculating a normal vector of the point cloud of the surface region, and obtaining an included angle of the surface of each region relative to a coordinate system of the Lidar; and

(3) obtaining pitching angle information of a vehicle at this time from a vehicle-mounted inertial measurement unit (IMU), and correcting the included angle with the pitching angle information to obtain a gradient of the surface of each region.

2. The method according to claim 1 , wherein step (1) specifically comprises the following steps:

(1.1) ranking all input point clouds by height in an ascending order, and calculating a height mean of first N LPR points;

(1.2) extracting a point cloud of an initial surface according to the height mean and a distance threshold, and obtaining a surface plane by fitting according to the point cloud of the initial surface; and

(1.3) performing N itr iterations, performing point cloud traversal, and continuously adding point clouds within the distance threshold to the surface plane to the point cloud of the initial surface, thereby obtaining a surface point cloud.

3. The method according to claim 1 , wherein step (2) comprises obtaining the corresponding normal vector n of the surface point cloud by solving an optimized objective function of each region, allowing point multiplication of a vector formed by a central point of the surface point cloud of the region and each point therein, and the normal vector nl to approach 0 as much as possible, namely, maintaining perpendicular as much as possible.

4. The method according to claim 3 , wherein step (2) further comprises obtaining an included angle θ between a component of the normal vector n on the plane of the coordinate system YZ of the Lidar and a negative direction of Y axis of the coordinate system YZ of the Lidar.

5. The method according to claim 4 , wherein the obtaining the normal vector n in step (2) specifically comprises the following steps:

expressing the optimized objective function as

min

c

,

n

,



n



=

1

∑

i

=

1

n

(

(

x

i

-

c

)

T

⁢

n

)

2

,

and letting y i =x i −c;

expressing the above formula as

min



n



=

1

∑

i

=

1

k

(

y

i

T

⁢

n

)

2

;

continuously deducing

min

n

T

⁢

n

=

1

⁢

∑

i

=

1

n

n

T

⁢

y

i

⁢

y

i

T

⁢

n

→

min

n

T

⁢

n

=

1

⁢

n

T

(

∑

i

=

1

n

y

i

⁢

y

i

T

)

⁢

n

→

min

n

T

⁢

n

=

1

⁢

n

T

(

YY

T

)

⁢

n

,

wherein YY T is a covariance matrix of 3*3;

solving by using Lagrange algorithm, and letting f((n)=n T (YY T ):

τ

⁢

(

n

,

λ

)

=

f

⁢

(

n

)

-

λ

⁢

(

n

T

⁢

n

-

1

)

;

∇

τ

=

0

⟹

∂

τ

∂

n

=

0

⟹

(

YY

T

)

⁢

n

=

λ

⁢

n

;

n

T

⁢

n

=

1

;

and

wherein the normal vector n is a feature vector of the covariance matrix YY T ; by implementing principal component analysis (PCA), a normal vector of a target point cloud cluster is a feature vector corresponding to a minimum feature value, and the included angle between the component of the normal vector on the plane of the coordinate system YZ of the Lidar and the negative direction of Y axis of the coordinate system YZ of the Lidar at this time is denoted as θ.

6. The method according to claim 5 , wherein the correcting the included angle with the pitching angle information in step (3) specifically comprises supplementing the pitching angle information to the included angle.