一种机器人开门控制方法与流程

本发明是植入式医疗系统的用于放置电极固定装置在颅骨上的工具，属于植入式医疗系统。

背景技术：

1、随着双臂机器人在工业和家用的广泛应用，机器人打开未知尺寸的房门之类的圆弧形作业轨迹的接触作业任务时，机器人能够完成在线作业轨迹的辨识与纠正并保持合适的作业接触力，成为提升机器人自主作业能力的必然要求。受人类手臂肌肉刚度调节作业机制启发，相关技术人员提出了采用阻抗控制的方法是机器人具备了作业柔顺特性。然而，目前的方法面临操作未知参数对象作业刚度参数难确定、作业轨迹跟踪精度差、接触过程不稳定等关键问题，难以完成在实际作业场景中的广泛应用。

2、授权公告号为cn113386138b的中国发明专利中的机器人开门控制作业过程一般依靠机器人自带的视觉传感器，首先对所要操作的房门尺寸进行测量分析，根据测量计算结果规划机器人抓握点的作业轨迹，以实现对房门的操作。但是，这一方法的突出缺点在于其机器人作业运动轨迹是根据视觉传感器、距离传感器等机器人所带的传感器预先对房门的运动尺寸进行测量计算，当测量结果与真值存在偏差时，机器人将在作业抓握点处产生与较大的偏差力，影响作业的进行，导致作业任务中止或失败。这一缺点所产生的原因在于现有的轨迹规划方法无法应对在初始条件存在偏差时的在线运动轨迹辨识与纠正。

3、申请公布号为cn115167112a的中国发明专利申请中的机器人作业机械臂使用阻抗模式来保证作业过程中期望运动与实际作业对象运动约束无法同时满足时的作业稳定。现有技术中的变刚度方法一般根据运动期望误差对机器人刚度进行调整。实际应用中其主要的工作在于确定适应特定任务的刚度参数，现有技术往往根据经验或者运动期望误差进行在线调节，这一方法不能很好的应对实际作业条件多样和轨迹在线动态变化的特点，且忽略了作业运动对作业力的要求。

4、在机器人开门和旋转阀门等一系列圆弧形运动轨迹接触作业过程中，面对未知作业对象时，作业对象的尺寸信息无法获得或者通过传感器测量信息所获得的尺寸偏差较大，为保证机器人的作业接触安全，机器人往往工作在阻抗模式下，传统的固定刚度参数运动控制方式虽然能在作业初始阶段保证作业的进行，但是随着轨迹误差的增加，接触与理想接触力之间的偏差就越大，无法稳定高效的完成整个作业过程。

技术实现思路

1、本发明的目的在于提供一种双臂机器人自主开门控制方法，可用于实时辨识并纠正机器人的作业轨迹，以适应作业对象运动参数未知或不准确的情况。根据历史运动数据完成对作业控制力的估计和机器人运动关节位置和关节刚度的优化求解，保证作业执行的稳定性。

2、本发明的技术方案如下。

3、本发明第一方面提供了一种机器人开门控制方法，包括：

4、步骤101，设定机器人初始作业轨迹参数与机器人阻抗参数；

5、步骤102，计算初始作业轨迹点并执行运动指令；

6、步骤103，采用基于末端作业接触点位姿的递推最小二乘的末端运动轨迹辨识；

7、步骤104，根据辨识结果，计算得出末端下一点位姿数据，纠正并更新末端期望控制位姿；

8、步骤105，基于历史实际运动方向与历史末端阻抗恢复力数据以及上一步末端位姿控制误差更新运动方向期望作业力；

9、步骤106，依据末端期望位姿与运动方向期望作业力采用差分进化算法进行优化得到关节运动控制逆解与关节刚度值；

10、步骤107，根据优化结果更新更新机器人关节刚度值和关节期望位置。

11、优选地，所述步骤101中的机器人初始作业轨迹参数为机器人抓握房门把手的接触点运动轨迹的半径与圆心的初始估计值；所述机器人阻抗参数为机器人阻抗控制模式中的刚度与阻尼参数。

12、优选地，所述步骤102进一步包括：

13、根据抓握点位置与初始估计门转轴方向与轨迹圆心建立门轴坐标系；

14、根据机器人初始抓握位置与姿态以及初始估计轨迹参数，得到机器人下一末端运动轨迹点位姿的迭代形式：

15、

16、

17、其中为机械臂基座到末端抓握点处的变换矩阵；为机械臂基座到当前门轴坐标系的变换矩阵；为门转轴坐标系转动变换矩阵；为末端抓握点在门轴坐标系下的变换矩阵；i表示轨迹点序号；θ为绕门轴转动角度。

18、根据所得到的机械臂抓握运动轨迹点位姿，对实际使用的机械臂进行逆解，得到机械臂各关节的期望关节位置，形成运动指令，控制机械臂运动。

19、优选地，所述步骤103进一步包括：

20、获取当前时刻的机器人抓取点位姿数据；

21、初始化轨迹球面辨识和平面辨识的p矩阵；

22、利用建立的输入激励矩阵对轨迹球面和平面参数进行迭代计算，得到辨识球面的参数和辨识平面的参数；

23、将轨迹球面参数向轨迹平面投影，得到投影纠正后的轨迹参数估计值。

24、优选地，所述轨迹球面辨识和平面辨识的p矩阵计算公式如下：

25、

26、其中，pball、pplane分别为轨迹球面和轨迹平面辨识的p矩阵；hball、hplane分别为轨迹球面和轨迹平面辨识的输入激励矩阵；

27、所述轨迹所在球面方程、与轨迹平面方程表示为：

28、

29、其中，运动轨迹辨识的参数为轨迹的圆心(a,b,c)、半径(rrls)与转动平面参数(pa,pb,pc,pd)；

30、得到球面参数辨识方程：

31、

32、其中，yb＝x2+y2+z2为球面辨识模型的观测值；x,y,z为抓握点笛卡尔空间位置；

33、所述轨迹平面参数方程：

34、yp＝[y z 1][pb pc pd]t＝hplanesp

35、其中，yp＝-x为球面辨识模型的观测值。

36、优选地，所述迭代更新公式如下：

37、球面参数：

38、

39、平面参数：

40、

41、所述圆心与半径辨识结果向辨识平面投影的计算方法：

42、

43、

44、根据末端接触点当前在机械臂基座坐标系下的位姿，得到最终辨识结果，轨迹圆心坐标(a’,b’,c’)，轨迹半径(r’)，轨迹平面的法向量(1,pb,pc)。

45、优选地，所述步骤104进一步包括

46、依据辨识结果轨迹半径、轨迹圆心，可以更新门轴坐标系的位姿，依据下式计算得到纠正轨迹点位姿。

47、

48、其中，为纠正后下一轨迹点的变换矩阵；为当前机械臂基座到辨识结果所更新的门轴坐标系的变换矩阵；为纠正后门转轴坐标系转动变换矩阵；为末端抓握点在纠正后的门轴坐标系下的变换矩阵；dr为当前门轴坐标系；d'r为按指定角度绕辨识结果平面法向量旋转后的坐标系。

49、变换矩阵计算公式如下：

50、

51、优选地，所述步骤105利用历史运动和期望误差反馈两部分对机器人阻抗模式下的抓握点处阻抗恢复力进行估计，进一步包括

52、利用运动历史数据对运动方向上作用力的大小计算：

53、

54、其中，w为末端抓取点接触力(f:fx,fy,fz)与力矩(t:tx,ty,tz)；j-t为机械臂雅可比矩阵转置的逆；为上一控制周期关节刚度值；δq为上一控制周期关节期望偏差；vlast为上一控制周期末端抓取点位移向量；fest为运动方向作用力估计；

55、利用末端期望位置反馈误差估计力计算：

56、

57、其中，δp为机器人末端抓取点位置误差，即末端抓取点期望位置与当前位置之差；v为末端抓取点期望运动的位移向量；kp、ki为比例与积分系数；

58、得到末端抓取点处期望阻抗恢复力为fim＝[(fest+fpi)v,01×3]。

59、优选地，所述步骤106依据步骤104和步骤105所得到的期望运动和运动恢复力，以机器人关节刚度和关节位置作为优化变量建立优化问题，使用差分进化算法进行求解；

60、其中，建立优化问题的目标函数与约束条件为：

61、

62、其中：q,k优化变量为关节期望位置与关节刚度值；q0为当前关节角；δqs为关节角变动范围；klow,kup为关节刚度值上下限；ft2(q,k)为机器人抓握点处与期望运动方向垂直的力向量；fv机器人抓握点处阻抗恢复力期望运动方向上的分量；t为机器人抓握点处阻抗恢复力矩；forward(q)为机器人运动学正解。

63、本发明第二方面提供了一种机器人，包括至少一条机械臂，所述机器人能够执行本发明第一方面中任一项所述的方法完成开门作业。

64、通过以上技术方案，本发明能够取得如下的有益效果。

65、本发明的方法能够适应未知尺寸的房门的机器人开门作业任务：机器人能够在面对未知的房门时进行自适应，无需提前知道作业对象的尺寸信息。这增加了机器人的灵活性，提升其应对多样的实际作业任务。

66、本发明的方法能够实现实时轨迹参数辨识与轨迹纠正以及关节位置和刚度优化：通过实时获取抓握点的位姿数据，机器人可以实时辨识作业过程中的轨迹参数。这有助于消除机器人因控制轨迹与实际运动不匹配引起接触力不合适，最终导致机器人接触点脱开，机器人可以根据实际作业环境的刚度参数自适应地调整自身的刚度，以确保与作业对象的交互力在安全范围内，并防止损坏机器人或作业对象，这有助于提高任务完成的准确性和可靠性。

67、总之，根据本发明的方法能够使机器人能够在房门尺寸未知或者尺寸存在误差的条件下自主完成作业任务，同时保持安全和高效。这对于开启未知尺寸的门或旋转阀门等圆弧形作业轨迹的任务特别有益，因为这些任务通常需要高度的精确性和安全性。