动手学无人驾驶(6):基于IMU和GPS数据融合的自车定位

在这里插入图片描述

在上一篇博文《动手学无人驾驶(5):多传感器数据融合》介绍了如何使用Radar和LiDAR数据对自行车进行追踪,这是对汽车外界运动物体进行定位。
对于自动驾驶的汽车来说,有时也需要对自身进行更精确的定位,今天就介绍如何使用IMUGPS进行自车定位(因为在上一篇文章中对kalmanExtend Kalman原理进行了比较详细的介绍,本文中理论部分不会再介绍的这么详细了,有需要的话可以回看上文)。
本文参考了Coursera自动驾驶课程项目,在此表示感谢。

大家可以先看看下面这个视频,对本项目要介绍的内容有个初步了解,视频链接为:https://www.bilibili.com/video/BV1cE411D7Y9?p=18

Coursera 自动驾驶教程:Part2 - State Estimation and Localization for Self-Driving Cars

文章目录

      • 1.IMU简介
      • 2.GPS简介
      • 3.数据融合
        • 3.1 Extend Kalman Filter
        • 3.2 Motion Model
        • 3.3 Measurement Model
        • 3.4 Sensor Fusion

1.IMU简介

惯性测量单元(Inertial Measurement Unit)通常由3个加速度计和3个陀螺仪组合而成,加速度计和陀螺仪安装在互相垂直的测量轴上,这里可以将其输出看作为三个方向的加速度和角速度,表示为:
imu=[axayazwxwywz]imu=\begin{bmatrix}a_x \\ a_y\\ a_z \\ w_x \\ w_y \\ w_z\end{bmatrix}imu=axayazwxwywz
在这里插入图片描述


2.GPS简介

全球定位系统(Global Positioning System)大家应该都不陌生,其输出常见为:经度,维度,和高度,表示为:
gps=[lnglatalt]gps=\begin{bmatrix} lng\\lat\\alt \end{bmatrix}gps=lnglatalt
在这里插入图片描述


3.数据融合

3.1 Extend Kalman Filter

整个流程框架如下图所示,这里需要注意的是IMU与GPS的输出信号频率是不同的,IMU输出频率常见为50-500Hz不等GPS输出频率常见为1-10Hz。因此要分为两部分来讨论:

(1)在只有IMU数据时(此时GPS还未有输出产生),IMU数据经过运动模型,得到预测状态xˇk\check {x}_kxˇk;然后预测状态传送回运动模型,继续下一步预测;
(2)当有GPS数据产生时,上一时刻产生的预测状态将会和接收到的GPS位置信息进行数据融合,得到修正后的状态x^k\hat {x}_kx^k。然后再传回运动模型,进行下一周期的运算。

在这里插入图片描述


3.2 Motion Model

运动模型如下,状态向量为10维状态向量,即x=[px,py,pz,vx,vy,vz,q0,q1,q2,q3]Tx=[p_x,p_y,p_z,v_x,v_y,v_z,q_0,q_1,q_2,q_3]^Tx=[px,py,pz,vx,vy,vz,q0,q1,q2,q3]T,模型可以分为三部分讨论:

(1)位置运动模型,假设载体做匀加速运动,则有:pk=Δtvk−1+Δt22(Cnsfk−1−g)p_k=\Delta{t}v_{k-1}+\frac{\Delta{t}^2}{2}(C_{ns}f_{k-1}-g)pk=Δtvk1+2Δt2(Cnsfk1g),其中fk−1f_{k-1}fk1imu的测量值,CnsC_{ns}Cns为旋转矩阵,用于对imu的测量值进行坐标变换。
(2)速度运动模型,同样假设载体做匀加速运动。
(3)方向运动模型,这里qk−1q_{k-1}qk1表示为四元数,关于四元数的旋转变换可以参考有关资料,这里不做展开了。

在这里插入图片描述
上面的模型不是线性的,课程中将上面的模型进行线性化处理,结果如下,处理后的误差状态向量为9维的状态向量,这里需要关注的是状态转移矩阵Fk−1F_{k-1}Fk1噪声协方差矩阵Lk−1L_{k-1}Lk1
在这里插入图片描述


3.3 Measurement Model

测量模型如下,这里我们需要用到的是GPS的位置数据。
在这里插入图片描述

3.4 Sensor Fusion

介绍完理论部分,下面我们开始一步步实现代码部分。
(1)使用IMU数据进行更新,需要注意旋转矩阵的计算。
在这里插入图片描述

    # 1. Update state with IMU inputsC_ns = Quaternion(*q_est[k-1]).to_mat() #rotational matrixC_ns_dot_f_km = np.dot(C_ns, imu_f.data[k-1])# 1.1 Linearize the motion model and compute Jacobiansp_est[k] = p_est[k-1] + delta_t * v_est[k-1] + (delta_t**2)/2.0 * (C_ns.dot(imu_f.data[k-1]) + g)v_est[k] = v_est[k-1] + delta_t*(C_ns.dot(imu_f.data[k-1]) + g)# Instead of using Omega, we use quaternion multiplication q_est[k] = Quaternion(axis_angle = imu_w.data[k-1] * delta_t).quat_mult_right(q_est[k-1])

(2)状态协方差矩阵的更新
在这里插入图片描述

	# 2. Propagate uncertainty# Global orientation error, over local orientation error# See Sola technical reportF = np.identity(9)F[:3, 3:6] = delta_t * np.identity(3)#F[3:6, 6:] = -(C_ns.dot(skew_symmetric(imu_f.data[k-1].reshape((3,1)))))F[3:6,6:9] = -skew_symmetric(C_ns_dot_f_km) *delta_tQ = np.identity(6)Q[:, :3] *= delta_t**2 * var_imu_fQ[:, -3:] *= delta_t**2 * var_imu_wp_cov[k] = F.dot(p_cov[k-1]).dot(F.T) + l_jac.dot(Q).dot(l_jac.T)  #uncertainty 

(3)计算kalman增益
在这里插入图片描述

    # 3.1 Compute Kalman GainK_k = p_cov_check.dot(h_jac.T).dot(np.linalg.inv(h_jac.dot(p_cov_check).dot(h_jac.T)+np.identity(3)*sensor_var))

(4)计算误差状态
在这里插入图片描述

    # 3.2 Compute error stateerrorState = K_k.dot(y_k - p_check)

(5)误差状态修正
在这里插入图片描述

    # 3.3 Correct predicted statep_hat = p_check + errorState[:3]v_hat = v_check + errorState[3:6]q_hat = Quaternion(euler=errorState[6:]).quat_mult_left(\q_check) # left or right

(6)修正状态协方差矩阵
在这里插入图片描述

    # 3.4 Compute corrected covariancep_cov_hat = (np.identity(9) - K_k.dot(h_jac)).dot(p_cov_check)

到这一步,就完成了整个处理过程,可以看看最终的结果,途中橙色为轨迹真值位置,蓝色为估计的轨迹位置。

在这里插入图片描述
也可以绘制误差分布图,如下图所示,这里使用的3σ3\sigma3σ标准。
在这里插入图片描述


至此,本文要介绍的内容就结束了。基于IMU和GPS的位置定位,关键点在于IMU的运动模型,特别是四元数更新部分,里面牵涉到的变化比较多,需要留心。

如果想深入了解IMU和GPS融合原理,可以看看这篇文章: 重读经典《Quaternion kinematics for the error-state Kalman filter》,这也是Coursera课程关于这一项目的参考文献。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/439674.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【Codeforces - 798C】 Mike and gcd problem(思维,贪心)

题干: Mike has a sequence A  [a1, a2, ..., an] of length n. He considers the sequence B  [b1, b2, ..., bn] beautiful if the gcd of all its elements is bigger than 1, i.e. . Mike wants to change his sequence in order to make it beauti…

一步步编写操作系统 48 加载内核1

其实,我们等了这一刻好久好久,即使我不说,大家也有这样的认识,linux内核是用c 语言写的,咱们肯定也要用c语言。其实...说点伤感情的话,今后的工作只是大部分(99%)都要用c语言来写&am…

Coursera自动驾驶课程第1讲:Welcome to the self-driving cars specialization

本专栏为Coursera自动驾驶课程笔记,B站上也有配套课程视频,对自动驾驶技术感兴趣的朋友可以看看。 本讲对应视频: https://www.bilibili.com/video/BV1WE411D74g?p1https://www.bilibili.com/video/BV1WE411D74g?p2https://www.bilibili.…

【CodeForces - 999D】Equalize the Remainders(思维,贪心)

题干: You are given an array consisting of nn integers a1,a2,…,ana1,a2,…,an, and a positive integer mm. It is guaranteed that mm is a divisor of nn. In a single move, you can choose any position ii between 11 and nn and increase aiai by 11. …

Coursera自动驾驶课程第2讲:The Requirements for Autonomy

上一讲《Coursera自动驾驶课程第1讲:Welcome to the self-driving cars specialization》对本课程进行了整体概述,同时回顾了自动驾驶汽车的发展历史。 从本讲我们开始进入正式的学习,我们将首先了解到如何划分自动驾驶汽车等级、以及自动驾…

一步步编写操作系统 51 加载内核4

咱们的内容都是连栽的,如果您没看过我之前的文章,本节您是看不懂的。 接上节。 介绍完内核初始化的函数kernel_init后,本节代码部分还差一点点没说啦,下面看代码: …略 179 ;在开启分页后,用gdt新的地址…

【CodeForces - 777C】Alyona and Spreadsheet(思维,前缀和)

题干: During the lesson small girl Alyona works with one famous spreadsheet computer program and learns how to edit tables. Now she has a table filled with integers. The table consists of n rows and mcolumns. By ai, j we will denote the integ…

Coursera自动驾驶课程第3讲:Self-Driving Hardware and Software Architectures

在上一讲《Coursera自动驾驶课程第2讲:The Requirements for Autonomy》中我们了解到了如何划分自动驾驶汽车等级、以及自动驾驶三大模块:感知、决策和执行。 本讲我们将学习新的模块:自动驾驶汽车的硬件和软件架构。 B站视频链接&#xff…

一步步编写操作系统 54 CPL和DPL入门1

我们在工作中,公司都给员工配有员工卡,此员工卡就是员工身份的“标签”,用它来出入公司、食堂就餐等。给公司创造价值的是员工的生产力,不是员工卡,员工卡只是公司人事部门管理员工的一种手段而已。 现在说计算机&…

Coursera自动驾驶课程第4讲:Safety Assurance for Autonomous Vehicles

在上一讲《Coursera自动驾驶课程第3讲:Self-Driving Hardware and Software Architectures》中我们了解了自动驾驶汽车常用的传感器和硬件组件、软件系统。 本讲我们将学习新的模块,也是自动驾驶汽车最重要的模块之一:安全模块。 B站视频链…

【Codeforces - 1000C】Covered Points Count(思维,离散化,差分)

题干: You are given nn segments on a coordinate line; each endpoint of every segment has integer coordinates. Some segments can degenerate to points. Segments can intersect with each other, be nested in each other or even coincide. Your task i…

Coursera自动驾驶课程第5讲:Vehicle Dynamic Modeling

在上一讲《Coursera自动驾驶课程第4讲:Safety Assurance for Autonomous Vehicles》中我们了解了自动驾驶汽车中一个非常重要的模块:安全模块。 本讲我们将学习新的模块:汽车运动学和动力学模块。(这部分可能需要一定的理论力学和…

详细解析电源滤波电容的选取与计算

电感的阻抗与频率成正比,电容的阻抗与频率成反比。所以,电感可以阻扼高频通过,电容可以阻扼低频通过。二者适当组合,就可过滤各种频率信号。如在整流电路中,将电容并在负载上或将电感串联在负载上,可滤去交…

【CodeForces - 124C】Prime Permutation(数学,思维,小结论)

题干: You are given a string s, consisting of small Latin letters. Lets denote the length of the string as |s|. The characters in the string are numbered starting from 1. Your task is to find out if it is possible to rearrange characters in st…

Java同步锁——lock与synchronized 的区别【转】

在网上看来很多关于同步锁的博文,记录下来方便以后阅读 一、Lock和synchronized有以下几点不同: 1)Lock是一个接口,而synchronized是Java中的关键字,synchronized是内置的语言实现,synchronized是在JVM层面…

Coursera自动驾驶课程第6讲:Vehicle Longitudinal Control

在上一讲《Coursera自动驾驶课程第5讲:Vehicle Dynamic Modeling》中我们了解了汽车运动学和动力学模块。 本讲我们继续学习新的模块:汽车纵向控制。具体地,我们将学习PID控制算法,看看该算法是如何在自动驾驶汽车中应用的。 B站…

【CodeForces - 599C 】Day at the Beach(思维)

题干: 给定一个数列A,要求你将这个数列划分成几个连续的部分,使得每部分分别从小到大排序后整个数列有序。 问最多可以划分成几个部分。 Input 第一行包含一个整数 n (1 ≤ n ≤ 100 000) — 表示数列的长度 之后一行 n 个整数 hi …

Java并发:线程共享变量可见性原理

0、线程安全性:线程安全性包括两个方面,①可见性。②原子性。 0.1、线程之间的通信:线程的通信是指线程之间以何种机制来交换信息。在命令式编程中,线程之间的通信机制有两种共享内存和消息传递。 (1)在共…

Coursera自动驾驶课程第7讲:Vehicle Lateral Control

在上一讲《Coursera自动驾驶课程第6讲:Vehicle Longitudinal Control》中我们了解了如何使用PID算法进行汽车纵向控制。 本讲我们继续学习新的模块:汽车横向控制。具体地,我们将学习三种控制算法:Pure pursuit,Stanle…

【CodeForces - 697C】Lorenzo Von Matterhorn(二叉树,思维)

题干: 巴尼住在NYC。NYC具有从1开始的正整数编号的无数个交叉点。在交叉点 i 和 2i 之间以及 i和 2i  1 之间存在双向道路,对任意整数 i 都满足。在任意两点之间都有且只有一条最短路。 最初任何人都可以通过任何道路免费。 但是后来发生了 q 个事件。…