[发明专利]一种应用于手术机器人的主动摆位方法及系统

权利要求书

1.一种应用于手术机器人的主动摆位方法，其特征在于，包括以下步骤：

S1：在机械臂运行过程中采集机械臂轴关节的关节角度，以及计算所述机械臂轴关节的关节角速度；

S2：依据所述关节角度计算重力辨识项，依据所述关节角速度计算库伦摩擦力辨识项，依据所述关节角速度计算粘滞摩擦力辨识项；

S3：基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，以减少所述机械臂摆位过程中使用者施加的力。

2.根据权利要求1所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S2中，依据所述关节角度计算所述重力辨识项之前，还包括：建立所述重力辨识项的理论模型，具体为：

利用所述机械臂的每个连杆的DH参数建立基于所述DH参数的齐次变换矩阵；

基于所述齐次变换矩阵，通过拉格朗日动力学得出重力项与所述关节角度的关系式，作为所述重力辨识项的所述理论模型的关系式。

3.根据权利要求2所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S2中，还包括：对重力项和库伦摩擦力项进行辨识拟合，获取所述重力辨识项和所述库伦摩擦力辨识项，具体为：

对所述机械臂做位置环控制，所述位置环控制包括设置所述机械臂运动时的穿越频率，输入正弦的电流激励，和所述关节角度进行对比，设计超前补偿器以补偿相位裕度，同时串联一个滞后补偿器以减小高频增益；

设计好所述位置环控制后，对所述电机进行S运动规划，使得所述机械臂做正反两个方向的低速匀速运动，通过采集所述低速匀速运动过程中同一位置下正反两个方向运动时的电流，做差和做和分别得到所述库伦摩擦力辨识项和所述重力辨识项的曲线，其中，所述库伦摩擦力辨识项取常量，所述重力辨识项的关系式的系数按照所述理论模型的关系式进行拟合。

4.根据权利要求3所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S2中，还包括：对所述重力辨识项、所述库伦摩擦力辨识项和所述粘滞摩擦力辨识项计算补偿电流，具体为：

通过低通滤波器获得当前的所述关节角速度以及当前关节位置；

通过所述当前关节位置计算出基于所述重力辨识项的重力补偿；

设定所述关节角速度的阈值，若当前所述关节角速度在所述阈值范围内，增加高频方波的静态输出以实现初始的助力，若当前所述关节角速度在所述阈值范围外，依据所述关节角速度的方向计算出基于所述库伦摩擦力辨识项的库伦摩擦力补偿；

基于粘滞摩擦力与所述关节角速度成正比，设置所述粘滞摩擦力与所述关节角速度的调节系数，以计算出基于所述粘滞摩擦力辨识项的粘滞摩擦力补偿。

5.根据权利要求5所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S3中，基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的电机进行补偿的电流，具体为：

将所述重力补偿、所述库伦摩擦力补偿和所述粘滞摩擦力补偿相加，得到为所述机械臂轴关节的所述电机进行补偿的目标输出电流。

6.根据权利要求4所述的应用于手术机器人的主动摆位方法，其特征在于，在步骤S3中，基于所述重力辨识项、所述库伦摩擦力辨识项以及所述粘滞摩擦力辨识项，计算为所述机械臂轴关节的所述电机进行补偿的电流，具体的计算公式为：

其中，G(θ)为所述重力辨识项，为所述库伦摩擦力辨识项，为所述粘滞摩擦力辨识项，θ为所述关节角度，为所述关节角速度；

在尚未拖动所述机械臂时，因为所述重力辨识项的补偿，所述机械臂会保持在当前位置，并且由于所述高频方波的存在，为使用者在拖动时提供初始助力，以减少静摩擦力的影响，当开始拖动所述机械臂时，所述高频方波被所述库伦摩擦力辨识项的补偿常量代替，同时根据所述关节角速度补偿相应的所述粘滞摩擦力辨识项，以实现摆位助力的功能。