CN112891162B - 基于移动可穿戴计算的智慧盲杖 - Google Patents

Abstract

基于移动可穿戴计算的智慧盲杖属于电子信息领域，是一种基于可穿戴计算技术的盲人辅助装置。装置自身具有照明、发声发光呼救功能并可通过按钮控制已连接的移动智能设备上功能的启用或关闭。移动智能设备上的功能有：使用阈值分割、RANSAC和区域生长算法实现障碍物检测功能；语音播报当前时间、位置信息功能；红绿灯检测功能。本发明采用新型的结构设计，具有单指操作的特性；自动连接移动智能设备，并与其高效通信；配套的APP上具有语音信息播报、障碍物检测、红绿灯检测功能；采用振动的方式向盲人迅速反馈，增强其的使用体验。

Description

基于移动可穿戴计算的智慧盲杖

技术领域

本发明属于电子信息领域，是一种基于移动互联网和可穿戴计算技术的辅助装置，主要借助智慧盲杖与移动智能手持设备的交互，为视障人士室外行走提供基于语音或触觉的反馈，以辅助盲人避障。

背景技术

在我国，视力障碍人士逾1730万，居世界第一。盲杖作为视障人士出行必需品，能够通过触碰地表来感知前方是否有障碍物。目前市场上的智能盲杖种类繁多，价格低廉的产品仅有语音求助功能，然而集成红外线或超声波进行障碍物探测的盲杖价格又十分高昂。所以造成了价格低的智能盲杖功能过于单一，高价智能盲杖又面临着功耗高、体积大等问题。

可穿戴计算以自然的穿戴形式为用户提供计算能力，让人们更加关注于产品功能而忽略计算设备的外观形态。移动计算使得计算机或其他信息智能终端设备在无线网络环境下实现数据及计算资源的共享，其随着移动通信、互联网、分布式计算等技术的发展日益融入到大家的工作生活中。短距离无线通信技术是将二者有机融合的桥梁，目前最常用的短距离无线通信技术有wifi、蓝牙、Zigbee。其中，蓝牙可以让移动设备在短距离内建立连接，实现数据传输和交换。与wifi和zigbee相比，蓝牙具备低功耗、低辐射、低成本、低延时等特点。因此，本发明采用蓝牙实现盲杖与移动智能设备间的通讯。

发明内容

为实现移动智能设备与盲杖通讯，并通过移动智能设备和盲杖辅助视障人士出行的目标，本发明借助可穿戴计算、移动计算和蓝牙技术的优势，提出的基于移动可穿戴计算的智慧盲杖包括：盲杖的杆体和杖柄。其中所述杖柄中设置有电源、主控模块；所述杖柄前侧设置有照明灯；所述杖柄内侧(即使用中大拇指所处位置)设置有按钮盘，所述按钮盘包括一个开关按钮以及五个功能按钮。所述主控模块集成了CC2640低功耗蓝牙微处理器、振动马达、蜂鸣器以及所述照明灯、按钮盘控制链路与接口连接。

本发明的特征如下：

①单指操作

本发明设计了一种圆盘式按钮组，按钮之间保留一定距离以防误触。按钮盘安装在盲杖杖柄的内侧，视障人士仅通过大拇指即可完成所有操作。按钮上带有盲文，代表不同功能。视障人士轻触某按钮后，盲杖会立即执行该功能。

②按钮功能

开关按钮：

圆盘中心的按钮为开关，处于关机状态时，长按3s后自动开机并迅速以振动方式向盲人反馈，同时尝试与移动智能设备连接，连接成功后由设备向用户语音反馈或由盲杖杖柄振动反馈提示“连接成功”。处于开机状态时，长按3s开关按钮后自动关机，并由盲杖振动反馈。

如遇配对不成功或用户无需连接设备等情况，蓝牙模块在3分钟后会进入休眠状态以节省能源。在休眠状态中轻触开关按钮，蓝牙模块重新尝试与设备连接并重新开始计时。

照明灯按钮：在光线较暗的情况下，照明灯辅助视力障碍人士夜间出行。

发声、发光呼救按钮：如遇险情，视障人士可按下呼救按钮，启动蜂鸣器发声呼救，同时照明灯不断开关形成曳光向救援人员告知其所处位置。

信息播报按钮：轻触该按钮，本发明由蓝牙向已连接的移动智能设备发送请求，设备接收后通过其扩音器向视障人士语音播报当前位置、朝向、时间。

障碍物检测开关：轻触该按钮，本发明向移动智能设备发送请求，打开或关闭移动智能设备的障碍物检测功能。

红绿灯检测开关：轻触该按钮，本发明向移动智能设备发送请求，打开或关闭移动智能设备的红绿灯检测功能。

③振动/语音反馈模式根据用户需求切换

反馈是人机交互的关键要素。本发明在设计过程中考虑到视障人士在户外行走时，双耳需密切关注周围环境中的声音，进而判断车辆的行驶方向、红绿灯起止时间等等，为了减少来自语音反馈给盲人判断环境状态带来的干扰，本发明采用默认振动反馈。振动反馈的优势在于简单迅速，轻触按钮后通过预设的几种振动模式向视障人士反馈必要信息。

本发明采用如下模式：

1)开机：1次连续长振动，持续时间0.8s

2)任意功能操作成功：3次短振动，每次振动持续时间0.2s，间隔0.1s。

3)任意功能操作失败：5次快速短促振动，每次振动持续0.1s，间隔0.1s。

4)障碍物警示：根据移动智能设备回传的前方障碍物距离数值，进行不同频率的振动。距离越小，振动频率越快。振动持续至前方无障碍物时停止。

5)关机：2次中等时间振动，每次振动持续时间0.4s，间隔0.1s。

在移动设备app中，视障人士可根据需要选择使用语音反馈或振动反馈。

④基于移动智能设备的盲人位置、朝向和时间信息语音播报

系统利用移动智能设备内置的GPS/北斗、指南针、时钟、蓝牙等元器件和功能模块，开发运行于移动智能设备的程序，通过接收智慧盲杖上“信息播报功能”的请求，为视障人士提供语音播报视障人士当前所处位置、朝向和时间，辅助视障人士出行。

⑤基于移动智能设备的障碍物检测报警

系统利用移动智能设备内置的摄像头、气压计等元器件和功能模块，开发基于阈值和区域成长的障碍物监测程序。当用户打开智慧盲杖上“障碍物检测开关”，移动智能设备运行该程序并进行基于视觉的障碍物检测，当检测到障碍物时为视障人士提供语音或振动告警。

⑥基于移动智能设备的红绿灯监测

系统利用移动智能设备内置的摄像头开发基于Yolo v4的红绿灯检测识别程序。当用户打开智慧盲杖上“红绿灯检测开关”，移动智能设备上的红绿灯检测程序将进行红绿灯的检测识别，并将检测结果通过语音告知视障人士。

与现有技术相比，本发明的有益效果是：采用新型的结构设计，使得本发明能够单指操作；自动连接移动智能设备，并与其高效通信；同时，将语音信息播报、障碍物检测、红绿灯检测等功能转移至移动智能设备上，既降低了盲杖的功耗，又能帮助盲人在行进中最大化、无障碍地利用盲杖和移动智能设备；采用振动的方式向盲人迅速反馈，增强盲人的使用体验。

附图说明

图1本发明所处网络环境

图2本发明的系统结构示意图

图3本发明杖柄部分的内部结构示意图

图4本发明开机及连接设备流程图

图5本发明功能执行流程图

图6本发明与移动智能设备交互流程图

图7障碍物检测功能执行流程

图8相机坐标系与世界坐标系示意图

图9摄像机高度示意图

图10区域划分和地面检测结果

具体实施方式

图1为本发明所处网络环境，图中：1、盲杖杖柄；2、智能手机；3、智能眼镜；4、可穿戴设备；5、平板电脑。如图所示，盲杖杖柄1中的蓝牙模块开启后可与指定移动智能设备建立蓝牙连接，本发明所指的移动智能设备包括：智能手机2；智能眼镜3；可穿戴设备4；平板电脑5。

图2为本发明的系统结构示意图，如图所示：本发明处于开机状态时，轻触开关11，系统产生配对中断请求，CC2640低功耗蓝牙微处理器6尝试与周围的移动智能设备配对。长按3s开关11，系统产生关机中断请求，CC2640低功耗蓝牙微处理器6进入关闭状态。轻触功能按钮12，系统产生对应功能的中断请求，CC2640低功耗蓝牙微处理器6接收请求后进行该功能的处理。CC2640低功耗蓝牙微处理器6直接控制振动马达7；照明灯8；时钟9；蜂鸣器10。时钟9到达3分钟后产生休眠中断请求，CC2640低功耗蓝牙微处理器6接收休眠中断后将时钟归0，并进入休眠状态。电源模块由充电控制器13；电池14构成。充电控制器13采用MAX1555充电控制器，电池14采用LP2992锂电池控制芯片，使模块既能通过3～5V锂电池供电，也可以通过USB接口电路供电，并可通过USB接口为锂电池充电。

图3为本发明杖柄部分的内部结构示意图，所述振动马达6；振动马达7；照明灯8；蜂鸣器10；开关11；功能按钮12均与CC2640低功耗蓝牙微处理器6电性连接。照明灯8安装在杖柄1前部；开关11和功能按钮12安装在杖柄1内侧中部，方便视障人士单指操作；蜂鸣器10和CC2640低功耗蓝牙微处理器6设置在杖柄1中部偏后；振动马达7设置在杖柄1偏后部，位于视障人士手掌下方；充电控制器13和电池14安装在杖柄1后部。

本发明开关机及连接设备的过程：

如图4所示当盲杖处于关机状态时，长按3s开关按钮开机，振动马达5通过0.8s的长振动提示已开机，并启动时钟。随后唤醒CC2640低功耗蓝牙微处理器6，自动进入配对模式，尝试与周围的移动智能设备配对。连接成功后通过移动智能设备语音提示“连接成功”，时钟归0。若连接不成功，且未接收到休眠中断，CC2640低功耗蓝牙微处理器6继续尝试配对。时钟到达3分钟后发送休眠中断请求并归0，CC2640低功耗蓝牙微处理器6进入休眠状态，振动马达7以5次快速短促振动，每次振动持续0.1s，间隔0.1s的方式提示配对失败。处于休眠模式时，轻触开关按钮，手动向CC2640低功耗蓝牙微处理器6发送信号，再次尝试配对。开机状态下，长按3s开关机按钮，振动马达7通过连续两次0.4s，间隔0.1s的振动提示已关机。

本发明功能的执行流程：

如图5所示，盲杖开机后，轻触照明灯开关，产生照明灯中断请求，CC2640低功耗蓝牙微处理器6接收请求后开启照明灯8，再次轻触照明灯按钮关闭照明灯。轻触呼救功能按钮，产生呼救中断请求，CC2640低功耗蓝牙微处理器6接收请求后开启蜂鸣器10，并不断开关照明灯8，再次轻触呼救功能按钮关闭蜂鸣器10和照明灯8。连接移动智能设备后，轻触语音播报当前位置、朝向、时间按钮、障碍物检测开关、红绿灯检测开关，分别产生语音播报中断、障碍物检测中断、红绿灯检测中断，CC2640低功耗蓝牙微处理器6向移动智能设备发送相应功能指令，设备接收并解析指令，执行对应功能并回传操作结果。盲杖接收操作结果后振动马达7通过3次短振动，每次振动持续时间0.2s，间隔0.1s提示操作成功，通过5次快速短促振动，每次振动持续0.1s，间隔0.1s提示操作失败。

本发明与移动智能设备交互流程：

如图6所示，盲杖开机并与移动智能设备连接成功后，轻触信息播报按钮，CC2640低功耗蓝牙微处理器6向设备发送指令F-INFO，设备接收并解析指令后读取当前设备的位置、朝向、时间信息，并通过扩音器播报。轻触障碍物检测开关，CC2640低功耗蓝牙微处理器6向设备发送指令F-OBSTICLE，设备接收并解析指令后打开或关闭障碍物检测功能，并向盲杖回传操作结果，盲杖振动反馈操作结果。轻触红绿灯检测开关，CC2640低功耗蓝牙微处理器6向设备发送指令F-TRAFFICLIGHT，设备接收并解析指令后打开或关闭红绿灯检测功能。并向盲杖回传操作结果，盲杖振动反馈操作结果。其中，操作结果代码R-SUCC表示操作成功；R-FAIL表示操作失败。

本发明障碍物检测功能执行流程：

如图7所示，盲杖开机并与移动智能设备连接成功后，进入休眠状态。当设备检测到前方出现障碍物时，向盲杖发送障碍物信息代码，盲杖接收到信息后被唤醒，随即解析前方障碍物信息。其中，障碍物信息代码规则如下：OBS-NONE，表示前方无障碍物；OBS-2，表示前方2米有障碍物；OBS-1，表示前方1米有障碍物。若信息代码表示前方有障碍物，盲杖则根据障碍物的距离信息选择不同的振动频率向用户持续振动反馈。振动频率选取：前方2米出现障碍物，盲杖振动频率为5Hz；前方1米出现障碍物，盲杖振动频率为10Hz。若接收的障碍物信息表示前方无障碍物，盲杖停止振动，进入休眠状态。

障碍物检测算法：

障碍物检测算法4部分组成，分别是阈值分割、坐标系转换、基于RANSAC的地面检测和基于地面区域生长的障碍物检测。障碍物检测算法运行流程如下：障碍物检测功能开启后，移动端app访问当前设备的摄像机，实时获取并处理前方场景的深度图数据。

首先运行阈值分割算法，由于深度图反映的是摄像头到物体的距离信息，因此参考公式(1)，将图像深度阈值σ设置为2000，单位mm，滤除有效感知距离之外的深度图数据，即忽略距离过远的物体，只处理在阈值σ范围内的数据，避免大量无效数据参与后续计算，有利于算法的实时性。公式(1)中i、j表示像素点在深度图中的行、列下标，a表示该像素点的深度值。

第二，如图8所示，本发明融合加速度计以及陀螺仪数据，将深度图数据生成为相机坐标系O_c-X_cY_cZ_c下的点云数据，再将其转换成世界坐标系O_w-X_wY_wZ_w下的点云数据，以矫正方向。坐标系转换方式如下所述。

首先利用当前设备的加速度传感器数据以及陀螺仪数据计算姿态角度。姿态解算选用的旋转顺序为ZYX，摄像头坐标系初始时刻与世界坐标系重合，然后依次绕自己的Z、Y、X轴进行旋转，绕着Z轴旋转角度γ、绕着Y轴旋转角度β、绕着X轴旋转角度α。当加速度计水平放置，即Z轴竖直向上时，Z轴可以读到1g的数值(g为重力加速度)，X轴和Y轴两个方向读到0。当加速度计旋转一定的姿态时，重力加速度会在加速度的3个轴上产生相应的分量。将加速度计读到的3个值记作[a_x,a_y,a_z]。根据公式(2)可以求出角度α和角度β，由于绕Z轴旋转时，感受到的重力加速度是不变的，因此仅靠加速度计无法计算γ角。

接着使用设备的陀螺仪测量绕3个轴转动的角速度，记作[g_x,g_y,g_z]，对角速度积分可以得到角度。如公式(3)所示，摄像头在t-1时刻时的姿态角度为α_t-1、β_t-1、γ_t-1，t时刻的姿态角为α_gyro、β_gyro、γ_gyro，摄像头由t-1时刻到t时刻经历了3次旋转，Δα、Δβ、Δγ为对应的姿态角度变化量。参考公式(4)，姿态角度的变化量可通过角速度与采样时间周期积分得到，Δt为采样时间周期，大小为0.02s。注：公式(4)中的角速度

以世界坐标系为参考系，而陀螺仪在第t时刻读出的角速度[g_x,g_y,g_z]以相机坐标系为参考系，故依据公式(5)将陀螺仪测量的角速度转换为姿态更新需要用到的角速度

根据上面的分析，加速度传感器在静止时刻，可以计算出α和β角，在短时间内因为运动造成的加速度是不准确的，在长时间内的平均值是准确的，即加速度数据存在高频噪声。陀螺仪仅对转动时的姿态变化敏感，且其若本身存在误差，使得陀螺仪数据存在低频噪声。因此，本发明使用互补滤波器结合两者计算的姿态角进行互补融合，如公式(6)所示。

其中，k为比例系数，

本发明设置为0.98 (6)

最后，利用相机姿态角实现将相机坐标系O_c-X_cY_cZ_c中数据向世界坐标系O_w-X_wY_wZ_w数据的转换。假定相机坐标系统中点P_c的坐标为(x_c,y_c,z_c)，姿态角为(α,β,γ)。公式(7)表示点P_c的旋转过程，通过3个旋转矩阵R₁、R₂、R₃将坐标又相机坐标系下转换为世界坐标系下。对点云数据中的所点进行此操作即可完成坐标系转换。

三个旋转矩阵分别为：

第三，如图9所示，为了拍摄到前方距离身体较近的障碍物，需稍向下倾斜摄像头，但由于身前的地面与摄像头距离太近，利用阈值分割无法简单地将地面和障碍物分开，所以还需要对这种情况进行单独的地面去除操作。对此，本发明采用改进后的随机样本一致性算法(random sample consensus，RANSAC)进行地面检测并滤除。公式(8)描述了三维空间中一个平面的数学模型。RANSAC算法采用迭代方式拟合平面数学模型公式(8)中的参数A、B、C、D，其具体步骤如下：

①从世界坐标系O_w-X_wY_wZ_w下的点云数据中随机选取一个集合的点，用于计算公式(8)中A、B、C、D的最初取值。

②依据公式(9)计算出点云中每个点(x_i,y_i,z_i)到该平面的距离d_i。

③设置距离阈值δ，如果d_i<δ，那么点(x_i,y_i,z_i)属于平面；否则，该点不属于平面。

④重复步骤②～④，迭代n次，比较并选取含有效数据点最多的平面作为最终结果；

Ax+By+Cz＝D (8)

据公式(10)来选择合适迭代次数n；在RANSAC算法中，迭代次数n和阈值δ的取值直接影响检测结果的精度。迭代次数n取值过大会导致运行时间过长，取值过小又不能较好的拟合平面。故本发明依据公式(10)来选择合适迭代次数k。其中，p为至少一次选取到正常点的概率，η为点云异常点的比率，s为每次迭代选取点的数量。此外，若阈值δ取值过小会漏掉属于地面的点云，而δ取值过大则会添加异常点。对此，本文按照公式(11)计算点到平面距离的标准差σ，并取2σ作为阈值(即δ＝2σ)。当d_i>δ时，此点被认为是异常值点，不当作地面点云提取；反之，为有效值点，被当作地面点云提取出来。

其中，

本发明对RANSAC算法的改进如下：首先，在步骤②拟合出平面参数后，按照公式(12)求解该平面的倾斜角度θ。世界坐标系O_w-X_wY_wZ_w下属于该平面的点云数据，其与地面间的倾斜角应该为零。如果拟合平面的倾斜角度大于30°，则该平面不是地平面，应剔除该平面并停止执行后续步骤。这一方法可有效避免场景中墙壁等倾斜面的干扰，减少了不必要的迭代计算。其次，本发明依据摄像机对地倾斜角度θ和距离地面的深度depth，按照公式(13)计算摄像机距离地面的高度height(如图9所示)。随后求出点云中所有的点的高度，并计算与地面的高度差，将高度差大于200(mm)的点清除不再执行RANSAC算法。

height＝cosθ×depth (13)

如果对整个RGB-D图像的点云都采用RANSAC算法进行平面拟合，计算量过大难以满足实时性要求。对此，本发明将摄像机采集的数据划分为A、B、C、D四个区域，如图10所示。其中，A为正常通行区域，B为左侧环境区域，C为右侧环境区域，D为前方较远区域。A区域位于使用者身前，包含最为丰富的地面信息，所以本发明将C区域视为感兴趣区域(Region ofInterest，ROI)，只对ROI的深度图数据生成点云，采用上述改进后的RANSAC算法进行地面拟合，并将拟合出来的地面点云映射到深度图中。

第四，采用改进后的RANSAC算法可以大致拟合出地平面，但其对图像边缘处理存在不足。并且如前所述，为了避免计算量过大，本发明只对ROI区域(A区域)生成点云并拟合地面。但当盲人行走方向发生变化时，还需要对左右两侧B、C区域的地面进行补充检测，因此本发明采用区域生长算法查找剩余地面。根据陀螺仪旋转角度的计算公式(4)，判断当前盲人的行走方向。Δγ数值为正代表此时盲人正在转向左方，左侧区域B的障碍物信息需要检测；Δγ数值为负代表此时盲人正在转向右方，右侧区域C的障碍物信息需要检测。

区域生长算法的作用是将符合生长规则的点合并到一起，进而形成平面区域。其运行流程为：

①将RANSAC算法拟合到的地面像素点作为种子点，将所有的种子点存储在集合S＝{G₁,G₂,…,G_s}中。

②从集合S中取出种子点G_i(i＝1,2,…,s)，以G_i为中心点进行地面区域生长，将种子点G_i的生长区域集合计作R_i，将点G_i的上、左、右邻域点记作P_ij(j＝1,2,3)。以下文定义的生长规则为标准，将P_ij(j＝1,2,3)中满足生长规则的邻域点添加到集合R_i中。

③重复步骤②，直到为集合S中的每个种子点都进行地面区域生长。

④最后将所有种子点G_i(i＝1,2,…,s)所产生的生长区域R_i合并，得到总地面区域R_g，如公式(14)所示，s为集合S中点的个数，∪表示取并集。

区域生长一般有8邻域和4邻域两种邻域选取方式，一旦确定邻域选取方式，须对邻域中每个点按照规定的生长规则进行判断。考虑到深度图中地面像素值仅与4邻域像素存在联系，且地面像素的左右邻域和上下邻域深度值变化方式不同，传统的方式不适用于本发明所处场景，故提出一种三邻域的选取方式。对于种子点G_i，首先以行为单位，选取与其处于同一行的左右邻域像素点，将满足生长规则的点添加到集合R_i中。当同一行元素生长结束后，再选取该行所有种子点的上邻域像素点，将这些点中满足生长规则的点添加到集合R_i中。

本发明规定的生长规则如下。S＝{G₁,G₂,…,G_s}为种子点集合，S中种子点个数为s。对于任一种子点G_i(i＝1,2,…,s)的上、左、右邻域点P_ij(j＝1,2,3)，若满足如下条件则添加到种子点G_i的生长区域R_i中：

(1)P_ij不属于任何生长区域，即

(m∈[1,s]且m≠i)。

(2)通常地面左右邻域像素的深度值之差不超过15(mm)，地面的下邻域像素到上邻域像素深度值会呈现递增态势。若P_ij满足公式(15)，则将其添加到R_i中。其中δ_h为增长阈值，设为100(mm)，h_G为种子点G_i的深度值，h为点P_ij的深度值。

(3)邻域像素点的选取必须在B、C区域，超出范围就会停止生长。

最后，将上述算法提取出的地面像素清除，留下的像素均为障碍物。

如果检测结果中A、B、C中某一区域出现障碍物像素数量超过10000个，就视这个区域存在障碍物，app向盲杖发送障碍物信息代码。如果一个区域障碍物像素数量低于10000，则视这个区域不存在障碍物，app向盲杖发送前方无障碍物代码。

Claims (1)

1.一种应用基于移动可穿戴计算的智慧盲杖的方法，该盲杖包括盲杖的杆体和杖柄；其中所述杖柄中设置有电源、主控模块；所述杖柄前侧设置有照明灯；所述杖柄内侧设置有按钮盘，所述按钮盘包括一个开关按钮以及五个功能按钮；所述主控模块集成了CC2640低功耗蓝牙微处理器、振动马达、蜂鸣器、照明灯及按钮盘控制链路；

其特征在于，障碍物检测算法包括：阈值分割、坐标系转换、基于RANSAC的地面检测和基于地面区域生长的障碍物检测；

障碍物检测算法运行流程如下：障碍物检测功能开启后，移动端app访问当前设备的摄像机，实时获取并处理前方场景的深度图数据；

第一，运行阈值分割算法，由于深度图反映的是摄像头到物体的距离信息，因此参考公式(1)，将图像深度阈值σ设置为2000，单位mm，滤除有效感知距离之外的深度图数据，即忽略距离过远的物体，只处理在阈值σ范围内的数据；公式(1)中i、j表示像素点在深度图中的行、列下标，a表示该像素点的深度值；depth为距地面的深度；

第二，融合加速度计以及陀螺仪数据，将深度图数据生成为相机坐标系O_c-X_cY_cZ_c下的点云数据，再将其转换成世界坐标系O_w-X_wY_wZ_w下的点云数据，以矫正方向；坐标系转换方式如下所述；

首先利用当前设备的加速度传感器数据以及陀螺仪数据计算姿态角度；姿态解算选用的旋转顺序为ZYX，摄像头坐标系初始时刻与世界坐标系重合，然后依次绕自己的Z、Y、X轴进行旋转，绕着Z轴旋转角度γ、绕着Y轴旋转角度β、绕着X轴旋转角度α；当加速度计水平放置，即Z轴竖直向上时，Z轴可以读到1g的数值，g为重力加速度；X轴和Y轴两个方向读到0；当加速度计旋转一定的姿态时，重力加速度会在加速度的3个轴上产生相应的分量；将加速度计读到的3个值记作[a_x,a_y,a_z]；根据公式(2)求出角度α_acc和角度β_bcc，由于绕Z轴旋转时，感受到的重力加速度是不变的，因此仅靠加速度计无法计算γ角；

接着使用设备的陀螺仪测量绕3个轴转动的角速度，记作[g_x,g_y,g_z]，对角速度积分可以得到角度；如公式(3)所示，摄像头在t-1时刻时的姿态角度为α_t-1、β_t-1、γ_t-1，t时刻的姿态角为α_gyro、β_gyro、γ_gyro，摄像头由t-1时刻到t时刻经历了3次旋转，Δα、Δβ、Δγ为对应的姿态角度变化量；参考公式(4)，姿态角度的变化量通过角速度与采样时间周期积分得到，Δt为采样时间周期，大小为0.02s；

公式(4)中的角速度

以世界坐标系为参考系，而陀螺仪在第t时刻读出的角速度[g_x,g_y,g_z]以相机坐标系为参考系，故依据公式(5)将陀螺仪测量的角速度转换为姿态更新需要用到的角速度

根据上面的分析，加速度传感器在静止时刻，计算出α和β角，在短时间内因为运动造成的加速度是不准确的，在长时间内的平均值是准确的，即加速度数据存在高频噪声；陀螺仪仅对转动时的姿态变化敏感，且其若本身存在误差，使得陀螺仪数据存在低频噪声；使用互补滤波器结合两者计算的姿态角进行互补融合，如公式(6)所示；

其中，k为比例系数，

k设置为0.98 (6)

最后，利用相机姿态角实现将相机坐标系O_c-X_cY_cZ_c中数据向世界坐标系O_w-X_wY_wZ_w数据的转换；假定相机坐标系统中点P_c的坐标为(x_c,y_c,z_c)，姿态角为(α,β,γ)；公式(7)表示点P_c的旋转过程，通过3个旋转矩阵R₁、R₂、R₃将坐标从相机坐标系下转换为世界坐标系下；对点云数据中的所有点进行此操作即完成坐标系转换；

三个旋转矩阵分别为：

第三，采用改进后的随机样本一致性算法(random sample consensus，RANSAC)进行地面检测并滤除；公式(8)描述了三维空间中一个平面的数学模型；RANSAC算法采用迭代方式拟合平面数学模型公式(8)中的参数A、B、C、D，其具体步骤如下：

①从世界坐标系O_w-X_wY_wZ_w下的点云数据中随机选取一个集合的点，用于计算公式(8)中A、B、C、D的最初取值；

②依据公式(9)计算出点云中每个点(x_i,y_i,z_i)到该平面的距离d_i；

③设置距离阈值δ，如果d_i<δ，那么点(x_i,y_i,z_i)属于平面；否则，该点不属于平面；

④重复步骤②～④，迭代n次，比较并选取含有效数据点最多的平面作为最终结果；

Ax+By+Cz＝D (8)

据公式(10)来选择合适迭代次数n；其中，p为至少一次选取到正常点的概率，η为点云异常点的比率，s为每次迭代选取点的数量；按照公式(11)计算点到平面距离的标准差σ，并取2σ作为阈值即δ＝2σ；当d_i>δ时，此点被认为是异常值点，不当作地面点云提取；反之，为有效值点，被当作地面点云提取出来；

其中，

首先，在步骤②拟合出平面参数后，按照公式(12)求解该平面的倾斜角度θ；世界坐标系O_w-X_wY_wZ_w下属于该平面的点云数据，其与地面间的倾斜角应该为零；如果拟合平面的倾斜角度大于30°，则该平面不是地平面，应剔除该平面并停止执行后续步骤；

其次，依据摄像机对地倾斜角度θ和距离地面的深度depth，按照公式(13)计算摄像机距离地面的高度height；随后求出点云中所有的点的高度，并计算与地面的高度差，将高度差大于200mm的点清除不再执行RANSAC算法；

height＝cosθ×depth (13)

将摄像机采集的数据划分为A、B、C、D四个区域；其中，A为正常通行区域，B为左侧环境区域，C为右侧环境区域，D为前方较远区域；A区域位于使用者身前，包含最为丰富的地面信息，将C区域视为感兴趣区域ROI，只对ROI的深度图数据生成点云，采用上述改进后的RANSAC算法进行地面拟合，并将拟合出来的地面点云映射到深度图中；

第四，采用区域生长算法查找剩余地面；根据陀螺仪旋转角度的计算公式(4)，判断当前盲人的行走方向；Δγ数值为正代表此时盲人正在转向左方，左侧区域B的障碍物信息需要检测；Δγ数值为负代表此时盲人正在转向右方，右侧区域C的障碍物信息需要检测；

区域生长算法的作用是将符合生长规则的点合并到一起，进而形成平面区域；其运行流程为：

(一)将RANSAC算法拟合到的地面像素点作为种子点，将所有的种子点存储在集合S＝{G₁,G₂,…,G_s}中；

(二)从集合S中取出种子点G_i(i＝1,2,…,s)，以G_i为中心点进行地面区域生长，将种子点G_i的生长区域集合计作R_i，将点G_i的上、左、右邻域点记作P_ij(j＝1,2,3)；以下文定义的生长规则为标准，将P_ij(j＝1,2,3)中满足生长规则的邻域点添加到集合R_i中；

(三)重复步骤(二)，直到为集合S中的每个种子点都进行地面区域生长；

(四)最后将所有种子点G_i(i＝1,2,…,s)所产生的生长区域R_i合并，得到总地面区域R_g，如公式(14)所示，s为集合S中点的个数，∪表示取并集；

考虑到深度图中地面像素值仅与4邻域像素存在联系，且地面像素的左右邻域和上下邻域深度值变化方式不同，提出一种三邻域的选取方式；对于种子点G_i，首先以行为单位，选取与其处于同一行的左右邻域像素点，将满足生长规则的点添加到集合R_i中；当同一行元素生长结束后，再选取该行所有种子点的上邻域像素点，将这些点中满足生长规则的点添加到集合R_i中；

规定的生长规则如下；S＝{G₁,G₂,…,G_s}为种子点集合，S中种子点个数为s；对于任一种子点G_i(i＝1,2,…,s)的上、左、右邻域点P_ij(j＝1,2,3)，若满足如下条件则添加到种子点G_i的生长区域R_i中：

(1)P_ij不属于任何生长区域，即

(2)若P_ij满足公式(15)，则将其添加到R_i中；其中δ_h为增长阈值，设为100mm，h_G为种子点G_i的深度值，h为点P_ij的深度值；

(3)邻域像素点的选取必须在B、C区域，超出范围就会停止生长；

将提取出的地面像素清除，留下的像素均为障碍物；

如果检测结果中A、B、C中某一区域出现障碍物像素数量超过10000个，就视这个区域存在障碍物，app向盲杖发送障碍物信息代码；如果一个区域障碍物像素数量低于10000，则视这个区域不存在障碍物，app向盲杖发送前方无障碍物代码。

Images