CN101975951B

CN101975951B - Field environment barrier detection method fusing distance and image information

Info

Publication number: CN101975951B
Application number: CN 201010195586
Authority: CN
Inventors: 付梦印; 杨建�; 孟红; 刘进; 杨毅
Original assignee: Beijing Institute of Technology BIT
Current assignee: Beijing Institute of Technology BIT
Priority date: 2010-06-09
Filing date: 2010-06-09
Publication date: 2013-03-20
Anticipated expiration: 2030-06-09
Also published as: CN101975951A

Abstract

The invention belongs to the technical field of obstacle detection, and specifically relates to a method for detecting obstacles in a field environment by fusing distance and image information. The invention provides a method for facilitating the vehicle to plan a driving path and improving the autonomous ability of the unmanned vehicle for field driving. It includes the following steps: establishment of a mathematical model, obstacle detection of laser radar distance data, camera image processing and result fusion. The present invention can detect and identify common obstacles such as grass, roads, trees, bushes, etc., which are commonly seen by unmanned vehicles in the field, and model the color of the drivable area to further detect abnormal parts of the drivable area . The surrounding environment of the vehicle can be divided into "non-drivable area", "drivable area" and "unknown area", which is conducive to vehicle planning driving path and improves the autonomy of unmanned vehicles in the wild.

Description

A Method of Obstacle Detection in Field Environment Based on Fusion of Distance and Image Information

技术领域 technical field

本发明属于障碍检测技术领域，具体涉及一种融合距离和图像信息的野外环境障碍检测方法。 The invention belongs to the technical field of obstacle detection, and in particular relates to a method for detecting obstacles in a field environment by fusing distance and image information. the

背景技术 Background technique

无人驾驶车辆在行驶道路上的障碍物检测是环境感知技术研究领域中的重要组成部分。在障碍检测的应用中，常用的传感器有激光雷达、摄像机、毫米波雷达、超声波传感器等。激光雷达通过测量发射光和从物体表面反射光之间的时间差来测量距离。它可以直接获取距离数据，为车辆提供了便捷直观的环境描述信息。激光雷达的具体应用有很多形式，例如物体的定位和跟踪，环境建模和避障，定位和地图构建(SLAM)，地形和地貌特征的分类等，还可以利用激光雷达的回波强度信息进行障碍检测和跟踪。摄像机得到的周围环境的物理信息丰富，且具有更高的隐蔽性，因此得到了广泛的应用。无论是单摄像机还是多摄像机的应用，现有技术中都有很多研究。但是摄像机图像容易受到光照、烟雾、环境的影响，多摄像机形成的立体视觉虽然能够得到实际距离，但是图像间的匹配复杂、耗时，影响了实际应用效果。毫米波雷达体积小，重量轻，可测距离远，穿透雾、烟、灰尘的能力强，但是分辨率和精度较低，主要用于汽车防撞以及基于车距检测功能的车辆追踪系统和追尾减轻控制刹车系统等。超声波传感器通过检测超声波从发射器发射到遇到障碍反射回接收器的时间来测距。超声波传感器体积小又廉价，应用较为普遍，但是由于精度低，只能用在对环境感知精度要求不高的场合。 Obstacle detection of unmanned vehicles on the road is an important part of the research field of environmental perception technology. In the application of obstacle detection, commonly used sensors include lidar, camera, millimeter wave radar, ultrasonic sensor, etc. LiDAR measures distance by measuring the time difference between when light is emitted and when it is reflected from an object's surface. It can directly obtain the distance data and provide convenient and intuitive environment description information for the vehicle. There are many specific applications of lidar, such as object positioning and tracking, environmental modeling and obstacle avoidance, localization and map construction (SLAM), classification of terrain and landform features, etc., can also use the echo intensity information of lidar for Obstacle detection and tracking. The physical information of the surrounding environment obtained by the camera is rich and has higher concealment, so it has been widely used. Regardless of the application of a single camera or multiple cameras, there are many studies in the prior art. However, camera images are easily affected by light, smog, and the environment. Although the stereo vision formed by multiple cameras can obtain the actual distance, the matching between images is complicated and time-consuming, which affects the actual application effect. Millimeter wave radar is small in size, light in weight, has a long measurable distance, and has a strong ability to penetrate fog, smoke, and dust, but its resolution and accuracy are low. It is mainly used for vehicle collision avoidance and vehicle tracking systems based on vehicle distance detection functions and Rear-end collision reduction control brake system, etc. Ultrasonic sensors measure distance by detecting the time from when an ultrasonic wave is emitted from the transmitter to when it encounters an obstacle and is reflected back to the receiver. Ultrasonic sensors are small and cheap, and are widely used. However, due to their low precision, they can only be used in occasions that do not require high precision in environmental perception. the

而激光雷达扫描数据和光学图像对环境的描述具有很强的互补性，如激光扫描数据可以快速准确地获取物体表面密集的三维坐标，而光学图像包含了丰富的色彩。因此，融合激光扫描数据与光学图像可以获得无人驾驶车辆行驶环境更加全面的信息，提高了障碍检测的快速性和对复杂环境的适应能力。关于距离和图像的信息融合问题，有不少学者进行了研究。Peter在室内环境下，将单目摄像机图像和声纳测距的数据进行融合，建立了室内环境模型，但是不适合在室外复杂野外环境下应用。马里兰大学的Tsai-hong Hong针对美国Demo III计划中试验无人车在野外环境行驶的要求，提出了融合三维激光雷达数据和摄像机图像的障碍检测算法，只针对三种典型路况即路标、池塘和道路进行识别，缺乏对环境中其它常见障碍例如草地、树木进行识别的研究。Monteiro使用激光雷达和摄像机融合后的信息，用于检测道路上移动的行人和车辆，没有对周围环境进行识别理解。项志宇提出了一种融合激光雷达与摄像机信息的草丛中障碍物检测方法。该方法先将激光雷达数据分组，判别出障碍点，然后映射到摄像机图像中，以此分割出摄像机图像中的不可行驶区域和非不可行驶区域，降低了纯激光雷达判别时出现的误判，并改善了最终障碍物轮廓检测的完整性。但是当障碍物和草丛的颜色很接近时，融合检测的效果会有所下降，而且没有根据判定的不可行驶区域，划分出可行驶区域，缺乏对环境更具体的理解。综上，现有技术没有能够为无人驾驶车辆在野外行驶条件下对常见障碍进行检测，从而便于车辆规划行驶路径，提高无人驾驶车辆野外行驶的自主能力的障碍检测方法。 However, lidar scanning data and optical images are highly complementary in describing the environment. For example, laser scanning data can quickly and accurately obtain dense three-dimensional coordinates on the surface of objects, while optical images contain rich colors. Therefore, the fusion of laser scanning data and optical images can obtain more comprehensive information on the driving environment of unmanned vehicles, which improves the speed of obstacle detection and the adaptability to complex environments. Many scholars have conducted research on the information fusion of distance and images. In the indoor environment, Peter fused the data of the monocular camera image and the sonar ranging to establish an indoor environment model, but it is not suitable for application in the complex outdoor environment. Tsai-hong Hong from the University of Maryland proposed an obstacle detection algorithm that combines three-dimensional lidar data and camera images in response to the requirements of the U.S. Demo III program to test unmanned vehicles in the wild environment, only for three typical road conditions, namely road signs, ponds and There is a lack of research on the identification of other common obstacles in the environment such as grass and trees. Monteiro uses information fused from lidar and cameras to detect moving pedestrians and vehicles on the road without recognizing and understanding the surrounding environment. Xiang Zhiyu proposed a method for detecting obstacles in grass by fusing lidar and camera information. In this method, the lidar data is grouped first, the obstacle points are identified, and then mapped to the camera image, so as to segment the non-drivable area and the non-drivable area in the camera image, which reduces the misjudgment that occurs when the pure lidar is judged. And improved the integrity of the final obstacle contour detection. However, when the colors of obstacles and grass are very close, the effect of fusion detection will be reduced, and the drivable area is not divided according to the determined undrivable area, lacking a more specific understanding of the environment. To sum up, the existing technology does not have an obstacle detection method that can detect common obstacles for unmanned vehicles under field driving conditions, so as to facilitate vehicle planning of driving paths and improve the autonomous ability of unmanned vehicles in the field. the

发明内容 Contents of the invention

本发明的目的是提供一种能够为无人驾驶车辆在野外行驶条件下对常见障碍进行检测，从而便于车辆规划行驶路径，提高无人驾驶车辆野外行驶的自主能力的融合距离和图像信息的野外环境障碍检测方法。 The purpose of the present invention is to provide a field that can detect common obstacles for unmanned vehicles under field driving conditions, thereby facilitating vehicle planning driving paths, and improving the fusion distance and image information of the autonomous ability of unmanned vehicles driving in the field. Environmental obstacle detection method. the

本发明是这样实现的： The present invention is achieved like this:

一种融合距离和图像信息的野外环境障碍检测方法，包括如下步骤： A method for detecting obstacles in a field environment by fusing distance and image information, comprising the following steps:

第一步：建立数学模型； The first step: establish a mathematical model;

第二步：激光雷达距离数据障碍检测； The second step: lidar distance data obstacle detection;

第三步：摄像机图像处理； The third step: camera image processing;

第四步：结果融合。 Step 4: Result fusion. the

如上所述的建立数学模型步骤中，建立车体及激光雷达、摄像机和惯导坐标系，并将所述坐标系相互关联，具体包括如下步骤： In the step of establishing a mathematical model as described above, the vehicle body, laser radar, camera and inertial navigation coordinate system are established, and the coordinate systems are correlated with each other, which specifically includes the following steps:

(1)建立车体坐标系O_bX_bY_bZ_b； (1) Establish car body coordinate system O _b X _b Y _b Z _b ;

原点O_b位于车体的某个固定点，原点为激光雷达光源在与地面重合的平面上的投影，X_b轴指向车体正右方，Y_b轴指向车体正前方，Z_b轴指向车体上方；车体坐标系原点到地面的高度为h₀； The origin O _b is located at a fixed point on the car body. The origin is the projection of the lidar light source on the plane coincident with the ground. The X _b axis points to the right of the car body, the Y _b axis points to the front of the car body, and the Z _b axis points to Above the car body; the height from the origin of the car body coordinate system to the ground is h ₀ ;

(2)建立激光雷达坐标系O_lX_lY_lZ_l； (2) Establish the laser radar coordinate system O _l X _l Y _l Z _l ;

定义该坐标系原点O_l位于激光雷达光源中心，X_l轴竖直向上，Y_l轴水平向右，Z_l轴指向车体前方； It is defined that the origin _O1 of the coordinate system is located at the center of the laser radar light source, the _X1 axis is vertically upward, the _Y1 axis is horizontally to the right, and the _Z1 axis points to the front of the car body;

(3)建立摄像机坐标系O_cX_cY_cZ_c； (3) Establish camera coordinate system O _c X _c Y _c Z _c ;

坐标系原点O_c位于摄像机镜头的焦点，X_c轴和图像平面水平方向平行，方向向右；Y_c轴和图像平面的竖直方向平行，方向向下；Z_c轴垂直于图像平面，指向正前方； The origin of the coordinate system O _c is located at the focal point of the camera lens, the X _c axis is parallel to the horizontal direction of the image plane, and the direction is to the right; the Y _c axis is parallel to the vertical direction of the image plane, and the direction is downward; the Z _c axis is perpendicular to the image plane, pointing to In front of;

(4)建立惯导坐标系O_IX_IY_IZ_I； (4) Establish the inertial navigation coordinate system O _I X _I Y _I Z _I ;

惯导坐标系和车体坐标系方向一致； The inertial navigation coordinate system is in the same direction as the vehicle body coordinate system;

(5)确定坐标系转换关系，具体包括如下步骤： (5) Determine the coordinate system conversion relationship, specifically including the following steps:

(a)从激光雷达坐标系O_lX_lY_lZ_l转换到车体坐标系O_bX_bY_bZ_b； (a) Transform from the lidar coordinate system O _l X _l Y _l Z _l to the car body coordinate system O _b X _b Y _b Z _b ;

先绕Y_l轴旋转90°，再绕Z_l轴旋转90°，然后沿Z_l轴方向向下平移h₀的距离； Rotate 90° around the Y _l- axis first, then rotate 90° around the Z _l- axis, and then translate down the distance of _h0 along the Z _l- axis;

$[\begin{matrix} {x x}_{b b} \\ {y the y}_{b b} \\ {z z}_{b b} \end{matrix}] = = [\begin{matrix} 00 & 11 & 00 \\ - - 11 & 00 & 00 \\ 00 & 00 & 11 \end{matrix}] [\begin{matrix} 00 & 00 & - - 11 \\ 00 & 11 & 00 \\ 11 & 00 & 00 \end{matrix}] [\begin{matrix} {x x}_{l l} \\ {y the y}_{l l} \\ {z z}_{l l} \end{matrix}] + + [\begin{matrix} 00 \\ 00 \\ {h h}_{00} \end{matrix}] = = [\begin{matrix} {y the y}_{l l} \\ {z z}_{l l} \\ {x x}_{l l} + + {h h}_{00} \end{matrix}] - - - - - - ((22))$

式中各参数含义与上述相同； The meaning of each parameter in the formula is the same as above;

(b)摄像机坐标系转换到图像坐标系； (b) Convert the camera coordinate system to the image coordinate system;

根据现有的摄像机针孔模型，将图像坐标系下点的三维坐标转换成图像上的像素位置； According to the existing camera pinhole model, the three-dimensional coordinates of the points in the image coordinate system are converted into pixel positions on the image;

$s the s [\begin{matrix} u u \\ v v \\ 11 \end{matrix}] = = [\begin{matrix} {α α}_{x x} & γ γ & {u u}_{00} \\ 00 & {α α}_{y the y} & {v v}_{00} \\ 00 & 00 & 11 \end{matrix}] [\begin{matrix} {x x}_{c c} \\ {y the y}_{c c} \\ {z z}_{c c} \end{matrix}] = = A A [\begin{matrix} {x x}_{c c} \\ {y the y}_{c c} \\ {z z}_{c c} \end{matrix}] - - - - - - ((33))$

其中A为摄像机内参数，

α_x、α_y分别是图像坐标系中u轴和v轴的尺度因子，u₀、v₀是光学中心，γ是u轴和v轴不垂直因子，很多情况下，令γ＝0；式中其它参数含义与上述相同； where A is the internal parameter of the camera,

α _x , α _y are the scale factors of u-axis and v-axis in the image coordinate system respectively, u ₀ and v ₀ are the optical center, γ is the non-perpendicular factor of u-axis and v-axis, in many cases, let γ=0; The meanings of other parameters in are the same as above;

(c)从激光雷达坐标系O_lX_lY_lZ_l转换到摄像机坐标系O_cX_cY_cZ_c； (c) Transform from the lidar coordinate system O _l X _l Y _l Z _l to the camera coordinate system O _c X _c Y _c Z _c ;

$[\begin{matrix} {x x}_{c c} \\ {y the y}_{c c} \\ {z z}_{c c} \end{matrix}] = = R R \cdot \cdot [\begin{matrix} {x x}_{l l} \\ {y the y}_{l l} \\ {z z}_{l l} \end{matrix}] + + t t - - - - - - ((1717))$

其中R和t分别为从激光雷达坐标系转换到摄像机坐标系的旋转矩阵和平移向量；式中其它参数含义与上述相同 Where R and t are the rotation matrix and translation vector converted from the lidar coordinate system to the camera coordinate system respectively; the meanings of other parameters in the formula are the same as above

(d)距离数据校正； (d) distance data correction;

假设惯导设备输出的车体俯仰角为α、滚转角为γ，校正后的距离数据通过下式获得： Assuming that the pitch angle of the car body output by the inertial navigation equipment is α, and the roll angle is γ, the corrected distance data is obtained by the following formula:

$[\begin{matrix} {x x}_{g g} \\ {y the y}_{g g} \\ {z z}_{g g} \end{matrix}] = = [\begin{matrix} 11 & 00 & 00 \\ 00 & cos cos ((- - α α)) & sin sin ((- - α α)) \\ 00 & - - sin sin ((- - α α)) & cos cos ((- - α α)) \end{matrix}] [\begin{matrix} cos cos ((- - γ γ)) & 00 & - - sin sin ((- - γ γ)) \\ 00 & 11 & 00 \\ sin sin ((- - γ γ)) & 00 & cos cos ((- - γ γ)) \end{matrix}] [\begin{matrix} {x x}_{b b} \\ {y the y}_{b b} \\ {z z}_{b b} \end{matrix}] - - - - - - ((3030))$

式中参数含义与上述相同。 The meanings of the parameters in the formula are the same as above. the

如上所述的激光雷达距离数据障碍检测步骤，包括如下步骤： The above-mentioned lidar distance data obstacle detection step includes the following steps:

(1)距离数据预处理； (1) distance data preprocessing;

对激光雷达距离数据进行滤波； Filter the lidar range data;

(2)区域分割； (2) Region segmentation;

将激光雷达扫描环境得到的三维点云进行分割，得到多个区域； Segment the 3D point cloud obtained by the lidar scanning environment to obtain multiple regions;

(3)区域识别； (3) Area identification;

对草地、道路、树木、灌木丛进行分析识别。 Analyze and identify grasslands, roads, trees, and bushes. the

如上所述的区域分割步骤，包括如下步骤： The region segmentation steps as described above include the following steps:

(a)检索某三维空间点p_i周围领域中最近的k个点，计算由这k+1个点组成的点集Q_i的法线，作为该扫描点的法向量； (a) Retrieve the nearest k points in the field around a certain three-dimensional space point p _i , and calculate the normal of the point set Q _i composed of these k+1 points, as the normal vector of the scanning point;

(b)将三维空间点p_i的坐标

和法线

组成特征向量

两个三维点的笛卡尔空间距离为ρ_e(p_i，p_j)＝‖p_i-p_j‖，其中，p_j的坐标

p_j和其法线组成特征向量角度距离为ρ_a(n_i，n_j)＝0.5-0.5×<n_i，n_j>/‖n_i‖‖n_j‖，其中，

为空间距离和角度距离设置不同的阈值ρ_e，max、ρ_a，max；当两个点之间的空间距离或者角度距离大于相应阈值时，认为这两个点不在同一区域； (b) The coordinates of point p _i in three-dimensional space

and normal

Composing eigenvectors

The Cartesian space distance of two three-dimensional points is ρ _e (p _i , p _j )=‖p _i -p _j ‖, where the coordinates of p _j

p _j and its normal Composing eigenvectors The angular distance is ρ _a (n _i , n _j )=0.5-0.5×<n _i , n _j >/‖n _i ‖‖n _j ‖, where,

Set different thresholds ρ _{e, max} , ρ _{a, max} for the spatial distance and angular distance; when the spatial distance or angular distance between two points is greater than the corresponding threshold, the two points are not considered to be in the same area;

(c)对所有相邻点计算相互之间的空间距离ρ_e和角度ρ_a距离；如果ρ_e≤ρ_e，max且ρ_a≤ρ_a，max，将这两个点聚类合并； (c) Calculate the mutual spatial distance ρ _e and angle ρ _a distance for all adjacent points; if ρ _e ≤ ρ _{e, max} and ρ _a ≤ ρ _{a, max} , cluster and merge these two points;

(d)如果某一点无法和其它点聚类，新建一个区域； (d) If a certain point cannot be clustered with other points, create a new area;

(e)将所有点聚类后，检验每类所拥有点的数量，如果数量少于某一阈值n_cmin，而且类内点的平均高度远远大于车辆跨越障碍的高度h_max，该类为噪声，将其删除。 (e) After clustering all the points, check the number of points owned by each class. If the number is less than a certain threshold n _cmin and the average height of the points in the class is much greater than the height h _max of the vehicle crossing the obstacle, the class is noise, remove it.

如上所述的区域分割步骤中，k的取值范围是10～30。 In the region division step described above, the value range of k is 10-30. the

如上所述的区域识别步骤具体包括如下步骤： The above-mentioned region identification step specifically includes the following steps:

(a)计算区域特征； (a) Computing regional features;

计算平均高度、奇异值、区域曲面法向量；计算方法如下： Calculate the average height, singular value, and area surface normal vector; the calculation method is as follows:

平均高度：

n为该区域内点的数量，i为点的序号； average height:

n is the number of points in the area, i is the serial number of the point;

奇异值：[σ₁ σ₂ σ₃]；将该区域内所有点的坐标组成矩阵，进行奇异值分解U∑V^T，∑中的元素即为奇异值，并且将σ₁、σ₂、σ₃由大到小排序； Singular value: [σ ₁ σ ₂ σ ₃ ]; the coordinates of all points in the area are formed into a matrix, and the singular value decomposition U∑V ^T is performed. The elements in ∑ are singular values, and σ ₁ , σ ₂ , σ ₃ Sort from big to small;

区域曲面法向量：γ；最小的奇异值σ₃对应的V^T中的单位向量； Area surface normal vector: γ; the unit vector in V ^T corresponding to the smallest singular value σ ₃ ;

(b)识别，包括如下步骤： (b) identification, including the following steps:

①草地识别； ①Grass recognition;

当满足以下几个条件时，识别为草地； When the following conditions are met, it is recognized as grassland;

1)区域内点平均高度低于车辆越障高度h_max； 1) The average height of points in the area is lower than the vehicle obstacle clearance height h _max ;

2)区域内点的穿透率高，奇异值分布为σ₁≈σ₂≈σ₃； 2) The penetration rate of points in the area is high, and the singular value distribution is σ ₁ ≈ _{σ 2} ≈ _{σ 3} ;

3)区域法向量γ向上； 3) The area normal vector γ is upward;

在识别出草地之后，将这些点合并到同一区域，并且标注该区域为可行驶区域； After identifying the grass, merge these points into the same area, and mark this area as a drivable area;

②道路识别； ②Road identification;

当满足以下几个条件时，识别为道路： When the following conditions are met, it is recognized as a road:

1)区域内点高度低于车辆越障高度

1) The point height in the area is lower than the vehicle obstacle clearance height

2)区域内点的穿透率低，σ₁＞σ₂＞＞σ₃； 2) The penetration rate of points in the area is low, σ ₁ >σ ₂ ＞＞σ ₃ ;

3)区域法向量γ向上； 3) The area normal vector γ is upward;

③树干识别； ③trunk identification;

当奇异值的分布规律为σ₁＞＞σ₂≈σ₃，区域法向量γ位于水平方向时，识别为树干； When the distribution of singular values is σ ₁ ＞＞σ ₂ ≈σ ₃ , and the area normal vector γ is in the horizontal direction, it is recognized as a tree trunk;

④灌木丛识别； ④Recognition of bushes;

当奇异值为σ₁＞σ₂＞σ₃，，区域法向量γ也位于水平方向时，识别为灌木丛。 When the singular value is σ ₁ >σ ₂ >σ ₃ , and the area normal vector γ is also in the horizontal direction, it is recognized as a bush.

如上所述的识别步骤中，当奇异值σ₁与σ₂的比值σ₁/σ₂在[1 4]内时，σ₁≈σ₂；当奇异值σ₁与σ₂的比值σ₁/σ₂大于50时，奇异值σ₁>>σ₂。 In the identification step mentioned above, when the ratio σ ₁ /σ ₂ of the singular value σ ₁ to σ ₂ is within [1 4], σ ₁ ≈σ ₂ ; when the ratio σ 1 /σ ₂ of the singular value σ ₁ to σ ₂ When σ ₂ is greater than 50, the singular value σ ₁ >>σ ₂ .

如上所述的摄像机图像处理步骤，具体包括如下步骤： The camera image processing steps as described above specifically include the following steps:

(1)将划分出来的区域内的激光雷达数据，转换到摄像机的图像坐标系下； (1) Transform the laser radar data in the divided area into the image coordinate system of the camera;

(2)对初步判断的可行驶区域进一步识别； (2) Further identification of the initially judged drivable area;

具体包括如下步骤： Specifically include the following steps:

(a)建立高斯混合模型； (a) Establish a Gaussian mixture model;

假设图像中像素点的序列为{x₁，x₂…x_i}，第i个像素点的值为x_i＝[R_i，G_i，B_i]，该像素点的概率为： Assuming that the sequence of pixels in the image is {x ₁ , x ₂ ... x _i }, the value of the i-th pixel is x _i =[R _i , G _i , B _i ], the probability of this pixel is:

$P P (({x x}_{i i})) = = {Σ Σ}_{k k = = 11}^{K K} {ω ω}_{k k,, i i} η η (({x x}_{i i},, {μ μ}_{k k,, i i},, {Σ Σ}_{k k,, i i})) - - - - - - ((3131))$

其中K是高斯分布的个数，选3～5个，ω_k，i是权重的估计值，μ_k，i是第k个高斯分布的均值，∑_k，i是第k个高斯分布的协方差矩阵，假设协方差矩阵符合的规律；像素点i在第k个高斯分布函数中的概率密度由下式计算： Among them, K is the number of Gaussian distributions, choose 3 to 5, ω _{k, i} is the estimated value of the weight, μ _{k, i} is the mean value of the kth Gaussian distribution, ∑ _{k, i} is the coordinating value of the kth Gaussian distribution variance matrix, assuming that the covariance matrix conforms to The law; the probability density of pixel i in the kth Gaussian distribution function is calculated by the following formula:

$η η (({x x}_{i i},, μ μ,, Σ Σ)) = = \frac{11}{{((22 π π))}^{n no / / 22} {| | Σ Σ | |}^{11 / / 22}} {e e}^{- - \frac{11}{22} {(({x x}_{i i} - - {μ μ}_{i i}))}^{T T} {Σ Σ}^{- - 11} (({x x}_{i i} - - {μ μ}_{i i}))} - - - - - - ((3232))$

μ_i为第i个高斯分布的均值，∑是高斯分布的协方差矩阵； μ _i is the mean value of the i-th Gaussian distribution, ∑ is the covariance matrix of the Gaussian distribution;

(b)区域识别； (b) area identification;

将建立的可行驶区域颜色的高斯模型与当前待判定区域的图像进行绝对值比较，如果|x_i-f|≥T，T为阈值，则该点为非可行驶区域点。 Compare the absolute value of the established Gaussian model of the color of the drivable area with the image of the current area to be determined. If | _xi -f|≥T, T is the threshold, then the point is a non-drivable area point.

如上所述的结果融合步骤，具体包括如下步骤： The above-mentioned result fusion step specifically includes the following steps:

将激光雷达和摄像机的判别结果进行融合，判断可行驶区域和不可行驶区域，构建环境地图；综合激光雷达和摄像机的判别结果，将栅格地图划分为可行驶区域和不可行驶区域，划分的原则是： Integrate the discrimination results of lidar and camera to judge the drivable area and non-drivable area, and construct an environmental map; integrate the discrimination results of lidar and camera, divide the grid map into drivable area and non-drivable area, the principle of division yes:

(1)若激光雷达判别结果为不可行驶区域，则将栅格地图划分为不可行驶区域； (1) If the lidar discrimination result is a non-driving area, divide the grid map into a non-driving area;

(2)若激光雷达判别结果为可行驶区域，则需要根据摄像机判别结果进行分析； (2) If the lidar discrimination result is a drivable area, it needs to be analyzed according to the camera discrimination result;

(a)若摄像机判别结果为可行驶区域，则为可行驶区域； (a) If the camera discrimination result is a drivable area, it is a drivable area;

(b)若摄像机判别结果为不可行驶区域，则为不可行驶区域； (b) If the camera discrimination result is a non-driving area, it is a non-driving area;

(3)若存在激光雷达和摄像机均未探测到的区域，则该区域为未知区域。 (3) If there is an area that neither the lidar nor the camera detects, then the area is an unknown area. the

本发明的有益效果是： The beneficial effects of the present invention are:

本发明可以为无人驾驶车辆在野外行驶条件下常见到的障碍例如草地、道路、树木、灌木丛等进行检测识别，并对可行驶区域的色彩建模，进一步检测出可行驶区域的异常部分。可以将车辆周围环境划分出“不可行驶区域”、“可行驶区域”和“未知区域”等，有利于车辆规划行驶路径，提高无人驾驶车辆野外行驶的自主能力。 The present invention can detect and recognize obstacles such as grassland, roads, trees, bushes, etc. commonly seen by unmanned vehicles in the field, and model the color of the drivable area to further detect abnormal parts of the drivable area . The surrounding environment of the vehicle can be divided into "non-drivable area", "drivable area" and "unknown area", which is conducive to the planning of the vehicle's driving path and improves the autonomous ability of the unmanned vehicle to drive in the wild. the

附图说明 Description of drawings

图1是本发明的一种融合距离和图像信息的野外环境障碍检测方法的流程图； Fig. 1 is a flow chart of the field environment obstacle detection method of a kind of fusion distance and image information of the present invention;

图2是本发明的车体坐标系、激光雷达坐标系和摄像机坐标系的相互位置关系示意图； Fig. 2 is the mutual position relation schematic diagram of vehicle body coordinate system, lidar coordinate system and camera coordinate system of the present invention;

图3是本发明的从激光雷达坐标系转换到摄像机坐标系坐标转换示意图。 Fig. 3 is a schematic diagram of coordinate conversion from the laser radar coordinate system to the camera coordinate system according to the present invention. the

具体实施方式 Detailed ways

下面结合附图和实施例对本发明的一种融合距离和图像信息的野外环境障碍检测方法进行介绍： A kind of field environment obstacle detection method of fusion distance and image information of the present invention is introduced below in conjunction with accompanying drawing and embodiment:

如图1所示，一种融合距离和图像信息的野外环境障碍检测方法，包括如下步骤： As shown in Figure 1, a field environment obstacle detection method that fuses distance and image information includes the following steps:

在建立车辆感知模型时，需要建立车体及激光雷达、摄像机和惯导坐标系，并将这些坐标系相互关联起来，具体包括如下步骤： When establishing a vehicle perception model, it is necessary to establish the coordinate system of the vehicle body, lidar, camera and inertial navigation, and correlate these coordinate systems with each other, including the following steps:

建立和车辆本体固连在一起的车体坐标系，它随车辆一起运动。车体坐标系的定义为：原点O_b位于车体的某个固定点，指定原点为激光雷达光源在与地面重合的平面上的投影，X_b轴指向车体正右方，Y_b轴指向车体正前方，Z_b轴指向车体上方。车体坐标系原点到地面的高度为h₀。 Establish a car body coordinate system that is fixedly connected with the vehicle body, and it moves with the vehicle. The car body coordinate system is defined as: the origin O _b is located at a fixed point on the car body, the designated origin is the projection of the laser radar light source on the plane coincident with the ground, the X _b axis points to the right of the car body, and the Y _b axis points to The car body is directly in front, and the Z _b axis points to the top of the car body. The height from the origin of the car body coordinate system to the ground is h ₀ .

激光雷达安装在云台上，激光雷达发射出的激光只能扫描到二维数据，通过云台摆动才能实现三维扫描。此时激光雷达坐标系并不和激光雷达固连，而是和云台一起作为整体考虑的，即定义该坐标系原点O_l位于激光雷达光源中心，X_l轴竖直向上，Y_l轴水平向右，Z_l轴指向车体前方。 The lidar is installed on the pan/tilt, and the laser emitted by the lidar can only scan two-dimensional data, and the three-dimensional scanning can only be realized by swinging the pan/tilt. At this time, the lidar coordinate system is not fixedly connected with the lidar, but is considered together with the gimbal as a whole, that is, the origin O _l of the coordinate system is defined to be located in the center of the lidar light source, the X _l axis is vertically upward, and the Y _l axis is horizontal To the right, the Z _l- axis points toward the front of the car body.

建立和摄像机固连在一起的摄像机坐标系。坐标系原点O_c位于摄像机镜头的焦点，X_c轴和图像平面水平方向平行，方向向右；Y_c轴和图像平面的竖直方向平行，方向向下；Z_c轴垂直于图像平面，指向正前方。 Establish a camera coordinate system that is fixedly connected to the camera. The origin of the coordinate system O _c is located at the focal point of the camera lens, the X _c axis is parallel to the horizontal direction of the image plane, and the direction is to the right; the Y _c axis is parallel to the vertical direction of the image plane, and the direction is downward; the Z _c axis is perpendicular to the image plane, pointing to In front of.

惯导设备固定安装在车体上。建立和车体坐标系方向一致的惯导坐标系。惯导设备的安装误差可以在安装时进行跑车实验解算出来，对安装误差校正后，惯导设备输出的姿态角就是车体的姿态角。 The inertial navigation equipment is fixedly installed on the vehicle body. Establish an inertial navigation coordinate system that is in the same direction as the vehicle body coordinate system. The installation error of the inertial navigation equipment can be solved by performing sports car experiments during installation. After the installation error is corrected, the attitude angle output by the inertial navigation equipment is the attitude angle of the car body. the

采用激光雷达数据进行障碍识别的时候，需要使用惯导敏感到的车体的俯仰角和滚转角；通过构造旋转矩阵，将距离数据校正到坐标原点在水平面上的坐标系中。 When using lidar data for obstacle recognition, it is necessary to use the pitch angle and roll angle of the car body sensitive to the inertial navigation; by constructing the rotation matrix, the distance data is corrected to the coordinate system whose coordinate origin is on the horizontal plane. the

车体坐标系、激光雷达坐标系和摄像机坐标系的相互位置关系如图2所示。 The mutual positional relationship of the car body coordinate system, the lidar coordinate system and the camera coordinate system is shown in Figure 2. the

先绕Y_l轴旋转90°，再绕Z_l轴旋转90°，然后沿Z_l轴方向向下平移h₀的距离。 First rotate 90° around the Y _l axis, then rotate 90° around the Z _l axis, and then translate down the distance _h0 along the Z _l axis.

可得： Available:

根据现有的摄像机针孔模型，将图像坐标系下点的三维坐标转换成图像上的像素位置。 According to the existing camera pinhole model, the three-dimensional coordinates of points in the image coordinate system are converted into pixel positions on the image. the

其中A为摄像机内参数，

α_x、α_y分别是图像坐标系中u轴和v轴的尺度因子，u₀、v₀是光学中心，γ是u轴和v轴不垂直因子，很多情况下，令γ＝0。A可由张正友等人提出的平面靶标法获得。 where A is the internal parameter of the camera,

α _x , α _y are scale factors of u-axis and v-axis in the image coordinate system respectively, u ₀ , v ₀ are optical centers, γ is a non-perpendicular factor of u-axis and v-axis, and in many cases, γ=0. A can be obtained by the planar target method proposed by Zhang Zhengyou et al.

根据张正友等人提出的基于平面靶标的摄像机标定方法，要求摄像机在两个以上不同的方位拍摄同一个平面靶标，摄像机和平面靶标都可以自由移动，无需知道运动参数。在标定过程中，假设摄像机的内部参数不变，在不同角度拍摄靶标时，只有外部参数改变，通过摄像机拍摄不同位置棋盘格靶标的图像，提取棋盘格靶标上方格的角点作为特征点，建立靶标点和对应图像点之间的关系，从而计算出摄像机的内参数和靶标相对于摄像机坐标系的外部参数。 According to the camera calibration method based on the planar target proposed by Zhang Zhengyou et al., the camera is required to shoot the same planar target in two or more different orientations, and both the camera and the planar target can move freely without knowing the motion parameters. In the calibration process, assuming that the internal parameters of the camera remain unchanged, only the external parameters change when the target is photographed at different angles. The images of the checkerboard target at different positions are captured by the camera, and the corner points of the grid above the checkerboard target are extracted as feature points. The relationship between the target point and the corresponding image point is established, so as to calculate the internal parameters of the camera and the external parameters of the target relative to the camera coordinate system. the

具体的，通过以平面棋盘格作为靶标，摄像机拍摄靶标在不同位置的图像，提取棋盘格靶标上方格的角点作为靶标平面的特征点，建立靶标平面的特征点和对应图像平面上的点之间的关系，计算出摄像机的内参数矩阵A和靶标相对于摄像机坐标系的外部参数R_c、t_c。标定过程如下所述： Specifically, by using a plane checkerboard as the target, the camera captures images of the target at different positions, extracts the corner points of the grid above the checkerboard target as the feature points of the target plane, and establishes the feature points of the target plane and the points on the corresponding image plane Calculate the relationship between the internal parameter matrix A of the camera and the external parameters R _c and t _c of the target relative to the camera coordinate system. The calibration process is as follows:

首先，求解单应矩阵H： First, solve the homography matrix H:

设靶标平面的特征点

相应的图像平面上的点二者的映射关系如公式(4)所示： Set the feature points of the target plane

Corresponding points on the image plane The mapping relationship between the two is shown in formula (4):

$s the s \overset{~ ~}{m m} = = A A [\begin{matrix} {r r}_{c c 11} & {r r}_{c c 22} & {t t}_{c c} \end{matrix}] \overset{~ ~}{M m} - - - - - - ((44))$

公式(4)中，s是尺度因子，r_c1、r_c2分别为靶标平面的特征点从靶标平面坐标系变换到图像坐标系的旋转矩阵r_c的前两列，t_c是相应的平移向量；A为摄像机内参数，

其中，α_x、α_y分别是图像坐标系中u轴和v轴的尺度因子，u₀、v₀是光学中心，γ是u轴和v轴不垂直因子，很多情况下，令γ＝0。 In formula (4), s is the scale factor, r _c1 and r _c2 are the first two columns of the rotation matrix r _c that transform the feature points of the target plane from the target plane coordinate system to the image coordinate system, and t _c is the corresponding translation vector ; A is the internal parameter of the camera,

Among them, α _x and α _y are the scale factors of u-axis and v-axis in the image coordinate system respectively, u ₀ and v ₀ are the optical center, γ is the non-perpendicular factor of u-axis and v-axis, in many cases, let γ=0 .

令H＝A[r_c1 r_c2 t_c]，则公式(4)可以简写成公式(5)： Let H=A[r _c1 r _c2 t _c ], then formula (4) can be abbreviated as formula (5):

$s the s \overset{~ ~}{m m} = = H h \overset{~ ~}{M m} - - - - - - ((55))$

令

其中，

表示H矩阵的三个列向量的转置，图像平面上的点和靶标平面的特征点的映射关系可以写成公式(6)的形式： make

in,

Representing the transposition of the three column vectors of the H matrix, the mapping relationship between points on the image plane and feature points on the target plane can be written in the form of formula (6):

$[\begin{matrix} {\overset{~ ~}{M m}}^{T T} & 00^{T T} & - - u u {\overset{~ ~}{M m}}^{T T} \\ 00^{T T} & {\overset{~ ~}{M m}}^{T T} & - - v v {\overset{~ ~}{M m}}^{T T} \end{matrix}] f f = = 00 - - - - - - ((66))$

根据拍摄的每幅图像上的n个点，采用最小二乘法可以求解出每幅图像所对应的矩阵H_i，i＝1，2，…。 According to the n points on each captured image, the matrix H _i corresponding to each image can be obtained by using the least square method, i=1, 2, . . . .

其次，根据得到的所有图像的H矩阵，求解中间向量b： Secondly, according to the obtained H matrix of all images, solve the intermediate vector b:

令B＝A^-TA^-1，则： Let B＝A ^-T A ^-1 , then:

$B B = = {A A}^{- - T T} {A A}^{- - 11} = = [\begin{matrix} {B B}_{1111} & {B B}_{1212} & {B B}_{1313} \\ {B B}_{21 twenty one} & {B B}_{22 twenty two} & {B B}_{23 twenty three} \\ {B B}_{3131} & {B B}_{3232} & {B B}_{3333} \end{matrix}] - - - - - - ((77))$

将上述矩阵写成向量形式，如公式(8)所示： Write the above matrix into a vector form, as shown in formula (8):

b＝[B₁₁ B₁₂ B₂₂ B₁₃ B₂₃ B₃₃]^T (8) b=[B ₁₁ B ₁₂ B ₂₂ B ₁₃ B ₂₃ B ₃₃ ] ^T (8)

那么，就有公式(9)所示关系式存在： Then, there is a relationship shown in formula (9):

${h h}_{i i}^{T T} {Bh Bh}_{j j} = = {v v}_{ij ij}^{T T} b b - - - - - - ((99))$

其中，h₁＝[h_i1 h_i2 h_i3]^T，h_j＝[h_j1 h_j2 h_j3]^T，i和j均表示第几幅图像，i＝1，2，…；j＝1，2，…； Among them, h ₁ ＝[h _i1 h _i2 h _i3 ] ^T , h _j ＝[h _j1 h _j2 h _j3 ] ^T , i and j both indicate which image, i=1, 2,...; j=1, 2,…;

v_ij＝[h_i1h_j1 h_i1h_j2+h_i2h_j1 h_i2h_j2 h_i3h_j1+h_i1h_j2 h_i3h_j2+h_i2h_j3 h_i3h_j3]^T v _ij ＝[h _i1 h _j1 h _i1 h _j2 +h _i2 h _j1 h _i2 h _j2 h _i3 h _j1 +h _i1 h _j2 h _i3 h _j2 +h _i2 h _j3 h _i3 h _j3 ] ^T

………………………………………………………(10) …………………………………………………(10)

将所有v_ij组成矩阵V，V是2n×6的矩阵，其中，n＝1，2，…；存在公式(11)所示关系式： Form matrix V of all v _ij , V is the matrix of 2n * 6, wherein, n=1, 2, ...; There is the relation shown in formula (11):

Vb＝0 (11) Vb=0 (11)

通过公式(11)可以求解出b。 b can be solved by formula (11). the

最后，根据b可以分解出摄像机的内参数矩阵A，并根据A^-1求解R_c，t_c： Finally, according to b, the internal parameter matrix A of the camera can be decomposed, and R _c , t _c can be solved according to A ^-1 :

根据b分解出摄像机的内参数具体为：将b利用Cholesky矩阵分解算法求解出A^-1，再求逆得到A； Decomposing the internal parameters of the camera according to b is specifically as follows: use the Cholesky matrix decomposition algorithm to solve b to obtain A ^-1 , and then invert to obtain A;

标定每幅图像的外部参数t_c为： The external parameter _tc for calibrating each image is:

r_c1＝λA^-1h₁ (12) r _c1 =λA ^-1 h ₁ (12)

r_c2＝λA^-1h₂ (13) r _c2 =λA ^-1 h ₂ (13)

r_c3＝r_c1×r_c2 (14) r _c3 =r _c1 ×r _c2 (14)

t_c＝λA^-1h₃ (15) t _c =λA ^-1 h ₃ (15)

其中λ＝1/‖A^-1h₁‖＝1/‖A^-1h₂‖，所以标定的外部参数中的旋转矩阵为： Where λ=1/‖A ^-1 h ₁ ‖=1/‖A ^-1 h ₂ ‖, so the rotation matrix in the calibrated external parameters is:

R_c＝[r_c1 r_c2 r_c3] (16) R _c ＝[r _c1 r _c2 r _c3 ] (16)

$[\begin{matrix} {x x}_{c c} \\ {y the y}_{c c} \\ {z z}_{c c} \end{matrix}] = = R R \cdot &Center Dot; [\begin{matrix} {x x}_{l l} \\ {y the y}_{l l} \\ {z z}_{l l} \end{matrix}] + + t t - - - - - - ((1717))$

其中R和t分别为从激光雷达坐标系转换到摄像机坐标系的旋转矩阵和平移向量，R和t计算过程如下： Among them, R and t are the rotation matrix and translation vector transformed from the lidar coordinate system to the camera coordinate system respectively, and the calculation process of R and t is as follows:

将一个黑白方块间隔的平面棋盘格靶标，将靶标放置于摄像机和激光雷达前面不同的位置，同时启动摄像机和激光雷达，采集可见光图像和距离数据并且记录。这样就会得到一系列“图像-距离”对。这种靶标既用于摄像机内参数A的标定，又可以用于摄像机和激光雷达的外部参数R和t的标定。 Place a planar checkerboard target with black and white squares, place the target at different positions in front of the camera and lidar, start the camera and lidar at the same time, collect visible light images and distance data and record them. This results in a sequence of "image-distance" pairs. This target is used not only for the calibration of the internal parameter A of the camera, but also for the calibration of the external parameters R and t of the camera and lidar. the

如图3所示，以第i个靶标平面为例，摄像机坐标系的原点O_c到第i个靶标平面的距离为λ_c，i，垂线方向为γ_c，i；三维激光雷达坐标系原点O_l到第i个靶标平面的距离为λ_l，i，垂线方向为γ_l，i，计算方法如下。 As shown in Figure 3, taking the i-th target plane as an example, the distance from the origin O _c of the camera coordinate system to the i-th target plane is λ _c,i , and the vertical direction is γ _c,i ; the three-dimensional lidar coordinate system The distance from the origin O _l to the i-th target plane is λ _l,i , and the vertical direction is γ _l,i , the calculation method is as follows.

${γ γ}_{c c,, i i} = = {R R}_{c c,, i i} [\begin{matrix} 00 \\ 00 \\ - - 11 \end{matrix}] = = - - {R R}_{33,, c c,, i i} - - - - - - ((1818))$

${λ λ}_{c c,, i i} = = {γ γ}_{c c,, i i}^{T T} \cdot &Center Dot; {t t}_{c c,, i i} - - - - - - ((1919))$

其中R_3，c，i表示R_c，i的第3列。摄像机坐标系原点到所有靶标平面的法线方向以及距离可以写成如下形式： where _R3,c,i represents column 3 of Rc _,i . The normal direction and distance from the origin of the camera coordinate system to all target planes can be written as follows:

Γ_c＝[γ_c，1 γ_c，2…γ_c，n] (20) Γ _c = [γ _{c, 1} γ _{c, 2} ... γ _{c, n} ] (20)

Λ_c＝[λ_c，1 λ_c，2…λ_c，n]^T (21) Λ _c = [λ _{c, 1} λ _{c, 2} ... λ _{c, n} ] ^T (21)

其中Γ_c为3×n矩阵，Λ_c为n×1列向量，n为“距离-图像”对数。 Where Γ _c is a 3×n matrix, Λ _c is an n×1 column vector, and n is the logarithm of "distance-image".

使用最小二乘法从激光雷达扫描的靶标点数据中拟合出靶标平面，可以得到激光雷达坐标系原点到第i个靶标平面的法线方向为γ_l，i和距离λ_l，i。设x_l，i为第i个靶标平面m_i×3的点集合，m_i是靶标平面上点的数量，

为点阵集合的均值，则 Using the least squares method to fit the target plane from the target point data scanned by the laser radar, the normal direction from the origin of the laser radar coordinate system to the i-th target plane can be obtained as γ _{l, i} and the distance λ _{l, i} . Let x _{l, i} be the set of points on the i-th target plane m _i ×3, m _i is the number of points on the target plane,

is the mean value of the lattice set, then

${x x}_{l l,, i i}^{' '} = = {x x}_{l l,, i i} - - {\overset{&OverBar; &OverBar;}{x x}}_{l l,, i i} - - - - - - ((22 twenty two))$

将

进行奇异值分解，可得 Will

Carrying out singular value decomposition, we can get

${U u}_{l l,, i i} {S S}_{l l,, i i} {V V}_{l l,, i i}^{T T} = = {x x}_{l l,, i i}^{' ' T T} {x x}_{l l,, i i}^{' '} - - - - - - ((23 twenty three))$

因此γ_l，i和λ_l，i为 Therefore γ _l,i and λ _l,i are

γ_l，i＝U_3，l，i (24) γ _{l, i} = U _{3, l, i} (24)

${λ λ}_{l l,, i i} = = {γ γ}_{l l,, i i}^{T T} {\overset{&OverBar; &OverBar;}{x x}}_{l l,, i i}^{T T} - - - - - - ((2525))$

其中，U_3，l，i表示U_l，i的第3列。激光雷达坐标系原点到所有靶标平面的法线方向和距离可以写成如下形式： Among them, U _3,l,i represents the 3rd column of U _l,i . The normal direction and distance from the origin of the lidar coordinate system to all target planes can be written as follows:

Γ_l＝[γ_l，1 γ_l，2…γ_l，n] (26) Γ _l = [γ _{l, 1} γ _{l, 2} ... γ _{l, n} ] (26)

Λ_l＝[λ_l，1 λ_l，2…λ_l，n]^T (27) Λ _l = [λ _{l, 1} λ _{l, 2} ...λ _{l, n} ] ^T (27)

其中Γ_l为3×n矩阵，Λ_l为n×1列向量，n为“距离-图像”对数。 Among them, Γ _l is a 3×n matrix, Λ _l is an n×1 column vector, and n is the logarithm of "distance-image".

R＝VU^T (28) R = V U ^T (28)

其中V和U是

进行奇异值分解的结果，即 where V and U are

The result of singular value decomposition, namely

$t t = = {(({Γ Γ}_{c c} {Γ Γ}_{c c}^{T T}))}^{- - 11} {Γ Γ}_{c c} (({Λ Λ}_{c c} - - {Λ Λ}_{l l})) - - - - - - ((2929))$

(d)距离数据校正； (d) distance data correction;

假设惯导设备输出的车体俯仰角为α、滚转角为γ，那么校正后的距离数据可以通过下式获得： Assuming that the pitch angle of the vehicle body output by the inertial navigation equipment is α, and the roll angle is γ, then the corrected distance data can be obtained by the following formula:

第二步：激光雷达距离数据障碍检测，包括如下步骤： The second step: lidar distance data obstacle detection, including the following steps:

(1)距离数据预处理； (1) distance data preprocessing;

由于激光雷达受漂移、物体表面特性以及车辆运动等因素的影响，测距数据中存在一定噪声，因此首先应对原始距离数据进行预处理。测量数据中包括由扰动带来的噪声点和误测点，对这些点进行滤波，可以提高运算速度和特征检测的精确度。另外，由于激光的混合像素现象，原始数据中会有一些数据孤点与前后数据点间距很大，这些点对于环境理解没有意义。因此，对于这些散乱数据点和孤立点，可以采用3×3的模板进行中值滤波加以处理。 Because the lidar is affected by factors such as drift, object surface characteristics, and vehicle movement, there is a certain amount of noise in the ranging data, so the original distance data should be preprocessed first. The measurement data includes noise points and mismeasurement points caused by disturbances. Filtering these points can improve the calculation speed and the accuracy of feature detection. In addition, due to the mixed pixel phenomenon of the laser, there will be some data solitary points in the original data with a large distance from the front and rear data points, and these points are meaningless for the understanding of the environment. Therefore, for these scattered data points and isolated points, a 3×3 template can be used for median filtering. the

(2)区域分割； (2) Region segmentation;

将激光雷达扫描环境得到的三维点云进行分割，得到多个区域，对这些区域分别进行识别。 Segment the 3D point cloud obtained by the lidar scanning environment to obtain multiple regions, and identify these regions respectively. the

对于处理点云数据，现有技术中有多种分割方法。一种是将点的三维坐标和颜色统一放在一个六维向量中，定义融合三维坐标和颜色距离的计算准则，采用区域增长方法，将场景中颜色相似的平面分割出来。另一种是采用平面对物体表面进行拟合，并且对平面种子不断进行区域增长，实现了对点云的分割。第三种是径向边界最近邻(A Radially Bounded Nearest Neighbor，RBNN)聚类方法，可以较好的将点云分割，步骤如下： For processing point cloud data, there are many segmentation methods in the prior art. One is to unify the three-dimensional coordinates and colors of points in a six-dimensional vector, define the calculation criteria for combining the three-dimensional coordinates and color distance, and use the region growing method to segment the planes with similar colors in the scene. The other is to use a plane to fit the surface of the object, and to continuously grow the area of the plane seed to realize the segmentation of the point cloud. The third is the Radially Bounded Nearest Neighbor (RBNN) clustering method, which can better segment the point cloud. The steps are as follows:

(a)检索某三维空间点p_i周围领域中最近的k个点，计算由这k+1个点组成的点集Q_i的法线，作为该扫描点的法向量。在本实施例中，k的取值范围是10～30，如10、20或30。 (a) Retrieve the nearest k points in the area around a point p _i in a certain three-dimensional space, and calculate the normal of the point set Q _i composed of these k+1 points as the normal vector of the scanning point. In this embodiment, the value range of k is 10-30, such as 10, 20 or 30.

具体计算方法如下：将Q_i中的点进行奇异值分解U∑V^T，将V^T中对应∑中最小值的单位向量，作为p_i的法线

上述奇异值分解为现有技术。 The specific calculation method is as follows: Singular value decomposition U∑V ^T is performed on the points in Q _i , and the unit vector corresponding to the minimum value in ∑ in V ^T is taken as the normal of p _i

The singular value decomposition described above is a prior art.

(b)将三维空间点p_i的坐标

和法线

组成特征向量

(b) The coordinates of point p _i in three-dimensional space

and normal

Composing eigenvectors

定义两个三维点的笛卡尔空间距离为ρ_e(p_i，p_j)＝‖p_i-p_j‖，其中，p_j的坐标

p_j和其法线

组成特征向量

角度距离为ρ_a(n_i，n_j)＝0.5-0.5×<n_i，n_j>/‖n_i‖‖n_j‖，其中，

为空间距离和角度距离设置不同的阈值ρ_e，max、ρ_a，max，当两个点之间的空间距离或者角度距离大于相应阈值时，即认为这两个点不在同一区域。 Define the Cartesian space distance of two three-dimensional points as ρ _e (p _i , p _j )=‖p _i -p _j ‖, where the coordinates of p _j

p _j and its normal

Composing eigenvectors

The angular distance is ρ _a (n _i , n _j )=0.5-0.5×<n _i , n _j >/‖n _i ‖‖n _j ‖, where,

Set different thresholds ρ _e,max and ρ _a,max for the spatial distance and angular distance. When the spatial distance or angular distance between two points is greater than the corresponding threshold, the two points are considered to be not in the same area.

(c)对所有相邻点计算相互之间的空间距离ρ_e和角度ρ_a距离。如果ρ_e≤ρ_e，max且ρ_a≤ρ_a，max，那么可以将这两个点聚类合并。将所有相邻点尽可能合并到一个区域。 (c) Calculate the mutual spatial distance ρ _e and angle ρ _a distance for all adjacent points. If ρ _e ≤ ρ _e,max and ρ _a ≤ ρ _a,max , then these two points can be clustered and merged. Merge all adjacent points into one region as much as possible.

(d)如果某一点无法和其它点聚类，那么就新建一个区域。 (d) If a point cannot be clustered with other points, create a new region. the

(e)将所有点聚类后，检验每类所拥有点的数量，如果数量少于某一阈值n_cmin，而且类内点的平均高度远远大于车辆跨越障碍的高度h_max，认为该类为噪声，可以删除。阈值n_cmin和车辆跨越障碍的高度h_max根据实际情况选取。 (e) After clustering all the points, check the number of points owned by each class. If the number is less than a certain threshold n _cmin and the average height of the points in the class is far greater than the height h _max of the vehicle crossing the obstacle, the class is considered For noise, it can be removed. The threshold n _cmin and the height h _max of the vehicle crossing the obstacle are selected according to the actual situation.

(3)区域识别； (3) Area identification;

无人驾驶车辆行驶在野外环境中，会有草地、道路、树木、灌木丛等各种路况，需要对这些地形进行分析识别，具体包括如下步骤： When unmanned vehicles drive in the wild environment, there will be various road conditions such as grasslands, roads, trees, bushes, etc. These terrains need to be analyzed and identified, including the following steps:

(a)计算区域特征； (a) Computing regional features;

识别之前，需要计算出区域几个特征，包括平均高度、奇异值、区域曲面法向量等。计算方法如下： Before recognition, several features of the region need to be calculated, including average height, singular values, and normal vectors of the surface of the region. The calculation method is as follows:

平均高度：

n为该区域内点的数量，i为点的序号。 average height:

n is the number of points in the area, and i is the serial number of the points.

奇异值：[σ₁ σ₂ σ₃]。将该区域内所有点的坐标组成矩阵，进行奇异值分解U∑V^T，∑中的元素即为奇异值，并且将σ₁、σ₂、σ₃由大到小排序。 Singular values: [σ ₁ σ ₂ σ ₃ ]. The coordinates of all points in the area are formed into a matrix, and the singular value decomposition U∑V ^T is performed. The elements in ∑ are the singular values, and the σ ₁ , σ ₂ , and σ ₃ are sorted from large to small.

区域曲面法向量：γ。最小的奇异值σ₃对应的V^T中的单位向量。 Area surface normal vector: γ. The smallest singular value σ ₃ corresponds to the unit vector in V ^T .

①草地识别； ①Grass recognition;

激光打到草地上的时候，由于草叶散乱，接收到的数据点非常杂乱，一般边缘检测方法无法准确检测出来。草地和普通道路的最大区别是扫描点的密度不同，也就是穿透率不同。枝叶稀疏的草丛，激光扫描的表面孔隙较多，穿透率较大；而浓密的灌木丛枝叶生长繁茂，则穿透率较小；如果激光扫描表面为坚硬固体平面，各个扫描点的距离值分布均匀，其穿透率就近似等于零。当无人驾驶车辆行驶在野外环境的时候，覆盖在地表的草丛是可以行驶通过的。因此当检测到穿透率较大的草丛时，应标识为可行驶区域。 When the laser hits the grass, the received data points are very messy due to the scattered grass blades, which cannot be accurately detected by general edge detection methods. The biggest difference between grassland and ordinary road is the density of scanning points, that is, the penetration rate is different. The grass with sparse branches and leaves has more pores on the surface of the laser scanning, and the penetration rate is relatively high; while the bushes with dense branches and leaves grow luxuriantly, the penetration rate is small; if the laser scanning surface is a hard solid plane, the distance value of each scanning point If the distribution is uniform, the penetration rate is approximately equal to zero. When the unmanned vehicle is driving in the wild environment, the grass covered on the ground can be driven through. Therefore, when a grass with a high penetration rate is detected, it should be marked as a drivable area. the

当满足以下几个条件时，识别为草地。 When the following conditions are met, it is recognized as grassland. the

1)区域内点平均高度低于车辆越障高度h_max。 1) The average height of points in the area is lower than the vehicle obstacle clearance height h _max .

2)区域内点的穿透率高，奇异值分布为σ₁≈σ₂≈σ₃。 2) The penetration rate of points in the region is high, and the singular value distribution is σ ₁ ≈σ ₂ ≈σ ₃ .

3)区域法向量γ向上。 3) The area normal vector γ is upward. the

由于在区域分割的时候，可能会把草地上的点划分到不同的区域，因此在识别出草地之后，需要将这些点合并到同一区域，并且标注该区域为可行驶区域。此处的合并是指把几个区域内的点放置在一个区域内。 Since the points on the grass may be divided into different areas during area segmentation, after the grass is identified, these points need to be merged into the same area and marked as a drivable area. Merging here refers to placing points in several areas in one area. the

②道路识别； ②Road recognition;

道路区域和草地区域的最大区别，在于激光束穿透能力不同。道路的表面比较平坦，因此光束照射在路面上，扫描点分布比较均匀。道路为可行驶区域。 The biggest difference between the road area and the grass area lies in the different penetration capabilities of the laser beam. The surface of the road is relatively flat, so the beam is irradiated on the road, and the distribution of scanning points is relatively uniform. Roads are drivable areas. the

当满足以下几个条件时，识别为道路。 When the following conditions are met, it is recognized as a road. the

1)区域内点高度低于车辆越障高度

2)区域内点的穿透率低，σ₁＞σ₂＞＞σ₃。 2) The penetration rate of points in the region is low, σ ₁ >σ ₂ ＞＞σ ₃ .

3)区域法向量γ向上。 3) The area normal vector γ is upward. the

③树干的识别； ③Recognition of trunk;

树干上的扫描点分布一般呈圆柱面的一部分。当奇异值的分布规律为σ₁＞＞σ₂≈σ₃，区域法向量γ位于水平方向时，识别为树干。 The distribution of scanning points on the tree trunk is generally a part of a cylindrical surface. When the distribution of singular values is σ ₁ ＞＞σ ₂ ≈σ ₃ , and the area normal vector γ is in the horizontal direction, it is recognized as a tree trunk.

④灌木丛的识别； ④Recognition of bushes;

灌木丛上扫描点都分布在一个平面上，当奇异值为σ₁＞σ₂＞σ₃，区域法向量γ也位于水平方向时，识别为灌木丛。 The scanning points on the bushes are all distributed on a plane. When the singular value is σ ₁ >σ ₂ >σ ₃ and the area normal vector γ is also in the horizontal direction, it is recognized as a bush.

在本发明中，当奇异值σ₁与σ₂的比值σ₁/σ₂在[14]内时，认为σ₁≈σ₂；当奇异值σ₁与σ₂的比值σ₁/σ₂大于50时，认为奇异值σ₁＞＞σ₂。 In the present invention, when the ratio σ ₁ /σ ₂ of the singular value σ ₁ to σ ₂ is within [14], it is considered that σ ₁ ≈ σ ₂ ; when the ratio σ ₁ /σ ₂ of the singular value σ ₁ to σ ₂ is greater than 50, it is considered that the singular value σ ₁ >>σ ₂ .

第三步：摄像机图像处理，具体包括如下步骤： The third step: camera image processing, specifically includes the following steps:

按照第一步中的坐标转换关系进行转换。 Transform according to the coordinate transformation relationship in the first step. the

由于在野外环境中，除了可行驶的道路、草丛之外，可能会有水滩、石块等。激光雷达扫描在水滩上，部分激光不会反射回来，造成激光雷达扫描数据具有空洞，也有可能会反射回一部分激光，这部分点的高度均在水平面上，因此只采用激光雷达数据进行障碍识别，很容易将水滩作为可行驶区域对待。而实际上，现在的技术还无法对水滩的深度、松软程度进行评估，因此无人驾驶车辆一旦驶入，就可能会造成危险。此外行驶环境中可能会有一些的石块，虽然体积不大，但是会使无人驾驶车辆颠簸，影响车辆的速度。只采用激光雷达的距离数据，由于激光雷达的分辨率相对较低，无法准确描绘出石块形状，所以此时采用摄像机的图像，可以对这些较小的障碍物进行判断。 Because in the wild environment, in addition to the drivable roads and grass, there may be water beaches, stones, etc. When the lidar is scanned on the water beach, part of the laser light will not be reflected back, resulting in holes in the lidar scan data, and a part of the laser light may also be reflected back. The height of these points is on the horizontal plane, so only the lidar data is used for obstacle identification , it is easy to treat the water beach as a drivable area. In fact, the current technology is still unable to evaluate the depth and softness of the water beach, so once an unmanned vehicle drives in, it may cause danger. In addition, there may be some stones in the driving environment. Although they are not large in size, they will make the unmanned vehicle bump and affect the speed of the vehicle. Only the distance data of the lidar is used. Since the resolution of the lidar is relatively low, the shape of the stone cannot be accurately depicted. Therefore, the image of the camera can be used at this time to judge these small obstacles. the

为了将可行驶区域和不可行驶区域分开，使用高斯混合模型(GaussianMixture Models，GMM)对颜色进行分类。可行驶区域的颜色服从一定的统计规律，每个像素的颜色值可以用几个高斯分布的混合模型来表示。当获得新的可行驶区域的图像后，就更新混合高斯分布的模型，如果当前图像的像素点与模型相匹配，那么判断该点为可行驶区域内的点，否则就判定该点为其它性质的点。 To separate the drivable and non-drivable areas, Gaussian Mixture Models (GMM) are used to classify the colors. The color of the drivable area obeys certain statistical laws, and the color value of each pixel can be represented by a mixture model of several Gaussian distributions. When the image of the new drivable area is obtained, the model of the mixed Gaussian distribution is updated. If the pixel point of the current image matches the model, then it is judged that the point is a point in the drivable area, otherwise it is judged that the point is of other properties point. the

具体包括如下步骤： Specifically include the following steps:

(a)建立高斯混合模型； (a) Establish a Gaussian mixture model;

其中K是高斯分布的个数，一般选3～5个，ω_k,i是权重的估计值，μ_k,i是第k个高斯分布的均值，∑_k，i是第k个高斯分布的协方差矩阵，一般假设协方差矩阵符合

Figure 603039DEST_PATH_RE-GDA00001744166200022

的规律。像素点i在第k个高斯分布函数中的概率密度可由下式计算： Where K is the number of Gaussian distributions, generally 3 to 5 are selected, ω _{k, i} is the estimated value of the weight, μ _{k, i} is the mean value of the kth Gaussian distribution, ∑ _{k, i} is the value of the kth Gaussian distribution Covariance matrix, it is generally assumed that the covariance matrix conforms to

the law. The probability density of pixel i in the kth Gaussian distribution function can be calculated by the following formula:

建立可行驶区域颜色特征的高斯混合模型的过程由以下几个步骤组成： The process of building a Gaussian mixture model of the color characteristics of the drivable area consists of the following steps:

①对处理过的图像区域进行颜色空间的转化。 ① Transform the color space of the processed image area. the

②初始化各个参数，包括全局可行驶区域阈值T，学习率α，高斯模型数量K，以及初始化权重ω_k，i。 ②Initialize various parameters, including global drivable area threshold T, learning rate α, number of Gaussian models K, and initialization weight ω _{k, i} .

③读取图像初始指定的可行驶的道路和草地图像的颜色值，作为高斯混合模型的均值，方差为预定的经验值，建立高斯混合模型。 ③ Read the color value of the drivable road and grassland image initially designated by the image as the mean value of the Gaussian mixture model, and the variance is the predetermined empirical value, and establish the Gaussian mixture model. the

④读取初始指定的可行驶区域的图像数据，将每个像素的颜色值与已有的高斯模型比较，判断下式是否成立： ④ Read the image data of the initially designated drivable area, compare the color value of each pixel with the existing Gaussian model, and judge whether the following formula is true:

$| | {x x}_{i i} - - {μ μ}_{k k} | | < < 2.5 2.5 {σ σ}_{k k}^{22} - - - - - - ((3333))$

式中，x_i为像素的颜色值，

为第k个高斯分布模型的方差。 In the formula, x _i is the color value of the pixel,

is the variance of the kth Gaussian distribution model.

匹配：更新第k个高斯混合模型的参数和权重，参数包括期望、方差、学习因子。 Matching: Update the parameters and weights of the kth Gaussian mixture model, the parameters include expectation, variance, and learning factors. the

μ_i＝(1-ρ)μ_i-1+ρx_i (34) μ _i =(1-ρ)μ _i-1 + _ρxi (34)

${σ σ}_{i i}^{22} = = ((11 - - ρ ρ)) {σ σ}_{i i - - 11}^{22} + + ρ ρ {(({x x}_{i i} - - {μ μ}_{i i}))}^{T T} (({x x}_{i i} - - {μ μ}_{i i})) - - - - - - ((3535))$

ρ＝αη(x_i|μ_k，σ_k) (36) ρ=αη( _xi |μ _k , σ _k ) (36)

式中，μ_k和σ_k分别是此时第k个高斯分布模型的均值矢量和方差，α为学习率，ρ是模型适应的学习因子，作用与α相似。 In the formula, μ _k and σ _k are the mean vector and variance of the kth Gaussian distribution model at this time respectively, α is the learning rate, ρ is the learning factor for model adaptation, and its function is similar to that of α.

不匹配：如果k＜K，增加一个高斯模型，新的高斯模型分布取新的像素点的颜色值为均值，方差和权重取经验值。如果k＝K，用新的高斯分布模型代替权重最低的模型，均值和方差同上。 Mismatch: If k<K, add a Gaussian model, the new Gaussian model distribution takes the color value of the new pixel as the mean value, and the variance and weight take the empirical value. If k=K, replace the model with the lowest weight with a new Gaussian distribution model with the same mean and variance as above. the

权重ω_k的更新公式为 The update formula of the weight ω _k is

ω_k，i＝(1-α)ω_k，i-1+αM_k，i (37) ω _k,i = (1-α)ω _k,i-1 +αM _k,i (37)

式中，ω_k，i为当前权重，α为学习率，ω_k，i-1为上一次对应权重，M_k，i为匹配量化值，如果匹配M_k，i＝1，不匹配则M_k，i＝0。 In the formula, ω _{k, i} is the current weight, α is the learning rate, ω _{k, i-1} is the corresponding weight of the last time, M _{k, i} is the matching quantization value, if it matches M _{k, i} = 1, and if it does not match, then M _k,i =0.

(b)区域识别； (b) area identification;

将建立的可行驶区域颜色的高斯模型与当前待判定区域的图像进行绝对值比较，如果|x_i-f|≥T，T为阈值，那么认为该点为非可行驶区域点。T的值根据实际需要选取。将该区域内的点建立起相应的高斯混合模型，建立过程和可行驶区域的模型相同，并且添加到知识库中，便于分析。 Compare the absolute value of the established Gaussian model of the color of the drivable area with the image of the current area to be determined. If | _xi -f|≥T, T is the threshold, then the point is considered as a non-drivable area point. The value of T is selected according to actual needs. The corresponding Gaussian mixture model is established for the points in the area. The establishment process is the same as the model of the drivable area, and it is added to the knowledge base for easy analysis.

第四步：结果融合，具体包括如下步骤： The fourth step: result fusion, specifically includes the following steps:

将激光雷达和摄像机的判别结果进行融合，判断可行驶区域和不可行驶区域，构建环境地图。 The identification results of the lidar and the camera are fused to determine the drivable area and non-drivable area, and construct an environmental map. the

形象、易用的栅格地图常常用来模拟地表环境。栅格地图最早由Elfes和Moravec提出，后来不断发展。现有的栅格地图一般可以分为两类，二维栅格地图和三维栅格地图。二维栅格地图中的每个栅格单元，只能表示有和无两种状态，但是应用方便；而三维栅格地图还能表示该单元的其他属性，例如高度，从而更精细的表示地形环境。构造栅格地图时，环境地形的分辨率和栅格尺寸的大小紧密相关。如果要求高分辨率，那么就要减小栅格的尺寸，栅格数量增加，造成计算的复杂性。而减少栅格数量，就必须增大栅格尺寸，分辨率降低。因此，栅格地图的建立需要对无人驾驶车辆本身的行驶能力、传感器分辨率、环境地形情况、系统的运算能力等各种因素综合考虑。 Graphical, easy-to-use grid maps are often used to simulate surface environments. The grid map was first proposed by Elfes and Moravec, and has been continuously developed since then. Existing grid maps can generally be divided into two categories, two-dimensional grid maps and three-dimensional grid maps. Each grid cell in a two-dimensional grid map can only represent two states of existence and non-existence, but it is convenient for application; while a three-dimensional grid map can also represent other attributes of the cell, such as height, so as to represent the terrain more finely environment. When constructing a raster map, the resolution of the environment terrain is closely related to the size of the raster. If high resolution is required, then the size of the grid must be reduced, and the number of grids will increase, resulting in computational complexity. To reduce the number of grids, the grid size must be increased and the resolution reduced. Therefore, the establishment of a grid map needs to comprehensively consider various factors such as the driving ability of the unmanned vehicle itself, sensor resolution, environmental terrain conditions, and system computing capabilities. the

综合激光雷达和摄像机的判别结果，将栅格地图划分为可行驶区域和不可行驶区域，更便于无人驾驶车辆对路径进行规划，因此建立了二维的栅格地图。上述划分的原则是： Combining the discrimination results of lidar and camera, the grid map is divided into drivable areas and non-drivable areas, which is more convenient for unmanned vehicles to plan the path, so a two-dimensional grid map is established. The principle of the above division is:

(b)若摄像机判别结果为不可行驶区域，则为不可行驶区域。 (b) If the camera judgment result is a non-driving area, then it is a non-driving area. the

建立了二维栅格地图的具体过程如下： The specific process of establishing a two-dimensional grid map is as follows:

(1)在车体坐标系中，划分栅格大小。 (1) In the car body coordinate system, divide the grid size. the

(2)将激光雷达数据和摄像机融合后识别的可行驶区域、不可行驶区域、未知区域进行标记。 (2) Mark the drivable area, non-drivable area, and unknown area identified by the fusion of the lidar data and the camera. the

下面结合一具体实施例对本发明的一种融合距离和图像信息的野外环境障碍检测方法进行描述： A kind of field environment obstacle detection method of fusion distance and image information of the present invention is described below in conjunction with a specific embodiment:

启动三维激光雷达，扫描周围场景，得到一组描述周围环境的距离数据，同时采集此刻惯导设备的姿态信息。 Start the 3D lidar, scan the surrounding scene, get a set of distance data describing the surrounding environment, and collect the attitude information of the inertial navigation equipment at the same time. the

按照上述步骤建立坐标系，根据惯导设备输出的俯仰角和姿态角，将该组距离数据校正到坐标原点在水平面上的坐标系中。 Establish a coordinate system according to the above steps, and correct the set of distance data to the coordinate system whose coordinate origin is on the horizontal plane according to the pitch angle and attitude angle output by the inertial navigation equipment. the

，然后再转换到车体坐标系下。 , and then transformed to the car body coordinate system. the

(1)数据预处理； (1) Data preprocessing;

依次扫描距离数据中的各个点，采用3×3的模板对数据进行中值滤波。 Each point in the distance data is scanned in turn, and a 3×3 template is used to perform median filtering on the data. the

(2)区域分割； (2) Region segmentation;

采用区域分割方法，对这些距离数据进行分割，参数设置为k＝20，n_cmin＝150，h_max＝0.1米，ρ_e，max＝0.04米，ρ_a，max＝0.011。分割之后可以得到若干区域。 These distance data were segmented using the region segmentation method, and the parameters were set as k=20, n _cmin =150, h _max =0.1 m, ρ _e,max =0.04 m, ρ _a,max =0.011. Several regions can be obtained after division.

(3)区域识别； (3) Area identification;

计算区域特征，其中包括平均高度，奇异值[σ₁ σ₂ σ₃]，区域曲面法向量γ。 Calculate area characteristics, including mean height , singular value [σ ₁ σ ₂ σ ₃ ], area surface normal vector γ.

根据区域的特征量，识别各个区域是哪种路况，如草地、树木、道路、灌木丛等。草地和道路可以认为是可行驶区域，而道路和灌木丛是障碍。参数设置为h_max＝0.1。 According to the feature quantity of the area, identify what kind of road conditions are in each area, such as grass, trees, roads, bushes, etc. Grass and roads can be considered as drivable areas, while roads and bushes are obstacles. The parameter is set to h _max =0.1.

启动摄像机拍摄图像。将可行驶区域内的距离数据，转换到摄像机坐标系下，根据摄像机图像，获得该区域相应的色彩信息。 Start the camera to capture images. The distance data in the drivable area is transformed into the camera coordinate system, and the corresponding color information of the area is obtained according to the camera image. the

第三步：摄像机图像处理； The third step: camera image processing;

分析摄像机图像，建立可行驶区域的高斯混合模型。参数设置为K＝5，初始化权重为ω＝0.05，全局可行驶区域阈值T＝0.7，学习率α＝0.2。 Analyze camera images to build a Gaussian mixture model of the drivable area. The parameter setting is K=5, the initialization weight is ω=0.05, the global drivable area threshold T=0.7, and the learning rate α=0.2. the

第四步：结果融合； The fourth step: result fusion;

对距离数据和摄像机图像的识别结果进行融合。根据高斯混合模型，进一步分析图像，识别可行驶区域内的障碍，将新的障碍添加到知识库中。 The distance data and the recognition results of the camera image are fused. According to the Gaussian mixture model, the image is further analyzed to identify obstacles in the drivable area and add new obstacles to the knowledge base. the

结合激光雷达数据和摄像机融合后识别的可行驶区域、不可行驶区域、未知区域，为相应栅格赋予相应的属性，建立栅格地图。 Combining the drivable area, non-drivable area, and unknown area identified by lidar data and camera fusion, assign corresponding attributes to the corresponding grid and establish a grid map. the

Claims

1. A field environment obstacle detection method of fusion distance and image information, comprising the steps:

The first step: establish a mathematical model;

In the described step of establishing a mathematical model, a car body, a laser radar, a video camera and an inertial navigation coordinate system are established, and the coordinate systems are interrelated, specifically including the following steps:

(1) Establish car body coordinate system O _b X _b Y _b Z _b ;

The origin O _b is located at a fixed point on the car body. The origin is the projection of the lidar light source on the plane coincident with the ground. The X _b axis points to the right of the car body, the Y _b axis points to the front of the car body, and the Z _b axis points to Above the car body; the height from the origin of the car body coordinate system to the ground is h ₀ ;

(2) Establish the laser radar coordinate system O _l X _l Y _l Z _l ;

It is defined that the origin _O1 of the coordinate system is located at the center of the laser radar light source, the _X1 axis is vertically upward, the _Y1 axis is horizontally to the right, and the _Z1 axis points to the front of the car body;

(3) Establish camera coordinate system O _c X _c Y _c Z _c ;

The origin of the coordinate system O _c is located at the focal point of the camera lens, the X _c axis is parallel to the horizontal direction of the image plane, and the direction is to the right; the Y _c axis is parallel to the vertical direction of the image plane, and the direction is downward; the Z _c axis is perpendicular to the image plane, pointing to In front of;

(4) Establish the inertial navigation coordinate system O _I X _I Y _I Z _I ;

The inertial navigation coordinate system is in the same direction as the vehicle body coordinate system;

(5) Determine the coordinate system conversion relationship, specifically including the following steps:

(a) Transform from the lidar coordinate system O _l X _l Y _l Z _l to the car body coordinate system O _b X _b Y _b Z _b ;

Rotate 90° around the Y _l- axis first, then rotate 90° around the Z _l- axis, and then translate down the distance of _h0 along the Z _l- axis;

(2)

The meaning of each parameter in the formula is the same as above;

(b) Convert the camera coordinate system to the image coordinate system;

According to the existing camera pinhole model, the three-dimensional coordinates of the points in the image coordinate system are converted into pixel positions on the image;

(3)

where A is the internal parameter of the camera,

(c) Transform from the lidar coordinate system O _l X _l Y _l Z _l to the camera coordinate system O _c X _c Y _c Z _c ;

(17)

Where R and t are the rotation matrix and translation vector converted from the lidar coordinate system to the camera coordinate system respectively; the meanings of other parameters in the formula are the same as above

(d) distance data correction;

Assuming that the pitch angle of the car body output by the inertial navigation equipment is α, and the roll angle is γ, the corrected distance data is obtained by the following formula:

(30)

The meanings of the parameters in the formula are the same as above;

The second step: lidar distance data obstacle detection;

The third step: camera image processing;

Step 4: Result fusion.

2. a kind of field environment obstacle detection method of fusion distance and image information according to claim 1, is characterized in that: described lidar distance data obstacle detection step, comprises the steps:

(1) Distance data preprocessing;

Filter the lidar range data;

(2) Region segmentation;

Segment the 3D point cloud obtained by the lidar scanning environment to obtain multiple regions;

(3) Area identification;

Analyze and identify grasslands, roads, trees, and bushes.

3. a kind of field environment obstacle detection method of fusing distance and image information according to claim 2, is characterized in that: described region segmentation step comprises the steps:

(a) Retrieve the nearest k points in the field around a certain three-dimensional space point p _i , and calculate the normal of the point set Q _i composed of these k+1 points, as the normal vector of the scanning point;

(b) The coordinates of point p _i in three-dimensional space

and normal Composing eigenvectors

The Cartesian space distance of two three-dimensional points is ρ _e (p _i , p _j )=||p _i -p _j ||, where the coordinates of p _j p _j and its normal

Composing eigenvectors

The angular distance is ρ _a (n _i , n _j )=0.5-0.5×<n _i , n _j >/||n _i |||n _j ||, where,

(c) Calculate the mutual spatial distance ρ _e and angle ρ _a distance for all adjacent points; if ρ _e ≤ ρ _{e, max} and ρ _a ≤ ρ _{a, max} , cluster and merge these two points;

(d) If a certain point cannot be clustered with other points, create a new area;

(e) After clustering all the points, check the number of points owned by each class. If the number is less than a certain threshold n _cmin and the average height of the points in the class is much greater than the height h _max of the vehicle crossing the obstacle, the class is noise, remove it.

4. A method for detecting obstacles in a field environment by fusing distance and image information according to claim 2, characterized in that: in the region segmentation step, the value of k ranges from 10 to 30.

5. a kind of field environment obstacle detection method of fusing distance and image information according to claim 2, is characterized in that: described area recognition step specifically comprises the following steps:

(a) Computing regional features;

Calculate the average height, singular value, and area surface normal vector; the calculation method is as follows:

average height:

n is the number of points in the area, i is the serial number of the point;

Singular value: [σ ₁ σ ₂ σ ₃ ]; the coordinates of all points in the area are formed into a matrix, and the singular value decomposition U∑V ^T is performed. The elements in ∑ are singular values, and σ ₁ , σ ₂ , σ ₃ Sort from big to small;

Area surface normal vector: γ; the unit vector in V ^T corresponding to the smallest singular value σ ₃ ;

(b) identification, including the steps of:

①Grass identification;

When the following conditions are met, it is recognized as grassland;

1) The average height of points in the area is lower than the vehicle obstacle clearance height h _max ;

2) The penetration rate of points in the area is high, and the singular value distribution is σ ₁ ≈ _{σ 2} ≈ _{σ 3} ;

3) The area normal vector γ is upward;

After identifying the grass, merge these points into the same area, and mark this area as a drivable area;

②Road identification;

When the following conditions are met, it is recognized as a road:

2) The penetration rate of points in the area is low, σ ₁ >σ ₂ ＞＞σ ₃ ;

3) The area normal vector γ is upward;

③ Trunk identification;

When the distribution of singular values is σ ₁ ＞＞σ ₂ ≈σ ₃ , and the area normal vector γ is in the horizontal direction, it is recognized as a tree trunk;

④Recognition of bushes;

When the singular value is σ ₁ >σ ₂ >σ ₃ and the area normal vector γ is also in the horizontal direction, it is recognized as a bush.

6. a kind of field environment obstacle detection method of fusing distance and image information according to claim 5, it is characterized in that: in described identification step, when the ratio σ ₁ /σ ₂ of singular value σ ₁ and σ ₂ is in When within [1 4], σ ₁ ≈σ ₂ ; when the ratio σ ₁ /σ ₂ of the singular value σ ₁ to σ ₂ is greater than 50, the singular value σ ₁ ＞＞σ ₂ .

7. a kind of field environment obstacle detection method of fusing distance and image information according to claim 1, is characterized in that: described camera image processing step, specifically comprises the following steps:

(1) Convert the laser radar data in the divided area to the image coordinate system of the camera;

(2) Further identification of the initially judged drivable area;

Specifically include the following steps:

(a) Establish a Gaussian mixture model;

Assuming that the sequence of pixels in the image is {x ₁ , x ₂ ... x _i }, the value of the i-th pixel is x _i =[R _i , G _i , B _i ], the probability of this pixel is:

Among them, K is the number of Gaussian distributions, choose 3 to 5, ω _{k, i} is the estimated value of the weight, μ _{k, i} is the mean value of the kth Gaussian distribution, ∑ _{k, i} is the coordinating value of the kth Gaussian distribution variance matrix, assuming that the covariance matrix conforms to

The law; the probability density of pixel i in the kth Gaussian distribution function is calculated by the following formula:

(32)

μ _i is the mean value of the i-th Gaussian distribution, ∑ is the covariance matrix of the Gaussian distribution;

(b) area identification;

Compare the absolute value of the established Gaussian model of the color of the drivable area with the image of the current area to be determined. If | _xi -f|≥T, T is the threshold, then the point is a non-drivable area point.

8. a kind of field environment obstacle detection method of fusion distance and image information according to claim 1, is characterized in that: described result fusion step, specifically comprises the following steps:

Integrate the discrimination results of lidar and camera to judge the drivable area and non-drivable area, and construct an environmental map; integrate the discrimination results of lidar and camera, divide the grid map into drivable area and non-drivable area, the principle of division yes:

(1) If the lidar judgment result is a non-driving area, divide the grid map into a non-driving area;

(2) If the lidar discrimination result is a drivable area, it needs to be analyzed according to the camera discrimination result;

(a) If the camera discrimination result is a drivable area, then it is a drivable area;

(b) If the camera discrimination result is a non-driving area, it is a non-driving area;

(3) If there is an area that neither the lidar nor the camera detects, then the area is an unknown area. the