[发明专利]一种基于姿态传感器的机器人关节角度测量方法

说明书

技术领域

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

背景技术

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

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

发明内容

本发明要解决现有技术中测量机器人关节角度存在的以下几个技术问题：(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₁，通过以下公式计算第二位置相对旋转矩阵：