一种基于速度偏差的机器人碰撞检测方法及装置

摘要

一种基于速度偏差的机器人碰撞检测方法及装置，具体采用对理想计算速度进行滤波优化，利用滤波器的滞后性处理理想计算速度；使其与实际速度相减得到优化速度偏差；设置机器人碰撞检测上下限阈值；当检测到优化速度偏差超过阈值时控制机器人停止运动或采取其他规避算法避免碰撞带来的人身伤害和机器人机械损害。通过上述设置，提高检测灵敏性，从而避免机器人加减速干扰碰撞检测。所述方法简单有效，不需要增加额外的力传感器，也不需要检测加速度、电流、力矩等其他参数，算法中也不包含复杂的机器人动力学运算。实验结果表明，该方法可以有效、灵敏、准确地检测到碰撞，以此为基础可控制机器人立即停止运动而避免对人和机器人本身造成损害。

说明书

技术领域

本发明涉及机器人控制技术领域，具体涉及一种机器人碰撞检测的方法及装置。

背景技术

机器人可以代替人执行高效、高精度重复性体力劳动。机器人工作时，如果和周围环境发生碰撞，可能造成周围环境或机器人本体损坏。在一些复杂精密的任务中，往往需要机器人和工人协同配合工作。这时，机器人的安全性尤为重要。机器人需要采取必要的安全防碰撞措施来保证人及机器人本体的安全。在碰撞发生时，机器人要能检测到碰撞并采取一定的控制策略避免碰撞造成严重后果。

目前，一些学者从多个角度出发提出了不同的碰撞检测方案。如在机器人表面包裹皮肤传感器。该方法能精确检测出碰撞及碰撞部位，但是大大增加了机器人的复杂度与成本，降低了灵活性。有的方案在机器人关节处增加力传感器。该方案增加了机器人成本并且只适用于装配了力传感器的机器人轴。有学者提出利用视觉传感器观测碰撞。该方案中图像处理信息量巨大、实时性差，并且视觉传感器一般都具有盲区。

有学者提出通过对比机器人关节位置指令值与实际关节位置值的误差来判断是否发生碰撞的方案。有的学者更进一步提出通过对机器人关节位置指令通过算法处理后与实际关节位置值对比的方案。事实上，机器人关节电机伺服控制的位置环处于机器人伺服的最外环。机器人关节位置指令值与实际关节位置值的误差不“灵敏”。由于碰撞造成的位置指令值与实际关节位置值的误差非常小，十分容易被其他噪声掩盖而无法判断机器人是否发生了碰撞。该方案的可行性低。

此外，还有学者提出根据机器人动力学计算力矩和实际采样力矩的对比检测碰撞以及基于能量、动量的检测方案。这些方案或是计算复杂，或是实时性，精度不高。

而目前尚未见到有学者提出基于对机器人速度进行处理来判断碰撞检测的相关方案。

发明内容

本发明的目的在于针对现有机器人碰撞检测方案具有增加额外传感器或计算复杂或实时性不高等缺点，提供一种不需额外增加传感器、简单有效的基于速度偏差的机器人碰撞检测方法和装置。

本发明通过如下技术方案实现：

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

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

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

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

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

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

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

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

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

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

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

进一步地，步骤2中机器人的运动轨迹、速度、加速度分别设置为机器人各个关节的关节角、角速度、角加速度。

进一步地，步骤3中的运动轨迹规划算法选择五次多项式插值函数：

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

其一阶和二阶导数为：

上式中，θ(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)代替。

进一步地，步骤3中的运动轨迹规划算法可选择为三次多项式插值算法、高阶多项式插值算法、三次样条插值算法、用抛物线过渡的线性插值算法。

进一步地，该方法适用于机器人离线轨迹规划或机器人在线轨迹规划；在离线轨迹规划中，通过滤波器处理得到所述的优化速度然后将优化速度数据存储于上位控制器中，实时与实际速度进行比较，用于判断碰撞是否发生；在线轨迹规划中，上位控制器根据轨迹规划指令，通过滤波器处理得到所述的优化速度并与实际速度进行比较，用于判断碰撞是否发生。

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

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

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

本发明的另一个方面提供了一种实现上述方法的机器人控制装置，所述控制装置包括上位控制器和伺服系统；

所述上位控制器用于机器人轨迹规划、系统层级信号处理、上层算法实现和人机交互；

伺服系统包括位置环控制器、速度环控制器和电流环控制器，所述位置环控制器、速度环控制器和电流环控制器依次相连，用于调节伺服电机；

所述控制装置还包括滤波器，连接至上位控制器，理想运动速度ω_ideal通过滤波器的处理得到优化速度ω′_ideal。

进一步地，所述滤波器有k个，k为大于等于1的整数，

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

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

本发明的上述技术方案具有如下有益的技术效果：

1)利用滤波器的滞后性处理理想计算速度，使其与实际速度能很好的切合。这样可以减小机器人正常工作时的速度偏差，提高检测灵敏性，从而避免机器人加减速干扰碰撞检测；

2)选取速度作为检测对象，计算方法简单有效，不需要增加额外的传感器，不需要检测加速度，电流，力矩等，也不需要进行复杂的机器人动力学运算；

3)设置机器人碰撞检测上下限阈值；当检测到优化速度偏差超过阈值时控制机器人停止运动或采取其他规避算法避免碰撞带来的人身伤害和机器人机械损害；

4)简化机器人碰撞检测系统，降低机器人成本，促进机器人技术的发展。

附图说明

图1是现有技术中机器人控制系统示意图；

图2是基于速度偏差的机器人控制系统示意图；

图3是理想速度偏差下的碰撞实验结果图；

图4是优化速度偏差算法框图；

图5是优化速度偏差下的碰撞实验结果图；

图6是基于优化速度偏差的机器人控制系统示意图；

图7是实际速度偏差与优化速度偏差下的碰撞实验结果对比图；

图8是多个滤波器优化速度的机器人控制系统示意图。

附图标记：

1：上位控制器；2：位置环控制器；3：速度环控制器；4：电流环控制器；5：伺服系统；6：滤波器。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

本发明的目的在于针对现有机器人碰撞检测方案具有增加额外传感器或计算复杂或实时性不高等缺点，提供一种不需额外增加传感器、简单有效的机器人碰撞检测的方案。一般而言，一台机器人具有多个自由度，而每个自由度的控制系统如图1所示。上位控制器主要负责机器人轨迹规划、系统层级信号处理、上层算法实现、人机交互等功能。伺服系统主要包括位置环、速度环、电流环，调节伺服电机运行于最优性能状态。上位控制器在做机器人轨迹规划时可以有各种各样的轨迹规划算法，本发明均适用。以一段轨迹为例，对机器人的始末位姿求运动学逆解得到机器人关节的初始值θ(0)和终值θ(t_f)。算法中可以采用三次多项式插值函数、三次样条插值、高阶多项式插值函数、用抛物线过渡的线性插值函数等关节轨迹插值函数，以插值函数为五次多项式函数为例：

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

其一阶和二阶导数也都是光滑的多项式：

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

机器人运行轨迹的位置、速度、加速度对多项式函数(1)构成了6个约束条件。将机器人关节轨迹初始状态量t＝0时和终末状态量t＝t_f时带入6个约束条件得如下式：

根据式(4)，可反解出五次多项式插值函数的6个系数a₀、a₁、a₂、a₃、a₄、a₅分别为：

故在该段轨迹中，机器人关节理想运动速度为：

根据上述算法，无论是通过离线计算，还是在线实时计算均可得到机器人关节在某一运动轨迹下各个时刻的理想计算速度ω_ideal。

考虑到机器人发生碰撞时，实际轨迹相比规划的轨迹会发生偏移，而其实际速度也会偏离理想计算速度，本发明提出了基于实际与理想计算速度偏差判断机器人是否发生碰撞的方法。

如图2所示，在图1所示的机器人控制系统的基础上，增加了在上位控制器中实时读取机器人关节实际速度值ω_real，并且对实际值ω_real与通过上述介绍的算法求解的ω_ideal做差比较得速度偏差Δω为：

Δω＝ω_ideal-ω_real>

图3展示的是理想速度偏差下的碰撞实验结果，机器人碰撞实验中，机器人某一关节的理想计算速度、实际速度及理想速度和实际速度的偏差。可以看到，在机器人匀速运动时，ω_real与ω_ideal的偏差很小，几乎为0。但是在加减速过程中，ω_real与ω_ideal的偏差将会变得非常大，甚至比碰撞造成的速度偏差还要大。这导致了控制算法不能区分速度偏差是由于机器人加减速造成的，还是由于碰撞造成的。造成这种速度偏差的原因是上位控制器在给出位置指令之后，需要经过伺服系统的计算延迟、控制延迟、执行延迟、采样延迟等才能使机器人到达指定位置。这使得实际速度值滞后于理想计算速度值。

为了解决上述问题，本发明提出了一种基于优化的速度偏差进行机器人碰撞检测来避免机器人加减速造成的速度偏差对检测的干扰。具体做法如图4所示，上位控制器计算出机器人理想运行速度后并不直接与该时刻的实际速度进行比较，而是使ω_ideal通过滤波器得到优化速度ω′_ideal后与实际速度ω_real比较得到优化速度偏差Δω。

Δω＝ω′_ideal-ω_real(8)

经过本发明所提的滤波器优化处理后的优化速度ω′_ideal与实际速度ω_real契合得非常好。即使在机器人加减速过程中，速度偏差也能保持很小值。这样，可以设置更小的碰撞检测阈值，从而使机器人的碰撞检测更为灵敏。如图5所示，设置碰撞检测上下限阈值Δω_upper、Δω_lower，当速度偏差Δω超过上下限阈值时，即Δω＞Δω_upper或Δω＜Δω_lower时，控制机器人停止运动或进行相应的规避算法从而避免碰撞给人或机器人本体造成严重的损害。

因此，系统的整体控制框图如图6所示，在现有的机器人控制系统的基础上增加滤波器，对理想速度ω_ideal通过滤波优化后得到优化速度ω′_ideal，在与实际速度ω_real相减得到速度偏差Δω，通过与碰撞检测上下限阈值进行比较来判断是否发生碰撞。

图7示出实际速度偏差与优化速度偏差的对比图，可以看出，在没有经过滤波器优化的实际速度偏差值波动较大，湮没了真正发生碰撞时刻的速度偏差，因此很难判断机器人是处于正常加减速状态下还是发生了碰撞。相反，经过滤波器优化后的优化速度与实际速度比较后的速度偏差值，在机器人正常运行状态下，都处于碰撞检测上下限范围内，当发生碰撞后，速度偏差值显著增大，超过了碰撞检测的上下限范围，因此能够准确检测出是否有碰撞发生，为进一步控制机器人停止运动或进行相应的规避运动提供了基础，从而避免碰撞给人或机器人本体造成严重的损害。

基于上述内容，本发明提供了一种基于速度偏差的机器人碰撞检测方法，所述方法包括如下步骤：

1)设置机器人关节碰撞上限阈值Δω_upper，碰撞下限阈值Δω_lower；

2)根据工作需要，由工作人员或机器人自主设计机器人运动轨迹、速度、加速度；

3)根据所用的轨迹规划算法求解机器人各个关节理想运动速度。以五次多项式插值函数为例，对于其中一段轨迹，根据机器人各个关节轨迹初始状和终末位置、速度、加速度状态量，反解出五次多项式插值函数的6个系数a₀、a₁、a₂、a₃、a₄、a₅：

4)根据上式所得系数，求解该段轨迹中机器人关节理想运动速度：

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

6)经过电机编码器测量，算法处理后得到机器人实时关节角度θ；

7)对机器人关节角度求微分得到机器人关节实际速度值ω_real；

8)对优化速度ω′_ideal与实际速度ω_real做差得到速度偏差Δω

Δω＝ω′_ideal-ω_real；

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

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

本发明的另一个方面提供了一种实现上述方法的机器人控制装置，所述控制装置包括上位控制器和伺服系统；所述上位控制器用于机器人轨迹规划、系统层级信号处理、上层算法实现和人机交互；伺服系统包括位置环控制器、速度环控制器和电流环控制器，所述位置环控制器、速度环控制器和电流环控制器依次相连，用于调节伺服电机；所述控制装置还包括滤波器，连接至上位控制器，理想运动速度ω_ideal通过滤波器的处理得到优化速度ω′_ideal。

本发明适用于各种轨迹规划算法，上述是以五次多项式作为插值函数为例，还可以选择三次多项式插值算法、高阶多项式插值算法、三次样条插值算法、用抛物线过渡的线性插值算法等等。本发明所提算法既适用于离线轨迹规划案例，也可应用于在线轨迹规划案例。在离线轨迹规划中，可借助如MATLAB的第三方软件根据上述方法计算所提的优化速度。然后将优化速度数据存储于上位控制器中，实时与实际速度进行比较，用于判断碰撞。离线轨迹规划案例可节省上位控制占用率。在线轨迹规划案例中，需要上位控制器根据轨迹规划指令，利用上述方法实时计算所提的优化速度，并与实际速度进行比较，用于判断碰撞。不同的案例只是计算的平台不一样，方法所需具体步骤均一致。

图6中滤波器的个数以及类型可根据实际工况与需求进行选择。一般要求滤波器具有滞后性、低通或带通性等。如可使用一阶低通滤波器、巴特沃斯滤波器、契比雪夫滤波器。不同的滤波器具有不同性能，针对不同的工况与需求，可对各个滤波器设计不同的权重系数，如图8所示，即设计各个滤波器对最终优化速度的处理贡献。

以本发明的验证实验为例，该实验使用了一阶数字低通滤波器与巴特沃斯滤波器对理想速度ω_ideal进行处理。

一阶数字低通滤波器的处理算法如下：

ω′_ideal1(i)＝(1-α)ω′_ideal1(i-1)+αω_ideal(i)(9)

其中，滤波平滑系数T_s为机器人系统运行最小周期，T_ω为周期的设计参数，可根据实际情况进行调节；ω_ideal(i)为第i个理想计算速度，i＝1…m，机器人完成某一动作共有m个理想计算速度目标设定值；ω′_ideal1(i-1)为第(i-1)个优化速度，ω′_ideal1(i)为第i个优化速度。

巴特沃斯滤波器幅频特性模的平方为：

其中，Ω为频率符号，单位为rad/s，j为虚数单位，n为滤波器的阶数，Ω_c为滤波器的截至频率。

本实验采用二阶巴特沃斯滤波器。其数字差分方程如下：

ω′_ideal2(i)＝ε₁ω_ideal(i)+ε₂ω_ideal(i-1)+ε₃ω_ideal(i-2)-β₁ω′_ideal2(i-1)-β₂ω′_ideal2(i-2)

其中，ω_ideal(i)、ω_ideal(i-1)、ω_ideal(i-2)为第i，(i-1)，(i-2)个理想计算速度，ω′_real2(i-1)、ω′_real2(i-2)分别为第(i-1)，(i-2)个优化速度，ε₁、ε₂、ε₃、β₁、β₂分别为设计参数。

根据判断机器人碰撞算法整体框图，设计好需要的各个滤波器所占的权重而得到最终的优化速度为：

ω′_ideal(i)＝η₁ω′_ideal1(i)+η₂ω′_ideal2(i)(11)

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

当然，所述滤波器的个数可以有k个，k为大于等于1的整数，则优化速度为：

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

其中，ω′_ideal1、ω′_ideal2、……ω′_ideal(k)分别为第1、第2、……第k个滤波器处理后的优化速度，η₁，η₂，…η_k分别为第1、第2、第k个滤波器所占的权重，有η₁+η₂+…η_k＝1；每个滤波器的权重根据实际需要进行设置。

综上所述，本发明提供了一种基于速度偏差的机器人碰撞检测方法及装置，所述方法采用对理想计算速度进行滤波优化，利用滤波器的滞后性处理理想计算速度；使其与实际速度相减得到优化速度偏差；设置机器人碰撞检测上下限阈值；当检测到优化速度偏差超过阈值时控制机器人停止运动或采取其他规避算法避免碰撞带来的人身伤害和机器人机械损害。本发明适用于各种机器人，适用于离线轨迹规划和在线轨迹规划案例，适用于各种轨迹规划算法，机器人控制策略。本发明不需要增加额外的传感器，不需要检测加速度，电流，力矩等，也不需要进行复杂的机器人动力学运算；不会对机器人已有控制策略造成影响，不会增加机器人控制延时等。实验结果表明，该方法可以有效、灵敏、准确地检测到碰撞，以此为基础可控制机器人立即停止运动而避免对人和机器人本身造成损害。

应当理解的是，本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制。因此，在不偏离本发明的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。