木牛的博客

规划（planning）承接，并下启车辆控制。其规划出来的轨迹是带速度信息的路径。广义上，规划（planning）可分为。：是全局路径规划，可简单的理解为传统地图导航+高精地图（包含车道信息和交通规则等）；:决策车辆是否跟车、在遇到交通灯和行人时的等待避让、以及路口和其他车辆的交互通过；：是局部路径规划，是在未来一段时间内的期望行驶路径，需满足汽车运动学、动力学、舒适性和无碰撞等要求。的任务是计算出一个无碰撞可执行的轨迹（包含路径和速度信息），保证车辆从起点安全的驾驶到目的地，并尽可能高效。

这种投影算法使得赤道附近的纬线较密，极地附近的纬线较稀。Google Maps的选取的范围为 -π<y<π ，这样近似的有 -85°<Φ<85°。，所以选和墨卡托投影方式类似的米勒投影。即假设有一个和赤道垂直的圆柱套在地球上，然后在地心点亮一盏灯，灯光将地球各个点投影在圆柱上，在把圆柱展开，就得到地球的平面投影。墨卡托投影把纬度为Φ (-90°<Φ<90°)的点投影到：y = sign(Φ)*ln(tan(45° + abs(Φ/2))经过墨卡托投影后的经线是均匀分布，在此主要介绍纬度的变换方法。

22年暑假实习过程中第一个开发任务，基于实现三边定位功能开发平台为ubuntu 18.04 + ros melodic日期：2022.7.27本次实现：在能对拟定圆心数据进行保存和读取之后，本次将用上一次保存的数据作为目标数据，再将激光雷达换位置后的当前数据与其进行匹配，从当前的五个反光柱中找出需要的三个目标反光柱。匹配的原理是将预先录入系统的反光柱之间的边长一一去和当前能识别到的反光柱之间的边长匹配，识别出目标反光柱。并返回目标反光柱的坐标以便后期进行三边实现。

笔者认为整个无人车广义决策规划控制（Decision,Planning & Control）范畴下的路由寻径（Routing），行为决策（Behavior Decision），动作规划（Motion Planning）以及反馈控制（Feedback Control）等几大模块，在当前的学术和工业界都有一些较为成熟的解决方案可以借鉴。这些解决方案有些有着牢固的理论基础和数学推导，还有的在实际的无人车相关比赛中有着的出色发挥。事实上，我们认为单独看每个层面需要解决的问题，都不是非常困难。

当传感器的姿态不同时，它在自身各个坐标轴检测到的重力加速度是不一样的，利用各方向的测量结果，根据力的分解原理，可求出各个坐标轴与重力之间的夹角。使用GPS可以直接检测出载体在地球上的坐标，假如载体在某时刻测得 坐标为A，另一时刻测得坐标为B，利用两个坐标即可求出它的航向，即可以确定 偏航角，且不受磁场的影响，但这种检测方式只有当载体产生大范围位移的时候 才有效(GPS民用精度大概为10米级)。2） 地磁传感器: 利用了地球南北极的磁场所产生的吸力，并投影到X，Y，Z轴上，测量出物体的方位和姿态。

mpu6050电磁干扰怎么解决 - 优快云文库

最近接手了一个项目，光通信方面的，我负责编写初测结果的数据处理算法，其中有一个算法，叫做 unwrap 与 wrap，之前没有听说过。通过询问同事与搜索引擎，原来，所谓的 unwrap 是将在 +180 度到 -180 度之间跳变的相位信号，也就是函数值，变为不跳变的形态。而 wrap 则是其逆运算，即将信号恢复成跳变状态。这样概括实在太模糊，我们用数据与图象来详细说明。

在融合定位实验中偶然发现IMU的角度存在跳变现象，本文主要记录问题分析和解决方法。最终通过上述方法，转换后的yaw角再无跳变现象，因此工程问题得以解决。如果单从问题本身来讲，对四元数的理解仍然不够深刻，后续需要进一步研究。IMU角度跳变问题_欧拉角突然改变-优快云博客。

首先从问题说起，因为我最近在和陀螺仪厂商讨论陀螺仪对我们真正使用的影响。所以我先从陀螺仪的指标说起，基本知识会放在后面。首先介绍一下零偏的的概念。

有很多困惑。有没有哪个大神赐教一下？对比的IMU：icm42688P, MPU6050,adis16465icm42688的全温度零偏误差。

大致的思路是将模块的姿态用四元素表示，作为系统的状态量，模块的姿态运动学方程作为滤波的状态转移方程，加速度信息作为滤波的观察量信息，然后利用卡尔曼滤波的计算方法迭代计算更新，详细的过程可以参考惯性导航方面的书籍。因为三轴是有耦合的，只有在三轴角度为小角度的时候可以这么算，角度大了以后，比如60度了，这么算的误差就很大。N(a，ω，T，t）表示加速度，角速度，温度和时间等总的因素造成的影响（G 的依赖性（加速度影响），对于mems陀螺仪来说，有可能受到重力的影响，可以通过建模并采用一定的方法去除这个影响）

同样的，我们也可以根据自己的需要，确定一个时间常数，进而获得合适的滤波系数，为了获取更加准确的值需要对系数进行一定的调整。角速度数据，加速度积分得到角度，但是由于传感器误差，积分作用会造成累计误差，所以角速度数据在短期是可以使用的，长期来看误差会很大是不可使用的。即，高频信号与低频信号都与我们将求得的量相关，我们将其进行滤波运算，给两部分一个权重得到一个相对准确的值，或者得到一个很好的近似的过程。加速度数据，不是很准确，但是趋势是对的，通过受力分能够显示角度变化趋势，在长期变化来看是可以利用的。

可以利用官方的dmp来解算姿态，但是代码移植是个问题，我用的是恩智浦的RT1064，手头只有MSP430和STM32的dmp库，在采用卡尔曼滤波算法发现收敛太慢的情况下，我决定采用经典的一阶互补滤波算法和简单的自适应一阶互补滤波算法来试试看，也从数学原理到代码彻底的把一阶互补滤波原理捋一遍。假设我们需要测定一个物理量，这个物理量可以通过两个传感器测得，而且其中一个传感器的测量噪声主要集中在高频段，另一个传感器的噪声主要集中在低频段，那么我们可以构造出两个滤波器，其中一个是低通滤波器，另一个是高通滤波器。

最近项目上想用MPU6050来自动探测物体的转向角度，花了2天时间学习如何拿陀螺仪的姿态角度，发现蛮难的，写点笔记。下面是哔哩哔哩的一堆废话讲解，只想看代码本体的可以直接跳到最后。应用场景是51单片机环境，有一块MPU6060，需要知道硬件板子水平摆放时，板子摆放的姿态和旋转的角度。编译环境只能用C语言。首先单片机通过TTL串口接到MPU6050上拿到通信数据，水平旋转角度需要另外加地磁仪通过南北极磁性拿到。很遗憾设计硬件时没注意这茬，只用了一块MPU6050。

在进行零偏数据的计算时，需要将200个原始数据相加，但是一开始我的总和定义的还是一个int型的数据，这时200个数据的和可能已经超过了int型数据的取值范围，所以产生了溢出。另外，当我们程序中总变量的字节数超过128字节时，必须对程序中所有变量进行初始化，否则没有被初始化的变量默认值不在是0，而是不确定的随机数。不管怎么修改量程，输出的波形震动范围总是在+-20000波动，但是当我把加速度计的量程设置为+-16G的时候，理论上其LSB应该为2048左右，但是发现输出的波形也是震动范围很大。

首先是数据采集,我让小车保持不动,也就是yaw一直为0的状态下,测试传感器实际输出的yaw的漂移情况.我利用串口对mpu6050的yaw角进行了采集.100ms一次,采集了6000个数据样本,耗时是10分钟.然后利用matlibplot对数据进行了图形化.图形的x轴就是时间T,y轴就是传感器输出的角度值direct,也就是航片角yaw.经过10分钟的运行,可以发现,我所使用的传感器的零漂其实是比较严重的.在温度稳定在30°C左右的条件下,就已经达到2°左右.这是非常糟糕的.

newangle：角速度计得到的角度值。* angleout：上次角度输出值。* newgyro：陀螺仪的角速度值。* tau：反应惯性滤波的系数。* dt：两次采样时间间隔。* 互补滤波计算得到的角度值。

加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号，例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；用的都是16位的ADC，所以显示的数字是从-32768——+32768，要看自己选择的量程进行换算，量程选择在配置里面找，比如选择-250——+250量程，那么-32768——+32768就对应-250——+250 进行换算就可以了。就是，短时间内用陀螺仪比较准确，以它为主；

陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。通过加速度计得到 Δ q a c c ^ \widehat{\Delta q_{acc}} Δqacc​​ ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。在 px4 中，磁力计使用 GPS 信息 [ 0 , 0 , m a g ] [0, 0, mag] [0,0,mag] 进行校准，故，公式与加速度计相同。在预测环节得到的四元数 q e b ( k ) q^b_e(k) qeb​(k) ，通过加速度计和磁力计的值进行校正。

陀螺仪一般为六轴或者九轴构成，六轴就是三轴角速度计加上三轴加速度计，九轴就是在六轴的情况下再加上一个三轴的磁力计，而应用最广泛的MPU6050就是一个六轴陀螺仪。有一种现象很普遍也包括我自己，就是在没有用过陀螺仪之前，总以为陀螺仪可以直接得到角度数据，实际上并不然，陀螺仪（在下文中就代表MPU6050等六轴陀螺仪）只能得到角速度数据和角速度数据，从而间接得到角度。可能数理基础不错的小伙伴会想到，对角速度积分不就成了角度吗？

同样的，跟前面一眼四元数的乘法也代表了旋转，四元数的积有多种定义，这里我们只讲被用在旋转操作的上积 —— 矢量积。

我们总结一下在库文件中都做了那些修改：把#include "stm32f4xx.h"修改为 #include "stm32f10x.h"1删除 #include "main.h" 和 #include "board-st_discovery.h" 头文件声明1修改 #include "uart.h" 为自己写的 #include "USART1.h"1修改 #include "i2c.h" 为 自己写的I2C头文件 #include "SI2C1.h"1。

还记得上一节中我们怎么旋转一个二维向量的吗？我们将其放入一个复平面中，通过乘子来旋转其所在的坐标系来得到结果是的，跟2*3的例子一样，将该系下(1+0i)的位置变换至乘子r的位置，同时为了确保不会变形，我们限定乘子的模长为1（或者叫范数?随你啦），整个旋转看起来就像在圆的边上滑动。四元数，是如何推导的。

由于每种滤波器的参数都会极大地影响该滤波器的性能（一阶滤波、二阶滤波各一个参数，卡尔曼滤波三个参数），因此难以互相比较，我建议根据单片机的资源、性能选择要用的滤波器，R参数:观测噪声协方差 R参数调整滤波后的曲线与实测曲线的相近程度，R越小越接近(收敛越快)我参考的大佬有取0.01，0.0003，0.01的，也有取0.001，0.005，0.5的。生成的程序较大，对于89C52，需要注释掉没用到的函数。（我参考的大佬有取0.8的，有取10的，我这里取5）。上位机：Vofa+ 1.3.10。

这是由于每个芯片在制作时都不一样，数据手册上的都是理论的 值，真正的芯片在水平时Z轴可能并不是16384.我们需要找到当各个轴在0g重力时的计数， 1g时的读数，以及-1g时的读数，得到一个补偿值，在每次读取ADC结果后都进行补偿。接收时，Ry，R Z与上的X，Y，Z轴的R矢量投影。目前市面上的加速度计从输出上区分为两种，一种是数字的，另一种是模拟 的.miniAHRS 使用的是MPU6050三轴加速度计，是I2C接口的数字传感器.通过特定的命令 可以配置加速度的量程,并将内部ADC的转换结果读出来.

四维空间（十一）：几何代数、四元数与空间旋转 | Blog de Hqak (WXYHLY)

早些年在鹅厂实习了一段时间，因为没有什么特别紧急的需求(hahahahaha)，所以主要花在了学习和捣鼓一些小工具上。有一个小需求是要实现鼠标拖动球体的转动，然后发现我不再能只用欧拉角来糊弄过去了。然后又发现，网上大部分资料的采用的欧拉角顺规都是xyz，然后我基于D3D11的辣鸡框架用了zxy，公式不太能直接套用，于是摸了两三天鱼，整理了一下几种三维旋转表示（欧拉角，四元数，旋转矩阵，轴角）与他们之间的相互转换的资料，并且加入了自己的一些推导，

与复数类似，因为四元数其实就是对于基 {1, 𝑖, 𝑗, 𝑘} 的线性组合，四元数也可以写成向量的形式：除此之外，我们经常将四元数的实部与虚部分开，并用一个三维的向量来表示虚部，将它表示为标量和向量的有序对形式：我们这里可能还感觉到迷茫，没关系，我们一步一步来！

四元数与欧拉角之间的转换百度百科四元素在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。本文主要归纳了两种表达方式的转换，计算公式采用3D笛卡尔坐标系：定义ψ�，θ�，ϕ�分别为绕Z轴、Y轴、X轴的旋转角度，如果用Tait-Bryan angle表示，分别为Yaw、Pitch、Roll。通过旋转轴和绕该轴旋转的角度可以构造一个四元数：其中α是一个简单的旋转角（旋转角的弧度值），而是定位旋转轴的“方向余弦”（欧拉旋转定理）。

可使用 View -> Message -> RTCM3中查看RTCM协议接受情况，当1005-1230全部为黑色（而非灰色）时，表明该基站已收敛，可用于流动站RTK差分。使用该模块时，如果需要GCJ02火星坐标系，则需要使用上述模块，将WGS 84坐标转换为GCJ02坐标。将USB-TTL模块与ZED-F9P连接，最好是接入J1接口，即右侧接口。打开u-center软件，配置端口和波特率，连接ZED-F9P模块。打开u-center软件，配置端口和波特率，连接ZED-F9P模块。

目录这一章主要介绍GPS信号结构和其播发的导航电文，内容丰富，对之后的学习较为重要，大家可以多看看书，多回顾回顾。笔者这次贴出了部分仿真代码（可能比较拙劣），也许可以帮助大家理解。这一章会涉及一些通信原理、信息论的知识，还会涉及一些概率论、随机信号的知识，所以学习之前复习复习，效果会比较好。

全球定位系统(GPS)是“授时、测距导航系统／全球定位系统(Navigation systerm Timing and Ranging／Global Positioning System)的简称。该系统是由美国国防部于1973年组织研制，主要为军事导航与定位服务的系统。历经20年，耗资300亿美元，于1993年建设成功。GPS是利用卫星发射的无线电信号进行导航定位，具有全球性、全天候、高精度、快速实时三维导航、定位、测速和授时功能，以及良好的保密性和抗干扰性。

过去，如果你的女友是个路痴，大概会有这样的对话……——你在哪儿呢？——啊？我在马路上啊。——有什么特征？——头顶有个月亮。——你旁边有什么啊？——有个路灯。——有没有路牌啊？路牌上写的什么？——我看看啊。还真的有，上边写着“禁止停车 违者罚款”。——姑奶奶，我真是服了你了……——哼，你是不是不爱我了，你肯定是不爱我了，你是不是喜欢上了新来的那个前台？——。。。——我跟你说那个前台绝对是个伪娘！（开启八卦模式）还有还有，小王买了个新口红真好看，你也给我买好不好……

而且，距离远了，超过了无线电台的通信范围，也就无法工作了。例如卫星信号穿透电离层和对流层时产生的误差，还有卫星高速移动产生的多普勒效应引起的误差，以及多径效应误差、通道误差、卫星钟误差、星历误差、内部噪声误差，等等。接下来，要给大家介绍一个可能比较陌生的概念：它也和卫星有关，是目前行业最常用的定位技术之一，为我们的工作和生活提供了很大的帮助，它就是——流动站在基准站数据和自身数据的基础上，根据相对定位原理，进行实时差分运算，从而解算出流动站的三维坐标及其精度，其定位精度可达1cm~2cm。

我们知道原有的GPS定位系统，接收器在接受卫星发射的信号时，由于各种原因（比如距离原因，电离层和对流层原因、时钟原因等）导致定位误差非常大，为了消除这种误差，我们首先在地面已知位置安装一个基准站，该基准站在接收到卫星信号后就会开始进行自我解算，将卫星传递的位置信号与自己的实际位置信息进行比较，找到传递过程的定位误差，然后通过技术手段将误差告之流动站，然后流动站在根据实时接收到的卫星信号，对误差进行修正，由此得到自身的。RTK的组成包括基准站和流动站。显然是有的，这就是接下来要介绍的RTK技术。

 对激光定位的原理作了初步的了解，主流的是三角定位和三边定位，其中三角定位原理类似GPS；但是对于激光定位而言，更多适用于室内，并且要在环境中布置安装一定数量反射板，同时需要注意反射板安装的精度，相隔距离，安装时避开窗户，非对称布置等要素；定位分为初始的静态定位和运动过程中的动态定位，静态定位用于确定初始位置，动态定位则根据运动状态不断更新；    在激光器扫描一周的过程中，理论上可以计算得到激...