[发明专利]一种基于外力观测器的机械臂安全碰撞策略

权利要求书

1.一种基于外力观测器的机械臂安全碰撞策略，其特征在于，所述机械臂为二连杆机械臂，其安全碰撞策略步骤为：

步骤1：利用拉格朗日函数建立外力干扰下柔性关节机械臂动力学模型，并对其进行受力分析，二连杆机械臂动力学模型分为连杆端动力学方程和电机端动力学方程，连杆端动力学方程如下：

电机端动力学方程如下：

式中q,θ∈Rⁿ，表示机械臂连杆和电机的位置；M(q)∈R^n×n，表示机械臂连杆惯量矩阵；表示机械臂连杆端离心力矩和哥氏力矩矢量；G(q)∈Rⁿ表示重力矩；

步骤2：对二连杆机械臂进行平面内的受力分析，根据力的平衡原理及相互作用力原理可得：

F_y1＝F_y2＝F_y1′＝F_y2′

F_x1＝F_x2＝F_x1′＝F_x2′

由虚功原理推断出雅克比矩阵的应用：

τ^T×Δq+(-F)^T×Δx＝0 (3)

X＝X(q) (4)

此方程描述二连杆机械臂的位移关系，建立关节空间与操作空间的映射关系：

表示接触力处的速度，为关节的速度，J(q)为雅克比矩阵，是关节到接触力处的速度映射的线性转换；得出公式为：

τ＝J^T×F (6)

但是(6)式中的F为相对于基坐标系的外力的分量；综合以上各式可得：

但是(7)式中的F为相对于基坐标系的外力的分量；根据第i个杆相对于基坐标系的齐次变换公式：

得到：F_i＝T_iF＝A₁A₂…A_iF (9)

步骤3：二连杆机械臂运动过程中，通过双编码器采集电机端角度θ_i,连杆端角度q_i,连杆端角速度

步骤4：通过步骤3中的三个参数，对关节接触转矩τ_o和外力矩等效到关节上的力矩τ_ext进行求解，动力学公式如下：

τ_ext＝J^T×F (10)

τ_o＝k_i(θ_i-q_i) (11)

其中F_n为外力，J_i＝diag(J₁J₂…J_n)为电机惯性矩阵，K＝diag(K₁K₂…K_n)为关节刚度矩阵，对角正定；

在(1)式的右边，τ_ot包含了对q执行的所有转矩，即通过双编码器弹性关节和来自外部环境传递的扭矩；其中

τ_ot＝τ_o+τ_ext (12)

当后者不存在时，式12变为

τ_ot＝τ_o＝k_i(θ_i-q_i)

当人与二连杆机械臂发生碰撞时，外部扭矩被作用于二连杆机械臂的每一个关节，因此可以通过观测外部扭矩变化来检测碰撞；

步骤5：根据关节接触转矩τ_o和外力矩等效到关节上的力矩τ_ext的确切数据，对广义动量p求解，由矩阵的斜对称性来进行分析可得：

机器人的总能量包括系统动能T及势能U：

E＝T+U (14)

其中

结合式(1)和式(14)，对式(13)两端同时对t进行求导可得：

式(14)代表机器人整个系统的能量平衡；

根据定义，机器人的广义动量为：

连杆端动力学方程(1)相关的广义动量满足一阶方程：

其中η的分量由式(18)给出：

步骤6：依据动力学模型求出的广义动量p，求解残余矢量r，对残余矢量r的定义为：

r＝K₁[∫(η+τ_o+r)dt-p] (19)

其中K₁为对角矩阵，大于0，剩余矢量满足：

其中为了避免将突变信号误认为是高频噪声，对上述一阶低通滤波结构的观测器进行优化设计，在上述一阶低通滤波结构的基础上串联一个高通滤波结构；最终得到优化的外力观测器方程为：

其中K₁K₂K₃为关节刚度；

步骤7：根据步骤6中所得到的优化的外力观测器方程r来判断是否有外力，如果r＝0，有外力，r≠0，则无外力；

步骤8：设置动态阈值；当出现高于动态阈值的一个或多个残差的突变证明该时刻发生碰撞，采取安全机制停止机械臂；否则没有发生碰撞，不采取安全机制。