基于触地力学反馈的行人导航系统及步行轨迹跟踪控制方法

本发明涉及残障人士辅助，尤其涉及一种基于触地力学反馈的行人导航系统及步行轨迹跟踪控制方法。

背景技术：

1、眼睛是人们感知周围环境最主要的工具，但对于盲人来说，视力障碍的问题严重影响了他们的正常生活，大多数视障人士使用的导盲工具为功能简单的手杖等简陋的设备，当今社会发展迅速，简单的手杖已经无法适应目前的复杂环境，然而导盲犬代表的生物导盲方式，受高训练成本、长训练周期和寿命等影响始终无法得到广泛的普及。

2、阿尔茨海默病，症状表现为逐渐严重的认知障碍(记忆障碍、学习障碍、注意障碍、空间认知机能、问题解决能力的障碍)，由于这些症状的发生导致患病者存在定位和导航困难，出现易走失的现象。并且因为该病的症状不明显，救助者对患者的状态和病情难以进行分辨，从而无法实现有效救助。

3、基于上述问题，当前智能化设备技术越来越成熟，一种基于触地力学反馈的行人导航系统是解决视障、智力障碍等特殊人士出行以及日常生活的较好选择。

4、目前功能较为完善的智能导航系统设备可以分为穿戴式导航设备、手持式导航设备、基于智能终端的导航系统，移动式导航机器人等。智能导航系统在传统导行系统的基础上增加多种传感器和计算平台，向行人提供路面信息，但无法提供具体的导航信息。穿戴式导航设备将导航设备利用左、右耳声音语音提示使人产生方向性，但无法给出实体的牵引感且质量过大使行人疲劳。手持式导航仪在行人手腕处和拇指处产生振动表示两处距离，距离越近振动越强，行人可以通过不断转动手腕扫描周围环境来躲避障碍物，但受精度影响较大。基于上述设备的优缺点，一个好的导航系统应该具备力反馈机制的牵引设备，基于人体动力学的行走轨迹跟踪，可以大大降低了行人受伤的概率，能够显著提高视障、智力障碍等特殊人士在室内外行走的效率和安全性。

技术实现思路

1、本发明为了解决上述问题，提出了一种基于触地力学反馈的行人导航系统及步行轨迹跟踪控制方法，可在人体运动学模型，动态环境，障碍物等约束下，通过步行轨迹跟踪控制方法控制力反馈机构实现对视障、智力障碍等特殊人士的自主引导。

2、为实现上述目的，该方法采用如下技术方案：第一方面，本发明提供一种基于触地力学反馈的行人导航系统，包括：

3、环境信息获取模块，用于获取环境的三维信息；

4、定位模块，用于获得当前行人和系统的当前位姿；

5、环境感知与探索模块，用于根据三维信息对环境进行语义化建模，形成增量式网格语义地图；

6、步态规划模块，规划路径作为全局参考路径，所述全局参考路径用于为行人行走引导过程中提供参考；

7、力反馈控制模块，对步态规划模块得到的路径进行跟踪控制，引导行人行走。

8、第二方面，本发明通过一种基于触地力学反馈的行人导航系统的步行轨迹跟踪控制方法，包括以下步骤：

9、根据滚动域条件，获取行走轨迹的初始状态和终止状态；

10、构建人体行走运动学模型离散化差分模型，具体为：根据人体行走姿态和路径规划需要的简化运动学模型，建立步行人体的非线性独轮车运动学模型；对模型进行离散化得到离散化方程；

11、对实时得到的初始状态和终止状态进行最优化控制，结合目标函数和约束条件进行最优求解得到两个状态之前的人体步行的最优轨迹，具体为：

12、对实时返回的行人位置状态量进行最优化控制，选择混合能量与目标代价方程为代价函数；

13、结合目标函数和约束条件进行最优求解得到当前时刻的状态向量，用kkt条件求解该非线性优化问题；

14、将求取的最优航向角作为控制输入序列，给到力反馈控制器，驱动力反馈机构跟踪最优航向角，沿规划的路径引导行人。

15、第三方面，本发明提出一种行人导航系统力反馈设备，包括设备的力反馈机械机构和设置于设备上的力反馈控制模块，满足行人导航系统的需求，步行轨迹跟踪控制方法所述的步骤。

16、第四方面，本发明提供一种电子设备，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成步行轨迹跟踪控制方法所述的步骤，该电子设备安装在第三方面上的设备上。

17、与现有技术相比，本发明的有益效果为：

18、(1)本发明能够通过系统对环境的自主感知，在复杂环境中规划行走方案，力反馈机制，智能语音以及震动提醒等功能，帮助特殊人士满足在家居环境和室外环境的安全通行需求，可以自主引导行人行走，提高视障、智力障碍等特殊人士自主生活的能力。

19、(2)本发明能够在动态环境下进行步行轨迹跟踪控制，最优求解得到人体步行的最优轨迹，使行人能够穿行于稠密人群的动态变化环境中，快速平稳地规划出一条最优行走方案，可在人体运动学模型，动态环境，障碍物等约束下，通过自身的导航方法实现对行人的自主引导。

20、(3)本发明具有力反馈设备，力反馈是在原地转弯中引导视觉障碍者达到目标航向角的最准确和最快的方法。与其他反馈方法，例如振动触觉反馈相比，力反馈的最终航向误差最小，到达目标角度的时间更短，并且作为时间函数的误差最小。

21、(4)本发明具有智能语音及震动交互功能，本发明的语音及震动交互模块可以接收行人的服务指令，做出相应的反馈。在遇到危险场景时，系统通过智能语音以及震动的强弱提示提醒行人注意安全。为满足流畅服务的要求，该语音交互模块能够完成准确语音识别，多轮语音对话等功能。

22、(5)本发明采用考虑人体动力学模型的控制方式，大大降低了行人受伤的概率，显著提高了视障、智力障碍等特殊人士在室内外行走的效率和安全性

技术特征：

1.一种基于触地力学反馈的行人导航系统，其特征在于，包括：

2.根据权利要求1所述的基于触地力学反馈的行人导航系统，其特征在于，系统还包括语音交互模块，语音交互模块包括麦克风阵列、语音播放器、震动马达以及语音处理模块；

3.根据权利要求2所述的基于触地力学反馈的行人导航系统，其特征在于，语音处理模块将感知到的危险因素生成语音提醒指令通过语音播放器进行播放，通过震动马达产生震动提醒行人；运动过程中的危险因素通过环境信息获取模块获取的环境的语义的信息以及动态障碍物预测出的危险信息确定。

4.一种基于权利要求1所述基于触地力学反馈的行人导航系统的步行轨迹跟踪控制方法，其特征在于，包括以下步骤：

5.根据权利要求4所述的方法，其特征在于，获取行走轨迹的初始状态xs和终止状态xg，建立步行人体地面参考系图；

6.根据权利要求5所述的构建人体行走运动学模型离散化差分模型，其特征在于，用非线性独轮车运动学模型建立步行人体运动模型，包括以下步骤：

7.根据权利要求6所述的方法，其特征在于，对实时得到的初始状态和终止状态进行最优化控制，结合目标函数和约束条件进行最优求解得到两个状态之前的人体步行的最优轨迹，具体步骤包括：

8.根据权利要求7中所述的方法，其特征在于，将上述步骤求取的最优航向角x3(k)作为控制输入序列，给到力反馈控制器，驱动力反馈机构跟踪最优航向角，沿规划的路径引导行人，再依次进行k+1,k+2,k+3,…,n状态的计算，重复以上过程，得到0～n各时刻的状态量，每次只取当前时刻的x3(k)作为控制输入施加在电机上；通过力反馈模块对步态规划模块得到的轨迹进行跟踪控制，引导行人行走。

9.一种基于权利要求1所述行人导航系统的行人导航系统力反馈设备，其特征在于，包括设备的力反馈机械机构和设置于设备上的力反馈控制模块。

10.一种电子设备，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成权利要求4-8所述的方法的步骤。

技术总结
本发明公开了一种基于触地力学反馈的行人导航系统及步行轨迹跟踪控制方法，系统包括环境信息获取模块、定位模块、环境感知与探索模块、步态规划模块、力反馈控制模块，方法包括：获取行走轨迹的初始状态和终止状态，构建人体行走运动学模型离散化差分模型，对实时得到的初始状态和终止状态进行最优化控制，结合目标函数和约束条件进行最优求解得到两个状态之前的人体步行的最优轨迹，将求取的最优航向角作为控制输入序列，给到力反馈控制器，驱动力反馈机构跟踪最优航向角，沿规划的路径引导行人。本发明可在人体运动学模型，动态环境，障碍物等约束下，通过步行轨迹跟踪控制方法控制力反馈机构实现对视障、智力障碍等特殊人士的自主引导。

技术研发人员：周慧,桂梦凡,那清权,滕洪璟,冯睿楠,袁海磊,熊傲雪
受保护的技术使用者：南京理工大学
技术研发日：
技术公布日：2024/1/15