[发明专利]一种基于速度偏差的机器人碰撞检测方法及装置

权利要求书

1.一种基于速度偏差的机器人碰撞检测方法，其特征在于，包括如下步骤：

1)设置机器人碰撞上限阈值Δω_upper和碰撞下限阈值Δω_lower，Δω为速度偏差；

2)设置机器人的运动轨迹、速度、加速度；

3)根据所选取的运动轨迹规划算法求解机器人的理想运动速度ω_ideal；

4)将理想运动速度ω_ideal通过滤波器的处理得到优化速度ω′_ideal；

5)获取机器人运动轨迹的实时测量值；

6)对机器人的运动轨迹求微分并带入实时测量值，得到机器人实际速度ω_real；

7)将优化速度ω′_ideal与实际速度ω_real相减得到所述速度偏差Δω：

Δω＝ω′_ideal-ω_real； (1)

8)判断是否发生碰撞，如果Δω＞Δω_upper或Δω＜Δω_lower，则认为碰撞发生；

9)检测到碰撞发生后，机器人采取相应措施避免碰撞对机器人本体或其周围环境造成严重损害；

步骤4中的所述滤波器有k个，k为大于1的整数，

ω′_ideal＝η₁ω′_ideal1+η₂ω′_ideal2+…+η_kω′_ideal(k) (7)

其中，ω′_ideal1、ω′_ideal2、……ω′_ideal(k)分别为第1、第2、……第k个滤波器处理后的优化速度，η₁，η₂，…η_k分别为第1、第2、……第k个滤波器所占的权重，有η1+η₂+…η_k＝1。

2.根据权利要求1所述的方法，其特征在于：步骤2中机器人的运动轨迹、速度、加速度分别设置为机器人各个关节的关节角、角速度、角加速度。

3.根据权利要求2所述的方法，其特征在于，步骤3中的运动轨迹规划算法选择五次多项式插值函数：

θ(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (2)

其一阶和二阶导数为：

上式中，θ(t)、分别为机器人的关节角、角速度、角加速度；a₀、a₁、a₂、a₃、a₄、a₅为多项式的系数；t为时间变量；

对于其中一段关节角的运动轨迹，根据机器人各个关节角轨迹初始状和终末位置、角速度、角加速度状态量，反解出五次多项式插值函数的6个系数a₀、a₁、a₂、a₃、a₄、a₅如下：

其中t_f为终末时间，根据(5)式所得系数，求解该段运动轨迹中机器人关节理想运动速度为：

上式(6)中的a₁、a₂、a₃、a₄、a₅5个系数分别由式(5)代替。

4.根据权利要求2所述的方法，其特征在于，步骤3中的运动轨迹规划算法可选择为三次多项式插值算法、高阶多项式插值算法、三次样条插值算法或用抛物线过渡的线性插值算法。