[发明专利]一种可穿戴外肢体机器人碰撞响应控制方法及系统

权利要求书

1.一种可穿戴外肢体机器人碰撞响应控制方法，其特征在于，包括：

获取机器人的各关节角度和关节力矩；

将获取的各关节角度和关节力矩代入动量观测器的动态方程中，获取机器人各关节的外力矩；

将各关节角度和外力矩代入末端接触力计算模型中，获取机器人的末端接触力；

根据机器人的末端接触力判断机器人末端是否发生碰撞；

当判断发生碰撞时，计算发生碰撞时的关节角度伺服误差，将发生碰撞时的关节角度伺服误差代入机器人末端可变刚度控制律模型中，获得所需的末端关节力矩，通过所需的末端关节力矩对机器人末端进行控制；

动量观测器的动态方程为：

其中，θ表示关节角度，表示关节角速度，τ_j表示关节力矩，r为外力矩观测值，为对动量p求导获得，为的观测值，K₀为动量观测器的调节系数，为对r求导获得；

末端接触力计算模型为：

其中，ⁱf_i表示坐标系{i}下施加在连杆i上的力，ⁱn_i表示坐标系{i}下施加在连杆i上的力矩，τ_i表示施加在关节i上的外力矩，表示坐标系{i+1}相对与坐标系{i}的旋转矩阵，ⁱP_i+1表示坐标系{i+1}相对与坐标系{i}的平移矩阵，旋转矩阵和平移矩阵通过关节角度获得，为坐标系{i}的Z方向；

机器人末端可变刚度控制律模型为：

其中，K_px表示笛卡尔空间中末端执行器的期望刚度，E表示关节角度伺服误差，θ为机器人的实际关节角度，K_d表示增益，J(θ)指的是在末端坐标系下写出的雅可比矩阵，为关节角速度误差，J^T(θ)为J(θ)的转置矩阵。

2.如权利要求1所述的一种可穿戴外肢体机器人碰撞响应控制方法，其特征在于，通过电机扭矩获取关节力矩；

或，将机器人的关节角度代入机器人的动力学模型中，获取关节力矩。

3.如权利要求1所述的一种可穿戴外肢体机器人碰撞响应控制方法，其特征在于，由机器人发生碰撞时的动力学方程和机器人的广义动量，构建动量观测器的动态方程。

4.如权利要求1所述的一种可穿戴外肢体机器人碰撞响应控制方法，其特征在于，根据机器人各连杆间的静力传递关系构建末端接触力计算模型。

5.如权利要求1所述的一种可穿戴外肢体机器人碰撞响应控制方法，其特征在于，将各关节角度和外力矩代入末端接触力计算模型中获取与机器人末端碰撞力矩等效的三维接触力矢量；

将三维接触力矢量取模获得机器人的末端接触力。

6.如权利要求1所述的一种可穿戴外肢体机器人碰撞响应控制方法，其特征在于，当机器人的末端接触力超过设定安全保护阈值时，判定机器人末端发生碰撞。

7.如权利要求1所述的一种可穿戴外肢体机器人碰撞响应控制方法，其特征在于，通过雅克比矩阵和静力平衡原理，构建机器人末端可变刚度控制律模型。

8.一种实现权利要求1-7任一项所述的可穿戴外肢体机器人碰撞响应控制方法的控制系统，其特征在于，包括：

数据获取模块，用于获取机器人的各关节角度和关节力矩；

外力矩估计模块，用于将获取的各关节角度和关节力矩代入动量观测器的动态方程中，获取机器人各关节的外力矩；

力矩/力转换模块，用于将各关节角度和外力矩代入末端接触力计算模型中，获取机器人的末端接触力；

碰撞判断模块，用于根据机器人的末端接触力判断机器人末端是否发生碰撞；

碰撞响应控制模块，用于当判断发生碰撞时，计算发生碰撞时的关节角度伺服误差，将发生碰撞时的关节角度伺服误差代入机器人末端可变刚度控制律模型中，获得所需的末端关节力矩，通过所需的末端关节力矩对机器人末端进行控制。