基于惯性导航对物体运动状态的推算

姚悦东 闫帅 吴林晋 米心馨

摘 要：为了解决获取物体在空间中的位置信息，本文研究试验出一种基于惯性导航系统推算物体运动状态和空间位置的定位方式。此定位方式可以不受环境限制、原理简单、可行性强，能有效满足学者需求。

关键词：惯性导航;运动状态;定位方式

中图分类号：TN96;TP212文献标识码：A文章编号：1003-5168（2019）11-0059-02

Abstract： In order to obtain the position information of objects in space， this paper studied and tested a positioning method based on inertial navigation system to calculate the motion state and position of objects. This positioning method is not limited by the environment， simple in principle， strong in feasibility， and can effectively meet the needs of scholars.

Keywords： inertial navigation;motion state;location mode

随着科技发展的日新月异，无人机航拍、室内扫地机器人、无人驾驶汽车等一系列高科技产物逐渐融入人们的生活。但目前，这些产品都面临着如何获取物体在空间中的位置信息的问题。通常情况下，常用的定位方法主要有基于卫星的GPS定位、基于机器视觉的SLAM定位和基于运动传感器的惯性导航定位等。这些定位方法各有优缺点，有的计算量复杂，有的只能在室外定位，有的对环境要求苛刻。由此，本文提出通过惯性导航推算出物体运动状态和空间位置。该方法不受环境限制，只需要在物体上安装一个陀螺仪，测得基本的运动数據即可定位。同时，其计算量不大，32系列的单片机即可工作

1 试验装置设计

使用一台无人机，通过采集其飞行控制器内置的电子罗盘和陀螺仪提供的数据建立惯性导航系统。

制作一台装有色板的遥控小车，在无人机头安装机器视觉摄像头OPENMV，在空中，摄像头实时返回小车的位置信息。

将小车的位置与无人机惯性导航得出的位置融合建立空间模型，使用驱动算法使无人机追踪小车[1]。

2 试验步骤

①建立惯性导航系统。试验采用地球固联坐标系作为惯性导航坐标系，以无人机起飞位置作为坐标原点。设X轴在水平面内，指向某一方向，Y轴垂直于地面向下。然后，按右手定则确定Z轴。

②通过电子罗盘与陀螺仪测得数据解算姿态。在使用惯性导航公式时，需要解算出物体的姿态角，最常用的方法有两种：一种是通过欧拉角微分方程;另一种是使用四元数软件解算。而在使用欧拉公式进行运算时，由于欧拉角微积分方程中有大量三角函数出现，使得解决问题变得尤为复杂。同时，欧拉公式具有限制性，其仅适用于姿态角变化量较小的情况，不适合全姿态的飞行姿态的确定。例如，俯仰角在90°时，出现万向节锁死的情况，限制了导航功效的展现。而四元数法因只含有四个未知量，计算量较小，是比较实用的工程方法。

③通过周期性速度积分获得位移、更新物体坐标。陀螺仪可形成一个导航坐标系的坐标测量轴，以稳定与反馈出航向轨迹角和偏航姿态角;而用来测量运动体加速度大小的加速度计通过测量运动体的加速度经过对时间的一次积分得到速度，速度再经过对时间的一次积分即可得到距离[2]。

④使用OPENMV视觉模块获得小车位置。在试验中，每隔2ms对传感器数据进行一次更新，带入惯性导航方程，推算出无人机在空间的位置。

OPENMV是一个机器视觉模块，其内置一个摄像头，进行颜色识别。为了实现准确识别，笔者选择了和地面反差较大的红色。通过编写程序，使模块每10ms对摄像头拍摄的图像进行扫描，提取整幅图像中颜色阈值接近红色的区域，并将其中心坐标发送给无人机飞行控制器。为了便于调试，制作了一辆小车，在其车身上放置了一块红色板，通过手机蓝牙APP进行遥控，经测试其最大速度为1m/s。

⑤算法驱动无人机跟踪小车。在获得小车与无人机在空间的位置后，使用卡尔曼滤波、串级PID等算法，同步演算和规划出小车和无人机的相对位置，对小车进行反馈性追踪。在反复试验，整定参数后，取得了较好的追踪效果。

3 该方法的创新点

3.1 使用互补滤波器解算姿态角

姿态角可以由加速度计和电子罗盘测量得到，漂移幅度小，却会产生较大噪声。同时，通过加速度计测得角速度积分后得到的偏航姿态角产生的噪声小，但漂移幅度大。互补滤波器的基本思想是利用其各自的优势，在频域上特征互补，得到更精确的姿态角。

互补滤波器估计俯仰角试验仿真图如图1所示，其中acc、wx和cf分别表示利用加速度估计的俯仰角、角速度直接积分后的俯仰角以及互补滤波后的俯仰角，利用Pixhawk自驾仪传感器实时传回的数据进行一阶向后差分法估计。从结果不难看出，通过互补滤波器，可以得到平滑的俯仰角估值，而对陀螺仪直接进行积分，结果是发散的。

3.2 使用离散卡尔曼滤波估算物体位置

卡尔曼滤波器是一种递归滤波器，可将过去的测量估计误差合并纳入新的测量误差中来估计将来的误差。在无人机的位置估算中引入离散卡尔曼滤波器，通过对运动状态卡尔曼滤波，推测下一时刻无人机位置，提前输入PID系统做出响应。在小车位置识别中引入离散卡尔曼滤波器，推测下一帧图像中小车的位置，缩小图片检测范围，加快处理速度。

3.3 融合惯导参数矫正物体位置误差

图2是使用OPENMV 摄像头识别色板后输出颜色位置。但是在无人机实际飞行中，为了追踪小车机身会倾斜，摄像头识别到的小车位置会产生误差，这种误差经过PID控制器放大后会导致系统严重超调。通过惯导数据的融合，小车的位置在机身倾斜的情况下基本不会改变，保证了整个系统的稳定性。

3.4 串级PID控制物体跟踪

在无人机追踪小车的过程中，普通的单级PID已经无法满足系统迅速消除误差的需求。因此，基于串级PID控制以小车速度作为内环使用。将小车位置设置为外环，由于控制系统中存在的内环回路，原有的对象特征被改变，等效时间常数变小，整个控制系统的过渡时间也被缩短，控制稳定性得到提高，并且外环控制器依据误差变换实时校准内环控制性的预设值，使得位置突发改变时，控制系统仍有较好的控制效果。

4 试验验证

在不断测试后，笔者整定了如图3所示的参数。导入经过飞控输出的惯性导航速度信息与OPENMV 输出的小车速度信息做差，由MATLAB工具箱画出无人机在25s内追踪速度图。

由图3可知，误差曲线基本平稳，实物追踪效果良好。

5 结语

本作品通过对无人机建立惯性导航系统，有效完成了对小车的追踪，是一次对经典力学的拓展与延伸。本作品可以直接应用于无人机在空中的定位问题，亦可以推广到机器人、无人汽车等定位问题上。

参考文献：

[1]杨鸿瑞.牛顿力学在惯性导航系统中的应用[J].科技创新导报，2015（32）：243-247.

[2]邓一民. 多旋翼无人机光流/惯性组合导航技术研究[D].南京：南京航空航天大学，2016.