一种基于机器视觉的按摩理疗机器人柔顺控制算法的制作方法

本发明涉及人工智能医疗，具体为一种基于机器视觉的按摩理疗机器人柔顺控制算法。

背景技术：

1、按摩机器人是中医按摩手法在现代康复医学、人工智能、智能感知及机器人技术领域相互渗透,结合的高技术产物,它可以协助或代替按摩医生完成常规按摩手法的动作,可把按摩医师从繁杂的体力劳动中解放出来,并通过其精确的力/位置反馈控制使按摩手法更加规范,按摩治疗效果更加稳定。近10年来,中医按摩机器人研究引起了国内外很多学者和科研机构的兴趣,成为了医疗保健机器人研究领域的热点之一。目前,无论是在国内还是国外,机器人技术在中医按摩领域的研究和应用只是处于起步阶段,尚未有可以直接引用的技术标准，于是，有鉴于此，针对现有的结构予以研究改良，提供一种基于机器视觉的按摩理疗机器人柔顺控制算法，以期达到更具有实用价值性的目的。

技术实现思路

1、为了解决上述技术问题，本发明提供了如下的技术方案：

2、本发明一种基于机器视觉的按摩理疗机器人柔顺控制算法，包括以下步骤：

3、步骤一：通过视觉传感器对背部穴位进行定位，获取定位点；

4、步骤二：按摩机器人执行末端轨迹进行规划；

5、步骤三：通过力反馈技术对步骤二的执行末端接触力进行测量；

6、步骤四：结合导纳控制算法实现对按摩接触力的控制；

7、步骤五：采用碰撞检测技术保证按摩机器人在工作过程中的安全性。

8、作为本发明的一种优选技术方案，所述步骤一人体背部穴位点定位，对采集的背部图像进行图像预处理以提高目标位置定位与检测的精度，首先，计算出目标穴位点的轮廓矩，其形式为m×n的数字图像f(x0,y0)，其i+j阶几何矩阵mij可写为：

9、

10、即可获得图像质心(x0,y0)的坐标公式：

11、

12、其中，i＝j＝0时，m00是零阶矩阵；i＝1，j＝0ori＝0，j＝1，m10是一阶矩阵；

13、像素坐标(u,v)与图像坐标(x0,y0)转换关系表示为：

14、

15、世界坐标系中任一点(xw,yw,zw)与像素坐标系对应点(u,v)与之间的转换关系表示为：

16、

17、矩阵r和t统称为相机的外参矩阵，zc为相机镜头光心到目标的距离。

18、作为本发明的一种优选技术方案，所述步骤二中按摩机器人执行末端位姿轨迹规划，人体背部穴位点可以用以x,y,z为变量且形如f(x,y,z)的方式表达，对于按摩机器人执行末端来说，在同一个坐标系中表示为：

19、

20、式中的(m,n,p)是按摩机械人执行末端的方向向量，向量的方向朝向人体背部，接触点p0(u,v,w)，即可得到执行末端与人体背部接触点的切面方程：

21、a(x-u)+b(y-v)+c(z-w)+d＝0

22、进而能够计算出按摩机器人的执行末端的方向向量和人体背部表面上接触点的夹角λ的表达式为：

23、

24、其中

25、m,n,p三个参数决定了按摩机器人执行末端与人体背部表面的夹角λ，u,v,w和m,n,p是两组线性无关的向量，这6个变量能够作为表示按摩机器人位姿的一组基，若要描述按摩机器人执行末端的位置及位姿，均可分别用上述两组的6个变量来精确表示执行末端和人体背部的位置关系和姿态。

26、作为本发明的一种优选技术方案，所述步骤三中六维力传感器接触力测量与补偿，机器人按摩工具重力在世界坐标系中的向量可表示为当按摩机器人末端姿态改变时，按摩工具重力在测量坐标系的xt,yt,zt轴上的分量为：

27、

28、由协作机器人运动学，设从世界坐标系到工件坐标系的变换矩阵为

29、

30、按摩工具重力在工具坐标系的三个分量为：

31、

32、令按摩工具重力在工件坐标系下三个方向的力矩分别为mx,my,mz，工件重心在工件坐标系下的坐标为lu,lv,lw，则可得力矩分别为：

33、

34、假设六维力传感器分离出按摩工具重力后的初始值为：

35、{fx0,fy0,fz0,mx0,my0,mz0}

36、而实际接触力为：

37、{fex,fey,fez,mex,mey,mez}

38、二者存在如下关系：

39、

40、至此，可以计算出六维力传感器在任意姿态下按摩工具重力在基坐标系下的分量，并能够进行实时补偿，且始终能对六维力传感器的初始值进行补偿，从而使六维力传感器能够精确地测量到末端接触力。

41、作为本发明的一种优选技术方案，所述步骤四中导纳控制算法实现对按摩接触力的控制，按摩机器人工作过程中，人体背部保持不动，采用voigt模型建立导纳控制器：

42、

43、其中xe＝x-x0，x为笛卡尔空间下的实际位置，x0为笛卡尔空间下的期望位置，xe,分别为位移、速度、加速度误差，md,bd,kd分别为6×6的惯性矩阵、阻尼矩阵、刚度矩阵，fe为末端接触力。

44、人体背部固定不动，采用voigt模型建立导纳控制器。导纳控制器作为控制外环，设立在位置内环之外，将环境模型作用的末端接触力fe与期望接触力fd之差通过导纳控制器转换为位置修正量，再叠加到期望轨迹xr中得到xd输入给位置控制器，调整按摩机器人末端的运动轨迹，实现柔顺性。

45、将按摩机器人位置内环简化为二阶线性传递函数，令输入位置为xr(s)，输出位置为x，则传递函数为

46、

47、从而搭建按摩机器人导纳控制simulink仿真框图。

48、作为本发明的一种优选技术方案，所述采用碰撞检测技术保证按摩机器人在工作过程中的安全性，首先设计观测器进行监测，所以设计的力矩观测器如下：

49、

50、其中，r为观测器的外力矩值；为机械臂广义动量估计值；k1k2k3为增益矩阵，带入机械臂广义动量，对上式变式得到：

51、

52、其中，τ为电机驱动力矩；τf为关节摩擦力矩；分别为机械臂各连杆角度、角速度；g为各关节的重力矩；c为科氏力；

53、对力矩观测器进行二阶微分，在对其进行拉普拉斯变换，得到力矩观测器的传递函数：

54、

55、可知该观测器是一个二阶系统，一旦碰撞力矩发生变换，力矩观测值也随之改变，稳定时，二者几乎相等，选择合适的k1k2k3，便可快速准确的检测出碰撞；

56、如下式表示第i个连杆发生碰撞：

57、

58、其中，设定动态阈值rth，上限为rup，下限为rdown，因此动态阈值设定为：

59、

60、其中，r0为动力学模型误差；为噪音引起的误差；k5为其他干扰误差；

61、当观测值超过阈值时，则可判断按摩机器人工作中发生碰撞，按摩机器人控制器立即转变为力矩控制模式，直到力矩观测值处于动态阈值的上下限，达到躲避碰撞的效果。

62、本发明的有益效果是：

63、当今生活质量不断提高，现有医疗保障存在极大的短板。按摩机器人成功地弥补了其一小部分。按摩机器人可以协助或代替按摩医生完成常规按摩手法的动作,可把按摩医师从繁杂的体力劳动中解放出来,并通过其精确的力/位置反馈控制使按摩手法更加规范,按摩治疗效果更加稳定，并且可以快速普及之全社会；本发明提出的一种基于机器视觉的按摩理疗机器人柔顺控制算法，该算法有效可靠地实现了对人体背部的按摩功能，基于视觉传感器与力传感器的柔顺控制保证按摩机器人执行末端定位的准确性且可使执行末端始终与人体背面达到最佳的贴合，具有较高的实用价值。