龙猫的博客

在无人机的日常问题排查过程中，看日志是必不可少的一个环节，飞机的本地日志是记录最全的信息，结合自己调试飞机经常查看的字段，制作了这个软件，也算是可以给大家一个新的工具选择吧，方便 px4 开发者以及无人机爱好者查看飞行日志。

卡尔曼滤波可以在线性模型，误差为高斯模型的情况下，对目标状态得出很好的估计效果，但如果系统存在非线性的因素，其效果就没有那么好了。其中，f 是非线性状态函数，h 是非线性量测函数，w 与 v 分别是高斯噪声，协方差矩阵为 Q 与 R，在上式中省略了。对于非线性系统的问题，比较常用的方法是将非线性的部分进行线性化处理，其中，EKF 就是对非线性系统进行线性化，即，将非线性函数，在上次估计值 x_hat(k-1) 处进行泰勒展开，其中，结合上面的非线性方程，之后将线性化的泰勒展开式代入上面的非线性方程。

在控制器的前面，再增加一个系数 k，用于调整幅频响应的增益，就可以调整穿越频率到指定的值上，系数 k 的计算原理也比较简单，求出不加 k 时，整个控制器加系统在期望穿越频率下的幅值，记作 g，然后求倒数赋给 k 即可。求解参数时，可以根据自己看重系统的哪些性能来联立方程，例如，给定系统的滤波系数T2，期望的系统截止频率 wn_ref，期望的系统穿越频率 wc_ref，期望的系统相位裕度 PM。证明计算是无误的，有一点点的误差关系不大，matlab画图与计算的精度有一些区别，不完全一样关系不大。

以上的代价函数是有加法的，运用最小二乘法来估计参数比较困难，因此使用 LM 算法来进行非线性估计，具体 LM 算法的原理与实现，参考主页中的上一篇文章《LM迭代估计法》，其与标准正交坐标系有一定的偏移，将各轴的偏移记作 β，之后建立误差模型，建立代价函数，进而求解估计参数。其中，T 为旋转矩阵，W(O) 表示陀螺仪的正交标准轴，与 AOF 重合，W(S) 是当前的陀螺仪轴。此方程中，含有 9 个未知量，三轴的轴偏移，三轴的比例系数，三轴的零偏。，理想的三轴坐标系，加速度计的理想坐标系可以记作 AOF，

其中 y(i) 为第 i 组数据的真实值或理论真实值，fi(θ) 为第 i 组数据的预测值，目的就是找到一组 θ，使得代价函数达到最小值。7.更改估计参数之后，再次使用 n 组采样数据，重复 6 步骤，如果代价函数值变化小于设定的迭代精度，或达到最大迭代次数，则停止迭代，返回跌代后的参数向量 θ。6.迭代更新参数向量，雅可比矩阵(有一些简单的情况雅可比矩阵只与采样值有关，就可以只计算一次)，这里将收敛补偿设置为1，即α = 1。迭代次数为 1000 次，采样 100 组 x，y 数据。

磁力计的数据在实际中是椭球的形状，在此之前使用了球体拟合进行校准，也就是简化为正球体的模型，得出的结果比较差，航向计算不准，还是需要用椭球的模型来估计偏移量，先使用标准的椭球方程，进行化简与变形，得到最小二乘法可以进行估计的标准形式，之后对原始数据进行最小二乘法矩阵的赋值，求解方程，最终观察拟合效果。但使用此方程会有问题，可以求出多个解，使用matlab求解的话，很容易就会求出异常的解，也就是Rx，Ry，Rz很大的时候，也是会满足方程的，但这个解肯定是不对的。实际输出为：y = φT * θ。

在上期，使用一个简单的一维应用实例来加深了卡尔曼滤波的印象后，使用一个二维的例子来看一下卡尔曼的效果。将上式写作状态方程的形式，定义 x1 代表位移，x2 代表速度，x1_dot = x2 (x1的微分就是x2)，将x1，x2合并为一个二维的状态矩阵 x。其中，r 为测量噪声，传感器只能观测位移，不能观测速度。

在实际中，测量噪声 v 的方差是比较容易得到的，因为是传感器的方差，可以预先测量 n 组数据来计算出来，计算出的方差 R 与真实方差也是接近的，但过程噪声方差 Q 是比较难确定的，它是由建模引起的，需要实际判断与调试。推导了卡尔曼滤波的原理之后，使用一个简单的一维应用实例来训练一下，加深印象。对于此一维的系统，矩阵 A = 1，H = 1，B = 0。其中 w 为过程噪声，方差为Q， v 为测量噪声，方差为R。温度计的测量噪声 v 的方差为 R，假设为1.0^2。绿色为测量值，红色为估计值，蓝色为实际值。

1）当估计误差远大于测量误差时，k>>1，此时 x_hat(k) = x_hat(k-1) + z(k) - x_hat(k-1) = z(k)，此时就完全信任测量值。在上一小节中求解出了卡尔曼增益的表达式，但在表达式中的先验误差的协方差矩阵 P-(k) 的未知的，因此接下来就需要求解先验误差的协方差矩阵 P-(k)。结合上面的两个公式，对 w，v 来说，期望值都为0 ，也就是 E(w) = 0，E(v) = 0，则。其中 e(k) 表示估计误差，x(k) 表示真实值，x_hat(k) 表示后验估计值。

在云台中，陀螺仪的角速度是短期可靠的，加速度计容易受到振动的影响，在长期是可靠的，互补滤波就是给陀螺仪积分得到的角度与加速度计得到的角度分别给权重，利用测量值(加速度计测量得到的角度)校正预测值(陀螺仪积分得到的角度)。陀螺仪直接测量到的角速度是机体角速度，在转换为欧拉角速度后，乘以时间间隔，再加上上次的估计值，就得到一个陀螺仪积分计算的角度，作为预测值。俄版云台也是同样，在采集了多轴旋转的数据之后，通过最小二乘法拟合出数据形成的球体，计算球心，就是磁力计校准需要记录的数据。

上面的图是单相的 SPWM 信号，电机中的三相线一般是用 U，V，W 或 A，B，C 来表示。最近想学习一下云台，发现资料确实还不太好找，比较有参考价值的是俄版的开源版本的云台代码，后面就不开源了，开源版本的是比较原始的算法，差不多是玩具级别的，不过还是决定学习一下，了解一下基础。云台的功能是要通过调整电机的机械角度来达到相应的姿态角，如果说通过校准，得到电角度与机械角度的偏移量，或者电角度与姿态角度的偏移量，会非常的麻烦，而且容易出错，的方式来控制三相无刷电机，并且为节省成本，直接使用的。

Dubins路径

vfh局部路径规划绕障

自校正PID是自校正控制的一种，其核心思想是使用在线辨识来辨识系统参数的变化，之后使用极点配置的思想来计算PID参数，达到自动调节的目的。

上节介绍了批处理最小二乘法，它比较适合数据的预处理，也即离线辨识，在实时性方面就不如递推最小二乘法，递推最小二乘法也就适合在线辨识。

在之前使用通用的A*算法来规划出路径之后，可以发现路径上其实是有许多的冗余点的，如下图所示：上图中可以明显看出有一些折弯点是冗余的，因此本节内容就介绍使用垂距限值法来优化冗余点。垂距限值法的思想也是比较简单的，概括一下就是：计算当前节点到上节点与下节点连线的距离，超过一定的阈值就从总集合中删除当前点。总体思想就如它的名字一样。这里我写成了一个函数Vertical_distance_limit：正在上传…重新上传取消当然，此函数中我又封装了一个函数，即计算垂距的函数compute_vertical_dista

在上篇增加了一个运动对象之后，就可以考虑给这个运动对象增加动力模型，之后按照动力模型来进行轨迹规划。这个动力模型我的思路是参照开源飞控中使用向量法指定速度方向，使用S曲线来规划速度轨迹的方式，最终做出来的效果看起来还是可以的。这里可以补习一下开源飞控中的速度S曲线规划的知识，总体思想为：保证加速度，速度连续，以固定加加速度来改变速度。总体的效果就是：先以恒加加速度将加速度a加到最大加速度a_max，时间为T1，之后以恒加速度运行T2时间，之后以恒加加速度将加速度从a_max减到0。这里PIX4的源码中也给出

在上一篇的路径规划文章中，使用多段五阶贝塞尔曲线优化RRT搜索后的路径，虽然视频上看起来还好，但还是不够好玩，为了让其有趣一些，突然想增加一个运动对象(例如一个三角形)，在生成平滑曲线后，沿着平滑曲线运动，这个是本期想要实现的目标，在实现之后，后面可以考虑再增加一些运动模型来使得这个运动对象符合生活中的一些运动对象。首先我们使用一个三角形来表示一个运动对象的话，需要找到一个画三角形的方法，这个需要借助 matlab 的 fill 函数，这个函数的作用大致是这样：给出几个点的横纵坐标，然后使用 fill 函数

在上一篇关于贝塞尔曲线平滑路径的文章中，只是大致的介绍了一下贝塞尔曲线的原理，并且初步测试了一下，最终平滑的效果，还是不尽人意的，最开始是使用了简易的多段二阶贝塞尔，得到的曲线有很多的尖锐：然后使用了全局的贝塞尔，也就是所有的点全一次用上，得到一个N阶贝塞尔，但是这样就会有一个缺点，那就是当转弯很多的时候，它的效果就并不那么好了，就像这样：有时就会出现上述的情况，比较容易压到障碍，并且与原路径差别较大。那么在实际的贝塞尔优化路径的使用中，经常使用的方法可以直接告诉大家：使用多段五阶贝塞尔 + 插辅助点的方法