一种基于点云近邻点密度的人机协调空间判断方法和装置

本发明实施例康复机器人运动学，特别涉及一种基于点云近邻点密度的人机协调空间判断方法和装置。

背景技术：

1、人体的上肢拥有较强的操作能力和较为复杂的协调能力，能够完成抓、放、推、拉等一系列的动作及动作组合，是人类整体运动能力的重要组成。随着人口老龄化的加速及寿命的延长，四肢肌力衰退、心血管疾病等导致的肢体残疾的患者数量越来越多，现有技术中，对肢体残疾的有效治疗方法是通过运动实现对受损神经及肌肉的康复，进而实现对肢体的康复。但是由于上肢的关节整体构成较为复杂，对于上肢的康复则需要长期细致、重复的康复训练来达到治疗此类疾病的效果。近些年，伴随着机器人硬件、控制技术的快速发展，和此类疾病的专业医疗康复护理人员的紧缺，康复机器人逐渐成为了解决目前这一问题的重要解决方案。

2、在大多数情况下，机器人都是与患者身体相连，并在同一工作空间内与患者紧密合作，因此，对于康复机器人人机交互的安全性也是一项关键问题。康复机器人作为一种康复训练的器材，如果控制不安全，就可能对康复者造成训练损伤，造成二次伤害，这种事情在近些年发生了很多起，机器人人机交互的安全性问题已经成为学术界及产业界的研究热点。目前，部分外骨骼式上肢康复机器人在设计之初会仿真康复机器人的工作空间及人类上肢的工作空间之间的映射关系，从而在机械结构设计上保证其整体的适应性及安全性。但是，部分康复机器人，尤其是牵引式康复机器人，因其拥有更高的适应性，所以很难从机械结构设计结构的角度去提高人机交互的安全按行，保证机器人的末端运动点在安全的人机协调空间内进行康复训练，这使得康复者在使用康复机器人进行主动式或介导式的康复训练时，存在着很大的安全隐患。现有技术中，康复机器人在控制层面上同样也没有对机器人末端运动点进行空间范围判断的相关方法，者致使牵引式机器人的机器交互安全性存在很大的隐患，阻碍了此类康复机器人的发展和应用。

3、对于人机协调空间的判断，可以等效于双机械臂协同空间的范围判断问题，常见的计算方法是通过数值逆运动学法或几何逆运动学法对双臂进行分别求解实现双臂协同空间的界定，但是传统的逆运动学解法常受限于机械臂自由度的数量，逆运动学求解的速度以及易出现奇异解的问题，因此有部分研究者应用了一些优化算法，以改进传统逆运动学解法所存在的缺陷。但是此类方法，在判断机器人末端运动点是否处在人机协调空间内时，因其计算速度较慢，且存在错误结果，所以不符合空间快速判断的安全控制系统的实时性要求。

4、因此，亟需一种能够快速、准确判断上肢康复机器人末端运动点所处人机协调空间的相对空间范围的方法，为安全控制提供运动学基础。

技术实现思路

1、本发明实施方式的目的在于提供一种基于点云近邻点密度的人机协调空间判断方法和装置，以根据不同体型康复者，精确、快速确定上肢康复机器人人机协调空间的范围，为康复机器人的安全控制提供运动学基础，从而在控制层面提高上肢康复机器人的安全性。

2、为解决上述技术问题，本发明的实施方式提供了一种基于点云近邻点密度的人机协调空间判断方法，具体包括：

3、根据深度相机建立人体上肢模型，建立康复机器人模型；

4、根据人体上肢模型和康复机器人模型，利用蒙特卡洛方法生成人体上肢工作空间的第一点云和康复机器人机械臂工作空间的第二点云；

5、对第一点云和第二点云进行滤波，得到密度均匀分布且大小一致的第三点云和第四点云；

6、根据康复对象基坐标系和康复机器人空间坐标系，混合第三点云和第四点云得到点云密度梯度变化的第五合成点云；

7、基于k近邻算法计算第三点云、第四点云和第五合成点云中各点的相对密度；

8、在计算第三点云、第四点云和第五合成点云中分别代入运动输入点，利用k近邻算法分别查找运动输入点在第三点云、第四点云和第五合成点云中的近邻点第一近邻点、第二近邻点和第三近邻点，分别获取第一近邻点、第二近邻点和第三近邻点的相对密度第一相对密度、第二相对密度和第三相对密度；

9、根据第一相对密度、第二相对密度和第三相对密度判断运动输入点是否在人机协调空间内。

10、优选的，基于k近邻算法计算第三点云、第四点云和第五合成点云中各点的相对密度，具体包括：

11、遍历第三点云、第四点云和第五合成点云中每一点，根据k近邻算法搜索当前点的相邻点，以当前点和最近相邻点的距离的倒数作为当前点的相对密度。

12、优选的，对于待计算点云中的一点p，p表示当前点，q表示p所在的待计算点云中其他任一点，n为待计算点云中的数据量，用dis(p,q)表示点p与点q之间的距离，用dp表示点p与待计算点云中其他点的最小距离，则当前点的相对密度：

13、。

14、优选的，根据第一相对密度、第二相对密度和第三相对密度判断运动输入点是否在人机协调空间内，具体包括：

15、若第三相对密度大于第二相对密度，则代表运动输入点在人机协调空间内，否则判定运动输入点不在人机协调空间内。

16、优选的，利用体素中心滤波器对第一点云和第二点云进行滤波，具体包括：

17、根据第一点云和第二点云分别计算其对应的最小包围盒边长；

18、基于八叉树结构，分别构建第一点云和第二点云的三维体素栅格；

19、分别遍历第一点云和第二点云中的非空体素，根据非空体素的行列层数计算非空体素的中心，得到密度均匀且大小一致的第三点云和第四点云。

20、优选的，根据非空体素的行列层数计算非空体素的中心，具体包括：

21、用每个体素的中心点来近似表达体素内所有其他的点，对于非空体素内的任一点，根据其所在体素的行row、列col、层数lay以及计算得到的其所在体素的坐标最小值xmin、ymin、zmin，计算所在体素的中心为ccenter(xcenter,ycenter,zcenter)为：

22、，

23、其中，r为设定的体素边长。

24、优选的，生成人体上肢工作空间的第一点云和康复机器人机械臂工作空间的第二点云，具体包括：

25、根据人体上肢模型求解得人体上肢正运动学方程，人体上肢模型具体为人体上肢dh参数；

26、根据康复机器人模型求解得到康复机器人的正运动学方程，康复机器人模型为康复机器人的dh参数；

27、分别在人体上肢和康复机器人的各个关节上下限中随机取值进行遍历；

28、将人体上肢关节的随机值代入到人体上肢正运动学方程中，将康复机器人关节的随机值代入到康复机器人的正运动学方程中，求得上肢的位置坐标点集合和康复机器人末端的位置坐标点集合。

29、优选的，在人体上肢和康复机器人的各个关节上下限中随机取值进行遍历，具体包括：，

30、gp,limb为人体上肢的第p个关节角度取到的随机值，gq,robot为康复机器人的第q个关节角度取到的随机值，gp,min表示第p个关节的角度最小值，gq,min表示第q个关节的角度最小值， gp,max表示第p个关节的角度最大值，gq,max表示第q个关节的角度最大值，rand随机取0~1间的随机数。

31、优选的，根据深度相机建立人体上肢模型，具体包括：

32、对深度相机与康复机器人进行手眼标定，得到像素坐标系与康复机器人坐标系之间的坐标转化关系，深度相机用来捕捉人体上肢的关节坐标；

33、利用深度相机多次拍摄康复者上肢，获取康复者的人体上肢的关节数据及三维坐标，计算得到人体上肢肩胛带、肱骨和前臂的长度；

34、获取康复者的上肢简化后的运动链，根据人体上肢肩胛带、肱骨和前臂的长度以及人体上肢关节极限运动范围，得到简化后的人体上肢dh参数。

35、本发明还提供一种基于点云近邻点密度的人机协调空间判断装置，包括：

36、建模模块，用于根据深度相机建立人体上肢模型，建立康复机器人模型；

37、点云生成模块，用于根据人体上肢模型和康复机器人模型，利用蒙特卡洛方法生成人体上肢工作空间的第一点云和康复机器人机械臂工作空间的第二点云；

38、滤波模块，用于对第一点云和第二点云进行滤波，得到密度均匀分布且大小一致的第三点云和第四点云；

39、合成模块，用于根据康复对象基坐标系和康复机器人空间坐标系，混合第三点云和第四点云得到点云密度梯度变化的第五合成点云；

40、密度计算模块，用于基于k近邻算法计算第三点云、第四点云和第五合成点云中各点的相对密度；

41、查找判断模块，用于在计算第三点云、第四点云和第五合成点云中分别代入运动输入点，利用k近邻算法分别查找运动输入点在第三点云、第四点云和第五合成点云中的近邻点第一近邻点、第二近邻点和第三近邻点，分别获取第一近邻点、第二近邻点和第三近邻点的相对密度第一相对密度、第二相对密度和第三相对密度；根据第一相对密度、第二相对密度和第三相对密度判断运动输入点是否在人机协调空间内。

42、本发明实施方式相对于现有技术而言，本发明提供的基于点云近邻点密度的人机协调空间判断方法和装置，借助点云在空间范围内对人体上肢结构层面确定人机协调空间的范围，提高了人机协调空间范围的求解速度和求解精度，从而提高了康复机器人操作安全性，满足当前康复训练的要求。直接利用人机协调空间点云密度的变化特点，根据相对密度数值大小快速判断运动输入点是否在人机协调空间内，利用点云的属性信息进行人机协调空间范围的定位，避免在三维空间中采用复杂的算法进行位置分析和判断，整个判断过程运算量小、判断速度快。另一方面，本发明能够根据不同体型康复者，建立与之对应的人体上肢模型，从而获得与之对应的准确的人机协调空间，为康复机器人的安全控制提供运动学基础，从而帮助开发人员从控制层面提高上肢康复机器人的安全性。

43、上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。