CN104914863A - 一种一体式无人运动平台环境理解系统及其工作方法 - Google Patents

Abstract

本发明涉及一种一体式无人运动平台环境理解系统，属于无人运动平台自主导航领域。所述系统包括嵌入式控制系统，传感器扫描驱动装置和环境感知传感器，其中环境感知传感器固定于传感器扫描驱动装置，嵌入式控制系统通过控制线与传感器扫描驱动装置相连，通过数据线与环境感知传感器相连；传感器扫描驱动装置包括底座、支架a、支架b、支架c、电机a、电机b、电机c、电机d和电机e，环境感知传感器包括二维激光雷达、两个摄像机和GPS接收机。对比现有技术，本发明能够在无人运动平台行驶过程中自动采集、处理环境信息，具结构简单、可靠性高、实时性好、安装方便的优点。

Description

一种一体式无人运动平台环境理解系统及其工作方法

技术领域

本发明涉及一种环境理解系统，特别涉及一种一体式无人运动平台环境理解系统及其工作方法，属于无人运动平台自主导航领域。

背景技术

无人运动平台在民用、军事、科研等各领域的应用前景日益广阔，环境理解技术是地面无人运动平台实现环境感知、平台定位、路径规划等平台自主导航控制最基本的前提。目前，无人运动平台的研究通常采用多种传感器进行环境感知，以克服传感器的数据可靠性低、有效探测范围小等局限性，根据各个传感器信息在时间或空间的冗余或互补特性进行容错处理，实时正确获取行驶区域目标的形状位置等环境信息。为了获得不同方向的环境信息，需安装多个摄像机检测周围障碍物；同时安装用于水平扫描和垂直扫描两个激光雷达，其中水平扫描激光雷达用于确定凸型障碍物，垂直扫描激光雷达用于估计前方地形高度、凹坑和陡坡等危险地形。由于无人运动平台安装的传感器数量多，环境感知成本高昂，而且分散式的安装使用不方便，布线连接复杂,降低了系统的可靠性。

发明内容

本发明的目的是为了解决现有问题中的上述问题，提出一种集成二维激光雷达、双目视觉、全球定位系统(GPS)接收机的一体式无人运动平台环境理解系统及其工作方法。

本发明的目的是通过以下技术方案实现的：

一种一体式无人运动平台环境理解系统，包括嵌入式控制系统，其特征在于，还包括传感器扫描驱动装置和环境感知传感器，其中环境感知传感器固定于传感器扫描驱动装置，嵌入式控制系统通过控制线与传感器扫描驱动装置相连，通过数据线与环境感知传感器相连；所述环境感知传感器用于获取一体式无人运动平台周围的环境信息，所述传感器扫描驱动装置用于调整环境感知传感器在水平和俯仰方向的角度，所述嵌入式控制系统用于接收所述环境感知传感器通过数据线传送的环境信息，并对环境信息进行处理后结合路径规划向所述传感器扫描驱动装置发出进一步的运动指令，控制所述传感器扫描驱动装置动作。

作为优选，所述传感器扫描驱动装置包括底座、支架a、支架b、支架c、电机a、电机b、电机c、电机d和电机e，所述环境感知传感器包括二维激光雷达、两个摄像机和GPS接收机；

所述电机a、电机b、电机c、电机d和电机e分别与所述嵌入式控制系统通过控制线相连，在所述嵌入式控制系统的控制下运动；

所述二维激光雷达、两个摄像机和GPS接收机分别与所述嵌入式控制系统通过数据线相连，将获取的环境与位置信息经由数据线传送给所述嵌入式控制系统；

所述二维激光雷达通过所述支架a和所述支架b与所述底座相连，所述电机a安装在所述底座上并与所述支架a相连，所述电机b安装在支架a上并与支架b相连，所述二维激光雷达随同所述电机a和所述电机b的运动相对于所述底座具有水平方向和俯仰方向的自由度；

所述两个摄像机通过所述支架c与所述支架b相连，构成双目视觉，所述支架c与所述电机c相连；所述两个摄像机分别与所述电机d和所述电机e相连，随同所述电机c、所述电机d和所述电机e的运动相对于激光雷达具有水平方向和俯仰方向的自由度；

所述GPS接收机固定于底座上，能够为路径规划提供当前位置信息和目的位置信息。

作为优选，所述底座为封闭壳体结构，顶部加工有连接轴孔，用来与所述支架a通过旋转轴连接；所述底座内腔固定安装GPS和电机a，所述电机a与所述旋转轴相连；

所述支架a为开口向上的U型结构，底部及两侧加工有连接轴孔，底部轴孔用来与所述底座连接、两侧轴孔用来与所述支架b连接，所述支架a通过与所述底座连接的所述旋转轴由所述电机a驱动以所述旋转轴为轴沿水平向旋转；

所述支架b为C型结构，背侧固定有旋转轴、顶部两侧加工有耳轴支座，背侧的旋转轴与所述电机b相连，并通过所述支架a两侧的轴孔与所述支架a连接、顶部的耳轴支座用于与所述支架c连接，所述支架b内截参数与所述二维激光雷达参数相对应，用于将所述二维激光雷达固定安装于所述支架b内，并由所述电机b驱动以旋转轴为轴在支架a中沿俯仰向旋转；

所述支架c为平板结构，在平板上加工有一对对称的连接轴孔，用来与所述两个摄像机通过旋转轴连接，平板底面两侧各固定有一个L形连杆，用于通过所述支架b的耳轴支座与支架b相连，并将两个L形连杆的外端部与所述电机c相连，由所述电机c驱动沿俯仰向旋转；

所述两个摄像机分别通过支架c上的2个连接轴孔与旋转轴的一端相连，旋转轴的另一端分别连接所述电机d和所述电机e，由所述电机d和电机e驱动以各自的旋转轴为轴沿水平向旋转。

作为优选，所述底座为圆台状封闭壳体结构。

一种一体式无人运动平台环境理解系统的工作方法，包括以下步骤：

步骤一、环境理解系统标定；

所述一体式无人运动平台环境理解系统安装于无人运动平台后对所述双目视觉和所述二维激光雷达分别进行如下所述单独标定和联合标定，分别获得所述双目视觉的内外参数、所述二维激光雷达的内部参数以及所述双目视觉和所述二维激光雷达的对应关系：

对双目视觉单独标定，分别得到摄像机A和B的有效焦距f、图像主点坐标(u0,v0)、尺度因子fx和fy等内参数，以及其各自正交旋转矩阵R和平移向量t等外参数，并通过外参数计算得到摄像机A13和B14之间的相对位置关系；

对激光雷达单独标定，获得点O₀在激光雷达坐标系{O_ldr}中的偏移坐标(x₀,y₀,z₀)，将坐标系{O_ldr}沿向量(x₀,y₀,z₀)平移得到坐标系{O₀}，其中O₀为实际的激光雷达原点，O_ldr为实际的激光雷达俯仰扫描旋转中心轴原点，坐标系{O_ldr}的Z轴垂直于激光雷达扫描平面，Y轴沿激光雷达正前方,X、Y、Z轴构成右手系；

对双目视觉和激光雷达进行联合标定，根据摄像机成像的线性理论,通过测量三维标定板上参考点在激光雷达坐标系{O_ldr}中的坐标,计算出坐标系{O_ldr}到摄像机A和摄像机B坐标系的投影矩阵P_A＝[p^A _ij]_4×4和P_B＝[p^B _ij]_4×4,(i,j＝1,2,3,4)。

步骤二、激光雷达信息采集与处理；

采集：在无人运动平台运动过程中，嵌入式控制系统根据获取的所述环境感知传感器信息选择合适的控制策略，控制电机a旋转以改变扫描方位角，控制电机b实时旋转使二维激光雷达在该方位角沿俯仰向匀速扫描，获取无人运动平台周围环境激光点云数据，即激光雷达测距数据P(ρ_i,j,θ_i,j)，其中ρ_i,j表示到激光雷达原点O₀的距离，θ_i,j表示扫描方向到主扫描方向角度，i表示数据帧的编号，j是测距数据在同一帧数据包中的排列序号；

处理：将激光雷达极坐标数据P(ρ_i,j,θ_i,j)转化为齐次坐标Ω₀＝(ρ_i，j cosθ_i，j ρ_i，jcosθ_i，j 0 1)^T，并通过如下空间坐标转换公式将齐次坐标Ω₀转化为其在激光雷达坐标系{O_ldr}对应的三维齐次坐标Ω₁，从而得到数据点P的三维空间坐标(X,Y,Z)|_ldr：

${\mathrm{\Ω}}_1={T|}_{O_0}^{O_{\mathrm{ldr}}}\·{\mathrm{\Ω}}_0;$

${T|}_{O_0}^{O_{\mathrm{ldr}}}=\begin{array}{}\end{array}\begin{array}{}\end{array}\left(\begin{array}{cccc}\mathrm{C\γ}& -\mathrm{C\αS\γ}& \mathrm{S\αS\γ}& x_0\mathrm{C\γ}-y_0\mathrm{C\αS\γ}+z_0\mathrm{S\αS\γ}\\ \mathrm{S\γ}& \mathrm{C\αC\γ}& -\mathrm{S\αC\γ}& x_0\mathrm{S\γ}+y_0\mathrm{C\αC\γ}-z_0\mathrm{S\αC\γ}\\ 0& \mathrm{S\α}& \mathrm{C\α}& Z_0\mathrm{C\α}+y_0\mathrm{S\α}\\ 0& 0& 0& 1\end{array}\right);$

其中(α,γ)为激光雷达7的俯仰和水平旋转角度，S表示正弦函数sin(),C表示余弦函数cos()；

步骤三、双目摄像机信息采集与处理；

采集：无人运动平台行驶过程中，嵌入式控制系统根据获取的传感器信息选择合适的控制策略，控制电机c、电机d、电机e旋转来改变摄像机A和B的俯仰角、方位角及其光轴的夹角，采集无人运动平台周围环境双目数字图像信息；

处理：首先对双目图像信息和进行边缘分割、特征提取、立体匹配处理，得到空间点P(X,Y,Z)在两幅数字图像中对应的像素点坐标对<(x_A,y_A)(x_B,y_B)>，其中(x_A,y_A)和(x_B,y_B)分别为空间点P在摄像机A和摄像机B坐标系中的坐标，然后根据下式采用最小二乘法求解得到空间点P在坐标系{O_ldr}中的三维坐标(X,Y,Z)|_cam：

$P_{\mathrm{xy}}\&CenterDot;\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\left[\begin{array}{c}{p}_{34}^A{x}_A\\ p_{34}^A{y}_A\\ p_{34}^B{x}_B\\ p_{34}^B{y}_B\end{array}\right];$

其中，矩阵P_xy为

$\left[\begin{array}{cccc}{p}_{11}^A-p_{31}^A{x}_A& p_{12}^A-p_{32}^A{x}_A& p_{13}^A-p_{33}^A{x}_A& p_{14}^A\\ p_{21}^A-p_{31}^A{x}_A& p_{22}^A-p_{32}^A{x}_A& p_{23}^A-p_{33}^A{x}_A& p_{23}^A\\ p_{11}^B-p_{31}^B{x}_A& p_{12}^B-p_{32}^B{x}_A& p_{13}^B-p_{33}^B{x}_A& p_{14}^B\\ p_{21}^B-p_{31}^B{x}_A& p_{22}^B-p_{32}^B{x}_A& p_{23}^B-p_{33}^B{x}_A& p_{24}^B\end{array}\right];$

p^A _ij和p^B _ij(i,j＝1,2,3,4)分别为空间投影矩阵P_A和P_B第i行第j列元素；

步骤四、双目摄像机与激光雷达信息融合处理；

无人平台行驶过程中，融合步骤二和和步骤三获取的环境三维坐标数据构建行驶环境的数字高程图，区分图中的可行进区域与障碍区域确定无人运动平台的可行驶区域；然后通过GPS接收机获取的信息确定无人运动平台的位置信息，并根据目的位置结合可行驶区域进行路径规划；

所述数字高程图的构建过程如下：

首先，对步骤二和步骤三中获取的环境三维坐标数据(X,Y,Z)|_ldr和(X,Y,Z)|_cam的地面投影坐标信息(X，Y)进行排序得到其在地面上的分布范围，并根据分布范围将整个地图划分为U cm×U cm的栅格，栅格高度方向采用V cm解析度，栅格中的数值代表该处的高度；然后，根据下述过程对栅格进行标定，最终形成行驶环境的数字高程图：

首先，对步骤二和步骤三中获取的环境三维坐标数据(X,Y,Z)|_ldr和(X,Y,Z)|_cam通过线性映射Θ:(X,Y,Z)→(m,n,H_[m,n])将其映射到栅格地图中，其中，m、n表示栅格坐标，H_[m,n]表示相应栅格的高度值；

其次，对激光雷达坐标和双目视觉坐标映射到同一栅格中的高度值和分别求取平均值后根据下式进行加权处理得到最终的栅格高度值H_[m,n]：

$H_{[m,n]}=\frac{\mathrm{\&omega;}}{s}\underset{i}{\overset{s}{\mathrm{\&Sigma;}}}{H}_{[m,n],i}^1+\frac{1-\mathrm{\&omega;}}{t}\underset{j}{\overset{t}{\mathrm{\&Sigma;}}}{H}_{[m,n]}^2,j;$

其中可变权值ω由下式确定：

$\mathrm{\&omega;}=\frac{{\mathrm{\&sigma;}}_2^2\left(x\right)}{{\mathrm{\&sigma;}}_1^2\left(x\right)+{\mathrm{\&sigma;}}_2^2\left(x\right)};$

其中s、t分别为激光雷达坐标和双目视觉坐标映射到栅格(m,n)的个数，和为激光雷达和双目视觉的方差先验分布；

作为优选，由于双目摄像机和激光雷达采集的信息均存在噪声干扰，因此在对二者的信息处理前需要采用快速二维中值滤波算法对所述信息进行滤波预处理,所述滤波算法的主要特点是，每当滤波滑动窗口移动一个像素后，保持前一滑动窗口中剩余数据排序不变,并将新加入当前窗口的数据通过折半查找法插入到该序列中以求出新的中值，从而避免了传统中值滤波算法中对大量没有变动数值的比较。

作为优选，得到所述分布范围后进行栅格划分前，需要剪切端点处精度较低的数据点以确定数字高程地图的边界。

作为优选，所述栅格高度值H_[m,n]，当被障碍物遮挡时设为与其相邻栅格中的最大值。

有益效果

与现有技术相比，本发明具有以下有益效果：

1.本发明装置的设计方案集成了多种环境感知传感器，相较于分散式的安装，简化安装并降低了成本，提高了信号传输可靠性。

2.二维激光雷达具有水平向和俯仰向的自由度，可获得三维的激光扫描数据；双目摄像机具有水平向和俯仰向的自由度，扩大了视场范围和测距范围，具有更好的自适应性。

3.本发明能够在无人运动平台行驶过程中自动采集、处理环境信息，分别由激光雷达和双目视觉获取空间点三维坐标数据，通过数据融合构建行驶环境的数字高程地图，获得无人运动平台周围环境的障碍物和可行驶区域，并结合通过GPS获得的位置数据进行路径规划。

附图说明

本发明共有附图4张，其中：

图1为本发明实施例一体式无人运动平台环境理解系统结构示意图。

图2为本发明实施例底座内部结构透视示意图。

图3为本发明实施例支架b立体结构示意图。

图4为本发明实施例支架c立体结构示意图。

附图标记：

1-底座，2-GPS接收机，3-嵌入式控制系统，4-支架a，5-支架b，6-支架c，7-二维激光雷达，8-电机b，9-电机c，10-电机d，11-电机e，12-电机a，13-摄像机A，14-摄像机B。

具体实施方式

下面将结合附图和实施例对本发明加以详细说明，同时也叙述了本发明技术方案解决的技术问题及有益效果，需要指出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

如图1所示，本发明一种一体式无人运动平台环境理解装置主要由两部分构成：传感器扫描驱动装置和环境感知传感器。其中，环境感知传感器包括二维激光雷达7、摄像机A13、摄像机B14和GPS接收机2，用于获取周围环境中激光、视觉和位置信息，环境感知传感器分别与嵌入式控制系统3通过数据线相连；传感器扫描驱动装置包括底座1、支架a4、支架b5、支架c6、电机a12、电机b8、电机c9、电机d10、电机e11，其中电机a12、电机b8、电机c9、电机d10和电机e11分别与嵌入式控制系统3通过控制线相连，在嵌入式控制系统3的控制下运动，以调整激光雷达、摄像机A13、摄像机B14的水平和俯仰角度；GPS接收机2用于获取无人运动平台的起始位置和目的位置信息；嵌入式控制系统3包括控制和数据采集单元，用于电机的驱动和数据的采集、处理、存储等。

底座1为壳体结构，其顶部与支架a4连接，承载二维激光雷达7、摄像机A13和摄像机B14，嵌入式控制系统3和GPS接收机2固定安装于系统底座1内腔。不失一般性，如图中所示，底座1可以为圆台状壳体结构或其它形状，只要能够支撑固定于其上的设备以及屏蔽无需外界看到的设备即可。

所述的支架a4为U型框架结构，通过旋转轴安装在底座1上，由电机a12驱动沿水平向旋转。

所述的支架b5为C型框架结构，内截参数与所述二维激光雷达7对应参数相同，支架b5通过旋转轴与支架a4相连，由电机b8的驱动沿俯仰向在支架a4中旋转；所述的二维激光雷达7固定安装在支架b5框架内部，具有水平向和俯仰向的自由度；当二维激光雷达7在某一方位角沿俯仰向匀速转动，结合当前的俯仰角度数据，通过坐标转换后可以获得该方向所有数据点的空间坐标。

所述的支架c6为平板结构，通过旋转轴与支架b5相连，由电机c9驱动沿俯仰向旋转；所述的摄像机A13、摄像机B14分别通过旋转轴以一定间距安置在支架c9的平板上，分别由电机d10、电机e11的驱动在支架c9的平板内旋转，相对于激光雷达7具有水平方向和俯仰方向的自由度，可获得一定范围内的视觉信息，当改变两个摄像机光轴的夹角时双目视觉的测距范围也发生改变，具有更好的自适应性。

如图1所示的一种一体式无人运动平台环境理解系统的工作方法，包括以下步骤：

步骤一、环境理解系统标定；

环境理解系统安装于无人运动平台后，首先需要对摄像机A13、B14构成的双目视觉和激光雷达7进行单独标定和联合标定。

对双目视觉单独标定，分别得到摄像机A13和B14的有效焦距f、图像主点坐标(u0,v0)、尺度因子fx和fy等内参数，以及其各自正交旋转矩阵R和平移向量t等外参数，并通过外参数计算得到摄像机A13和B14之间的相对位置关系；

对激光雷达7单独标定，获得点O₀在激光雷达坐标系{O_ldr}中的偏移坐标(x₀,y₀,z₀)，将坐标系{O_ldr}沿向量(x₀,y₀,z₀)平移得到坐标系{O₀}，其中O₀实际的激光雷达原点，O_ldr为实际的激光雷达俯仰扫描旋转中心轴，坐标系{O_ldr}的Z轴垂直于激光雷达扫描平面，Y轴沿激光雷达正前方,X、Y、Z轴构成右手系；

对双目视觉和激光雷达7进行联合标定，根据摄像机成像的线性理论,通过测量三维标定板上参考点在激光雷达坐标系{O_ldr}中的坐标,计算出坐标系{O_ldr}到摄像机A13和摄像机B14坐标系的投影矩阵P_A＝[p^A _ij]_4×4和P_B＝[p^B _ij]_4×4,(i,j＝1,2,3,4)。

步骤二、激光雷达信息采集与处理；

激光雷达7设定在180°/0.5°工作模式,即在180°范围内以0.5°间隔实现快速扫描采样,水平扫描一周可以得到361个数据点，当采用RS-422通讯方式以50OKbPs的通讯速率传输时,水平扫描周期T仅为26ms；激光雷达7俯仰扫描范围为-45°到+45°，俯仰扫描间隔Δ设定为0.45°，俯仰扫描速度V＝Δ/T＝0.45°/26ms＝17.3°/s每幅距离图像大小为201×361(pixels)；

在无人运动平台运动过程中，嵌入式控制系统3根据获取的传感器信息选择合适的控制策略，控制电机a4旋转以改变扫描方位角，控制电机b5实时旋转使二维激光雷7达在该方位角沿俯仰向匀速扫描，获取无人运动平台周围环境激光点云数据；

由于激光雷达的测距数据中包含一定的混合像素干扰、相近频率近光源干扰等噪声，本实施例采用如下快速二维中值滤波算法对激光雷达的测距数据进行滤波预处理：

针对极坐标表示的激光雷达测距数据(ρ_i,j,θ_i,j)，建立以ρ_i,j为中心的滑动窗口[ρ_i,j]_3×3：

$\left(\begin{array}{ccc}{\mathrm{\&rho;}}_{i-1,j-1}& {\mathrm{\&rho;}}_{i-1,j}& {\mathrm{\&rho;}}_{i-1,j+1}\\ {\mathrm{\&rho;}}_{i,j-1}& {\mathrm{\&rho;}}_{i,j}& {\mathrm{\&rho;}}_{i,j+1}\\ {\mathrm{\&rho;}}_{i+1,j-1}& {\mathrm{\&rho;}}_{i+1,j}& {\mathrm{\&rho;}}_{i+1,j+1}\end{array}\right)$

其中ρ_i,j表示到激光雷达原点O₀的距离，θ_i,j表示扫描方向到主扫描方向角度，i表示数据帧的编号，j是测距数据在同一帧数据包中的排列序号；

对每幅距离图像进行滤波时，将滑动窗口[ρ_i,j]_3×3中的值进行排序，得到序列Q_i,j[9]和其中值M_i,j，滑动窗每移过一个像素后将窗口[ρ_i,j]_3×3最左边一列数据去掉而Q_i,j[9]中剩余数据排序不变，最右边加入一列数据形成新窗口[ρ_i,j+1]_3×3，并通过折半查找法插入将新数据插入到该序列中以求出的新的中值M_i,j+1,从而避免了传统中值滤波算法中对大量没有变动数值的比较,对距离图像进行快速滤波；

将滤波处理后的激光雷达极坐标数据P(ρ_i,j,θ_i,j)转化为齐次坐标Ω₀＝(ρ_i,jcosθ_i,j ρ_i,jcosθ_i,j 0 1)^T，设在j时刻环境感知扫描机械装置的状态S_j＝(α,γ)，再通过空间坐标转换公式

${\mathrm{\&Omega;}}_1=\mathrm{Rot}(\mathrm{\&gamma;},z)\&CenterDot;\mathrm{Rot}(\mathrm{\&alpha;},x)\&CenterDot;\mathrm{Trans}(x_0,y_0,z_0)\&CenterDot;{\mathrm{\&Omega;}}_0={T|}_{O_0}^{O_{\mathrm{ldr}}}\&CenterDot;{\mathrm{\&Omega;}}_0$

将齐次坐标Ω₀转化为其在激光雷达坐标系{O_ldr}对应的三维齐次坐标Ω₁，从而得到数据点P的三维空间坐标(X,Y,Z)|_ldr，计算得到

${T|}_{O_0}^{O_{\mathrm{ldr}}}=\begin{array}{}\end{array}\begin{array}{}\end{array}\left(\begin{array}{cccc}\mathrm{C\&gamma;}& -\mathrm{C\&alpha;S\&gamma;}& \mathrm{S\&alpha;S\&gamma;}& x_0\mathrm{C\&gamma;}-y_0\mathrm{C\&alpha;S\&gamma;}+z_0\mathrm{S\&alpha;S\&gamma;}\\ \mathrm{S\&gamma;}& \mathrm{C\&alpha;C\&gamma;}& -\mathrm{S\&alpha;C\&gamma;}& x_0\mathrm{S\&gamma;}+y_0\mathrm{C\&alpha;C\&gamma;}-z_0\mathrm{S\&alpha;C\&gamma;}\\ 0& \mathrm{S\&alpha;}& \mathrm{C\&alpha;}& Z_0\mathrm{C\&alpha;}+y_0\mathrm{S\&alpha;}\\ 0& 0& 0& 1\end{array}\right)$

其中(α,γ)为激光雷达7的俯仰和水平旋转角度，Rot(α,x)为绕X轴旋转角度α的旋转变换矩阵，Rot(γ,z)为绕Z轴旋转角度β的旋转变换矩阵，Trans(x₀,y₀,z₀)为沿向量(x₀,y₀,z₀)平移的平移变换矩阵,S表示正弦函数sin(),C表示余弦函数cos()。

步骤三、双目摄像机信息采集与处理；

摄像机A13和B14规格相同，水平对称安装在支架c6上并保持光轴在同一平面内；无人运动平台行驶过程中，嵌入式控制系统3根据获取的传感器信息选择合适的控制策略，控制电机c9、电机d10、电机e11旋转来改变摄像机A13和B14的俯仰角、方位角及其光轴的夹角，采集无人运动平台周围环境双目数字图像信息；

由于环境光及摄像机内、外部噪音的影响，每次获取的双目图像信息和在生成和传输过程中，不可避免地存在噪声干扰和失真，因而会造成图像质量的下降，采用步骤二中的快速二维中值滤波对双目图像信息进行滤波，改善图像的质量；

对经过滤波处理后的双目图像信息和进行边缘分割、特征提取、立体匹配等处理，即可得到空间点P(X,Y,Z)在两幅数字图像中对应的像素点坐标对<(x_A,y_A)(x_B,y_B)>，其中(x_A,y_A)和(x_B,y_B)分别为空间点P在摄像机A13和B14坐标系中的坐标，(X,Y,Z)为空间点P坐标系{O_ldr}中的坐标,并根据空间投影矩阵P_A和P_B推到可得坐标对<(x_A,y_A)(x_B,y_B)>和空间点P坐标(X,Y,Z)之间的关系式为：

$P_{\mathrm{xy}}\&CenterDot;\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\left[\begin{array}{c}{p}_{34}^A{x}_A\\ p_{34}^A{y}_A\\ p_{34}^B{x}_B\\ p_{34}^B{y}_B\end{array}\right]$

无论双目摄像机光轴是否平行，用最小二乘法解上述方程均可求出各组匹配点对应的空间点P的三维坐标(X,Y,Z)|_cam,其中，矩阵P_xy为

$\left[\begin{array}{cccc}{p}_{11}^A-p_{31}^A{x}_A& p_{12}^A-p_{32}^A{x}_A& p_{13}^A-p_{33}^A{x}_A& p_{14}^A\\ p_{21}^A-p_{31}^A{x}_A& p_{22}^A-p_{32}^A{x}_A& p_{23}^A-p_{33}^A{x}_A& p_{23}^A\\ p_{11}^B-p_{31}^B{x}_A& p_{12}^B-p_{32}^B{x}_A& p_{13}^B-p_{33}^B{x}_A& p_{14}^B\\ p_{21}^B-p_{31}^B{x}_A& p_{22}^B-p_{32}^B{x}_A& p_{23}^B-p_{33}^B{x}_A& p_{24}^B\end{array}\right]$

p^A _ij和p^B _ij(i,j＝1,2,3,4)分别为矩阵空间投影矩阵P_A和P_B第i行第j列元素；

步骤四、双目摄像机与激光雷达信息融合处理；

无人平台行驶过程中，融合步骤二和和步骤三获取的环境三维坐标数据构建行驶环境的数字高程图，确定无人运动平台可行驶区域；

对环境三维点云的坐标信息进行排序得到其在地面上的分布范围，并根据分布范围将整个地图划分为U cm×U cm的栅格，栅格高度方向采V cm解析度，栅格中的数值代表该处的高度，最终形成行驶环境的数字高程图；

作为优选，进行栅格划分前，需要剪切端点处精度较低的数据点以确定数字高程地图的边界。具体可以采用如下方式进行剪切：分别根据设定的激光雷达阈值(L₁，W₁)和双目视觉阈值(L₂，W₂)，将|X_ldr|>X₁或|Y_ldr|>W₁、|X_cam|>X₂或|Y_cam|>W₂处精度较低的数据点切除，以确定数字高程地图的边界，其中X_ldr、L₁、W₁表示激光雷达坐标数据的横坐标、X方向阈值、Y方向阈值，X_cam、L₂、W₂表示双目视觉坐标数据的横坐标、X方向阈值、Y方向阈值，|K|表示K的绝对值；

对步骤二和步骤三中获取的环境三维坐标数据(X,Y,Z)|_ldr和(X,Y,Z)|_cam，通过线性映射Θ:(X,Y,Z)→(m,n,H_[m,n])将其映射到栅格地图中，线性映射Θ计算公式为：

$\left\{\begin{array}{c}m=\mathrm{floor}(X/U)\\ n=\mathrm{floor}(Y/U)\\ H_{[m,n]}=\mathrm{floor}(Z/V)\end{array}\right.$

其中，floor为向下取整函数，m、n表示在基准面投影上相应的栅格坐标，H_[m,n]表示相应栅格的高度值；

作为优选，被障碍物遮挡的栅格高度值H_[m,n]设为与其相邻栅格中的最大值；

对激光雷达坐标和双目视觉坐标映射到同一栅格中的高度值和分别求取平均值后进行加权处理，可变权值由下式确定：

$\mathrm{\&omega;}=\frac{{\mathrm{\&sigma;}}_2^2\left(x\right)}{{\mathrm{\&sigma;}}_1^2\left(x\right)+{\mathrm{\&sigma;}}_2^2\left(x\right)}$

得到最终的栅格高度值H_[m,n]为

$H_{[m,n]}=\frac{\mathrm{\&omega;}}{s}\underset{i}{\overset{s}{\mathrm{\&Sigma;}}}{H}_{[m,n],i}^1+\frac{1-\mathrm{\&omega;}}{t}\underset{j}{\overset{t}{\mathrm{\&Sigma;}}}{H}_{[m,n]}^2,j$

其中s、t分别为激光雷达坐标和双目视觉坐标映射到栅格(m,n)的个数，和为激光雷达和双目视觉的方差先验分布；

无人运动平台根据构建的行驶环境的数字高程图，区分环境地图中的可行进区域与障碍区域，嵌入式控制系统3通过GPS接收机2获取的信息确定无人运动平台的位置信息，并根据目的位置进行路径规划。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种一体式无人运动平台环境理解系统，其特征在于：包括控制系统，其特征在于，还包括传感器扫描驱动装置和环境感知传感器，其中环境感知传感器固定于传感器扫描驱动装置，控制系统通过控制线与传感器扫描驱动装置相连，通过数据线与环境感知传感器相连；所述环境感知传感器用于获取一体式无人运动平台周围的环境信息，所述传感器扫描驱动装置用于调整环境感知传感器在水平和俯仰方向的角度，所述嵌入式控制系统用于接收所述环境感知传感器通过数据线传送的环境信息，并对环境信息进行处理后结合路径规划向所述传感器扫描驱动装置发出进一步的运动指令，控制所述传感器扫描驱动装置动作。

2.根据权利要求1所述的一种一体式无人运动平台环境理解系统，其特征在于，所述传感器扫描驱动装置包括底座、支架a、支架b、支架c、电机a、电机b、电机c、电机d和电机e，所述环境感知传感器包括二维激光雷达、两个摄像机和GPS接收机；

所述电机a、电机b、电机c、电机d和电机e分别与所述控制系统通过控制线相连，在所述控制系统的控制下运动；

所述二维激光雷达、两个摄像机和GPS分别与所述控制系统通过数据线相连，将获取的环境与位置信息经由数据线传送给所述控制系统；

所述二维激光雷达通过所述支架a和所述支架b与所述底座相连，所述支架a与所述电机a相连，所述支架b与所述电机b相连，所述二维激光雷达随同所述电机a和所述电机b的运动相对于所述底座具有水平方向和俯仰方向的自由度；

所述两个摄像机通过所述支架c与所述支架b相连，构成双目视觉，所述支架c与所述电机c相连；所述两个摄像机分别与所述电机d和所述电机e相连，随同所述电机c、所述电机d和所述电机e的运动相对于激光雷达具有水平方向和俯仰方向的自由度；

所述GPS接收机固定于底座上，能够为路径规划提供当前位置信息和目的位置信息。

3.根据权利要求2所述的一种一体式无人运动平台环境理解系统，其特征在于：

所述底座为封闭壳体结构，顶部加工有连接轴孔，用来与所述支架a通过旋转轴连接；所述底座内腔固定安装GPS和电机a，所述电机a与所述旋转轴相连；

所述支架a为开口向上的U型结构，底部及两侧加工有连接轴孔，底部轴孔用来与所述底座连接、两侧轴孔用来与所述支架b连接，所述支架a通过与所述底座连接的所述旋转轴由所述电机a驱动以所述旋转轴为轴沿水平向旋转；

所述支架b为C型结构，背侧固定有旋转轴、顶部两侧加工有耳轴支座，背侧的旋转轴与所述电机b相连，并通过所述支架a两侧的轴孔与所述支架a连接、顶部的耳轴支座用于与所述支架c连接，所述支架b内截参数与所述二维激光雷达参数相对应，用于将所述二维激光雷达固定安装于所述支架b内，并由所述电机b驱动以旋转轴为轴在支架a中沿俯仰向旋转；

所述支架c为平板结构，在平板上加工有一对对称的连接轴孔，用来与所述两个摄像机通过旋转轴连接，平板底面两侧各固定有一个L形连杆，用于通过所述支架b的耳轴支座与支架b相连，并将两个L形连杆的外端部与所述电机c相连，由所述电机c驱动沿俯仰向旋转；

所述两个摄像机分别通过支架c上的2个连接轴孔与旋转轴的一端相连，旋转轴的另一端分别连接所述电机d和所述电机e，由所述电机d和电机e驱动以各自的旋转轴为轴沿水平向旋转。

4.根据权利要求3所述的一种一体式无人运动平台环境理解系统，其特征在于：所述底座为圆台状封闭壳体结构。

5.一种一体式无人运动平台环境理解系统的工作方法，其特征在于,包括以下步骤：

步骤一、环境理解系统标定；

所述一体式无人运动平台环境理解系统安装于无人运动平台后对所述双目视觉和所述二维激光雷达分别进行如下所述单独标定和联合标定，分别获得所述双目视觉的内外参数、所述二维激光雷达的内部参数以及所述双目视觉和所述二维激光雷达的对应关系：

对双目视觉单独标定，分别得到摄像机A和B的有效焦距f、图像主点坐标(u0,v0)、尺度因子fx和fy等内参数，以及其各自正交旋转矩阵R和平移向量t等外参数，并通过外参数计算得到摄像机A13和B14之间的相对位置关系；

对激光雷达单独标定，获得点O₀在激光雷达坐标系{O_ldr}中的偏移坐标（x₀,y₀,z₀），将坐标系{O_ldr}沿向量（x₀,y₀,z₀）平移得到坐标系{O₀}，其中O₀为实际的激光雷达原点，O_ldr为实际的激光雷达俯仰扫描旋转中心轴原点，坐标系{O_ldr}的Z轴垂直于激光雷达扫描平面，Y轴沿激光雷达正前方,X、Y、Z轴构成右手系；

对双目视觉和激光雷达进行联合标定，根据摄像机成像的线性理论,通过测量三维标定板上参考点在激光雷达坐标系{O_ldr}中的坐标,计算出坐标系{O_ldr}到摄像机A和摄像机B坐标系的投影矩阵P_A=[p^A _ij]_4×4和P_B＝[p^B _ij]_4×4,(i,j＝1,2,3,4)；

步骤二、二维激光雷达信息采集与处理；

采集：在无人运动平台运动过程中，嵌入式控制系统根据获取的所述环境感知传感器信息选择合适的控制策略，控制电机a旋转以改变扫描方位角，控制电机b实时旋转使二维激光雷达在该方位角沿俯仰向匀速扫描，获取无人运动平台周围环境激光点云数据，即激光雷达测距数据P(ρ_i,j,θ_i,j)，其中ρ_i，j表示到激光雷达原点O₀的距离，θ_i，j表示扫描方向到主扫描方向角度，i表示数据帧的编号，j是测距数据在同一帧数据包中的排列序号；

处理：将激光雷达极坐标数据P(ρ_i,j,θ_i,j)转化为齐次坐标Ω₀＝(ρ_i，jcosθ_i，j ρ_i，jcosθ_i，j 0 1)^T，并通过如下空间坐标转换公式将齐次坐标Ω₀转化为其在激光雷达坐标系{O_ldr}对应的三维齐次坐标Ω₁，从而得到数据点P的三维空间坐标(X,Y,Z)|_ldr：

${\mathrm{\&Omega;}}_1=T{|}_{O_0}^{O_{1\mathrm{dr}}}\&CenterDot;{\mathrm{\&Omega;}}_0;$

${T|}_{O_0}^{O_{\mathrm{ldr}}}=\left(\begin{array}{cccc}\mathrm{C\&gamma;}& -\mathrm{C\&alpha;S\&gamma;}& \mathrm{S\&alpha;S\&gamma;}& x_0\mathrm{C\&gamma;}-y_0\mathrm{C\&alpha;S\&gamma;}+z_0\mathrm{S\&alpha;S\&gamma;}\\ \mathrm{S\&gamma;}& \mathrm{C\&alpha;C\&gamma;}& -\mathrm{S\&alpha;C\&gamma;}& x_0\mathrm{S\&gamma;}+y_0\mathrm{C\&alpha;C\&gamma;}-z_0\mathrm{S\&alpha;C\&gamma;}\\ 0& \mathrm{S\&alpha;}& \mathrm{C\&alpha;}& z_0\mathrm{C\&alpha;}+y_0\mathrm{S\&alpha;}\\ 0& 0& 0& 1\end{array}\right);$

其中(α,γ)为激光雷达7的俯仰和水平旋转角度，S表示正弦函数sin(),C表示余弦函数cos()；

步骤三、双目摄像机数据采集与处理；

采集：无人运动平台行驶过程中，嵌入式控制系统根据获取的传感器信息选择合适的控制策略，控制电机c、电机d、电机e旋转来改变摄像机A和B的俯仰角、方位角及其光轴的夹角，采集无人运动平台周围环境双目数字图像信息；

处理：首先对双目图像信息和进行边缘分割、特征提取、立体匹配处理，得到空间点P(X,Y,Z)在两幅数字图像中对应的像素点坐标对＜(x_A，y_A)(x_B，y_B)＞，其中(x_A，y_A)和(x_B，y_B)分别为空间点P在摄像机A和摄像机B坐标系中的坐标，然后根据下式采用最小二乘法求解得到空间点P在坐标系{O_ldr}中的三维坐标(X,Y,Z)|_cam：

$P_{\mathrm{xy}}\&CenterDot;\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\left[\begin{array}{c}{p}_{34}^A{x}_A\\ p_{34}^A{y}_A\\ p_{34}^B{x}_B\\ p_{34}^B{y}_B\end{array}\right];$

其中，矩阵P_xy为

$\left[\begin{array}{cccc}{p}_{11}^A-p_{31}^A{x}_A& p_{12}^A-p_{32}^A{x}_A& p_{13}^A-p_{33}^A{x}_A& p_{14}^A\\ p_{21}^A-p_{31}^A{x}_A& p_{22}^A-p_{32}^A{x}_A& p_{23}^A-p_{33}^A{x}_A& p_{23}^A\\ p_{11}^B-p_{31}^B{x}_A& p_{12}^B-p_{32}^B{x}_A& p_{13}^B-p_{33}^B{x}_A& p_{14}^B\\ p_{21}^B-p_{31}^B{x}_A& p_{22}^B-p_{32}^B{x}_A& p_{23}^B-p_{33}^B{x}_A& p_{24}^B\end{array}\right];$

p^A _ij和p^B _ij(i,j＝1,2,3,4)分别为空间投影矩阵P_A和P_B第i行第j列元素；

步骤四、所述双目摄像机与所述二维激光雷达信息融合处理；

无人平台行驶过程中，融合步骤二和和步骤三获取的环境三维坐标数据构建行驶环境的数字高程图，区分图中的可行进区域与障碍区域确定无人运动平台的可行驶区域；然后通过GPS接收机获取的信息确定无人运动平台的位置信息，并根据目的位置结合可行驶区域进行路径规划；

所述数字高程图的构建过程如下：

首先，对步骤二和步骤三中获取的环境三维坐标数据(X,Y,Z)|_ldr和(X,Y,Z)|_cam的地面投影坐标信息(X，Y)进行排序得到其在地面上的分布范围，并根据分布范围将整个地图划分为Ucm×Ucm的栅格，栅格高度方向采用V cm解析度，栅格中的数值代表该处的高度，然后，根据下述过程对栅格进行标定，最终形成行驶环境的数字高程图：

首先，对步骤二和步骤三中获取的环境三维坐标数据(X,Y,Z)|_ldr和(X,Y,Z)|_cam通过线性映射Θ:(X,Y,Z)→(m,n,H_[m,n])将其映射到栅格地图中，其中，m、n表示栅格坐标，H_[m,n]表示相应栅格的高度值；

其次，对激光雷达坐标和双目视觉坐标映射到同一栅格中的高度值和分别求取平均值后根据下式进行加权处理得到最终的栅格高度值H_[m,n]：

$H_{[m,n]}=\frac{\mathrm{\&omega;}}{s}\underset{i}{\overset{s}{\mathrm{\&Sigma;}}}{H}_{[m,n],i}^1+\frac{1-\mathrm{\&omega;}}{t}\underset{j}{\overset{t}{\mathrm{\&Sigma;}}}{H}_{[m,n],j}^2;$

其中可变权值ω由下式确定：

$\mathrm{\&omega;}=\frac{{\mathrm{\&sigma;}}_2^2\left(x\right)}{{\mathrm{\&sigma;}}_1^2\left(x\right)+{\mathrm{\&sigma;}}_2^2\left(x\right)};$

其中s、t分别为激光雷达坐标和双目视觉坐标映射到栅格(m,n)的个数，和为激光雷达和双目视觉的方差先验分布。

6.根据权利要求5所述的一种一体式无人运动平台环境理解系统的工作方法，其特征在于：所述对双目摄像机以及二维激光雷达采集的信息进行处理前需要采用快速二维中值滤波算法进行滤波预处理，所述快速二维中值滤波算法的特点是：每当滤波滑动窗口移动一个像素后，保持前一滑动窗口中剩余数据排序不变,并将新加入当前窗口的数据通过折半查找法插入到该序列中以求出新的中值，从而避免了传统中值滤波算法中对大量没有变动数值的比较。

7.根据权利要求5所述的一种一体式无人运动平台环境理解系统的工作方法，其特征在于：所述步骤4得到所述分布范围后进行栅格划分前，需要剪切端点处精度较低的数据点以确定数字高程地图的边界。

8.根据权利要求5-8任一所述的一种一体式无人运动平台环境理解系统的工作方法，其特征在于：步骤4所述栅格高度值H_[m,n]，当被障碍物遮挡时设为与其相邻栅格中的最大值。