[发明专利]一种基于预瞄跟踪的盲人行走闭环诱导控制方法

摘要

一种基于预瞄跟踪的盲人行走闭环诱导控制方法，在盲人行走过程中不断获取盲人当前位置和行走方向角，按预瞄半径在给定路径中搜索预瞄点作为跟踪目标，并以当前位置到预瞄点的连线作为预瞄方向，计算盲人行走方向角关于预瞄方向角的偏差，根据控制律计算和输出行走方向角控制量，控制盲人调整行走方向，实现盲人行走趋于给定路径的诱导控制。本发明引入预瞄跟踪机制，对盲人的行走方向角进行连续检测和控制，实现对盲人行走的闭环连续诱导，提高盲人行走趋于给定路径的准确性，有利于提高导盲算法的安全性和可靠性，更适于推广应用。

主权项

一种基于预瞄跟踪的盲人行走闭环诱导控制方法，在盲人行走过程中不断获取盲人当前位置和行走方向角，按预瞄半径在给定路径中搜索预瞄点作为跟踪目标，并以当前位置到预瞄点的连线作为预瞄方向，计算盲人行走方向角关于预瞄方向角的偏差，根据控制律计算和输出行走方向角控制量，控制盲人调整行走方向，实现盲人行走趋于给定路径的诱导控制；所述的预瞄点搜索首先获得盲人当前位置和行走方向角，再以当前位置为圆心，以预瞄半径L为半径，在当前行走方向±90°的范围内的给定路径上搜索预瞄点，其中预瞄半径L是关于速度的函数，其关系如下：L=Lmin,v<vminλv,vmin<v<vmaxLmax,v>vmax]]>该式中，Lmax和Lmin分别是预瞄半径的上限和下限距离；vmax和vmin分别是普通人在正常情况下行走的最大速度和最小速度；λ为行走速度与预瞄半径的比例系数；所述的控制律为PID控制，被控量为预瞄方向角与盲人行走方向角的偏差，预瞄方向角是盲人当前位置指向预瞄点的矢量在地面坐标系下的方向角，盲人行走方向角指的是盲人当前行走速度矢量在地面坐标系下的方向角，将预瞄方向角减去盲人行走方向角得到角度偏差，该偏差输入至PID控制器计算输出得到控制量；PID控制器采用抗饱和积分和不完全微分结构，传递函数为：Gc(s)=Kp+Ki·1s+1+Kd·Tf·sTf·s+1]]>其中Kp为比例增益，Ki为积分增益，Kd为微分增益，Tf为微分时间；所述的调整盲人行走方向是利用控制量的极性和大小来诱导盲人转向，控制量为正和为负时分别诱导盲人向左和向右转向，控制量越大则盲人转向的角度也越大，盲人的运动模型可以用下式来描述：该式中J表示盲人偏航轴的转动惯量，M表示盲人转向时施加于自身的动力矩，Md表示盲人偏航自由度上的阻尼力矩，β表示盲人行走方向角，K1表示盲人转向时施加于自身的动力矩与控制量之间的比例系数，K2表示阻尼系数，uc表示PID控制器输出的控制量。