阅读说明：本技术 基于多传感器的低速车辆的前向地形三维构建方法 (Forward terrain three-dimensional construction method of low-speed vehicle based on multiple sensors ) 是由 李忠利 杨淑君 高永升 卢耀真 韦宇豪 杨永军 于 2020-07-08 设计创作，主要内容包括：基于多传感器的低速车辆的前向地形三维构建方法,步进电机的旋转轴与雷达组合后与地面呈倾斜角度α设置；上位机的四个串口分别连接并接受单片机、雷达、GPS、陀螺仪数据,分别解析出角度、距离、车速、姿态角信息；雷达实时监测距离信息,将距离信息进行频谱分析,进行低通滤波器滤波并处理得到的距离数据；单片机控制步进电机做摆动运动并实时监测角度信息,将角度信息数据进行线性插值处理,将角度信息与去除噪点的距离信息同步；建立雷达坐标轴和车辆坐标轴,通过坐标变换将雷达坐标轴和车辆坐标轴统一；将X坐标进行修正处理,得到当前位置时物体所有点距离车辆的距离；将该三维坐标导入Matlab后,得到三维模型。(A forward terrain three-dimensional construction method of a low-speed vehicle based on multiple sensors is characterized in that a rotating shaft of a stepping motor is combined with a radar and then forms an inclination angle alpha with the ground; four serial ports of the upper computer are respectively connected with and receive data of the single chip microcomputer, the radar, the GPS and the gyroscope, and angle, distance, vehicle speed and attitude angle information are respectively analyzed; the radar monitors the distance information in real time, performs spectrum analysis on the distance information, performs low-pass filter filtering and processes the obtained distance data; the single chip microcomputer controls the stepping motor to swing and monitors angle information in real time, linear interpolation processing is carried out on angle information data, and the angle information and distance information for removing noise points are synchronized; establishing a radar coordinate axis and a vehicle coordinate axis, and unifying the radar coordinate axis and the vehicle coordinate axis through coordinate transformation; correcting the X coordinate to obtain the distances between all points of the object and the vehicle at the current position; and importing the three-dimensional coordinates into Matlab to obtain a three-dimensional model.)

基于多传感器的低速车辆的前向地形三维构建方法

技术领域

本发明属于汽车技术领域，具体涉及基于多传感器的低速车辆的前向地形三维构建方法。

背景技术

目前，车辆智能化是车辆的重要发展方向，车辆智能驾驶技术是车辆智能化的代表与核心技术之一，前向地形是车辆智能驾驶的前提，智能车主要是由环境感知、定位系统、车辆控制系统、决策规划系统等多系统共同组成的，在这众多因素中，环境感知是获得周围信息的媒介，是汽车行驶过程中的一双眼睛，实时准确地可以为智能车提供前方的地形信息，为智能车的路径规划和自主决策行为提供了可靠依据。激光雷达测量精度高、速度快并且不容易受到光照条件的影响，是无人驾驶汽车环境感知中的重要传感器之一，目前无人驾驶研究者一般采用多线激光雷达，虽然多线激光雷达的效率特别高、效果好，目前国内外研究无人驾驶的车企都是采用多线激光雷达，但是由于多线激光雷达硬件成本昂贵，数据处理难度大，对于其大范围使用以及学者研究造成了一定的限制。提出一种基于多传感的低速车辆前向地形构建系统，可以克服多线激光雷达价格高昂的缺点，之所以将其选择用于低速车辆，是由于单线激光雷达与多线激光雷达相比扫描速度慢，以及电机旋转速度慢，以至于获得的路面信息较少，低速车辆相对于乘用车来说，不需要考虑高速道路，并给与单线激光雷达更多时间采集前方的地形信息。

发明内容

有鉴于此，为解决上述现有技术的不足，本发明的目的在于提供了基于多传感器的低速车辆的前向地形三维构建方法，利用多个传感器实现对于车辆前方的物体的三维构建，能够用较少的成本去实现构建三维模型。

为实现上述目的，本发明所采用的技术方案是：

基于多传感器的低速车辆的前向地形三维构建方法，包括单片机、步进电机、雷达、GPS、陀螺仪；所述步进电机与雷达连接且带动雷达旋转，所述步进电机的旋转轴与雷达垂直设置，步进电机的旋转轴与雷达组合后与地面呈倾斜角度α设置，所述步进电机、雷达、低通滤波器、GPS、陀螺仪均与单片机相连，单片机控制步进电机做摆动运动，所述单片机与上位机相连；

该三维构建方法，具体包括以下步骤：

S1：上位机的四个串口分别连接并接受单片机、雷达、GPS、陀螺仪的数据，分别解析出角度、距离、车速、姿态角这些信息，之后进行数据处理；

S2：所述雷达实时监测距离信息，将距离信息进行频谱分析，而后进行低通滤波器滤波并处理得到的距离数据；

S3：所述单片机控制步进电机做摆动运动并实时监测角度信息，将角度信息数据进行线性插值处理，将角度信息与去除噪点的距离信息同步；

S4：建立雷达坐标轴和车辆坐标轴，通过坐标变换将雷达坐标轴和车辆坐标轴统一；

S41：建立以雷达为极点，以距离为极径，以角度为极角的极坐标系，然后将极坐标转换为以雷达为原点的二维直角坐标系；之后将其整个坐标系绕Y轴逆时针旋转可以得到三维直角坐标系；

S5：将陀螺仪得到的姿态角进行RBF神经网络得到的补偿后的姿态角，将得到修正后的空间坐标系；而后通过GPS得到的速度信息修正x信息，得到当前位置下的物体距离车辆的三维坐标；

S6：将当前位置下的物体距离车辆的三维坐标导入Matlab后，得到三维模型。

进一步的，所述雷达为单线激光雷达。

进一步的，所述低通滤波器为巴特沃斯低通滤波器。

进一步的，所述步骤S2具体包括以下步骤：

将保存的距离信息先进行快速傅里叶变换：

式中F(w)为f(t)的像函数，f(t)为F(w)的像原函数，即我们即将输入的距离信息，然后再进行频谱分析，观察在高能量位置时的频率wc，而后再进行滤波器分离出噪点值，低通滤波器可用如下振幅的平方对频率的公式表示：

其中，n是低通滤波器的阶数，wc是截止频率，wp是采样频率；经过低通滤波器的数据将去除一些噪点值，以减少数据的误差。

进一步的，所述步骤S1具体包括：上位机的四个串口分别连接并接受单片机、雷达、GPS、陀螺仪的数据，在同一定时器中分别解析单片机、雷达、GPS、陀螺仪的数据，使其在同一时间采集到的是同一位置的角度、距离、速度、姿态角的信息。

进一步的，所述步骤S3中，对角度数据进行线性插值处理具体为：

先计算角度数据中需要插值的数量：

其中，n_r是采样得到的激光雷达个数即距离数据，n_m采样得到的角度信息个数，n_u：表示的是角度个数中需要***多少个数，才能和距离信息的个数相同，同时也需要计算每次插值入的间隔值，用Δn表示：

其中，Δγ表示步进电机的步进角。

进一步的，所述步骤S51中，具体包括以下：

A1：径向基神经网络的激活函数可表示为：

式中，||Lp-ci||为欧式范数，ci为高斯函数的中心，δ为高斯函数的方差；

A2：由于我们有多个输入和多个输出，则其径向基神经网络的结构可得到网络的输出为：

式中，Lp为第p个输入样本，p＝1,2,3,...,P，P为样本总数，ci为网络隐含层节点的中心，Wij为隐含层到输出层的连接权值，i＝1,2,3,...,h为隐含层节点数，yi为与输入样本对应的网络的第j个输出节点的实际输出；

A3：将测量姿态角的陀螺仪的x轴加速度、y轴加速度、z轴加速度、z轴角速度和速度信息作为网络的输入变量，车辆的侧倾角、俯仰角、偏航角作为网络的输出层，达到补偿姿态角的目的。

本发明的有益效果是：

本发明利用多个传感器实现对于车辆前方的物体的三维构建，能够用较少的成本去实现构建三维模型。本发明中，包括单片机、步进电机、雷达、低通滤波器、GPS、陀螺仪；步进电机与雷达连接且带动雷达旋转，步进电机的旋转轴与雷达垂直设置，步进电机的旋转轴与雷达组合后与地面呈倾斜角度α设置；上位机的四个串口分别连接并接受单片机、雷达、GPS、陀螺仪的数据，分别解析出角度、距离、车速、姿态角这些信息；雷达实时监测距离信息，将距离信息进行频谱分析，而后进行低通滤波器滤波并处理得到的距离数据；单片机控制步进电机做摆动运动并实时监测角度信息，将角度信息数据进行线性插值处理，将角度信息与去除噪点的距离信息同步；建立雷达坐标轴和车辆坐标轴，通过坐标变换将雷达坐标轴和车辆坐标轴统一；将X坐标进行修正处理，得到当前位置时物体所有点距离车辆的距离；将当前位置下的物体距离车辆的三维坐标导入Matlab后，得到三维模型。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的原理框图；

图2为直角坐标系的原理示意图；

图3为本发明的流程原理图。

具体实施方式

下面给出具体实施例，对本发明的技术方案作进一步清楚、完整、详细地说明。本实施例是以本发明技术方案为前提的最佳实施例，但本发明的保护范围不限于下述的实施例。

基于多传感器的低速车辆的前向地形三维构建方法，包括单片机、步进电机、雷达、GPS、陀螺仪；所述步进电机与雷达连接且带动雷达旋转，所述步进电机的旋转轴与雷达垂直设置，步进电机的旋转轴与雷达组合后与地面呈倾斜角度α设置，这样当我们的地面是平面时，雷达的测量面将与地面相交为一条直线，将雷达为极点以其的测量面作极坐标系，测量距离为极径，步进电机旋转角度为极角，得到的极坐标系先转变为以雷达为原点的直角坐标系，并将这个直角坐标系记为Oxy；所述步进电机、雷达、GPS、陀螺仪均与单片机相连，单片机控制步进电机做摆动运动，所述单片机与上位机相连；

进一步的，本实施例中，步进电机带动雷达在一定的角度γ往复运动，将一维激光雷达变为了有角度信息的二维雷达，摆动角度γ和电机的安装位置以及安装高度有关，需要得知步进电机往复摆动的角度，当选用车辆型号一定时，即安全距离S、安装高度h就确定了，根据实际中我们需要车辆前方的视觉宽度D，可通过公式计算的出，如图1、2

需要将以雷达为极点、以距离为极径的有序数对(L，β)的极坐标，首先我们需要将极坐标转换为以电机为原点的直角坐标系，将激光雷达测量平面以激光雷达为原点建立一个Oxy坐标轴，如图2

x＝L*sin β (1)

y＝L*cos β (2)

其中，L表示单线激光雷达的测得的到物体时的距离，β表示的是极角，得到在如图2所示坐标系的直角坐标x,y。

如图3所示，该三维构建方法，具体包括以下步骤：

S1：用vs编写上位机程序，上位机的四个串口分别连接并接受单片机、雷达、GPS、陀螺仪的数据，分别解析出角度、距离、车速、姿态角这些信息，之后进行数据处理；

S2：所述雷达实时监测距离信息，将距离信息进行频谱分析，而后进行低通滤波器滤波并处理得到的距离数据；

S3：所述单片机控制步进电机做摆动运动并实时监测角度信息，将角度信息数据进行线性插值处理，将角度信息与去除噪点的距离信息同步；

S4：建立雷达坐标轴和车辆坐标轴，通过坐标变换将雷达坐标轴和车辆坐标轴统一；

S41：建立以雷达为极点，以距离为极径，以角度为极角的极坐标系，然后将极坐标转换为以雷达为原点的二维直角坐标系；之后将其整个坐标系绕Y轴逆时针旋转可以得到三维直角坐标系；

S5：将陀螺仪得到的姿态角进行RBF神经网络得到的补偿后的姿态角，将得到修正后的空间坐标系；而后通过GPS得到的速度信息修正x信息，得到当前位置下的物体距离车辆的三维坐标；

S6：将当前位置下的物体距离车辆的三维坐标导入Matlab后，得到三维模型。

进一步的，所述雷达为单线激光雷达。

进一步的，所述低通滤波器为巴特沃斯低通滤波器。

进一步的，所述步骤S2具体包括以下步骤：

将保存的距离信息先进行快速傅里叶变换：

式中F(w)为f(t)的像函数，f(t)为F(w)的像原函数，即我们即将输入的距离信息，然后再进行频谱分析，观察在高能量位置时的频率wc，而后再进行滤波器分离出噪点值，低通滤波器可用如下振幅的平方对频率的公式表示：

其中，n是低通滤波器的阶数，wc是截止频率，wp是采样频率；经过低通滤波器的数据将去除一些噪点值，以减少数据的误差。

进一步的，所述步骤S1具体包括：上位机的四个串口分别连接并接受单片机、雷达、GPS、陀螺仪的数据，在同一定时器中分别解析单片机、雷达、GPS、陀螺仪的数据，使其在同一时间采集到的是同一位置的角度、距离、速度、姿态角的信息。将采样时间也同样分别保存在角度、距离、车速、姿态角文件中，以便在进行以下数据处理后，保证各类传感器数据信息同步。

进一步的，由于单片机的发送频率远远小于单线激光雷达的发送频率，所以每秒钟接受和保存的数据数量不同，为了每个距离数值都有一个角度数值对应，需要对角度数据进行线性插值处理；所述步骤S3中，对角度数据进行线性插值处理具体为：

先计算角度数据中需要插值的数量：

其中，n_r是采样得到的激光雷达个数即距离数据，n_m采样得到的角度信息个数，n_u：表示的是角度个数中需要***多少个数，才能和距离信息的个数相同，同时也需要计算每次插值入的间隔值，用Δn表示：

其中，Δγ表示步进电机的步进角。

进一步的，所述步骤S51中，具体包括以下：使用陀螺仪测量车身姿态角，当车身发生俯仰、侧倾、偏航时车辆的直角坐标系将会发生偏移，所以通过测得的姿态角矫正坐标系，但是陀螺仪由于干扰将会产生偏离稳定输出，从而造成漂移，因此需要进行姿态角的补偿，使用RBF神经网络对非线性连续函数的一致逼近性能进行姿态角补偿，RBF神经网络结构简单，训练简洁而且学习收敛速度快，能够逼近任意非线性函数，我们选用自组织选取中心的RBF神经网络学习法；

A1：径向基神经网络的激活函数可表示为：

式中，||Lp-ci||为欧式范数，ci为高斯函数的中心，δ为高斯函数的方差；

A2：由于我们有多个输入和多个输出，则其径向基神经网络的结构可得到网络的输出为：

式中，Lp为第p个输入样本，p＝1,2,3,...,P，P为样本总数，ci为网络隐含层节点的中心，Wij为隐含层到输出层的连接权值，i＝1,2,3,...,h为隐含层节点数，yi为与输入样本对应的网络的第j个输出节点的实际输出；

A3：将测量姿态角的陀螺仪的x轴加速度、y轴加速度、z轴加速度、z轴角速度和速度信息作为网络的输入变量，车辆的侧倾角、俯仰角、偏航角作为网络的输出层，达到补偿姿态角的目的。

进一步的，本实施例中，单线激光雷达和步进电机的旋转轴是与地面倾斜一定的角度α安装的，而且雷达直角坐标轴同样与地面倾斜一定的角度。

建立一个以低速车辆以车辆前进方向为x_o轴、垂直地面为z_o轴建立一个Ox_oy_oz_o三维坐标系，将Oxy激光雷达坐标轴通过坐标变换绕y轴旋转一定的角度θ_y＝90-α可与车辆坐标轴统一，从而可以得到以车辆坐标系下r_o＝(x_o,y_o,z_o)的空间坐标值：

从而可以三维坐标r_o＝(x_o,y_o,z_o)，再将补偿后的姿态角经坐标变换的得到修正后空间位置坐标，始状态O系与W系重合，然后O系先绕Zo轴旋转一个角度Ψ，然后绕Yo轴旋转一个角度θ，然后绕Xo轴旋转一个角度Φ，得到了O系(也就是车辆的最终姿态)。这种欧拉角顺序有的书上称为“航空次序欧拉角(aerospace sequence Euler angles)”。然后经过三个欧拉角转动后，世界坐标系下的一个矢量rW＝(xW,yW,zW)与其对应的运载体坐标系下的矢量ro＝(xo,yo,zo)之间的关系可以表示为

记陀螺仪得到的俯仰角、偏航角、侧斜角分别用θ、Ψ、Φ表示，用简化的写法为

其中

称为从坐标系W到坐标系O的变换矩阵。

反过来其中又叫做欧拉角形式的方向余弦矩阵，现已知

则

进一步的，由于车辆不断的前进，那么当前车辆和物体距离不断变化即需要不断修正，需要每时刻速度信息。首先将坐标变换后第一时刻的坐标记为(x₁ ^w,y₁ ^w,z₁ ^w)，速度记为V1；第二时刻的坐标记为(x₂ ^w,y₂ ^w,z₂ ^w)，速度记为V2，第一时刻到第二时刻的时间记为Δt₂……，第n时刻的坐标记为(x_n ^w,y_n ^w,z_n ^w)，速度记为V3，第n-1时刻到第n时刻的时间记为Δt_n，然而由于车辆不断的前进，需要将n时刻之前的点的坐标进行修正到当前时刻：

之后得到修正后的坐标点，将其再重新记为r^w，将修正后的空间位置r^w，导入Matlab中，用绘图函数即可得出当下的三维模型。

进一步的，图1所示：o点放置步进电机和雷达，并与地面倾斜一定的角度α，当车辆型号选定时，其安装高度h已知，根据车辆可推算其安全距离S，即激光雷达到地面的距离为

图2所示：图1中垂直与OL测量面得到图2，o点放置步进电机和雷达，图中三角形为雷达的测量面以及步进电机的旋转面，L为图1中的数值，根据实际中车辆前方需要的视觉宽度D，可计算得出电机应该摆动的角度γ

根据步进电机的步进角Δθ，以及摆动角可以得到每转动一次的极角β：

建立如图2所示的直角坐标轴，将极坐标转变为直角坐标：

x＝L*sin β

y＝L*cos β。

综上所述，本发明中，采用一维的单线激光雷达，我们想要得到物体的距离与宽度，在这用的是步进电机带动激光雷达的旋转，可以在功能上实现二维激光雷达的作用，并能够灵活控制激光雷达的转动，需要将步进电机与激光雷达固定连接，并将步进电机的旋转轴与激光雷达垂直安装，然后将步进电机以及激光雷达安装在车辆顶部，但是与地面倾斜一定的角度α放置，当我们扫描的面是平面时，则激光雷达的扫描面将会与平面相交为一条直线，使用单片机控制步进电机每次旋转的角度、旋转方向、旋转速度，以便能准确得到物体的位置。此时我们将能得到以激光雷达为极点、以距离为极径、以步进电机旋转角度为极角的有序数对(L，θ)的极坐标，首先我们需要将极坐标转换为以激光雷达为原点的直角坐标系，并将这个直角坐标系记为Oxy，由于我们的极坐标系是地面呈一定的角度的α，同理此激光雷达的直角坐标系也同地面呈一定的角度的α。我们建立以激光雷达为原点、以车辆前进为x轴、垂直地面为z轴的车辆直角坐标系。即将Oxy二维直角坐标系绕y轴逆时针旋转90-α角度后可得到三维坐标系。我们将激光雷达绕y轴逆时针旋转90-α，则激光雷达的直角坐标系便可以和车辆坐标系同一，由于车辆在行驶时不断的振动、颠簸，我们需要得到车辆在行驶过程中的姿态角，以便补偿由于车辆行驶造成的坐标轴偏移，采用的陀螺仪测量车辆的每时刻的姿态角，然后通过旋转的坐标变换修正坐标系，但是由于陀螺仪的干扰产生的偏离稳定的输出造成的漂移，需要进行姿态角的补偿，选择合适的算法修正姿态角。由于车辆不断的前进，需要用GPS测量车辆每时刻的车速信息，以便修正车辆前方的物体和当前车辆的距离。即使用陀螺仪测量车辆的姿态信息，修正由于车辆行驶过程中不断的发生振动、颠簸而造成的坐标轴的偏移；通过GPS测得的速度信息将X坐标进行修正处理，得到当前位置时物体所有点距离车辆的距离；将该三维坐标导入Matlab后，得到三维模型。

以上显示和描述了本发明的主要特征、基本原理以及本发明的优点。本行业技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会根据实际情况有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。