一种OBVP主动运动控制轨迹优化方法、控制方法及装置与流程

本发明属于康复控制，具体涉及一种obvp(optimal boundary valueproblem)主动运动控制轨迹优化方法、控制方法及装置。

背景技术：

1、康复医学中制定的运动疗法可按运动方式，肌肉收缩型式和治疗作用分为多种类型。由外力作用于人体某一部分引起的动作称被动运动。一般用于维持正常或增大已受限的关节活动范围、防止肌肉萎缩和关节挛缩；依靠患者自身的肌力进行运动的方法称主动运动，患者肌力在3级以上者，均可进行主动运动，单纯的主动运动一般不给予辅助、也不施加阻力，主要用于维持关节的活动范围、进行增强肌力和持久力的训练和增强肌肉之间的协调性的训练。

2、在桌面型康复系统中，不少企业和研究机构都采用十字滑台作为机械联动机构，高精度力传感器作为力学数据输入机构。患者使用该平台作康复运动时，将上臂放于康复平台的手托机构上，手握摇杆进行发力，电机驱动机构根据患者出力的大小进行评估，进而测算运动范围大小，实现患者的主动康复运动。在图1是以申请人开发的u-resmart桌面上肢桌面康复机器人给出了这类型机构的常见样式。

3、如专利cn115047823a，公开了一种基于轨迹规划的机器人的控制方法。该基于轨迹规划的机器人的控制方法包括：获得由起始位置至终止位置的当前轨迹方程；其中，当前轨迹方程的确定，包括：获得预设轨迹方程；获得预设轨迹方程中每个位置对应的理论肌肉出力；根据与全部理论肌肉出力之和相关的优化目标，对预设轨迹方程中的参数进行优化，以获得当前轨迹方程；根据当前轨迹方程控制机器人。采用该基于轨迹规划的控制方法可降低训练过程对康复训练师或教练的技能或知识的要求。此专利中需要患者使用该平台作康复运动时，将上臂放于康复平台的手托机构上，手握摇杆进行发力，在整个康复周期中，二维力传感采集系统承担了整个康复系统的力学输入信号采集工作。高精度工业力传感器随其量程、精度等参数的不同，售价在几百到上万元不等。为满足较高的实时响应性能和测量准确度，理想情况是系统应该能对患者输出力的大小进行全量程高精度的监控，所以大部分厂家通常会为其选配高性能的力传感器，该类传感器采集和传感器硬件成本较高，限制了该类型产品的大规模量产使用。而实际的康复过程中调研发现，不少处于康复前期的患者实际肌力较弱，根本无法对全量程输入进行覆盖。

技术实现思路

1、技术问题：设计一种obvp主动运动控制轨迹优化方法、控制方法及装置，以解决上述背景技术中的问题。

2、技术方案：本发明公开了一种obvp主动运动控制轨迹优化方法，包括以下步骤：

3、获取十字滑台当前位置，根据患者运动发力计算末位置；

4、设定目标函数为最小化加速度平方的积分，构建损失函数；

5、基于哈密尔顿函数，利用位置固定和速度固定的边界条件，获取协态输入矩阵；

6、求解所述协态输入矩阵，获取最优状态输入，计算十字滑台末位置力，获取十字滑台的最优轨迹。

7、进一步地，所述“获取十字滑台当前位置，根据患者运动发力计算末位置”的具体的步骤为：

8、任一时刻的5阶多项式表示这一时刻机器的空间信息和运动学信息方程x(t)如下：

9、x(t)＝c5t5+c4t4+c3t3+c2t2+c1t+c0                 公式1

10、其中，设定边界条件：当t分别为第一时刻t1和第二时刻t2时即t1＝0和t2＝t的系统状态条件，位置、速度、加速度系统状态如下：

11、t1＝0，位置为a，速度为0，加速度为0；

12、t2＝t，位置为b，速度为0，加速度为0；

13、将上述第一时刻t1和第二时刻t2的条件代入公式1可以描述为如下矩阵等式：

14、

15、其中，第一行为初位置pstart，第二行为末位置pend，第三行为初速度vstart，第四行为末速度vend，vend是pend导数；第五行为初加速度astart，第六行为末加速度aend，aend是vend导数；

16、获取系统状态量x(t)与控制量u(t)：

17、

18、其中，x(t)表示系统当前的状态，px为t时刻系统在x轴上的位置，py为t时刻系统在y轴上的位置，pz为t时刻系统在z轴上的位置，vx为t时刻系统在x轴上的线速度，vy为t时刻系统在y轴上的线速度，vz为t时刻系统在z轴上的位置；u(t)为输入量，ax、ay、az分别是t时刻x、y、z轴上的线加速度；

19、通过公式3获得系统状态量与控制量的导数如下：

20、

21、进一步地，所述“设定目标函数为最小化加速度平方的积分，构建损失函数”的具体步骤为：

22、定义损失函数j为：

23、

24、其中αx、αy、αz分别为x，y，z轴上的线加速度，x,u为分别为公式3中的的矩阵，公式5通过使用庞德里亚金极小值原理对运行过程进行约束，求解最优控制问题。

25、进一步地，所述“基于哈密尔顿函数，利用位置固定和速度固定的边界条件，获取协态输入矩阵”的具体步骤为：

26、构建哈密尔顿函数：

27、

28、其中h(x,u,λ)为汉密尔顿函数，这里设计为损失函数j和状态方程λtf(x,u)的和，λt是协态系数矩阵，f(x,u)是状态方程，是对汉密尔顿函数的协态λ进行求导，(0,0,0,-λ1,-λ2,-λ3)t是求导的最终结果。其中ax、ay、az分别为x,y,z轴上的线加速度，-λ1,-λ2,-λ3分别代表x,y,z方向上的协态系数。

29、利用边界条件的位置固定，速度固定，加速度不加限制条件，求解协态输入矩阵：

30、

31、解得的最优输入求解为u*：

32、

33、其中，公式7和公式8中各参数的具体含义如下：。

34、2α1表示x轴的速度多项式，2α2表示y轴的速度多项式，2α3表示z轴速度多项式，-2α1t-2β1表示x轴加速度多项式，-2α2t-2β2表示y轴加速度多项式，-2α3t-2β3表示z轴加速度多项式。α1表示x轴多项式一次项待定系数，β1表示x轴多项式常数项待定系数。α2表示y轴多项式一次项待定系数，β2表示y轴多项式常数项待定系数。α3表示z轴多项式一次项待定系数，β3表示z轴多项式常数项待定系数。

35、进一步地，所述“求解所述协态输入矩阵，获取最优状态输入，计算十字滑台末位置力，获取十字滑台的最优轨迹”的具体步骤为：

36、最优状态输入求解为x*：

37、

38、其中，下标f代表终点的,下标0代表起点，x,y,z分别代表x,y,z轴，p代表位置，v代表线速度，如vy0代表起点y轴上的线速度，vz0代表起点z轴上的线速度，pz0代表起点z轴上的位置，px0代表起点x轴上的位置。

39、三个协态量被解析为公式10的状态矩阵：

40、

41、其中，δpx代表t时刻内x轴位置的变化量，δpy代表t时刻内y轴位置的变化量，δpz代表t时刻内z轴位置的变化量，δvx代表t时刻内x轴速度的变化量，δvy代表t时刻内y轴速度的变化量，δvz代表t时刻内z轴速度的变化量，α1表示x轴多项式一次项待定系数，β1表示x轴多项式常数项待定系数。α2表示y轴多项式一次项待定系数，β2表示y轴多项式常数项待定系数。α3表示z轴多项式一次项待定系数，β3表示z轴多项式常数项待定系数。

42、损失函数j用公式10的协态表达为：

43、

44、其中，α1表示x轴多项式一次项待定系数，β1表示y轴多项式常数项待定系数。α2表示y轴多项式一次项待定系数，β2表示y轴多项式常数项待定系数。α3表示z轴多项式一次项待定系数，β3表示z轴多项式常数项待定系数。

45、把两个协态用已知的起始速度，末速度，起始位置，末位置表示

46、

47、其中，δpx代表t时刻内x轴位置的变化量，δpy代表t时刻内y轴位置的变化量，δpz代表t时刻内z轴位置的变化量，δvx代表t时刻内x轴速度的变化量，δvy代表t时刻内y轴速度的变化量，δvz代表t时刻内z轴速度的变化量，α1表示x轴多项式一次项待定系数，β表示x轴多项式常数项待定系数。

48、得到最终的代价函数的表达形式，求取公式13和公式14整个函数极小值，得到当前最优输入，位置速度连续且能量损失最小；

49、

50、其中，下标f代表终点的,下标0代表起点，x、y、z分别代表x、y、z轴，p代表位置，v代表线速度，如vyf代表终点y轴上的线速度，vzf代表终点z轴上的线速度,vz0代表起点z轴上的线速度，pxf代表终点x轴上的位置；

51、代价函数极小值的公式：

52、

53、在速度和位置变化确定的情况下，只和时间t相关，得到公式14函数最小的t代入协态转换为带位置变化和速度变化的表述形式即得到最优轨迹。

54、本发明还公开了一种基于obvp的主动运动控制方法，包括位置模块和与位置模块相连通的电位传感器，所述控制方法包括以下步骤；

55、从位置模块获取当前位置，从力学采集模块获取患者运动发力，计算末位置；

56、计算最小运动代价矩阵，生成轨迹库；

57、代价矩阵是否存在虚数值和不合理的值(极大值或为0，inf或者为0)，若是抛弃虚数值和不合理的值对应的代价矩阵；若否，则进入轨迹生成；

58、轨迹生成开始：通过代价矩阵，选取其中最小的代价作为最有轨迹，计算所有生成出来的轨迹，加入轨迹库；

59、从轨迹库中选取最小代价的轨迹。

60、本发明还公开了一种力学采集装置，包括双维度两方向电位传感器，采集患者运动发力，然后通过上述的方法实现最优轨迹。

61、本发明还公开一种电子设备，包括处理器及存储介质，其特征在于，所述存储介质用于存储计算机程序；

62、所述处理器用于根据所述计算机程序进行操作以执行上述方法的步骤。

63、本发明还公开了一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述方法的步骤。

64、本发明在申请人开发的u-resmart桌面型上肢康复平台上，将主动运动定义为患者在康复平面上施加一定的力，设备驱动机构即可相应运动对应的距离，协助患者上肢完成一次对应的主动运动。针对上述康复场景，本发明旨在提供一种基于摇杆电位器的简易力学信号采集装置和基于obvp的主动运动控制算法，可以使该类型的桌面康复设备都能以较为低廉的硬件价格获得力学控制输入信号，并不比高性能力传感器的控制品质差，具体来说，如果患者运动发力较小则小量程传感器和大量程传感器的运动并无太大区别，压片式等力传感器根据形变量来测定力，通常量程较大，而电位器实际是根据位移量来估计力的，通常量程较小。当患者出力更大的时候，实际上可认为是小量程的满量程输出，调节这个满量程的增益比例可以实现接近于大量程传感器的输出，也即小量程下本方法采用的电位器方案采取和力传感器方案基本一致的输出，大量程下采用小量程的满量程增益输出。

65、本发明一是电位器元件在成本上对比大量程力传感器拥有数量级的优势(十几块vs几百块以上)，二是结合冗余度比较高的优化方式(通常是给无人机或者自动驾驶车辆使用)对各个时刻的速度位置等都做限制，可以在廉价和小量程(0～30n)的输入器件下实现原先高规格器(0～200n)件能达到的效果，且转向更加灵敏。三提出的obvp的优化实现方式计算速度快，基本在十毫秒级别，生成轨迹更加平滑，并考虑了运动学约束，抑制了一些最终轨迹上的突变。

66、有益效果：相比于现有技术，本发明具备以下实质性的特点和进步：

67、本发明提供的obvp的主动运动控制算法，能够使得桌面康复设备都能以较为低廉的硬件价格获得力学控制输入信号，生成的轨迹更平滑，响应更灵敏，且计算速度快，单次采集耗时短，输出波形更平滑，拥有不输于高性能力传感器的控制品质。