CN115042182B - 一种基于倾角传感器的机械臂位姿控制方法 - Google Patents

Abstract

本申请涉及工程机械技术领域，公开了一种基于倾角传感器的机械臂位姿控制方法，包括以下步骤：S1、构建根据所述倾角传感器测量所述机械臂旋转角度的数学模型；S2、获取所述倾角传感器的输出值，所述倾角传感器安装在所述机械臂的关节处；S3、通过所述倾角传感器的输出值和所述数学模型解算所述机械臂的旋转角度；S4、根据所述机械臂的旋转角度解算所述机械臂的偏航角；S5、根据所述机械臂的偏航角，优化所述机械臂的位姿控制。本申请的机械臂位姿控制方法测量结果稳定、可靠，给工程人员带来了极大的方便。

Description

一种基于倾角传感器的机械臂位姿控制方法

技术领域

本发明属于工程机械领域，尤其涉及基于倾角传感器的机械臂位姿控制方法。

背景技术

现代技术中，关于机械臂空间旋转角度的测量有多种，如陀螺仪等。陀螺仪能检测当前旋转速率，通过速率积分获得当前的旋转角度，进而对机械臂进行全方位的角度检测。但是陀螺仪存在游走误差，即使在机械臂角度不变的情况下，陀螺仍有输出信号，无法做到精确测量旋转角度。现有技术中存在利用倾角传感器测量机械壁旋转角度的方案，如授权公告号为CN205219117U的中国实用新型专利中，公开了一种基于倾角传感器的姿态自纠正的机械手系统，通过倾角传感器测量机械手实际运动角度，然后利用微处理器来判断和控制运动角度补偿，以实现对于机械手的自校准。但是，该专利仅公开了倾角传感器、电机驱动器和电源模块等结构特征，对于如何解算倾角传感器输出值和机械手的运动角度之间的关系，以期能够实现对于机械臂的运动控制，是本领域技术人员所未知的。

发明内容

本发明提供了一种基于倾角传感器的机械臂位姿控制方法，以解决上述技术问题的至少一个技术问题。

本发明所采用的技术方案为：

本发明提供了一种基于倾角传感器的机械臂位姿控制方法，其特征在于，包括以下步骤：

S1、构建根据所述倾角传感器测量所述机械臂旋转角度的数学模型；

S2、获取所述倾角传感器的输出值，所述倾角传感器安装在所述机械臂的关节处；

S3、通过所述倾角传感器的输出值和所述数学模型解算所述机械臂的旋转角度；

S4、根据所述机械臂的旋转角度解算所述机械臂的偏航角；

S5、根据所述机械臂的偏航角，优化所述机械臂的位姿控制。

作为本发明的一种优选实施方式，所述倾角传感器为双轴倾角传感器，所述步骤S1包括：

设置水平面为所述倾角传感器测量轴的参考平面，所述倾角传感器的测量轴X、Y轴互相垂直；

根据所述参考平面，建立第一坐标系oxyz，y轴为所述机械臂的旋转轴在水平面上的投影，x轴垂直于y轴且与y轴同处于水平面内，z轴垂直于水平面并与x、y轴相交于o点；

将机械臂旋转一定角度后，建立第二坐标系ouvw，ou轴与x轴重合，ov轴与所述机械臂的旋转轴重合，ow轴为过点o的uov平面的法向量；

根据所述第一坐标系oxyz和第二坐标系ouvw，建立所述倾角传感器的测量值和所述机械臂旋转角度的数学模型。

作为本发明的一种优选实施方式，所述步骤S1中，根据所述第一坐标系oxyz和第二坐标系ouvw，建立所述倾角传感器的测量值和所述机械臂旋转角度的数学模型具体如下：

tx＝arcsin(sinθcosβ) (1)

ty＝β (2)

其中，tx为所述机械臂转动角度θ后所述倾角传感器测量轴X轴输出值，ty为所述倾角传感器测量轴Y轴输出值，θ为所述机械臂的旋转角度，β为所述机械臂的旋转轴与水平面的夹角。

作为本发明的一种优选实施方式，所述步骤S4中，根据机械臂的旋转角度解算机械臂的偏航角的具体方法如下：

设tx、ty为倾角传感器X、Y轴输出值，由公式(1)得所述机械臂未旋转时初始旋转角度θ₁′表达式如下：

其中，θ₁′为未旋转的情况下因机械臂载体倾斜计算得到的旋转角度，tx₁为所述倾角传感器X₁轴未旋转时的输出值，ty₁为倾角传感器Z₁轴与平面的夹角；

所述倾角传感器旋转角度θ后，旋转角度θ₁″的表达式如下：

其中，θ₁″为旋转角度θ计算得到的旋转角度，tx₁′为所述倾角传感器的测量轴X轴的输出值为；

根据机械臂的旋转角度解算偏航角的表达式为：

其中，α'为所述机械臂未旋转时相对于水平面的偏航角，α”为所述机械臂旋转角度θ后相对于水平面的偏航角；

对(3)、(4)、(5)、(6)式进行处理得:

α＝α”-α'

其中，α为所述机械臂实际的偏航角。

作为本发明的一种优选实施方式，所述步骤S2中，所述倾角传感器的测量轴与机械臂的旋转轴平行且处于同一方向。

作为本发明的一种优选实施方式，所述机械臂的旋转轴与水平面的夹角为β，β的取值范围为β∈(0,90°)。

由于采用了上述技术方案，本发明所取得的有益效果为：

本发明涉及一种通过倾角传感器测量机械臂偏航角并根据偏航角控制机械臂到达目标位置的方法。倾角传感器能够检测任何一个非垂直于水平面的检测位置与水平面之间的夹角，而且测量结果稳定、可靠，比较适用于工程应用，可通过应用倾角传感器测量机械臂的偏航位置进而控制机械臂的位姿达到目标状态。该方法采用倾角传感器为测量工具，根据倾角传感器的安装方式和非垂直于水平面的旋转轴之间的空间关系形成未知的空间角度，由此建立倾角传感器输出值与旋转角度的数学模型，根据建好的数学模型和已知条件求出旋转角度，最终推导出求解机械臂偏航角的公式。根据偏航角以此来控制机械臂偏离目标位置的情况，进而优化机械臂的位姿控制。本发明方法避免了工程机械采用陀螺仪等方式测量偏航角导致精度降低、累积误差增大的问题，给工程人员带来了极大的方便。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本申请，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例的基于倾角传感器的机械臂位姿控制方法的流程示意图；

图2为本发明实施例的空间模型的坐标系示意图；

图3为本发明实施例提供的机械臂的结构示意图；

图4为本发明实施例中应用空间模型测量偏航角的示意图。

其中，

1-机械臂；

2-倾角传感器；

3-车体。

具体实施方式

为了更清楚的阐释本申请的整体构思，下面结合说明书附图以示例的方式进行详细说明。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

另外，在本发明的描述中，需要理解的是，术语“顶”、“底”、“内”、“外”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是通信；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。

机械臂1的种类有很多，多数为喷涂类机械臂1，本专利中的机械臂1以巷道支护中喷浆机械臂1为例。

本发明提供的一种基于倾角传感器2的机械臂1位姿控制方法，如图1所示，包括以下步骤：

S1、构建根据所述倾角传感器2测量所述机械臂1旋转角度的数学模型；

S2、获取所述倾角传感器2的输出值，所述倾角传感器2安装在所述机械臂1的关节处；

S3、通过所述倾角传感器2的输出值和所述数学模型解算所述机械臂1的旋转角度；

S4、根据所述机械臂1的旋转角度解算所述机械臂1的偏航角；

S5、根据所述机械臂1的偏航角，优化所述机械臂1的位姿控制。

作为本发明的一种优选实施方式，所述倾角传感器为双轴倾角传感器，所述步骤S1包括：

设置水平面为所述倾角传感器测量轴的参考平面，所述倾角传感器的测量轴X、Y轴互相垂直；

根据所述参考平面，建立第一坐标系oxyz，y轴为所述机械臂的旋转轴在水平面上的投影，x轴垂直于y轴且与y轴同处于水平面内，z轴垂直于水平面并与x、y轴相交于o点；

将机械臂旋转一定角度后，建立第二坐标系ouvw，ou轴与x轴重合，ov轴与所述机械臂的旋转轴重合，ow轴为过点o的uov平面的法向量；

根据所述第一坐标系oxyz和第二坐标系ouvw，建立所述倾角传感器的测量值和所述机械臂旋转角度的数学模型。

作为一种优选实施方式，如图2-图3所示，所述步骤S1中，所述倾角传感器2为双轴倾角传感器2，利用倾角传感器2测量机械臂1旋转角度的数学模型具体如下：

使得双轴倾角传感器2的测量轴X、Y轴互相垂直，其输出信号是以水平面H为参考的角度；设倾角传感器的旋转轴为L，逆时针旋转为正，旋转角度为θ，设旋转轴L与水平面夹角为β，以旋转轴L在水平面上的投影为y轴；x轴垂直于y轴且与y轴同处于H面内，z轴垂直于水平面并与x、y轴相交于o点，构成笛卡尔坐标系oxyz；令倾角传感器2的测量轴X轴与笛卡尔坐标系的x轴重合，测量轴Y轴与旋转轴L重合，设转动一定角度后倾角传感器2的测量轴X轴转到X'；建立新的笛卡尔坐标系ouvw，设ou与x轴重合，ov与旋转轴L重合，设过点o的uov平面的法向量为ow，XOY平面的法向量为M；

以M在yoz平面上为初始参考零位；

坐标系ouvw由坐标系oxyz绕ox轴为中心顺时针旋转角度β后得到，其旋转矩阵为:

设z轴在oxyz坐标系的单位向量为Z＝[0 0 1]，其在ouvw坐标系的表示为：

Z′＝Z·R_OX(β)＝[0 -sinβ cosβ]

旋转轴L、测量轴X在ouvw坐标系下的单位向量分别为：

L＝[0 1 0]

X＝[1 0 0]

倾角传感器2在ouvw坐标系下绕ov轴旋转角度θ，其旋转矩阵为：

则，测量轴X'的向量为：

X'＝R_v(θ).X^T＝[cosθ 0 -sinθ]

测量轴Y与水平面夹角始终为ty＝β；

设测量轴X'与OZ轴夹角记<X',Z′>，X'与水平面夹角为tx，则而的余弦为：

倾角传感器2的测量值为：

tx＝arcsin(sinθcosβ) (1)

ty＝β (2)

由此，建立倾角传感器2的X、Y轴输出值与旋转角θ之间的数学模型。

可以理解的是，所述的倾角传感器2可为双轴或单轴，本实施例中以双轴倾角传感器2为例。

在一个实施方式，如图3所示，所述步骤S2中，预定安装方式中，所述倾角传感器2的测量轴与机械臂1的旋转轴平行且处于同一方向，以使得旋转过程中倾角传感器2的输出不变，始终等于机械臂1的旋转轴与水平面的夹角β，若旋转轴L垂直于水平面，倾角传感器无法测量机械臂的偏航角，β的取值范围为β∈(0,90°)。

如图4所示，所述步骤S3中，根据机械臂1的旋转角度解算机械臂1的偏航角的具体方法如下：

设tx、ty为倾角传感器2的测量轴X、Y轴的输出值，α为喷浆机械臂1实际的偏航角，tx₁为倾角传感器2的测量轴X₁轴未旋转时的输出值，α'为此时机械臂1相对于水平面的偏航角；设所述机械臂1旋转角度θ，旋转后的倾角X₁轴记为倾角X₁′，倾角传感器2的测量轴X₁轴新的输出值为tx₁′，α”为此时机械臂1相对于水平面的偏航角；θ₁′为未旋转的情况下因机械臂载体比如车体3倾斜而导致的解算角度，ty₁为Z₁轴与平面的夹角，θ₁″为旋转后根据公式所得到由初始值即X轴与坐标轴X重合所旋转的角度；

由公式(1)得未旋转时θ₁'

旋转角度θ后，

由图4中几何关系推得：

同样得：

将(3)、(4)式代入(5)、(6)式整理得:

由图4中几何关系推得α为：

α＝α”-α'

由此，倾角传感器2每测出一组数据得都能计算出相应的偏航角，根据解算出机械臂1的偏航角后，进而对机械臂1的偏航角的大小进行控制，最终达到优化机械臂1位姿的目的，提升机械臂1位姿控制精确度。

本发明中未述及的地方采用或借鉴已有技术即可实现。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。

以上所述仅为本发明的实施例而已，并不用于限制本发明。对于本领域技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims (2)

1.一种基于倾角传感器的机械臂位姿控制方法，其特征在于，包括以下步骤：

S1、构建根据所述倾角传感器测量所述机械臂旋转角度的数学模型，包括：

设置水平面为所述倾角传感器测量轴X、Y轴的参考平面，所述倾角传感器的测量轴X、Y轴互相垂直；

根据所述参考平面，建立第一坐标系oxyz，y轴为所述机械臂的旋转轴在水平面上的投影，x轴垂直于y轴且与y轴同处于水平面内，z轴垂直于水平面并与x、y轴相交于o点；

将机械臂旋转一定角度后，建立第二坐标系ouvw，ou轴与x轴重合，ov轴与所述机械臂的旋转轴重合，ow轴为过点o的uov平面的法向量；

根据所述第一坐标系oxyz和第二坐标系ouvw，建立所述倾角传感器的测量值和所述机械臂旋转角度的数学模型，具体如下：

tx＝arcsin(sinθcosβ) (1)

ty＝β (2)

其中，tx为所述机械臂转动角度θ后所述倾角传感器测量轴X轴输出值，ty为所述倾角传感器测量轴Y轴输出值，θ为所述机械臂的旋转角度，β为所述机械臂的旋转轴与水平面的夹角；

S2、获取所述倾角传感器的输出值，所述倾角传感器安装在所述机械臂的关节处；其中，所述倾角传感器的测量轴与机械臂的旋转轴平行且处于同一方向；

S3、通过所述倾角传感器的输出值和所述数学模型解算所述机械臂的旋转角度；

S4、根据所述机械臂的旋转角度解算所述机械臂的偏航角，包括：

由公式(1)得所述机械臂未旋转时初始旋转角度θ₁′表达式如下：

其中，θ₁′为未旋转的情况下因所述机械臂载体倾斜计算得到的旋转角度，tx₁为所述倾角传感器的测量轴X轴未旋转时的输出值，ty₁为所述倾角传感器Y₁轴与平面的夹角；

所述倾角传感器旋转角度θ后，旋转角度θ₁″的表达式如下：

其中，θ₁″为所述机械臂旋转角度θ计算得到的旋转角度，tx₁′为所述倾角传感器的测量轴X轴的输出值为；

根据机械臂的旋转角度解算偏航角的表达式为：

其中，α'为所述机械臂未旋转时相对于水平面的偏航角，α”为所述机械臂旋转角度θ后相对于水平面的偏航角；

对(3)、(4)、(5)、(6)式进行处理得:

α＝α”-α'

其中，α为所述机械臂实际的偏航角；

S5、根据所述机械臂的偏航角，优化所述机械臂的位姿控制。

2.根据权利要求1所述的一种基于倾角传感器的机械臂位姿控制方法，其特征在于，β的取值范围为β∈(0,90°)。