CN106996765A - 一种基于姿态传感器的机器人关节角度测量方法 - Google Patents

Abstract

本发明提供了一种基于姿态传感器的机器人关节角度测量方法，机器人关节由第一连杆和第二连杆活动连接组成，在第一连杆上固定安装第一姿态传感器，在第二连杆上固定安装第二姿态传感器；第一姿态传感器和第二姿态传感器分别返回第一欧拉角和第二欧拉角，根据第一欧拉角和第二欧拉角测算机器人关节角度。本发明的有益效果：(1)制造成本低；(2)安装简单，对安装精度、安装位置没有特殊要求；(3)可以集成到电路板上，体积、重量可以忽略不计；(4)不受粉尘、油渍以及地磁场等外界因素干扰；(5)无需动力学正解计算，可以直接得到机器人某个连杆的姿态。

Description

一种基于姿态传感器的机器人关节角度测量方法

技术领域

本发明涉及一种角度测量方法，特别涉及一种基于姿态传感器的机器人关节角度测量方法，属于机器人技术领域。

背景技术

近年来，机器人技术不论在工业还是在民用领域的应用都有飞速的发展，而机器人各个关节角是机器人控制的基础。因此，具有高度通用性的机器人关节角的测量方法显得十分必要。

现有的机器人关节角通常使用安装在关节上的角度传感器直接测量，其中主要包括霍尔磁性角度传感器，光学编码盘等。这些角度传感器虽然精度高，但是价格昂贵，对安装方式有极高的要求，所占空间较大，且易受环境因素干扰。

发明内容

本发明要解决现有技术中测量机器人关节角度存在的以下几个技术问题：(1)传感器制造成本高的问题；(2)安装困难的问题；(3)体积大，重量大的问题；(4)易受环境因素干扰。

为了实现上述目的，本发明提供了一种基于姿态传感器的机器人关节角度测量方法，机器人关节包括相连的第一连杆和第二连杆，在第一连杆和/或第二连杆上安装姿态传感器，由姿态传感器返回的欧拉角或四元数测算机器人关节角度；欧拉角包括俯仰角、横滚角及航向角。

优选地，第一连杆和第二连杆活动连接，在第一连杆上固定安装第一姿态传感器，在第二连杆上固定安装第二姿态传感器；第一姿态传感器和第二姿态传感器分别返回第一欧拉角和第二欧拉角，根据第一欧拉角和第二欧拉角测算机器人关节角度。

优选地，上述测量方法包括如下步骤：

步骤一、定义惯性坐标系，定义固连于第一姿态传感器的第一坐标系，定义固连于第二姿态传感器的第二坐标系；

步骤二、将机器人关节复位至第一位置，第一姿态传感器返回第一欧拉角，第二姿态传感器返回第二欧拉角；

步骤三、根据第一欧拉角计算第一旋转矩阵，根据第二欧拉角计算第二旋转矩阵；再根据第一旋转矩阵和第二旋转矩阵计算第一连杆与第二连杆的第一位置相对旋转矩阵；

步骤四、将机器人关节调节至第二位置，第一姿态传感器返回第三欧拉角，第二姿态传感器返回第四欧拉角；

步骤五、根据第三欧拉角计算第三旋转矩阵，根据第四欧拉角计算第四旋转矩阵；再根据第三旋转矩阵和第四旋转矩阵计算第一连杆与第二连杆的第二位置相对旋转矩阵；

步骤六、根据第一位置相对旋转矩阵和第二位置相对旋转矩阵计算旋转矩阵变化量，再根据旋转矩阵变化量计算机器人关节角度。

优选地，第一欧拉角包括第一横滚角、第一俯仰角、第一航向角，第二欧拉角包括第二横滚角、第二俯仰角、第二航向角，第三欧拉角包括第三横滚角、第三俯仰角、第三航向角，第四欧拉角包括第四横滚角、第四俯仰角、第四航向角。

优选地，第一欧拉角是第一姿态传感器与惯性坐标系之间的夹角，第二欧拉角是第二姿态传感器与惯性坐标系之间的夹角。

优选地，惯性坐标系是固定于地球的大地坐标系。

优选地，测量方法包括如下具体步骤：

步骤一、定义坐标系：定义惯性坐标系为xyz，定义第一坐标系为x’y’z’，惯性坐标系和第一坐标系原点重合为O，惯性坐标系和第一坐标系之间的转换用欧拉角来描述，其中N为地磁北极方向；按同样的方式定义第二坐标系为x”y”z”；

步骤二、初始化：将机器人关节复位至第一位置，记为α₀；此时第一姿态传感器返回第一横滚角记为φ_1|0，第一俯仰角记为θ_1|0，第一航向角记为ψ_1|0；第二姿态传感器返回第二横滚角记为φ_2|0，第二俯仰角记为θ_2|0，第二航向角记为ψ_2|0；

步骤三、标定：计算第一旋转矩阵并记为¹R₀，计算第二旋转矩阵记为²R₀，第一位置相对旋转矩阵表示为通过以下公式计算：

步骤四、数据采集：机器人运动至第二位置，第三横滚角、第三俯仰角、第三航向角、第四横滚角、第四俯仰角、第四航向角分别记为φ_1|1，θ_1|1，ψ_1|1，φ_2|1，θ_2|1，ψ_2|1；

步骤五、标定：计算第三旋转矩阵记为¹R₁，计算第四旋转矩阵记为²R₁，通过以下公式计算第二位置相对旋转矩阵：

步骤六、关节角解算：计算旋转矩阵变化量，公式如下：

将旋转矩阵变化量用等效轴坐标系方法表示，即绕轴旋转角度β：

其中，r_ij表示矩阵的第i行第j列元素；

此时机器人关节对应的旋转角度为：

α₁＝α₀+β

优选地，以Z-Y-X欧拉角表示法计算¹R₀和²R₀、¹R₁和²R₁，公式如下：

将φ_1|0，θ_1|0，ψ_1|0和φ_2|0，θ_2|0，ψ_2|0、φ_1|1，θ_1|1，ψ_1|1和φ_2|1，θ_2|1，ψ_2|1分别代入上式计算¹R₀和²R₀、¹R₁和²R₁。

本发明的有益效果：(1)制造成本低；(2)安装简单，对安装精度、安装位置没有特殊要求；(3)可以集成到电路板上，体积、重量可以忽略不计；(4)不受粉尘、油渍以及地磁场等外界因素干扰；(5)无需动力学正解计算，可以直接得到机器人某个连杆的姿态。

附图说明

图1是本发明一个较佳实施例中的机器人关节的结构示意图；

图2是本发明一个较佳实施例中的欧拉角示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。需说明的是，本发明附图均采用简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施例的目的。

如图1所示，本发明提供了一种基于姿态传感器的机器人关节角度测量方法，机器人关节包括相连的连杆1和连杆2，连杆1和连杆2通过关节3活动连接，在连杆1上固定安装姿态传感器5，在连杆2上固定安装姿态传感器6；姿态传感器5和姿态传感器6分别返回欧拉角，根据欧拉角测算机器人关节角度4。传感器与连杆应固定连接，对安装精度、安装位置没有特殊要求。姿态传感器需返回欧拉角(俯仰角、横滚角及航向角)或四元数。

以欧拉角为例，测量方法如下：

(1)定义：如图2所示，定义惯性坐标系(固定于地球的大地坐标系)xyz，定义固连于姿态传感器的坐标系x’y’z’，其中，x’轴指向姿态传感器正前方，z’轴垂直于x’轴指向姿态传感器上方，y’轴垂直于x’轴和z’轴指向传感器左侧，坐标系原点为O。两个坐标系之间的转换可以用欧拉角来描述，其中N为地磁北极方向。本发明所使用的欧拉角即姿态传感器与惯性坐标系之间的夹角。

(2)初始化(复位)：机器人系统上电或者需要标定时，将关节角复位至某一固定角度(记为α₀)。此时相邻连杆上的姿态传感器返回的分别角度记为φ_1|0(横滚角)，θ_1|0(俯仰角)，ψ_1|0(航向角)和φ_2|0，θ_2|0，ψ_2|0(以φ_1|0为例，其中，φ表示横滚角，下标1为连杆编号，下标0为t₀时刻的状态编号)。

(3)标定：分别计算每个欧拉角相应的旋转矩阵，¹R₀，²R₀。(欧拉角旋转矩阵共有24种表示方法，且都被称作角坐标系表示法，采用任意一种即可。)以Z-Y-X欧拉角表示法为例：

将0时刻从连杆1到连杆2的旋转矩阵表示为

(4)数据采集：机器人运动至某一时刻t₁，此时连杆上的姿态传感器返回的分别角度记为φ_1|1，θ_1|1)，ψ_1|1和φ_2|1，θ_2|1，ψ_2|1。对应旋转矩阵为¹R₁，²R₁。将t₁时刻从连杆1到连杆2的旋转矩阵表示为

(5)关节角解算：将连杆1与连杆2旋转矩阵的变化记为

将该旋转矩阵用等效轴坐标系方法表示，即绕轴旋转角度β：

其中，r_ij表示矩阵的第i行第j列元素。

此时关节对应的旋转角度为：

α₁＝α₀+β

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (8)

1.一种机器人关节角度测量方法，其特征在于，所述机器人关节包括相连的第一连杆和第二连杆，在所述第一连杆和/或所述第二连杆上安装姿态传感器，由所述姿态传感器返回的欧拉角或四元数测算所述机器人关节角度；所述欧拉角包括俯仰角、横滚角及航向角。

2.根据权利要求1所述的一种机器人关节角度测量方法，其特征在于，所述第一连杆和所述第二连杆活动连接，在所述第一连杆上固定安装第一姿态传感器，在所述第二连杆上固定安装第二姿态传感器；所述第一姿态传感器和所述第二姿态传感器分别返回第一欧拉角和第二欧拉角，根据所述第一欧拉角和所述第二欧拉角测算所述机器人关节角度。

3.根据权利要求2所述的一种机器人关节角度测量方法，其特征在于，包括如下步骤：

步骤一、定义惯性坐标系，定义固连于所述第一姿态传感器的第一坐标系，定义固连于所述第二姿态传感器的第二坐标系；

步骤二、将所述机器人关节复位至第一位置，所述第一姿态传感器返回第一欧拉角，所述第二姿态传感器返回第二欧拉角；

步骤三、根据所述第一欧拉角计算第一旋转矩阵，根据所述第二欧拉角计算第二旋转矩阵；再根据所述第一旋转矩阵和所述第二旋转矩阵计算所述第一连杆与所述第二连杆的第一位置相对旋转矩阵；

步骤四、将所述机器人关节调节至第二位置，所述第一姿态传感器返回第三欧拉角，所述第二姿态传感器返回第四欧拉角；

步骤五、根据所述第三欧拉角计算第三旋转矩阵，根据所述第四欧拉角计算第四旋转矩阵；再根据所述第三旋转矩阵和所述第四旋转矩阵计算所述第一连杆与所述第二连杆的第二位置相对旋转矩阵；

步骤六、根据所述第一位置相对旋转矩阵和所述第二位置相对旋转矩阵计算旋转矩阵变化量，再根据所述旋转矩阵变化量计算所述机器人关节角度。

4.根据权利要求3所述的一种机器人关节角度测量方法，其特征在于，所述第一欧拉角包括第一横滚角、第一俯仰角、第一航向角，所述第二欧拉角包括第二横滚角、第二俯仰角、第二航向角，所述第三欧拉角包括第三横滚角、第三俯仰角、第三航向角，所述第四欧拉角包括第四横滚角、第四俯仰角、第四航向角。

5.根据权利要求3所述的一种机器人关节角度测量方法，其特征在于，所述第一欧拉角是所述第一姿态传感器与所述惯性坐标系之间的夹角，所述第二欧拉角是所述第二姿态传感器与所述惯性坐标系之间的夹角。

6.根据权利要求3所述的一种机器人关节角度测量方法，其特征在于，所述惯性坐标系是固定于地球的大地坐标系。

7.根据权利要求4所述的一种机器人关节角度测量方法，其特征在于，包括如下步骤：

步骤一、定义坐标系：定义所述惯性坐标系为xyz，定义所述第一坐标系为x’y’z’，所述惯性坐标系和所述第一坐标系原点重合为O，所述惯性坐标系和所述第一坐标系之间的转换用欧拉角来描述，其中N为地磁北极方向；按同样的方式定义所述第二坐标系为x”y”z”；

步骤二、初始化：将所述机器人关节复位至第一位置，记为α₀；此时所述第一姿态传感器返回第一横滚角记为φ_1|0，第一俯仰角记为θ_1|0，第一航向角记为ψ_1|0；所述第二姿态传感器返回第二横滚角记为φ_2|0，第二俯仰角记为θ_2|0，第二航向角记为ψ_2|0；

步骤三、标定：计算所述第一旋转矩阵记为¹R₀，计算所述第二旋转矩阵记为²R₀，所述第一位置相对旋转矩阵表示为通过以下公式计算：

${R_1^2}_0={\left({R_1}_0\right)}^{-1}{R_2}_0$

步骤四、数据采集：机器人运动至第二位置，所述第三横滚角、所述第三俯仰角、所述第三航向角、所述第四横滚角、所述第四俯仰角、所述第四航向角分别记为φ_1|1，θ_1|1，ψ_1|1，φ_2|1，θ_2|1，ψ_2|1；

步骤五、标定：计算所述第三旋转矩阵记为¹R₁，计算所述第四旋转矩阵记为²R₁，通过以下公式计算所述第二位置相对旋转矩阵：

${R_1^2}_1={\left({R_1}_1\right)}^{-1}{R_2}_1$

步骤六、关节角解算：计算所述旋转矩阵变化量，公式如下：

$R_0^1{}_1{}^2={\left({R_1^2}_0\right)}^{-1}{R_1^2}_1$

将所述旋转矩阵变化量用等效轴坐标系方法表示，即绕轴旋转角度β：

$\mathrm{\β}={\cos }^{-1}\left(\frac{r_{11}+r_{22}+r_{33}-1}{2}\right)$

其中，r_ij表示矩阵的第i行第j列元素；

此时所述机器人关节对应的旋转角度为：

α₁＝α₀+β

8.根据权利要求7所述的一种机器人关节角度测量方法，其特征在于，以Z-Y-X欧拉角表示法计算¹R₀和²R₀、¹R₁和²R₁，公式如下：

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

将φ_1|0，θ_1|0，ψ_1|0和φ_2|0，θ_2|0，ψ_2|0、φ_1|1，θ_1|1，ψ_1|1和φ_2|1，θ_2|1，ψ_2|1分别代入上式计算¹R₀和²R₀、¹R₁和²R₁。