一种剪叉车水平传感器解决方案制造技术

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

【技术实现步骤摘要】
一种剪叉车水平传感器解决方案


[0001]本专利技术涉及水平传感器
，具体为一种剪叉车水平传感器解决方 案。

技术介绍

[0002]剪叉车为了安全运行，需要加装水平传感器，超过一定倾斜角度后进行 报警、限制行走，防止高空行走出现倾覆危险；目前剪叉车需要加装一个独 立的水平传感器模块，通过该传感器输出倾斜报警信号；该传感器产品需要 专门安装，并且提供线束接口，输出的信号只能是开关量。
[0003]目前外加水平传感器的方案，增加了剪叉车厂家的成本、影响了生产效 率；该产品只能输出倾斜报警信号，不能计算剪叉车倾斜的角度；剪叉车爬 坡或者下坡无法实现更好的控制，水平传感器模块增加了机器故障点，因此 本专利技术提供了一种剪叉车水平传感器解决方案。

技术实现思路

[0004](一)解决的技术问题
[0005]针对现有技术的不足，本专利技术提供了一种剪叉车水平传感器解决方案， 解决了上述
技术介绍
中提出的问题。
[0006](二)技术方案
[0007]为实现以上目的，本专利技术提供如下技术方案：一种剪叉车水平传感器解 决方案，所述假设下图三轴原点处为剪叉车，X轴正方向为叉车前进方向,需 得到剪叉车绕y轴旋转的角度下文中称为俯仰角pitch，绕x轴旋转的角度下 文中都称为横滚角roll；所述加速度计在静态条件下准确测出三个方向的加 速度，但在动态条件下存在一定的局限性和无法避免的误差，单纯的单轴陀 螺仪可以测量绕某轴旋转的角速度，但是在积分过程存在累计误差，陀螺仪 本身的特性也存在零漂和温漂。
[0008][0009][0010]优选的，所述选择ST公司六轴传感器(MEMS)LSM6DS，具有3D数字加 速度计和3D数字陀螺仪，根据这两个传感器进行原始数据互补融合，而达到 数据精确角度定位；控制器以100Hz的频率通过扫描方式读取传感器六个数 据：三个加速度和三个角加速度，LSM6DS3安装于控制器中，可通过软件设置 传感器三轴方向，以适应控制器的不同安装方式。
[0011]优选的，所述该方法采用四元数法计算我们所需要的姿态角，所述由四 元数与姿态关系得出四元数表示的从物体系变换到地理系的姿态变换矩阵 (四元数已进行归一化处理)；
[0012][0013]所述欧拉角表示的从物体系变换到地理系的姿态变换矩阵；
[0014][0015]令：
[0016]g1＝2(q1q2
‑
q0q2)
[0017]g2＝2(q2q3+q0q1)
[0018][0019]g4＝2(q1q2+q0q3)
[0020][0021]可得：
[0022]γ＝
‑
arcsin(g1)
[0023][0024][0025]优选的，所述姿态四元数参数计算求解：通过权利要求3所列公式推导， 若知道q0、q1、q2、q3的具体数值，就能计算得到需要的姿态角，但目前我 们只有通过LSM6DS3传感器获得的三轴角速度和三轴角加速度下面引入微分 方程，假设有一微分方程；
[0026][0027]微分方程解的形式为；
[0028]x[t+Δt]＝x[t]+Δtf[x[t]，w[t]][0029]可以看出求其解可以用迭代的方法(工程上也叫一阶毕卡法)，四元数微 分方程为：
[0030][0031]其解的形式可写为；
[0032][0033]通过一系列推导可求得(具体过程可查阅四元数相关资料)：
[0034][0035]通过迭代，可由以下步骤计算四元数参数：
[0036][0037][0038][0039][0040]所述陀螺仪测得的角速度，由上式可知，可用前一时刻四元数的值，迭 代更新求得下一时刻四元数的值，可在初始时刻给定四元数初值，通过陀螺 仪测得的角速度去更新四元数，通过四元数就可求得姿态变换矩阵，求出姿 态角，也通过四元数不断的更新，实现了物体姿态的实时更新，但由于陀螺 仪测得的角速度有偏差，会导致最终角度出现偏差，下面利用三轴加速度计 获得的角度信息去矫正陀螺仪获得的姿态信息，通过PI控制器消除偏差。
[0041]优选的，所述加速度计获得的值为，对其进行归一化处理：
[0042][0043][0044][0045]优选的，所述加速度计所测得的数据时物体坐标系下的值，需要算出利 用角速度计算出的姿态矩阵中的物体坐标系下的重力分量，将重力分量记为Vx、Vy、Vz；
[0046][0047]优选的，所述将加速度计获得的重力向量归一化后的值与提取的姿态矩 阵的重力向量叉乘获得姿态误差向量；
[0048][0049]记；
[0050]ex＝ay*Vz
‑
az*Vy
[0051]ey＝az*Vx
‑
ax*Vz
[0052]ez＝ax*Vy
‑
ay*Vx
[0053]优选的，所述对误差进行积分，积分可消除误差。记accex、accey、accez为x、y、z轴对应的误差积分结果，ki为积分系数，dt为积分周期；
[0054]accex＝accex+ex*ki*dt
[0055]accey＝accey+ey*ki*dt
[0056]accez＝accez+ez*ki*dt
[0057]所述将误差输入PI控制器与本次姿态更新中陀螺仪测得的角速度相加， 获得修正的角速度值，再利用修正的角速度值取更新四元数，从而可以获得 较为准确的姿态角信息，此过程也相当于一个互补滤波的过程，其中kp为比 例系数；
[0058]gx＝gx+kp*ex+accex
[0059]gy＝gy+kp*ey+accey
[0060]gz＝gz+kp*ez+accez
[0061](三)有益效果
[0062]与现有技术相比，本专利技术提供了一种剪叉车水平传感器解决方案，具备 以下有益效果：
[0063]1、该剪叉车水平传感器解决方案，通过采用加速度计和数字陀螺仪两个 传感器，进行原始数据互补融合，达到精确角度定位；控制器以100Hz的频 率通过扫描方式读取传感器六个数据：三个加速度和三个角加速度，可通过 软件设置传感器三轴方向，以适应控制器的不同安装方式。
[0064]2、该剪叉车水平传感器解决方案，通过采用四元数法计算姿态角，并用 PI调节器消除误差，提高角度计算准确性，剪叉车将不再需要加装水平传感 器；该方案不仅能输出倾斜报警信号，还能输出前进方向、左右方向的倾斜 角度，在爬坡或者下坡时实现更好的安全控制。
[0065]3、该剪叉车水平传感器解决方案，通过水平零位自学习功能，剪叉控制 器产品安装好之后可以进行自学习，记录由于安装导致的角度误差，剪叉控 制器不需要绝对水平安装，生产、装配更加容易。
附图说明
[0066]图1为本专利技术提出的一种剪叉车水平传感器解决方案倾角计算流程图；
[0067]图2为本专利技术提出的一种剪叉车水本文档来自技高网...

【技术保护点】

【技术特征摘要】
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)arcsin(g1)4.根据权利要求1所述的一种剪叉车水平传感器解决方案，其特征在于：所述姿态四元数参数计算求解：通过权利要求3所列公式推导，若知道q0、q1、q2、q3的具体数值，就能计算得到需要的姿态角，但目前我们只有通过LSM6DS3传感器获得的三轴角速度和三轴角加速度下面引入微分方程，假设有一微分方程；
微分方程解的形式为；x[t+Δt]＝x[t]+Δtf[x[t]，w[t]]可以看出求其解可以用迭代的方法(工程上也叫一阶毕卡法)，四元数微分方程为：其解的形式可写为；通过一系列推导可求得(具体过程可查阅四元数相关资料)：通过迭代，可由以下步骤计算四元数参数：通过迭代，可由以下步...

【专利技术属性】
技术研发人员：曹成昆，
申请(专利权)人：汉博来自控科技上海有限公司，
类型：发明
国别省市：