CN104776822A - 多节臂架的姿态检测系统和方法 - Google Patents

Abstract

本发明公开了一种多节臂架姿态检测方法，包括：(1)通过各臂架首末两端设置的传感器模块中的三轴加速度传感器，分别测量对应臂架上相应端部的X、Y和Z方向上的加速度值；(2)根据该加速度值可获得各臂架的弯曲角度以及两相邻臂架之间的夹角；(3)利用上述弯曲角度以及夹角可得到臂架因变形产生的偏移，(4)根据所述偏移即可获得对各臂架节点空间位置坐标，从而即可实现对臂架姿态的准确测定。本发明还公开了相应的姿态检测系统。本发明可以实现对每节臂架的形变量检测并进而实现对臂架空间位置的不断修正，完成对臂架姿态的准确检测。

Description

多节臂架的姿态检测系统和方法

技术领域

本发明属于姿态控制与检测技术领域，具体涉及一种用于作业设备上的多节臂架的姿态检测系统及方法。

背景技术

作业设备的本体端和作业端之间通常设置有臂架，特别是在本体端与作业区域之间的空间距离较远时，还需要设置多节臂架，以便于远程操作控制。多节臂架通常是由多个单臂以可相对转动的方式铰接连接而成，通过控制各机械臂的姿态和方向实现对末端的作业端的位置控制。

作业设备工作时，正常工况下通常有静止工作状态和平滑移动状态两种，部分设备可能还有其他自由度的运动状态。以常见的混凝土泵送车为例，静止工作状态是指在开始泵送混凝土前对臂架进行移动，摆好臂架姿态，开始泵送混凝土后臂架始终保持不变，如在楼房装修时，用混凝土泵车将混凝土从1楼泵送到4楼的窗口，则属于静止工作状态。平滑移动状态是指泵送混凝土时，需要将臂架末端进行平滑移动，均匀地进行浇筑，例如混凝土泵车浇筑混凝土路面时，则属于平滑移动状态。

对于多节臂架的上述工况，会存在比较大的缺陷，比如静止工况下产生不规律的振动可能会带来共振，平移状态下臂架会由于惯性作用发生摆动，这些都会对作业设备整体带来安全风险。

为克服上述问题，现有技术中一般通过减振的方式避免可能产生的安全风险。减振方式通常可以分为被动减振和主动减振，其中主动减振是目前一种常见的减振方式，主动减振的关键或核心是准确检测出臂架的位置，以此为依据进行位置控制从而实现减振的目的。

现有方案中，位置检测通常通过在臂架上设置位置检测系统实现，即通过在臂架上设置加速度传感器和倾角传感器，通过检测各臂架之间的夹角和臂架末端的加速度，从而反馈给控制系统以对臂架进行控制，减小振动的影响。

但是，上述方案存在比较明显的缺陷，首先是因为多节臂架的存在，其多次累积的误差会导致最后检测结果误差较大；另外，由于臂架在受力时会产生形变，多节臂架的形变量使得上述臂架位置会存在很大的误差，而上述检测系统无法实现对单节臂架的形变检测，从而使得臂架整体的位置检测精度大大降低。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种多臂架的姿态检测系统及方法，通过在多节臂架的每节臂架上设置传感器模块并分别检测每节臂架的形变量，并进而实现对每节臂架的形变量检测并进而实现对臂架空间位置的不断修正，完成对臂架姿态的准确检测。

为实现上述目的，按照本发明一个方面，提供一种多臂架姿态检测系统，用于实现对作业设备上依次连接的多臂架的空间位置准确检测，其特征在于，该姿态检测系统包括多个传感器模块，其中每节臂架两端均设置有一个所述传感器模块，该传感器模块中具有三轴加速度传感器，以用于分别测量对应臂架上相应端部的X、Y和Z方向上的加速度值，根据该加速度值可获得各臂架的弯曲角度以及两相邻臂架之间的夹角，进而利用上述弯曲角度以及夹角可得到臂架因变形产生的偏移，根据所述偏移即可获得对各臂架节点空间位置坐标，从而即可实现对臂架姿态的准确测定。

作为本发明的改进，所述根据偏移获得各臂架节点空间位置坐标具体计算过程为：

$(\mathrm{Xe},\mathrm{Ye})=L*(\frac{1}{\sqrt{1+k^2}},\frac{k}{\sqrt{1+k^2}})\left[\begin{array}{c}\cos \left({\mathrm{\β}}_n\right),-\sin \left({\mathrm{\β}}_n\right)\\ \sin \left({\mathrm{\β}}_n\right),\cos \left({\mathrm{\β}}_n\right)\end{array}\right]+(\mathrm{Xs},\mathrm{Ys})$

式中，(Xs,Ys)为任一臂架起始坐标(Xs,Ys)，(Xe,Ye)为该节臂架末端坐标，L为该任一臂架的长度，k为偏移，β_n是该臂架n与臂架移动平面上的X轴(即水平坐标轴)的夹角。

作为本发明的改进，所述偏移k通过如下公式计算得到：

$k=\left(\frac{\mathrm{bend}\left(X\right)}{X}\right)$

式中，(X,bend(X))为曲线bend(x)上使bend′(x)＝tan(abs(Ain[2n]))的点的坐标，曲线bend(x)＝asinh(tan(x/2))/sin(x/2)，Ain[2n]为任一第n节臂架因变形而弯曲的弯曲角度。

作为本发明的改进，所述任一第n节臂架因变形而弯曲的弯曲角度A_in[2n]通过如下过程计算得到:

A_in[2n]＝θ_2n-θ_2n-1

式中，θ_2n为位于第n节臂架末端的传感器的倾角值，θ_2n-1为位于该第n节臂架起始端传感器的倾角值。

作为本发明的改进，所述任一传感器的空间倾斜角θ_i具体为：

θ_i＝arctan2(ACCZi/ACCXi)-γi

式中，θ_i为传感器i的空间倾斜角，ACCYi为传感器i检测得到的Y轴加速度，ACCXi为传感器i检测得到的X轴加速度，γ_i为传感器的重力加速度偏移值。

作为本发明的改进，所述传感器的重力加速度偏移值γ_i通过标定得到，即通过倾角仪获得传感器i所处平面的倾角真值α_i，同时获得传感器i测量得到的加速度值ACCZ_i、ACCX_i，然后根据下述公式即可得到重力加速度偏移值γ_i：

γ_i＝arctan2(ACCZ_i/ACCX_i)-α_i。

按照本发明的另一方面，提供一种多臂架姿态检测方法，用于实现对作业设备上依次连接的多臂架的空间位置准确检测，其特征在于，该姿态检测方法包括：

(1)通过各臂架首末两端设置的传感器模块中的三轴加速度传感器，分别测量对应臂架上相应端部的X、Y和Z方向上的加速度值；

(2)根据该加速度值可获得各臂架的弯曲角度以及两相邻臂架之间的夹角；

(3)利用上述弯曲角度以及夹角可得到臂架因变形产生的偏移，

(4)根据所述偏移即可获得对各臂架节点空间位置坐标，从而即可实现对臂架姿态的准确测定。

作为本发明的改进，所述根据偏移获得各臂架节点空间位置坐标具体计算过程为：

$(\mathrm{Xe},\mathrm{Ye})=L*(\frac{1}{\sqrt{1+k^2}},\frac{k}{\sqrt{1+k^2}})\left[\begin{array}{c}\cos \left({\mathrm{\β}}_n\right),-\sin \left({\mathrm{\β}}_n\right)\\ \sin \left({\mathrm{\β}}_n\right),\cos \left({\mathrm{\β}}_n\right)\end{array}\right]+(\mathrm{Xs},\mathrm{Ys})$

式中，(Xs,Ys)为任一臂架起始坐标(Xs,Ys)，(Xe,Ye)为该节臂架末端坐标，L为该任一臂架的长度，k为偏移，β_n是该臂架n与臂架移动平面上的X轴(即水平坐标轴)的夹角

作为本发明的改进，所述偏移k通过如下公式计算得到：

$k=\left(\frac{\mathrm{bend}\left(X\right)}{X}\right)$

式中，(X,bend(X))为曲线bend(x)上使bend′(x)＝tan(abs(Ain[2n]))的点的坐标，曲线bend(x)＝asinh(tan(x/2))/sin(x/2)，Ain[2n]为任一第n节臂架因变形而弯曲的弯曲角度。

作为本发明的改进，所述任一第n节臂架因变形而弯曲的弯曲角度Ain[2n]通过如下过程计算得到:

A_in[2n]＝θ_2n-θ_2n-1

式中，θ_2n为位于第n节臂架末端的传感器的倾角值，θ_2n-1为位于该第n节臂架起始端传感器的倾角值。

作为本发明的改进，所述任一传感器的空间倾斜角θ_i具体为：

θ_i＝arctan2(ACCZi/ACCXi)-γi

式中，θ_i为传感器i的空间倾斜角，ACCYi为传感器i检测得到的Y轴加速度，ACCXi为传感器i检测得到的X轴加速度，γ_i为传感器的重力加速度偏移值。

作为本发明的改进，所述传感器的重力加速度偏移值γ_i通过标定得到，即通过倾角仪获得传感器i所处平面的倾角真值α_i，同时获得传感器i测量得到的加速度值ACCZ_i、ACCX_i，然后根据下述公式即可得到重力加速度偏移值γ_i：

γ_i＝arctan2(ACCZ_i/ACCX_i)-α_i。

本发明中，传感器模块还可以包括三轴陀螺传感器，三轴陀螺仪传感器携带有3个与笛卡尔坐标系相同的陀螺仪传感器(GROX、GROY、GROZ)，分别用于X、Y、Z方向上角速度的测量，在泵车移动时补偿加速度传感器产生的误差。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效果：

(1)本发明的方案通过在每节臂架两端均设置加速度传感器，从而可以精确获得每节臂架的变形以及相邻臂架之间的夹角；

(2)本发明的方案中，通过获得的变形可采用特殊的算法获取臂架因变形产生的偏移，并进而结合臂架之间的夹角，准确获得各节臂架的精确位置，实现对臂架姿态的实时检测。

(3)本发明的方案通过获取各节臂架的形变，进而可避免对多节臂架因累积变形产生的位置精度的累积误差，并结合各节臂架上设置的陀螺仪传感器对臂架上加速度传感器误差的补偿，使得对应臂架的位置检测精度大大提高，从而反馈给控制系统以对臂架进行控制，减小振动的影响，提高安全性。

附图说明

图1为按照本发明实施例所构建的一种姿态检测系统应用于水泥泵车多臂架上的结构示意图；

图2为按照本发明实施例所构建的一种姿态检测方法中各臂架完全角度以及臂架之间夹角计算的原理示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

按照本发明实施例所构建的一种多臂架姿态检测系统为水泥泵送车的臂架，但本发明中的臂架不限于于此，可以是其他各种类型的多臂架结构，例如其他类型的工程机械或作业设备等。

本实施例中的泵送车优选有5个臂架2，但本发明中对臂架节数并不进行具体数量的限定，可以为任意其他节数。本实施例中，包括靠近泵送车的为第一臂架，它与泵车车身1相连接，紧邻第一臂架的为第二臂架，另外还包括依次相位相互连接的第三臂架、第四臂架和第五臂架，其中第五臂架距离泵车最远，作业端位于第五臂架末端。任意相邻两个臂架2之间采用液压系统3，以在臂架连接部位进行驱动，用于控制臂架2在立体空间自如伸缩、自动调节姿态或相对距离。液压系统3上的驱动杆通过连杆4与臂架连接，以用于实现相邻臂架2之间的相对转动。臂架末端5上设置用于作业的作业部件，通过各节臂架的联动实现对作业部件的位置控制。

为监测泵车臂架空间、运动等状态，在各节臂架2两端均安装有传感器模块，具体地，在第一节臂架两端分别安装两个传感器模块，分别为01和02，在第二臂架两端也分别安装两个相同类型的传感器模块，分别为03、04，以此类推，第n臂架上安装2个相同的传感器，分别为2n-1、2n，本实施例中臂架数量优选5，因此n＝5，即直至在第五臂架两端安装传感器模块09和10。这10个传感器模块可通过电缆连接到CAN总线上，各传感器模块检测到的实时信号通过例如CAN总线3传送到控制器4。

各传感器模块内部均包含有三轴加速度传感器和三轴陀螺仪传感器。其中，三轴加速度传感器携带有3个与笛卡尔坐标系相同的加速度传感器(ACCX、ACCY、ACCZ)，分别用于X、Y、Z方向上加速度的测量。三轴陀螺仪传感器携带有3个与笛卡尔坐标系相同的陀螺仪传感器(GROX、GROY、GROZ)，分别用于X、Y、Z方向上角速度的测量，用于实时检测泵车移动时测量点处的空间角速度。

各传感器模块外部设置有传感器盒体，其中的三轴加速度和三轴陀螺仪传感器设置于对应的传感器盒内部，传感器盒安装在每个臂架的端部，用于保护传感器在恶劣环境中不受干扰。

优选地，安装传感器盒时要保证传感器ZX平面尽量平行于泵车臂架竖直运行平面，所述运行平面是指泵车转台不动时，泵车臂架伸直后从垂直于地面运行到水平于地面所扫过的平面。

控制器4用于接收每个传感器传来的数据，其首先对每一个传感器数据进行处理，得到传感器真值，然后对相邻传感器的真值进行计算处理，得到每两个相邻臂架角度，及每节臂架的弯曲程度。

数据处理器4计算所得每两个相邻臂架角度及每节臂架弯曲程度的精度、稳定性，在此方案中算法将会对测算精度造成较大影响，本实施例中数据处理器4进行数据处理获得相邻臂架角度以及每节臂架的弯曲程度的具体算法下面将会详细说明。

优选地，通过三轴陀螺仪传感器对三个方向的加速度误差进行修正，从而消除累积误差。本实施例中修正方法为二阶互补滤波算法，能够有效的祛除漂移误差同时，二阶互补滤波器输入arctan2(ACCZn/ACCXn)、GROYn，输出传感器测量空间倾角真值。

标定传感器，在臂架收起的状态下，臂架没有受到负载或外力，每节臂处于近似无弯曲状态下，使用倾角仪(完备仪器)测量任一传感器i所处的平面倾角真值αi(一般优选是传感器盒体的平行于水平面的上平面的倾角)，同时记录数据处理器4处理后的传感器加速度真值数据ACCZn、ACCXn，通过如下公式：

γ_i＝arctan2(ACCZ_i/ACCX_i)-α_i

得到传感器的重力加速度相对于地球重力方向偏移值γ_i，本实施例中也称其为传感器的笛卡尔坐标系与泵车笛卡尔坐标系偏移值。

其中，αi为传感器i的实际空间倾角值，ACCYi为传感器的Y轴加速度，ACCXi为传感器的X轴加速度。

在臂架收起的状态下，臂架没有受到负载或外力，每节臂处于近似无弯曲状态，同一节臂架两端传感器倾角值相同A_in[2n]＝＝A_in[2n-1]＝＝0，其中n为节臂序号。

检测时，首先使用加速度传感器计算每个传感器的空间倾斜角θ_i：

θ_i＝arctan2(ACCZi/ACCXi)-γi

其中，i为传感器序号，θ_i为任一传感器i的空间倾斜角，ACCY_i为传感器i检测得到的Y轴加速度，ACCX_i为传感器i检测得到的X轴加速度，γ_i为传感器的重力加速度偏移值。

如图2所示，为本发明实施例中计算各臂架的弯曲角度以及臂架之间夹角的原理图。其中，P[1,2n-1]是第n节臂架起始点的X坐标，P[2,2n-1]是第n节臂架起始点的Y坐标，P[3,2n-1]是第n节臂架起始点原X方向的辐角，P[1,2n]是第n节臂架中点的X坐标，P[2,2n]是第n节臂架中点的Y坐标，P[3,2n]是第n节臂架中点原X方向的辐角。

根据各个传感器的倾角数据，计算臂架倾角A_in，其中：

A_in[1]＝θ₁；

A_in[2n]＝θ_2n-θ_2n-1；

A_in[2n-1]＝θ_2n-1-θ_2n-2 (n≠1)；

上式中，A_in[2n]表示第n节臂架的弯曲角度，A_in[2n-1]表示相邻的第n-1节臂架和第n节臂架之间的夹角。

通过上述过程，即可获得臂架的弯曲角度以及两相邻臂架之间的夹角。

在此已经获得相邻臂架的夹角与同一节臂架的弯曲度，同时通过泵车厂家可以查询到每节臂架两个轴点之间的距离也就是臂架长度，可以计算出臂架各个节点的空间笛卡尔坐标系(节点为单节臂架两端端点)下的空间坐标。

具体地，其通过如下公式计算：

bend(x)＝asinh(tan(x/2))/sin(x/2)

其中，bend(x)曲线形状近似为平抛下落曲线，在bend(0)处切线倾角为0度，asinh为双曲余弦函数。

首先，找到使bend′(x)＝tan(abs(Ain[2i]))的对应点(X,bend(X))，得到偏移比 $k=\left(\frac{\mathrm{bend}\left(X\right)}{X}\right),$

然后，根据偏移比k得到臂架的切向偏移值，具体地，对于第i节臂架起始坐标(Xs,Ys)，那么第i节臂架末端坐标为

$(\mathrm{Xe},\mathrm{Ye})=L*(\frac{1}{\sqrt{1+k^2}},\frac{k}{\sqrt{1+k^2}})\left[\begin{array}{c}\cos \left({\mathrm{\β}}_n\right),-\sin \left({\mathrm{\β}}_n\right)\\ \sin \left({\mathrm{\β}}_n\right),\cos \left({\mathrm{\β}}_n\right)\end{array}\right]+(\mathrm{Xs},\mathrm{Ys}),$

其中L为任一臂架n的长度，β_n是该臂架n与臂架移动平面上的X轴(即水平坐标轴)的夹角。

β_n通过臂架之间的夹角A_in[2n-1]计算获得，具体地，如图2所示，A_in[1]就是第一节臂架在起始端与水平坐标轴的夹角β₁，β₂＝A_in[1]+A_in[3]-180°，依次可以计算出每节臂架的夹角，即β_n＝β_n-1+Ain[2n-1]-180°。

第一节臂架的起始点坐标可通过直接标定得到，根据上述过程即可获得依次相连的各个臂架的首末端点坐标，进而模拟出臂架的空间位置，实现对臂架的姿态的准确测定。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种多节臂架姿态检测系统，用于实现对作业设备上依次连接的多臂架的空间位置准确检测，其特征在于，该姿态检测系统包括多个传感器模块，其中每节臂架两端均设置有一个所述传感器模块，该传感器模块中具有三轴加速度传感器，以用于分别测量对应臂架上相应端部的X、Y和Z方向上的加速度值，根据该加速度值可获得各臂架的弯曲角度以及两相邻臂架之间的夹角，进而利用上述弯曲角度以及夹角可得到臂架因变形产生的偏移，根据所述偏移即可获得对各臂架节点空间位置坐标，从而即可实现对臂架姿态的准确测定。

2.根据权利要求1所述的一种多节臂架姿态检测系统，其中，所述根据偏移获得各臂架节点空间位置坐标具体计算过程为：

式中，(Xs,Ys)为任一臂架n的起始坐标(Xs,Ys)，(Xe,Ye)为该节臂架末端坐标，L为该任一臂架的长度，k为偏移，β_n是该臂架n与臂架移动平面上的X轴的夹角。

3.根据权利要求2所述的一种多节臂架姿态检测系统，其中，所述偏移k通过如下公式计算得到：

式中，(X,bend(X))为曲线bend(x)上使bend′(x)＝tan(abs(Ain[2n]))的点的坐标，曲线bend(x)＝asinh(tan(x/2))/sin(x/2)，A_in[2n]为任一臂架n因变形而弯曲的弯曲角度。

4.根据权利要求3所述的一种多节臂架姿态检测系统，其中，所述任一第n节臂架因变形而弯曲的弯曲角度A_in[2n]通过如下过程计算得到:

A_in[2n]＝θ_2n-θ_2n-1

式中，θ_2n为位于第n节臂架末端的传感器的倾角值，θ_2n-1为位于该第n节臂架起始端传感器的倾角值。

5.根据权利要求4所述的一种多节臂架姿态检测系统，其中，所述任一传感器的空间倾斜角θ_i具体为：

θ_i＝arctan2(ACCZ_i/ACCX_i)-γ_i

式中，θ_i为传感器i的空间倾斜角，ACCYi为传感器i检测得到的Y轴加速度，ACCXi为传感器i检测得到的X轴加速度，γ_i为传感器的重力加速度偏移值。

6.一种多节臂架姿态检测方法，用于实现对作业设备上依次连接的多臂架的空间位置准确检测，其特征在于，该姿态检测方法包括：

(1)通过各臂架首末两端设置的传感器模块中的三轴加速度传感器，分别测量对应臂架上相应端部的X、Y和Z方向上的加速度值；

(2)根据该加速度值可获得各臂架的弯曲角度以及两相邻臂架之间的夹角；

(3)利用上述弯曲角度以及夹角可得到臂架因变形产生的偏移；

(4)根据所述偏移即可获得对各臂架节点空间位置坐标，从而即可实现对臂架姿态的准确测定。

7.根据权利要求6所述的一种多节臂架姿态检测方法，其中，所述根据偏移获得各臂架节点空间位置坐标具体计算过程为：

式中，(Xs,Ys)为任一臂架起始坐标(Xs,Ys)，(Xe,Ye)为该节臂架末端坐标，L为该任一臂架的长度，k为偏移，β_n是该臂架n与臂架移动平面上的X轴的夹角。

8.根据权利要求7所述的一种多节臂架姿态检测方法，其中，所述偏移k通 过如下公式计算得到：

式中，(X,bend(X))为曲线bend(x)上使bend′(x)＝tan(abs(Ain[2n]))的点的坐标，曲线bend(x)＝asinh(tan(x/2))/sin(x/2)，A_in[2n]为任一臂架n因变形而弯曲的弯曲角度。

9.根据权利要求8所述的一种多节臂架姿态检测方法，其中，所述任一第n节臂架因变形而弯曲的弯曲角度A_in[2n]通过如下过程计算得到:

A_in[2n]＝θ_2n-θ_2n-1

式中，θ_2n为位于第n节臂架末端的传感器的倾角值，θ_2n-1为位于该第n节臂架起始端传感器的倾角值。

10.根据权利要求9所述的一种多节臂架姿态检测方法，其中，所述任一传感器的空间倾斜角θ_i具体为：

θ_i＝arctan2(ACCZ_i/ACCX_i)-γ_i

式中，θ_i为传感器i的空间倾斜角，ACCYi为传感器i检测得到的Y轴加速度，ACCXi为传感器i检测得到的X轴加速度，γ_i为传感器的重力加速度偏移值。