Pixhawk-串级pid介绍

     根深方能叶茂 在等待的日子里，刻苦读书，谦卑做人，养得深根，日后才能枝叶茂盛。 --Better(根爷)

    鉴于串级PID在pixhawk系统中的重要性，无论是误差的补偿，如姿态解算；还是控制的实现，如姿态控制，位置控制，靠的都是串级的pid，这里我们先对串级pid做一个介绍，后面会再接着分析，姿态的控制以及位置的解算和控制。他们的分析都还将从原理框图和源码注释上说明，就是把自己平时的一点整理与大家交流一下，也希望大神能带我飞。

    这一部分说三部分内容：

     1、pid的介绍
     2、pid调参
     3、串级pid

     4、pid与滤波的关系，这也是一个很有意思的问题，一个是从控制角度理解，一个是从滤波角度理解。这一个我只是一点理解，就在这里先说一点。pid中，i相当于低通滤波器，极限情况下理解：直流信号肯定会持续积分，反而高频的噪声正负叠加被屏蔽了，所以i是低通滤波器。而D是高通滤波器，同样极限情况下理解：直流信号微分为0，高频的噪声微分却有了值，所以D是高通滤波器，和我们平时说到的D太大容易放大噪声造成震动等效。

     1、pid的介绍
    在工业应用中PID及其衍生算法是应用最广泛的算法之一，是当之无愧的万能算法，如果能够熟练掌握PID算法的设计与实现过程，对于一般的研发人员来讲，应该是足够应对一般研发问题了，而难能可贵的是，在我所接触的控制算法当中，PID控制算法又是最简单，最能体现反馈思想的控制算法，可谓经典中的经典。经典的未必是复杂的，经典的东西常常是简单的，而且是最简单的，简单的不是原始的，简单的也不是落后的，简单到了美的程度。现在虽然已经演变出很多智能的算法，如蚁群，神经网络等，感兴趣可以看一下刘金琨老师的《先进pid控制》，但是在实际应用中还是以串级pid为主，因为它可靠。

    先看看PID算法的一般形式：

   PID的流程简单到了不能再简单的程度，通过误差信号控制被控量，而控制器本身就是比例、积分、微分三个环节的加和。这里我们规定（在t时刻）：

   1.输入量为rin(t);

   2.输出量为rout(t);

   3.偏差量为err(t)=rin(t)-rout(t);

   pid的控制规律为

   1.说明一下反馈控制的原理，通过上面的框图不难看出，PID控制其实是对偏差的控制过程;

   2.如果偏差为0,则比例环节不起作用，只有存在偏差时，比例环节才起作用。

   3.积分环节主要是用来消除静差，所谓静差，就是系统稳定后输出值和设定值之间的差值，积分环节实际上就是偏差累计的过程，把累计的误差加到原有系统上以抵消系统造成的静差。

   4.而微分信号则反应了偏差信号的变化规律，或者说是变化趋势，根据偏差信号的变化趋势来进行超前调节，从而增加了系统的快速性。

   

    下面将对PID连续系统离散化，从而方便在处理器上实现。下面把连续状态的公式再贴一下：

    假设采样间隔为T，则在第K T时刻：

    偏差err(K)=rin(K)-rout(K);

    积分环节用加和的形式表示，即err(K)+err(K+1)+……;

    微分环节用斜率的形式表示，即[err(K)-err(K-1)]/T;

    从而形成如下PID离散表示形式：

    则u(K)可表示成为：

   至于说Kp、Ki、Kd三个参数的具体表达式，我想可以轻松的推出了，这里节省时间，不再详细表示了。

其实到这里为止，PID的基本离散表示形式已经出来了。目前的这种表述形式属于位置型PID，另外一种表述方式为增量式PID，由U上述表达式可以轻易得到：

    那么：

    这就是离散化PID的增量式表示方式，由公式可以看出，增量式的表达结果和最近三次的偏差有关，这样就大大提高了系统的稳定性。需要注意的是最终的输出结果应该为

       u(K)+增量调节值;

     PID的离散化过程基本思路就是这样，下面是将离散化的公式转换成为C语言，从而实现微控制器的控制作用。

那么如何用c语言进行表达？下面将对pid一种常见的形式进行c语言的描述，注意他们的演变过程，在pixhawk中也用到这其中的一些注意事项，如积分分离。

     

位置型PID的C语言实现：

变积分的PID控制算法C语言实现：

    最后给出大家专家系统中控制经验，自己理解吧。

    反应系统性能的两个参数是系统误差e和误差变化律ec，

    首先我们规定一个误差的极限值，假设为Mmax；规定一个误差的比较大的值，假设为Mmid；规定一个误差的较小值，假设为Mmin；

     e*ec>0  误差在朝向误差绝对值增大的方向变化(可以理解成速度和加速度)

             若此时 abs(e)>Mmid ：误差较大 强控制 

             若此时 abs(e)<Mmid ：误差绝对值本身并不是很大 一般的控制作用

     e*ec<0  误差在朝向误差绝对值减小的方向变化

             若此时 e*err(k-1)>0或者e=0 ：误差的绝对值向减小的方向变化，或者已经达到平衡状态，

                    此时保持控制器输出不变即可。 

             若此时e*err(k-1)<0 ： 误差处于极限状态。如果误差的绝对值>min，强控制 （调节幅度比较大）                       如果此时误差绝对值较小，可以考虑实施较弱控制作用。

     当abs（e）>Mmax时，说明误差的绝对值已经很大了，都应该考虑控制器的输入应按最大（或最小） 输出，以                          达到迅速调整误差的效果，使误差绝对值以最大的速度减小。

     当abs(e)<Mmin时，说明误差绝对值很小，此时加入积分，减小静态误差。

2、pid调参你怎么看

1）．PID调试一般原则 

a.在输出不振荡时，增大比例增益P。 

b.在输出不振荡时，减小积分时间常数Ti。 

c.在输出不振荡时，增大微分时间常数Td。

（他们三个任何谁过大都会造成系统的震荡。）

2）．一般步骤

 a.确定比例增益P ：确定比例增益P 时，首先去掉PID的积分项和微分项，一般是令Ti=0、Td=0（具体见PID的参数设定说明），使PID为纯比例调节。输入设定为系统允许的最大值的60%~70%，由0逐渐加大比例增益P，直至系统出现振荡；再反过来，从此时的比例增益P逐渐减小，直至系统振荡消失，记录此时的比例增益P，设定PID的比例增益P为当前值的60%~70%。比例增益P调试完成。

b.确定积分时间常数Ti比例增益P确定后，设定一个较大的积分时间常数Ti的初值，然后逐渐减小Ti，直至系统出现振荡，之后在反过来，逐渐加大Ti，直至系统振荡消失。记录此时的Ti，设定PID的积分时间常数Ti为当前值的150%~180%。积分时间常数Ti调试完成。

c.确定积分时间常数Td 积分时间常数Td一般不用设定，为0即可。若要设定，与确定 P和Ti的方法相同，取不振荡时的30%。

 d.系统空载、带载联调，再对PID参数进行微调，直至满足要求：理想时间两个波，前高后低4比

3、串级pid简介

    串级pid内外两环并联调节，这样的好处的是增加系统的稳定性，抗干扰。同时调节系统缓慢过度，注意外环都是本身误差，内环是速度，如位置控制外环是位置，内环是速度，是因为位置改变的实现是靠三个方向的速度积分出来的。同样姿态控制中，外环是角度差，内环是加速度，是因为角度的实现是靠角速度过渡来的，他们都是这样的一个过渡过程。实际中如果你追求响应的快捷，你也可以直接控制内环，或者直接控制姿态。

    串级PID两个PID控制算法，只不过把他们串起来了(更精确的说是套起来)。那这么做有什么用？答案是，它增强了系统的抗干扰性(也就是增强稳定性)，因为有两个控制器控制飞行器，它会比单个控制器控制更多的变量，使得飞行器的适应能力更强。画出串级PID的原理框图，

    在整定串级PID时的经验则是：先整定内环PID，再整定外环P。因为内环靠近输出，效果直接。

    内环P：从小到大，拉动四轴越来越困难，越来越感觉到四轴在抵抗你的拉动；到比较大的数值时，四轴自己会高频震动，肉眼可见，此时拉扯它，它会快速的振荡几下，过几秒钟后稳定；继续增大，不用加人为干扰，自己发散翻机。
    特别注意：只有内环P的时候，四轴会缓慢的往一个方向下掉，这属于正常现象。这就是系统角速度静差。
    内环I：前述PID原理可以看出，积分只是用来消除静差，因此积分项系数个人觉得没必要弄的很大，因为这样做会降低系统稳定性。从小到大，四轴会定在一个位置不动，不再往下掉；继续增加I的值，四轴会不稳定，拉扯一下会自己发散。
    特别注意：增加I的值，四轴的定角度能力很强，拉动他比较困难，似乎像是在钉钉子一样，但是一旦有强干扰，它就会发散。这是由于积分项太大，拉动一下积分速度快，给  的补偿非常大，因此很难拉动，给人一种很稳定的错觉。
    内环D：这里的微分项D为标准的PID原理下的微分项，即本次误差-上次误差。在角速度环中的微分就是角加速度，原本四轴的震动就比较强烈，引起陀螺的值变化较大，此时做微分就更容易引入噪声。因此一般在这里可以适当做一些滑动滤波或者IIR滤波。从小到大，飞机的性能没有多大改变，只是回中的时候更加平稳；继续增加D的值，可以肉眼看到四轴在平衡位置高频震动(或者听到电机发出滋滋的声音)。前述已经说明D项属于辅助性项，因此如果机架的震动较大，D项可以忽略不加。
   外环P：当内环PID全部整定完成后，飞机已经可以稳定在某一位置而不动了。此时内环P，从小到大，可以明显看到飞机从倾斜位置慢慢回中，用手拉扯它然后放手，它会慢速回中，达到平衡位置；继续增大P的值，用遥控器给不同的角度给定，可以看到飞机跟踪的速度和响应越来越快；继续增加P的值，飞机变得十分敏感，机动性能越来越强，有发散的趋势。

4、最后给你贴上pixhawk有关pid的源码，就是位置式的很简单，自己理解一下吧。需要说明的是位置式pid容易导致积分的饱和，所以在积分上过了很多处理。如在位置控制中，推力的积分量就是进行了饱和处理。

__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
{
	if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
		return pid->last_output;
	}

	float i, d;

	/* current error value */
	float error = sp - val;

	/* current error derivative */
	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
		pid->error_previous = error;

	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
		pid->error_previous = -val;

	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
		d = -val_dot;

	} else {
		d = 0.0f;
	}

	if (!isfinite(d)) {
		d = 0.0f;
	}

	/* calculate PD output */
	float output = (error * pid->kp) + (d * pid->kd);

	if (pid->ki > SIGMA) {
		// Calculate the error integral and check for saturation
		i = pid->integral + (error * dt);

		/* check for saturation */
		if (isfinite(i)) {
			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
			    fabsf(i) <= pid->integral_limit) {
				/* not saturated, use new integral value */
				pid->integral = i;
			}
		}

		/* add I component to output */
		output += pid->integral * pid->ki;
	}

	/* limit output */
	if (isfinite(output)) {
		if (pid->output_limit > SIGMA) {
			if (output > pid->output_limit) {
				output = pid->output_limit;

			} else if (output < -pid->output_limit) {
				output = -pid->output_limit;
			}
		}

		pid->last_output = output;
	}

	return pid->last_output;
}


__EXPORT void pid_reset_integral(PID_t *pid)
{
	pid->integral = 0.0f;
}