[发明专利]一种无需传感器的机械臂碰撞检测方法

权利要求书

1.一种无需传感器的机械臂碰撞检测方法，其特征在于：

步骤一：计算机械臂末端负载对机械臂产生的关节力矩矢量的正交投影矩阵PF，根据该正交投影矩阵PF得到机械臂末端负载的力矩变换矩阵P；

步骤二：建立机械臂本体动力学模型，根据该动力学模型中的各项建立机械臂动量偏差观测器，通过所述机械臂动量偏差观测器得到机械臂关节外力矩矢量r，根据该外力矩矢量r与所述机械臂末端负载的力矩变换矩阵P计算出机械臂的碰撞力矩；

所述步骤二中，计算机械臂的碰撞力矩的具体步骤如下：

步骤2.1：建立机械臂本体动力学模型，表达式为：式中，M(q)为惯性项；为非线性耦合项；G(q)为重力项，为摩擦力项，τ为由机械臂动力学模型计算得到的指令力矩；

步骤2.2：根据机械臂本体动力学模型中的惯性项M(q)，计算机械臂的广义动量p，表达式为：；

步骤2.3：根据机械臂本体动力学模型中的惯性项M(q)和非线性耦合项，计算机械臂本体动力学模型中非线性耦合项的转置矩阵表达式为：；

步骤2.4：建立机械臂动量偏差观测器，表达式为：

式子中，为机械臂关节电机输出力矩，r为机械臂关节外力矩矢量，K为机械臂动量偏差观测器调节系数；

步骤2.5：将步骤一中得到的所述力矩变换矩阵P点乘机械臂关节外力矩矢量r得到机械臂的碰撞力矩，表达式为：；

步骤三：将所述机械臂的碰撞力矩与预设阈值相比较，判断机械臂是否发生碰撞。

2.如权利要求1所述的无需传感器的机械臂碰撞检测方法，其特征在于：所述步骤一中，计算机械臂末端负载的力矩变换矩阵P具体步骤如下：

步骤1.1：根据机械臂驱动器反馈的数据得到机械臂关节反馈的关节角度q、角速度，将关节角速度进行差分得到关节角加速度；

步骤1.2：根据机械臂本体的D-H参数建立机械臂速度雅克比矩阵J，根据反馈的关节角速度和机械臂速度雅克比矩阵J得到机械臂末端的速度矢量，表达式为：；

步骤1.3：将所述机械臂末端的速度矢量差分，得到机械臂末端的加速度矢量a，将该机械臂末端的加速度矢量a和重力矢量g求和得到机械臂末端总的加速度矢量A，将机械臂末端负载质量点乘机械臂末端总的加速度矢量A，得到机械臂末端总的力矢量F；

步骤1.4：根据机械臂本体的D-H参数建立机械臂力雅克比矩阵，根据机械臂末端力矢量F和机械臂力雅克比矩阵，计算得到机械臂关节力矩矢量，表达式为：； 步骤1.5：根据所述机械臂关节力矩矢量，建立机械臂关节力矩矩阵，接着计算出所述机械臂关节力矩矢量的正交投影矩阵PF，表达式为：；

步骤1.6：根据所述正交投影矩阵PF，计算机械臂末端负载的力矩变换矩阵P，表达式

为：。

3.如权利要求2所述的无需传感器的机械臂碰撞检测方法，其特征在于：所述步骤三中，将机械臂的碰撞力矩与预设阈值相比较，判断机械臂是否发生碰撞，表达式为：式子中，TH1为力矩负向恒定阈值；TH2为力矩正向恒定阈值；通过参数d的数值来判断机械臂是否发生碰撞，当d为1时，检测出机械臂发生碰撞；当d为0时，机械臂无碰撞发生。