本发明涉及一种基于连续帧的导盲仪组合路径规划方法，在双目相机拍摄的每一帧通过对全局路径的预瞄跟踪来确定局部规划目标点，根据相机实时检测到的环境信息进行局部路径规划，输出行走方向误差角来诱导盲人行走，直到达到全局目标点。在局部路径规划中，首先对当前帧的环境信息进行栅格可视化，然后使用A*算法进行局部路径规划，最后采用二次加权的路径转换方法计算当前帧行走方向与局部规划路径的角度偏差，作为诱导盲人行走控制器的输入信息。本发明设计了一种新型的导盲仪组合路径规划方法，能够实现实时规划路径，为引导盲人在躲避障碍物的前提下、跟随盲道行走、最终到达目的地提供必要基础。
1.一种基于连续帧的导盲仪组合路径规划方法，其特征在于：步骤如下：
(1)根据穿戴式视觉导盲系统的避障要求，在全局目标点位置确定而环境信息未知的情况下，采用一种全局路径规划与局部路径规划相结合的组合路径规划方法；
(2)在步骤(1)全局目标点的基础上，首先进行全局路径规划：全局路径由预存的GIS地图查找获得，随后在双目相机拍摄的当前帧时刻，计算当前点的位置，并从当前点出发，通过对全局路径进行预瞄跟踪来确定局部规划目标点；
(3)在步骤(2)局部规划目标点的基础上，进行基于A*算法的局部路径规划：在局部路径规划前，首先对获取的当前帧的环境信息进行栅格可视化处理，即对栅格图中的所有栅格单元进行信息存储和栅格单元间的连线代价标识，随后使用A*算法进行局部路径规划，得到局部规划路径；
(4)在步骤(3)局部规划路径的基础上，采用二次加权的路径转换方法计算当前帧的行走方向与局部规划路径的角度偏差，即行走方向误差角，作为诱导盲人行走的控制器的输入信息，直到达到全局目标点；
所述步骤(3)中，对当前帧的环境信息进行栅格可视化处理具体实现为：
(31)将当前帧的环境信息映射到行数为M，列数为N的栅格图中；栅格图中的任意一个栅格称作栅格单元，栅格单元采用直角坐标系法和序号法相结合的栅格标识方式；栅格单元的信息存储，指对其所在的直角坐标、序号和环境信息进行记录与标识；
(32)直角坐标系法通过在当前帧的栅格图中，定义水平方向向右为x轴正方向，竖直方向向上为y轴正方向的栅格直角坐标系xOy，任意的栅格单元用直角坐标(x,y)唯一标识；序号法主要按照逐列、逐行递增的顺序给每个栅格单元标识唯一的序号n；将直角坐标法与序号法标识的栅格单元建立一一对应的关系，如下：n＝y*N+x；
(33)每个栅格单元对应唯一的环境信息，在环境信息的存储过程中，按照环境信息的不同，将栅格单元分为普通点、障碍物点、安全距离点、盲道中心线点和盲道扩展线点：安全距离点是距离障碍物点一定安全距离的栅格单元，目的是提高避障的安全性；盲道中心线点为盲道中心线的空间方程投影在栅格图上的栅格单元，向盲道中心线点两侧扩展一层栅格单元设置为盲道扩展线点，目的是更加真实地反映盲道的实际宽度；
(34)栅格单元间的连线代价标识方法遵循最优优先的路径搜索原则，根据在躲避障碍物的前提下、优先跟随盲道的要求设置不同元素栅格点间的连线代价优先级，计算时首先判断当前点n与相邻点m各自所属的元素种类，确定两者连线的代价优先级Lnm；
式中：S为初始代价基准，r为相邻优先级间的代价差，两者均为正常数。
2.根据权利要求1所述的基于连续帧的导盲仪组合路径规划方法，其特征在于：所述步骤(2)中，全局路径由预存的GIS地图查找获得，并分段存储为空间方程形式，全局目标点以空间位置坐标的形式进行描述；当前点的位置和行走方向来源于GPS/IMU组合导航装置的实时检测与定位，分别存储为空间位置坐标和地理方向形式；当前帧获取的环境信息主要包括障碍物和盲道的位置信息，其获取过程为：首先由双目相机采集环境图像，随后经过图像处理算法计算，最终得到障碍物的空间位置坐标和盲道中心线的空间位置方程。
3.根据权利要求1所述的基于连续帧的导盲仪组合路径规划方法，其特征在于：所述步骤(2)中，通过对全局路径的预瞄跟踪来确定局部规划目标点的过程为：首先获得盲人的当前点位置和行走方向，再以当前位置为圆心、R为预瞄半径作圆弧，弧线与全局路径的交点即为预瞄点，坐标记为(x',y')，设全局路径方程为y＝f(x)，计算公式如下：联立(1)式中的两个等式方程组，求得该方程组的解即为预瞄点坐标，随后，将求得的预瞄点与当前点进行连线，取连线与当前帧栅格图边沿的栅格交点为局部规划目标点。
4.根据权利要求1所述的基于连续帧的导盲仪组合路径规划方法，其特征在于：所述步骤(3)中，A*算法过程为：从当前点出发，使用代价函数f(n)＝g(n)+h(n)对将要搜索的8邻域栅格点进行评估，其中f(n)表示当前节点n的总代价值，g(n)表示从起始点到节点n的实际代价值，h(n)表示从节点n到目标点的估计代价值，并采用切比雪夫距离计算；选择f(n)值最小的栅格点为当前最优路径点，从这个栅格点继续搜索，直到到达局部目标点。
5.根据权利要求1所述的基于连续帧的导盲仪组合路径规划方法，其特征在于：所述步骤(4)中，基于二次加权的路径转换方法过程为：通过对局部路径规划得到的路径点集进行两次加权得到转换与滤波的效果，最终以当前帧行进方向与局部规划路径的角度偏差作为行走方向误差角反馈给盲人，来诱导其避障行走；第一次加权平均通过计算当前帧栅格图中的有效路径点集所在位置相对于盲人当前行进方向的误差角θ来拟合行走方向误差角，按照从当前点到局部目标点的寻路顺序，对各路径点对应的误差角赋予递减的权值，计算公式为：θ(lT)＝θ1(lT)f1+θ2(lT)f2+...+θk(lT)fk
式中：θ1(lT),...,θk(lT)为当前帧时刻lT，T为局部路径规划的时间周期，l为当前时刻的序号，该序号以起始时刻为0依次递增1，所得规划路径上的第k个点相对于起始点的偏移角度，k为局部路径点由起点至终点依次排序的编号，f1,...,fk为对应偏移角度的权值，满足f1＞f2＞...＞fk＞0且f1+f2+...+fk＝1；第二次加权平均指通过将上述计算得出的θ(lT)与前几帧行走方向误差角进行融合计算，得出二次加权后的角度，也即当前帧最终的行走方向误差角 计算公式为：式中： 为时刻[(l-τ)T]的行走方向误差角，其中，τ为正整数，l-τ表示l时刻前、并与l相差τ个周期的时刻序号，fl,...,fl-τ为对应角度的权值，满足fl＞fl-1＞...＞fl-τ且fl+fl-1+...+fl-τ＝1。
6.根据权利要求1所述的基于连续帧的导盲仪组合路径规划方法，其特征在于：所述步骤(4)中，控制器是利用控制量，即行走方向误差角的正负极性和大小来控制盲人的行走方向，控制量为正和为负时分别诱导盲人向左和向右转向，控制量越大，盲人转向的角度也越大，从而引导盲人朝着全局目标点行走。
一种基于连续帧的导盲仪组合路径规划方法
技术领域
[0001] 本发明涉及一种基于连续帧的导盲仪组合路径规划方法，用于导盲设备，为引导盲人在躲避障碍物的前提下、跟随盲道行走、并最终到达目的地提供必要基础，尤其适用于未知环境下的实时导盲任务。
背景技术
[0002] 盲人由于先天或后天的生理原因而丧失了视觉，大部分盲人群体期望智能仪器能够帮助他们解决出行困难问题。近年来，国内外研制出的导盲仪种类繁多，如导盲机器人、智能手杖、穿戴式导盲设备等，其中穿戴式导盲设备凭借其更直观的感知方式、更方便的携带方式以及更好的用户体验已经成为现今导盲仪研究的主流方向。
[0003] 在导盲仪的设计过程中，除了解决环境元素的探测与定位导航问题，对于如何在存在障碍物区域内找出一条安全快捷道路的研究也十分必要。现有的大多数导盲仪只具备单一的避障能力，不具备完整的路径规划功能，导盲仪的路径规划算法是当今导盲仪研发的重要技术发展方向。
[0004] 路径规划问题的研究方法主要分为全局方法、局部方法以及组合方法三种。全局规划方法依照己获取的环境信息规划出一条路径，规划路径的精确程度取决于获取环境信息的准确程度，并且计算量较大，如遗传算法、粒子群算法等。局部规划方法仅对当前获取的局部环境信息进行规划，和全局规划方法相比，局部规划方法更具有实时性和实用性，但缺陷是在某些情况下会产生局部极点、不能顺利到达目的地，如人工势场法等，且现有的局部路径规划方法过于单一，不适用于盲人在复杂环境中行走。在实际应用中，导盲仪需要引导盲人沿着可行区域(盲道、无障碍道路等)行走一段较长距离、并到达目的地，故单独的全局方法和局部方法并不适用于导盲仪路径规划。组合路径规划方法结合了全局和局部方法的优点，能够在进行实时的局部规划的同时，到达全局目的地。然而，大多数组合路径规划算法直接将全局规划得到的粗略路径点作为局部规划的目标点，导致引导盲人的行走轨迹过于生硬，异于正常人行走规则，不符合人体工程学；另外，针对导盲仪的不同导盲需求，尚未有功能较为完整的导盲仪路径规划系统，故上述方法均并不适用于导盲仪路径规划。
发明内容
[0005] 本发明的技术解决问题：克服现有方法在未知环境下的导盲仪实时路径规划中不适用的缺陷，提供一种基于连续帧的导盲仪组合路径规划方法，能够实现引导盲人在躲避障碍物的前提下、尽量跟随盲道、并最终到达目的地的导盲仪路径规划任务，提高了导盲算法的适用性。
[0006] 本发明的技术解决方案是：所述的一种基于连续帧的导盲仪组合路径规划方法，其特征在于步骤如下：
[0007] (1)根据穿戴式视觉导盲系统的避障要求，在全局目标点位置确定而环境信息未知的情况下，采用一种全局路径规划与局部路径规划相结合的组合路径规划方法；
[0008] (2)在步骤(1)全局目标点的基础上，首先进行全局路径规划：全局路径由预存的GIS地图查找获得，随后在双目相机拍摄的当前帧时刻，计算当前点的位置，并从当前点出发，通过对全局路径进行预瞄跟踪来确定局部规划目标点；
[0009] (3)在步骤(2)局部规划目标点的基础上，进行基于A*算法的局部路径规划：在局部路径规划前，首先对获取的当前帧的环境信息进行栅格可视化处理，即对栅格图中的所有栅格单元进行信息存储和栅格单元间的连线代价标识，随后使用A*算法进行局部路径规划，得到局部规则路径；
[0010] (4)在步骤(3)局部规划路径的基础上，采用二次加权的路径转换方法计算当前帧的行走方向与局部规划路径的角度偏差，即行走方向误差角，作为诱导盲人行走的控制器的输入信息，直到达到全局目标点。
[0011] 所述的基于连续帧的导盲仪组合路径规划方法，其特征在于：所述的全局路径由预存的GIS地图查找获得，并分段存储为空间方程形式，全局目标点以空间位置坐标的形式进行描述；当前点的位置和行走方向来源于GPS/IMU组合导航装置的实时检测与定位，分别存储为空间位置坐标和地理方向形式；当前帧获取的环境信息主要包括障碍物和盲道的位置信息，其获取过程为：首先由双目相机采集环境图像，随后经过图像处理算法计算，最终得到障碍物的空间位置坐标和盲道中心线的空间位置方程。
[0012] 所述步骤(2)中，通过对全局路径的预瞄跟踪来确定局部规划目标点的过程为：首先获得盲人的当前点位置和行走方向，再以当前位置为圆心、R为预瞄半径作圆弧，弧线与全局路径的交点即为预瞄点，坐标记为(x',y')，设全局路径方程为y＝f(x))，计算公式如下：
[0013]
[0014] 联立(1)式中的两个等式方程组，求得该方程组的解即为预瞄点坐标，随后，将求得的预瞄点与当前点进行连线，取连线与当前帧栅格图边沿的栅格交点为局部规划目标点。
[0015] 所述步骤(3)中，对当前帧的环境信息进行栅格可视化处理，实现为：
[0016] (31)将当前帧的环境信息映射到行数为M，列数为N的栅格图中；栅格图中的任意一个栅格称作栅格单元，栅格单元采用直角坐标系法和序号法相结合的栅格标识方式；栅格单元的信息存储，指对其所在的直角坐标、序号和环境信息进行记录与标识；
[0017] (32)直角坐标法通过在当前帧的栅格图中，定义水平方向向右为x轴正方向，竖直方向向上为y轴正方向的栅格直角坐标系xOy，任意的栅格单元用直角坐标(x,y)唯一标识；序号法主要按照逐列、逐行递增的顺序给每个栅格单元标识唯一的序号n；将直角坐标法与序号法标识的栅格单元建立一一对应的关系，如下：
[0018] n＝y*N+x；
[0019] (33)每个栅格单元对应唯一的环境信息，在环境信息的存储过程中，按照环境信息的不同，将栅格单元分为普通点、障碍物点、安全距离点、盲道中心线点和盲道扩展线点：安全距离点是距离障碍物点一定安全距离的栅格单元，目的是提高避障的安全性；盲道中心线点为盲道中心线的空间方程投影在栅格图上的栅格单元，向盲道中心线点两侧扩展一层栅格单元设置为盲道扩展线点，目的是更加真实地反映盲道的实际宽度；
[0020] (34)栅格单元间的连线代价标识方法遵循最优优先的路径搜索原则，根据在躲避障碍物的前提下、优先跟随盲道的要求设置不同元素栅格点间的连线代价优先级，计算时首先判断当前点n与相邻点m各自所属的元素种类，确定两者连线的代价优先级Lnm；
[0021] (35)根据Lnm，计算连线代价Cnm为：
[0022] Cnm＝S+Lnm×r
[0023] 式中：S为初始代价基准，r为相邻优先级间的代价差，两者均为正常数。
[0024] 所述步骤(3)中，A*算法过程为：从当前点出发，使用代价函数f(n)＝g(n)+h(n)对将要搜索的8邻域栅格点进行评估，其中f(n)表示当前节点n的总代价值，g(n)表示从起始点到节点n的实际代价值，h(n)表示从节点n到目标点的估计代价值，并采用切比雪夫距离计算；选择f(n)值最小的栅格点为当前最优路径点，从这个栅格点继续搜索，直到到达局部目标点。
[0025] 所述步骤(4)中，基于二次加权的路径转换方法过程为：通过对局部路径规划得到的路径点集进行两次加权得到转换与滤波的效果，最终以当前帧行进方向与局部规划路径的角度偏差作为行走方向误差角反馈给盲人，来诱导其避障行走；第一次加权平均通过计算当前帧栅格图中的有效路径点集所在位置相对于盲人当前行进方向的误差角θ来拟合行走方向误差角，按照从当前点到局部目标点的寻路顺序，对各路径点对应的误差角赋予递减的权值，计算公式为：
[0026] θ(lT)＝θ1(lT)f1+θ2(lT)f2+...+θk(lT)fk
[0027] 式中：θ1(lT),...,θk(lT)为当前帧时刻lT(T为局部路径规划的时间周期，l为当前时刻的序号，该序号以起始时刻为0依次递增1)，所得规划路径上的第k(k为局部路径点由起点至终点依次排序的编号)个点相对于起始点的偏移角度，f1,...,fk为对应偏移角度的权值，满足f1＞f2＞...＞fk＞0且f1+f2+...+fk＝1；第二次加权平均指通过将上述计算得出的θ(lT)与前几帧行走方向误差角进行融合计算，得出二次加权后的角度，也即当前帧最终的行走方向误差角 计算公式为：
[0028]
[0029] 式中： (τ为正整数，l-τ表示l时刻前、并与l相差τ个周期的时刻序号)为时刻[(l-τ)T]的行走方向误差角，fl,...,fl-τ为对应角度的权值，满足fl＞fl-1＞...＞fl-τ且fl+fl-1+...+fl-τ＝1。
[0030] 所述步骤(4)中，控制器是利用控制量(即行走方向误差角)的正负极性和大小来控制盲人的行走方向，控制量为正和为负时分别诱导盲人向左和向右转向，控制量越大，盲人转向的角度也越大，从而引导盲人朝着全局目标点行走。
[0031] 本发明与现有技术相比的优点在于：
[0032] (1)与现有的已知环境下的全局路径规划算法相比，本发明能够根据盲人当前位置和行走状况做出实时的决策和控制，在诱导盲人趋于目的地行走的同时，能够安全、有效地避障，并且无冗余信息存储等问题，提高了导盲算法的适用性。
[0033] (2)与现有的未知环境下的局部路径规划算法相比，本发明在计算局部路径的过程中综合考虑多种环境信息，能够实现引导盲人在躲避障碍物的前提下、尽量跟随盲道、最终到达目的地的多目标导盲任务。
[0034] (3)与现有的组合路径规划算法相比，本发明构建了较完整的导盲仪路径规划系统，整体路径规划方法合理、可行且高效，另外，在实现导盲仪需求的同时，能够引导盲人以较为自然的行走轨迹行走。
附图说明
[0035] 图1为本发明的总体流程图；
[0036] 图2为本发明的全局路径的预瞄跟踪示意图；
[0037] 图3为本发明的栅格可视化示意图；
[0038] 图4为本发明的基于A*的局部路径规划算法流程图；
[0039] 图5为本发明的基于二次加权的路径转换示意图。
具体实施方式
[0040] 如图1所示，本发明一种基于连续帧的导盲仪组合路径规划方法的总体流程具体包括如下步骤：
[0041] (1)根据穿戴式视觉导盲系统的避障要求，在全局目标点位置确定而环境信息未知的情况下，采用一种全局路径规划与局部路径规划相结合的组合路径规划方法；
[0042] (2)在步骤(1)全局目标点的基础上，首先进行全局路径规划：全局路径由预存的GIS地图查找获得，随后在双目相机拍摄的当前帧时刻，计算当前点的位置，并从当前点出发，通过对全局路径进行预瞄跟踪来确定局部规划目标点；
[0043] (3)在步骤(2)局部规划目标点的基础上，进行基于A*算法的局部路径规划：在局部路径规划前，首先对获取的当前帧的环境信息进行栅格可视化处理，即对栅格图中的所有栅格单元进行信息存储和栅格单元间的连线代价标识，随后使用A*算法进行局部路径规划，得到局部规则路径；
[0044] (4)在步骤(3)局部规划路径的基础上，采用二次加权的路径转换方法计算当前帧的行走方向与局部规划路径的角度偏差，即行走方向误差角，作为诱导盲人行走的控制器的输入信息，直到达到全局目标点。
[0045] 所述的全局路径由预存的GIS地图查找获得，并分段存储为空间方程形式，全局目标点以空间位置坐标的形式进行描述；当前点的位置和行走方向来源于GPS/IMU组合导航装置的实时检测与定位，分别存储为空间位置坐标和地理方向形式；当前帧获取的环境信息主要包括障碍物和盲道的位置信息，其获取过程为：首先由双目相机采集环境图像，随后经过图像处理算法计算，最终得到障碍物的空间位置坐标和盲道中心线的空间位置方程。
[0046] 下面结合说明书附图对本发明中的各个部分做详细说明：
[0047] 本发明的全局路径的预瞄跟踪示意图如图2所示，设定全局坐标系为XOY，导盲任务的起点和终点分别为Ps和Pg，所述步骤(2)中，通过对全局路径的预瞄跟踪来确定局部规划目标点的过程为：首先获得盲人的当前位置点和行走方向，盲人当前位置点Pb在XOY下的坐标为(x,y)，行走方向v与栅格图直角坐标系的y轴正方向一致；以当前位置Pb点为圆心、R为预瞄半径作圆弧，弧线与全局路径(设全局路径方程为y＝f(x))的交点即为预瞄点，其坐标记为(x',y')，计算过程如下：
[0048]
[0049] 联立(1)式中的两个等式方程组，求得该方程组的解即为预瞄点坐标(x',y')；局部窗口坐标系(栅格直角坐标系)为xOy，将求得的预瞄点与当前点进行连线，将连线PbPf称为预瞄方向，计算盲人行走方向v关于预瞄方向的偏差β，取连线PbPf与当前帧栅格图边沿的交点Psub为局部规划目标点。
[0050] 本发明的栅格可视化示意图如图3所示，步骤(3)中，对当前帧的环境信息进行栅格可视化处理，实现为：
[0051] (31)将当前帧的环境信息映射到行数为M，列数为N的栅格图中，本发明中栅格图的行数设为M＝21，列数设为N＝21，各个栅格边长代表实际场景距离为0.19m，即栅格图映射双目相机前方4×4m2的视野区域，默认当前点为视野底边中点，且当前行走方向与底边保持垂直；栅格图中的任意一个栅格称作栅格单元，栅格单元采用直角坐标系法和序号法相结合的栅格标识方式；栅格单元的信息存储，指对其所在的直角坐标、序号和环境信息进行记录与标识；
[0052] (32)直角坐标法通过在当前帧的栅格图中，定义水平方向向右为x轴正方向，竖直方向向上为y轴正方向的栅格直角坐标系xOy，任意的栅格单元用直角坐标(x,y)唯一标识；序号法主要按照逐列、逐行递增的顺序给每个栅格单元标识唯一的序号n；将直角坐标法与序号法标识的栅格单元建立一一对应的关系，如下：
[0053] n＝y*N+x；
[0054] (33)每个栅格单元对应唯一的环境信息，在环境信息的存储过程中，按照环境信息的不同，将栅格单元分为普通点、障碍物点、安全距离点、盲道中心线点和盲道扩展线点：安全距离点是距离障碍物点一定安全距离的栅格单元，目的是提高避障的安全性；盲道中心线点为盲道中心线的空间方程投影在栅格图上的栅格单元，向盲道中心线点两侧扩展一层栅格单元设置为盲道扩展线点，目的是更加真实地反映盲道的实际宽度；
[0055] (34)栅格单元间的连线代价标识方法遵循最优优先的路径搜索原则，根据在躲避障碍物的前提下、优先跟随盲道的要求设置不同元素栅格点间的连线代价优先级，在赋予障碍物点与其8邻域点的连线代价为无穷后，其他元素点与其相邻点的连线代价远小于无穷，并依据下表1和表2中所示元素连线及其对应的代价优先级进行设置：首先，由表1判断当前点n与相邻点m各自所属的元素查找对应的连线序号(①—⑨)(表1中“—”代表“重复/无”)；随后，根据表2的连线序号和连线属性(直线/对角线)来确定两者连线的代价优先级Lnm(Lnm范围为1-11)；
[0056] 表1不同元素连线序号
[0057]
[0058] 表2不同种类连线的代价优先级设置
[0059]
[0060] (35)根据Lnm，计算连线代价Cnm为：
[0061] Cnm＝S+Lnm×r
[0062] 式中：S为初始代价基准，r为相邻优先级间的代价差，两者均为正常数；本发明设置S＝1.4，r＝0.5。
[0063] 本发明的基于A*的局部路径规划算法流程图如图4所示，所述步骤(3)中，A*算法过程为：从当前点出发，使用代价函数f(n)＝g(n)+h(n)对将要搜索的8邻域栅格点进行评估，其中f(n)表示当前节点n的总代价值，g(n)表示从起始点到节点n的实际代价值，h(n)表示从节点n到目标点的估计代价值，并采用切比雪夫距离计算；选择f(n)值最小的栅格点为当前最优路径点，从这个栅格点继续搜索，直到到达局部目标点。具体步骤详细说明如下：
[0064] 步骤1：首先建立2个表格，OPEN表和CLOSED表，OPEN表内存放已生成而未考察的节点，CLOSED表中记录已访问过的节点。执行步骤2。
[0065] 步骤2：将初始点s加入到OPEN表中，并初始化其估价值fs(s)＝h(s)；CLOSED为空表。执行步骤3。
[0066] 步骤3：选取OPEN表中具有最小估价值的节点v，对v进行考察：如果是目标点，则成功求得目标解，直接转入步骤7；如果不是，则扩展v的8邻域节点，并剔除已在CLOSE表中的节点，生成下一步待考察节点k，进入步骤4计算得到fk(k)。执行步骤5。
[0067] 步骤4：计算代价函数fk(k)＝g(k)+h(k)：若g(v)+Wvk＜g(k)，修改g(k)＝g(v)+Wvk，并更新fk(k)。
[0068] 步骤5：对k依次进行下列判断与操作过程：若k∈OPEN，且fk(k)小于OPEN表中的估价值，则设置k的父辈节点为v，并更新OPEN表中的最小估价值；如果 将v设置为k的父辈节点，进入步骤4计算得到fk(k)，将k放入OPEN表中。执行步骤6。
[0069] 步骤6：将v节点从OPEN表中删除并放入CLOSED表中，并按照估价值将OPEN表中的节点排序。返回步骤3，循环计算。
[0070] 步骤7：终止循环，输出路径点集。
[0071] 本发明的基于二次加权的路径转换示意图如图5所示，所述步骤(4)中，基于二次加权的路径转换方法过程为：通过对局部路径规划得到的路径点集进行两次加权得到转换与滤波的效果，最终以当前帧行进方向与局部规划路径的角度偏差作为行走方向误差角反馈给盲人，来诱导其避障行走；第一次加权平均通过计算当前帧栅格图中的有效路径点集所在位置相对于盲人当前行进方向的误差角θ来拟合行走方向误差角，按照从当前点到局部目标点的寻路顺序，对各路径点对应的误差角赋予递减的权值，计算公式为：
[0072] θ(lT)＝θ1(lT)f1+θ2(lT)f2+...+θk(lT)fk
[0073] 式中：θ1(lT),...,θk(lT)为当前帧时刻lT(T为局部路径规划的时间周期，l为当前时刻的序号，该序号以起始时刻为0依次递增1)，所得规划路径上的第k(k为局部路径点由起点至终点依次排序的编号)个点相对于起始点的偏移角度，f1,...,fk为对应偏移角度的权值，满足f1＞f2＞...＞fk＞0且f1+f2+...+fk＝1；第二次加权平均指通过将上述计算得出的θ(lT)与前几帧行走方向误差角进行融合计算，得出二次加权后的角度，也即当前帧最终的行走方向误差角 计算公式为：
[0074]
[0075] 式中： (τ为正整数，l-τ表示l时刻前、并与l相差τ个周期的时刻序号)为时刻[(l-τ)T]的行走方向误差角，fl,...,fl-τ为对应角度的权值，满足fl＞fl-1＞...＞fl-τ且fl+fl-1+...+fl-τ＝1。
[0076] 步骤(4)中，控制器是利用控制量(即行走方向误差角)的正负极性和大小来控制盲人的行走方向，控制量为正和为负时分别诱导盲人向左和向右转向，控制量越大，盲人转向的角度也越大，从而引导盲人朝着全局目标点行走。
[0077] 提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。
