基于微型惯性测量单元的家用机器人碰撞检测方法

刘天一，白亮，杨鹏翔

（西安现代控制技术研究所，陕西 西安 710065）

碰撞检测是家用机器人的关键技术之一. 现有的家用机器人通常设置碰撞板机构用于检测碰撞[1]，没有碰撞板覆盖的部位不能检出碰撞，因此，家用机器人的碰撞检测存在盲区. 此外，碰撞检测板容易因杂物卡死而产生故障，且碰撞板只能定性检测碰撞，不能定量检测碰撞的方向或大小.本文以扫地机器人[2]为研究对象，以改善家用机器人碰撞检测能力为目的开展研究.

在引信[3-4]和车辆[5-6]中有时采用惯性传感器检测碰撞，利用加速度阈值、时间窗、加速度梯度等方法识别碰撞[7]. 惯性传感器可以检测任意位置的碰撞，有助于解决家用机器人碰撞漏检问题.但是家用机器人的碰撞加速度远小于汽车和引信的加速度，因此汽车或引信的碰撞检测方法不能直接用于家用机器人，必须加以改进.

本文基于微型惯性测量单元（miniature inertial measurement unit，MIMU）实现家用机器人任意部位碰撞的检测. MIMU是具有三轴加速度和三轴角速度测量功能的多轴惯性传感器. 本文将在两方面进行改进：1）较为精确地补偿干扰加速度，补偿机器人启停、俯仰横滚、杆臂效应等造成的干扰加速度，以提取碰撞造成的加速度信号；2）选取合适的碰撞判断标准，采用滑动窗积分的方式处理加速度信号，以幅度、持续时间两个标准判断碰撞.

1 MIMU 的补偿方法

家用机器人在发生碰撞时加速度有明显变化，可以通过测量加速度变化以检测碰撞. 碰撞加速度不超过2 g 的碰撞定义为轻微碰撞，超过2 g 的碰撞属于强烈碰撞. 引信与目标碰撞时的加速度达到30 000 g 至60 000 g[8]，车辆与道路护栏碰撞的加速度达到20 g[9]，均属于强烈碰撞. 而家用机器人工作速度较低、薄壁塑料结构刚性较差，属于轻微碰撞. 家用机器人正常工作情况下的加速度与碰撞加速度幅值接近，为检测碰撞增加了难度. 因此，必须对MIMU 数据中正常运行时的加速度成分进行补偿，只保留碰撞产生的加速度信号.

MIMU测量的加速度，与机器人基准位置加速度相比，存在杆臂效应加速度、重力加速度分量和测量误差，即

由于机器人结构布局的限制，MIMU往往不能安装在机器人的中心位置. 惯性传感器与载体（机器人）转动中心不重合，因此机器人转动时产生了额外的加速度信号，称之为杆臂效应. 杆臂效应加速度为

式中，r 为从机器人基准位置到MIMU位置的距离向量，ω 为角速度[10]. 角速度由MIMU中的陀螺仪测得. 以BMI055型MIMU为例并考虑实际工况，陀螺仪综合精度可达到2 /s° 或2%内，能满足碰撞检测的需求. 式（2）补偿杆臂加速度需要获取角加速度，而MIMU直接测量得到角速度. 对角速度直接微分得到的角加速度有较大噪声，不能用于补偿杆臂效应. 设计传递函数如式（3）的滤波器.

该滤波器在低于50 Hz 的范围内传递特性与微分环节近似. 角速度信号经过该滤波器处理后的信号近似为角加速度且噪声较小. 该滤波器输出信号作为式（2）的角加速度，用于补偿杆臂效应加速度.

图1 横滚方向的Mahony 互补滤波器

Mahony互补滤波器中陀螺仪角速度信号积分后与加速计信号比对，经过滤波网络后得到X 轴陀螺仪零偏，以及Y 轴重力加速度分量与加速度计缓慢变化误差的估计和. 在碰撞检测中不必进一步分离，利用式（1）一并补偿. 该滤波器对于加速计信号具有低通特性，因此家用机器人碰撞产生的瞬时加速度几乎不影响滤波器的输出.

2 碰撞的判断方法

在机器人发生碰撞或通过地面不平处时加速度都有明显波动，碰撞检测模块需要区分对待这两种情况，发生碰撞时应触发，而通过地面不平处时碰撞检测应当不触发. 采用简单的加速度幅值或类似指标作为判断标准容易引起误检测[14]. 碰撞时机器人会突然停止运动，而地面不平时机器人速度有所波动但是最终会恢复正常运行速度. 因此可以用速度变化量作为判断碰撞的标准. 速度变量法利于抑制干扰[15].

为了防止误检测，进一步采用滑动窗积分方法判断碰撞[16]，滑动窗积分法是速度变量法的改进方法. 滑动窗积分是对当前时刻之前一段时间内的信号做积分运算. 家用机器人为二维运动，因此对X 和Y 轴的加速度信号分别做滑动窗积分，取向量模长作为变换后信号.

理论上，碰撞变换后信号具有一个较宽的峰，而地面不平变换后信号具有两个较窄的峰. 原因是碰撞时加速度主要表现为单向波动，而地面不平时加速度信号表现为双向波动. 因此进一步使用时间阈值作为判断标准能区分碰撞和地面不平这两种情况.

碰撞检测方法包括3个步骤：加速度补偿、信号变换、碰撞判断. 采取滑动窗积分后的幅度阈值和时间阈值两个尺度共同作为判断碰撞的标准. 当加速度滑动窗积分值超过幅度阈值，且维持时间超过了时间阈值，则判定发生了碰撞.

3 实验及结果分析

3.1 实验装置

碰撞检测模块主要包括处理器和MIMU. 处理器选用较为常见的STM32L151 型微控制器，主频32 MHz. 用于导航的 MIMU 侧重于精度，如零偏稳定性等指标[17]；而用于碰撞检测的 MIMU 侧重于频率特性，因为碰撞过程持续时间很短、信号变化很快. 因此选用型号为BMI055[18]的MIMU，加速度计带宽1 000 Hz，陀螺仪带宽523 Hz，采样率2 000 Hz. 加速度计量程设定为16 g±，标度因数128 LSB/g（LSB 即Least Significant Bit，最低有效位）.

为了采集家用机器人的运动信号并验证碰撞检测方法，实验装置如图2所示. 在扫地机器人上胶粘固定电源、通信模块、碰撞检测模块（MIMU和处理器）. 碰撞判断的幅度阈值设定为0.08 m/s，时间阈值设定为75 ms. 滑动窗长度128 ms，即256采样点. 扫地机器人前方有碰撞检测板.

图2 碰撞实验装置

3.2 加速度补偿实验

为了检验加速度补偿的效果，在双轴转台上安装碰撞检测模块. 保持转台内框固定不动，只转动转台外框轴，如图3所示. MIMU安装位置为M点；转台外框轴保持水平，旋转中心为N点. 待碰撞检测模块工作稳定后，转台从 10-°旋转至 10+°，产生杆臂效应加速度并改变横滚角度决定的重力加速度分量. MIMU于M点随转台转动；N点模拟机器人基准位置. 记录补偿前后加速度如图4.

图3 加速度补偿实验

图4 中碰撞检测模块根据加速度计和陀螺仪数据获得了杆臂效应加速度和横滚角引起的重力加速度分量. 在转台启停时杆臂效应加速度有明显变化，最大为20.230 m/s . 重力加速度分量估计值从- 1 .684 m/s2变为 1 .761 m/s2. 因为转台角度对称地从 -1 0°旋转至 + 1 0°，重力加速度分量的变化理论上也是对称的. 实验结果中重力加速度分量的变化不完全对称，一方面源于陀螺仪标度因数误差，另一方面受限于加速度计分辨率. 理论上补偿后的N点加速度为0，实验中的补偿后加速度在 ±2 LSB以内，满足碰撞检测的需求.

图4 加速度补偿实验结果

3.3 碰撞判断实验

扫地机器人在家居环境中运行，碰撞检测模块对俯仰横滚、杆臂效应、启停引起的干扰加速度实时补偿，输出补偿后的加速度数据，该数据被计算机记录. 机器人正常运行时加速度比较平稳. 机器人发生碰撞或通过地面不平处时，加速度有明显波动，典型曲线分别如图5和图6，横坐标为时间，纵坐标为加速度. 图5是机器人与木制家具碰撞时的加速度曲线，图6是机器人通过地面不平处的加速度曲线.

图5 碰撞时的加速度

图6 通过地面不平处的加速度

两种情况下加速度曲线的主峰峰值分别为 9 .72 m/s2和 7 .49 m/s2，主峰宽度分别为35.0 ms 和39.5 ms. 家用机器人碰撞属于微弱碰撞，碰撞和地面不平的加速度曲线在峰值和宽度上没有显著差别、不易区分. 因此不能直接采用车辆碰撞或引信中使用的碰撞检测技术.

图5 和图6中的加速度信号进行滑动窗积分变换后信号如图7所示. 碰撞时变换后信号具有一个较宽的峰；地面不平时变换后信号具有两个较窄的峰. 在发生碰撞时变换后信号在B点超过幅度阈值，在E点回落. 地面不平时变换后信号在中部（C至D）因为正负抵消而回落，虽然变换后信号在A点和D点两次超过幅度阈值，但是分别在C点和F点回落. 碰撞时变换后信号维持在幅度阈值以上的时间（B点至E点）较长，超过了时间阈值；而地面不平时变换后信号维持在幅度阈值以上的时间（A点至C点，D点至F点）较短，未达到时间阈值. 采用幅度阈值和时间阈值共同作为判断碰撞的标准，可以有效区分这两种情况.

图7 滑动窗积分变换后信号

为了进一步检验碰撞检测方法的性能，设定6种实验工况：其中2种工况分别为机器人与木制家具或水泥墙壁发生碰撞，2种工况为机器人分别从低到高和从高到低通过台阶，台阶落差为5.6 mm；2种工况为机器人启动、停止（刹车）. 机器人在不同速度下每种工况试验10次. 对照组不使用碰撞检测模块，而使用机器人自身的碰撞检测板. 记录碰撞检测模块触发情况如表1.

表1 不同工况下的碰撞次数对比

4 结论

实验表明，本文方法能有效检测家用机器人的碰撞，优于传统的碰撞检测板方法. 同时，本文改进了惯性传感器检测碰撞的方法，使其适用于微弱碰撞的检测，对其他领域的微弱碰撞检测具有借鉴意义，但该方法不适合检测低于0.1 m/s 的极低速碰撞. 后续研究可综合利用多种方法，实现优缺点互补、进一步提高碰撞检测的适应性和准确性.