一种应用于为人导航系统的人机队形控制方法与流程

本发明涉及导盲助航设备控制技术领域，特别是涉及一种应用于为人导航系统的人机队形控制方法。

背景技术

随着移动机器人技术的发展，机器人的功能越来越多，应用也越来越广泛，使人们的生活更加的方便和智能。在为视力障碍者导航方面，诸如大型商场、图书馆、博物馆等室内环境下，移动机器人被用来引导视力障碍者到达目标位置。目前研制的基于移动机器人的为人导航系统，通常在移动机器人上安装弹性尼龙绳或者轻质刚性伸缩长柄，使用者通过感知尼龙绳或者长柄上的反馈力，获得左转、右转和加速前进等导航信息。使用这些为人导航系统容易导致使用者过分接近机器人，也可能过分偏右于或偏左于机器人，使用者与移动机器人之间没有固定的人机队形，导航性能不佳；使用者只能被动地跟随移动机器人，必须根据机器人的速度来调节自己的速度，降低了系统对使用者的友好交互性。

技术实现要素：

为了解决问题，本发明提供一种应用于为人导航系统的人机队形控制方法，充分考虑使用者在系统中的重要地位，根据使用者的线速度调节机器人的线速度和角速度，根据使用者与机器人之间的相对位置，指示使用者调整自己的位置，使得使用者与机器人之间保持一定的相对距离和相对角度，从而形成人机队形控制，达到准确为人导航的效果，为达此目的，本发明提供一种应用于为人导航系统的人机队形控制方法，所述的为人导航系统包括移动机器人和基于振动触觉反馈的便携式穿戴装置，移动机器人安装的传感器有360°激光扫描雷达和rgb-d摄像头，具体步骤如下：

(1)激光雷达扫描周围环境，将与周围物体之间的距离信息传送至上位机，上位机对距离信息进行处理后得到二维环境地图；

(2)上位机根据地图信息，利用navigation软件功能包实时进行最优路径规划，生成规划速度(vi,ωi)；

(3)rgb-d摄像头以一定的采样频率采集使用者的骨架信息并传送至上位机，上位机处理后得到使用者的位置(pm)和速度信息(vh)；

(4)人机队形控制算法融合使用者速度(vh)和规划速度(vi,ωi)，产生机器人实际控制速度(vr,ωr)，传送至下位机，控制机器人运动；

(5)上位机根据使用者位置信息(pm)，通过基于振动触觉反馈的便携式穿戴装置和语音提示装置，指示使用者调整自己的位置，使机器人和使用者之间保持一定的相对距离(sd)和相对角度(βd)，实现为人导航的功能。

本发明的进一步改进，步骤二中最优路径规划包括全局规划和局部规划，全局规划采用a*或者dijkstra法，l是全局规划算法规划出的起点到目标位置的最佳路径，由于全局规划，未考虑实时环境信息变化，随着机器人的前进，周围环境信息与绘制地图时会有所不同，可能会出现新的障碍物，所以要采用局部规划。局部规划采用动态窗口法，能规划出一定距离si内的规划速度(vi,ωi)，实时躲避静态障碍物和动态障碍物，si是光滑的曲线，是l在一定时间内的分割曲线，si曲线的曲率半径为αi:

要使机器人跟随规划的轨迹前进，每小段si的曲率半径必须准确地传送至下位机，并执行相应的运动控制。

本发明的进一步改进，步骤三中rgb-d摄像头以10hz的采样频率采集使用者的骨架信息并传送至上位机，rgb-d摄像头能够跟踪使用者身上15个骨骼点的信息，通过采集使用者左右肩的空间坐标p1(x1v,y1v,z1v)，p2(x2v,y2v,z2v)，使用视场坐标系与位姿估测坐标系的坐标转化公式和中点公式：

计算出双肩中点的位置坐标，用来代表使用者的位置pm(xm,ym,zm)，其中ym为固定值，因为双肩中点的运动轨迹受到行走引起的振动的影响要小于其他骨骼点，通过两次采集的位置信息和两次采集相隔时间0.1s，计算得到使用者的速度信息(vh)；

本发明的进一步改进，步骤四中由多机器人领域中的领导者—跟随者控制模型所知，双轮差速移动机器人是非完整约束移动机器人，其运动模型可以简化为：

关于距离srh和角度βrh的运动学方程：

其中，d是跟随者参考点的偏轴距离，srh和βrh是两机器人之间的实际相对距离和相对角度，ψrh＝θr+βrh-θh，为了防止机器人之间发生碰撞，必须保证srh足够大。

上述运动学方程化简为：

其中，输出变量uh＝[vh,ωh]^t指跟随者的运动线速度和角速度，输入变量ur＝[vr,ωr]^t指领导者的运动线速度和角速度，zrh＝[srh,βrh]^t；

其中d/srh>0；

考虑到为人导航与多机器人领域的编队控制有着相似的功能，即将目标物体引导到目标位置，所以将领导者-跟随者控制模块应用到为人导航系统的人机队形控制算法：

其中，

考虑局部路径规划中αi的作用，将机器人角速度设定为：

将机器人角速度公式代入人机队形控制算法化简得：

其中a,b,c,d>0；

从以上公式可得，当使用者的线速度vh增大时，机器人的线速度vr也会增大。同样，当使用者减速时，机器人的线速度vr也会减小，致使机器人与使用者之间保持适当的距离。人机队形控制算法融合使用者速度(vh)和规划速度(vi,ωi)，产生机器人实际控制速度(vr,ωr)，传送至下位机，控制机器人跟随规划轨迹前进，只要使用者调节运动的角速度(ωh)，就能使机器人和使用者之间保持一定的相对距离(sd)和相对角度(βd)，保持一定的人机队形；

本发明的进一步改进，步骤五中利用使用者与机器人的相对位置，通过基于振动触觉反馈的便携式穿戴装置和语音提示装置，指示使用者调整自己的位置，使机器人和使用者之间保持一定的相对距离(sd)和相对角度(βd)，实现为人导航的功能，通过以下公式实时更新使用者的相对位置pm(xm,ym,zm)：

t1，t2，t3分别代表振动电机1，2，3，振动电机的振动控制策略与使用者位置对应的关系如下所示：

语音提示的控制策略与使用者位置对应的关系：当使用者在frⅱ区域，提示为大步左后退；当使用者在frⅰ区域，提示为小步左后退；当使用者在rⅱ区域，提示为大步向左移；当使用者在rⅰ区域，提示为小步向左移；当使用者在brⅱ区域，提示为大步左前进；当使用者在brⅰ区域，提示为小步左前进；当使用者在f区域，提示为后退；当使用者在flⅱ区域，提示为大步右后退；当使用者在flⅰ区域，提示为小步右后退；当使用者在lⅱ区域，提示为大步向右移；当使用者在lⅰ区域，提示为小步向右移；当使用者在blⅱ区域，提示为大步右前进；当使用者在blⅰ区域，提示为小步右前进；当使用者在b区域，提示为前进。

本发明的进一步改进，所述激光雷达的测量范围为0.15～6m。

本发明的进一步改进，所述rgb-d摄像头水平视场角是57.5°，垂直视场角是43°。

本发明的进一步改进，所述控制方法的最佳交互距离为1.2m～3.5m区间。

本发明的进一步改进，所述基于振动触觉反馈的便携式穿戴装置由安装在无名指和食指部位的振动电机，手腕部位的振动电机和控制盒组成，所述振动电机是偏心旋转质量振动电机，所述控制盒由zigbee无线通讯模块，电机驱动模块和无线充电模块组成。

本发明提供了一种应用于为人导航系统的人机队形控制方法，有益效果如下：

本发明是一种基于小型移动机器人的为人导航系统的人机队形控制方法，以使用者为主导，使用者能自由地选择行走的速度，不必被动地跟随机器人移动，使用者能与机器人之间保持一定的相对距离和相对角度，形成人机队形控制，系统能准确地引导使用者到达目标位置，具有良好导航效果和交互性。小型移动机器人具有体积小，机动性强的特点，还具备防跌落和躲避障碍物的功能，能很好地保证使用者的安全。本发明不仅适用于完全失明的人群，还适用于弱视人群，而且可以在夜间使用。基于振动触觉反馈的便携式穿戴装置舒适性好，可适用于不同体型用户，不增加负担，使用方便。本发明使用了两种人机交互方式—基于振动触觉和听觉的交互方式，增强了系统的环境适应性和抗干扰能力。

附图说明

图1是本发明中小型移动机器人的示意图；

图2是本发明中基于振动触觉反馈的便携式穿戴装置的示意图；

图3是本发明所述控制方法的流程图；

图4是本发明所述采集使用者骨架信息的示意图；

图5是多机器人领域中的领导者—跟随者控制模型；

图6是本发明所述人机队形控制算法；

图7是本发明所述调整人机队形偏差的控制策略。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明提供一种应用于为人导航系统的人机队形控制方法，充分考虑使用者在系统中的重要地位，根据使用者的线速度调节机器人的线速度和角速度，根据使用者与机器人之间的相对位置，指示使用者调整自己的位置，使得使用者与机器人之间保持一定的相对距离和相对角度，从而形成人机队形控制，达到准确为人导航的效果。

如图1所示，描述了本发明所述小型移动机器人的整体结构组成。主要由上位机pc端1，rgb-d摄像头2，语音提示装置3，zigbee无线通讯模块4，360°激光扫描雷达5，车体6和防跌落传感器7组成。rgb-d摄像头和激光雷达通过usb串口线与上位机pc连接，上位机pc端安装ubuntu环境下的机器人操作系统ros(robotoperationsystem)开发软件，便携式穿戴装置与机器人之间通过zigbee无线通信。激光雷达的测量范围为0.15～6m，rgb-d摄像头的水平视场角是57.5°，垂直视场角是43°，在最佳交互距离1.2m～3.5m区间，rgb-d摄像头能跟踪到身高1.8m以下的使用者。zigbee通讯具有近距离、自组织、低功耗、低成本的特点，适用于本发明。如图2所示，描述了本发明所述基于振动触觉反馈的便携式穿戴装置的结构组成，主要由安装在无名指和食指部位的振动电机1，2，手腕部位的振动电机3和控制盒4组成。振动电机是偏心旋转质量(erm)振动电机，控制盒由zigbee无线通讯模块，电机驱动模块和无线充电模块组成。

如图3所示，描述了基于小型移动机器人的为人导航系统的人机队形控制方法，具体步骤如下所示：

(1)激光雷达扫描周围环境，将与周围物体之间的距离信息传送至上位机，上位机利用gmapping、hector或者cartographer软件功能包对距离信息进行处理后得到二维环境地图；

(2)上位机根据地图信息，利用navigation软件功能包实时进行最优路径规划，最优路径规划包括全局规划和局部规划，全局规划采用a*或者dijkstra法，图6中的l是全局规划算法规划出的起点到目标位置的最佳路径，由于全局规划，未考虑实时环境信息变化，随着机器人的前进，周围环境信息与绘制地图时会有所不同，可能会出现新的障碍物，所以要采用局部规划。局部规划采用动态窗口法，能规划出一定距离si内的规划速度(vi,ωi)，实时躲避静态障碍物和动态障碍物，si是光滑的曲线，是l在一定时间内的分割曲线，si曲线的曲率半径为αi:

要使机器人跟随规划的轨迹前进，每小段si的曲率半径必须准确地传送至下位机，

并执行相应的运动控制。

(3)如图4所示，rgb-d摄像头以10hz的采样频率采集使用者的骨架信息并传送至上位机。rgb-d摄像头能够跟踪使用者身上15个骨骼点的信息，通过采集使用者左右肩的空间坐标p1(x1v,y1v,z1v)，p2(x2v,y2v,z2v)，使用视场坐标系与位姿估测坐标系的坐标转化公式和中点公式：

计算出双肩中点的位置坐标，用来代表使用者的位置pm(xm,ym,zm)(ym为固定值)，因为双肩中点的运动轨迹受到行走引起的振动的影响要小于其他骨骼点。通过两次采集的位置信息和两次采集相隔时间0.1s，可以计算得到使用者的速度信息(vh)；

(4)如图5所示，描述的是多机器人领域中的领导者—跟随者控制模型，双轮差速移动机器人是非完整约束移动机器人，其运动模型可以简化为：

关于距离srh和角度βrh的运动学方程：

其中，d是跟随者参考点的偏轴距离，srh和βrh是两机器人之间的实际相对距离和相对角度，ψrh＝θr+βrh-θh。为了防止机器人之间发生碰撞，必须保证srh足够大。

上述运动学方程化简为：

其中，输出变量uh＝[vh,ωh]^t指跟随者的运动线速度和角速度，输入变量ur＝[vr,ωr]^t指领导者的运动线速度和角速度，zrh＝[srh,βrh]^t。

其中d/srh>0；

如图6所示，考虑到为人导航与多机器人领域的编队控制有着相似的功能，即将目标物体引导到目标位置，所以将领导者-跟随者控制模块应用到为人导航系统的人机队形控制算法：

其中，

考虑局部路径规划中αi的作用，将机器人角速度设定为：

将机器人角速度公式代入人机队形控制算法化简得：

其中a,b,c,d>0；

从以上公式可得，当使用者的线速度vh增大时，机器人的线速度vr也会增大。同样，当使用者减速时，机器人的线速度vr也会减小，致使机器人与使用者之间保持适当的距离。人机队形控制算法融合使用者速度(vh)和规划速度(vi,ωi)，产生机器人实际控制速度(vr,ωr)，传送至下位机，控制机器人跟随规划轨迹前进，只要使用者调节运动的角速度(ωh)，就能使机器人和使用者之间保持一定的相对距离(sd)和角度(βd)，保持一定的人机队形；

(5)考虑到使用者的运动角速度(ωh)无法精确控制，如图7所示，本发明未直接将使用者运动角速度(ωh)传达给使用者，而是利用使用者与机器人的相对位置，通过基于振动触觉反馈的便携式穿戴装置和语音提示装置，指示使用者调整自己的位置，使机器人和使用者之间保持一定的相对距离(sd)和相对角度(βd)，实现为人导航的功能。通过以下公式实时更新使用者的相对位置pm(xm,ym,zm)：

t1，t2，t3分别代表振动电机1，2，3，振动电机的振动控制策略与使用者位置对应的关系如下所示：

语音提示的控制策略与使用者位置对应的关系如下表所示：

本发明能实现应用于为人导航系统中的人机队形控制方法，以使用者为主导，使用者能自由地选择行走的速度，不必被动地跟随机器人移动，使用者能与机器人保持一定的相对距离和相对角度，形成人机队形控制，系统能准确地引导使用者到达目标位置，具有良好导航效果和交互性。小型移动机器人具有体积小，机动性强的特点，还具备防跌落和躲避障碍物的功能，能很好地保证使用者的安全。本发明不仅适用于完全失明的人群，还适用于弱视人群，而且可以在夜间使用。基于振动触觉反馈的便携式穿戴装置舒适性好，可适用于不同体型用户，不增加负担，使用方便。本发明使用了两种人机交互方式—基于振动触觉和听觉的交互方式，增强了系统的环境适应性和抗干扰能力。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。