一种基于运动状态检测与超声波测距的环境感知方法

技术领域

本发明属于移动载体定位与环境感知技术领域，尤其是涉及一种基于运动状态检测与超声波测距的环境感知方法。

背景技术

超声波传感器的工作基于发射和接收超声波，主要组成部分为传感器中的压电晶片(一般为石英晶片)。超声波传感器基于压电晶片的压电效应和逆压电效应工作。压电晶片受到机械应力后，内部产生极化现象，会在两个表面形成等量异号电荷，若施加的为机械震动，则将产生交变电场。相反，如果在该晶体上施加电场，该材料的晶格将出现形变,若施加的为交变电场,晶体将产生机械振动。超声波产生和发射利用了逆压电效应，将高频电压变化转换成高频机械振动，形成超声波；超声波接收利用正压电效应，将超声波高频机械振动转换成电信号，接收超声波。超声波探头按压电晶片结构分两种:只能发射或只能接收的单向压电晶片；既可发射又可接收的可逆压电晶片。相对应的，常见超声波传感器分为收发分离式和收发一体式。本专利采用的超声波传感器方案为收发一体式。

超声波传感器测距使用时间差法，即脉冲激励发生器发射一个激励脉冲，超声波发生器将发送一个脉冲序列超声波；该脉冲波经物体发射后被2超声波接收器接收；接收的脉冲波检测为有效反射；在上述过程中，时间控制系统记录超声波脉冲发射与接收的时间差；该时间差的一半乘以声速即为传感器与前方物体间的距离。具体计算公式如下：

其中L为传感器与被检测物体间的距离，v为当前条件下的声速，t为发射和接收脉冲波的时间间隔。

如果单纯利用超声波传感器的测距数据进行环境感知，存在以下几点问题：

1、超声波传感器的检测速度相比于激光雷达和视觉检测慢很多，这就导致超声波系统的测距频率较低，无法实现高帧率的距离检测，进一步在位置获取的层面无法实时的、高更新率的定位。

2、即使使用多传感器阵列对载体周围进行检测，也由于超声波较弱的指向性，只能实现几十度的检测角度，无法像激光雷达一样实现一度以下的角度分辨率，进一步也无法通过较密集的数据点判断周围环境特征的变化，无法推断出载体的运动情况。

3、通过反射超声波进行距离检测极度依赖前方障碍物，如果障碍物较远，则无法进行超声波测距，则超声波系统不会提供任何数据，无法获取定位信息。

惯性导航系统(Inertial Navigation System,INS)是以陀螺计和加速度计等为敏感器件的导航参数解算系统，是一种不依赖于外部信息、也不向外部辐射能量的自主导航系统。INS通过测量载体的角速度和加速度等数据，根据牛顿定律自动解算出载体当前的位置、速度和姿态信息，从而推算出目标的运动轨迹，实现目标的精确定位导航。具有不依赖于外界环境、不易受到干扰、定位精度高等优点，因此被广泛应用于军事、经济、医疗、民生多种领域。

惯性导航系统大致分为两种类型。根据配置和物理组成可以分为平台型和捷联式惯性导航系统。

在平台式惯性导航系统中，由于存在电机控制的实体稳定平台，加速度计直接固定在稳定平台上，陀螺与稳定平台的转动轴连接，稳定回路接收陀螺信息并发送控制信息控制稳定平台的转动，使稳定平台与水平面平行，从而隔离外界干扰。稳定平台的动态调节使得加速度计与设定的导航坐标系平行，直接测量载体在导航坐标系下的加速度，经过导航计算机算出载体在导航坐标系中的速度和位置信息，稳定平台则可以输出载体相对于导航坐标系下的姿态信息。由于稳定平台的存在，隔离了载体的角运动，设计姿态解算的工作量较小，对导航计算机的压力也较小。

捷联惯导系统直接安装于待定位的运载体之上，系统依靠陀螺信号输出角度位姿数据，结合加速度信号则能够通过多次积分获得位置信息，从而确定目标体在导航坐标系中的位姿。与平台惯导系统相比，捷联惯导系统节省了复杂的结构系统，其可靠性相对更高，体积与质量更小，成本更低。但是捷联系统主要依赖于一个计算机平台的数学解算，因此对处理器性能有一定的要求，但是现如今随着惯性系统与处理器的飞速发展，目前计算性能已不再是制约其发展的因素。

由于捷联式惯导系统安装与使用的便捷性，目前已经得到了广泛的应用，本项目使用的惯导系统即为捷联式惯性系统。惯性导航系统内部集成了加速度计，用于测量系统在三个坐标轴上的加速度变化。根据牛顿的第二定律(F＝ma)，系统可以通过测量施加在物体上的力以及物体的质量，计算出物体的加速度。同时，系统还包含陀螺仪，用于测量物体绕其三个坐标轴的角速度(即旋转速度)。通过测量角速度，系统能够捕捉物体的方向变化。对加速度计和陀螺仪的输出进行积分和运算，以获得物体的位置、速度和方向。通过对加速度积分两次，可以获得位置信息。通过对陀螺仪输出积分，可以获得方向信息。且得益于惯导系统的高采样率，理论上即可实时输出载体当前的位姿信息。

结合惯性导航系统，可以实现对载体当前运动状态的检测，进而可以实时推算出载体当前的位置，再结合上述超声波传感器系统对周围的障碍物距离进行检测，即可实现对周围环境的检测，实现环境感知

但是惯性导航系统有一个致命的问题就是：传感器存在漂移和噪声等问题，随着时间的推移，积分误差会逐渐累积，导致导航系统的精度下降。这一问题在常用的低成本MEMS惯导系统中尤其严重，几分钟的积分运算就会产生巨大的累计误差，导致数据彻底无法使用。因此要想利用惯导系统结合超声波传感器系统实现环境感知，必须减小惯性导航系统巨大的累计误差。

发明内容

本发明的目的是提供一种基于运动状态检测与超声波测距的环境感知方法，解决现有技术存在的惯性导航系统的累计误差对载体位置计算影响、惯性导航系统精度差的问题。

为实现上述目的，本发明提供一种基于运动状态检测与超声波测距的环境感知方法，包括以下步骤：

步骤一、将12个超声波传感器以间隔角度为30度分布在载体四周，将一个IMU传感器放置在载体上来获取载体的姿态角以及加速度数据，传输给系统；

步骤二、系统获取到IMU传感器输出的姿态角以及加速度数据后对数据进行初步处理，包括进行坐标系的转换，运动加速度的计算；

步骤三、对当前的运动状态进行判断，判断系统是处于静止还是运动状态；

步骤四、根据运动状态的结果，对载体的位置进行更新；

步骤五、判断当前超声波传感器阵列是否已经完成了数据采集，如果已经完成了数据采集，则对障碍物的位置进行计算，如果没有完成，则重新接收下一次的IMU数据，以此循环进行。

优选的，步骤二的具体过程如下：

S21、IMU输出姿态为欧拉角序列，欧拉角序列中(θ

C(θ

将欧拉角序列的方向余弦矩阵用欧拉角表示可得

设加速度计输出为a

此时获得的加速度为惯性坐标系下的加速度，结合当前载体的实际运动状态，载体只会在平面进行运动，所以不会在z轴方向有大范围位移，且由于载体的运动方式只能相对于载体自身在前后方向进行运动，因此只有运动方向的加速度为有效信息，将偏航角设置为0，如下式：

计算得到x方向加速度为

a

上式得出的加速度为载体在水平面沿着运动方向的加速度，在后续计算过程中a

S22、在S21基础上结合偏航角大小进行积分计算在惯性坐标系水平面上x，y方向的速度以及位置；载体在惯性坐标系x方向的速度为：v

载体在惯性坐标系y方向的速度为：v

优选的，步骤三中对当前的运动状态进行判断的具体过程如下：

S31、设置IMU数据判断缓存区的长度为N，每次获得IMU的数据，进行加速度计算之后，将加速度存入缓存区，同时删除时间最久的历史数据，对缓存区的加速度数据计算平方差σ

S32、判断载体当前的运动状态属于静止状态还是运动状态；

S33、在载体的运动状态判断完成后，如果载体处于静止状态，则将速度设置为0；如果载体处于运动状态，对载体的速度和位置进行积分。

优选的，S32中判断载体当前的运动状态属于静止状态还是运动状态的依据为：如果σ

优选的，步骤五中对障碍物的位置进行计算的具体过程如下：

S51、获取IMU数据计算的结果，载体在惯性坐标系水平面的坐标(x,y)；

S51、获取12个超声波传感器的测距数据，每个超声波传感器的测距数据代表在传感器对应方向的障碍物的距离，设每个超声波传感器相对于x轴正方向的偏转角度为θ，距离数据为L，则障碍物测距点的位置信息为：

因此，本发明采用上述一种基于运动状态检测与超声波测距的环境感知方法，具有以下有益效果：

1、在不需要其他定位传感器或者定位信息的参与下，极大减小了载体在二维平面运动过程中通过惯导系统定位结果的累计误差，使得系统可以在一定时间的运动之后，定位误差仍保持在可接受的程度；

2、通过超声波传感器阵列与惯性导航传感器相结合，可以在不需要外部信息的条件下实现对周围环境的基本感知；在周围环境光照不佳或者能见度不佳导致激光、视觉等传感器失效的情况下完成基本的地图构建。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1是本发明一种基于运动状态检测与超声波测距的环境感知方法的整体流程图；

图2是本发明传感器排布图；

图3是本发明静止状态加速度波形；

图4是本发明运动状态加速度波形；

图5是本发明惯性导航运动状态判断流程图；

图6是本发明实验场地尺寸图；

图7是本发明惯性声学结合试验轨迹1的示意图；

图8是本发明惯性声学结合试验轨迹1的实测图。

具体实施方式

以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-2，一种基于运动状态检测与超声波测距的环境感知方法，系统共包含12个超声波传感器，每个传感器间隔角度为30度分布在载体四周，从而实现对载体周围环境的检测。同时系统还包含一个IMU传感器，可以输出载体的运动姿态以及加速度信息。底层的姿态解算和陀螺仪传感器等底层驱动已经由IMU传感器模块集成，可以自动输出当前的姿态和加速度信息，此类传感器目前在市场上已经较为普遍。传感器的排布方式如图1。具体包括以下步骤：

步骤一、将12个超声波传感器以间隔角度为30度分布在载体四周，将一个IMU传感器放置在载体上来获取载体的姿态角以及加速度数据，传输给系统；

步骤二、系统获取到IMU传感器输出的姿态角以及加速度数据后对数据进行初步处理，包括进行坐标系的转换，运动加速度的计算；具体过程如下：

S21、IMU输出姿态为欧拉角序列，欧拉角序列中(θ

C(θ

将欧拉角序列的方向余弦矩阵用欧拉角表示可得

设加速度计输出为a

此时获得的加速度为惯性坐标系下的加速度。

结合当前载体的实际运动状态，载体只会在平面进行运动，所以不会在z轴方向有大范围位移，且由于载体的运动方式只能相对于载体自身在前后方向进行运动，因此只有运动方向的加速度为有效信息。因此可以将偏航角设置为0，如下式：

计算得到x方向加速度为

a

上式得出的加速度为载体在水平面沿着运动方向的加速度，在后续计算过程中a

S22、在S21基础上结合偏航角大小进行积分计算在惯性坐标系水平面上x，y方向的速度以及位置；载体在惯性坐标系x方向的速度为：v

载体在惯性坐标系y方向的速度为：v

上述两个步骤在理想状态下就可以积分获得载体当前的位置以及运动状态，但是在现实情况中，IMU输出的数据存在误差，如果不断使用积分就会使得数据误差迅速扩大，即使载体在静止状态，积分得到的位置也会在几分钟内快速漂移，根本无法实际使用。在此基础上提出本发明的一个重要关键点，对载体的运动状态进行静止状态检测。通过采集观察载体在不同运动状态的加速度输出，如图3，图4可以看出，载体在运动过程中的加速度波动状态与静止过程中的波动状态有着明显的区分。

在图3中，载体处于静止状态，但是加速度仍旧存在一定输出，但是可以看出输出的抖动范围基本小于0.0008。

在图4中，起始加速度为正向，是载体在从静止状态转换到运动状态，之后在中间段进入匀速运动状态，加速度输出维持在0附近，但是可以看出，加速度的波动范围较大，达到了0.01以上，这与静止状态的加速度数据有着明显区分。在曲线的后段，加速度为负值，这是载体在进行减速，从匀速状态减速到静止状态，之后经过一段加速度的波动，完全恢复到图3的静止状态的数据输出。

因此通过对加速度输出进行一定的计算可以判断出当前载体的运动状态，从而可以检测到载体的静止状态，进一步将加速度完全归零，从而避免了持续积分造成了大范围数据漂移。每次载体静止之后重新启动，都相当于是一次全新的运动，可以达到减小累计误差的作用，使得IMU数据可以有效推算出载体的运动状态。

步骤三、对当前的运动状态进行判断，判断系统是处于静止还是运动状态，如图5；具体过程如下：

S31、设置IMU数据判断缓存区的长度为N，每次获得IMU的数据，进行加速度计算之后，将加速度存入缓存区，同时删除时间最久的历史数据，对缓存区的加速度数据计算平方差σ

S32、判断载体当前的运动状态属于静止状态还是运动状态，如果σ

S33、在载体的运动状态判断完成后，如果载体处于静止状态，则将速度设置为0，保证载体的位置判定不会出现漂移；如果载体处于运动状态，则进行正常的积分计算，对载体的速度和位置进行积分。

步骤四、根据运动状态的结果，对载体的位置进行更新；

步骤五、判断当前超声波传感器阵列是否已经完成了数据采集，如果已经完成了数据采集，则对障碍物的位置进行计算，如果没有完成，则重新接收下一次的IMU数据，以此循环进行。对障碍物的位置进行计算的具体过程如下：

S51、获取IMU数据计算的结果，载体在惯性坐标系水平面的坐标(x,y)；

S51、获取12个超声波传感器的测距数据，每个超声波传感器的测距数据代表在传感器对应方向的障碍物的距离，设每个超声波传感器相对于x轴正方向的偏转角度为θ，距离数据为L，则障碍物测距点的位置信息为：

从上述方法中即可获得障碍物的位置信息，在载体的运动过程中，即可实现对周围环境的感知。

实施例

按照上述流程实现系统进行测试。测试环境如图6，小车运行轨迹1如图7。小车从最右侧开始运动，运动速度为1m/s，在右侧走廊结束后，向左转弯90°，之后再向前运动到达大厅边缘，再向右转弯90°，之后继续前进，到达左侧大厅边缘后右转90°，继续运动到左侧走廊起始，再向左转弯90°，最后一直运动到走廊末端。运动轨迹的长度54m左右，测试场景的长度为51m，宽度为5.3m。在小车以轨迹1运动过程中合并输出当前的运动位姿信息以及超声波数据，对数据进行绘制，结果如图8，图中数据单位为mm，根据图中标示的数据可以看出，系统检测到的大厅宽度为5.32m，测得走廊宽度为2.43m，符合实际值，直线的运动轨迹的最低端与最顶端长度为48m左右，与实际运动轨迹符合。周围数据点为声学系统系统的环境数据，可以看出与实际环境障碍物基本吻合，可以实现对周围环境的基本感知。

因此，本发明采用上述一种基于运动状态检测与超声波测距的环境感知方法，在载体的运动过程中IMU传感器会将载体的姿态角以及三轴加速度信息输出到处理器，同时，周围12个超声波传感器也会检测到12个距离信息，系统利用IMU的数据以及超声波传感器的数据对载体的运动状态进行计算以及获取周围环境的障碍物信息，提高了惯性导航系统的精度。

最后应说明的是：以上实施例仅用以说明本发明的技术方案而非对其进行限制，尽管参照较佳实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对本发明的技术方案进行修改或者等同替换，而这些修改或者等同替换亦不能使修改后的技术方案脱离本发明技术方案的精神和范围。