有关自平衡车姿态测量的一些总结，大家看看能不能用得上
我从去年开始一直在做载人的电动独轮车，去年做姿态测量部分时找了些资料，自己也发挥了一些，写了点关于自平衡车姿态测量的总结报告。一直放在电脑里，现在逛论坛时发现好多同学在找这一方面的资料，同时我觉着这段时间在阿莫论坛里学到了不少东西，自己也应该贡献一份力量，把东西传上来，大家放开点评，我也是刚开始涉猎这一方面，如果大家发现文档里有错误，请指出来让我知道，我好改正。（文档里涉及的传感器融合算法融合陀螺仪和加速度计的输出值，可以得出俯仰和横滚两个姿态角，既可以用于自平衡车的姿态部分，也可以用于微型飞行器的两个姿态角。） 赞一下分享精神！ 写的真心不错 谢谢楼主分享，下面的PID部分呢？可以给大家看看不？ 看不到，是不是我等级不够啊？ 楼主是个善于总结的人，赞一个。独轮车做得怎样了？ 文档非常详实
赞楼主一个 支持LZ无私的分享！ chenjiawei7 发表于 2012-12-14 09:33 static/image/common/back.gif
写的真心不错 谢谢楼主分享，下面的PID部分呢？可以给大家看看不？
其实我知道我的算法也是错误的，真正的姿态解算应该用四元数之类的，但我的那个算法对于自平衡车的精度已经够用了。PID部分其实细细的弄起来也是有一些东西的，写好了会公布的。 Eastnorth 发表于 2012-12-14 10:14 static/image/common/back.gif
楼主是个善于总结的人，赞一个。
独轮车做得怎样了？
现在智能小车里用的卡尔曼滤波算法基本是这个：
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt;
Pdot=Q_angle - P - P;
Pdot=- P;
Pdot=- P;
Pdot=Q_gyro;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P;
PCt_1 = C_0 * P;
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P;
P -= K_0 * t_0;
P -= K_0 * t_1;
P -= K_1 * t_0;
P -= K_1 * t_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;
}
这个也不错，但我看不懂，因为我不知道怎么调参数，所以我不相信其输出精度。
独轮车做的不太好，原来一直在用别人的驱动，后来感觉还是自己做一下比较好，所以还在做驱动。 hitor 发表于 2012-12-14 12:04 static/image/common/back.gif
现在智能小车里用的卡尔曼滤波算法基本是这个：
void Kalman_Filter(float angle_m,float gyro_m) //gy ...
这个参数不好调节，就目前的参数而言，滞后严重。但是加大参数了，输出的角速度值收敛太慢，影响控制。
还是楼主的公式也不是最直观的，建议直接用公式，那5个公式一步步的写，输出很精准，参数的调节也一目了然。
就是现在的几个程序。把卡尔曼的算法弄复杂了 卡尔曼滤波器的增益是不会随着测量值的变化而变的，在收敛之后就是个固定值(加入这个固定值是最优的)了，和互补没啥两样。
有人又说，那在收敛之前呢？
收敛之前更惨，这个增益值一直不是最优值，这滤波效果能好吗？ 再发一些我认为不错的文档吧！
首先是一阶互补滤波算法：
1、一阶互补滤波
这种滤波的原理是对AngleGyro进行高通滤波，而对AngleAccel进行低通滤波，最后再将两者加合，输出姿态角估计值。其公式如下：
angle = a * (angle + AngleRateGyro * dt) + (1 - a) * AngleAccel;
高通滤波的目的是过滤掉长期的变量，而让短期的变量通过，这可以用来去掉陀螺仪的漂移；低通滤波的目的是过滤掉短期的变量，而让长期的变量通过，这可以用来去掉加速度计的噪声。为了确定滤波系数a，得确定一个时间常数tau，
tau = a * dt / (1 - a);
a = tau / (tau + dt);
dt为滤波取样时间。tau代表的是你对陀螺仪的信任度和对加速度计的信任度的界限。
当时间小于tau，AngleGyro处于优先地位，AngleAccel的噪声将被过滤。当时间大于tau时，AngleAccel的平均值将被优先考虑，这时的AngleGyro将产生漂移。tau的选择将取决于AngleGyro何时产生漂移，观察数据拟合曲线即可确定。我们选取tau = 1s，带入以下程序中：
void ComplementaryFilter(float AngleRateGyro,float AngleAccel)
{
//tau is a time constant
//a = tau / (tau + dt);
//angle = a * (angle + AngleRateGyro * dt) + (1 - a) * AngleAccel
float tau = 1.0;
float a = 0.0;
a = tau / (tau + dt);
angle = a * (angle + AngleRateGyro * dt) + (1 - a) * AngleAccel;
}
一阶互补滤波的资料来源于MIT的一篇文章：
《The Balance Filter》，Shane Colton <scolton@mit.edu>
这篇文章里所提到的算法是最简单也是最简洁最实用的算法，个人大力推荐，但在参数a的选择上要根据实际加以调整，方能得到好的滤波效果。具体实验效果在一开始发的PDF文档里有介绍，大家最好自己买IMU来亲自实验一下，比较不同的滤波算法，这样体会更深。
附上《The Balance Filter》，Shane Colton <scolton@mit.edu>
hitor 发表于 2012-12-14 13:04 static/image/common/back.gif
再发一些我认为不错的文档吧！
首先是一阶互补滤波算法：
1、一阶互补滤波
然后是二阶互补滤波：
这种滤波的原理我不太看得懂，好像是对（AngelAccel - angle）进行PI控制之类的，然后再与AngleRateGyro相加，作为新的AngleRate再进行积分得到角度。程序如下：
void ComplementaryFilter2(float AngleRateGyro,float AngleAccel)
{
//the second complementary filter
float k = 10;
float y1 = 0;
float y2 = 0;
y1 = y1 + (AngleAccel - angle) * k * k * dt;
y2 = (AngleAccel - angle) * 2 * k;
AngleRate = AngleRateGyro + y1 + y2;
angle = angle + AngleRate * dt;
}
我把程序原理图贴出来，请大家看看能不能把原理弄清楚。
我用IMU也做过实验，结果在一楼所发的文档里。
附上二阶互补滤波算法的原始文档
kmani 发表于 2012-12-14 12:27 static/image/common/back.gif
卡尔曼滤波器的增益是不会随着测量值的变化而变的，在收敛之后就是个固定值(加入这个固定值是最优的)了，和 ...
就我观察，互补滤波的效果，对于持续颠簸的路面会出现比较大的抖动。
楼上的是不是说反了？卡尔曼的增益Kg是会变化的，是随着测量误差而变化的。收敛是指对角速度进行卡尔曼滤波后输出的角速度的收敛而言的，我现在不对角速度进行卡尔曼滤波，完全没必要，只需要做好低通滤波和零点漂移就好了，能增强车子的灵敏度。 接下来是我把别人写的一个卡尔曼滤波算法程序移植过来，我以前写立项报告时了解过一些相关知识，知道卡尔曼滤波能用来融合传感器数据，那我们就试试其实际效果如何。刚开始我嫌麻烦，把别人的一个用卡尔曼滤波控制温度的程序移植到我的独轮车模型上，输入AngleAccel和AngleRateGyro，以姿态角作为估计值，调了调参数，就运行程序，拟合成曲线后效果还不错。但精度太低，而且我又看不懂程序，所以只能作罢。其程序如下：
void KalmanFilter(float AngleRateGyro,float AngleAccel)
{
float gyroVar = 0.4;
float deltaGyroVar = 0.1;
float accelVar = 2;
float Pxx = 0.1; // angle variance
float Pvv = 0.1; // angle rate variance
float Pxv = 0.1; // angle and angle rate covariance
float Kx = 0;
float Kv = 0;
angle = angle + AngleRateGyro * dt;
Pxx = Pxx + (2 * Pxv + dt * Pvv) * dt;
Pxv = Pxv + Pvv * dt;
Pxx += dt * gyroVar;
Pvv += dt * deltaGyroVar;
Kx = Pxx * (1 / (Pxx + accelVar));
Kv = Pxv * (1 / (Pxx + accelVar));
angle += (AngleAccel - angle) * Kx;
Pxx *= (1 - Kx);
Pxv *= (1 - Kx);
Pvv -= Kv * Pxv;
}
不知论坛里的大牛们能否把这个滤波器研究一下，讲一讲原理和参数调整。 大家干脆来研究一下自平衡车的倾角测量算法吧！之前看到大家做自平衡小车一直在用这个算法，我没看懂原理，大家集思广益来说一下这个算法的原理和参数调整方法。算法之前贴过，再贴一次
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt;
Pdot=Q_angle - P - P;
Pdot=- P;
Pdot=- P;
Pdot=Q_gyro;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P;
PCt_1 = C_0 * P;
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P;
P -= K_0 * t_0;
P -= K_0 * t_1;
P -= K_1 * t_0;
P -= K_1 * t_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;
}
这个也不错，但我看不懂，因为我不知道怎么调参数，所以我不相信其输出精度。
hitor 发表于 2012-12-14 13:21 static/image/common/back.gif
大家干脆来研究一下自平衡车的倾角测量算法吧！之前看到大家做自平衡小车一直在用这个算法，我没看懂原理， ...
我也看不懂，数学比较差啊，所以就不用这个了 zhanglu1990 发表于 2012-12-14 13:23 static/image/common/back.gif
我也看不懂，数学比较差啊，所以就不用这个了
呵呵，没事，我也是初学，大家试试看嘛 还有一个我认为是正确的算法，就是用了四元数来解算姿态角，程序贴在下面，相信很多同学应该看过，我只是集合一下。//=====================================================================================================
// IMU.c
// S.O.H. Madgwick
// 25th September 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' .
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.See my report for an overview of the use of quaternions in this application.
//
// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
// and accelerometer ('ax', 'ay', 'ay') data.Gyroscope units are radians/second, accelerometer
// units are irrelevant as the vector is normalised.
//
//=====================================================================================================
//----------------------------------------------------------------------------------------------------
// Header files
#include "IMU.h"
#include <math.h>
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.5f // half the sample period
//---------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
//====================================================================================================
// Function
//====================================================================================================
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
float norm;
float vx, vy, vz;
float ex, ey, ez;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// error is sum of cross product between reference direction of field and direction measured by sensor
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
//====================================================================================================
// END OF CODE
//====================================================================================================
大家可以参考一下，顺便附上作者的论文
q_bias是啥 就这么多了，我觉得这些资料虽不是那么完善，但对于解决智能小车论坛里的自平衡车倾角测量问题，我觉得已经够用了，但是对于微小飞行器的姿态测量，这些资料还不够，算法也不正确，请大家慎重选择，希望对大家有所帮助。 number007cool 发表于 2012-12-14 13:46 static/image/common/back.gif
q_bias是啥
是角速度的偏差 zhanglu1990 发表于 2012-12-14 13:17 static/image/common/back.gif
就我观察，互补滤波的效果，对于持续颠簸的路面会出现比较大的抖动。
楼上的是不是说反了？卡尔曼的增益K ...
网上有很多卡尔曼的资料，你多看看，多想想，多算算... kmani 发表于 2012-12-14 21:42 static/image/common/back.gif
网上有很多卡尔曼的资料，你多看看，多想想，多算算...
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
P(k|k-1)这个值是预估计协方差吧，难道不是变化的？你能得出卡尔曼的增益是不变的？
大侠请指教 zhanglu1990 发表于 2012-12-14 21:54 static/image/common/back.gif
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
P(k|k-1)这个值是预估计协方差吧，难道不是变化的？你能 ...
大侠不敢当。只是最近看卡尔曼挺火的，就看了。
你看这个公式里面哪一个值受测量值影响了?
（测量值是加速度计算的角度和陀螺仪积分的角度或者加速度计和陀螺仪的原始测量值） kmani 发表于 2012-12-14 22:07 static/image/common/back.gif
大侠不敢当。只是最近看卡尔曼挺火的，就看了。
你看这个公式里面哪一个值受测量值影响了?
（测量值是加 ...
P(k|k-1)=A P(k-1|k-1) A’+Q 预计协方差不变？？ zhanglu1990 发表于 2012-12-14 22:11 static/image/common/back.gif
P(k|k-1)=A P(k-1|k-1) A’+Q 预计协方差不变？？
你的这个公式在收敛时确实在变，一旦收敛了，基本不变了。
还有，你把我上一次说的，再认真看几遍！ kmani 发表于 2012-12-14 22:19 static/image/common/back.gif
你的这个公式在收敛时确实在变，一旦收敛了，基本不变了。
还有，你把我上一次说的，再认真看几遍！ ...
哈哈，卡尔曼滤波就是一种根据预测值和测量值不断修改增益的自回归滤波器，不要说什么让我去看这看到了，卡尔曼就那5个公式，很容易就能看懂
一般由于预定不确定度很大，卡尔曼增益的变化极小，基本可以认为是常数，互补滤波可以看作是固定增益的一种滤波器。
请你指出我哪里说错了 zhanglu1990 发表于 2012-12-14 22:25 static/image/common/back.gif
哈哈，卡尔曼滤波就是一种根据预测值和测量值不断修改增益的自回归滤波器，不要说什么让我去看这看到了， ...
既然你说懂了，那就算了。 kmani 发表于 2012-12-14 22:31 static/image/common/back.gif
既然你说懂了，那就算了。
我是说那几个公式很简单，我能看懂，除此外，你继续指教！ LZ你好，我的电机PWM=150的时候才刚刚开始转，要到200的时候才比较有力，参数怎么都调不好啊，怎么弄啊 chenjiawei7 发表于 2012-12-15 17:06 static/image/common/back.gif
LZ你好，我的电机PWM=150的时候才刚刚开始转，要到200的时候才比较有力，参数怎么都调不好啊，怎么弄啊 ...
PWM满量程是-255到255吗？还是0-512这种类型。要么是你的电机驱动器没设置好，要么是你的电机有问题。你具体说一下，电机、驱动器、还有你的控制算法 hijxyz_k999 发表于 2012-12-15 12:04 static/image/common/back.gif
学习学习学习，希望楼主有很多的资料分享，我想做一个平衡车作为我的毕业设计！ ...
呵呵，这个论坛里满天飞了，基本是zlstone的版本或改进版，你找找看 hitor 发表于 2012-12-15 17:38 static/image/common/back.gif
PWM满量程是-255到255吗？还是0-512这种类型。要么是你的电机驱动器没设置好，要么是你的电机有问题。你 ...
用的12Ｖ锂电池，电机说是用１２Ｖ的，PWM的范围是0~255，电机在PWM=150的时候还是不转的，到２００的时候才开始有点力气，我用的298驱动，驱动是自己用热转印做的单面板，没有隔离，298上的续流二极管用的1N5819，刚刚量了一下，电机直接加5V电压转的没有力气，电机内阻用万用表量的10欧姆左右，电机就是这样的 chenjiawei7 发表于 2012-12-15 17:52 static/image/common/back.gif
用的12Ｖ锂电池，电机说是用１２Ｖ的，PWM的范围是0~255，电机在PWM=150的时候还是不转的，到２００的时 ...
这个你先问一问电机卖家，看看这个电机是不是出问题了，还有再问一问卖家有没有这个电机的驱动要怎么做？我估计可能是你驱动板的问题，你可以买一个专用的驱动板试试 {:lol:}很好 非常好 楼主的东西太好了，感激不尽 楼主，你好我自己看过你的文档，首先非常感谢你能总结这么多资料文档给大家。论坛上大部分网友使用的大都是zlstone的版本，我注意到文档里你还提到了自己编写的卡尔曼滤波程序，我非常佩服。不知能否分享一下自己写的代码的实际调试经验以及效果如何？不管怎样都要感谢你的分享精神 mark一下 最近对这个东西也茫恰恰。。研究Ing。。 mark 写的不错哦 学习{:sad:}{:smile:} 我来学习学习 好资料 顶一个~~~~ mark 不错不错！ 你好,本公司正在研发自平衡独轮车,自平衡方面的程序遇到问题,渴望请教一下,本人电话13612915729,QQ号:65343386,汪先生 wang999505 发表于 2013-10-26 17:28 static/image/common/back.gif
你好,本公司正在研发自平衡独轮车,自平衡方面的程序遇到问题,渴望请教一下,本人电话13612915729,QQ号:65343 ...
哎，早不玩了。 wwxxcc 发表于 2013-1-23 21:01 static/image/common/back.gif
楼主，你好我自己看过你的文档，首先非常感谢你能总结这么多资料文档给大家。论坛上大部分网友使用的大都 ...
他那个测角精度不高 wang999505 发表于 2013-10-26 17:28 static/image/common/back.gif
你好,本公司正在研发自平衡独轮车,自平衡方面的程序遇到问题,渴望请教一下,本人电话13612915729,QQ号:65343 ...
这些东西论坛里很多的，你仔细找下 顶一下 顶一下 顶一下 MARKMARKMARK hitor 发表于 2013-10-27 22:48 static/image/common/back.gif
这些东西论坛里很多的，你仔细找下
目前程序调试无法让独轮车平衡立起来,问题出在那,我们使用的是位置式PID算法做的,是不是误差太大了?还有电机给他快的速度,就出现前后换向时抖的很厉害,慢速度的话角度前后换向时就不会抖,很平稳! 谢楼主分享~~~顶一个~~~ 很好！多谢分享啊 收下了，谢谢楼主！尤其喜欢你描述自己如何探索卡尔曼滤波的经过，之前我也是在那里纠结了很久很久，后面车子还是用互补滤波站起来的。 chenjiawei7 发表于 2012-12-15 17:52
用的12Ｖ锂电池，电机说是用１２Ｖ的，PWM的范围是0~255，电机在PWM=150的时候还是不转的，到２００的时 ...
嘿嘿，兄弟你的PWM频率太高啦？ hitor 大师:
请教你独轮车PID算法,能共享下么?
文献上说,用角度PD内环+速度PD外环,这两个环怎么叠加在一起? 总结的非常不错，卡尔曼也非常好， 感谢楼主！ 好好学习学习LZ总结的精华 楼主，我看了你的文章，觉得非常好，我也学过卡尔曼滤波，我有一个问题想问你，就是两个噪声序列的方差Q,R的数据你是怎么得到的，是靠实验时不断取值得到的还是别的方法，谢谢！ 好文章，学习
页:
[1]