一种基于重力向量和角速度向量的汽车变道识别方法
【技术领域】
[0001] 本发明涉及汽车变道识别的研究领域，特别涉及一种基于重力向量和角速度向量 的汽车变道识别方法。
【背景技术】
[0002] 随着城市车辆数量的逐渐增多，道路的车辆也越来越多，交通越来越堵塞，在车辆 拥挤的交通道路上，车辆可能会以多种情况进行变道，可能是前方建设需要，可能是前方的 交通事故，也可能仅仅是因为想超车。违法变道是很常见的一种交通违规事件，据统计有超 过半数的交通事故都和违法变道有关系，由此可见其危害性巨大。所以违法变道的检测具 有重要的实际意义。传统的交通事件检测系统需在道路下面物理埋设大量环形线圈检测器 或在路边安装超声波检测器。它们的缺点环形线圈检测器需要破环现有的公路地面以安装 地感线圈，都存在工作寿命、使用环境及可靠性不足的问题。
【发明内容】
[0003] 本发明的主要目的在于克服现有技术的缺点与不足，提供一种基于重力向量和角 速度向量的汽车变道识别方法。
[0004] 为了达到上述目的，本发明采用以下技术方案：
[0005] -种基于重力向量和角速度向量的汽车变道识别方法，包括下述步骤：
[0006] (1)通过固定在车上的六轴传感器得到重力向量及角速度向量，读取六轴传感器 的四元数及角速度向量；
[0007] (2)根据四元数计算出六轴传感器的重力向量；
[0008] (3)进行水平面校准，使得六轴传感器的空间坐标系的水平面与汽车的空间坐标 系水平面平行；
[0009] (4)进行上坡面校准，使得水平面的坐标系重合；
[0010] (5)经过校准后得到了汽车的重力向量及角速度向量，判断方向是否已经偏航，若 是则转至步骤（6)，若否则转至步骤（1);
[0011] (6)计算偏航时段特征，所述偏航时段为从汽车方向发生改变到重新回到直线行 驶的时段；
[0012] (7)偏航时段过滤，按照时段出现的先后顺序得到了变道的候选偏航序列；
[0013] (8)寻找可能的变道组合，筛选出所有可能的偏航时段组合，得到变道候选组合；
[0014](9)变道操作最优匹配；
[0015] (10)输出变道的类型，跳转至步骤（1)。
[0016] 优选的，步骤（2)中，假设六轴传感器的四元数为（(11，(12，(13，(14)，角速度向量为 〇^，巧，^),通过以下公式得到重力向量&1，87,82):
【主权项】
1. 一种基于重力向量和角速度向量的汽车变道识别方法，其特征在于，包括下述步骤： (1) 通过固定在车上的六轴传感器得到重力向量及角速度向量，读取六轴传感器的四 元数及角速度向量； (2) 根据四元数计算出六轴传感器的重力向量； (3) 进行水平面校准，使得六轴传感器的空间坐标系的水平面与汽车的空间坐标系水 平面平行； (4) 进行上坡面校准，使得水平面的坐标系重合； (5) 经过校准后得到了汽车的重力向量及角速度向量，判断方向是否已经偏航，若是则 转至步骤（6)，若否则转至步骤（1); (6) 计算偏航时段特征，所述偏航时段为从汽车方向发生改变到重新回到直线行驶的 时段； (7) 偏航时段过滤，按照时段出现的先后顺序得到了变道的候选偏航序列； (8) 寻找可能的变道组合，筛选出所有可能的偏航时段组合，得到变道候选组合； (9) 变道操作最优匹配； (10) 输出变道的类型，跳转至步骤（1)。
2. 根据权利要求1所述的基于重力向量和角速度向量的汽车变道识别方法，其特征在 于，步骤（2)中，假设六轴传感器的四元数为（91，92,93,94)，角速度向量为〇^，巧，^)，通 过以下公式得到重力向量（gx，gy，gz): gx= 2*(q2*q4_ql*q3); gy= -2*(ql*q2+q3*q4); gz=ql*ql_q2*q2_q3*q3+q4*q4 ; 其中每隔固定时长读取一次数据。
3. 根据权利要求1所述的基于重力向量和角速度向量的汽车变道识别方法，其特征在 于，步骤（3)中，水平面校准的具体步骤为： (3. 1)把汽车停放在水平地面上，记录下六轴传感器的重力向量（cgxQ，cgy(l，cgzQ),记此 时汽车的重力向量为（^!￡(|，￥87(|，￥82(|)，其中￥8 !￡(|=0，￥87(|=0，￥82(|=^表示重力单位 值，六轴传感器的重力向量通过两步旋转来得到汽车的重力向量： (3. 1. 1)绕Z轴旋转角度a，使得六轴传感器的重力向量为（
，），因 此得到： 0. cgx0*cos ( a ) +cg〇〇*sin ( a ) (1)
求解方程组（1)得到旋转角度a; (3. 1.2)经过（3. 1.1)之后，绕X轴旋转角度0，使得六轴传感器的重力向量为
求解方程组（2)得到旋转角度3 ; (3. 2)进行水平面旋转： 在得到水平面校准角度a、卩后，其他任意时刻下，由六轴传感器的重力向量 (cgx，cgy，cgz)及角速度向量（crx，cry，crz)得到汽车的重力向量（vgx，vgy，vgz)及角速度向 M(crxJcryJcrz)： vgx=egx^cos(a) +cgy^sin(a) vgy=(_cgx木sin(a) +cgy木cos(a))>!<cos( 3 ) +cgz木sin( 3 )vgz=(egx木sin(a) _cgy木cos(a))木sin( 3 ) +cgz木cos( 3 ) vrx=crx木cos(a) +cry木sin(a) (3) vry=(_crx木sin(a) +cry木cos(a))>!<cos( 3 ) +crz木sin( 3 )vrz=(crx木sin(a) _cry木cos(a))木sin( 3 ) +crz木cos( 3 ) O
4. 根据权利要求3所述的基于重力向量和角速度向量的汽车变道识别方法，其特征在 于，步骤（4)中，上坡面校准的具体步骤为： (4. 1)把车辆停放于斜坡上，保持车左右两边水平，记录下六轴传感器的重力向量 (cgxl,cgyl,cgzl),经过步骤（3. 2)的水平面旋转后组件的重力向量记为（cgx2,cgy2,cgz2),当 汽车置于上坡面时，车辆的重力向量为
六轴传感器重力向量绕Z轴 旋转9角度后可得到汽车重力向量，因此： 0 =cgx2*cos( 0 ) +cgy2*sin( 0 ) (4)
求解方程组（4)得到旋转角度e; (4. 2)上坡面旋转； 在得到上坡面校准时的选择角度0后，其他任意时刻下，由经过水平面旋转得到的 六轴传感器重力向量（Cgx，cgy，cgz)及角速度向量（crx，cry，crz)得到汽车的重力向量 (vgx，vgy，vgz)及角速度向量（crx，cry，c;rz): vgx=egx*cos( 0 ) +cgy*sin( 0 ) vgy=-egx*sin( 0 ) +cgy*cos( 0 ) vgz=egz vrx=crx*cos( 0 ) +cry*sin( 0 ) (5) vry=-crx*sin( 0 ) +cry*cos( 0 )vrz=crz〇
5. 根据权利要求4所述的基于重力向量和角速度向量的汽车变道识别方法，其特征在 于，步骤（5)中，判断方向是否已经偏航的方法是进行状态建模，其具体方法为： 首先选取角速度Z轴分量crz序列，逆时针方向为cr2的正方向，对Z轴序列做如下处理：
其中1~_是一个经过优化的角速度限值，当IcrzI<rmin时认为角速度的变化是由于设备 测量误差引起的，此时车处于直线行驶状态；当|crz|彡1*_时认为汽车方向发生了改变。
6. 根据权利要求5所述的基于重力向量和角速度向量的汽车变道识别方法，其特征在 于，步骤（6)中，计算偏航时段特征的方法为： 根据好的状态模型得到汽车行程的偏航时段序列，对于偏航时段i，记开始时间为tbp结束时间为tep在这段时间内汽车水平角速度最大值记为max^iv角速度积分值记为 totalcr^偏航持续时间为tli=te厂1^，用dire表示偏航方向，当totalcrAO时记为左 偏航dire= 1 ;当totalcr^O时记为右偏航dire= 2。
7. 根据权利要求5所述的基于重力向量和角速度向量的汽车变道识别方法，其特征在 于，步骤（7)中，偏航时段过滤的方法为： 在正常的驾驶过程中，汽车在变道中的偏航角度及持续时间有一定的范围值，设定合 适的持续时间最小限值min_tl、最大限值max_tl，偏航最大角速度最小限值m_cr，偏航最 大角度积分mt_cr，当偏航时段符合如下的所有条件时，判定该偏航时段为变道候选偏航时 段： tl^min_tl max_tl max-cr^m-cr (7) total-cr^mt-cr 偏航时段经过过滤后，按照时段出现的先后顺序得到了变道的候选偏航序列 〈moc^，…，mod)。
8. 根据权利要求1所述的基于重力向量和角速度向量的汽车变道识别方法，其特征在 于，步骤（8)中，两个偏航时段有可能组成变道操作的必要条件为： 1) 两个偏航时段为相邻模块； 2) 两个偏航时段的方向相反，即direi#direi+1; 3) 两个偏航时段覆盖总时长不超过时长A，即AT1; 4) 两个偏航时段的间隔时间不超过时长AT2，即tbi+1-tei<AT2; 5) 两个偏航时段的角速度积分绝对值之和超过设定的合适限值minAll，即|total_ cri|+total_cri+11 ^minAll； 6) 两个偏航时段的角速度绝对值只差不超过设定的合适限值max_d，即| |total_ cr； | -total_cri+11 ^max_d； 经过以上的判断必要条件，筛选出所有有可能的偏航时段组合，得到变道候选组合 〈（modi,modi+1)，…，（modj，modj+1) > 〇
9. 根据权利要求1所述的基于重力向量和角速度向量的汽车变道识别方法，其特征在 于，步骤（9)具体为： 对于出现在变道候选组合〈(modi,modi+1)，…，（modj,modj+1) >中的modi,当只存在 (modpmoU或者（mod^modi)时，则（modDmoU或者（mod^modi)直接判定为变道； 当（mod^moU(mod^modi)均存在时，若mod^modi+i的角速度积分差值要比mod# modH的角速度积分差值小时，判定（modpmodi+i)为变道，否则判定（modi+modi)为变道。
10. 根据权利要求1所述的基于重力向量和角速度向量的汽车变道识别方法，其特征 在于，步骤（10)中，在得到变道（moc^mocU)后，当modi*左偏航时段时，为向左变道，否 则为右变道；变道开始时间为tbp结束时间为tei+1。
【专利摘要】本发明公开了一种基于重力向量和角速度向量的汽车变道识别方法，包括下述步骤：通过固定在车上的六轴传感器得到重力向量及角速度向量，读取六轴传感器的四元数及角速度向量，计算六轴传感器的重力向量，再进行水平面和上坡面校准得到六轴传感器坐标系旋转到车辆坐标系的转换参数，通过转换参数把六轴传感器的重力向量及角速度向量映射成车辆的重力向量及角速度向量，运用角速度判断驾驶方向是否已经偏航并计算偏航时段特征；然后进行偏航时段过滤，寻找可能变道组合；最后匹配最优的变道操作，输出变道的类型。发明使用重力向量可以计算出汽车的空间状态如水平、倾斜、上坡、下坡等，使用角速度向量识别车辆变道，能够提高行车安全及驾驶技能。
【IPC分类】B60W40-114
【公开号】CN104554277
【申请号】CN201510038913
【发明人】刘锬, 令狐铁民, 邱珠成
【申请人】广州通易科技有限公司
【公开日】2015年4月29日
【申请日】2015年1月26日