CN106182087A - 基于六维力传感器的碰撞位置感知方法 - Google Patents

Abstract

本发明涉及基于六维力传感器的碰撞位置感知方法，包括以下步骤：在机械臂的第一关节与基座之间安置六维力传感器；将六维力传感器反馈的力和力矩信息代入含有重力补偿和动力学补偿的力分析总体方程，得到碰撞点的作用力和力矩；根据碰撞点的作用力和力矩得到碰撞点在基坐标系的位置。本发明可快速、方便、准确地实现机械臂上点接触的位置信息计算，使机器人具有更强的环境适应能力。

Description

基于六维力传感器的碰撞位置感知方法

技术领域

本发明涉及一种基于六维力传感器的碰撞位置感知方法，提出利用六维力传感器计算点接触时刚性机械臂受力点位置，主要用于机器人交互领域。

背景技术

随着机器人技术的不断发展，传统的无交互能力的机器人已经逐渐向具有环境感知及环境交互方向发展，相关技术如视觉技术，力反馈技术，温度感知技术等，同时对于环境的交互和感知能力要求越来越高。

力感知不仅是机器人与外界交互的重要方式，而且还是部分装配技术的关键，可以使机器人具备更强的环境适应能力。依靠力信号，调整机器人相关状态，以完成相关的任务。机械臂碰撞点的检测，有助于辅助机器人更好的适应环境，能够更加精确的了解环境的状况，从而做出决策。

随着对信息的进一步了解，要求机器人具有类似于人皮肤的感知性能，以满足应用需求。麻省理工学院研究出一款QCT的复合材料做成电子皮肤，其能够感知到机器人与外界发生作用时的作用点及力的大小。其原理是这种材料在微小的压力下，改变相应的电阻值，从而或者综合感知信息，类似的产品还有日本和飞利浦公司研制的电子皮肤，此类系统的优点在于能够直接感知力的作用点，并计算出力的作用方向和大小。但安装于机器人中，要求完全覆盖机器人外表面，线路较为复杂，且算法需要针对外形进行复杂的设计，解析度低、采样速度慢、价格昂贵，适用性较差，易破损。

发明内容

针对上述技术不足，本发明的目的提供一种通过计算的方式间接得到作用点的位置信息和力信息的方法。

本发明解决其技术问题所采用的技术方案是：基于六维力传感器的碰撞位置感知方法，包括以下步骤：

1)在机械臂的第一关节与基座之间安置六维力传感器；

2)将六维力传感器反馈的力和力矩信息代入考虑机械臂重力和运动学影响的力分析总体方程，得到碰撞点的作用力和力矩；

3)根据碰撞点的作用力和力矩得到碰撞点在基坐标系的位置。

所述力分析总体方程为：

$\left\{\begin{array}{c}f=f_0+f_{\mathrm{cB}}+f_{\mathrm{Bd}}\\ m={\mathrm{\τ}}_0+{\mathrm{\τ}}_{\mathrm{cB}}+{\mathrm{\τ}}_{\mathrm{Bd}}\end{array}\right.$

f和m分别为六维力传感器测量的力和力矩；f₀和τ₀分别为仅考虑重力影响下机械臂的重力补偿和重力力矩补偿；f_cB和τ_cB分别为不考虑重力影响及机械臂运动的动力学影响下、基于基坐标系原点的碰撞点的作用力和力矩；f_Bd和τ_Bd分别为表示仅考虑动力学补偿的力补偿和力矩补偿。

所述仅考虑重力影响下机械臂的重力补偿为各关节所受重力之和，重力力矩补偿为各关节重力与基于基座标系的质心的叉乘之和。

所述仅考虑动力学补偿的力补偿和力矩补偿分别为和 ${\mathrm{\τ}}_{\mathrm{Bd}}=\underset{n=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\τ}}_{\mathrm{Bdn}};$

其中，f_Bdn表示关节n运动产生的加速度力和科氏力转换到基座标下的等效力；τ_Bdn表示关节n运动产生的对基座标原点的等效力矩，i表示总的关节数量。

所述根据碰撞点的作用力和力矩得到碰撞点在基坐标系的位置通过对下述公式求解得到：

$\left[\begin{array}{c}{f}_{\mathrm{cB}}\\ m_{\mathrm{cB}}\end{array}\right]=M\left[\begin{array}{c}{\left(T{\left({\stackrel{\‾}{p}}^{T}T_B^c\right)}_{\mathrm{3,1}}\right)}^T\\ \left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)\end{array}\right]$

f_cB和τ_cB分别为不考虑重力影响及机械臂运动的动力学影响下、基于基坐标系原点的碰撞点的作用力和力矩；M为关系矩阵；表示碰撞点在碰撞点坐标系的位置；表示碰撞点在基坐标系的位置；表示由碰撞点坐标到基坐标的变换矩阵；T(·)_3,1表示取矩阵的前3行前1列的元素。

所述通过对下述公式求解根据关系矩阵与机械臂轮廓方程的相容性得到唯一的碰撞点，包括以下步骤：

关系矩阵M的秩rank(M)＝6时，依据方程 $\left[\begin{array}{c}{f}_{\mathrm{cB}}\\ m_{\mathrm{cB}}\end{array}\right]=M\left[\begin{array}{c}{\left(T{\left({\stackrel{\‾}{p}}^{T}T_B^c\right)}_{\mathrm{3,1}}\right)}^T\\ \left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)\end{array}\right]$ 并联立机械臂轮廓方程，得到6组解；去除奇异解后，求取剩余解的平均值；然后根据机械臂轮廓方程得到距离平均解最近的轮廓上的点，即为碰撞点。

本发明具有以下有益效果及优点：

1.本发明可快速、方便、准确地实现机械臂上点接触的位置信息计算，使机器人具有更强的环境适应能力。

2.本发明充分考虑地面的机器人系统的运行条件，引入了重力补偿和动力学补偿，使计算碰撞点的位置更加精确。

3.本发明利用1个六维力传感器，使系统能够感知到任意点的碰撞，相对于采用皮肤传感器的方式降低了成本，且系统便于维护，可广泛的应用于力装配机械臂、服务机器人等。

4.本发明对于通过相容性求解，能够提高求解碰撞位置的精度，提供更精确的碰撞位置信息，为后续决策提供更有效的信息。

附图说明

图1为本发明的方法流程图。

图2为机械臂轮廓建模示意图。

图3为理想状态下的受力分析示意图。

图4为引入重力补偿时受力分析示意图。

图5为机械臂运动状态下的动态力分析示意图。

图6为碰撞点相容性分析步骤示意图。

具体实施方式

下面结合实施例对本发明做进一步的详细说明。

本发明为实现点接触时刚性机械臂受力点位置信息的获取的一种基于力、力矩传感器的位置感知方法，在机械臂的第一关节与基座之间设置六维力传感器，提出利用六维力传感器计算点接触时刚性机械臂受力点位置。如图1所示，包括以下步骤：

(1)引入重力补偿和动力学补偿，列出力分析总体方程；

(2)结合力分析约束、电流约束、法向向量约束和机械臂轮廓约束对算法可解性进行分析；

(3)根据相容原理，消除无解现象，解出受力点位置。

所述重力补偿为考虑重力效应对传感器测量结果的影响，添加对应的力补偿项f₀和力矩补偿项τ₀；

其中，f₀＝G₀+G₁+G₂+...+G_n-1+G_n+..+G_i (1)

G_i＝(0,0,G_i)^T为关节i所受的重力；

τ₀＝r₀×G₀+r₁×G₁+...+r_n-1×G_n-1+r_n×G_n+...+r_i×G_i (2)

r_n为关节n的质心基于基座标系(B坐标系)的矢量，n＝1…i。

所述动力学补偿为机械臂各关节运动时施加于B坐标系原点的等效力和等效力矩，

$\begin{array}{cc}{f}_{\mathrm{Bd}}=\underset{n=1}{\overset{i}{\mathrm{\Σ}}}{f}_{\mathrm{Bdn}}& {\mathrm{\τ}}_{\mathrm{Bd}}=\underset{n=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\τ}}_{\mathrm{Bdn}}\end{array}---\left(3\right)$

其中，f_Bdn表示n关节运动产生的加速度力和科氏力转换到基座标下的等效力；τ_Bdn表示关节n运动产生的对基座标原点的等效力矩。i表示总的关节数量。

所述力分析约束可以描述为通过对关系矩阵M秩的分析将力和力矩方程组分为5个求解方程和1个线性相关方程，进而得到结果的相容性。

所述电流约束可以描述为假设碰撞发生在第n关节，则第n关节的电流必然会发生突变，由于力的相互作用，编号小于n的关节都会存在电流突变的现象。而对于关节编号大于n，则电流不存在相应的突变影响。

所述法向向量约束可以表示为

p^Tn＜0

其中，p^T表示碰撞点所受外界作用力的转置；n表示碰撞点的法向量。

所述机械臂轮廓约束可以描述为碰撞点位于机械臂表面，且满足方程

S(c)＝0；其中S()表示机械臂轮廓方程。

所述相容性分析即根据相容原理，消除无解现象，解出受力点位置；可以描述为由于噪声等因素所导致的解的偏离并不大，因此可以通过相容分析求得碰撞的估计点，其将接近实际的碰撞点。

本发明利用力、力矩传感器来间接计算受力点位置信息，其流程图如图1所示。具体实施步骤如下：

(1)列出力分析总体方程；

图3表示理想状态下的受力分析。在每个关节上设置本地坐标系；图中显示了机械臂的某一关节(n关节)上任意点c的受力分析，坐标系B为力传感器的本地坐标系即基座标系，c坐标系为接触点的坐标系，T_n为接触点c所在刚体的本地坐标系即关节n的本地坐标系，T_i为i关节的本地坐标系(i＝1、2…)，c坐标系与B坐标系之间可通过坐标转换得到。

假设接触点位于第n关节，接触点c的坐标系通过第n关节(受碰撞的关节)的本地坐标系平移得到，假设由坐标c变换到B的变换矩阵为则变换矩阵为 为关节1的本地坐标变换到基座标系的转换矩阵，依次类推，为坐标c变换到关节n本地坐标系的转换矩阵。

当发生点接触时，假设c点的外界作用力为p＝(f_cx,f_cy,f_cz)^T，f_cx,f_cy,f_cz分别为c点在c坐标系x、y、z轴上的力，在无重力影响下，力传感器的测量值分别为f＝(f_x,f_y,f_z)^T和m＝(τ_x,τ_y,τ_z)^Tf_x,f_y,f_z为六维力传感器沿基座标系x,y,z方向的力，τ_x,τ_y,τ_z为六维力传感器相对基座标系x,y,z轴的力矩。

将碰撞点c的力及力矩通过坐标变换可转换相对于基座标的力和力矩。则

$f_{\mathrm{cB}}={(f_{\mathrm{cBx}},f_{\mathrm{cBy}},f_{\mathrm{cBz}})}^T={\left(T{\left({\stackrel{\‾}{p}}^{T}T_B^c\right)}_{\mathrm{3,1}}\right)}^T---\left(4\right)$

$\stackrel{\‾}{p}={(f_{\mathrm{cx}},f_{\mathrm{cy}},f_{\mathrm{cz}},1)}^T---\left(5\right)$

${\mathrm{\τ}}_{\mathrm{cB}}={({\mathrm{\τ}}_{\mathrm{cBx}},{\mathrm{\τ}}_{\mathrm{cBy}},{\mathrm{\τ}}_{\mathrm{cBz}})}^T=c_B\×p\left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)\×p---\left(6\right)$

${\stackrel{\‾}{c}}_n={(x_{\mathrm{cn}},y_{\mathrm{cn}},z_{\mathrm{cn}},1)}^T---\left(7\right)$

f_cB和τ_cB分别为不考虑重力影响下基于基坐标系原点的碰撞点的作用力和力矩；表示碰撞力在碰撞点坐标系的力向量，f_cx,f_cy,f_cz分别表示碰撞力在碰撞点坐标系x,y,z方向的力；表示碰撞点在相应碰撞关节坐标系的位置向量，x_cn,y_cn,z_cn表示碰撞点在相应碰撞关节坐标系沿x,y,z方向的位置；表示由碰撞点所在关节的坐标到基坐标的变换矩阵；T(·)_3,1表示取矩阵的前3行前1列的元素，c_B表示碰撞点在基座标系的坐标位置。

由于力矩传感器的数值是由c点的力及力矩产生的，因此在理想状态下，有如下关系式

$\left\{\begin{array}{c}f=f_{\mathrm{cB}}\\ m={\mathrm{\τ}}_{\mathrm{cB}}\end{array}\right.---\left(8\right)$

图4表示重力效应下受力分析，事实上，重力对传感器读数偏差的影响可以从本质上改变计算结果。而读数偏差是关于各连杆参数(连杆重心和质量)及关节转角的函数。为消除由于重力产生的偏差影响，应对重力效应下模型进行分析。如图4所示，假设连杆的重心位于轴线上。

重力效应作用下的力分析，假设重力效应下传感器读数为f₀∈R³，τ₀∈R³，则两者都是杆件质心、杆件重量、关节转角(r_n是关节转角和杆件质心位置的函数；G_n表示重力，是连杆质量的函数)的函数。根据图4求取之间关系为：

f₀＝G₀+G₁+G₂+...+G_n-1+G_n+..+G_i (9)

G_n＝(0,0,G_n)^T

$\begin{array}{c}{\mathrm{\τ}}_0=r_0\×G_0+r_1\×G_1+...+r_{n-1}\×G_{n-1}+r_n\×G_n+...+r_i\×G_i\\ ={\hat{G}}_0{r}_0+{\hat{G}}_1{r}_1+...+{\hat{G}}_{n-1}{r}_{n-1}+{\hat{G}}_n{r}_n+...+{\hat{G}}_i{r}_i\end{array}---\left(10\right)$

其中G_n的置换矩阵 ${\hat{G}}_n=\left[\begin{array}{ccc}0& 0& -G_n\\ 0& 0& 0\\ G_n& 0& 0\end{array}\right],$ G_n为n关节的重力。

则，考虑重力效应下的受力点位置分析方程为：

$\left\{\begin{array}{c}f=f_0+f_{\mathrm{cB}}\\ m={\mathrm{\τ}}_0+{\mathrm{\τ}}_{\mathrm{cB}}\end{array}\right.---\left(11\right)$

$\left[\begin{array}{c}f\\ m\end{array}\right]=M\left[\begin{array}{c}{\left(T{\left({\stackrel{\‾}{p}}^{T}T_B^c\right)}_{\mathrm{3,1}}\right)}^T\\ \left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)\end{array}\right]+M_0\left[\begin{array}{c}{f}_0\\ r_0\end{array}\right]+...+M_i\left[\begin{array}{c}{f}_i\\ r_i\end{array}\right]=M\left[\begin{array}{c}{\left(T{\left({\stackrel{\‾}{p}}^{T}T_B^c\right)}_{\mathrm{3,1}}\right)}^T\\ \left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)\end{array}\right]+\underset{n=0}{\overset{i}{\mathrm{\Σ}}}{M}_n\left[\begin{array}{c}{f}_n\\ r_n\end{array}\right]---\left(12\right)$

$M\left[\begin{array}{c}{\left(T{\left({\stackrel{\‾}{p}}^{T}T_B^c\right)}_{\mathrm{3,1}}\right)}^T\\ \left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)\end{array}\right]$ 这一项就是碰撞产生的力和力矩，相当于f_cB和τ_cB； $M_0\left[\begin{array}{c}{f}_0\\ r_0\end{array}\right]+...+M_i\left[\begin{array}{c}{f}_i\\ r_i\end{array}\right]$ 为 $\underset{n=0}{\overset{i}{\mathrm{\Σ}}}{M}_n\left[\begin{array}{c}{f}_n\\ r_n\end{array}\right]$ 相当于所有关节从1到i个关节的重力影响的和，相当于f₀和τ₀；M为关系矩阵。

f和m分别为六维力传感器测量的力和力矩；f₀和τ₀分别为仅考虑重力影响、基于基坐标系原点的作用力和力矩；f_cB和τ_cB分别为不考虑重力影响、碰撞时碰撞点基于基坐标系原点的作用力和力矩；

同样对于式(12)，将得到5个非线性方程，需要通过机械臂的轮廓的边界约束，得到解。

机械臂的各关节在运动时，必然会产生反向力，作用于力传感器。在分析碰撞点时，需要将该力进行考虑，并计算其大小。图5表示机械臂运动状态下动态力分析，是所有运动物体的累加效果。假设n关节的角速度为ω_n，加速度为α_n，关节转角为θ_n；则关节n的重心的加速度为a_n＝α_n×D_n，关节n此刻产生的力和力矩为f_αdn和τ_αdn；关节重心的坐标相对于关节n的坐标系为D_n＝(x_dn,y_dn,z_dn)，则m_n表示关节n的质量，a_nx、a_ny、a_nz,表示关节n的重心相对第n关节坐标系沿x,y,z方向的加速度。

${\widehat{\mathrm{\α}}}_n=\left[\begin{array}{ccc}0& -a_{\mathrm{nz}}& a_{\mathrm{ny}}\\ a_{\mathrm{nz}}& 0& -a_{\mathrm{nx}}\\ -a_{\mathrm{ny}}& a_{\mathrm{nx}}& 0\end{array}\right]$

τ_αdn＝I_nα_n (13)

其中I_n为第n关节的转动惯量，力矩可以等效到任一点，力进行等效时，则需要等效为力和力矩。第n关节相对于T_n(第n关节的坐标系)坐标系的科氏力f_kdn＝2m_nv_Tn×ω_Tn，，v_Tn表示T_n坐标系相对基座标的速度，ω_Tn表示T_n相对于基座标系的角速度。

$\begin{array}{cc}{f}_{\mathrm{fBdn}}={\left(T{\left(f_{\mathrm{\αdn}}^{T}T_B^D\right)}_{\mathrm{3,1}}\right)}^T& f_{\mathrm{kBdn}}={\left(T{\left(f_{\mathrm{kdn}}^{T}T_B^n\right)}_{\mathrm{3,1}}\right)}^T\end{array}---\left(14\right)$

$\begin{array}{cc}{\mathrm{\τ}}_{\mathrm{fBdn}}=f_{\mathrm{fBdn}}\×D_{\mathrm{Bn}}={\hat{f}}_{\mathrm{fBdn}}{\left(T{\left({\stackrel{\‾}{D}}_{n}T_B^n\right)}_{\mathrm{3,1}}\right)}^T& {\mathrm{\τ}}_{\mathrm{kBdn}}=f_{\mathrm{kBdn}}\×T_n={\hat{f}}_{\mathrm{kBdn}}{\left(T{\left(T_B^n\right)}_{\left(\mathrm{1,2,3}\right),4}\right)}^T\end{array}---\left(15\right)$

从而得到总的作用力和力矩为

f_Bdn＝f_fBdn+f_kBdn (16)

τ_Bdn＝τ_αBdn+τ_fBdn++τ_kBdn (17)

其中τ_αBdn＝τ_αdn

f_fBdn表示f_αdn转换到基座标系的力，τ_fBdn表示f_αdn转换时产生的力矩，f_kBdn表示f_kdn转换到基座标系的力，τ_kBdn表示f_kdn转换时产生的力矩。是对D_n＝(x_dn,y_dn,z_dn)的变换。T_n表示n坐标系在基座标系下的位置矢量，T(·)_(1,2,3),4表示取矩阵的第四列的前三行元素，表示关节n坐标系到基座标系的转换矩阵；D_Bn表示第n关节重心在基座标系中的矢量。τ_αBdn表示力矩τ_αdn转换到基座标系下的力矩。

f_Bdn表示由于动力学基座产生的总的力。

τ_Bdn表示由于动力学基座产生的总的力矩对于机械臂的多个关节，可以得到运动产生的机械臂的

$\begin{array}{cc}{f}_{\mathrm{Bd}}=\underset{n=1}{\overset{i}{\mathrm{\Σ}}}{f}_{\mathrm{Bdn}}& {\mathrm{\τ}}_{\mathrm{Bd}}=\underset{n=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{\τ}}_{\mathrm{Bdn}}\end{array}---\left(18\right)$

$\left\{\begin{array}{c}f=f_0+f_{\mathrm{cB}}+f_{\mathrm{Bd}}\\ m={\mathrm{\τ}}_0+{\mathrm{\τ}}_{\mathrm{cB}}+{\mathrm{\τ}}_{\mathrm{Bd}}\end{array}\right.---\left(19\right)$

因此，考虑重力和机械臂运动时反作用力的影响，列出力分析的总体方程

$\left\{\begin{array}{c}f=f_0+f_{\mathrm{cB}}+f_{\mathrm{Bd}}\\ m={\mathrm{\τ}}_0+{\mathrm{\τ}}_{\mathrm{cB}}+{\mathrm{\τ}}_{\mathrm{Bd}}\end{array}\right.---\left(20\right)$

其中f和m分别表示在考虑重力效应和机械臂运动影响的情况下，传感器测出接触点的力和力矩；f₀和τ₀分别表示仅考虑重力效应时的机械臂的重力补偿(项)和力矩补偿(项)；f_Bd和τ_Bd分别表示仅考虑机械臂运动所带来的力补偿(项)和力矩补偿(项)；f_cB和τ_cB分别表示理想状态下，即不考虑重力影响及机械臂运动的动力学影响下，静态机械臂点接触时基坐标下原点的等效力和力矩。

(2)可解性分析，具体包含以下4个方面：

1.力分析约束

将作用于刚体上的力p表示为矩阵形式

$\hat{p}=\left[\begin{array}{ccc}0& -f_{\mathrm{cz}}& f_{\mathrm{cy}}\\ f_{\mathrm{cz}}& 0& -f_{\mathrm{cx}}\\ -f_{\mathrm{cy}}& f_{\mathrm{cx}}& 0\end{array}\right]$

其中，f_cx,f_cy,f_cz分别表示碰撞点所受作用力在碰撞点坐标系(c坐标系)下沿x轴，y轴，z轴分力。

此时传感器测出的理想状态下力矩

其中表示由c坐标到B坐标的变换矩阵；为碰撞点在第n关节的碰撞位置，x_cn，x_yn，x_zn表示碰撞点在第n关节坐标系的坐标位置；T(·)_3,1表示取矩阵的前3行前1列的元素。

将 $m={\mathrm{\τ}}_{\mathrm{cB}}=\hat{p}\left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)$ 和 $\left\{\begin{array}{c}f=f_{\mathrm{cB}}\\ m={\mathrm{\τ}}_{\mathrm{cB}}\end{array}\right.$ 联立，可得

$\left[\begin{array}{c}f\\ m\end{array}\right]=M\left[\begin{array}{c}{\left(T{\left({\stackrel{\‾}{p}}^{T}T_B^c\right)}_{\mathrm{3,1}}\right)}^T\\ \left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)\end{array}\right]---\left(21\right)\mathrm{}M=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& -f_{\mathrm{cz}}& f_{\mathrm{cy}}\\ 0& 0& 0& f_{\mathrm{cz}}& 0& -f_{\mathrm{cx}}\\ 0& 0& 0& -f_{\mathrm{cy}}& f_{\mathrm{cx}}& 0\end{array}\right]---\left(22\right)$

在去除重力因素和动力学影响因素后，通过对公式(21)求解得到碰撞点在基坐标系的位置。其中M为关系矩阵，理想状态下，rank(M)＝5，因此M为奇异矩阵，不能解出相应接触点的位置及作用力大小。通过式(21)可得到5个相关的求解方程，另一方程与其线性相关，可作为对方程求解结果的验证。

2.电流约束

根据各关节电流的检测，如果某一关节的电流突变，则该关节受碰撞。

假设碰撞发生在第n关节，则第n关节的电流必然会发生突变，由于力的相互作用，编号小于n的关节都会存在电流突变的现象。而对于关节编号大于n，电流不存在相应的突变影响。根据该约束，可直接判断作用点位于哪一关节，一方面避免了多解的存在，另一方面，加快了解的求解速度。

3.法向向量约束

通过法向向量约束p^Tn＜0可以将方程产生的多解进行筛选，保证所求的力符合客观事实。即满足接触点的法向向量与作用力向量的夹角小于90°的方程约束。

4.机械臂轮廓约束

为寻求解的存在，将需要对机械臂的轮廓进行几何建模，碰撞点位于机械臂的表面，方程可表示为S(c)＝0，S(c)＝c^TA^TAc＝1；其中，A为常系数的对角阵，且 $A=\left[\begin{array}{ccc}1/\mathrm{\α}& 0& 0\\ 0& 1/\mathrm{\β}& 0\\ 0& 0& 1/\mathrm{\γ}\end{array}\right],$ c表示接触点在所在关节坐标系的坐标。

如图2所示的机械臂轮廓建模方法示意，常见的几何外形多为平面、球形、圆柱形，及椭球形，四种形状基本能够构建所有的简单曲面。机械臂的外观形状可以基本完全由四种形状拼接构建起来。从几何变化的角度讲，四种几何形状的核心形状为椭球形，其余三种形状都可由其变化产生。本文将针对椭球形球面的接触点方程进行计算与研究。我们假设，坐标系原点在椭球球心，本地坐标系的xyz三轴分别为椭球的三轴，则可得到椭球面方程：

S(c)＝c^TA^TAc＝1 (23)

其中，A为常系数的对角阵，且 $A=\left[\begin{array}{ccc}1/\mathrm{\α}& 0& 0\\ 0& 1/\mathrm{\β}& 0\\ 0& 0& 1/\mathrm{\γ}\end{array}\right]$

则椭球形的三个轴的轴长分别为2α，2β，2γ，且α＞0β＞0γ＞0。当α＝β＝γ则方程表示球形表面；当α→+∞趋于无穷时，则表示半径为1/β圆柱，当α→+∞且β→+∞，γ→0则表示平面。

点c位于曲面上，则c处的单位法向量n为

$n=\frac{\▿S\left(c\right)}{\left|\right|\▿S\left(c\right)\left|\right|}=\frac{{2A}^{2}c}{\left|\right|{2A}^{2}c\left|\right|}=\frac{A^{2}c}{\left|\right|A^{2}c\left|\right|}---\left(24\right)$

式中，表示梯度，c表示接触点坐标。

(3)相容性分析

步骤(2)中所得关系矩阵M的秩rank(M)＝5时，所求的解具有唯一性，此时相容性最佳。但是许多因素都将导致rank(M)＝6，这就导致所求解的矛盾性。由于噪声等因素所导致的解的偏离并不大，因此通过相容分析方法求得碰撞的估计点，如图6所述，求将接近实际的碰撞点。当rank(M)＝6时，联立式(21)与轮廓描述方程，通过从式(21)中选取5个方程，并结合轮廓方程的方式，共计得到6组解，去除奇异解后，求取剩余解的平均值，然后依靠轮廓描述方程得到距离平均解最近的轮廓上的点，即为碰撞点，之后可求取碰撞力。

因此，在对式(21)的求解具体步骤如下(如图6)：

首先，理论上通过式(21)，Rank(M)＝5，通过联立机械臂的轮廓方程，可得到碰撞点的解，但是由于测量的偏差，通过式(21)可得到6个非线性方程，联立机械臂轮廓方程S(c)＝0，将产生6个未知数，7个方程的状况，因此在式(21)中任意选择5个方程，并联立轮廓方程可求得方程的一组解；

上述中的轮廓方程是根据各关节电流的检测，电流突变的关节中，编号最大的关节(电流突变的关节很多，是从0开始，0,1,2,3….等，一直到碰撞关节，并且不是判断谁变的大，而是变的这些关节中，编号最大的)为受碰撞的关节，从而可以迅速确定具体关节的轮廓方程，而无需建立整个机械臂轮廓方程。

在求解的过程中，根据法向向量约束p^Tn＜0，满足接触点的法向向量与作用力向量的夹角小于90°，用于排除求解过程中无效解的状况。

上述中，式(21)中六个方程中选择5个方程，有六种选择方式，因此可得到六组解。

最后，通过去除奇异解，求取剩余解的平均值，然后依靠轮廓描述方程得到距离平均解最近的轮廓上的点，即为碰撞点。

Claims (6)

1.基于六维力传感器的碰撞位置感知方法，其特征在于包括以下步骤：

1)在机械臂的第一关节与基座之间安置六维力传感器；

2)将六维力传感器反馈的力和力矩信息代入考虑机械臂重力和运动学影响的力分析总体方程，得到碰撞点的作用力和力矩；

3)根据碰撞点的作用力和力矩得到碰撞点在基坐标系的位置。

2.根据权利要求1所述的基于六维力传感器的碰撞位置感知方法，其特征在于所述力分析总体方程为：

$\left\{\begin{array}{c}f=f_0+f_{\mathrm{cB}}+f_{\mathrm{Bd}}\\ m={\mathrm{\τ}}_0+{\mathrm{\τ}}_{\mathrm{cB}}+{\mathrm{\τ}}_{\mathrm{Bd}}\end{array}\right.$

f和m分别为六维力传感器测量的力和力矩；f₀和τ₀分别为仅考虑重力影响下机械臂的重力补偿和重力力矩补偿；f_cB和τ_cB分别为不考虑重力影响及机械臂运动的动力学影响下、基于基坐标系原点的碰撞点的作用力和力矩；f_Bd和τ_Bd分别为表示仅考虑动力学补偿的力补偿和力矩补偿。

3.根据权利要求2所述的基于六维力传感器的碰撞位置感知方法，其特征在于所述仅考虑重力影响下机械臂的重力补偿为各关节所受重力之和，重力力矩补偿为各关节重力与基于基座标系的质心的叉乘之和。

4.根据权利要求2所述的基于六维力传感器的碰撞位置感知方法，其特征在于所述仅考虑动力学补偿的力补偿和力矩补偿分别为和

其中，f_Bdn表示关节n运动产生的加速度力和科氏力转换到基座标下的等效力；τ_Bdn表示关节n运动产生的对基座标原点的等效力矩，i表示总的关节数量。

5.根据权利要求1所述的基于六维力传感器的碰撞位置感知方法，其特征在于所述根据碰撞点的作用力和力矩得到碰撞点在基坐标系的位置通过对下述公式求解得到：

$\left[\begin{array}{c}{f}_{\mathrm{cB}}\\ m_{\mathrm{cB}}\end{array}\right]=M\left[\begin{array}{c}{\left(T{\left({\stackrel{\‾}{p}}^{T}T_B^c\right)}_{\mathrm{3,1}}\right)}^T\\ \left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)\end{array}\right]$

f_cB和τ_cB分别为不考虑重力影响及机械臂运动的动力学影响下、基于基坐标系原点的碰撞点的作用力和力矩；M为关系矩阵；表示碰撞点在碰撞点坐标系的位置；表示碰撞点在基坐标系的位置；表示由碰撞点坐标到基坐标的变换矩阵；T(·)_3,1表示取矩阵的前3行前1列的元素。

6.根据权利要求5所述的基于六维力传感器的碰撞位置感知方法，其特征在于所述通过对下述公式求解根据关系矩阵与机械臂轮廓方程的相容性得到唯一的碰撞点，包括以下步骤：

关系矩阵M的秩rank(M)＝6时，依据方程 $\left[\begin{array}{c}{f}_{\mathrm{cB}}\\ m_{\mathrm{cB}}\end{array}\right]=M\left[\begin{array}{c}{\left(T{\left({\stackrel{\‾}{p}}^{T}T_B^c\right)}_{\mathrm{3,1}}\right)}^T\\ \left(T{\left({\stackrel{\‾}{c}}_{n}T_B^c\right)}_{\mathrm{3,1}}\right)\end{array}\right]$ 并联立机械臂轮廓方程，得到6组解；去除奇异解后，求取剩余解的平均值；然后根据机械臂轮廓方程得到距离平均解最近的轮廓上的点，即为碰撞点。