CN102928012A - 一种引流线舞动监测装置和方法 - Google Patents

Abstract

本发明提供一种引流线舞动在线监测装置和方法，所叙述的方法包括：舞动在线监测前端对引流线最大弧垂点处的三维舞动加速度和三维舞动角速度进行实时在线监测；监测得到的数据通过Zigbee通信模块传输至通信控制主机；通信控制主机接收各监测点的舞动监测数据，进行现场数据存储，同时根据远程监控中心下发的控制指令，通过GPRS通信模块将所需监测点的监测数据上传至远程监控中心，远程监控中心根据接收到的数据，分析计算后，实时显示引流线舞动幅度、舞动速度和舞动频率等参数，并动态再现引流线在三维空间中的舞动轨迹。

Description

一种引流线舞动监测装置和方法

技术领域

发明关于电力系统中引流线舞动的监测技术，特别是关于引流线舞动的在线监测技术，具体是一种引流线舞动在线监测装置及方法。 

背景技术

20世纪50年代以来，随着输电线路的发展、超高压线路的广泛兴建，舞动事故日益频繁。如2008年1月至2月，我国南方地区发生了罕见的大面积、长时间冰冻自然灾害。我国中南部地区电网因冰灾停运电力线路达36740条；因灾停运变电站共2018座；110千伏至500千伏线路因灾倒塔共8381基。其中，因导线发生舞动而导致变电设备损害，是这次电网受灾严重的一个重要原因。导线舞动会使相邻悬垂串产生剧烈摆动，两端导线张力发生显著变化，引起差频荷载导致金具损坏、导线断股、相间短路、杆塔倾斜或倒塌等严重事故。因此，对舞动的研究越来越受到人们的重视。 

随着信息技术的迅速发展，架空电力线路正逐渐采用无线监测系统实现对输电线路运行状态的监测。中国发明专利申请，公开号为CN101470013A的发明专利公开了一种对输电线路舞动的监测方法，根据接收到的线路舞动加速度计算舞动参数，并生成舞动轨迹。但是，由于传感器种类单一，仅用加速度信息对时间二次积分无法得到精确的舞动参数，而且监测对象针对整个输电线路。作为输电线路中的关键部分——引流线，因其自身弧垂大、拉伸力小等特点，相对于架空输电线路更容易发生舞动。本发明首次使用加速度计和陀螺仪实时监测引流线三维舞动加速度和三维舞动角速度，通过无线传输和相关计算后，在远程监控中心计算机实时显示引流线舞动速度、舞动幅度和舞动频率等参数，并动态再现引流线在三维空间中的舞动轨迹。 

发明内容

针对目前引流线舞动监测装置及方法的不足，本发明提供一种引流线舞动在线监测装置及方法，以定量监测引流线舞动幅度、舞动速度和舞动频率等参数，并动态再现引流线在三维空间中的舞动轨迹。 

本发明的目的之一是：提供一种引流线舞动在线监测装置，包括舞动在线监测前端、Zigbee通信模块、通信控制主机、GPRS通信模块、远程监控中心及电源模块等。其中舞动在线监测前端包括控制器单元、加速度计和陀螺仪，控制器单元用于采集并处理传感器监测数据，加速度计用于监测引流线舞动监测点的三维舞动加速度，陀螺仪用于监测引流线舞动监测点的三维舞动角速度；Zigbee通信模块用于舞动在线监测前端与通信控制主机之间双向数据无线传输；GPRS通信模块用于远程监控中心和通信控制主机之间双向数据无线传输； 通信控制主机接收周围各监测点的舞动监测数据，并进行现场数据存储，同时根据远程监控中心下发的控制指令，通过GPRS通信模块将所需监测点的监测数据上传至远程监控中心；远程监控中心用于接收舞动监测数据，进行分析计算后，实时显示引流线舞动幅度、舞动速度和舞动频率等参数，并动态再现引流线三维空间中的舞动轨迹；电源模块采用太阳能取电方式，用于提供舞动在线监测前端、Zigbee通信模块、通信控制主机和GPRS通信模块工作所需电能。 

本发明的目的之二是：提供一种引流线舞动在线监测方法，舞动在线监测前端在线监测到的引流线舞动参数，首先通过Zigbee通信模块传输至通信控制主机，通信控制主机将数据存储后，根据接收到的远程监控中心指令将所需数据通过GPRS通信模块传输至远程监控中心，远程监控中心使用所述方法对接收到的数据进行分析、计算，从而实时显示引流线舞动幅度、舞动速度和舞动频率等参数，并动态再现引流线三维空间中的舞动轨迹。 

所叙述的方法包括舞动起停时刻的确定、漂移误差修正和参数分析计算三个步骤。 

其舞动起停时刻的确定过程如下：根据大量数据采样、分析获取最小角速度向量ω_min＝(ω_x，min，ω_y，min，ω_z，min)。设定角速度向量阈值为K＝1.05ω_min，如果ω_i≤K，且至少持续两个采样周期，则认为i时刻为前一舞动停止时刻，当ω_j＞K时，则认为j时刻为当前舞动起始时刻，每监测出一次舞动起停时刻，计为一次舞动，每相邻两次舞动时间间隔计为舞动周期，其倒数为舞动频率。 

其漂移误差修正的过程如下：选用ZUPT算法对漂移误差进行修正，即每次舞动起始时刻，如果V不为零，则强制置为零，将因传感器信号漂移等原因引起的误差限制在单次采样周期内，不会随着积分的进行被连续放大。 

其参数分析计算的过程如下：首先建立基坐标系和舞动坐标系，其中，基坐标系以大地作为参考，舞动坐标系以引流线上的传感器测量轴作为参考。通过加速度计测得舞动坐标系中的三维加速度值，再根据旋转变换矩阵计算得到基坐标系中的舞动加速度a_x，a_y，a_z，计算式为： 

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\\ 1\end{array}\right]=\mathrm{Rot}(z,\mathrm{\γ})\mathrm{Rot}(y,\mathrm{\β})\mathrm{Rot}(x,\mathrm{\α})\left[\begin{array}{c}{a}_{\mathrm{xraw}}\\ a_{\mathrm{yraw}}\\ a_{\mathrm{zraw}}\\ 1\end{array}\right]$

$=\left[\begin{array}{cccc}\mathrm{c\γc\β}& -\mathrm{s\γc\α}+\mathrm{c\γs\βs\α}& \mathrm{s\γs\α}+\mathrm{c\γs\βc\α}& 0\\ \mathrm{s\γc\β}& \mathrm{c\γc\α}+\mathrm{s\γs\βs\α}& -\mathrm{c\γs\α}+\mathrm{s\γs\βc\α}& 0\\ -\mathrm{s\β}& \mathrm{c\βs\α}& \mathrm{c\βc\α}& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{a}_{\mathrm{xraw}}\\ a_{\mathrm{yraw}}\\ a_{\mathrm{zraw}}\\ 1\end{array}\right]$

式中Rot(z，γ)、Rot(y，β)、Rot(x，α)分别表示绕Z轴旋转γ角度、绕Y轴旋转β角度、绕X轴旋转α角度，a_x，a_y，a_z为基坐标系中X、Y、Z三个方向上加速度分量，a_xraw，a_yraw，a_zraw为舞动坐标系中X、Y、Z三个方向上加速度分量，可由加速度计测得，s表示正弦公式，c表示余弦公式。 

α、β、γ可由以下公式求得： 

$\mathrm{\α}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\∫}_0^T{\mathrm{\ω}}_{x\left(i\right)}\mathrm{dt},$ $\mathrm{\β}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\∫}_0^T{\mathrm{\ω}}_{y\left(i\right)}\mathrm{dt},$ $\mathrm{\γ}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\∫}_0^T{\mathrm{\ω}}_{z\left(i\right)}\mathrm{dt}$

式中，ω_x、ω_y、ω_z分别为绕X轴、Y轴、Z轴的旋转角速度，可由陀螺仪测得，n为采样周期的个数，T为采样周期。 

换算后，a_x、a_y、a_z可用α、β、γ、a_xraw、a_yraw、a_zraw表示如下： 

a_x＝a_xrawcγcβ-a_yrawsγcα+a_yrawcγsβsα+a_zrawsγsα+a_zrawcγsβcα 

a_y＝a_xrawsγcβ+a_yrawcγcα+a_yrawsγsβsα-a_zrawcγsα+a_zrawsγsβcα 

a_z＝-a_xrawsβ+a_yrawcβsα+a_zrawcβcα 

消除重力加速度影响后： 

$a_{z_0}=-a_{\mathrm{xraw}}\mathrm{s\β}+a_{\mathrm{yraw}}\mathrm{c\βs\α}+a_{\mathrm{zraw}}\mathrm{c\βc\α}-(-1)$

X、Y、Z三个方向上第n个采样时刻舞动速度分量为： 

$v_{x\left(n\right)}=v_{x(n-1)}+{\∫}_0^T{a}_{x\left(n\right)}\mathrm{dt}=v_{x(n-1)}+a_{x\left(n\right)}T$

$v_{y\left(n\right)}=v_{y(n-1)}+{\∫}_0^T{a}_{y\left(n\right)}\mathrm{dt}=v_{y(n-1)}+a_{y\left(n\right)}T$

$v_{z\left(n\right)}=v_{z(n-1)}+{\∫}_0^T{a}_{z\left(n\right)}\mathrm{dt}=v_{z(n-1)}+a_{z\left(n\right)}T$

X、Y、Z三个方向上第n个采样时刻舞动位移分量为： 

$d_{x\left(n\right)}=d_{x(n-1)}+{\∫}_0^T{v}_{x\left(n\right)}\mathrm{dt}=d_{x(n-1)}+\frac{1}{2}{a}_{x\left(n\right)}{T}^2$

$d_{y\left(n\right)}=d_{y(n-1)}+{\∫}_0^T{v}_{y\left(n\right)}\mathrm{dt}=d_{y(n-1)}+\frac{1}{2}{a}_{y\left(n\right)}{T}^2$

$d_{z\left(n\right)}=d_{z(n-1)}+{\∫}_0^T{v}_{z\left(n\right)}\mathrm{dt}=d_{z(n-1)}+\frac{1}{2}{a}_{z\left(n\right)}{T}^2$

式中，T为采样时间间隔；d_x(i)、d_y(i)、d_z(i)为第i个采样周期时X、Y、Z三个方向上的舞动幅度；v_y(i)、v_y(i)、v_z(i)为第i个采样周期时X、Y、Z三个方向上的舞动速度。由d_x、d_y、d_z合成相对于地面的舞动轨迹。 

本发明的特点和显著效果主要体现在： 

a)、克服了已有监测方法中传感器单一的弊端，结合加速度计和陀螺仪，针对引流线自身的特点，专门进行引流线舞动参数的精确测量和舞动轨迹的动态再现； 

b)、收集大量可靠的引流线舞动监测数据，为建立合理的引流线舞动数学模型和广泛开展引流线舞动防治研究提供第一手资料。 

附图说明

图1为本发明装置结构框图； 

图2为舞动在线监测前端安装示意图； 

图3为舞动在线监测前端电气原理图； 

图4为坐标系示意图。 

具体实施方式

下面结合附图对发明做进一步的说明。如图1所示，本发明具体实施例包括舞动在线监测前端101、Zigbee通信模块102、通信控制主机103、GPRS通信模块104、远程监控中心105及电源模块106。 

舞动在线监测前端101装于金属筒内，固定于引流线最大弧垂点处，与引流线等电位，外观图如图2所示，安装示意图如图3所示。舞动在线监测前端101包括控制器单元201、加速度计202和陀螺仪203，电气原理图如图4所示。控制器单元201选用TI公司的MSP430F149控制器，主要用于采集和处理加速度计202和陀螺仪203的监测数据，其连接简单外围电路构成最小系统；加速度计202选用ADX345，主要用于采集监测点的三维舞动加速度值，其通过SPI接口方式和控制器单元201连接，具体引脚连接方式为：引脚1、6接3.3V电源正极，引脚4、5和6接电源地，引脚12、13和14分别接MSP430F149引脚30、29和31，引脚7、8和9分别接MSP430F149引脚21、20和28，其余引脚悬空；陀螺仪203选用ITG3205，主要用于采集监测点的三维舞动角速度值，其通过I²C接口方式和控制器单 元201连接，具体引脚连接方式为引脚8、13接3.3V电源正极，引脚11、18接电源地，引脚10、20分别通过0.1μF电容和2.2nF电容接地，引脚9、12分别接MSP430F149引脚19、18，引脚23、24分别接MSP430F149引脚15、16，其余引脚悬空。 

Zigbee通信模块102选用NRF24L01模块，分别与舞动在线监测前端101和通信控制主机103连接，完成二者间的双向数据通信。 

通信控制主机103选用PC104工控机，并配备大容量硬盘，其通过Zigbee通信模块102接收周围各监测点的舞动监测数据，并进行现场数据存储，同时通过GPRS通信模块104接收远程监控中心105下发的控制指令，通过GPRS通信模块104将所需监测点的监测数据上传至远程监控中心105。 

GPRS通信模块104选用SIM900，分别于通信控制主机103和远程监控中心计算机105连接，主要用于二者间控制指令和监测数据的双向传输。 

远程监控中心105选用工业PC机，和GPRS通信模块104连接，主要用于接收舞动监测数据，进行分析计算后，通过人机界面以曲线方式实时显示引流线舞动幅度、舞动速度和舞动频率等参数，并动态再现引流线三维空间中的舞动轨迹，也可以读取历史监测数据进行舞动分析。 

电源模块106选用太阳能取电方式，分别为舞动在线监测前端101、Zigbee通信模块102、通信控制主机103和GPRS通信模块104提供工作所需电能。 

系统工作过程如下：舞动在线监测前端101中的控制器单元201控制加速度计202和陀螺仪203实时在线采集引流线三维舞动加速度和三维舞动角速度，然后将采集到的数据通过Zigbee通信模块102传输至通信控制主机103，通信控制主机103通过GPRS通信模块104接收远程监控中心105发送的控制指令，没有接收到控制指令，只对数据进行现场存储，否则，根据接收到的控制指令，通过GPRS通信模块104将所需监测点数据传送至远程监控中心105，远程监控中心105存储所接收的数据的同时，对其进行处理、分析，主要包括舞动起停时刻的确定、漂移误差修正和参数分析计算三个步骤。 

舞动起停时刻的确定过程如下：根据大量数据采样、分析获取最小角速度向量ω_min＝(ω_x，min，ω_y，min，ω_z，min)。设定角速度向量阈值为K＝1.05ω_min，如果ω_i≤K，且至少持续两个采样周期，则认为i时刻为前一舞动停止时刻，当ω_j＞K时，则认为j时刻为当前舞动起始时刻，每监测出一次舞动起停时刻，计为一次舞动，每相邻两次舞动时间间隔计为舞动周期，其倒数为舞动频率。 

漂移误差修正的过程如下：选用ZUPT算法对漂移误差进行修正，即每次舞动起始时刻， 如果V不为零，则强制置为零，将因传感器信号漂移等原因引起的误差限制在单次采样周期内，不会随着积分的进行被连续放大。 

参数分析计算的过程如下：首先建立基坐标系和舞动坐标系，其中基坐标系以大地作为参考，舞动坐标系以引流线上的传感器测量轴作为参考，如图4所示。通过加速度计测得舞动坐标系中的三维加速度值，再根据旋转变化矩阵计算得到基坐标系中的舞动加速度a_x，a_y，a_z，计算式为： 

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\\ 1\end{array}\right]=\mathrm{Rot}(z,\mathrm{\γ})\mathrm{Rot}(y,\mathrm{\β})\mathrm{Rot}(x,\mathrm{\α})\left[\begin{array}{c}{a}_{\mathrm{xraw}}\\ a_{\mathrm{yraw}}\\ a_{\mathrm{zraw}}\\ 1\end{array}\right]$

$=\left[\begin{array}{cccc}\mathrm{c\γc\β}& -\mathrm{s\γc\α}+\mathrm{c\γs\βs\α}& \mathrm{s\γs\α}+\mathrm{c\γs\βc\α}& 0\\ \mathrm{s\γc\β}& \mathrm{c\γc\α}+\mathrm{s\γs\βs\α}& -\mathrm{c\γs\α}+\mathrm{s\γs\βc\α}& 0\\ -\mathrm{s\β}& \mathrm{c\βs\α}& \mathrm{c\βc\α}& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{a}_{\mathrm{xraw}}\\ a_{\mathrm{yraw}}\\ a_{\mathrm{zraw}}\\ 1\end{array}\right]$

式中Rot(z，γ)、Rot(y，β)、Rot(x，α)分别表示绕Z轴旋转γ角度、绕Y轴旋转β角度、绕X轴旋转α角度，a_x，a_y，a_z为基坐标系中X、Y、Z三个方向上加速度分量，a_xraw，a_yraw，a_zraw为舞动坐标系中X、Y、Z三个方向上加速度分量，可由加速度计测得，s表示正弦公式，c表示余弦公式。。 

α、β、γ可由以下公式求得： 

$\mathrm{\α}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\∫}_0^T{\mathrm{\ω}}_{x\left(i\right)}\mathrm{dt},$ $\mathrm{\β}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\∫}_0^T{\mathrm{\ω}}_{y\left(i\right)}\mathrm{dt},$ $\mathrm{\γ}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\∫}_0^T{\mathrm{\ω}}_{z\left(i\right)}\mathrm{dt}$

式中，ω_x、ω_y、ω_z分别为绕X轴、Y轴、Z轴的旋转角速度，可由陀螺仪测得，n为采样周期的个数，T为采样周期。 

换算后，a_x、a_y、a_z可用α、β、γ、a_xraw、a_yraw、a_zraw表示如下： 

a_x＝a_xrawcγcβ-a_yrawsγcα+a_yrawcγsβsα+a_zrawsγsa+a_zrawcγsβcα 

a_y＝a_xrawsγcβ+a_yrawcγcα+a_yrawsγsβsα-a_zrawcγsa+a_zrawsγsβcα 

a_z＝-a_xrawsβ+a_yrawcβsα+a_zrawcβcα 

消除重力加速度影响后： 

$a_{z_0}=-a_{\mathrm{xraw}}\mathrm{s\β}+a_{\mathrm{yraw}}\mathrm{c\βs\α}+a_{\mathrm{zraw}}\mathrm{c\βc\α}-(-1)$

X、Y、Z三个方向上第n个采样时刻舞动速度分量为： 

$v_{x\left(n\right)}=v_{x(n-1)}+{\∫}_0^T{a}_{x\left(n\right)}\mathrm{dt}=v_{x(n-1)}+a_{x\left(n\right)}T$

$v_{y\left(n\right)}=v_{y(n-1)}+{\∫}_0^T{a}_{y\left(n\right)}\mathrm{dt}=v_{y(n-1)}+a_{y\left(n\right)}T$

$v_{z\left(n\right)}=v_{z(n-1)}+{\∫}_0^T{a}_{z\left(n\right)}\mathrm{dt}=v_{z(n-1)}+a_{z\left(n\right)}T$

X、Y、Z三个方向上第n个采样时刻舞动位移分量为： 

$d_{x\left(n\right)}=d_{x(n-1)}+{\∫}_0^T{v}_{x\left(n\right)}\mathrm{dt}=d_{x(n-1)}+\frac{1}{2}{a}_{x\left(n\right)}{T}^2$

$d_{y\left(n\right)}=d_{y(n-1)}+{\∫}_0^T{v}_{y\left(n\right)}\mathrm{dt}=d_{y(n-1)}+\frac{1}{2}{a}_{y\left(n\right)}{T}^2$

$d_{z\left(n\right)}=d_{z(n-1)}+{\∫}_0^T{v}_{z\left(n\right)}\mathrm{dt}=d_{z(n-1)}+\frac{1}{2}{a}_{z\left(n\right)}{T}^2$

式中，T为采样时间间隔；d_x(i)、d_y(i)、d_z(i)为第i个采样周期时X、Y、Z三个方向上的舞动幅度；v_y(i)、v_y(i)、v_z(i)为第i个采样周期时X、Y、Z三个方向上的舞动速度。由d_x、d_y、d_z合成相对于地面的舞动轨迹。 

Claims (6)

1.一种引流线舞动在线监测装置，其特征在于，所述的在线监测装置包括：舞动在线监测前端、Zigbee通信模块、通信控制主机、GPRS通信模块、远程监控中心及电源模块等，

所述的舞动在线监测前端，用于对引流线的三维舞动加速度和三维舞动角速度进行实时在线监测；

所述的Zigbee通信模块，用于舞动在线监测前端和通信控制主机之间数据的双向无线传输；

所述的通信控制主机，用于接收周围各监测点的舞动监测数据，并进行现场数据存储，同时根据远程监控中心下发的控制指令，通过GPRS通信模块将所需监测点的监测数据上传至远程监控中心；

所述的GPRS通信模块，用于通信控制主机和远程监控中心之间数据的双向无线传输；

所述的远程监控中心，用于接收舞动监测数据，进行分析计算后，实时显示引流线舞动幅度、舞动速度和舞动频率等参数，并动态再现引流线三维空间中的舞动轨迹；

所述的电源模块选用太阳能取电方式，用于提供舞动在线监测前端、Zigbee通信模块、通信控制主机和GPRS通信模块工作所需电能。

2.根据权利要求1所述的装置，其特征在于，所述的舞动在线监测前端包括：控制器单元、加速度计和陀螺仪，

所述的控制器单元用于采集并处理各传感器监测数据；

所述的加速度计用于对引流线舞动监测点的三维舞动加速度进行监测；

所述的陀螺仪用于对引流线舞动监测点的三维舞动角速度进行监测。

3.一种引流线舞动在线监测方法，其特征在于，所述的方法包括：舞动起停时刻的确定、漂移误差修正和参数分析计算三个步骤。

4.根据权利要求3所述的方法，其特征在于，采用最小角速度向量法确定舞动起停时刻，根据大量数据采样、分析获取最小角速度向ω_min＝(ω_x，min，ω_y，min，ω_z，min)，设定角速度向量阈值为K＝1.05ω_min，如果ω_i≤K，且至少持续两个采样周期，则认为i时刻为前一舞动停止时刻，当ω_j＞K时，则认为j时刻为当前舞动起始时刻，每监测出一次舞动起停时刻，计为一次舞动，每相邻两次舞动时间间隔计为舞动周期，其倒数为舞动频率。

5.根据权利要求3所述的方法，其特征在于，选用ZUPT算法对漂移误差进行修正，即每次舞动起始时刻，如果V不为零，则强制置为零，将因传感器信号漂移等原因引起的误差限制在单次采样周期内，不会随着积分的进行被连续放大。

6.根据权利要求3所述的方法，其特征在于，所述的参数分析计算如下：

首先建立基坐标系和舞动坐标系，其中，基坐标系以大地作为参考，舞动坐标系以引流线上的传感器测量轴作为参考，通过加速度计测得舞动坐标系中的加速度值，再根据旋转变换矩阵计算得到基坐标系中的舞动加速度a_x，a_y，a_z，计算式为：

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\\ 1\end{array}\right]=\mathrm{Rot}(z,\mathrm{\γ})\mathrm{Rot}(y,\mathrm{\β})\mathrm{Rot}(x,\mathrm{\α})\left[\begin{array}{c}{a}_{\mathrm{xraw}}\\ a_{\mathrm{yraw}}\\ a_{\mathrm{zraw}}\\ 1\end{array}\right]$

$=\left[\begin{array}{cccc}\mathrm{c\γc\β}& -\mathrm{s\γc\α}+\mathrm{c\γs\βs\α}& \mathrm{s\γs\α}+\mathrm{c\γs\βc\α}& 0\\ \mathrm{s\γc\β}& \mathrm{c\γc\α}+\mathrm{s\γs\βs\α}& -\mathrm{c\γs\α}+\mathrm{s\γs\βc\α}& 0\\ -\mathrm{s\β}& \mathrm{c\βs\α}& \mathrm{c\βc\α}& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{a}_{\mathrm{xraw}}\\ a_{\mathrm{yraw}}\\ a_{\mathrm{zraw}}\\ 1\end{array}\right]$

式中Rot(z，γ)、Rot(y，β)、Rot(x，α)分别表示绕Z轴旋转γ角度、绕Y轴旋转β角度、绕X轴旋转α角度，a_x，a_y，a_z为基坐标系中X、Y、Z三个方向上加速度分量，a_xraw，a_yraw，a_zraw为舞动坐标系中X、Y、Z三个方向上加速度分量，可由加速度计测得，s表示正弦公式，c表示余弦公式；

α、β、γ可由以下公式求得：

$\mathrm{\α}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\∫}_0^T{\mathrm{\ω}}_{x\left(i\right)}\mathrm{dt},$ $\mathrm{\β}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\∫}_0^T{\mathrm{\ω}}_{y\left(i\right)}\mathrm{dt},$ $\mathrm{\γ}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\∫}_0^T{\mathrm{\ω}}_{z\left(i\right)}\mathrm{dt}$

式中，ω_x、ω_y、ω_z分别为绕X轴、Y轴、Z轴的旋转角速度，可由陀螺仪测得，n为采样周期的个数，T为采样周期；

换算后，a_x、a_y、a_z可用α、β、γ、a_xraw、a_yraw、a_zraw表示如下：

a_x＝a_xrawcγcβ-a_yrawsγcα+a_yrawcγsβsα+a_zrawsγsα+a_zrawcγsβcα

a_y＝a_xrawsγcβ+a_yrawcγcα+a_yrawsγsβsα-a_zrawcγsα+a_zrawsγsβcα

a_z＝-a_xrawsβ+a_yrawcβsα+a_zrawcβcα

消除重力加速度影响后：

$a_{z_0}=-a_{\mathrm{xraw}}\mathrm{s\β}+a_{\mathrm{yraw}}\mathrm{c\βs\α}+a_{\mathrm{zraw}}\mathrm{c\βc\α}-(-1)$

X、Y、Z三个方向上第n个采样时刻舞动速度分量为：

$v_{x\left(n\right)}=v_{x(n-1)}+{\∫}_0^T{a}_{x\left(n\right)}\mathrm{dt}=v_{x(n-1)}+a_{x\left(n\right)}T$

$v_{y\left(n\right)}=v_{y(n-1)}+{\∫}_0^T{a}_{y\left(n\right)}\mathrm{dt}=v_{y(n-1)}+a_{y\left(n\right)}T$

$v_{z\left(n\right)}=v_{z(n-1)}+{\∫}_0^T{a}_{z\left(n\right)}\mathrm{dt}=v_{z(n-1)}+a_{z\left(n\right)}T$

X、Y、Z三个方向上第n个采样时刻舞动位移分量为：

$d_{x\left(n\right)}=d_{x(n-1)}+{\∫}_0^T{v}_{x\left(n\right)}\mathrm{dt}=d_{x(n-1)}+\frac{1}{2}{a}_{x\left(n\right)}{T}^2$

$d_{y\left(n\right)}=d_{y(n-1)}+{\∫}_0^T{v}_{y\left(n\right)}\mathrm{dt}=d_{y(n-1)}+\frac{1}{2}{a}_{y\left(n\right)}{T}^2$

$d_{z\left(n\right)}=d_{z(n-1)}+{\∫}_0^T{v}_{z\left(n\right)}\mathrm{dt}=d_{z(n-1)}+\frac{1}{2}{a}_{z\left(n\right)}{T}^2$

式中，T为采样时间间隔，d_x(i)、d_y(i)、d_z(i)为第i个采样周期时X、Y、Z三个方向上的舞动幅度，v_y(i)、v_y(i)、v_z(i)为第i个采样周期时X、Y、Z三个方向上的舞动速度，由d_x、d_y、d_z合成相对于地面的舞动轨迹。

Images