基于EKF的AMR锂电池SOC动态估计研究

AMR在科研和军事方面的应用决定了其需要一个能够准确、实时估计出锂电池SOC的估计值。由于AMR工作中的放电具有很强的动态性,使得传统估计方法用于锂电池SOC的动态估计效果很不理想。本文描述了EKF方法在AMR锂电池SOC动态估计中的应用。模拟AMR工况进行动态放电试验。试验结果表明,该估计方法能够实时而准确地得到SOC值。

估计从UUV航行参数的混合坐标系下的EKF算法

在主从式UUV 协作系统中，由于定位和导航的需要，要求尽快估计出从UUV 的航行参数，但通常所用的递推最小二乘（RLS）算法，其初始方位测量对滤波结果影响大且存在收敛速度慢、计算精度低的缺点，难以满足应用需求，而推广卡尔曼滤波（EKF）算法能较好地克服上述问题。在直角坐标系下（CEKF），方位信息与距离信息相互耦合导致初始振荡剧烈，改为混合坐标系（MEKF）后问题得到了极大的改善。最后，通过仿真及现场试验验证了此改进方法的有效性。

Suboptimal system recovery from communication loss in a multi-robot localization scenario using EKF algorithms

This paper describes a multi-robot localization scenario where, for a period of time, the robot team loses communication with one of the robots due to system error. In this novel approach, extended Kalman filter (EKF) algorithms utilize relative measurements to localize the robots in space. These measurements are used to reliably compensate "dead-com" periods were no information can be exchanged between the members of the robot group.

Investigation of MEMS inertial sensors and aircraft dynamic models in global positioning system integrity monitoring for approaches with vertical guidance

An Approach with Vertical Guidance (APV) is an instrument approach procedure which provides horizontal and vertical guidance to a pilot on approach to landing in reduced visibility conditions. APV approaches can greatly reduce the safety risk to general aviation by improving the pilot’s situational awareness. In particular the incidence of Controlled Flight Into Terrain (CFIT) which has occurred in a number of fatal air crashes in general aviation over the past decade in Australia, can be reduced. APV approaches can also improve general aviation operations. If implemented at Australian airports, APV approach procedures are expected to bring a cost saving of millions of dollars to the economy due to fewer missed approaches, diversions and an increased safety benefit. The provision of accurate horizontal and vertical guidance is achievable using the Global Positioning System (GPS). Because aviation is a safety of life application, an aviation-certified GPS receiver must have integrity monitoring or augmentation to ensure that its navigation solution can be trusted. However, the difficulty with the current GPS satellite constellation alone meeting APV integrity requirements, the susceptibility of GPS to jamming or interference and the potential shortcomings of proposed augmentation solutions for Australia such as the Ground-based Regional Augmentation System (GRAS) justifies the investigation of Aircraft Based Augmentation Systems (ABAS) as an alternative integrity solution for general aviation. ABAS augments GPS with other sensors at the aircraft to help it meet the integrity requirements. Typical ABAS designs assume high quality inertial sensors to provide an accurate reference trajectory for Kalman filters. Unfortunately high-quality inertial sensors are too expensive for general aviation. In contrast to these approaches the purpose of this research is to investigate fusing GPS with lower-cost Micro-Electro-Mechanical System (MEMS) Inertial Measurement Units (IMU) and a mathematical model of aircraft dynamics, referred to as an Aircraft Dynamic Model (ADM) in this thesis. Using a model of aircraft dynamics in navigation systems has been studied before in the available literature and shown to be useful particularly for aiding inertial coasting or attitude determination. In contrast to these applications, this thesis investigates its use in ABAS. This thesis presents an ABAS architecture concept which makes use of a MEMS IMU and ADM, named the General Aviation GPS Integrity System (GAGIS) for convenience. GAGIS includes a GPS, MEMS IMU, ADM, a bank of Extended Kalman Filters (EKF) and uses the Normalized Solution Separation (NSS) method for fault detection. The GPS, IMU and ADM information is fused together in a tightly-coupled configuration, with frequent GPS updates applied to correct the IMU and ADM. The use of both IMU and ADM allows for a number of different possible configurations. Three are investigated in this thesis; a GPS-IMU EKF, a GPS-ADM EKF and a GPS-IMU-ADM EKF. The integrity monitoring performance of the GPS-IMU EKF, GPS-ADM EKF and GPS-IMU-ADM EKF architectures are compared against each other and against a stand-alone GPS architecture in a series of computer simulation tests of an APV approach. Typical GPS, IMU, ADM and environmental errors are simulated. The simulation results show the GPS integrity monitoring performance achievable by augmenting GPS with an ADM and low-cost IMU for a general aviation aircraft on an APV approach. A contribution to research is made in determining whether a low-cost IMU or ADM can provide improved integrity monitoring performance over stand-alone GPS. It is found that a reduction of approximately 50% in protection levels is possible using the GPS-IMU EKF or GPS-ADM EKF as well as faster detection of a slowly growing ramp fault on a GPS pseudorange measurement. A second contribution is made in determining how augmenting GPS with an ADM compares to using a low-cost IMU. By comparing the results for the GPS-ADM EKF against the GPS-IMU EKF it is found that protection levels for the GPS-ADM EKF were only approximately 2% higher. This indicates that the GPS-ADM EKF may potentially replace the GPS-IMU EKF for integrity monitoring should the IMU ever fail. In this way the ADM may contribute to the navigation system robustness and redundancy. To investigate this further, a third contribution is made in determining whether or not the ADM can function as an IMU replacement to improve navigation system redundancy by investigating the case of three IMU accelerometers failing. It is found that the failed IMU measurements may be supplemented by the ADM and adequate integrity monitoring performance achieved. Besides treating the IMU and ADM separately as in the GPS-IMU EKF and GPS-ADM EKF, a fourth contribution is made in investigating the possibility of fusing the IMU and ADM information together to achieve greater performance than either alone. This is investigated using the GPS-IMU-ADM EKF. It is found that the GPS-IMU-ADM EKF can achieve protection levels approximately 3% lower in the horizontal and 6% lower in the vertical than a GPS-IMU EKF. However this small improvement may not justify the complexity of fusing the IMU with an ADM in practical systems. Affordable ABAS in general aviation may enhance existing GPS-only fault detection solutions or help overcome any outages in augmentation systems such as the Ground-based Regional Augmentation System (GRAS). Countries such as Australia which currently do not have an augmentation solution for general aviation could especially benefit from the economic savings and safety benefits of satellite navigation-based APV approaches.

GPS fault detection with IMU and aircraft dynamics

Approaches with Vertical Guidance (APV) can provide greater safety and cost savings to general aviation through accurate GPS horizontal and vertical navigation. However, GPS needs augmentation to achieve APV fault detection requirements. Aircraft Based Augmentation Systems (ABAS) fuse GPS with additional sensors at the aircraft. Typical ABAS designs assume high-quality inertial sensors with Kalman filters but these are too expensive for general aviation. Instead of using high-quality (and expensive) sensors, the purpose of this paper is to investigate augmenting GPS with a low-quality MEMS IMU and Aircraft Dynamic Model (ADM). The IMU and ADM are fused together using a multiple model fusion strategy in a bank of Extended Kalman Filters (EKF) with the Normalized Solution Separation (NSS) fault detection scheme. A tightly-coupled configuration with GPS is used and frequent GPS updates are applied to the IMU and ADM to compensate for their errors. Based upon a simulated APV approach, the performance of this architecture in detecting a GPS ramp fault is investigated showing a performance improvement over a GPS-only “snapshot” implementation of the NSS method. The effect of fusing the IMU with the ADM is evaluated by comparing a GPS-IMU-ADM EKF with a GPS-IMU EKF where a small improvement in protection levels is shown.

Fixed-wing attitude estimation using temporal tracking of the horizon and optical flow

In this paper, a method has been developed for estimating pitch angle, roll angle and aircraft body rates based on horizon detection and temporal tracking using a forward-looking camera, without assistance from other sensors. Using an image processing front-end, we select several lines in an image that may or may not correspond to the true horizon. The optical flow at each candidate line is calculated, which may be used to measure the body rates of the aircraft. Using an Extended Kalman Filter (EKF), the aircraft state is propagated using a motion model and a candidate horizon line is associated using a statistical test based on the optical flow measurements and the location of the horizon. Once associated, the selected horizon line, along with the associated optical flow, is used as a measurement to the EKF. To test the accuracy of the algorithm, two flights were conducted, one using a highly dynamic Uninhabited Airborne Vehicle (UAV) in clear flight conditions and the other in a human-piloted Cessna 172 in conditions where the horizon was partially obscured by terrain, haze and smoke. The UAV flight resulted in pitch and roll error standard deviations of 0.42◦ and 0.71◦ respectively when compared with a truth attitude source. The Cessna flight resulted in pitch and roll error standard deviations of 1.79◦ and 1.75◦ respectively. The benefits of selecting and tracking the horizon using a motion model and optical flow rather than naively relying on the image processing front-end is also demonstrated.

Multi-sensor data fusion for UAV navigation during landing operations

This paper presents a practical framework to synthesize multi-sensor navigation information for localization of a rotary-wing unmanned aerial vehicle (RUAV) and estimation of unknown ship positions when the RUAV approaches the landing deck. The estimation performance of the visual tracking sensor can also be improved through integrated navigation. Three different sensors (inertial navigation, Global Positioning System, and visual tracking sensor) are utilized complementarily to perform the navigation tasks for the purpose of an automatic landing. An extended Kalman filter (EKF) is developed to fuse data from various navigation sensors to provide the reliable navigation information. The performance of the fusion algorithm has been evaluated using real ship motion data. Simulation results suggest that the proposed method can be used to construct a practical navigation system for a UAV-ship landing system.

Robotics toolbox for MATLAB, release 9 [Software]

The ninth release of the Toolbox, represents over fifteen years of development and a substantial level of maturity. This version captures a large number of changes and extensions generated over the last two years which support my new book “Robotics, Vision & Control”. The Toolbox has always provided many functions that are useful for the study and simulation of classical arm-type robotics, for example such things as kinematics, dynamics, and trajectory generation. The Toolbox is based on a very general method of representing the kinematics and dynamics of serial-link manipulators. These parameters are encapsulated in MATLAB ® objects - robot objects can be created by the user for any serial-link manipulator and a number of examples are provided for well know robots such as the Puma 560 and the Stanford arm amongst others. The Toolbox also provides functions for manipulating and converting between datatypes such as vectors, homogeneous transformations and unit-quaternions which are necessary to represent 3-dimensional position and orientation. This ninth release of the Toolbox has been significantly extended to support mobile robots. For ground robots the Toolbox includes standard path planning algorithms (bug, distance transform, D*, PRM), kinodynamic planning (RRT), localization (EKF, particle filter), map building (EKF) and simultaneous localization and mapping (EKF), and a Simulink model a of non-holonomic vehicle. The Toolbox also including a detailed Simulink model for a quadcopter flying robot.

Integrated vision-based attitude estimation

For many years, computer vision has lured researchers with promises of a low-cost, passive, lightweight and information-rich sensor suitable for navigation purposes. The prime difficulty in vision-based navigation is that the navigation solution will continually drift with time unless external information is available, whether it be cues from the appearance of the scene, a map of features (whether built online or known a priori), or from an externally-referenced sensor. It is not merely position that is of interest in the navigation problem. Attitude (i.e. the angular orientation of a body with respect to a reference frame) is integral to a visionbased navigation solution and is often of interest in its own right (e.g. flight control). This thesis examines vision-based attitude estimation in an aerospace environment, and two methods are proposed for constraining drift in the attitude solution; one through a novel integration of optical flow and the detection of the sky horizon, and the other through a loosely-coupled integration of Visual Odometry and GPS position measurements. In the first method, roll angle, pitch angle and the three aircraft body rates are recovered though a novel method of tracking the horizon over time and integrating the horizonderived attitude information with optical flow. An image processing front-end is used to select several candidate lines in a image that may or may not correspond to the true horizon, and the optical flow is calculated for each candidate line. Using an Extended Kalman Filter (EKF), the previously estimated aircraft state is propagated using a motion model and a candidate horizon line is associated using a statistical test based on the optical flow measurements and location of the horizon in the image. Once associated, the selected horizon line, along with the associated optical flow, is used as a measurement to the EKF. To evaluate the accuracy of the algorithm, two flights were conducted, one using a highly dynamic Uninhabited Airborne Vehicle (UAV) in clear flight conditions and the other in a human-piloted Cessna 172 in conditions where the horizon was partially obscured by terrain, haze and smoke. The UAV flight resulted in pitch and roll error standard deviations of 0.42° and 0.71° respectively when compared with a truth attitude source. The Cessna 172 flight resulted in pitch and roll error standard deviations of 1.79° and 1.75° respectively. In the second method for estimating attitude, a novel integrated GPS/Visual Odometry (GPS/VO) navigation filter is proposed, using a structure similar to a classic looselycoupled GPS/INS error-state navigation filter. Under such an arrangement, the error dynamics of the system are derived and a Kalman Filter is developed for estimating the errors in position and attitude. Through similar analysis to the GPS/INS problem, it is shown that the proposed filter is capable of recovering the complete attitude (i.e. pitch, roll and yaw) of the platform when subjected to acceleration not parallel to velocity for both the monocular and stereo variants of the filter. Furthermore, it is shown that under general straight line motion (e.g. constant velocity), only the component of attitude in the direction of motion is unobservable. Numerical simulations are performed to demonstrate the observability properties of the GPS/VO filter in both the monocular and stereo camera configurations. Furthermore, the proposed filter is tested on imagery collected using a Cessna 172 to demonstrate the observability properties on real-world data. The proposed GPS/VO filter does not require additional restrictions or assumptions such as platform-specific dynamics, map-matching, feature-tracking, visual loop-closing, gravity vector or additional sensors such as an IMU or magnetic compass. Since no platformspecific dynamics are required, the proposed filter is not limited to the aerospace domain and has the potential to be deployed in other platforms such as ground robots or mobile phones.

Extracting deck trend to plan descent trajectory for safe landing of UAVs

This paper outlines a feasible scheme to extract deck trend when a rotary-wing unmanned aerial vehicle (RUAV)approaches an oscillating deck. An extended Kalman filter (EKF) is de- veloped to fuse measurements from multiple sensors for effective estimation of the unknown deck heave motion. Also, a recursive Prony Analysis (PA) procedure is proposed to implement online curve-fitting of the estimated heave mo- tion. The proposed PA constructs an appropriate model with parameters identified using the forgetting factor recursive least square (FFRLS)method. The deck trend is then extracted by separating dominant modes. Performance of the proposed procedure is evaluated using real ship motion data, and simulation results justify the suitability of the proposed method into safe landing of RUAVs operating in a maritime environment.

Outdoor flight testing of a pole inspection UAV incorporating high-speed vision

We present a pole inspection system for outdoor environments comprising a high-speed camera on a vertical take-off and landing (VTOL) aerial platform. The pole inspection task requires a vehicle to fly close to a structure while maintaining a fixed stand-off distance from it. Typical GPS errors make GPS-based navigation unsuitable for this task however. When flying outdoors a vehicle is also affected by aerodynamics disturbances such as wind gusts, so the onboard controller must be robust to these disturbances in order to maintain the stand-off distance. Two problems must therefor be addressed: fast and accurate state estimation without GPS, and the design of a robust controller. We resolve these problems by a) performing visual + inertial relative state estimation and b) using a robust line tracker and a nested controller design. Our state estimation exploits high-speed camera images (100Hz) and 70Hz IMU data fused in an Extended Kalman Filter (EKF). We demonstrate results from outdoor experiments for pole-relative hovering, and pole circumnavigation where the operator provides only yaw commands. Lastly, we show results for image-based 3D reconstruction and texture mapping of a pole to demonstrate the usefulness for inspection tasks.

A sensor fault detection and isolation method in interior permanent-magnet synchronous motor drives based on an extended Kalman filter

Interior permanent-magnet synchronous motors (IPMSMs) become attractive candidates in modern hybrid electric vehicles and industrial applications. Usually, to obtain good control performance, the electric drives of this kind of motor require one position, one dc link, and at least two current sensors. Failure of any of these sensors might lead to degraded system performance or even instability. As such, sensor fault resilient control becomes a very important issue in modern drive systems. This paper proposes a novel sensor fault detection and isolation algorithm based on an extended Kalman filter. It is robust to system random noise and efficient in real-time implementation. Moreover, the proposed algorithm is compact and can detect and isolate all the sensor faults for IPMSM drives. Thorough theoretical analysis is provided, and the effectiveness of the proposed approach is proven by extensive experimental results.

Close-quarters Quadrotor flying for a pole inspection with position based visual servoing and high-speed vision

This paper presents a 100 Hz monocular position based visual servoing system to control a quadrotor flying in close proximity to vertical structures approximating a narrow, locally linear shape. Assuming the object boundaries are represented by parallel vertical lines in the image, detection and tracking is achieved using Plücker line representation and a line tracker. The visual information is fused with IMU data in an EKF framework to provide fast and accurate state estimation. A nested control design provides position and velocity control with respect to the object. Our approach is aimed at high performance on-board control for applications allowing only small error margins and without a motion capture system, as required for real world infrastructure inspection. Simulated and ground-truthed experimental results are presented.

Shared autonomy for close-quarters navigation and control of a VTOL platform

This thesis presents an approach for a vertical infrastructure inspection using a vertical take-off and landing (VTOL) unmanned aerial vehicle and shared autonomy. Inspecting vertical structure such as light and power distribution poles is a difficult task. There are challenges involved with developing such an inspection system, such as flying in close proximity to a target while maintaining a fixed stand-off distance from it. The contributions of this thesis fall into three main areas. Firstly, an approach to vehicle dynamic modeling is evaluated in simulation and experiments. Secondly, EKF-based state estimators are demonstrated, as well as estimator-free approaches such as image based visual servoing (IBVS) validated with motion capture ground truth data. Thirdly, an integrated pole inspection system comprising a VTOL platform with human-in-the-loop control, (shared autonomy) is demonstrated. These contributions are comprehensively explained through a series of published papers.

Extended Kalman filters using explicit and derivative-free local linearizations

We propose three variants of the extended Kalman filter (EKF) especially suited for parameter estimations in mechanical oscillators under Gaussian white noises. These filters are based on three versions of explicit and derivative-free local linearizations (DLL) of the non-linear drift terms in the governing stochastic differential equations (SDE-s). Besides a basic linearization of the non-linear drift functions via one-term replacements, linearizations using replacements through explicit Euler and Newmark expansions are also attempted in order to ensure higher closeness of true solutions with the linearized ones. Thus, unlike the conventional EKF, the proposed filters do not need computing derivatives (tangent matrices) at any stage. The measurements are synthetically generated by corrupting with noise the numerical solutions of the SDE-s through implicit versions of these linearizations. In order to demonstrate the effectiveness and accuracy of the proposed methods vis-à-vis the conventional EKF, numerical illustrations are provided for a few single degree-of-freedom (DOF) oscillators and a three-DOF shear frame with constant parameters.