CN115406405A - 一种剪叉车水平传感器解决方案 - Google Patents

Abstract

本发明涉及水平传感器技术领域，且公开了一种剪叉车水平传感器解决方案，所述假设下图三轴原点处为剪叉车，X轴正方向为叉车前进方向,需得到剪叉车绕y轴旋转的角度下文中称为俯仰角pitch，绕x轴旋转的角度下文中都称为横滚角ro l l，所述加速度计在静态条件下准确测出三个方向的加速度，但在动态条件下存在一定的局限性和无法避免的误差。该剪叉车水平传感器解决方案，通过采用加速度计和数字陀螺仪两个传感器，进行原始数据互补融合，达到精确角度定位；控制器以100Hz的频率通过扫描方式读取传感器六个数据：三个加速度和三个角加速度，可通过软件设置传感器三轴方向，以适应控制器的不同安装方式。

Description

一种剪叉车水平传感器解决方案

技术领域

本发明涉及水平传感器技术领域，具体为一种剪叉车水平传感器解决方 案。

背景技术

剪叉车为了安全运行，需要加装水平传感器，超过一定倾斜角度后进行 报警、限制行走，防止高空行走出现倾覆危险；目前剪叉车需要加装一个独 立的水平传感器模块，通过该传感器输出倾斜报警信号；该传感器产品需要 专门安装，并且提供线束接口，输出的信号只能是开关量。

目前外加水平传感器的方案，增加了剪叉车厂家的成本、影响了生产效 率；该产品只能输出倾斜报警信号，不能计算剪叉车倾斜的角度；剪叉车爬 坡或者下坡无法实现更好的控制，水平传感器模块增加了机器故障点，因此 本发明提供了一种剪叉车水平传感器解决方案。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种剪叉车水平传感器解决方案， 解决了上述背景技术中提出的问题。

(二)技术方案

为实现以上目的，本发明提供如下技术方案：一种剪叉车水平传感器解 决方案，所述假设下图三轴原点处为剪叉车，X轴正方向为叉车前进方向,需 得到剪叉车绕y轴旋转的角度下文中称为俯仰角pitch，绕x轴旋转的角度下 文中都称为横滚角roll；所述加速度计在静态条件下准确测出三个方向的加 速度，但在动态条件下存在一定的局限性和无法避免的误差，单纯的单轴陀 螺仪可以测量绕某轴旋转的角速度，但是在积分过程存在累计误差，陀螺仪 本身的特性也存在零漂和温漂。

优选的，所述选择ST公司六轴传感器(MEMS)LSM6DS，具有3D数字加 速度计和3D数字陀螺仪，根据这两个传感器进行原始数据互补融合，而达到 数据精确角度定位；控制器以100Hz的频率通过扫描方式读取传感器六个数 据：三个加速度和三个角加速度，LSM6DS3安装于控制器中，可通过软件设置 传感器三轴方向，以适应控制器的不同安装方式。

优选的，所述该方法采用四元数法计算我们所需要的姿态角，所述由四 元数与姿态关系得出四元数表示的从物体系变换到地理系的姿态变换矩阵 (四元数已进行归一化处理)；

所述欧拉角表示的从物体系变换到地理系的姿态变换矩阵；

令：

g1＝2(q1q2-q0q2)

g2＝2(q2q3+q0q1)

g4＝2(q1q2+q0q3)

可得：

γ＝-arcsin(g1)

优选的，所述姿态四元数参数计算求解：通过权利要求3所列公式推导， 若知道q0、q1、q2、q3的具体数值，就能计算得到需要的姿态角，但目前我 们只有通过LSM6DS3传感器获得的三轴角速度和三轴角加速度下面引入微分 方程，假设有一微分方程；

微分方程解的形式为；

x[t+Δt]＝x[t]+Δtf[x[t]，w[t]]

可以看出求其解可以用迭代的方法(工程上也叫一阶毕卡法)，四元数微 分方程为：

其解的形式可写为；

通过一系列推导可求得(具体过程可查阅四元数相关资料)：

通过迭代，可由以下步骤计算四元数参数：

所述陀螺仪测得的角速度，由上式可知，可用前一时刻四元数的值，迭 代更新求得下一时刻四元数的值，可在初始时刻给定四元数初值，通过陀螺 仪测得的角速度去更新四元数，通过四元数就可求得姿态变换矩阵，求出姿 态角，也通过四元数不断的更新，实现了物体姿态的实时更新，但由于陀螺 仪测得的角速度有偏差，会导致最终角度出现偏差，下面利用三轴加速度计 获得的角度信息去矫正陀螺仪获得的姿态信息，通过PI控制器消除偏差。

优选的，所述加速度计获得的值为，对其进行归一化处理：

优选的，所述加速度计所测得的数据时物体坐标系下的值，需要算出利 用角速度计算出的姿态矩阵中的物体坐标系下的重力分量，将重力分量记为Vx、Vy、Vz；

优选的，所述将加速度计获得的重力向量归一化后的值与提取的姿态矩 阵的重力向量叉乘获得姿态误差向量；

记；

ex＝ay*Vz-az*Vy

ey＝az*Vx-ax*Vz

ez＝ax*Vy-ay*Vx

优选的，所述对误差进行积分，积分可消除误差。记accex、accey、accez为x、y、z轴对应的误差积分结果，ki为积分系数，dt为积分周期；

accex＝accex+ex*ki*dt

accey＝accey+ey*ki*dt

accez＝accez+ez*ki*dt

所述将误差输入PI控制器与本次姿态更新中陀螺仪测得的角速度相加， 获得修正的角速度值，再利用修正的角速度值取更新四元数，从而可以获得 较为准确的姿态角信息，此过程也相当于一个互补滤波的过程，其中kp为比 例系数；

gx＝gx+kp*ex+accex

gy＝gy+kp*ey+accey

gz＝gz+kp*ez+accez

(三)有益效果

与现有技术相比，本发明提供了一种剪叉车水平传感器解决方案，具备 以下有益效果：

1、该剪叉车水平传感器解决方案，通过采用加速度计和数字陀螺仪两个 传感器，进行原始数据互补融合，达到精确角度定位；控制器以100Hz的频 率通过扫描方式读取传感器六个数据：三个加速度和三个角加速度，可通过 软件设置传感器三轴方向，以适应控制器的不同安装方式。

2、该剪叉车水平传感器解决方案，通过采用四元数法计算姿态角，并用 PI调节器消除误差，提高角度计算准确性，剪叉车将不再需要加装水平传感 器；该方案不仅能输出倾斜报警信号，还能输出前进方向、左右方向的倾斜 角度，在爬坡或者下坡时实现更好的安全控制。

3、该剪叉车水平传感器解决方案，通过水平零位自学习功能，剪叉控制 器产品安装好之后可以进行自学习，记录由于安装导致的角度误差，剪叉控 制器不需要绝对水平安装，生产、装配更加容易。

附图说明

图1为本发明提出的一种剪叉车水平传感器解决方案倾角计算流程图；

图2为本发明提出的一种剪叉车水平传感器解决方案水平零位自学习过 程图。

图3为本发明提出的一种剪叉车水平传感器解决方案轴向示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行 清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而 不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做 出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-2，本发明提供一种技术方案：假设下图三轴原点处为剪叉车， X轴正方向为叉车前进方向,需得到剪叉车绕y轴旋转的角度下文中称为俯仰 角pitch，绕x轴旋转的角度下文中都称为横滚角roll；加速度计在静态条 件下准确测出三个方向的加速度，但在动态条件下存在一定的局限性和无法 避免的误差，单纯的单轴陀螺仪可以测量绕某轴旋转的角速度，但是在积分 过程存在累计误差，陀螺仪本身的特性也存在零漂和温漂，选择ST公司六轴 传感器(MEMS)LSM6DS，具有3D数字加速度计和3D数字陀螺仪，根据这两个传感器进行原始数据互补融合，而达到数据精确角度定位；控制器以100Hz 的频率通过扫描方式读取传感器六个数据：三个加速度和三个角加速度， LSM6DS3安装于控制器中，可通过软件设置传感器三轴方向，以适应控制器的 不同安装方式，该方法采用四元数法计算我们所需要的姿态角，由四元数与姿 态关系得出四元数表示的从物体系变换到地理系的姿态变换矩阵(四元数已 进行归一化处理)；

欧拉角表示的从物体系变换到地理系的姿态变换矩阵；

令：

g1＝2(q1q2-q0q2)

g2＝2(q2q3+q0q1)

g4＝2(q1q2+q0q3)

可得：

γ＝-arCsin(g1)

姿态四元数参数计算求解：通过权利要求3所列公式推导，若知道q0、 q1、q2、q3的具体数值，就能计算得到需要的姿态角，但目前我们只有通过 LSM6DS3传感器获得的三轴角速度和三轴角加速度下面引入微分方程，假设有 一微分方程；

微分方程解的形式为；

x[t+Δt]＝x[t]+Δtf[x[t]，w[t]]

可以看出求其解可以用迭代的方法(工程上也叫一阶毕卡法)，四元数微 分方程为：

其解的形式可写为；

通过一系列推导可求得(具体过程可查阅四元数相关资料)：

通过迭代，可由以下步骤计算四元数参数：

陀螺仪测得的角速度，由上式可知，可用前一时刻四元数的值，迭代更新 求得下一时刻四元数的值，可在初始时刻给定四元数初值，通过陀螺仪测得 的角速度去更新四元数，通过四元数就可求得姿态变换矩阵，求出姿态角， 也通过四元数不断的更新，实现了物体姿态的实时更新，但由于陀螺仪测得 的角速度有偏差，会导致最终角度出现偏差，下面利用三轴加速度计获得的 角度信息去矫正陀螺仪获得的姿态信息，通过PI控制器消除偏差，加速度计 获得的值为，对其进行归一化处理：

加速度计所测得的数据时物体坐标系下的值，需要算出利用角速度计算 出的姿态矩阵中的物体坐标系下的重力分量，将重力分量记为Vx、Vy、Vz；

将加速度计获得的重力向量归一化后的值与提取的姿态矩阵的重力向量 叉乘获得姿态误差向量；

记；

ex＝ay*Vz-az*Vy

ey＝az*Vx-ax*Vz

ez＝ax*Vy-ay*Vx

对误差进行积分，积分可消除误差。记accex、accey、accez为x、y、z 轴对应的误差积分结果，ki为积分系数，dt为积分周期；

accex＝accex+ex*ki*dt

accey＝accey+ey*ki*dt

accez＝accez+ez*ki*dt

将误差输入PI控制器与本次姿态更新中陀螺仪测得的角速度相加，获得 修正的角速度值，再利用修正的角速度值取更新四元数，从而可以获得较为 准确的姿态角信息，此过程也相当于一个互补滤波的过程，其中kp为比例系 数；

gx＝gx+kp*ex+accex

gy＝gy+kp*ey+accey

gz＝gz+kp*ez+accez。

综上所述，该剪叉车水平传感器解决方案，通过采用加速度计和数字陀 螺仪两个传感器，进行原始数据互补融合，达到精确角度定位；控制器以100Hz 的频率通过扫描方式读取传感器六个数据：三个加速度和三个角加速度，可 通过软件设置传感器三轴方向，以适应控制器的不同安装方式。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来 将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示 这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包 括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包 括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括 没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备 所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的 要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外 的相同要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而 言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行 多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限 定。

Claims (8)

1.一种剪叉车水平传感器解决方案，其特征在于：所述假设下图三轴原点处为剪叉车，X轴正方向为叉车前进方向,需得到剪叉车绕y轴旋转的角度下文中称为俯仰角pitch，绕x轴旋转的角度下文中都称为横滚角roll；所述加速度计在静态条件下准确测出三个方向的加速度，但在动态条件下存在一定的局限性和无法避免的误差，单纯的单轴陀螺仪可以测量绕某轴旋转的角速度，但是在积分过程存在累计误差，陀螺仪本身的特性也存在零漂和温漂。

2.根据权利要求1所述的一种剪叉车水平传感器解决方案，其特征在于：所述选择ST公司六轴传感器(MEMS)LSM6DS，具有3D数字加速度计和3D数字陀螺仪，根据这两个传感器进行原始数据互补融合，而达到数据精确角度定位；控制器以100Hz的频率通过扫描方式读取传感器六个数据：三个加速度和三个角加速度，LSM6DS3安装于控制器中，可通过软件设置传感器三轴方向，以适应控制器的不同安装方式。

3.根据权利要求1所述的一种剪叉车水平传感器解决方案，其特征在于：所述该方法采用四元数法计算我们所需要的姿态角，所述由四元数与姿态关系得出四元数表示的从物体系变换到地理系的姿态变换矩阵(四元数已进行归一化处理)；

所述欧拉角表示的从物体系变换到地理系的姿态变换矩阵；

令：

g1＝2(q1q2-q0q2)

g2＝2(q2q3+q0q1)

g4＝＝2(q1q2+q0q3)

可得：

γ＝＝-arcsin(g1)

4.根据权利要求1所述的一种剪叉车水平传感器解决方案，其特征在于：所述姿态四元数参数计算求解：通过权利要求3所列公式推导，若知道q0、q1、q2、q3的具体数值，就能计算得到需要的姿态角，但目前我们只有通过LSM6DS3传感器获得的三轴角速度和三轴角加速度下面引入微分方程，假设有一微分方程；

微分方程解的形式为；

x[t+Δt]＝x[t]+Δtf[x[t]，w[t]]

可以看出求其解可以用迭代的方法(工程上也叫一阶毕卡法)，四元数微分方程为：

其解的形式可写为；

通过一系列推导可求得(具体过程可查阅四元数相关资料)：

通过迭代，可由以下步骤计算四元数参数：

所述陀螺仪测得的角速度，由上式可知，可用前一时刻四元数的值，迭代更新求得下一时刻四元数的值，可在初始时刻给定四元数初值，通过陀螺仪测得的角速度去更新四元数，通过四元数就可求得姿态变换矩阵，求出姿态角，也通过四元数不断的更新，实现了物体姿态的实时更新，但由于陀螺仪测得的角速度有偏差，会导致最终角度出现偏差，下面利用三轴加速度计获得的角度信息去矫正陀螺仪获得的姿态信息，通过PI控制器消除偏差。

5.根据权利要求1所述的一种剪叉车水平传感器解决方案，其特征在于：所述加速度计获得的值为，对其进行归一化处理：

6.根据权利要求1所述的一种剪叉车水平传感器解决方案，其特征在于：所述加速度计所测得的数据时物体坐标系下的值，需要算出利用角速度计算出的姿态矩阵中的物体坐标系下的重力分量，将重力分量记为Vx、Vy、Vz；

7.根据权利要求1所述的一种剪叉车水平传感器解决方案，其特征在于：所述将加速度计获得的重力向量归一化后的值与提取的姿态矩阵的重力向量叉乘获得姿态误差向量；

记；

ex＝ay*Vz-az*Vy

ey＝az*Vx-ax*Vz

ez＝ax*Vy-ay*Vx

8.根据权利要求1所述的一种剪叉车水平传感器解决方案，其特征在于：所述对误差进行积分，积分可消除误差。记accex、accey、accez为x、y、z轴对应的误差积分结果，ki为积分系数，dt为积分周期；

accex＝accex+ex*ki*dt

accey＝accey+ey*ki*dt

accez＝accez+ez*ki*dt

所述将误差输入PI控制器与本次姿态更新中陀螺仪测得的角速度相加，获得修正的角速度值，再利用修正的角速度值取更新四元数，从而可以获得较为准确的姿态角信息，此过程也相当于一个互补滤波的过程，其中kp为比例系数；

gx＝gx+kp*ex+accex

gy＝gy+kp*ey+accey

gz＝gz+kp*ez+accez

Images