汽车起重机的运动轨迹检测方法及系统

1.本发明涉及信号处理领域，具体地，涉及一种汽车起重机的运动轨迹检测方法及系统。


背景技术：

2.随着我国的工业化的迅猛发展，现代化生产过程中自动化和智能化的水平越来越高，汽车起重机行业也经历着革新，汽车起重机在工作时，吊摆不断地进行起吊、横移和落钩等动作，由于惯性力或者外力的作用会产生摇晃现象，不仅影响工作效率，而且还为威胁到人们的生命健康安全。对吊摆的运行轨迹进行检测，可以为起重机的控制系统提供参考量，还可以提高定位精度。
3.利用传感器的采集的信号来判断汽车起重机吊摆的运动轨迹。目前起重机操作大部分是靠着有经验的操作人员来执行，通过适当的减慢起重机作业速度、来回移动悬挂位置或者直接等待摆动衰减到差不多后再操作等方法来达到工作要求。这些方法在一定程度上能够控制吊车的位置以及负载的摆动。但是，这种工作强度是较大的，也是较困难的，因为这其中存在着许多不确定的因素，而且消费大量时间，工作效率低下。因此，汽车起重机的吊摆运动轨迹的检测系统具有一定的拓展应用价值。


技术实现要素：

4.针对现有技术中的缺陷，本发明的目的是提供一种汽车起重机的运动轨迹检测方法及系统。
5.根据本发明提供的汽车起重机的运动轨迹检测方法，包括如下步骤：
6.步骤s1：获取起重机吊摆的初始姿态；
7.步骤s2：获取起重机吊摆的加速度信号和角速度信号，将所述加速度信号和角速度信号转换为数字信号数据，并对所述数字信号数据进行滤波生成第一目标加速度数据和目标角速度数据；
8.步骤s3：将所述第一目标加速度数据由动态坐标系转换至静态坐标系生成静态坐标系的第二目标加速度数据；
9.步骤s4：根据所述初始姿态、第二目标加速度数据以及所述角速度数据确定所述起重机吊摆运动的轨迹。
10.优选地，在步骤s2中通过三轴加速度传感器测量起重机吊摆运动的加速度，所述加速度用于计算重机吊摆运动的速度和位移；
11.通过陀螺仪来测量重机吊摆运动的角速度；
12.通过a/d转换器将模拟信号形式的加速度和角速度转换成数字信号数据后进行存储。
13.优选地，在步骤s3中，采用四元法计算出旋转矩阵，使运动中的动态坐标系转换成初始状态的静态坐标系。
14.优选地，在在静止状态下，加速度检测值合力的矢量的值即为重力加速度，因此有：
[0015][0016]
其中，a
x
为x方向的加速度，ay为y方向的加速度，az为z方向的加速度；
[0017]
所述初始姿态包括初始姿态角(ρ，φ，θ)，其中ρ为俯仰角，φ为横滚角，θ为偏航角；
[0018]
初始姿态角与加速度检测值之间的关系为：
[0019][0020][0021][0022]
优选地，所述数字信号数据中存在随机误差和仪器误差；对于所述随机误差通过滤波进行处理，对于所述仪器误差通过零位偏移进行校正。
[0023]
优选地，在步骤s2中通过卡尔曼滤波或滑动平均滤波以有效的滤除传感器输出中的随机噪声信号。
[0024]
优选地，通过对陀螺仪采集的目标角速度数据进行求解姿态角，采用kalman滤波与加速度传感器进行角度的补偿。
[0025]
根据本发明提供的汽车起重机的运动轨迹检测系统，包括如下模块：
[0026]
初始姿态获取模块，用于获取起重机吊摆的初始姿态；
[0027]
信号获取模块，用于获取起重机吊摆的加速度信号和角速度信号，将所述加速度信号和角速度信号转换为数字信号数据，并对所述数字信号数据进行滤波生成第一目标加速度数据和目标角速度数据；
[0028]
坐标系转换模块，用于将所述第一目标加速度数据由动态坐标系转换至静态坐标系生成静态坐标系的第二目标加速度数据；
[0029]
轨迹确定模块，用于根据所述初始姿态、第二目标加速度数据以及所述角速度数据确定所述起重机吊摆运动的轨迹。
[0030]
根据本发明提供的汽车起重机的运动轨迹检测系统，包括如下模块：
[0031]
数据采集处理模块，用于采集加速度信号和和角速度信号，将所述加速度信号和角速度信号转换为数字信号数据，并对所述数字信号数据进行滤波生成第一目标加速度数据和目标角速度数据；
[0032]
数据发送模块，用于发送所述第一目标加速度数据和目标角速度数据；
[0033]
数据接收模块，用于接收第一目标加速度数据和目标角速度数据，并将接收到第一目标加速度数据和目标角速度数据通上传至上位机；
[0034]
电源块，用于为数据采集处理模块、数据发送模块以及数据接收模块供电。
[0035]
与现有技术相比，本发明具有如下的有益效果：
[0036]
本发明先由惯性导航器件采集相关的数据信号，为了降低数据测量误差，设计了kalman滤波器来消除随机噪声，根据四元数理论建立微分方程，并由此计算姿态矩阵，从而
求得姿态角，通过多传感器数据融合算法，提高了姿态角检测精度；本发明针对吊钩的位置检测，通过检测吊绳的摆角实现，间接计算得到吊钩的运动轨迹，实现对汽车起重机运动轨迹的检测。
附图说明
[0037]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0038]
图1为本发明实施例中汽车起重机传感器安装的示意图；
[0039]
图2为本发明实施例中汽车起重机的运动轨迹检测方法的步骤流程图；
[0040]
图3为本发明实施例中汽车起重机的运动轨迹检测系统的系统框图；
[0041]
图4为本发明实施例中kalman滤波算法流程图；
[0042]
图5为本发明实施例中汽车起重机的运动轨迹检测方法具体实施的步骤流程图；
[0043]
图6为本发明实施例中汽车起重机的运动轨迹检测方系统的模块示意图。
具体实施方式
[0044]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
[0045]
本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例，例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
[0046]
下面以具体地实施例对本发明的技术方案进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。
[0047]
下面以具体地实施例对本发明的技术方案以及本技术的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本发明的实施例进行描述。
[0048]
图1为本发明实施例中汽车起重机传感器安装的示意图，图2为本发明实施例中汽车起重机的运动轨迹检测方法的步骤流程图，如图1、图2所示，本发明提供的汽车起重机的运动轨迹检测方法，包括如下步骤：
[0049]
步骤s1：获取起重机吊摆的初始姿态；
[0050]
步骤s2：获取起重机吊摆的加速度信号和角速度信号，将所述加速度信号和角速
度信号转换为数字信号数据，并对所述数字信号数据进行滤波生成第一目标加速度数据和目标角速度数据；
[0051]
步骤s3：将所述第一目标加速度数据由动态坐标系转换至静态坐标系生成静态坐标系的第二目标加速度数据；
[0052]
步骤s4：根据所述初始姿态、第二目标加速度数据以及所述角速度数据确定所述起重机吊摆运动的轨迹。
[0053]
在本发明实施例中，在步骤s2中通过三轴加速度传感器测量起重机吊摆运动的加速度，所述加速度用于计算重机吊摆运动的速度和位移；
[0054]
通过陀螺仪来测量重机吊摆运动的角速度；
[0055]
通过a/d转换器将模拟信号形式的加速度和角速度转换成数字信号数据后进行存储。
[0056]
图3为本发明实施例中汽车起重机的运动轨迹检测系统的系统框图，如图3所示，首先进行坐标变换，即以物体的加速度作为参数把动态坐标系下的加速度值转换为静态坐标系下进行计算。传感器获得的数据是载体坐标系下的运动数据，需要进行坐标转换获得地理坐标系下的数据。
[0057]
在本发明实施例中，采用四元法计算出旋转矩阵，使运动中的动态坐标系转换成初始状态的静态坐标系。
[0058]
所述动态坐标系为汽车起重机吊摆的载体坐标，所述静态坐标系为地球坐标系。
[0059]
根据牛顿定律，对加速度的二重积分则是物体的位置，但是其中会存在着一些误差，并随着时间的增大，累积误差也逐渐增大，为此，采取相关算法对随机误差进行消除，最后对数据进行积分处理，求解轨迹。
[0060]
通过加速度传感器获得的运动状态数据是相对于载体坐标系下的运动信号，为此，进行坐标变换获得地理坐标系下的运动状态，然后对陀螺仪的数据进行求解姿态角，采用kalman滤波与加速度传感器实现角度的补偿。
[0061]
根据牛顿定律，只要测出物体运动的加速度值，便可以求得物体运动的速度与位移，即对吊摆运动的加速度进行积分可以得到吊摆运动的速度，二重积分可以得到吊摆的位移。因此，使用加速度传感器便可以得到吊摆运动的瞬时速度和运动轨迹，即
[0062][0063][0064]
对于离散的数据，为了简化计算，可以使用迭代的方法，即
[0065][0066][0067]
但是，对位置的积分运算会产生加速度传感器和陀螺仪的误差的累积，对
[0068]
在本发明实施例中，所述加速度传感器选用anolog devices公司的adxl序列三轴加速度数字传感器芯片，该传感器体积小、运行功耗低、具有13位采样分辨率，而且可以根据需求设置加速度的量程。
[0069]
所述陀螺仪选用了adi公司生产的三轴陀螺仪adis16355。adis16355是adi公司isensortm集成技术推出的小封装单芯片的完整角速度测量系统，可以通过简单的spi串行接口提供出厂校准的和数字可调节的传感器数据。
[0070]
在本发明实施例中，在在静止状态下，加速度检测值合力的矢量的值即为重力加速度，因此有：
[0071][0072]
其中，a
x
为x方向的加速度，ay为y方向的加速度，az为z方向的加速度。
[0073]
所述初始姿态包括初始姿态角(ρ，φ，θ)，其中ρ为俯仰角，φ为横滚角，θ为偏航角；
[0074]
初始姿态角与加速度检测值之间的关系为：
[0075][0076][0077][0078]
图4为本发明实施例中kalman滤波算法流程图，如图4所示，在步骤s2中通过卡尔曼滤波或滑动平均滤波以有效的滤除传感器输出中的随机噪声信号。在获取加速度信号的过程中可能存在着一些随机的噪声信号，对于这些信号，不可以使用积分进行求解，为此，采用kalman滤波的方法滤除随机噪声。
[0079]
kalman滤波的作用是利用目标的动态信息，从一系列有限的包含噪声的测量结果中设法去除掉噪声的影响，得到有用信号估计的结果。其状态方程为：
[0080]
x(k)＝ak(k-1)+bu(k)+w(k)
[0081]
其中，x(k)表示系统在k时刻的状态，u(k)表示k时刻控制器对系统的控制量，w(k)表示过程噪声，a表示作用在(k-1)上的状态变换参数，b表示作用在控制量u(k)上的输入控制参数。
[0082]
用来描述当前状态的可见输出的观测方程，如下式所所示：
[0083]
z(k)＝hk(k)+v(k)
[0084]
其中，z(k)表示k时刻的观测值，h表示观测参数，v(k)表示观测噪声。一般认为，在初始状态x(0)以及每个时刻的噪声w(k)和v(k)都是相互独立的。
[0085]
kalman器的设计过程包含两个阶段：预测阶段与更新阶段。在预测阶段中，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用当前状态的观测值优化在预测阶段获取的预测值，进而获得一个更加精确的新估计值，
[0086]
kalman滤波算法通过对测量数据前的预估计数据与测量之后的校正补偿可以实现误差的最小化。kalman滤波算法主要有五大黄金公式，它们分别是：
[0087]
计算先验状态估计：
[0088]
计算协方差矩阵：
[0089]
计算卡尔曼增益：
[0090]
计算后状态估计：
[0091]
计算后验误差协方差矩阵：
[0092]
图5为本发明实例中累积误差消除的算法流程图，如图5所示，在kalman滤波完成之后，在对加速度进行积分的过程中存在着微小的误差，在不断积分的过程中会随着时间的积累，误差会不断增大，影响运动轨迹检测的精准性。为此，需要采取相关的算法来消除累积误差。结合汽车起重机实际运行时的一些特点，并根据这些特点进行积累误差的后期补偿，在设计是使用补偿的方法对加速度进行修正和补偿，加速度的修正公式为：
[0093][0094]
积累误差与时间成正比，可以得到加速度的补偿公式：
[0095][0096]
经过加速度修正后，对积分得到的速度进行修正，其修正公式为：
[0097][0098]
其中，k1、k2分别为运动开始的时间和运动结束的时间。
[0099]
在本发明实施例中，所述数字信号数据中存在随机误差和仪器误差；对于所述随机误差通过滤波进行处理，对于所述仪器误差通过零位偏移进行校正。
[0100]
在本发明实施例中，本发明提供的汽车起重机的运动轨迹检测系统，包括如下模块：
[0101]
初始姿态获取模块，用于获取起重机吊摆的初始姿态；
[0102]
信号获取模块，用于获取起重机吊摆的加速度信号和角速度信号，将所述加速度信号和角速度信号转换为数字信号数据，并对所述数字信号数据进行滤波生成第一目标加速度数据和目标角速度数据；
[0103]
坐标系转换模块，用于将所述第一目标加速度数据由动态坐标系转换至静态坐标系生成静态坐标系的第二目标加速度数据；
[0104]
轨迹确定模块，用于根据所述初始姿态、第二目标加速度数据以及所述角速度数据确定所述起重机吊摆运动的轨迹。
[0105]
图3为本发明实施例中汽车起重机的运动轨迹检测系统的系统框图，图6为本发明实施例中汽车起重机的运动轨迹检测方系统的模块示意图，如图3、图6所示，本发明提供的汽车起重机的运动轨迹检测系统，包括如下模块：
[0106]
数据采集处理模块，用于采集加速度信号和和角速度信号，将所述加速度信号和角速度信号转换为数字信号数据，并对所述数字信号数据进行滤波生成第一目标加速度数据和目标角速度数据；
[0107]
数据发送模块，用于发送所述第一目标加速度数据和目标角速度数据。
[0108]
数据接收模块，用于接收第一目标加速度数据和目标角速度数据，并将接收到第一目标加速度数据和目标角速度数据通通过rs-232串口上传至上位机；
[0109]
电源块，用于为数据采集处理模块、数据发送模块以及数据接收模块供电。
[0110]
在本发明实施例中，将连续获取的加速度数据暂存在主芯片的内部存储空间中，在本发明实例中采用型号为st意法半导体公司cortex-m系列stm32f750芯片，stm32f750芯片时钟频率为216mhz，具有64kb flash存储空间，适用于大量数据高速运算和存储。
[0111]
数据发送模块，采集电路发送的加速度数据，采用nordic公司推出的射频片上系统nrf24e1。该芯片内嵌8051兼容微控制器、rf收发器和9通道10位a/d转换器。芯片采用fsk调制/解调方式，其工作频率稳定，外围电路器件少，功耗低，系统稳定性高。
[0112]
在本发明实施例中，软件系统模块包含：cpu初始化模块、i/o口定义和控制模块、main函数模块、通讯模块、历史数据存储、数据标注模块、权限设置模块、显示模块等。这些软件设计模块与硬件系统模块一一对应，实现汽车起重机吊摆运动轨迹检测系统的检测目的。
[0113]
由于加速度传感器和串口通信单元的工作电压均为3.3v，而陀螺仪的工作电压为5v。因此选用3.3v和5v的直流稳压模块分别为加速度传感器和陀螺仪等模块供电。
[0114]
本发明实施案例中，将加速度传感器和陀螺仪采集信号经过a/d转换、滤波处理，对处理过的数据使用四元法进行坐标变换，然后采取卡尔曼滤波算法对随机噪声进行消除，根据牛顿定律，对加速度的二重积分则是物体的位置，但是其中会存在着一些误差，并随着时间的增大，累积误差也逐渐增大，为此，采取相关算法对随机误差进行消除，最后对数据进行积分处理，求解轨迹。对随机噪声的消除和累积误差的消除，可以更好的判断物体运动时所在位置的准确度，更好检测出汽车起重机吊摆的运动轨迹的变化。
[0115]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
[0116]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。