CN113237478B - 一种无人机的姿态和位置估计方法及无人机 - Google Patents
一种无人机的姿态和位置估计方法及无人机 Download PDFInfo
- Publication number
- CN113237478B CN113237478B CN202110584179.XA CN202110584179A CN113237478B CN 113237478 B CN113237478 B CN 113237478B CN 202110584179 A CN202110584179 A CN 202110584179A CN 113237478 B CN113237478 B CN 113237478B
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- coordinate system
- data
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 230000001133 acceleration Effects 0.000 claims abstract description 48
- 238000012545 processing Methods 0.000 claims abstract description 18
- 238000004590 computer program Methods 0.000 claims abstract description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 42
- 238000001914 filtration Methods 0.000 claims description 21
- 238000006073 displacement reaction Methods 0.000 claims description 18
- 230000003044 adaptive effect Effects 0.000 claims description 16
- 238000005259 measurement Methods 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 9
- 230000004927 fusion Effects 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 8
- 238000012937 correction Methods 0.000 claims description 7
- 230000000295 complement effect Effects 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 6
- 239000000126 substance Substances 0.000 claims description 5
- 238000010606 normalization Methods 0.000 claims description 2
- 230000010354 integration Effects 0.000 abstract description 3
- 230000004069 differentiation Effects 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 14
- 238000004364 calculation method Methods 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 6
- 238000004891 communication Methods 0.000 description 5
- 238000007476 Maximum Likelihood Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 4
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000009466 transformation Effects 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000013500 data storage Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 229910052744 lithium Inorganic materials 0.000 description 2
- 230000006855 networking Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 239000003381 stabilizer Substances 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 101000701286 Pseudomonas aeruginosa (strain ATCC 15692 / DSM 22644 / CIP 104116 / JCM 14847 / LMG 12228 / 1C / PRS 101 / PAO1) Alkanesulfonate monooxygenase Proteins 0.000 description 1
- 101000983349 Solanum commersonii Osmotin-like protein OSML13 Proteins 0.000 description 1
- 101000983338 Solanum commersonii Osmotin-like protein OSML15 Proteins 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000005347 demagnetization Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000031700 light absorption Effects 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 210000001747 pupil Anatomy 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
- G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明的一种无人机的姿态和位置估计方法及无人机涉及无人机控制领域，目的是为了克服现有无人机采用PID控制器并不能表现出较好的鲁棒性的问题，其中无人机，包括无人机机体和与无人机机体固定的惯性传感单元、图像处理单元和微控制单元；惯性传感单元自身坐标系与机体坐标系保持一致，该惯性传感单元与微控制单元连接，用于测量得到无人机的三轴角速度数据和三轴加速度数据；图像处理单元固定于无人机机体的下方，该图像处理单元与微控制单元连接，用于采集一个AprilTag标签的图像；微控制单元包括存储设备、处理器以及存储在存储设备中并可在处理器上运行的计算机程序，处理器执行计算机程序实现上述其中一项的方法。
Description
技术领域
本发明涉及无人机控制领域。
背景技术
四旋翼无人机作为小型无人机家族的成员，具备体积较小、飞行灵活、可垂直起降与定点悬停、便携等优点。但是无人机仍然面临着控制稳定性差、多无人机组网通信效率低、相对位置保持困难与碰撞等诸多实际难题。无人机的稳定控制是进行多机编队控制的重要前提，而时钟温漂、传感器噪声、信号干扰等硬件问题，电机失速、高温退磁等机械问题，内存泄漏、编译优化错误、运算精度不高等软件问题，以及吸光导致激光测距失效反光导致机器视觉摄像头失效等传感器问题。
在控制算法方面，现有的位置式或增量式PID能够解决大部分控制问题，四旋翼无人机也不例外。但是面对气流干扰、执行器间歇性失控等问题，现有PID控制器并不能表现出较好的鲁棒性。
发明内容
本发明的目的是为了克服现有无人机采用PID控制器并不能表现出较好的鲁棒性的问题，提供了一种无人机的姿态和位置估计方法及无人机。
本发明的一种无人机的姿态和位置估计方法，包括无人机姿态估计方法，无人机姿态估计方法具体步骤如下：
步骤一、对无人机三轴加速度量测数据进行归一化，得到单位观测加速度向量
单位观测加速度向量
为重力加速度方向在机体坐标系中的表示；
步骤二、计算导航坐标系下的运动速度向量
在机体坐标系中的表示，得到单位估计加速度向量
步骤三、在机体坐标系下，计算单位观测向量
(机载IMU传感器直接提供的机体坐标系b系的三轴归一化加速度向量)和单位估计加速度向量
的叉积(重力加速度归一化向量在姿态解算算法计算所得机体坐标系b系上的投影)，得到误差向量
步骤四、将误差向量
作为自适应卡尔曼滤波器的输入，得到自适应卡尔曼滤波器的输出作为三轴角速度数据的补偿量，并通过补偿量与未经过补偿的三轴角速度数据得到得到补偿后的三轴角速度数据；
步骤五、通过补偿后的三轴角速度数据，估计无人机姿态。
进一步地，步骤一中的单位观测加速度向量
通过如下公式计算得到：
其中，ax、ay和az表示经过滤波处理的无人机三轴加速度量测数据；无人机三轴加速度量测数据通过搭载于无人机上的惯性传感单元测量得到。
进一步地，步骤二中获得运动速度向量
的具体步骤如下；
步骤二一、将机体坐标系下的加速度向量
通过旋转矩阵
旋转至导航坐标系，并去除旋转后加速度向量
中的重力加速度分量，得到导航坐标系下的运动加速度向量
步骤二二、通过对导航坐标系下的运动加速度向量
进行积分，得到导航坐标系下的运动速度向量
进一步地，步骤二中的单位估计加速度向量
通过如下公式计算得到：
其中，q0、q1、q2、q3分别为用于表示导航坐标系n系到机体坐标系b系两者旋转过程的四元数q的实部和三个虚部，即q＝[q0,q1,q2,q3]。
进一步地，步骤五的具体方法如下：
求解四元数微分方程：
采用一阶龙格库塔方法计算无人机当前的姿态角以估计无人机姿态；
其中，
为上一次四元数估计向量；
为当前四元数估计向量，即四元数微分方程的解；T为四元数互补滤波算法的迭代周期，单位为秒；gyrox、gyroy、gyroz分别为机体坐标系b系上的三轴角速度，由机载IMU直接测量后滤波得到。
其中，
表示当前未经过补偿的三轴角速度数据，
表示经过补偿的三轴角速度数据，
为三轴角速度数据的补偿量。
进一步地，还包括无人机位置估计方法，无人机位置估计方法具体步骤如下：
步骤六一、得到无人机X轴、Y轴和Z轴的位移初始数据；
步骤六二、获得无人机的Z轴位移校正数据，并利用Z轴位移校正数据对Z轴的位移初始数据进行校正；
步骤六三、通过AprilTags标签平面阵列得到地面坐标系下无人机的水平位置数据，并利用水平位置数据对X轴和Y轴的位移初始数据进行校正，完成无人机位置估计。
进一步地，无人机的Z轴位移校正数据是通过对地距离和海拔高度进行数据融合得到的。
进一步地，
步骤六三一、在地面上将多个AprilTags标签排列为AprilTags标签平面阵列，各AprilTags标签包括自身的像素位置与ID号；
步骤六三二、无人机采集一个AprilTag标签的图像，通过该AprilTags标签的像素位置、ID号以及无人机当前的姿态角，计算出无人机在AprilTags标签阵列中的坐标，即为地面坐标系下无人机的水平位置数据。
本发明的一种无人机，包括无人机机体和与无人机机体固定的惯性传感单元、图像处理单元和微控制单元；
惯性传感单元自身坐标系与机体坐标系保持一致，该惯性传感单元与微控制单元连接，用于测量得到无人机的三轴角速度数据和三轴加速度数据，并发送至微控制单元；
图像处理单元固定于无人机机体的下方，该图像处理单元与微控制单元连接，用于采集一个AprilTag标签的图像，并发送至微控制单元；
微控制单元包括存储设备、处理器以及存储在存储设备中并可在处理器上运行的计算机程序，处理器执行计算机程序实现上述其中一项的方法。
进一步地，还包括激光测距模组和气压计；
激光测距模组固定于无人机机体的下方，该激光测距模组与微控制单元连接，用于测量对地距离，并发送至微控制单元；
气压计，与微控制单元连接，用于测量海拔高度，并发送至微控制单元。
本发明的有益效果是：
本发明以无人机定点飞行、无人机组网为背景，依据视觉定位、姿态控制基本原理和流程，设计实现了无人机飞控电路板和控制***。
无人机飞行测试并定点悬停时，悬停效果良好，充分证明了本发明所设计的无人机实现了预期设计目标和预期测试目标，实现了小型化、飞行灵活、定位精确和控制稳定等目标。本发明中所提出并应用的基于自适应卡尔曼滤波器的姿态解算与组合导航位姿估计算法，相比于经典的四元数互补滤波算法等，对无人机飞行过程中的突发干扰(气流、电磁、大机动)等具有更好的鲁棒性。
附图说明
图1为本发明的无人机的处理器的电路拓扑图；
图2为本发明的无人机的处理器与惯性传感单元MPU9250相连的电路拓扑图；
图3为本发明的无人机的处理器与WIFI模块ESP8266相连的电路拓扑图；
图4为本发明的无人机的处理器与数据存储模块EEPROM相连的电路拓扑图；
图5为本发明的无人机的处理器与激光测距模组TOF10120、图像处理单元OpenMv4、光流传感器PMW3901和USART3相连的电路拓扑图；
图6为本发明的无人机的处理器与2.4G无线芯片NRF24L01相连的电路拓扑图；
图7为本发明的无人机的处理器与程序下载器SWD Download相连的电路拓扑图；
图8为本发明的无人机的处理器与存储卡TF Card相连的电路拓扑图；
图9为本发明的无人机的处理器与显示屏相连的电路拓扑图；
图10为本发明的无人机的处理器与电机驱动相连的电路拓扑图；
图11为本发明的无人机的处理器与气压计相连的电路拓扑图；
图12为本发明的无人机的模块结构示意图；
图13为本发明的一种无人机的姿态和位置估计方法中AprilTags标签平面阵列的示意图；
图14本发明的一种无人机的姿态和位置估计方法中图像坐标转换算法原理示意图。
具体实施方式
其中，无人机的PCB制版技术：
由于无人机体积小的特性，所以对PCB板的布线要求高，需要将PCB板上的飞控***进行缩小，尽量减少各元件之间的交叉。无人机PCB板布局如果不好，则会使输入的交流信号的干扰变大影响电路的输出，从而影响整个***的性能。
本发明针对无人机以上特点，对开发的无人机的PCB板进行精小化设计，优化PCB板的布局和布线。：
无人机的姿态解算技术
四旋翼无人机的姿态控制器需要当前姿态角和目标姿态角之差作为输入。惯性传感单元提供的是三轴加速度和三轴角速度数据，需要通过姿态解算算法计算出实时姿态角数据。常用的无人机姿态角的估算算法主要有直接积分法和由Madgwick提出的四元数互补滤波算法。
无人机的位置估计技术：
四旋翼无人机的位置控制器需要当前位置和目标位置之差作为输入。当前位置的估计需要三轴加速度与三轴姿态角数据，经过位置估计算法计算而得。通常情况下，机载惯性传感单元有两种机载情况，分别为平台式和捷联式。
本发明所设计的无人机为捷联式机载情况，位置估计对应采用捷联式惯性导航算法，并结合包括机器视觉定位算法在内的多种位置估计方法进行观测矫正。捷联式机载情况下，惯性传感单元自身坐标系与机体坐标系保持一致。在此情况下需要先以软件定义平台坐标系，再进行位置估计，这种算法为捷联式惯性导航算法。此算法机械和硬件实现简单，但算法复杂，计算量较大。
基于机器视觉的定位技术：
本发明选用星瞳科技公司以STM32H743VIT6型号MCU作为微控制单元的OpenMV4模组，作为机器视觉定位算法的运行平台，是一个合适的选择。为减轻重量，本专利选用了轻量级的OpenMV4 mini摄像头作为无人机机载图像处理单元，结合AprilTags标签识别技术，搭建可表示全局坐标的AprilTags阵列，并通过高级语言Python脚本，实现基于机器视觉的无人机导航坐标系定位算法。
电路板上设置有电机驱动模块U1、惯性传感器U2、气压计U3通过测算当前环境气压换算为地球海拔高度、U4提供足够的算力来实现无人机姿态解算与控制算法、2.4G定频天线U5用于控制无人机飞行、OLED屏幕U6显示无人机当前状态、EEPROM存储芯片U7用于存储无人机数据、电压检测模块U8用于检测无人机电池电压、wifi模块U9可以通过wifi控制无人机、SWD(调试模式)调试接口U10用于下载和调试无人机程序、U11和U12电压模块提供稳定的3.3V电压和5V电压给无人机供电。在无人机位姿控制与导航定位算法中，采用基于ML估计的AKF算法处理IMU等传感器量测；采用分布式融合算法实现多IMU最优信息融合；采用PDA算法实现多测距仪容错信息融合；采用基于AKF的松组合导航算法实现INS与视觉定位***的信息融合；采用四环串级PID算法实现定点位姿控制。
其中，如图1～图12所示，处理器stm32的PC0、PC1、PC2、PC3与屏幕相连；
处理器stm32的PA0、PA1与激光测距传感器TOF10120的TXD、RXD相连；
处理器stm32的PA2、PA3与光流传感器PMW3901的TXD、RXD相连；
处理器stm32的PB10、PB11与USART3的TXD、RXD相连；
处理器stm32的PA9、PA10与图像识别单元OpenMv4的TXD、RXD相连；
处理器stm32的PA4与IMU器件MPU9250相连；
处理器stm32的PA5、PA6、PA7与气压传感器MS5611、惯性传感单元MPU9250相连；
处理器stm32的PC4与气压传感器MS5611相连；
处理器stm32的PC5与电压检测模块ADC相连；
处理器stm32的PB0和PB1与数据存储模块EEPROM的SDA、SCL相连；
处理器stm32的PB12、PB13、PB14、PB15、PC6、PC7与2.4G无线芯片NRF24L01相连；
处理器stm32的PC8、PC9、PC10、PC11、PC12、PD2与存储卡TF Card相连；
处理器stm32的PB2、PA8、PA11、PA12与状态灯相连；
处理器stm32的PA13、PA14与程序下载器SWD Download相连；
处理器stm32的PA15、PB3、PB4、PB5、PC13与WIFI模块ESP8266相连；
处理器stm32的PB6、PB7、PB8、PB9与Motor Driver相连；
处理器stm32的NRST与RESET相连；
第60号引脚与BOOT LOADER相连；
其中，处理器stm32与存储卡TF Card组成微控制单元1。
下面结合对无人机的具体设计进行说明：
无人机机体的四个角，分别焊有一个NMOS驱动模块U1；中心部位的左上方是陀螺仪U2以及气压计U3；中心部位外侧是OLED显示屏U6；U6的内侧是微控制单元U4和WIFI模块U9；中心部位的左下方是2.4G定频天线；中心部位的右下方是SWD下载调试接口U10；中心部位从左至右还含有EEPROM模块U7、电压检测模块U8、5V升压电路U11、3.3V降压电路U12。布局上精致美观，面积利用率高，大大降低无人机的重量，从而减少了电能损耗，提高了无人机的续航能力。
下面对部分单元进行详细说明：
微控制单元使用的是ST公司的STM32F405RGT6型号MCU，可以提供足够的算力来实现本文提到的姿态解算与控制算法，且没有过多的性能浪费。
存储单元使用Microchip公司的24LC02BT-I/OT型号EEPROM存储IC，负责重要数据的掉电存储，包括但不限于控制器相关参数、角速度计零飘测量数据、加速度计零飘测量数据等，其可以选用任意通用GPIO而无需考虑特殊硬件接口，且SOT23-5的微小封装有利于硬件空间利用率的提高。
惯性传感单元选用Invensense公司的MPU9250型号IMU，很大程度上减小了通信负担。
辅助传感单元选用MEAS公司的MS5611型号气压计和Sharp公司的TOF10120激光测距模组，可利用数据融合算法互补长短，实现对高度信息的估计；激光测距模组挂载在无人机的底部。
无线通信单元采用了基于Zigbee的无线自组织网络通信结构，可在高速移动节点情况下的自适应调整能力。
电源管理单元选用TI公司的LMR62014S型号Boost升压稳压器提供5V电源，建立锂电池指向OpenMV模组的拓扑路线；选用TI公司的TPS63000.Q1型号Buck.Boost升降压稳压器提供3.3V电源，建立锂电池指向相关单元的拓扑路线。
无人机的驱动部分是连接硬件和软件部分的重要桥梁，该发明根据对应硬件单元的负责功能和硬件接口定义，使用C语言和汇编语言进行联编，为各硬件单元设计驱动程序，确保微控制单元可以与其他功能性单元协同工作，功能部分定义了无人机的任务，主要由传感器数据处理、姿态解算、姿态与位置控制、无线通讯和显存刷新等部分组成，下面对无人机的算法程序设计进行说明：
数字滤波器算法方面应用了基于极大似然估计的自适应卡尔曼滤波进行去噪，虽然卡尔曼滤波只能够拟合线性高斯***，但是其最大的优点在于计算量很小的同时滤波效果也比较好，因此适用于采样频率为1kHz的惯性数据的滤波处理。为了提高运行速度，且六轴惯性数据之间理论上没有耦合关系，本发明采用独立的一维自适应线性卡尔曼滤波器分别对六轴惯性数据进行处理，不考虑协方差。
姿态解算算法方面采用基于自适应卡尔曼滤波的四元数组合姿态估计方法，以上次估计姿态与本次加速度计计算姿态之差作为卡尔曼滤波器的输入，利用它的输出去修正本次三轴角速度，最后通过一阶龙格库塔法将修正过的三轴角速度积分到上次估计姿态上，得到本次估计姿态。这种算法较为复杂，计算量较大，但是同时利用了角速度计的动态优势和加速度计的静态优势，且基于极大似然估计的自适应卡尔曼滤波器能够有效适应机体的机动不确定性，以及非线性***下协方差矩阵的传递。因此，这可以在长时间内保证稳定性和准确性，适用于无人机。
自适应卡尔曼滤波算法：
进行自适应卡尔曼滤波算法迭代之前，需要通过采样统计确定过程噪声协方差Q，通过物理模型建立完善的过程方程和量测方程，人为确认衰减因子参数γ和待用历史新息数N的大小，并在之后的过程中始终不变。
第一步，确认初值
和Pk-1|k-1。
第二步，确认一步状态预测
及其相伴协方差矩阵Pk|k-1。
Pk|k-1＝Φk-1Pk-1|k-1Φk-1 T+Gk-1Qk-1Gk-1 T
第三步，确认一步量测预测
第四步，得到新的量测值Zk，计算新息，得过去N个量测值对应的新息{vk-N+1,…,vk}。
第五步，确认一步量测预测协方差矩阵的极大似然估计
其中ξi为指数衰减因子，描述为：
第六步，确认过程噪声方差矩阵的极大似然估计
并确认自适应卡尔曼滤波增益Kk。
第七步，确认***状态滤波估计值
及其相伴协方差矩阵Pk|k。
Pk|k＝Pk|k-1-KkHkPk|k-1
基于自适应卡尔曼滤波的姿态解算算法：
第一步，归一化三轴加速度量测数据，得到单位观测加速度向量
即重力加速度方向在机体坐标系中的表示。
其中，ax、ay和az表示经过滤波处理的三轴加速度数据，无单位要求。
第二步，计算得到导航坐标系的
向量在机体坐标系中的表示，得到单位估计加速度向量
第三步，在机体坐标系下，计算单位观测向量
和单位估计向量
的叉积，得到误差向量
第四步，以误差向量作为自适应卡尔曼滤波器的输入，以其输出作为三轴角速度数据的补偿量：
其中，
表示本次未经补偿的三轴角速度数据，
表示经过补偿的三轴角速度数据。
第五步，求解四元数微分方程，采用一阶龙格库塔方法计算本次估计所得的无人机姿态，即导航坐标系n系到机体坐标系b系之间的旋转关系，用归一化四元数q表示。则t时刻的q可以用t-1时刻的q迭代计算得到。
其中，
为上次四元数估计向量，
为本次四元数估计向量，T为四元数互补滤波算法的迭代周期，单位为秒。
捷联式惯性导航：将机体坐标系下的通过IMU器件直接测量所得的加速度向量
系下的加速度向量)通过旋转矩阵
旋转至导航坐标系，并去除重力加速度分量，得到导航坐标系下的运动加速度向量
通过对导航坐标系下的运动加速度向量
进行积分，可以得到导航坐标系下的运动速度向量
以及位移向量
辅助传感定位：依靠惯性传感单元执行捷联式惯性导航算法所得的运动速度和位移数据具有良好的动态响应特性，但是惯性传感单元本身存在不可避免的零飘问题，所以所得数据存在漂移问题，尤其是经过二次积分的位移数据。TOF10120激光测距模组与MS5611气压计可以分别以对地距离、海拔高度作为原数据，提供较为准确的无人机Z轴位移数据，以此作为校准值对捷联式惯性导航的Z轴结果进行矫正。PWM3901光流传感器与机载OpenMV可以通过对地图像处理，以某种规则获取无人机在XY轴的准确位移数据，以此作为校准值对捷联式惯性导航的XY轴结果进行校准，机载OpenMV提供了可用于检测图像中AprilTags的ID、位置、偏角等数据的功能库。结合规律可循的AprilTags平面阵列，经坐标变换算法，即可由图像坐标系中的标签坐标，得到地面坐标系下无人机的水平位置数据。
基于自适应卡尔曼滤波的组合导航定位算法：本发明采用基于自适应卡尔曼滤波的组合导航定位算法，利用辅助传感单元所得位置数据对捷联式惯性导航所得加速度、速度与位置数据，结合无人机六维自由度下的运动学模型，实现对无人机位姿的最优估计。
基于机器视觉的定位算法：为减轻重量，本发明选用了轻量级的OpenMV4 mini摄像头作为无人机机载图像处理单元，结合AprilTags标签识别技术，搭建可表示全局坐标的AprilTags阵列，并通过高级语言Python脚本，实现基于机器视觉的无人机导航坐标系定位算法。
AprilTags标签定位：OpenMV4可以快速识别图片中的AprilTags标签，提供标签ID号、图像坐标系中的标签位置数据，将多个AprilTags按规律等距离排列为二维阵列，由OpenMV4提供的视野中某一个AprilTag的像素位置与ID号，结合无人机当前的姿态角信息，可直接计算出无人机机体在AprilTags阵列中的坐标。AprilTags阵列如图13所示。以AprilTags标签阵列所建立的直角坐标系作为导航坐标系，即可对无人机进行位置观测。再通过三阶融合互补滤波算法，校准捷联式惯性导航的位置估计结果，即可获得更可靠的位置数据。
图像坐标转换算法：机载OpenMV4摄像头与无人机机体相对固定，且摄像头方向指向机体坐标系Z轴反方向，因此机体坐标系向量
可以表示摄像头方向向量，并通过描述向量
(机体坐标系Z轴在世界坐标系中的表示)至向量
(导航坐标系Z轴在世界坐标系中的表示)旋转过程的旋转矩阵
旋转图像中心坐标，得到转化之后的位置坐标。但是，图像中心坐标同样需要标签在图像中的位置与无人机当前姿态、高度进行计算获得。图像中心、标签中心与无人机中心位置关系如图14所示。其中，OXYZs表示以机体为圆心，与导航坐标系平行的坐标系，下文简称s系。OXYZb表示机体坐标系，下文简称b系。α平面表示AprilTags阵列平面，β平面表示b系Z轴以法线方向的图像平面。T点为图像中AprilTags标签在α平面的位置，Ti点为图像中AprilTags标签在β平面的位置，TiC为α与β平面交线的垂线。A与B分别为图像中心与无人机机体在s系中的投影点。PP'与PP”描述了投影点A与B基于s系的位置变换关系。
令向量
和向量
为θT，向量
和向量
的夹角为γT，以右手螺旋定则定义旋转正方向，经由两次坐标系旋转(从像素坐标系(图像处理单元所采集的一个AprilTags标签的图像所在平面为XOY的右手坐标系)到机体坐标系，再从机体坐标系到导航坐标系)，无人机当前位置
可表示如下：
其中，c(*)和s(*)分别表示cos(*)和sin(*)，tx,n和ty,n表示被定位标签在AprilTags阵列所定义的导航坐标系中的XY轴坐标。
Claims (9)
1.一种无人机的姿态和位置估计方法，其特征在于，包括无人机姿态估计方法，所述无人机姿态估计方法具体步骤如下：
步骤一、对无人机三轴加速度量测数据进行归一化，得到单位观测加速度向量
所述单位观测加速度向量
为重力加速度方向在机体坐标系中的表示；
步骤二、计算导航坐标系下的运动速度向量
在机体坐标系中的表示，得到单位估计加速度向量
步骤三、在机体坐标系下，计算单位观测向量
和单位估计加速度向量
的叉积，得到误差向量
步骤四、将误差向量
作为自适应卡尔曼滤波器的输入，得到所述自适应卡尔曼滤波器的输出作为三轴角速度数据的补偿量，并通过补偿量与未经过补偿的三轴角速度数据得到补偿后的三轴角速度数据；
步骤五、通过补偿后的三轴角速度数据，估计无人机姿态；
还包括无人机位置估计方法，所述无人机位置估计方法具体步骤如下：
步骤六一、得到无人机X轴、Y轴和Z轴的位移初始数据；
步骤六二、获得无人机的Z轴位移校正数据，并利用Z轴位移校正数据对Z轴的位移初始数据进行校正；
步骤六三、通过AprilTags标签平面阵列得到地面坐标系下无人机的水平位置数据，并利用所述水平位置数据对X轴和Y轴的位移初始数据进行校正，完成无人机位置估计。
2.根据权利要求1所述的一种无人机的姿态和位置估计方法，其特征在于，步骤一中的单位观测加速度向量
通过如下公式计算得到：
其中，ax、ay和az表示经过滤波处理的无人机三轴加速度量测数据；所述无人机三轴加速度量测数据通过搭载于无人机上的惯性传感单元测量得到。
3.根据权利要求2所述的一种无人机的姿态和位置估计方法，其特征在于，步骤二中获得运动速度向量
的具体步骤如下；
步骤二一、将机体坐标系下的加速度向量
通过旋转矩阵
旋转至导航坐标系，并去除旋转后加速度向量
中的重力加速度分量，得到导航坐标系下的运动加速度向量
步骤二二、通过对导航坐标系下的运动加速度向量
进行积分，得到导航坐标系下的运动速度向量
4.根据权利要求3所述的一种无人机的姿态和位置估计方法，其特征在于，步骤二中的单位估计加速度向量
通过如下公式计算得到：
其中，q0、q1、q2、q3分别为用于表示导航坐标系到机体坐标系两者旋转过程的四元数q的实部和三个虚部。
5.根据权利要求3所述的一种无人机的姿态和位置估计方法，其特征在于，步骤五的具体方法如下：
求解四元数微分方程：
采用一阶龙格库塔方法计算无人机当前的姿态角以估计无人机姿态；
其中，
为上一次四元数估计向量；
为当前四元数估计向量，即四元数微分方程的解；T为四元数互补滤波算法的迭代周期，单位为秒；
其中，
表示当前未经过补偿的三轴角速度数据，
表示经过补偿的三轴角速度数据，
为三轴角速度数据的补偿量。
6.根据权利要求1所述的一种无人机的姿态和位置估计方法，其特征在于，无人机的Z轴位移校正数据是通过对地距离和海拔高度进行数据融合得到的。
7.根据权利要求1所述的一种无人机的姿态和位置估计方法，其特征在于，
步骤六三一、在地面上将多个AprilTags标签排列为AprilTags标签平面阵列，各AprilTags标签包括自身的像素位置与ID号；
步骤六三二、无人机采集一个AprilTag标签的图像，通过该AprilTags标签的像素位置、ID号以及无人机当前的姿态角，计算出无人机在AprilTags标签阵列中的坐标，即为地面坐标系下无人机的水平位置数据。
8.一种无人机，其特征在于，包括无人机机体和与无人机机体固定的惯性传感单元(1)、图像处理单元(2)和微控制单元(3)；
所述惯性传感单元(1)自身坐标系与机体坐标系保持一致，该惯性传感单元(1)与微控制单元(3)连接，用于测量得到无人机的三轴角速度数据和三轴加速度数据，并发送至微控制单元(3)；
所述图像处理单元(2)固定于无人机机体的下方，该图像处理单元(2)与微控制单元(3)连接，用于采集一个AprilTag标签的图像，并发送至微控制单元(3)；
所述微控制单元(3)包括存储设备、处理器以及存储在所述存储设备中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序实现权利要求7所述的方法。
9.根据权利要求8所述的一种无人机，其特征在于，还包括激光测距模组(4)和气压计(5)；
所述激光测距模组(4)固定于无人机机体的下方，该激光测距模组(4)与微控制单元(3)连接，用于测量对地距离，并发送至微控制单元(3)；
所述气压计(5)，与微控制单元(3)连接，用于测量海拔高度，并发送至微控制单元(3)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110584179.XA CN113237478B (zh) | 2021-05-27 | 2021-05-27 | 一种无人机的姿态和位置估计方法及无人机 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110584179.XA CN113237478B (zh) | 2021-05-27 | 2021-05-27 | 一种无人机的姿态和位置估计方法及无人机 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113237478A CN113237478A (zh) | 2021-08-10 |
| CN113237478B true CN113237478B (zh) | 2022-10-14 |
Family
ID=77139108
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110584179.XA Active CN113237478B (zh) | 2021-05-27 | 2021-05-27 | 一种无人机的姿态和位置估计方法及无人机 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113237478B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114007047B (zh) * | 2021-11-02 | 2024-03-15 | 广西电网有限责任公司贺州供电局 | 一种基于机器视觉的变电现场作业实时监控与报警*** |
| CN116298882A (zh) * | 2023-05-18 | 2023-06-23 | 深圳市好盈科技股份有限公司 | 一种目标无人机电机消磁检测方法和装置 |
Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105136145A (zh) * | 2015-08-11 | 2015-12-09 | 哈尔滨工业大学 | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 |
| CN106197422A (zh) * | 2016-06-27 | 2016-12-07 | 东南大学 | 一种基于二维标签的无人机定位及目标跟踪方法 |
| CN107139178A (zh) * | 2017-05-10 | 2017-09-08 | 哈尔滨工业大学深圳研究生院 | 一种无人机及其基于视觉的抓取方法 |
| CN108007474A (zh) * | 2017-08-31 | 2018-05-08 | 哈尔滨工业大学 | 一种基于地面标识的无人飞行器自主定位及位姿校正技术 |
| CN108592917A (zh) * | 2018-04-25 | 2018-09-28 | 珠海全志科技股份有限公司 | 一种基于失准角的卡尔曼滤波姿态估计方法 |
| CN109724602A (zh) * | 2018-12-17 | 2019-05-07 | 南京理工大学 | 一种基于硬件fpu的姿态解算***及其解算方法 |
| CN110017837A (zh) * | 2019-04-26 | 2019-07-16 | 沈阳航空航天大学 | 一种姿态抗磁干扰的组合导航方法 |
| CN110081878A (zh) * | 2019-05-17 | 2019-08-02 | 东北大学 | 一种多旋翼无人机的姿态及位置确定方法 |
| CN110375773A (zh) * | 2019-07-29 | 2019-10-25 | 兰州交通大学 | Mems惯导***姿态初始化方法 |
| CN110702107A (zh) * | 2019-10-22 | 2020-01-17 | 北京维盛泰科科技有限公司 | 一种单目视觉惯性组合的定位导航方法 |
| CN110793515A (zh) * | 2018-08-02 | 2020-02-14 | 哈尔滨工业大学 | 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法 |
| CN111197984A (zh) * | 2020-01-15 | 2020-05-26 | 重庆邮电大学 | 一种基于环境约束下的视觉-惯性运动估计方法 |
| CN111831962A (zh) * | 2020-07-14 | 2020-10-27 | 河北科技大学 | 四旋翼无人机姿态解算方法、装置及终端设备 |
| CN112013836A (zh) * | 2020-08-14 | 2020-12-01 | 北京航空航天大学 | 一种基于改进自适应卡尔曼滤波的航姿参考***算法 |
| CN112630813A (zh) * | 2020-11-24 | 2021-04-09 | 中国人民解放军国防科技大学 | 基于捷联惯导和北斗卫星导航***的无人机姿态测量方法 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11294397B2 (en) * | 2017-02-24 | 2022-04-05 | Teledyne Fur Detection, Inc. | Control systems for unmanned aerial vehicles |
| CN108225370B (zh) * | 2017-12-15 | 2024-01-30 | 路军 | 一种运动姿态传感器的数据融合与解算方法 |
| CN109916407B (zh) * | 2019-02-03 | 2023-03-31 | 河南科技大学 | 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法 |
| CN109931926B (zh) * | 2019-04-04 | 2023-04-25 | 山东智翼航空科技有限公司 | 一种基于站心坐标系的小型无人机无缝自主式导航方法 |
| CN111076722B (zh) * | 2019-11-18 | 2022-07-19 | 广州南方卫星导航仪器有限公司 | 基于自适应的四元数的姿态估计方法及装置 |
-
2021
- 2021-05-27 CN CN202110584179.XA patent/CN113237478B/zh active Active
Patent Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105136145A (zh) * | 2015-08-11 | 2015-12-09 | 哈尔滨工业大学 | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 |
| CN106197422A (zh) * | 2016-06-27 | 2016-12-07 | 东南大学 | 一种基于二维标签的无人机定位及目标跟踪方法 |
| CN107139178A (zh) * | 2017-05-10 | 2017-09-08 | 哈尔滨工业大学深圳研究生院 | 一种无人机及其基于视觉的抓取方法 |
| CN108007474A (zh) * | 2017-08-31 | 2018-05-08 | 哈尔滨工业大学 | 一种基于地面标识的无人飞行器自主定位及位姿校正技术 |
| CN108592917A (zh) * | 2018-04-25 | 2018-09-28 | 珠海全志科技股份有限公司 | 一种基于失准角的卡尔曼滤波姿态估计方法 |
| CN110793515A (zh) * | 2018-08-02 | 2020-02-14 | 哈尔滨工业大学 | 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法 |
| CN109724602A (zh) * | 2018-12-17 | 2019-05-07 | 南京理工大学 | 一种基于硬件fpu的姿态解算***及其解算方法 |
| CN110017837A (zh) * | 2019-04-26 | 2019-07-16 | 沈阳航空航天大学 | 一种姿态抗磁干扰的组合导航方法 |
| CN110081878A (zh) * | 2019-05-17 | 2019-08-02 | 东北大学 | 一种多旋翼无人机的姿态及位置确定方法 |
| CN110375773A (zh) * | 2019-07-29 | 2019-10-25 | 兰州交通大学 | Mems惯导***姿态初始化方法 |
| CN110702107A (zh) * | 2019-10-22 | 2020-01-17 | 北京维盛泰科科技有限公司 | 一种单目视觉惯性组合的定位导航方法 |
| CN111197984A (zh) * | 2020-01-15 | 2020-05-26 | 重庆邮电大学 | 一种基于环境约束下的视觉-惯性运动估计方法 |
| CN111831962A (zh) * | 2020-07-14 | 2020-10-27 | 河北科技大学 | 四旋翼无人机姿态解算方法、装置及终端设备 |
| CN112013836A (zh) * | 2020-08-14 | 2020-12-01 | 北京航空航天大学 | 一种基于改进自适应卡尔曼滤波的航姿参考***算法 |
| CN112630813A (zh) * | 2020-11-24 | 2021-04-09 | 中国人民解放军国防科技大学 | 基于捷联惯导和北斗卫星导航***的无人机姿态测量方法 |
Non-Patent Citations (5)
| Title |
|---|
| A Multi-source Fused Location Estimation Method for UAV Based on Machine Vision and Strapdown Inertial Navigation;Jiapeng Li等;《Artificial Intelligence for Communications and Networks. Second EAI International Conference, AICON 2020》;20210430;全文 * |
| The Attitude Adjustment of Multi-rotor Unmanned Aerial Vehicles Based on Information Fusion Analysis;Peng Xie等;《2019 International Conference on Smart Grid and Electrical Automation (ICSGEA)》;20191114;全文 * |
| 基于STM32的多传感器四旋翼姿态控制***设计;任剑秋等;《电子技术应用》;20210506(第05期);全文 * |
| 时变通信延迟下的无人机编队鲁棒自适应控制;郑重等;《中国惯性技术学报》;20160215(第02期);全文 * |
| 面向建筑物检测的无人机定位导航***研究;罗阳;《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》;20200215(第02期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113237478A (zh) | 2021-08-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11802769B2 (en) | Lane line positioning method and apparatus, and storage medium thereof | |
| US11279045B2 (en) | Robot pose estimation method and apparatus and robot using the same | |
| CN109885080B (zh) | 自主控制***及自主控制方法 | |
| Achtelik et al. | Autonomous navigation and exploration of a quadrotor helicopter in GPS-denied indoor environments | |
| CN113237478B (zh) | 一种无人机的姿态和位置估计方法及无人机 | |
| EP3388786A1 (en) | Autonomous positioning and navigation device, positioning and navigation method and autonomous positioning and navigation system | |
| EP1901153A1 (en) | Control system for unmanned 4-rotor-helicopter | |
| CN104280022A (zh) | 视觉辅助惯性测量单元的数字头盔显示设备跟踪*** | |
| CN114088087B (zh) | 无人机gps-denied下高可靠高精度导航定位方法和*** | |
| WO2018182524A1 (en) | Real time robust localization via visual inertial odometry | |
| CN105843249A (zh) | 基于Pixhawk飞控的无人机自动巡航***及其巡航方法 | |
| CN110763224A (zh) | 一种自动导引运输车导航方法及导航*** | |
| CN102506875B (zh) | 无人机的导航方法及装置 | |
| CN104406592B (zh) | 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法 | |
| Riether | Agile quadrotor maneuvering using tensor-decomposition-based globally optimal control and onboard visual-inertial estimation | |
| CN116952229A (zh) | 无人机定位方法、装置、***和存储介质 | |
| Luo et al. | An autonomous helicopter with vision based navigation | |
| Hafner et al. | An autonomous flying robot for testing bio-inspired navigation strategies | |
| Wu et al. | Integrated navigation algorithm based on vision-inertial extended Kalman filter for low-cost unmanned aerial vehicle | |
| Hassaballa et al. | Real Time Full States Integrated Low Cost Navigation System for Autonomous Vehicles | |
| CN117408084B (zh) | 一种用于无人机航迹预测的增强卡尔曼滤波方法及*** | |
| Singhania et al. | Autonomous navigation of a multirotor using visual odometry and dynamic obstacle avoidance | |
| Wang et al. | Real-time visual odometry for autonomous MAV navigation using RGB-D camera | |
| Mendes | Cost-effective robot for steep slope crops monitoring | |
| Hou et al. | Visual Inertial Navigation Optimization Method Based on Landmark Recognition |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |