按类别存档： 机器人

从类似小米平衡车里面的MPU，可以计算得到几个不同的小车倾角，分别是根据加速度计算出的倾角、根据陀螺仪计算出的倾角和互补算法计算出的倾角、卡尔曼算法计算出的倾角。

• 加速度计算的角度

加速度计可以测量某时刻XYZ三个方向的加速度值，而通过重力加速度在XYZ三个轴的分量，就可以计算出小车的倾角。

不过小车在非静止状态下，加速度计测得的结果并不非常精确。因为小车在动态时受电机作用还有前进或者后退的力，所以加速度计测得是总的加速度。这表明加速度数据在静态下才准确，不过好消息是陀螺仪在动态下的数据准确，所以需要综合加速度计的加速度数据和陀螺仪的角速度数据才会更准确，这些是所谓的互补算法或卡尔曼滤波算法。

从简单的开始，把MPU平放在车体上，定义X为前进方向，则Y轴平行于左右车轮的轴线，如下图：

可见车体Y轴与地平面夹角是不会发生变化的，一直为0度。能发生变化的是X轴与平面的夹角，或者说Z轴与平面的夹角，可以证明X轴Z轴夹角变化是一致的。下面抛开上面的透视图，采用正试图来看：

上图中，g为重力加速度，gx和gz为MPU测得的加速度数据，但同时也是重力加速度在x和z轴的分量。

很容易看出小车倾角就是atan（gx/gz），实际使用考虑到零点偏移：Angle_Z = (AccZ – AccZ_Offset) * R_Z。

其中AccZ是Z轴加速度实时测得的数据，AccZ_Offset是Z轴加速度的静差，R_Z是加速度计Z轴比例系数。

上面是用的三角函数的反正切函数，从数值计算角度也可以用asin（gx/g），因为在小角度时候有sin（dalpha）=alpha的近似（弧度方式）成立。角度方式是sin（alpha）=alpha*pi/180，MPU给出的是后面这种角度方式。经过拟合试验后可以进一步作修正：sin（alpha）=0.92*alpha*pi/180，因此由gx/g=sin(alpha)=0.92*alpha*pi/180，可以得角度alpha=180*gx/0.92*pi*g，这是个近似的方法，这个数据不很准确，单是速度快。

• 陀螺仪计算的角度

通过陀螺仪计算角度很简单，陀螺仪测得的是角速度，角速度乘某段时间就是该段时间所转过的角度，如果再把每次计算所得的角度累加（积分）就得到当前位置的倾角了。

假设，陀螺仪是最初平行于桌面，单片机每delta_t时间读一次陀螺仪的角速度，读三次以后 Z轴顺时针转到下图中位置：

那么这段时间内转过的总角度为角1+角2+角3，如果陀螺仪测得的角速度分别为w1/w2/w3，则总角度为w1*delt_t1+w2*delta_t2+w3*delta_t3，这其实就是个离散积分的过程，一般写成：angle_n =  angle_n-1 + (Gyro – Gyro_Offset) * R_Gyro。

其中angle_n是当前角度值，angle_n-1是上次计算所得的角度。Gyro是陀螺仪实时测得角速度，Gyro_Offset是陀螺仪零点偏移值。R_Gyro是陀螺仪比例。

但陀螺仪计算所得的角度也存在误差，而且误差越积累越大，最终导致计算角度与实际角度相差很大，因此需要考虑加速度计算的角度。

•  互补算法计算的角度

 //一阶互补算法
factor = 0.095;
K =factor / (factor + delta_t);   //K开始时0.98
angle_Comp1 = K * (angle_Comp1 + omega * delta_t) + (1-K) * angle_Acc;
// 二阶互补算法
factor = 0.2;
x1 = (angle_Acc – angle_Comp2)*(1-factor)*(1-factor);
y1 = y1 + x1 * delta_t;
x2 = y1 + 2 * (1-factor) *(angle_Acc – angle_Comp2) + omega;
angle_Comp2 = angle_Comp2 + x2 * delta_t;

• 卡尔曼算法计算的角度

angle_Kalman += (omega – Gbias) * delta_t;  //先验估计

Pdot[0] = -PP[0][1] – PP[1][0] + Q_angle;
Pdot[1] = – PP[1][1];
Pdot[2] = – PP[1][1];
Pdot[3] = Q_gbias;//

PP[0][0] += Pdot[0] * delta_t;
PP[0][1] += Pdot[1] * delta_t;
PP[1][0] += Pdot[2] * delta_t;
PP[1][1] += Pdot[3] * delta_t;     //先验估计误差协方差

PCt_0 = C_0 * PP[0][0];  PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;

if(E!=0) {
K_0 = PCt_0 / E; K_1 = PCt_1 / E;
}

PP[0][0] -= K_0 * PCt_0;
PP[0][1] -= K_0 * PCt_1;
PP[1][0] -= K_1 * PCt_0;
PP[1][1] -= K_1 * PCt_1;      //后验估计协方差

angle_Err = angle_Acc – angle_Kalman;
angle_Kalman += K_0 * angle_Err;    //后验估计
Gbias += K_1 * angle_Err;

• 坐标系

无论是飞行器还是平衡车，都需要清楚机体自身的姿态。要了解姿态，首先需要知道地面坐标系和机体坐标系。

地面坐标系（earth-surface inertial reference frame），Sg=O_XgYgZg

在地面上选一点Og，使Xg轴在水平面内，并指向某一方向；Yg轴在水平面内，并垂直于Xg轴；Zg轴垂直于地面，并指向地心。

机体坐标系(aircraft body coordinate frame)，Sb=O_XbYbZb

原点O取在机体质心处，坐标系与机体固连；Xb轴在机体对称平面内，平行于机体设计轴线，而且指向机头；Yb轴垂直于飞机对称平面，指向机身右方；Zb轴在飞机对称平面内，与XY轴垂直并指向机身下方。

地面坐标系Sg和机体坐标系Sb的XYZ三个轴，其指向均由右手定则确定，即食指Ｘ／三小指Ｙ／拇指Ｚ，如下图：

• 姿态角（欧拉角）

姿态角，又叫欧拉角（Euler Angle），包括pitch/yaw/roll。这三个角度表示机体坐标系与地面坐标系的关系，可以反应机体相对于地面的姿态。（姿态角在机械行业也有借用，丝杆规格书中的pitch/yaw/roll就是表示类似概念的三个误差。）

下面借用飞机的动画说明这三个角。

pitch，俯仰角，θ：是机体坐标系Xb轴与水平面的夹角；当Xb轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。

yaw，偏航角，ψ：是机体坐标系Xb轴在水平面上的投影与地面坐标系Xg轴之间的夹角，由Xg轴逆时针转至机体Xb的投影线时偏航角为正，即机头右偏航为正，反之为负。

roll，滚转角，Φ：是机体坐标系Zb轴与通过机体Xb轴的铅垂面间的夹角，即机体向右滚为正，反之为负。

航空领域把欧拉角叫作泰特-布莱恩特角（Tait-Bryan angle），由pitch（俯仰角）/roll（横滚角）/yaw（导航角）构成。它们在地朝下、在天朝上，选取东-北-天（E-N-U）地理坐标系为导航坐标系，称作n系，即以东面为X轴的正方向，北面为Y轴的正方向，天上（上面）为Z的正方向，如下图。

在上图中，Pitch、Yaw、Roll一般用表示，分别为绕Y轴、Z轴、X轴的旋转角度。

• 姿态矩阵

姿态角作为一种描述方位的方法，是用数学家Euler名字命名的，他首先证明了角位移序列等价于单个角位移，所以也叫欧拉角。欧拉角的基本思想是，将角位移分解为绕着三个正交轴的三个旋转角组成的序列。这对于习惯了笛卡儿坐标系的人们来说听着很复杂，其实是非常直观的，但更主要的是有用，因为欧拉角能用来描述任意旋转，用来构建姿态矩阵。

姿态矩阵（旋转矩阵）是3×3的矩阵，可以表示姿态。以工业机器人这样有级联旋转机构的设备为例，例如该机器人的手臂某时刻处于零姿态，其一级手臂末端点空间坐标为（X0，Y0，Z0），这是已知的。现在手臂旋转运动到了另一个姿态，此时一级手臂末端点空间坐标表示为（X1，Y1，Z1），那么如何求取现坐标（X1，Y1，Z1），这个时候就要用到姿态矩阵。 因为机械臂的旋转是由电机驱动的，电机旋转了多少角度可以通过编码器获知，那么就能找到这个时候手臂的欧拉角姿态，通过欧拉角就可以算出旋转矩阵，然后用（X0，Y0，Z0）的转置（3行1列向量）去乘以这个3×3矩阵，得到的3行1列向量就是（X1，Y1，Z1）。同理，手臂的第二级，第三级……，都可以照章办理。

• 四元数

四元数的定义是：

不经意看有点类似于机器视觉里面的齐次坐标，但四元数的内涵超出齐次坐标很多，有一些人在用四元数研究爱因斯坦的相对论。

对于视觉和运动中常见三种坐标变换：平移、缩放和旋转，旋转时最复杂。按照学习的习惯，人们更熟悉的是笛卡儿坐标的矩阵旋转和上面提到的欧拉旋转。矩阵旋转使用4*4的矩阵表示绕任意轴旋转的变换矩阵。欧拉旋转按照一定的坐标轴顺序绕每个轴轴旋转一定的角度来变换坐标向量。还有一种旋转的表示方法，就是四元数。

四元数又是什么？简单说四元数本质上是一种高阶复数，是一个四维空间。复数的二维空间在中学学过，一个复数由实部和虚部组成即x = a + bi，其中i是虚数单位，而且i^2 = -1。四元数其实与此类似，不同的是它的虚部包含了三个虚数单位i、j、k，即一个四元数由三个虚部组成x = a + bi + cj + dk。

那么四元数为什么又和三维旋转有关系呢？为了方便改写一下四元数x = ((x, y, z)，w) = (v, w)，其中v是向量，w是实数。

通过旋转轴和绕该轴旋转的角度，可以构造出四元数：

一般思路是思路： 6轴数据–>四元数–>欧拉角。