[发明专利]多节臂架的姿态检测系统和方法

权利要求书

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

其中，所述根据偏移获得各臂架节点空间位置坐标具体计算过程为：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，所述根据偏移获得各臂架节点空间位置坐标具体计算过程为：

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

6.根据权利要求5所述的一种多节臂架姿态检测方法，其中，所述偏移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因变形而弯曲的弯曲角度。