一种基于预瞄跟踪的盲人行走闭环诱导控制方法
【技术领域】
[0001] 本发明设及一种基于预瞄跟踪的盲人行走闭环诱导控制方法，用于导盲设备W引 导盲人沿给定路径行走，尤其适用于穿戴式导盲设备。
【背景技术】
[0002] 出行困难一直是困扰盲人和视障人±的首要问题，提供一种有效的导盲方法或系 统，为视障人±的出行提供方便可靠的诱导方式，一直是社会和科研领域关注的热点问题。
[0003] 为解决解决盲人出行问题，近年来陆续出现了一些导盲装置，大致可W分类为开 环控制导盲系统和闭环控制导盲系统。现有的开环控制导盲系统具有实时定位盲人位置、 探测周围环境、检测障碍物距离等功能，能够帮助盲人认知周围的环境信息，代表性装置包 括GI^S语音导航、超声波盲杖等。现有的闭环控制导盲系统在开环控制导盲系统的基础上 进行了改进，在感知环境的同时并进行实时的分析与控制，对盲人施加离散的逻辑反馈控 制信号，在行走过程中为盲人提供左右转向的提示。
[0004] W上方法具有各自的优点，同时也具有一些缺陷。现有的开环控制导盲系统具有 装置简单，可靠性强，适用范围广等优点，但是该系统只能提供盲人周围的环境信息，却不 能给出灵敏直接的控制信号来引导盲人行走，使导盲效果的实时性和便捷性下降。现有的 闭环控制导盲系统实现了完整的闭环控制和人机交互，但是该系统提供的离散逻辑控制信 号只能对盲人的行走方向进行控制，却无法对转向角度的大小进行调节，导致其控制精度 较差。由于运些方法本身的缺陷和限制，致使精确导盲不能实现。
【发明内容】
阳〇化]本发明的技术解决问题：克服上述现有技术的不足，提供一种基于预瞄跟踪的盲 人行走闭环诱导控制方法。引入预瞄跟踪机制，通过简单可靠的行走角度控制，W-种有效 的人机交互的控制模式和直接的信息传递方式，实现对盲人行走的闭环连续诱导，达到精 确导盲的目的。
[0006] 本发明的技术解决方案是：一种基于预瞄跟踪的盲人行走闭环诱导控制方法，在 盲人行走过程中不断获取盲人当前位置和行走方向角，按预瞄半径在给定路径中捜索预瞄 点作为跟踪目标，并W当前位置到预瞄点的连线作为预瞄方向，计算盲人行走方向角关于 预瞄方向角的偏差，根据控制律计算和输出行走方向角控制量，控制盲人调整行走方向，实 现盲人行走趋于给定路径的诱导控制。
[0007] 所述的给定路径是已知的，该路径来源于实时检测、规划或预先设置的路径，可W 采用轨迹方程的形式进行描述，也可W采用一系列有序的点进行描述。
[0008] 所述的预瞄点捜索首先获得盲人当前位置和行走方向角，再W当前位置为圆屯、， W预瞄半径L为半径，在当前行走方向±90°的范围内的给定路径上捜索预瞄点，其中预 瞄半径L是关于速度的函数，其关系如下：
[0009]
[0010] 该式中，Lm。、和Lmm分别是预瞄半径的上限和下限距离；Vm。、和Vmi。分别是普通人在 正常情况下行走的最大速度和最小速度；A为行走速度与预瞄半径的比例系数。
[0011] 所述的控制律为PID控制，被控量为预瞄方向角与盲人行走方向角的偏差。预瞄 方向角是盲人当前位置指向预瞄点的矢量在地面坐标系下的方向角，盲人行走方向角指的 是盲人当前行走速度矢量在地面坐标系下的方向角。将预瞄方向角减去盲人行走方向角得 到角度偏差，该偏差输入至PID控制器计算输出得到控制量；PID控制器采用抗饱和积分和 不完全微分结构，传递函数为：
[0012]
[0013] 其中Kp为比例增益，K1为积分增益，Kd为微分增益，Tf为微分时间。
[0014] 所述的调整盲人行走方向是利用控制量的极性和大小来诱导盲人转向。控制量为 正和为负时分别诱导盲人向左和向右转向，控制量越大则盲人转向的角度也越大，盲人的 运动模型可W用下式来描述：
[0015]
[0016] 该式中J表示盲人偏航轴的转动惯量，M表示盲人转向时施加于自身的动力矩，Md 表示盲人偏航自由度上的阻尼力矩，P表示盲人行走方向角度，Ki表示盲人转向时施加于 自身的动力矩与控制量之间的比例系数，K2表示阻尼系数，U。表示PID控制器输出的控制 量。
[0017] 本发明与现有技术相比的优点在于：
[0018] (1)与现有的开环控制导盲系统相比，本发明能够根据盲人当前位置和行走状 况，作出实时的决策和控制，引导盲人按照给定路径行走，实现了有效的人机交互和闭环控 制；
[0019] (2)与现有的闭环控制导盲系统相比，本发明能够对盲人的行走方向角进行连续 检测和控制，实现对盲人行走的闭环连续诱导，提高盲人行走趋于给定路径的准确性，有利 于提高导盲算法的安全性和可靠性，更适于推广应用。
【附图说明】
[0020] 图1为本发明的总流程图；
[0021] 图2为本发明的预瞄点捜索示意图；
【具体实施方式】
[0022] 下面结合说明书附图对本发明中的各个部分做详细说明：
[0023] 本发明一种基于预瞄跟踪的盲人行走闭环诱导控制方法的总体流程如图1所示， 具体包括：在盲人行走过程中不断获取盲人当前位置和行走方向角，按预瞄半径在给定路 径中捜索预瞄点作为跟踪目标，并W当前位置到预瞄点的连线作为预瞄方向，计算盲人行 走方向角关于预瞄方向角的偏差，根据控制律计算和输出行走方向角控制量，控制盲人调 整行走方向，实现盲人行走趋于给定路径的诱导控制。
[0024] 本发明的给定路径是已知的，该路径来源于实时检测、规划或预先设置的路径，可 W采用轨迹方程的形式进行描述，也可W采用一系列有序的点进行描述。
[00巧]本发明的预瞄点捜索示意图如图2所示，其中M点为盲人的当前位置，N点为预瞄 点，给定路径为一条任意曲线，预瞄点捜索的步骤如下：
[00%] 步骤1:通过相关的环境感知和状态检测模块，获得盲人当前位置M点在地面坐标 系X0Y下的坐标（X。，y。），W及当前运动状态，即速度矢量V的大小和在地面坐标系X0Y下 的角度0;
[0027] 步骤2 :计算预瞄半径L L是关于速度的函数，函数关系式如下：
[0028]
[0029]该式中，Lm。济Lmm分别是预瞄半径的上限和下限距离；Vm。、和Vmi。分别是普通人在 正常情况下行走的最大速度和最小速度；A为预瞄半径与速度的比例系数。
[0030] 步骤3 当前位置M点为圆心预瞄半径L为半径，在当前行走方向±90。的范 围内作圆弧，弧线与期望轨迹的交点N即为预瞄点，其坐标记为（X，y)。
[0031] 本发明的控制律采用PID控制，被控量为预瞄方向角与盲人行走方向角的偏差。 详细步骤如下：
[0032] 步骤1 :求预瞄方向角，即由当前位置指向预瞄点的矢量在地面坐标系X0Y下的角 度值a:
[0033]
[0034] 步骤2:将预瞄方向角减去盲人行走方向角，得到角度偏差0:
[0035] 0 =曰-P
[0036] 步骤3 :将该偏差输入至PID控制器，计算得到控制量。PID控制器采用的是抗饱 和积分和不完全微分结构，其传递函数为：
[0037]
[0038]其中Kp为比例增益，K1为积分增益，Kd为微分增益，Tf为微分时间。
[0039] 本发明利用控制量的极性和大小来诱导盲人转向。控制量为正和为负时分别诱导 盲人向左和向右转向，控制量越大则盲人转向的角度也越大，盲人的运动模型可W用下式 来描述：
[0040]
[0041] 该式中J表示盲人偏航轴的转动惯量，M表示盲人转向时施加于自身的动力矩，Md表示盲人偏航自由度上的阻尼力矩，P表示盲人行走方向角，Ki表示盲人转向时施加于自 身的动力矩与控制量之间的比例系数，K2表示阻尼系数，U。表示PID控制器输出的控制量。
【主权项】
1. 一种基于预瞄跟踪的盲人行走闭环诱导控制方法，在盲人行走过程中不断获取盲人 当前位置和行走方向角，按预瞄半径在给定路径中搜索预瞄点作为跟踪目标，并以当前位 置到预瞄点的连线作为预瞄方向，计算盲人行走方向角关于预瞄方向角的偏差，根据控制 律计算和输出行走方向角控制量，控制盲人调整行走方向，实现盲人行走趋于给定路径的 诱导控制。2. 根据权利要求1所述的一种基于预瞄跟踪的盲人行走闭环诱导控制方法，其特征在 于：所述的给定路径是已知的，该路径来源于实时检测、规划或预先设置的路径，可以采用 轨迹方程的形式进行描述，也可以采用一系列有序的点进行描述。3. 根据权利要求1所述的一种基于预瞄跟踪的盲人行走闭环诱导控制方法，其特征在 于：所述的预瞄点搜索首先获得盲人当前位置和行走方向角，再以当前位置为圆心，以预瞄 半径L为半径，在当前行走方向±90°的范围内的给定路径上搜索预瞄点，其中预瞄半径L 是关于速度的函数，其关系如下：该式中，LniaJPLnun分别是预瞄半径的上限和下限距离；7_和￥_分别是普通人在正常 情况下行走的最大速度和最小速度；λ为行走速度与预瞄半径的比例系数。4. 根据权利要求1所述的一种基于预瞄跟踪的盲人行走闭环诱导控制方法，其特征在 于：所述的控制律为PID控制，被控量为预瞄方向角与盲人行走方向角的偏差。预瞄方向角 是盲人当前位置指向预瞄点的矢量在地面坐标系下的方向角，盲人行走方向角指的是盲人 当前行走速度矢量在地面坐标系下的方向角。将预瞄方向角减去盲人行走方向角得到角度 偏差，该偏差输入至PID控制器计算输出得到控制量；PID控制器采用抗饱和积分和不完全 微分结构，传递函数为：其中Kp为比例增益，K i为积分增益，K d为微分增益，T f为微分时间。5. 根据权利要求1所述的一种基于预瞄跟踪的盲人行走闭环诱导控制方法，其特征在 于：所述的调整盲人行走方向是利用控制量的极性和大小来诱导盲人转向。控制量为正和 为负时分别诱导盲人向左和向右转向，控制量越大则盲人转向的角度也越大，盲人的运动 模型可以用下式来描述：该式中J表不盲人偏航轴的转动惯量，M表不盲人转向时施加于自身的动力矩，]\^表不 盲人偏航自由度上的阻尼力矩，β表示盲人行走方向角，1表示盲人转向时施加于自身的 动力矩与控制量之间的比例系数，1(2表示阻尼系数，u。表示PID控制器输出的控制量。
【专利摘要】一种基于预瞄跟踪的盲人行走闭环诱导控制方法，在盲人行走过程中不断获取盲人当前位置和行走方向角，按预瞄半径在给定路径中搜索预瞄点作为跟踪目标，并以当前位置到预瞄点的连线作为预瞄方向，计算盲人行走方向角关于预瞄方向角的偏差，根据控制律计算和输出行走方向角控制量，控制盲人调整行走方向，实现盲人行走趋于给定路径的诱导控制。本发明引入预瞄跟踪机制，对盲人的行走方向角进行连续检测和控制，实现对盲人行走的闭环连续诱导，提高盲人行走趋于给定路径的准确性，有利于提高导盲算法的安全性和可靠性，更适于推广应用。
【IPC分类】A61H3/06
【公开号】CN105147505
【申请号】CN201510571640
【发明人】魏彤, 张琳, 封洁轩, 贾文渊, 袁磊, 金砺耀
【申请人】北京航空航天大学
【公开日】2015年12月16日
【申请日】2015年9月9日