Integrating rotation from angular velocity

The integration of the rotation from a given angular velocity is often required in practice. The present paper explores how the choice of the parametrization of rotation, when employed in conjuction with different numerical time-integration schemes, effects the accuracy and the computational efficiency. Three rotation parametrizations - the rotational vector, the Argyris tangential vector and the rotational quaternion - are combined with three different numerical time-integration schemes, including classical explicit Runge-Kutta method and the novel midpoint rule proposed here. The key result of the study is the assessment of the integration errors of various parametrization-integration method combinations. In order to assess the errors, we choose a time-dependent function corresponding to a rotational vector, and derive the related exact time-dependent angular velocity. This is then employed in the numerical solution as the data. The resulting numerically integrated approximate rotations are compared with the analytical solution. A novel global solution error norm for discrete solutions given by a set of values at chosen time-points is employed. Several characteristic angular velocity functions, resulting in small, finite and fast oscillating rotations are studied.     

应用于多旋翼MAVs的姿态测量系统

应用一个三轴加速度计、三个单轴角速率陀螺和一个三轴磁强计等微机械惯性传感器,设计廉价轻量姿态测量系统,研究了姿态角推算算法。在以往的姿态测量系统中,陀螺偏差和动加速度的影响限制其应用。将角速度陀螺的误差作为状态量导入到系统,动加速度作为噪音项导入到观测方程中,然后利用扩展卡尔曼滤波器来构成姿态估计算法来降低误差。实际飞行中对比商用高精度传感器和多次室外飞行测试表明,设计的系统能够应于旋翼MAVs。     

Rigid body attitude estimation based on the Lagrange–d’Alembert principle

Estimation of rigid body attitude and angular velocity without any knowledge of the attitude dynamics model is treated using the Lagrange–d’Alembert principle from variational mechanics. It is shown that Wahba’s cost function for attitude determination from two or more non-collinear vector measurements can be generalized and represented as a Morse function of the attitude estimation error on the Lie group of rigid body rotations. With body-fixed sensor measurements of direction vectors and angular velocity, a Lagrangian is obtained as the difference between a kinetic energy-like term that is quadratic in the angular velocity estimation error and an artificial potential obtained from Wahba’s function. An additional dissipation term that depends on the angular velocity estimation error is introduced, and the Lagrange–d’Alembert principle is applied to the Lagrangian with this dissipation. A Lyapunov analysis shows that the state estimation scheme so obtained provides stable asymptotic convergence of state estimates to actual states in the absence of measurement noise, with an almost global domain of attraction. These estimation schemes are discretized for computer implementation using discrete variational mechanics. A first order Lie group variational integrator is obtained as a discrete-time implementation. In the presence of bounded measurement noise, numerical simulations show that the estimated states converge to a bounded neighborhood of the actual states.     

A theoretical approach to observability analysis of the SDINS/GPS in maneuvering with horizontal constant velocity

A theoretical method for analyzing the observability of a strapdown inertial navigation system (SDINS) integrated with the global positioning system (GPS) is proposed. The analysis is performed based on two types of maneuvers for a vehicle on a horizontal trajectory: level flight with constant north velocity and level flight with constant east velocity. The observability also is analyzed using the convergence theorem, stationary state observability analysis results, and Kalman filter measurement information to rearrange the SDINS error model equation. The state variables are divided into observable and unobservable parts, and determine which state variables are observable and estimable with some errors from the relationship of observable and unobservable state variables. Our results have shown that the north and east axes accelerometer bias errors were unobservable, and that attitude errors, and east and down axes gyro bias errors were estimable with some unknown bias errors. It has been shown that horizontal maneuvering improves the observability of down axis gyro bias error compared with the stationary state, and the estimation errors of the heading error state and east axis gyro bias error are dependent on the magnitude of north velocity. The results of the theoretical observability analysis are confirmed through computer simulation.     

Global optimal attitude estimation using uncertainty ellipsoids

A deterministic attitude estimation problem for a rigid body in a potential field, with bounded attitude and angular velocity measurement errors is considered. An attitude estimation algorithm that globally minimizes the attitude estimation error is obtained. Assuming that the initial attitude, the initial angular velocity and measurement noise lie within given ellipsoidal bounds, an uncertainty ellipsoid that bounds the attitude and the angular velocity of the rigid body is obtained. The center of the uncertainty ellipsoid provides point estimates, and the size of the uncertainty ellipsoid measures the accuracy of the estimates. The point estimates and the uncertainty ellipsoids are propagated using a Lie group variational integrator and its linearization, respectively. The attitude and angular velocity estimates are optimal in the sense that the sizes of the uncertainty ellipsoids are minimized.     

Attitude Tracking With Adaptive Rejection of Rate Gyro Disturbances

The classical attitude control problem for a rigid body is revisited under the assumption that measurements of the angular rates obtained by means of rate gyros are corrupted by harmonic disturbances, a setup of importance in several aerospace applications. This note extends previous methods developed to compensate bias in angular rate measurements by accounting for a more general class of disturbances, and by allowing uncertainty in the inertial parameters. By resorting to adaptive observers designed on the basis of the internal model principle, it is shown how converging estimates of the angular velocity can be used effectively in a passivity-based controller yielding global convergence within the chosen parametrization of the group of rotations. Since a persistence of excitation condition is not required for the convergence of the state estimates, only an upper bound on the number of distinct harmonic components of the disturbance is needed for the applicability of the method.     

低成本传感器数据融合技术辅助iPhone导航研究

多传感器数据融合辅助iPhone导航研究的是一个基于IMU/GPS/磁力计的联合解算导航算法,即将手机中加速度计测量到的比力信息与陀螺仪测量到的角速率信息通过扩展卡尔曼滤波技术进行融合。针对低成本传感器的特性,设计了相对应的初始对准方法。在GPS/INS组合阶段,通过添加载体坐标系下左右方向和竖直方向的载体速度为观测量来增加系统的可观测性,从而达到对状态量的更优估计,最终通过反馈校正得到较为精确的载体位置、速度和姿态信息。     

基于Kalman滤波算法的姿态传感器信号融合技术研究

针对应用三轴陀螺仪和三轴加速度传感器的四旋翼飞行器姿态角测量问题,提出了基于Kalman滤波算法的姿态传感器信号融合方法。该方法将陀螺仪输出的角速度误差作为时变误差处理,认为陀螺仪输出的角速度误差与其所测角速度及上一时刻的角速度输出误差相关,并据此建立陀螺仪测量线性方程,在此基础上,应用Kalman滤波算法,以加速度计输出的姿态角对陀螺仪测量的姿态角进行修正,从而达到姿态角准确测量的目的。实验结果表明：应用Kalman滤波算法对加速度传感器和陀螺仪信号融合后可有效消除姿态角测量累积误差并显著改善姿态角测量的动态特性。     

A nonlinear position and attitude observer on SE(3) using landmark measurements

This paper addresses the problem of position and attitude estimation, based on landmark readings and velocity measurements. A derivation of a nonlinear observer on SE(3) is presented, using a Lyapunov function conveniently expressed as a function of the difference between the estimated and the measured landmark coordinates. The resulting feedback laws are explicit functions of the landmark measurements and velocity readings, exploiting the sensor information directly in the observer. The proposed observer yields almost global asymptotic stabilization of the position and attitude errors and exponential convergence in any closed ball inside the region of attraction. Also, it is shown that the asymptotic convergence of the estimation error trajectories is shaped by the landmark geometry and observer design parameters. The problem of non-ideal velocity readings is also considered, and the observer is augmented to compensate for bias in the angular and linear velocity measurements. The resulting position, attitude, and bias estimation errors are shown to converge exponentially fast to the desired equilibrium points, for bounded initial estimation errors. Simulation results are presented to illustrate the stability and convergence properties of the observer.     

Finite‐time angular velocity observers for rigid‐body attitude tracking with bounded inputs

The attitude tracking of a rigid body without angular velocity measurements is addressed. A continuous angular velocity observer with fractional power functions is proposed to estimate the angular velocity via quaternion attitude information. The fractional power gains can be properly tuned according to a homogeneous method such that the estimation error system is uniformly almost globally finite‐time stable, irrespective of control inputs. To achieve output feedback attitude tracking control, a quaternion‐based nonlinear proportional‐derivative controller using full‐state feedback is designed first, yielding uniformly almost globally finite‐time stable of the attitude tracking system as well as bounded control torques a priori. It is then shown that the certainty equivalent combination of the observer and nonlinear proportional‐derivative controller ensures finite‐time convergence of the attitude tracking error for almost all initial conditions. The proposed methods not only avoid high‐gain injection, as opposed to the semi‐global results, but also overcome the unwinding problem associated with some quaternion‐based observers and/or controllers. Numerical simulations are presented to verify the effectiveness of the proposed methods. Copyright © 2016 John Wiley & Sons, Ltd.     

惯性测量组合的优化设计与误差补偿算法研究

设计了九加速度计惯性测量组合用于测量飞行体的姿态和位置。通过理论分析和计算机仿真,结果显示由9个加速度计的输出可解算出载体运动角速度和线加速度,但由于加速度计的测量误差引起的角速度的解算误差会随飞行时间的增长而累积,采用误差补偿算法可有效地减小误差。另对加速度计位置误差进行了理论分析和仿真,结果表明,由加速度计组合的惯性测量方案对加速度计位置误差非常敏感。     

姿态航向测量与误差补偿方法研究

设计了一种基于MEMS陀螺仪、加速度计、磁传感器的小型姿态航向参考系统;以四元数和角速率偏差为状态矢量,磁场强度和加速度计信息为量测矢量,构建基于Kalman的四元数姿态航向解算方法;通过调整测量噪声方差矩阵,解决动态过程中由于运动加速度造成的姿态角误差;采用陀螺仪误差建模和磁航向罗差补偿技术,进一步提高了系统测量精度。根据飞行数据分析,姿态航向参考系统具有较高测量精度和较好的稳定性、动态性,姿态角均方根误差小于1.5°,航向角均方根误差小于3°。     

Comments on the stabilizability of the angular velocity of a rigid body

This paper discusses the stabilizability by smooth feedback of the angular velocity of a rigid body. First we show how to asymptotically stabilize the system with a single control. Then it is shown that a single control aligned with a principal axis cannot asymptotically stabilize the system. Finally we present some new results on stabilization by means of scalar feedback control.     

基于多位置的MEMS加速度计快速自标定

针对MEMS加速度计输出信息受自身误差项(如零偏、标度因数、非正交误差等)干扰而影响器件自身测量精度的问题,提出一种不依赖转台设备的快速24位置标定方法.在分析MEMS加速度计输出特性基础上建立MEMS加速度计输出误差模型,设计并展开连续转停标定,利用重力特征实现加速度计误差修正.基于器件零偏、标度因数、非正交误差9个误差参数建立MEMS加速度计标定模型后,提出基于牛顿法对误差参数最优值进行估计.加速度计标定补偿实验结果表明,多位置标定方法能有效补偿自身误差并提高输出加速度信息的精度.     

小型尾坐式飞行器航姿参考系统

针对小型尾坐式飞行器姿态实时解算问题,研究了低成本的航姿参考系统(AHRS).由于MEMS惯性器件精度较低,设计了混合卡尔曼滤波器,以姿态四元数和陀螺随机漂移为状态变量,抑制了载体长时间飞行时陀螺漂移造成的累积误差.由于加速度计输出值在除重力加速度之外的附加加速度较大时不可信,完善了判断载体运动状态的方法,根据加速度计的实际输出,选择加速度值或者磁场强度作为观测量.实验结果表明,设计的算法在精度和计算效率方面都能满足控制系统的需求,更加适用于对实时性有较高要求的飞行器.     

On stabilization by means of the Energy-Casimir method

Recently a stabilization result on the angular velocity equations of the rigid body has been derived by means of the Energy-Casimir method. In this paper an alternative derivation of this result is given based on a topological condition on the level surfaces defined by the constants of motion. This condition is then reinterpreted, leading to a relaxed form of the Energy-Casimir theorem. The theorem is applied to the angular velocity equations of a rigid body.     

微机电加速度计的六位置标定

加速度计是惯性导航系统测量载体加速度和影响惯性导航系统精度的主要元器件.为提高惯性导航系统的精度,在使用加速度计以前需要进行加速度计的标定测试[1].主要介绍了微机电(Micro Electro Mechanical Systems,简称MEMS)加速度计的六位置标定法,以便从MEMS加速度计的误差模型中分离出MEMS加速度计的各项标定参数.这些参数包括MEMS加速度计的标度因数、零位漂移以及安装误差系数.并且在得到MEMS加速度计的各项标定参数后将其封装在C函数中进行了验证实验.实验结果表明MEMS加速度计的六位置标定法的原理简单并且在工程应用中容易实现,所得到的MEMS加速度计的输出不但反映了MEMS加速度计的实际输出,而且使MEMS加速度计的线性度得到改善.     

六组合传感器的测量算法研究

基于六组合高精度陀螺(如激光陀螺、光纤陀螺等)传感器测量载体的三向角速率和三向加速度的数据,对三向姿态角进行求解测量算法的研究。利用一种四元数法的数学间接计算方法,即利用四维空间中的四元数的性质和运算规则来研究三维空间中刚体转动的问题,在科研试飞测量中具有很好的实际应用价值。     

Sensor Fusion for Joint Kinematic Estimation in Serial Robots Using Encoder,Accelerometer and Gyroscope

Open-chain manipulator robots play an important role in the industry, since they are utilized in applications requiring precise motion. High-performance motion of a robot system mainly relies on adequate trajectory planning and the controller that coordinates the movement. The controller performance depends of both, the employed control law and the sensor feedback. Optical encoder feedback is the most used sensor for angular position estimation of each joint in the robot, since they feature accurate and low noise angular position measurements. However, it cannot detect mechanical imperfections and deformations common in open chain robots. Moreover, velocity and acceleration cannot be extracted from the encoder data without adding phase delays. Sensor fusion techniques are found to be a good solution for solving this problem. However, few works has been carried out in serial robots for kinematic estimation of angular position, velocity and acceleration, since the delays induced by the filtering techniques avoids its use as controller feedback. This work proposes a novel sensor-fusion-based feedback system capable of providing complete kinematic information from each joint in 4-degrees of freedom serial robot, with the contribution of a proposed methodology based on Kalman filtering for fusing the information from optical encoder, gyroscope and accelerometer appended to the robot. Calibration and experimentation are carried out for validating the proposal. The results are compared with another kinematic estimation technique finding that this proposal provides more information about the robot movement without adding state delays, which is important for being used as controller feedback.