[发明专利]一种机器人末端外力预估方法

权利要求书

1.一种机器人末端外力预估方法，其特征在于：包括以下步骤：

Step1:建立机器人与外力交互的末端动力学模型；

Step2:建立机器人动力学补偿误差模型；

Step3:建立机器人末端外力预估模型；

Step4:采用粒子群算法标定外力预估模型参数；

所述Step3中，机器人末端受到的外力F_ext包括机器人末端与环境的交互力，包括力和力矩，即：

F_ext＝[f_{ext_x},f_{ext_y},f_{ext_z},τ_{ext_x},τ_{ext_y},τ_{ext_z}]^T；

其中，F_ext为基于机器人基坐标系表示机器人末端受到的外力F_ext矩阵；

所述Step3中，机器人末端动力学模型由机器人关节空间动力学模型通过末端雅克比矩阵J_c(q)转换而来，机器人关节空间动力学模型为：

其中，M_s(q)是机器人连杆惯量矩阵；是离心力科氏力矢量；G(q)是机器人重力力矩；为关节摩擦力矩；τ_c为机器人驱动力矩；为F_ext在机器人关节空间的等效关节力矩；q、分别为机器人连杆角度位置、速度和加速度；

所述Step1中，机器人与外力的末端动力学为：

其中，x为机器人末端位姿；

其中，Λ_s(q)为操作空间惯量矩阵；为操作空间中的摩擦力；为操作空间中的科氏力；F_g(q)为操作空间中的重力；F_c为操作空间中的关节驱动力；为机器人末端雅可比矩阵的加权广义逆矩阵；

当机器人雅可比矩阵处于奇异状态或者接近奇异状态时，可采用阻尼最小二乘法避开奇异状态，保证机器人关节角速度连续；

所述Step2中，机器人动力学补偿误差模型用于分析机器人末端动力学补偿误差因素对外力预估的影响；

令分别为F_g(q)动力学前馈补偿项，在外力作用下所述机器人驱动力F_c可以表示为：

其中，F_res为包含F_ext的残余有效力；

所述Step2中，由于现实中不能完全精确掌握机器人动力学模型参数，前馈补偿项相对实际存有误差，机器人动力学补偿误差e_dyn为：

e_dyn＝e_ΛU+e_f+e_g；

其中，

其中e_ΛU为操作空间惯性力和科氏力的补偿误差；e_f为操作空间摩擦力的补偿误差；e_g为操作空间重力的补偿误差；

基于所述机器人的末端动力学和所述机器人的动力学补偿误差，得到机器人动力学补偿误差对机器人外力预估影响的表达式，即：

F_ext＝e_dyn-F_res；

其中，F_ext为需要预估的外力；e_dyn为动力学补偿误差；F_res为参与有效力；

所述机器人的末端外力预估模型是用于在无力传感器条件下，基于机器人本体传感器信号包括电机角度位置、电机角速度和电机扭矩，实时预估机器人末端与环境发生接触的力。