ロボットアームの6次元力センサーの重力補償原理

重力補償原理

1. 力覚センサーデータ解析

6次元力覚センサの3つの力成分とトルク成分のゼロ点値を$(F_{\times }、F_{y0}、F_{z0})$，$(M_{\times }、M_{y0}、M_{z0}）$、

センサーとエンドツールの重力は$G$、6 次元力センサー座標系の重心の座標は$(x,よ、z）$、

グラビティGG3 つの座標軸における$G$$(G_{}、G_{}、G_{})$，$(M_{}、M_{ジー}、M_{ジーズ}）$。

力とモーメントの関係から、次のように求めることができます（正の方向は右手の法則で決まります） {
M gx = G z × y − G y × z M gy = G x × z − G z × x M gz = G y × x − G x × y \left\{ \begin{array}{c} M_{gx}=G_z \times y-G_y \times z\\ M_{gy}=G_x \times z-G_z \times x\\ M_ {gz}=G_y \times x-G_x \times y \end{array} \right。⎩ ⎨ ⎧MGX=Gz×y−Gはい×zMジー_=G××z−Gz×バツMジーズ_=Gはい×バツ−G××はい
6次元力覚センサで直接測定した3方向の力成分とトルク成分をそれぞれ$(F_{}、F_{}、F_{})$，$(M_{}、M_{}、M_{}）$、

校正中にエンドホルダーに外力がかからないと仮定すると、力センサーで測定される力とモーメントは荷重の重力とゼロ点で構成され、{
F x = G x + F x 0 F y = G y + F y 0 F z = G z + F z 0 \left\{ \begin{array}{c} F_x=G_x+F_{x0}\\ F_y=G_y+F_{y0}\\ F_z=G_z+F_ { z0} \end{配列} \right。⎩ ⎨ ⎧F×=G×+F× 0Fはい=Gはい+Fy0 _Fz=Gz+Fz0 _​

{ M x = M gx + M x 0 M y = M gy + M y 0 M z = M gz + M z 0 \left\{ \begin{array}{c} M_x=M_{gx}+M_{x0 }\\ M_y=M_{gy}+M_{y0}\\ M_z=M_{gz}+M_{z0} \end{array} \right。⎩ ⎨ ⎧M×=MGX+M× 0Mはい=Mジー_+My0 _Mz=Mジーズ_+Mz0 _​

联立得：
{ M x = ( F z − F z 0 ) × y − ( F y − F y 0 ) × z + M x 0 M y = ( F x − F x 0 ) × z − ( F z − F z 0 ) × x + M y 0 M z = ( F y − F y 0 ) × x − ( F x − F x 0 ) × y + M z 0 \left\{ \begin{array}{c } M_x=(F_z-F_{z0}) \times y-(F_y-F_{y0}) \times z+M_{x0}\\ M_y=(F_x-F_{x0}) \times z-(F_z- F_{z0}) \times x+M_{y0}\\ M_z=(F_y-F_{y0}) \times x-(F_x-F_{x0}) \times y+M_{z0} \end{array} \右。⎩ ⎨ ⎧M×=( Fz−Fz0 _)×y−( Fはい−Fy0 _)×z+M× 0Mはい=( F×−F× 0)×z−( Fz−Fz0 _)×バツ+My0 _Mz=( Fはい−Fy0 _)×バツ−( F×−F× 0)×y+Mz0 _​

{ M x = F z × y − F y × z + M x 0 + F y 0 × z − F z 0 × y M y = F x × z − F z × x + M y 0 + F z 0 × x − F x 0 × z M z = F y × x − F x × y + M z 0 + F x 0 × y − F y 0 × x \left\{ \begin{array}{c} M_x=F_z \times y - F_y \times z + M_{x0} + F_{y0} \times z -F_{z0} \times y \\ M_y=F_x \times z - F_z \times x + M_{y0} + F_{ z0} \times x -F_{x0} \times z \\ M_z=F_y \times x - F_x \times y + M_{z0} + F_{x0} \times y -F_{y0} \times x \end{配列} \右。⎩ ⎨ ⎧M×=Fz×y−Fはい×z+M× 0+Fy0 _×z−Fz0 _×yMはい=F××z−Fz×バツ+My0 _+Fz0 _×バツ−F× 0×zMz=Fはい×バツ−F××y+Mz0 _+F× 0×y−Fy0 _××

この中の$F_{\times }、F_{y0}、F_{z0}、M_{\times }、M_{y0}、M_{z0}、\times 、よ、z$は固定値の定数です（力センサーデータの変動は無視し、先端工具は交換しません）

2. 最小二乗法を使用してパラメータを解く

2.1 モーメント方程式

令
{ k 1 = M x 0 + F y 0 × z − F z 0 × yk 2 = M y 0 + F z 0 × x − F x 0 × zk 3 = M z 0 + F x 0 × y − F y 0 × x \left\{ \begin{array}{c} k_1=M_{x0} + F_{y0} \times z -F_{z0} \times y\\ k_2=M_{y0} + F_{z0 } \times x -F_{x0} \times z\\ k_3=M_{z0} + F_{x0} \times y -F_{y0} \times x \end{array} \right。⎩ ⎨ ⎧k1=M× 0+Fy0 _×z−Fz0 _×yk2=My0 _+Fz0 _×バツ−F× 0×zk3=Mz0 _+F× 0×y−Fy0 _××
例：
$$\begin{array}{c}\left[\begin{array}{c}{M}_{}\\ M_{}\\ M_{}\end{array}\right]=\left[\begin{array}{cccccc}0& F_{}& -F{\_}_{}& 1& 0& 0\\ -F{\_}_{}& 0& F_{}& 0& 1& 0\\ F_{}& -F{\_}_{}& & & & \end{array}\right]\left[\begin{array}{c}バツ\\ y\\ z\\ k1\_\\ k2\_\_\\ k3\end{array}\right]\end{array}$$

ロボットアームの先端にあるグリッパーの姿勢を制御し、N の異なる姿勢を取ります (N $\ge$ 3、少なくとも 3 つの姿勢におけるロボット アームのエンド グリッパーの指示ベクトルが同一平面上にないことが必要)、N セットの 6 次元力データ センサー データが取得され、

$M=F\cdot A$，その中$あ=[\times 、よ、z、k_{}、k_{}、k_{}{】}^T$の場合、行列の最小二乗解は$あ=(F^{TF}）\_{}^{-1}\cdot F^T\cdot ま$、

複数のデータから、 6次元力覚センサ座標系における荷重重心の座標$(x,よ、z)$と定数$(k_{}、k_{}、k_{})$値。

2.2 力の方程式

ロボットの設置と固定のプロセス中、設置プラットフォームには一定の傾斜があるため、ロボットのベース座標系と世界座標系には一定の角度があり、これによりロボットの測定値との間に一定の偏差が生じます。センサーと実際の値が異なるため、傾き角度の計算が必要となります。

世界座標系を$O$、基本座標系は$B$、フランジ座標系は$W$、力センサー座標系は$C$の場合、ベース座標系からワールド座標系への姿勢変換行列は次のようになります。
$$\begin{array}{c}{}_B^{}R=\left[\begin{array}{ccc}1& 0& 0\\ 0& cosU& アメリカにある\\ & & cos\end{array}\right]\left[\begin{array}{ccc}バスケットV& 0& sinV\_\\ 0& 1& 0\\ -s& & バスケット\end{array}\right]\end{array}$$
其中$U$は世界座標系$X$軸の回転角度$V\; は$ベース座標系の周りの$y$軸の回転角度

エンドフランジからベース座標系への姿勢変換行列は次のとおりです。
$${}_W^{}R=R_{}\left(A\right)R_{}\left(B\right)R_{}\left(C\right)$$
$ABC$の値はロボットのティーチングペンダントから読み取ることができます。

最後に力覚センサーを取り付けると$Z$軸上の$\alpha$ (ロボット基準座標系を変更することで手動で一致させることができます。つまり、たわみ角が 0 になります) 力覚センサからフランジ座標系への姿勢変換行列は、 CWR = [ cos α − sin α 0 sin α cos
$$\begin{array}{c}{}_C^{}R=\left[\begin{array}{ccc}cos\alpha \_\_& -\alpha のs& 0\\ \alpha の\_& cos\alpha \_\_& 0\\ & & \end{array}\right]\end{array}$$
このとき、世界座標系から 6 次元力覚センサ座標系への姿勢変換行列は
$${}_0^{}R={}_W^{}R\cdot {}_B^{}R\cdot {}_{○}^{}R={{}_C^{}R}^T\cdot {{}_W^{}R}^T\cdot {{}_B^{}R}^{}$$
世界座標系O {O}$${}^{におけるT重力}$$$O$の式は${}^{そして}G=[0,0、-g]\_{}^{て}$、

座標変換により、重力は力センサー座標系$C$得られる
$$\begin{gathered} {}^{CG}\_{=}_{○}^{}R\cdot {}^{そして}G={}_W^{}R\cdot {}_B^{}R\cdot {}_{○}^{}R\cdot {}^{そして}G={{}_C^{}R}^T\cdot {{}_W^{}R}^T\cdot {{}_B^{}R}^T\cdot {}^{そして}G \\ ={{}_C^{}R}^T\cdot {{}_W^{}R}^T\cdot \left[\begin{array}{c}cosUsinV\ast g\\ -Uのs\ast g\\ -cosUcosV\ast \end{array}\right] \end{gathered}$$

例：
$$[F_{}、F_{}、F_{}{】}^T=[G_{}、G_{}、G_{}{】}^T+[F_{\times }、F_{y0}、F_{z0}{】}^T{=}_{○}^{}R\cdot {}^{そして}G+[F_{\times }、F_{y0}、F_{z0}{】}^T$$

$$\begin{array}{c}\left[\begin{array}{c}{F}_{}\\ F_{}\\ F_{}\end{array}\right]=[\begin{array}{cc}{{}_C^{}R}^T\cdot {{}_W^{}R}^{}& \end{array}】\left[\begin{array}{c}cosUsinV\ast g\\ -Uのs\ast g\\ -cosUcosV\ast g\\ F_{\times }\\ F_{y0}\\ F_{z0}\end{array}\right]\end{array}$$
令：
{ L x = cos U sin V ∗ g L y = − sin U ∗ g L z = − cos U cos V ∗ g \left\{ \begin{array}{c} L_x=cosUsinV * g\\ L_y =-sinU * g\\ L_z= -cosUcosV * g \end{array} \right。⎩ ⎨ ⎧L×=cos U s inV∗gLはい=- Uのs∗gLz=− cos U cos V∗g
これは次のように表すことができます:
$$\begin{array}{c}\left[\begin{array}{c}{F}_{}\\ F_{}\\ F_{}\end{array}\right]=[\begin{array}{cc}{{}_C^{}R}^T\cdot {{}_W^{}R}^{}& \end{array}】\left[\begin{array}{c}{L}_{}\\ L_{}\\ L_{}\\ F_{\times }\\ F_{y0}\\ F_{z0}\end{array}\right]\end{array}$$

同様に、N 個の異なる姿勢をとることにより、N 個の 6 次元力覚センサーデータが取得されます。

$f=R\cdot B$，その中$B=[L_{}、L_{}、L_{}、F_{\times }、F_{y0}、F_{z0}{】}^T$の場合、行列の最小二乗解は$B=(R^{ＴＲ}）\_{}^{-1}\cdot R^T\cdot ふ$、

力覚センサーのゼロ点値(F x 0 、F y 0 、F z 0 ) (F_{x0},F_{y0},F_{z0}) を複数のデータから求めることができます。$(F_{\times }、F_{y0}、F_{z0})$、取付傾斜角$う、V$と$グラム$サイズ。
$$g=\sqrt{{L_{}}^2+{L_{}}^2+{L_{}}^2}$$

$$U=\left(のrcs\right)\frac{-{大}_{}}{g})$$

$$V=アークタン（\_\_\_\frac{-{大}_{}}{L_{}})$$

3. 外部接触力の計算

外部接触力は次のように結論付けることができます:
{ F ex = F x − F x 0 − G x F ey = F y − F y 0 − G y F ez = F z − F z 0 − G z \left \ { \begin{array}{c} F_{ex}=F_x-F_{x0}-G_x\\ F_{ey}=F_y-F_{y0}-G_y\\ F_{ez}=F_z-F_{z0 } -G_z \end{配列} \right。⎩ ⎨ ⎧Fエックス_=F×−F× 0−G×Fええ=Fはい−Fy0 _−GはいFいいえ=Fz−Fz0 _−Gz​

$$\begin{array}{c}\left[\begin{array}{c}{F}_{エックス}\\ F_{}\\ F_{}\end{array}\right]=\left[\begin{array}{c}{F}_{}\\ F_{}\\ F_{}\end{array}\right]-[\begin{array}{cc}{}_{○}^{}& \end{array}】\left[\begin{array}{c}0\\ 0\\ -g\_\\ F_{\times }\\ F_{y0}\\ F_{z0}\end{array}\right]\end{array}$$
又：
{ M x 0 = k 1 − F y 0 × z + F z 0 × y M y 0 = k 2 − F z 0 × x + F x 0 × z M z 0 = k 3 − F x 0 × y + F y 0 × x \left\{ \begin{array}{c} M_{x0}=k_1 - F_{y0} \times z +F_{z0} \times y \\ M_{y0}=k_2 - F_{z0} \times x +F_{x0} \times z\\ M_{z0}=k_3 - F_{x0} \times y +F_{y0} \times x \end{array} \right。⎩ ⎨ ⎧M× 0=k1−Fy0 _×z+Fz0 _×yMy0 _=k2−Fz0 _×バツ+F× 0×zMz0 _=k3−F× 0×y+Fy0 _××

$${}^{CG}\_{=}_{○}^{}R\cdot {}^{そして}G=[G_{}、G_{}、G_{}{】}^T$$

{ M gx = G z × y − G y × z M gy = G x × z − G z × x M gz = G y × x − G x × y \left\{ \begin{array}{c} M_ {gx}=G_z \times y-G_y \times z\\ M_{gy}=G_x \times z-G_z \times x\\ M_{gz}=G_y \times x-G_x \times y \end{array} \右。⎩ ⎨ ⎧MGX=Gz×y−Gはい×zMジー_=G××z−Gz×バツMジーズ_=Gはい×バツ−G××はい

つまり:
$$\begin{array}{c}\left[\begin{array}{c}{M}_{}\\ M_{ジー}\\ M_{ジーズ}\end{array}\right]=\left[\begin{array}{ccc}0& -z\_& y\\ z& 0& -\times \\ -& & \end{array}\right]\left[\begin{array}{c}{G}_{}\\ G_{}\\ G_{}\end{array}\right]=\left[\begin{array}{ccc}0& -z\_& y\\ z& 0& -\times \\ -& & \end{array}\right]{\cdot }_{○}^{}R\cdot \left[\begin{array}{c}0\\ 0\\ -\end{array}\right]\end{array}$$
外部接触モーメントは次のとおりです。
{ M ex = M x − M x 0 − M gx M ey = M y − M y 0 − M gy M ez = M z − MZ 0 − M gz \left\{ \begin{ array }{c} M_{ex}=M_x-M_{x0}-M_{gx}\\ M_{ey}=M_y-M_{y0}-M_{gy}\\ M_{ez}=M_z-M_{ Z0 }-M_{gz} \end{array} \right。⎩ ⎨ ⎧Mエックス_=M×−M× 0−MGXMええ=Mはい−My0 _−Mジー_Mいいえ=Mz−MZ0 _−Mジーズ_​

[1] Gao Peiyang. 力センサーに基づく産業用ロボットの定力研削および研磨システムの研究 [D]. 華中科技大学、2019. [2] Zhang Lijian、Hu Ruiqin、Yi Wangmin. Force on the
end 6 次元の力センサーに基づく産業用ロボットの負荷 Perception Research[J]. Journal of Automation, 2017, 43(03):439-447.DOI:10.16383/j.aas.2017.c150753. [3]Dong Hao
.軸穴組立産業用ロボットのコンプライアンス制御アルゴリズムの研究[D] .河南科学技術大学、2018.

コードは以下のように表示されます:

from numpy import *
import numpy as np
'''
Made by 水木皆Ming
重力补偿计算
'''
class GravityCompensation:
    M = np.empty((0, 0))
    F = np.empty((0, 0))
    f = np.empty((0, 0))
    R = np.empty((0, 0))

    x = 0
    y = 0
    z = 0
    k1 = 0
    k2 = 0
    k3 = 0

    U = 0
    V = 0
    g = 0

    F_x0 = 0
    F_y0 = 0
    F_z0 = 0

    M_x0 = 0
    M_y0 = 0
    M_z0 = 0

    F_ex = 0
    F_ey = 0
    F_ez = 0

    M_ex = 0
    M_ey = 0
    M_ez = 0

    def Update_M(self, torque_data):
        M_x = torque_data[0]
        M_y = torque_data[1]
        M_z = torque_data[2]

        if (any(self.M)):
            M_1 = matrix([M_x, M_y, M_z]).transpose()
            self.M = vstack((self.M, M_1))
        else:
            self.M = matrix([M_x, M_y, M_z]).transpose()

    def Update_F(self, force_data):
        F_x = force_data[0]
        F_y = force_data[1]
        F_z = force_data[2]

        if (any(self.F)):
            F_1 = matrix([[0, F_z, -F_y, 1, 0, 0],
                          [-F_z, 0, F_x, 0, 1, 0],
                          [F_y, -F_x, 0, 0, 0, 1]])
            self.F = vstack((self.F, F_1))
        else:
            self.F = matrix([[0, F_z, -F_y, 1, 0, 0],
                             [-F_z, 0, F_x, 0, 1, 0],
                             [F_y, -F_x, 0, 0, 0, 1]])

    def Solve_A(self):
        A = dot(dot(linalg.inv(dot(self.F.transpose(), self.F)), self.F.transpose()), self.M)

        self.x = A[0, 0]
        self.y = A[1, 0]
        self.z = A[2, 0]
        self.k1 = A[3, 0]
        self.k2 = A[4, 0]
        self.k3 = A[5, 0]
        # print("A= \n" , A)
        print("x= ", self.x)
        print("y= ", self.y)
        print("z= ", self.z)
        print("k1= ", self.k1)
        print("k2= ", self.k2)
        print("k3= ", self.k3)

    def Update_f(self, force_data):
        F_x = force_data[0]
        F_y = force_data[1]
        F_z = force_data[2]

        if (any(self.f)):
            f_1 = matrix([F_x, F_y, F_z]).transpose()
            self.f = vstack((self.f, f_1))
        else:
            self.f = matrix([F_x, F_y, F_z]).transpose()

    def Update_R(self, euler_data):
        # 机械臂末端到基坐标的旋转矩阵
        R_array = self.eulerAngles2rotationMat(euler_data)

        alpha = (0) * 180 / np.pi

        # 力传感器到末端的旋转矩阵
        R_alpha = np.array([[math.cos(alpha), -math.sin(alpha), 0],
                            [math.sin(alpha), math.cos(alpha), 0],
                            [0, 0, 1]
                            ])

        R_array = np.dot(R_alpha, R_array.transpose())

        if (any(self.R)):
            R_1 = hstack((R_array, np.eye(3)))
            self.R = vstack((self.R, R_1))
        else:
            self.R = hstack((R_array, np.eye(3)))

    def Solve_B(self):
        B = dot(dot(linalg.inv(dot(self.R.transpose(), self.R)), self.R.transpose()), self.f)

        self.g = math.sqrt(B[0] * B[0] + B[1] * B[1] + B[2] * B[2])
        self.U = math.asin(-B[1] / self.g)
        self.V = math.atan(-B[0] / B[2])

        self.F_x0 = B[3, 0]
        self.F_y0 = B[4, 0]
        self.F_z0 = B[5, 0]

        # print("B= \n" , B)
        print("g= ", self.g / 9.81)
        print("U= ", self.U * 180 / math.pi)
        print("V= ", self.V * 180 / math.pi)
        print("F_x0= ", self.F_x0)
        print("F_y0= ", self.F_y0)
        print("F_z0= ", self.F_z0)

    def Solve_Force(self, force_data, euler_data):
        Force_input = matrix([force_data[0], force_data[1], force_data[2]]).transpose()

        my_f = matrix([cos(self.U)*sin(self.V)*self.g, -sin(self.U)*self.g, -cos(self.U)*cos(self.V)*self.g, self.F_x0, self.F_y0, self.F_z0]).transpose()

        R_array = self.eulerAngles2rotationMat(euler_data)
        R_array = R_array.transpose()
        R_1 = hstack((R_array, np.eye(3)))

        Force_ex = Force_input - dot(R_1, my_f)
        print('接触力：\n', Force_ex)

    def Solve_Torque(self, torque_data, euler_data):
        Torque_input = matrix([torque_data[0], torque_data[1], torque_data[2]]).transpose()
        M_x0 = self.k1 - self.F_y0 * self.z + self.F_z0 * self.y
        M_y0 = self.k2 - self.F_z0 * self.x + self.F_x0 * self.z
        M_z0 = self.k3 - self.F_x0 * self.y + self.F_y0 * self.x

        Torque_zero = matrix([M_x0, M_y0, M_z0]).transpose()

        Gravity_param = matrix([[0, -self.z, self.y],
                                [self.z, 0, -self.x],
                                [-self.y, self.x, 0]])

        Gravity_input = matrix([cos(self.U)*sin(self.V)*self.g, -sin(self.U)*self.g, -cos(self.U)*cos(self.V)*self.g]).transpose()

        R_array = self.eulerAngles2rotationMat(euler_data)
        R_array = R_array.transpose()

        Torque_ex = Torque_input - Torque_zero - dot(dot(Gravity_param, R_array), Gravity_input)

        print('接触力矩：\n', Torque_ex)

    def eulerAngles2rotationMat(self, theta):
        theta = [i * math.pi / 180.0 for i in theta]  # 角度转弧度

        R_x = np.array([[1, 0, 0],
                        [0, math.cos(theta[0]), -math.sin(theta[0])],
                        [0, math.sin(theta[0]), math.cos(theta[0])]
                        ])

        R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
                        [0, 1, 0],
                        [-math.sin(theta[1]), 0, math.cos(theta[1])]
                        ])

        R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
                        [math.sin(theta[2]), math.cos(theta[2]), 0],
                        [0, 0, 1]
                        ])

        # 第一个角为绕X轴旋转，第二个角为绕Y轴旋转，第三个角为绕Z轴旋转
        R = np.dot(R_x, np.dot(R_y, R_z))
        return R


def main():
    #
    force_data = [-6.349214527290314e-05, 0.0016341784503310919, -24.31537437438965]
    torque_data= [-0.25042885541915894, 0.32582423090934753, 2.255179606436286e-05]
    euler_data = [-80.50866918099089, 77.83705434751874, -9.294185889510375 + 12]

    force_data1 = [-7.469202995300293, 2.3709897994995117, -23.0179500579834]
    torque_data1= [-0.2169264256954193, 0.3719269931316376, 0.10870222747325897]
    euler_data1 = [-105.99038376663763, 60.89987226261212, -10.733422007074305 + 12]

    force_data2 = [-14.45930004119873, 0.995974063873291, -19.523677825927734]
    torque_data2= [-0.19262456893920898, 0.3845194876194, 0.1622740775346756]
    euler_data2 = [-114.24258417090118, 43.78913507089547, -19.384088817327235 + 12]


    compensation = GravityCompensation()

    compensation.Update_F(force_data)
    compensation.Update_F(force_data1)
    compensation.Update_F(force_data2)

    compensation.Update_M(torque_data)
    compensation.Update_M(torque_data1)
    compensation.Update_M(torque_data2)

    compensation.Solve_A()

    compensation.Update_f(force_data)
    compensation.Update_f(force_data1)
    compensation.Update_f(force_data2)

    compensation.Update_R(euler_data)
    compensation.Update_R(euler_data1)
    compensation.Update_R(euler_data2)

    compensation.Solve_B()

    # compensation.Solve_Force(force_data, euler_data)
    # compensation.Solve_Force(force_data1, euler_data1)
    # compensation.Solve_Force(force_data2, euler_data2)
    #
    # compensation.Solve_Torque(torque_data, euler_data)
    # compensation.Solve_Torque(torque_data1, euler_data1)
    # compensation.Solve_Torque(torque_data2, euler_data2)


if __name__ == '__main__':
    main()

Made by 水木皆Ming