一种基于多传感器的智能汽车自主组合控制方法

1.本发明涉及多传感器组合的智能汽车自主控制系统，具体包含包括单目摄像头、电感电容谐振滤波传感器、光电编码器和姿态传感器等多种传感器在自主寻迹功能上的组合应用，属于机电智能控制领域。


背景技术：

2.现有的交通系统是由多种类型汽车组成的复杂系统，人参与并主导整个系统，人的行为对整个系统的性能起着举足轻重的作用。经过科学研究分析表明，在频发的交通事故中，由于驾驶员操作不当引起的占绝大部分，无法人为地有效避免，需要运用当今先进的科学技术来增加车辆感知环境的能力，实现汽车的辅助或者无人驾驶，因此研究和发展自主寻迹汽车是解决交通状况的重要途径。汽车发展至今，驾驶员所得到的环境信息大部分是通过视觉感知，如车道边界线、障碍物等，但车辆的姿态、速度等信息很难通过图像观测获得。此外在光照过亮和过暗的情况下，视觉感知会受到严重干扰，需要结合电磁导航等其它寻迹方式使得车辆能够正常运行。因此研究基于多传感器的组合寻迹方式十分重要。
3.《基于多传感器的机器人小车导航定位研究》(智凯旋，广东工业大学,2020.)一文中提出了基于多传感器的小车导航定位方式，利用激光雷达、双目视觉摄像头和惯性导航单元，弥补了单激光雷达在导航定位中无法适应复杂环境的缺点。但是改方案对于传感器的硬件成本以及控制单元的运算成本有很高的要求。本专利设计的智能控制方案能够适应复杂环境下的寻迹导航，并且降低了经济和开发成本，从而扩展了智能汽车的应用范围。


技术实现要素：

4.要解决的技术问题
5.为了克服使用单一传感器的车辆在复杂环境下自动寻迹时易受到干扰的缺陷，本发明提出一种基于多传感器的智能汽车自主组合控制方法。
6.技术方案
7.一种基于多传感器的智能汽车自主组合控制方法，所述的多种传感器包括单目摄像头、电感电容谐振滤波传感器、光电编码器和姿态传感器，其中单目摄像头和姿态传感器安装于车身中轴线上，姿态传感器位于单目摄像头之前，电感电容谐振滤波传感器安装于车辆前轮主轴之前，光电编码器安装于后轮齿轮上；其特征在于步骤如下：
8.步骤1：通过单目摄像头感知环境信息，校正图像畸变，对图像阈值分割提取车道边界特征；
9.步骤2：通过电感电容谐振滤波传感器检测车道上不同位置的电动势；所述的电感电容谐振滤波传感器包括一段导线以及位于导线两端的电感，导线的铺设位于车道中线上；
10.令两个电感之间的中点坐标为(xe,ye)，可以计算左右电感的电动势分别为
[0011][0012][0013]
式中，k为比例系数，l为导线的长度，h为电磁传感器离地高度，θ为电感在赛道上的投影与赛道中心线的夹角；
[0014]
步骤3：结合姿态传感器采集的数据，基于互补滤波算法感知车辆姿态；
[0015]
步骤4：组合方向及姿态信息的转向控制和速度控制；
[0016]
首先将车道边界特征线性化，在像素坐标系下将像素点使用下式所示的最小二乘法拟合为直线：
[0017][0018]
式中，n为需要拟合的边界线点数，x
ci
表示需要拟合的横坐标点值，y
ci
表示需要拟合的纵坐标值，a、b分别为车道边界线的直线斜率和截距；通过预设参数t1、t2计算相机获取的方向信息d：
[0019]
d＝t1*a t2*b
[0020]
通过下式描述的比值计算方法，获取偏差值e表示电感电容谐振滤波传感器获取的方向信息：
[0021][0022]
式中，ke表示尺度参数；
[0023]
结合步骤3得到的车身姿态，最终的方向数据dir写作下式以实现智能车转向控制
[0024]
dir＝w*d (1-w)*e
[0025][0026]
式中，w表示权重系数，c1和c2分别表示阈值分割时被分割为前景和背景的像素数量，pitch表示车身姿态的俯仰角度；
[0027]
车速v决策通过下式给出
[0028]
[0029]
式中，v
max
、v
min
表示预设的最大和最小速度，dir为方向数据，kv为尺度参数；读取光电编码器数据作为速度反馈，使用pid算法进行车速的闭环控制。
[0030]
本发明进一步的技术方案：步骤1中采用自适应阈值分割方法分割提取车道边界特征。
[0031]
本发明进一步的技术方案：步骤3具体如下：
[0032]
姿态传感器中集成了三轴加速度计和三轴陀螺仪；姿态传感器中集成了三轴加速度计和三轴陀螺仪；分别表示姿态传感器中加速度计采集的线加速度数据和陀螺仪采集的角速度数据；
[0033]
在三维空间中刚体姿态的四元数表示如下
[0034][0035][0036]
式中，q为四元数，qw、q
x
、qy、qz为四元数各分量，为相互正交的单位方向向量；
[0037]
n表示当前采样时刻，采样时间为t，用q(n)表示当前时刻的四元数，向量之间的误差用向量叉乘来表示；互补滤波过程可以表示为
[0038][0039][0040][0041][0042][0043][0044][0045]
q(n)＝qw(n) q
x
(n)i qy(n)j qz(n)k
[0046]
其中，为当前加速度量测值与预测值间的误差，为上个采样时刻的误差，为当前时刻的误差，表示加速度的预测值，为修正后的角速度数据，k
p
和ki为给定的调节参数；的预测过程为
[0047][0048][0049][0050]
得到滤波后的姿态四元数表示后，通过四元数与欧拉角的转换关系可得到当前时
刻三轴姿态的欧拉角pitch、yaw、roll，转换关系为：
[0051]
首先进行四元数归一化
[0052][0053][0054]
再转换为欧拉角
[0055][0056]
yaw＝sin-1
(-2(q
x
(n)qz(n)-qw(n)qy(n)))
[0057][0058]
式中，yaw、pitch、roll分别为车身姿态的偏航、俯仰、翻滚角度。
[0059]
有益效果
[0060]
本发明针对车辆寻迹导航设计了一种多传感器组合控制系统，采用自适应阈值分割方法，提取车道的边缘特征，初步实现寻迹导航；同时根据毕奥-萨伐尔定理感知磁导航线的方向，在复杂环境情况下辅助寻迹；并且根据不同车道类型和车身姿态，完成智能车的转向和车速控制。突破了目前已有基于摄像头等单个传感器无法在复杂光线环境下易受干扰的限制，提高了车辆运行的稳定性。
[0061]
此外，本发明针对不同车道环境下，采用自适应传感器权重配比和二次项规划的方法进行方向和车速控制，提升车辆在不同车道的适应性和运行的鲁棒性。
附图说明
[0062]
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
[0063]
图1各传感器模块安装示意图；
[0064]
图2车道坐标系与电感排布方案；
[0065]
图3本发明执行步骤流程图。
具体实施方式
[0066]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0067]
本发明提出一种基于多种传感器组合寻迹导航控制方法，多种传感器包括单目摄像头、电感电容谐振滤波传感器、光电编码器和姿态传感器，其中单目摄像头和姿态传感器安装于车身中轴线上，姿态传感器位于摄像头之前，电感电容谐振滤波传感器安装于车辆
前轮主轴之前，光电编码器安装于后轮齿轮上，如附图1所示。该方法能够提升车辆运行时的稳定性，解决了不能稳定寻迹的问题，具体步骤为：
[0068]
步骤一：通过单目摄像头感知环境信息，校正图像畸变，对图像阈值分割提取车道边界特征；
[0069]
对相机建立小孔成像模型，场景中的三维点为pc＝(xc,yc,zc)，成像平面距离相机重心的距离为f，需要将三维点变换为成像平面下的二维像素点，依据小孔成像的原理，可以得到下面的变换公式
[0070][0071]
式中，x
dis
、y
dis
分别为像素平面坐标系(以图像左上角为原点)下的横坐标和纵坐标，dx、dy为一个像素的长和高，c
x
、cy表示从成像平面坐标系至像素平面坐标系下的平移量，xc、yc、zc分别为相机坐标系下现实场景中点的三维坐标值。r、t分别表示旋转变换和平移变换，组成齐次变换矩阵，将以相机坐标系为参考系的坐标点(xc，yc，zc)变换到世界坐标系下。
[0072]
考虑到实际情况下相机透镜制造的工艺导致的径向畸变和相机元件安装时导致的切向畸变问题。使用r＝0处的泰勒级数展开的前几项来近似描述径向畸变。
[0073]
径向和切向畸变可用以下公式修正
[0074]
xc＝x
dis
(1 k1r2 k2r4 k3r6) x
dis
[2p1xy p2(r2 2x
dis2
)]
[0075]
yc＝y
dis
(1 k1r2 k2r4 k3r6) y
dis
[p1(r2 y
dis2
) 2p2x
disydis
]
[0076]
式中，xc、yc分别表径向修正后的坐标，k1、k2、k3分别表示径向畸变的参数，p1、p2分别表示切向畸变的参数。
[0077]
阈值分割使用大津算法动态计算阈值，存在阈值k将图像所有像素分为称作前景像素和背景像素的两类，c1(小于k)和c2(大于k)，两类像素各自均值分别为m1、m2，图像全局均值为mg，像素被分为c1和c2的概率分别为p1、p2，因此可以写出下式：
[0078]
p1＊m1 p2＊m2＝mg
[0079]
p1 p2＝1
[0080]
根据方差的概念，类间方差表达式为：
[0081]
σ2＝p1(m1-mg)2 p2(m2-mg)2[0082]
式中σ2为方差。将其简化可得：
[0083]
σ2＝p1p2(m1-m2)2[0084][0085]
[0086][0087]
式中，i为灰度级数(取值范围0～255)，pi为灰度级数i对应的像素在图像中出现的概率，k为分割阈值，l为8位灰度值能表示的最大级数l＝28＝256。
[0088]
灰度级的累加均值m和图像全局均值mg分别可以表示为
[0089][0090][0091]
因此可以将m1、m2变为以下形式
[0092]
m1＝1/p1*m
[0093]
m2＝1/p2*(mg-m)
[0094]
得到最终的类间方差公式：
[0095][0096]
根据公式使方差σ2最大化的k即为图像全局最佳分割阈值，实现图像的二值化，对车道边缘的提取，用于步骤四中方向控制信号的计算。
[0097]
步骤二：电感电容谐振滤波传感器检测车道上不同位置的电动势；
[0098]
沿车道的走向事先铺架通有固定频率交变信号的电磁线路，使用lc谐振电路结合放大电路检测该信号以获取环境信息。
[0099]
首先，为了获取正确频率的信号，在硬件上需要根据以下公式选取合适参数的电感-电容配对。
[0100][0101]
式中，f为交变信号的固定频率。
[0102]
为方便论述，假设车辆沿着车道前进的方向为z轴，垂直地面向上为y轴，在地面所在平面垂直于车道中心线为x轴，建立如图2所示的三轴坐标系，满足右手定则，电磁传感器的长度为l。
[0103]
导线的铺设位于车道中线上，如附图2所示。沿着车道前进方向，交变信号产生的磁场分布为以导线为中心的同心圆，在计算磁场强度时仅需考虑坐标(xe,ye)，车道上的磁导线对于电磁传感器可等效为无限长直导线，根据毕奥-萨伐尔定理可以得到单个电感感应电动势e的大小可以用下式表示
[0104][0105]
式中，k为比例系数，与电路中使用的运算放大器放大倍数有关，h为电磁传感器离
地高度，xe为单个电感偏离磁导线的距离，θ为电感在赛道上的投影与赛道中心线的夹角。
[0106]
步骤三：基于互补滤波算法感知车辆姿态；
[0107]
对于斜坡路面或颠簸路面，很难依靠摄像头等传感器精确计算车身姿态并及时做出相应的措施。因此采用姿态传感器解算姿态的精确值，并运用一定方法抑制零漂等问题。
[0108]
姿态传感器中集成了三轴加速度计和三轴陀螺仪。姿态传感器中集成了三轴加速度计和三轴陀螺仪。分别表示姿态传感器中加速度计采集的线加速度数据和陀螺仪采集的角速度数据。对于陀螺仪来说存在温漂和零漂，在低频段动态响应特性差而高频动态响应特性好；对于加速度计来说，它的静态特性好，动态特性较差。因此对陀螺仪测量的角度数据进行高通滤波，对加速度计测量的加速度数据进行低通滤波，然后通过互补，实现在整个频域上表现良好的特性。
[0109]
在三维空间中刚体姿态的四元数表示如下
[0110][0111][0112]
式中，q为四元数，qw、q
x
、qy、qz为四元数各分量，为相互正交的单位方向向量。
[0113]
n表示当前采样时刻，采样时间为t，用q(n)表示当前时刻的四元数，向量之间的误差用向量叉乘来表示。互补滤波过程可以表示为
[0114][0115][0116][0117][0118][0119][0120][0121]
q(n)＝qw(n) q
x
(n)i qy(n)j qz(n)k
[0122]
其中，为当前加速度量测值与预测值间的误差，为上个采样时刻的误差，为当前时刻的误差，表示加速度的预测值，为修正后的角速度数据，k
p
和ki为给定的调节参数。的预测过程为
[0123][0124]
[0125][0126]
得到滤波后的姿态四元数表示后，通过四元数与欧拉角的转换关系可得到当前时刻三轴姿态的欧拉角pitch、yaw、roll，转换关系为：
[0127]
首先进行四元数归一化
[0128][0129][0130]
再转换为欧拉角
[0131][0132]
yaw＝sin-1
(-2(q
x
(n)qz(n)-qw(n)qy(n)))
[0133][0134]
式中，yaw、pitch、roll分别为车身姿态的偏航、俯仰、翻滚角度。
[0135]
步骤四：组合方向及姿态信息的转向控制和速度控制；
[0136]
组合相机传感器和电磁传感器的方向控制。首先将车道边界特征线性化，在像素坐标系下将像素点使用下式所示的最小二乘法拟合为直线
[0137][0138]
式中，n为需要拟合的边界线点数，x
ci
表示需要拟合的横坐标点值，y
ci
表示需要拟合的纵坐标值，a、b分别为车道边界线的直线斜率和截距。通过预设参数t1、t2计算相机获取的方向信息d
[0139]
d＝t1*a t2*b
[0140]
再根据感生电动势的计算原理，令两个电感之间的中点坐标为(xe,ye)，可以计算左右电感的电动势分别为
[0141][0142][0143]
式中，l为传感器的长度。通过下式描述的比值计算方法，获取偏差值e表示电磁传
感器获取的方向信息。
[0144][0145]
式中，ke表示尺度参数。
[0146]
结合imu得到的车身姿态，最终的方向数据dir写作下式以实现智能车转向控制
[0147]
dir＝w*d (1-w)*e
[0148][0149]
式中，w表示权重系数，c1和c2分别表示阈值分割时被分割为前景和背景的像素数量，pitch表示车身姿态的俯仰角度。
[0150]
车速v决策通过下式给出
[0151][0152]
式中，v
max
、v
min
表示预设的最大和最小速度，dir为方向数据，kv为尺度参数。下层读取光电编码器数据作为速度反馈，使用pid算法进行车速的闭环控制。这样的车速控制决策简单，速度梯度明显，具有明显的前瞻性，能够及时地加减速。
[0153]
实施例1：
[0154]
为了提升车辆寻迹导航的稳定性，本发明设计了一种基于多传感器组合的控制系统，增强车辆自主运行的鲁棒性，下面结合实际的车模运行过程阐述本发明的具体实施方式：
[0155]
执行步骤一：通过单目摄像头视觉感知环境信息，校正图像畸变，对图像阈值分割提取车道边界特征；
[0156]
使用cmos摄像头，通过硬件完成图像的拍摄和坐标变换，实现视觉方法对环境的感知。
[0157]
使用张氏相机标定方法，拍摄多张特征点明显的照片，借助计算机辅助软件实现畸变参数k1,k2,k3,p1,p2的校正。
[0158]
动态阈值分割算法以处理分辨率为188*180灰度图为例，主要进行两个步骤，一是直方图统计，二是阈值计算。
[0159]
图像的直方图统计为
[0160][0161]
其中p(si)为灰度级si在图像中出现的概率，ni为灰度级为si的像素个数，n＝188*180为总像素个数。直方图的统计量为：
[0162][0163][0164][0165]
其中s为总灰度值，s1为前景(c1类)像素点的灰度均值，s2为背景(c2类)像素点的灰度均值，k为所需求解的最佳阈值。
[0166]
阈值计算根据公式
[0167]
σ2＝p1(1-p1)(m1-m2)2[0168]
p1＝p(sk)，(k＝0，1，2...255)
[0169]
m1＝s1[0170]
m2＝s2[0171]
迭代求解使得σ2最大的灰度阈值k。并根据以下公式完成图像的二值化，实现车道边界特征提取。
[0172][0173]
执行步骤二：电磁传感器检测车道上不同位置的电动势；
[0174]
在磁导线中通入电流为100ma，频率为20khz的交变信号。首先根据lc振荡电路谐振周期计算公式，计算电感、电容参数
[0175][0176][0177]
这里选择10mh电感和6.3mf电容配对用于检测20khz的信号。
[0178]
取两个电感之间的中心坐标为(0.1,0)，电磁传感器长度l为30cm，离地高度h为5cm，左电感的坐标为(0.3,0.05)，右电感的坐标为(-0.1,0.05)，两个电感在赛道上的投影与赛道中心线的夹角α取比例系数k取0.1。左右电感的感生电动势e1、e2分别为
[0179]
[0180][0181]
执行步骤三：基于互补滤波算法的感知车辆姿态；
[0182]
将姿态传感器上一次计算所得四元数转换到车身机体坐标系，可以推算得到前机体坐标系下重力单位向量为
[0183][0184]vx
＝2(q
xqz-qwqy)
[0185]
vy＝2(q
wqx
qyqz)
[0186][0187]
加速度计测得当前机体坐标系下重力单位向量为
[0188][0189]
得到当前估算姿态和测量姿态之间的误差为
[0190][0191]
用向量叉乘表示为
[0192][0193]
将上次误差表示为
[0194][0195]
使用线性叠加的方式表示为本次实际误差
[0196][0197][0198]
本次误差和陀螺仪误差均在机体坐标系上，且两者的大小成正比，可用来纠正陀螺仪。对本次误差使用pi控制进行零偏校正，方法如下：
[0199][0200][0201][0202]
式中，为陀螺仪数据，n为第几个控制周期，通过调整k
p
和ki参数修正陀螺仪数据。然后通过四元数微分方程，对修正后的陀螺仪数据积分，得到当前的四元数表示为
[0203]qw
(n)＝qw(n-1) (-q
xgx-q
ygy-q
zgz
)t
[0204]qx
(n)＝q
x
(n-1) (q
wgx
q
ygz-q
zgy
)t
[0205]
qy(n)＝qy(n-1) (-q
wgy-q
xgz
q
zgx
)t
[0206]
qz(n)＝qz(n-1) (qwg2 q
xgy-q
ygx
)t
[0207]
式中，t为控制周期。最后将更新后的四元数归一化，得到最新的四元数姿态。
[0208][0209]
通过四元数与欧拉角之间的转换公式，得到直观的欧拉角姿态形式。
[0210][0211]
pitch＝sin-1
(-2(q
xqz-qwqy))
[0212][0213]
式中，yaw、pitch、roll分别为车身姿态的偏航、俯仰、翻滚角度。
[0214]
步骤四：组合方向及姿态信息的转向控制和速度控制；
[0215]
阈值分割后的二值化图像可以用以下矩阵表示
[0216][0217]
式中，1表示分割后的背景，0表示前景。使用4邻域种子生长的方法获取边界像素点的坐标，代入最小二乘方程求解以图像左下角为原点的左边界线y
l
和以图像右下角为原点右边界线yr，并计算方向数据d
[0218]yl
＝a
l
x b
l
[0219]
yr＝arx br[0220][0221][0222]
取a
l
＝4，b
l
＝9，ar＝-3.75，br＝-36，前景像素点数c1＝25380，背景像素点数c2＝6345
[0223]
d＝α*a β*b＝0.25α-27β
[0224]
感生电动势e1＝0.077v，e2＝1v
[0225][0226]
令智能车在平整的路面上，车身俯仰姿态角pitch＝0，控制转向的方向数据为
[0227][0228]
dir＝w*d (1-w)*e
[0229]
＝0.75*(0.25α-27β)-0.25*0.67ke[0230]
令v
max
＝200，v
min
＝150，车速数据为
[0231][0232]
其中车速使用光电编码器的反馈值作为表示，α、β、ke、kv等系数需要根据实际情况调节。将控制转向和车速的数据输入底层pid控制器实现多传感器组合的自主寻迹智能车控制。
[0233]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。