一种基于协方差的碰撞检测模块的检测方法及装置与流程

一种基于协方差的碰撞检测模块的检测方法及装置
【技术领域】
1.本发明涉及电机碰撞检测领域，特别涉及一种基于协方差的碰撞检测模块检测方法及装置。


背景技术：

2.现今交流永磁伺服电机大量用于机械设备，机器人等领域，在使用的过程中难免会遇到障碍物导致碰撞的发生。如果不及时检测到碰撞的发生并给予保护处理，会导致更严重的后果，例如其他设备的损坏，甚至导致驱动和电机的损坏。
3.目前，在机器人碰撞检测方面也已提出了很多的方法。其中较为普遍的方法是在机械臂上安装各种传感器来检测碰撞的发生，例如腕部传感器、视觉传感器、感知皮肤等，安装传感器虽然可以快速检测到碰撞，但它同时会增加机器人的生产成本以及系统的复杂性。考虑到这些问题的存在，也有学者提出了使用无传感的方法来检测碰撞。例如有人提出利用机械臂关节电机的扭矩输出与建立的数学模型估计的扭矩输出进行比较，计算两者之间的误差，进而为检测系统设置阈值。如果误差超过预定阈值，则机器人断定此时受到碰撞。
4.申请公布号为cn11212333a的发明申请为通过采集一遍正常运行时的轨迹电流然后保存下来，作为后续运动的参考电流，与实际电流对比超过一定的阈值则判断为碰撞，该方法通用性较低，每次运行不同的轨迹需要重新采集实际电流参考。
5.申请公布号为cn109940622a的发明申请为通过搭建神经网络并应用反向传播算法更新神经网络的权重和偏差，得到估计的电流值；根据机械臂关节电机输出电流与神经网络输出的估计电流之间的误差值与碰撞检测阈值比较用于碰撞判定。该方法计算量大不适合嵌入式微处理器使用。
6.申请公布号为cn113021353a的发明申请为利用机器人关节电机电流i可分解为实现动力学运动的电机电流分量i1，克服关节摩擦的电机电流分量i2，以及碰撞所致的电机电流分量i3的原理，当碰撞所致的电机电流分量i3超过碰撞阈值，则判定机器人该关节连杆发生碰撞。该方法计算量大不适合嵌入式微处理器使用，而且针对不同设备需要重新建模，通用性低。


技术实现要素：

7.本发明的旨在解决上述问题而提供一种基于协方差的碰撞检测模块检测方法及装置，解决现有技术需要在伺服电机上增加传感器，使用成本高，对于不同的机器人，需要设备重新建模，而且针对不同的负载又要对模型参数进行重新的辨识，不符合伺服电机通用性的要求，且计算量大，不适合用于嵌入式微处理器的问题。
8.为达此目的，本发明采用以下技术方案：
9.一种基于协方差的碰撞检测模块检测方法，包括以下步骤：
10.s1：采集电机三相电流数据，通过clark变换和park变换得到交轴电流iq和直轴电
流id；
11.采集电机编码器值，进行二次微分得到反馈加速度a；
12.s2：将所述碰撞检测模块与运动控制器连通，通过所述运动控制器对所述碰撞检测模块下发转动惯量m，如无下发则默认所述转动惯量为1；
13.s3：对所述交轴电流iq和所述反馈加速度a进行低通滤波处理，得到所需的滤波交轴电流iq2和滤波反馈加速度a2；
14.s4：缓存最近n个周期的转动惯量m(n)、滤波交轴电流iq2(n) 和滤波反馈加速度a2(n)，并对上述缓存的n个周期的转动惯量m (n)、滤波交轴电流iq2(n)和滤波反馈加速度a2(n)按固定间隔 n进行数据抽取得到转动惯量m(n/n)、滤波交轴电流iq2(n/n)和滤波反馈加速度a2(n/n)；
15.s5：将s4中抽取的转动惯量m(n/n)、滤波交轴电流iq2(n/n) 和滤波反馈加速度a2(n/n)进行协方差计算，得到所需的协方差值 r；
16.s6：将所得的协方差值r与预先设定得协方差阈值t进行比较，若所述协方差值r小于所述协方差阈值t，则判定为碰撞。
17.优选的，所述步骤s1中是通过clark变换将静止的三相电流ia、 ib和ic变换成静止的两相电流iα和iβ，所述电流iα和iβ再通过park变换得到交轴电流iq和直轴电流id。
18.优选的，所述clark变换的公式如下：
19.iα＝ia；
20.iβ＝(iα+2*ib)/√3；
21.优选的，所述park变换的公式如下：
22.iq＝iβ*cosθ-iα*sinθ；
23.id＝iα*cosθ+iβ*sinθ；
24.其中θ为旋转的角度。
25.优选的，所述反馈加速度a的计算公式如下：
26.a＝d^2p/dt^2；
27.其中p为所述编码器值的位置，t为采样周期。
28.优选的，所述滤波交轴电流iq2和所述滤波反馈加速度a2的计算公式如下：
29.iq2＝x*iqact+(1-x)*iqref；
30.a2＝y*aact+(1-y)*aref；
31.其中iqact和aact均为当前周期数值，iqref和aref为上一周期数值，x和y均为滤波系数。
32.优选的，所述协方差的公式如下：
33.x＝iq2(n/n)*m(n/n)；
34.y＝a2(n/n)；
35.cov(x,y)＝e{[x-e(x)]*[y-e(y)]}。
[0036]
一种基于协方差的碰撞检测模块检测装置，包括运动控制器、伺服驱动、三环控制模块、反馈模块和上述所述的碰撞检测模块。
[0037]
优选的，所述运动控制器的输出端分别与所述伺服驱动和所述碰撞检测模块电联接，所述伺服驱动的输出端与所述三环控制模块的输入端电联接，所述三环控制模块的输
出端与所述反馈模块电联接，所述反馈模块的输出端分别于所述三环控制模块和所述碰撞检测模块的输入端电联接，所述碰撞检测模块的输出端与所述伺服驱动的输入端电联接。
[0038]
优选的，所述三环控制模块包括位置环、速度环和电流环；
[0039]
所述反馈模块包括电机、位置反馈模块、速度反馈模块和电流反馈模块；
[0040]
所述位置环、速度环和电流环的输入端均与所述伺服驱动电联接，所述位置环、速度环和电流环的输出端均与所述电机的输入端电联接，所述电机的输出端分别与所述位置反馈模块、速度反馈模块和电流反馈模块的输入端电联接，所述位置反馈模块的输出端分别与所述位置环和所述碰撞检测模块的输入端电联接，所述速度反馈模块的输出端与所述速度环的输入端电联接，所述电流反馈模块的输出端分别与所述电流环和所述碰撞检测模块的输入端电联接。
[0041]
本发明的贡献在于：本发明通过协方差计算反馈电流、反馈加速度和转动惯量来判断相关性，进而检测出碰撞是否发生，无需在电机对的伺服驱动上添加任何额外的传感器，很好的节省了成本，且针对不同的负载无需重新建模，无需融入动力学建模及参数辨识，具有较强的通用性和较小的计算量，适合嵌入式这种计算量不大的微处理器使用，增加转动惯量的输入，从而使得运动过程中即使转动惯量改变，也不影响对碰撞的判断，并可以扩展其应用范围至多轴联动机器人。
【附图说明】
[0042]
图1是本发明碰撞检测模块的流程图；
[0043]
图2是本发明检测装置的简略示意图；
[0044]
图3是本发明检测装置的使用示意图；
【具体实施方式】
[0045]
下列实施例是对本发明的进一步解释和补充，对本发明不构成任何限制。
[0046]
一种基于协方差的碰撞检测模块检测方法，包括以下步骤：
[0047]
s1：采集电机三相电流数据，通过clark变换和park变换得到交轴电流iq和直轴电流id；
[0048]
采集电机编码器值，进行二次微分得到反馈加速度a；
[0049]
s2：将所述碰撞检测模块与运动控制器连通，通过所述运动控制器对所述碰撞检测模块下发转动惯量m，如无下发则默认所述转动惯量为1；
[0050]
s3：对所述交轴电流iq和所述反馈加速度a进行低通滤波处理，得到所需的滤波交轴电流iq2和滤波反馈加速度a2，并将所述滤波交轴电流iq2和所述滤波反馈加速度a2输入所述碰撞检测模块；
[0051]
s4：缓存最近n个周期的转动惯量m(n)、滤波交轴电流iq2(n) 和滤波反馈加速度a2(n)，并对上述缓存的n个周期的转动惯量m (n)、滤波交轴电流iq2(n)和滤波反馈加速度a2(n)按固定间隔 n进行数据抽取得到转动惯量m(n/n)、滤波交轴电流iq2(n/n)和滤波反馈加速度a2(n/n)；
[0052]
s5：将s4中抽取的转动惯量m(n/n)、滤波交轴电流iq2(n/n) 和滤波反馈加速度a2(n/n)进行协方差计算，得到所需的协方差值 r；
[0053]
s6：将所得的协方差值r与预先设定得协方差阈值t进行比较，若所述协方差值r小于所述协方差阈值t，则判定为碰撞。
[0054]
本发明通过电机的碰撞检测模块来检测判断是否发生碰撞，具体的，如图1所示，首先是通过采集三相电流的数据，然后通过clark 变换将静止的三相电流ia、ib和ic变换成静止的两相电流iα和i β，再通过park变换将静止的两相电流iα和iβ变换得到交轴电流 iq和直轴电流id，同时对电机的编码器值进行采集，并对其进行二次微分得到反馈加速度a，在采集数据的同时，通过运动控制器对碰撞检测模块下发转动惯量，并对交轴电流iq和反馈加速度a进行低通滤波处理，得到滤波交轴电流iq2和滤波反馈加速度a2，缓存最近 n个周期的转动惯量m(n)、滤波交轴电流iq2(n)和滤波反馈加速度a2(n)，并对上述缓存的n个周期的转动惯量m(n)、滤波交轴电流iq2(n)和滤波反馈加速度a2(n)按固定间隔n进行数据抽取得到转动惯量m(n/n)、滤波交轴电流iq2(n/n)和滤波反馈加速度a2(n/n)，其中n为小于或等于n的值，将s4中抽取的转动惯量 m(n/n)、滤波交轴电流iq2(n/n)和滤波反馈加速度a2(n/n)进行协方差计算，得到所需的协方差值r，将协方差值r与协方差阈值 t进行比较，从而判断是否发生碰撞，具体为，当协方差值r小于协方差阈值t时判断为发生碰撞，当协方差值r大于协方差阈值t时判断为未发生碰撞。
[0055]
如图2-3所示，图2和图3为装置的具体使用示例，从图中可以看出运动控制器首先对伺服驱动发送位置和控制字，同时运动控制器对碰撞检测模块下发一个转动惯量(如无下发则默认转动惯量为1)，伺服驱动在接收到运动控制器的位置和控制字后对三环控制模块中的位置环、速度环和电流环进行控制，位置环，速度环和电流环通过计算后，输出相应的三相电压，进而对电机进行控制。通过电机位置采样，三相电流采样得到位置反馈速度反馈和电流反馈，其中速度反馈模块直接作用于速度环上，对速度进行控制，位置反馈模块和电流反馈模块均分别作用于其相应的位置环和电流环上，且同时将反馈的信号传递给碰撞检测模块，碰撞检测模块在接受到位置反馈(二次微分得到反馈加速度)、电流反馈(通过clark和park变换后再进行滤波处理后得到所需交轴电流)和运动控制器所发送的转动惯量后进行协方差的计算处理(处理过程如图1所示)，从而得到协方差值，再将协方差值与协方差阈值比较，即可判断电机是否发生碰撞，并将判断出来的结果传递给伺服驱动进行保护及使能逻辑处理。
[0056]
本发明通过协方差计算反馈电流、反馈加速度和转动惯量来判断相关性，进而检测出碰撞是否发生，无需在电机对的伺服驱动上添加任何额外的传感器，很好的节省了成本，且针对不同的负载无需重新建模，无需融入动力学建模及参数辨识，具有较强的通用性和较小的计算量，适合嵌入式这种计算量不大的微处理器使用，增加转动惯量的输入，从而使得运动过程中即使转动惯量改变，也不影响对碰撞的判断，并可以扩展其应用范围至多轴联动机器人。
[0057]
更进一步的说明，所述步骤s1中是通过clark变换将静止的三相电流ia、ib和ic变换成静止的两相电流iα和iβ，所述电流i α和iβ再通过park变换得到交轴电流iq和直轴电流id。
[0058]
更进一步的说明，所述clark变换的公式如下：
[0059]
iα＝ia；
[0060]
iβ＝(iα+2*ib)/√3；
[0061]
更进一步的说明，所述park变换的公式如下：
[0062]
iq＝iβ*cosθ-iα*sinθ；
[0063]
id＝iα*cosθ+iβ*sinθ；
[0064]
其中θ为旋转的角度。
[0065]
θ为旋转的角度，也称为电角度，电角度等于机械角度乘以极对数，空间电角度就是电角度。
[0066]
更进一步的说明，所述反馈加速度a的计算公式如下：
[0067]
a＝d^2p/dt^2；
[0068]
其中p为所述编码器值的位置，t为采样周期。
[0069]
更进一步的说明，所述滤波交轴电流iq2和所述滤波反馈加速度 a2的计算公式如下：
[0070]
iq2＝x*iqact+(1-x)*iqref；
[0071]
a2＝y*aact+(1-y)*aref；
[0072]
其中iqact和aact均为当前周期数值，iqref和aref为上一周期数值，x和y均为滤波系数。
[0073]
更进一步的说明，所述协方差的公式如下：
[0074]
x＝iq2(n/n)*m(n/n)；
[0075]
y＝a2(n/n)；
[0076]
cov(x,y)＝e{[x-e(x)]*[y-e(y)]}。
[0077]
一种基于协方差的电机碰撞检测模块检测装置，包括运动控制器、伺服驱动、三环控制模块、反馈模块和上述所述的碰撞检测模块。
[0078]
更进一步的说明，所述运动控制器的输出端分别与所述伺服驱动和所述碰撞检测模块电联接，所述伺服驱动的输出端与所述三环控制模块的输入端电联接，所述三环控制模块的输出端与所述反馈模块电联接，所述反馈模块的输出端分别于所述三环控制模块和所述碰撞检测模块的输入端电联接，所述碰撞检测模块的输出端与所述伺服驱动的输入端电联接。
[0079]
更进一步的说明，所述三环控制模块包括位置环、速度环和电流环；
[0080]
所述反馈模块包括电机、位置反馈模块、速度反馈模块和电流反馈模块；
[0081]
所述位置环、速度环和电流环的输入端均与所述伺服驱动电联接，所述位置环、速度环和电流环的输出端均与所述电机的输入端电联接，所述电机的输出端分别与所述位置反馈模块、速度反馈模块和电流反馈模块的输入端电联接，所述位置反馈模块的输出端分别与所述位置环和所述碰撞检测模块的输入端电联接，所述速度反馈模块的输出端与所述速度环的输入端电联接，所述电流反馈模块的输出端分别与所述电流环和所述碰撞检测模块的输入端电联接。
[0082]
尽管通过以上实施例对本发明进行了揭示，但本发明的保护范围并不局限于此，在不偏离本发明构思的条件下，对以上各构件所做的变形、替换等均将落入本发明的权利要求范围内。