一种机器人防碰撞检测方法、设备及存储介质技术

本发明专利技术公布一种机器人防碰撞检测方法，通过建立动力学模型，获取指定轨迹位置的速度、加速度及力矩，采用最小二乘法识别动力学参数，机器人运行中同时获取机器人的实时位置、速度、加速度及力矩，采用非线性卡尔曼滤波算法计算外力观察矩阵，并与第一残差力矩向量相乘之后得到第二残差力矩向量，并判断第二残差力矩向量是否为零，从而判断机器人是否发生碰撞，进一步根据向量分布估计发生碰撞的具体位置。采用本发明专利技术的防碰撞检测方法，该技术方案无需进行传统的防碰撞检测方案的阈值测试，直接可以给出发生碰撞结果，适合任何机器人运行轨迹，对动力学参数模型精度要求不高，抗干扰性好，可以估算判断机器人发生在某个位置上。可以估算判断机器人发生在某个位置上。可以估算判断机器人发生在某个位置上。

【技术实现步骤摘要】
一种机器人防碰撞检测方法、设备及存储介质


[0001]本专利技术涉及医疗器械领域，具体涉及到一种机器人防碰撞检测方法、设备及存储介质。

技术介绍

[0002]在工业领域尤其是医疗器械领域，机器人的应用正在加速推广，现有的机器人已经发展为多机械臂，由此带来的直接问题就是多机械臂之间的相互配合及防碰撞。
[0003]传统的机械臂防碰撞机制基于动力学模型计算实时力矩和反馈力矩之差，进一步设定阈值大小来判断机器人是否发生碰撞，而碰撞检测的阈值需要测试，根据阈值来判断碰撞发生与否，机器人运行轨迹不同则阈值不同，因此，传统的碰撞检测系统要求的动力学参数模型精度要高，抗干扰性差，无法判断机器人发生碰撞的位置，且受限机器人的运行轨迹。
[0004]中国专利申请202211246926X提供一种机器人及其机械臂的碰撞检测方法、控制装置，该机器人包括：第一、第二机械臂；及控制装置，其被配置成：基于第一、第二机械臂的结构特征，将第一机械臂抽象并分割形成第一空间线段、将第二机械臂抽象并分割形成第二空间线段；获取第一空间线段与所述第二空间线段之间的最短距离；获取第一空间线段与第二空间线段之间的安全距离，安全距离基于第一空间线段的第一膨胀半径和第二空间线段的第二膨胀半径之和被配置；比较最短距离与安全距离，基于比较获得的结果确定第一机械臂与第二机械臂之间是否发生碰撞。
[0005]上述现有技术的碰撞检测精度有待提高，且基于最短距离和安全距离的检测方案并不能真正实现防碰撞。
[0006]因此，有必要开发一种新的防碰撞检测方法及设备，实现无需测试阈值即可实现防碰撞检测，且对动力学参数模型精度要求不高，抗干扰性较好。

技术实现思路

[0007]本专利技术提供一种机器人防碰撞检测方法、设备及存储介质：
[0008]第一方面，本专利技术提供一种机器人防碰撞检测方法，包括以下步骤：
[0009]步骤101：建立关节机器人动力学方程；
[0010]步骤102：对动力学方程进行线性化简；
[0011]步骤103：采集各关节的指定位置、速度、加速度及力矩；
[0012]步骤104：采用最小二乘法进行动力学参数识别；
[0013]步骤105：进行卡尔曼滤波处理，获取动力学参数；
[0014]步骤106：启动并运行机器人；
[0015]步骤107：采集各关节的实时位置、速度、加速度及力矩；
[0016]步骤108：获取关节的实时位置、速度及加速度，将获取的机器人各关节的实时位置、速度、加速度为状态矢量状态方程，计算的力矩作为观察方程；
[0017]步骤109：获取采集的实时关节力矩；
[0018]步骤110：进一步计算关节力矩；
[0019]步骤111：根据步骤109的输出结果计算估计协方差矩阵：
[0020]步骤112：计算卡尔曼增益矩阵K；
[0021]步骤113：根据步骤106和步骤110的输出结果进一步获取第一残差力矩向量；
[0022]步骤114：计算外力观察矩阵J；
[0023]步骤115：根据步骤113和114的输出进一步计算第二残差力矩向量T；
[0024]步骤116：根据T值大小判断机器人是否发生碰撞，若T值为零向量T＝{0，
…
0}，则未发生碰撞，继续返回步骤107继续运行机器人；若T值为非零向量T＝{T1，
…
T
n
}，即T1，
…
T
n
至少有一个不为零，则机器人发生碰撞，由步骤117立即停止运行机器人。
[0025]进一步的，所述关节机器人具有N自由度，所述动力学方程由下列公式1表示：
[0026][0027]式中：τ∈R
n
是控制力矩输入向量，τ
fi
∈R
n
是关节摩擦力力矩向量，是离心力和哥氏力向量，G(q)∈R
n
是重力向量；M(q)∈R
n
是质量矩阵，q∈R
n
是关节位置向量。
[0028]进一步的，所述线性化简可得公式2：
[0029][0030]式中τ∈R
n
控制力矩输入向量，是辨识矩阵或观测矩阵，θ
s
∈R
12n
是惯性参数，每个连杆的惯性参数及关节摩擦系数组成如公式3：
[0031]θ
s
＝[I
xx
，I
xy
，I
xz
，I
yy
，I
yz
，I
zz
，Mx，My，Mz，M，fv，fc]ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(公式3)
[0032]其中fv，fc是关节摩擦力的项；
[0033]公式4为摩擦力的建模方程式：
[0034][0035]其中的为关节的角速度；
[0036][0037]由公式2惯性参数化简成最小集中惯性参数，得到公式6：
[0038][0039]其中是最小参数的系数矩阵，是最小参数集；是计算力矩方程。
[0040]进一步的，所述采用最小二乘法进行动力学参数识别具体是采用公式7进行：
[0041][0042]式中是由关节位置，速度，加速度构成矩阵，τ为采集关节力矩。
[0043]进一步的，所述获取动力学参数采用公式6的进行
雅可比矩阵处理：
[0044][0045]其中F1为关节1的计算力矩方程，F
n
为关节n的计算力矩方程。
[0046]进一步的，所述状态矢量状态方程和观察方程如公式9～10所示：
[0047]X
k
＝A*X
k
‑1+Q
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(公式9)
[0048]τ＝H*X
k
+R
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(公式10)
[0049]其中为k时刻的各关节位置、速度、加速度数据，A为状态矩阵，Q为过程的标准差，H为测量矩阵，R为测量方差。
[0050]进一步的，所述计算估计协方差矩阵通过公式11进行：
[0051]P
k
＝A*P
k
‑1*A
T
+Q
ꢀꢀꢀ
(公式11)。
[0052]进一步的，所述卡尔曼增益矩阵K由公式12获取；
[0053]K＝P
k
*H
T
*(H*P
k
*H
T
+R)
‑1ꢀꢀ
(公式12)。
[0054]进一步的，所述外力观察矩阵J根据公式13：J＝K
T
*K
ꢀꢀ
(公式13)获取。
[0055]进一步的，所述第二残差力矩向量T通过公式14获取：
...

【技术保护点】

【技术特征摘要】
1.一种机器人防碰撞检测方法，其特征在于，包括以下步骤：步骤101：建立关节机器人动力学方程；步骤102：对动力学方程进行线性化简；步骤103：采集各关节的指定位置、速度、加速度及力矩；步骤104：采用最小二乘法进行动力学参数识别；步骤105：进行卡尔曼滤波处理，获取动力学参数；步骤106：启动并运行机器人；步骤107：采集各关节的实时位置、速度、加速度及力矩；步骤108：获取关节的实时位置、速度及加速度，将获取的机器人各关节的实时位置、速度、加速度为状态矢量状态方程，计算的力矩作为观察方程；步骤109：获取采集的实时关节力矩；步骤110：进一步计算关节力矩；步骤111：根据步骤109的输出结果计算估计协方差矩阵：步骤112：计算卡尔曼增益矩阵K；步骤113：根据步骤106和步骤110的输出结果进一步获取第一残差力矩向量；步骤114：计算外力观察矩阵J；步骤115：根据步骤113和114的输出进一步计算第二残差力矩向量T；步骤116：根据T值大小判断机器人是否发生碰撞，若T值为零向量T{0,
…
0}，则未发生碰撞，继续返回步骤107继续运行机器人；若T值为非零向量T＝T1,
…
T
n
}，即T1,
…
T
n
至少有一个不为零，则机器人发生碰撞，由步骤117立即停止运行机器人。2.如权利要求1所述的机器人防碰撞检测方法，其特征在于，所述关节机器人具有N自由度，所述动力学方程由下列公式1表示：式中：τ∈R
n
是控制力矩输入向量，τ
fi
∈R
n
是关节摩擦力力矩向量，是离心力和哥氏力向量，G(q)∈R
n
是重力向量；M(q)∈R
n
是质量矩阵，q∈R
n
是关节位置向量。3.如权利要求1所述的机器人防碰撞检测方法，其特征在于，所述线性化简可得公式2：式中τ∈R
n
控制力矩输入向量，是辨识矩阵或观测矩阵，θ
s
∈R
12n
是惯性参数，每个连杆的惯性参数及关节摩擦系数组成如公式3：θ
s
＝[I
xx
,I
xy
,I
xz
,I
yy
,I
yz
,I
zz
,Mx,My,Mz,M,fv,fc]
ꢀꢀꢀꢀꢀꢀ
(公式3)其中fv,fc是关节摩擦力的项；公式4为摩擦力的建模方程式：其中的为关节的角速度；
...

【专利技术属性】
技术研发人员：罗建平，王建平，
申请(专利权)人：深圳市资福医疗技术有限公司，
类型：发明
国别省市：