[发明专利]一种无传感器式协作机器人碰撞检测方法

权利要求书

1.无传感器式协作机器人碰撞检测方法，其特征在于包括以下步骤：

步骤一、采用拉格朗日方程建立机器人动力学模型，模型的表达式如下：

M ( q ) q ·· + C ( q , q · ) q · + G ( q ) + τ f = τ d - τ c ]]>

τe＝τc+τf

式中：q、分别为机器人待测定的旋转关节的关节角度、角速度、角加速度；M(q)为机器人惯性项系数矩阵，为n×n矩阵，n代表机器人的旋转关节数；代表科氏力和离心力项系数，为n×n矩阵；G(q)为重力项；τf为关节处传动过程中产生的摩擦力矩；τd为关节驱动力矩，近似于待测定关节的电机扭矩乘上该关节的减速比；τc为外部碰撞力等效作用在关节上的力矩，所述的τe为外力矩；

步骤二、求解拉格朗日方程中的机器人惯性项系数矩阵，科氏力和离心力项系数，重力项，具体步骤为：

(a)通过机器人控制系统采集各个关节角度、角速度、角加速度,然后进行算术平均值滤波处理，并通过solidworks软件得到机器人各个关节的惯性参数：质量、质心位置向量、惯性张量矩阵；

(b)通过所述的机器人各个关节的惯性参数计算获得惯性项系数矩阵，科氏力和离心力项系数，重力项的表达式；

步骤三、基于机器人广义动量方程得到外力矩观测器模型，求解外力矩观测器模型中机器人待测定的关节观测值向量r，当机器人发生碰撞时，则待测定的关节观测值向量r近似等于外力矩τe；

所述的外力矩观测器模型为：

式中，P为机器人广义动量，Kr为外力矩观测器的增益矩阵，r初始值为0，r(t)表示t时刻各个关节观测值向量r的值；

步骤四、机器人正常运动没有发生碰撞时，有τc＝0，此时，r≈τe＝τf，利用下式建立摩擦力矩模型以求解机器人待测定的关节传动过程中产生的摩擦力矩τf，得到机器人没有发生碰撞正常运行时的观测值r：

式中，τf为关节摩擦力矩，τh为静摩擦力矩，τs为最大静摩擦力矩，τk为库伦摩擦力矩，a1、a2、a3分别为与速度相关摩擦系数；

通过采集多组机器人没有发生碰撞正常运动过程中的观测值r以及关节速度并通过MATLAB拟合曲线工具箱求解出摩擦力矩模型τf中的静摩擦力矩τh、最大静摩擦力矩τs、库伦摩擦力矩τk以及与速度相关摩擦系数a1、a2、a3；

步骤五、通过公式τc＝r-τf，得到碰撞力作用在关节上的碰撞力矩τc，设置一个阈值τnormal，当|τc|≥|τnormal|时，机器人立刻停止运行。