天空工厂姿态解算及控制探究

姿态解算及控制天空工厂周欢班门弄斧抛砖引玉大纲几个概念姿态解算姿态控制常见算法基础知识PID实现姿态解算几个概念方向余弦阵欧拉角四元数坐标系坐标系地球坐标系机体坐标系即我们的参考坐标系即机体自身坐标系欧拉角定义：构件在三维空间中的有限转动，可依次用三个相对转角表示，即进动角、章动角和自旋角，这三个转角统称为欧拉角。横滚角俯仰角偏航角方向余弦阵坐标系变换的关系定义：设有空间两点，若以P1为始点，另一点P2为终点的线段称为有向线段.通过原点作一与其平行且同向的有向线段.将与Ox,Oy,Oz三个坐标轴正向夹角分别记作α,β,γ.这三个角α,β,γ称为有向线段的方向角.其中0≤α≤π,0≤β≤π,0≤γ≤π.若有向线段的方向确定了，则其方向角也是唯一确定的。四元数定义：四元数是最简单的超复数。由实数加上三个元素i、j、k组成，而且它们有如下的关系：i^2=j^2=k^2=ijk=-1,每个四元数都是1、i、j和k的线性组合，即是四元数一般可表示为a+bi+cj+dk。单位四元数的对可以表示四维空间中的一个转动百度知道空间的位置可以由一个四元数阵确定姿态解算常见算法欧拉角法方向余弦法四元数法核心：从两个坐标系之间的变换阵求出姿态角方法：求姿态更新方程条件：加速度计——加速度

陀螺仪——角速度等效旋转矢量法卡尔曼滤波欧拉角法微分方程解算欧拉角微分方程需要解三个微分方程但在用计算机进行数值积分时，要进行超越函数（三角函数）的运算，从而加大了计算的工作量。当载体的纵摇角（俯仰角）为90°时，将出现奇点，因此该方法不能进行全姿态解算。方向余弦法本质：直接求解方向余弦阵的微分，即随时间矩阵的变化可以全角度解算姿态角，避免了欧拉法的退化四元数法空间姿态可以由四元数唯一表示与姿态角的关系等价四元数的更新难点：微分方程求解一阶近似：归一化初始对准怎么获得四元数？等效旋转矢量法特别适用于角机动频繁激烈或存在严重角振动的运载体的姿态更新基于四元数，克服普通四元数更新算法只合适低动态运动的特性几种算法对比算法是否全姿态计算复杂度精度实用性欧拉法否（90°出现退化）微分方程+三角超越函数微分方程近似时有误差小机动条件方向余弦法是9个未知量微分方程同上计算实时性差四元数法是4个未知量微分方程同上现在最常用等效旋转矢量法是微分方程误差补偿大机动卡尔曼滤波卡尔曼滤波器由一系列递归数学公式描述。它们提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。卡尔曼滤波器应用广泛且功能强大：它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。知识基础：状态方程观测方程状态变量状态空间方程核心目标：离散卡尔曼滤波算法连续条件进行离散化即可卡尔曼滤波算法关键建立系统的状态空间方程多种方法四元数四元数的更新方程就是状态方程，再建立观测方程即可方向余弦法物理意义法卡尔曼滤波卡尔曼滤波floatkalman(unsignedcharaxis,floatnewAcc_Angle,floatnewReading_Gyro){ angle[axis]+=dt*(newReading_Gyro-gyro_Bias[axis]); P_00[axis]+=-dt*(P_10[axis]+P_01[axis])+dt*Q_00; P_01[axis]+=-dt*P_11[axis]; P_10[axis]+=-dt*P_11[axis]; P_11[axis]+=dt*Q_11; Err=newAcc_Angle-angle[axis]; K_0=P_00[axis]/(P_00[axis]+R); K_1=P_10[axis]/(P_00[axis]+R); angle[axis]+=K_0*Err; gyro_Bias[axis]+=K_1*Err; P_00[axis]-=K_0*P_00[axis]; P_01[axis]-=K_0*P_01[axis]; P_10[axis]-=K_1*P_00[axis]; P_11[axis]-=K_1*P_01[axis]; returnangle[axis];}姿态控制基础知识比例调节积分调节微分调节在P调节中，调节器的输出信号u与偏差信号e成比例，即，u=Kpe式中Kp称为比例增益比例调节P调节对偏差信号能做出及时反应，没有丝毫的滞后。比例调节的显著特点就是有差调节。放大系数越小，余差就越大；放大系数越大，余差就越小。K过大，系统容易振荡积分调节调节器的输出信号的变化速度du/dt与偏差信号e成正比，或者说调节器的输出与偏差信号的积分成正比，即：只要偏差存在，调节器的输出就会不断变化，直到偏差为零调节器的输出才稳定下来不再变化。所以积分调节作用能自动消除余差。稳定性差，有滞后，必然产生超调微分控制调节器的输出u与被调量或其偏差e对于时间的导数成正比，即式中，S2——微分时间。微分调节只与偏差的变化成比例，偏差变化越剧烈，由微分调节器给出的控制作用越大，从而及时地抑制偏差的增长，提高系统的稳定性。有超前控制的作用，但是对于系统扰动会放大PID调节器//横滚角有误差向右滚为正

temp=FFRoll-roll; err_I_roll+=temp; temp2=Kp_roll*(temp+Ti_roll*err_I_roll+Td_roll*(temp-err_prew_roll));

if((m2>=min_v&&m2<=max_v)||(m4>=min_v&&m4<=max_v)) { m2=m2+temp2; m4=m4-temp2; } else { err_I_roll-=temp; } err_prew_roll=temp;PID实现谢谢！欢迎批评指正！假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White

Gaussian

Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian

Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那