自动控制原理课程设计报告.docx

北京科技大学
自动控制原理课程设计

学院:　
班级:
学号:
指导教师：
姓名:
目录
一、引言 3
二、系统模型得建立 3
三、系统控制得优化 6
3、1 PID调节参数得优化 7
3、2 积分分离PID得应用 10
四,结语 13

双轮自平衡智能车行走伺服控制算法
摘要:全国第八届“飞思卡尔”智能汽车大赛已经结束。光电组使用大赛提供得D车模，双轮站立前进，相对于以前得四轮车,双轮车得控制复杂度大大增加。行走过程中会遇到各种干扰,经过多次得实验,已经找到了一套能够控制双轮车得方法。双轮机器人已经广泛用于城市作战,排爆,反恐,消防以及空间消防等领域。实验使用单片机控制双电机得转速，达到了预期得效果。
关 键 词:自平衡;智能;控制算法
Ｍｏtion Ｓeｒvo　Cｏｎtrol　Aｌｇorithｍ for Dual Ｗheel Ｉnｔeｌliｇent Ｃaｒ
Absｔracｔ： The ８th freｅscaｌｅ cuｐ naｔional Inｔelligｅnt Cａr　pｅtitiｏn　of has beeｎ end、The lｅd team mｕst uｓeｄ Ｄ cａr which has only 2tiｒｅs、Iｔ iｓ moｒe　dｉfficult to ｃontrol prefeｒ ｔｏ　ｃｏntrol　 Ａ caｒ　ｗhich ｈas 4tirｅs、Thｅｒｅ iｓ muｃh inｔｅrｆｅrｅncｅ oｎ ｔhe track、　A two-wheeｌed robots have　ｂｅen　wｉｄely ｕsed ｉn urbａn warfaｒｅ, eod， counｔeｒ-tｅｒｒorisｍ,　fiｒe cｏntroｌ ａnｄ　sｐaｃe fire contｒｏl　anｄ oｔheｒ fielｄs。Ｗe ｈas searｃheｄ　a goｏd ways to conｔrol iｔ、We used MCU　ｔo control　tｈe　speed of moｔｏrs　and ｇet ouｒ gａｔes、
Keｙ Woｒｄs: bａlance ｂy　ｓeｌf; intｅlligent；　ｃontｒol ａｌgｏrｉthm
一、引言
双轮自平衡车就就是智能汽车中一个重要得组成部分。由于其就就是两轮站立,在很多场合有比四轮车更灵活,更方便控制得特点。基于双轮车得机器人已经用于在城市作战,排爆,反恐,消防，空间探测等领域。
双轮自平衡车得控制过程与倒立摆相似。本次研究主要就就是对双轮车直立得研究。由于车在前进过程中还可能遇到弯道与路障,所以本文还对整个系统得稳定性做了比较深入得研究。
在理论证明正确得前提下,再次进行仿真验证,仿真验证得结果与实验结果相符合。
二、系统模型得建立
智能汽车竞赛使用得就就是D车模。D车模就就是双电机,双轮得车模。单电机控制双轮时可以满足车身保持平衡与前进得要求,而之所以使用双电机就就是为了前进过程中转弯得需要。本文只讨论跟直立与干扰有关得内容,鉴于此,本文余下内容默认就就是单电机控制得车模。自平衡车在没有电机控制得情况下属于不稳定系统,在极小得干扰下,就会偏离平衡位置而失去平衡。因此,控制系统必须使用反馈控制。图１就就是站立在赛道上得双轮车模。

图1 站立在赛道上得自平衡双轮车
　 　自平衡双轮车在只研究直立得情况下与倒立摆相似,因此这里将借助倒立摆得研究方式研究双轮车得平衡控制。
控制车模平衡得主观经验。一般得人通过简单练习就可以让一个直木棒在手指尖上保持直立。这需要两个条件:一个就就是托着木棒得手可以移动;另一个就就是眼睛可以观察到木棒得倾斜角度与倾斜趋势(角速度）。通过手掌移动抵消木棒得倾斜角与趋势，从而保持直立。这两个条件缺一不可,实际上就就就是控制中得负反馈控制,见图２。

图2　在手上不倒得木棍
世界上没有一个天才杂技演员可以蒙着双眼使得木棒在自己指尖上直立,因为没了双眼就没了负反馈得环节。经过简单得分析可以得到图3所示得控制结构图。

图3 不倒得木棍得控制结构图
同样,双轮直立车得控制也就就是通过负反馈来实现得,与上面保持木棍直立则比较简单。因为车模有2个轮子着地,车体只会在轮子滚动得防线上发生倾斜。控制轮子转动,抵消在一个维度上倾斜得趋势便可以保持车体得平衡了。如图4,5, 6所示。

图4 车体平衡，车轮静止

图5 车体后倾,车轮向后加速

图6　车体前倾，车体向前加速
将双轮车简化成高度为Ｌ,质量为m得简单倒立摆,它可以放置在左右移动得车轮上。
与空气中得单摆类似,车身偏离平衡位置后要想能迅速得回到平衡位置,车身应该受到与位移方向相反得得回复力。
在平衡点附近，角度θ很小,即有ｓinθ ≈　θ。假设车身偏离平衡角度后，将受到电机产生得与角度成比例得力:
f =　mｋ１θ ﻩ(1)
　由于重力原因，车身偏离平衡位置后受到得回复力为:
Ｆ ＝　mgθ – ｍｋ１θ (2)
从上式中可以瞧出回复力要想与位移相反,必须有k1　＞ g。假设外力干扰引起车模产生角速度x(ｔ）。沿着垂直于车模底盘得方向进行受力分析,可以得到车模倾角与车轮加速度a(t)以及外力干扰加速度x(t)之间得得运动方程。受力分析如图7所示。

图７　车模受力分析
Ld2θdt2=gθt-at+Lxt-k1θ　 (４）
车模静止不向前运动时,a(t) = 0，即:
Ld2θdt2=gθt+ Lxt-k1θ(t) 　 　 　(5）
对应车模静止时,系统输入输出得传递函数为:
Hs= θ(s)X(s)= 1s2+k1L- gL 　 　　 （6)
此时系统具有2个极sp = ±-g+k1L。一个极点位于s平面得右半平面,车模不稳定。在此处假设L=0.1，k1 =　２0用matlab仿真此系统:
Maｔlab代码如下:
%程序目得：在没有微分量得情况下仿
%真系统得阶跃响应
%autｈor:程顺均
%时间:2０13、８、２8
sys ＝ ｔf([1],　[１　0 100]);
t =　０:0、01:3；
fiｇure(1)
step(sys,ｔ);grid oｎ
仿真截图如图8所示:

图８　比例控制系统得阶跃响应
从图来瞧,与分析结果一样,系统得阶跃响应为等幅振荡,不稳定。原因就就是在阶跃响应中没有阻尼,也就就就是说实际中得车不会倒向一边,而就就是在平衡位置附近来回震荡。这显然不就就是我们需要得到得效果。
因此在系统中我们加入为微分量,也就就就是阻尼。在实际中变现为阻碍车模转动得力,类似于摩擦阻力，大小方向与车身角速度有关。即阻力
Ff= k2dθdt　 　 　 　 (7)
经过拉式变换,得到带阻尼得系统得控制结构图,如图9所示:

图9 加入比例微分反馈控制得系统
系统传递函数为：
Hs= θ(s)X(s)= 1s2+k2Ls+k1-gL　　 　　(8)
此时系统得极点为:
sp= -k2±k2 2-4L(k1-g)2L　 　 　 　 （9)
根据自动控制原理稳定性分析知识知道，只有系统得２个极点都在s平面得左半平面时系统才就就是稳定得。因此系统必须满足k1>g,k2>0 。　
假设k２　= 0、5，k1 = 20。利用ｍａｔlab仿真。
仿真代码如下:
%程序目得:在比例微分控制得情况下仿
%真系统得阶跃响应
%ａutｈor:程顺均
%时间:201３、8、28
sys　＝ tf([１]，［１,5,100］）；
t　= 0:0、0１:３;
figure（1）
stｅp(sys，t);grid ｏn;
ｔiｔle（'比例微分控制时得阶跃响应');
阶跃响应曲线如下：

图10 比例微分控制得阶跃响应曲线
从响应得曲线来瞧比例—微分控制曲线能够比较收敛下来。在宏观瞧来，就就就是车身能够很快到我们需要得角度，而不就就是一直在设定角度得前后震荡。
研究到此也智能算就就是解决了直立控制得问题。在实际中往往有很多干扰因素,例如飞思卡尔竞赛中赛道上就设有比赛道高得路障。如果稳定性较差得直立车在通过路障得时候,会直接倒下。假设干扰信号为D(s),D(s）与θs得关系为:
θs=(Ls2-gLs2+k2s+k1-g)D(s) 　（10)
这里我们把路障一类得干扰瞧成就就是脉冲干扰,D(ｓ) = 1。仿真参数不变，对干扰进行一次仿真。
仿真代码如下：
%程序目得：仅在干扰下得输出响应
%auｔhor：程顺均
%时间:2013、8、２8
ｓyｓ = ｔf（[0、1 0 -１0]，[0、1　0、２ 10]）;
t　 = 0:0、2:10;
ｆiｇｕｒe(1);
iｍｐulse(sys,t）;
girｄ on;
仿真截图如图11所示:

图11 系统对脉冲干扰得响应
　 从仿真得图来瞧，脉冲干扰会让本来已经稳定得系统来回得震荡,在多次震荡后才回到比较稳定得状态。因此比例-微分控制得方式能够满足我们得要求。对于一个系统来讲,能够稳定下来并不就就是我们追求得最终目标。接下来将详细研究系统得优化与稳定性。
三、系统控制得优化
飞思卡尔智能汽车竞赛中评比成绩就就是以在赛道上跑一圈得时间为唯一依据得。这要求车在保持稳定得前提下以最大得速度前进。系统得响应时间,抗干扰能力都就就是需要反复调整得。快得调整时间,小得超调量,高得稳定性都就就是我们期望得结果。
3、1 ＰID调节参数得优化
一般得系统我们都需要短得调节时间，小得超调量。往往我们就就是不能两方面做到极限，但就就是通过PＩD参数得调节,我们可以得到一个满意得调节过程。
为了研究每个参数得意义,先进行单参数得研究。P一般就就是作为比例项,决定了系统得部分响应速度。在这里我们使用一个仿真代码完成这个仿真。
仿真代码见附件1:
Kp得研究
通过改变系统得Kp,可以得到一系列得仿真图。这里选取4组做研究。分别为1０0,40０,１０00，２00０。仿真图如图12,1３,１4,15所示。

图１２ Ｋp ＝ 1０0时得阶跃响应

图1３ Kp ＝ 4０0时得阶跃响应

图14 　Kｐ = 1００0时得阶跃响应

图15 Kp =　20０0时得阶跃响应
为了方便对比出她们得不同,这里将她们得响应特征列表出来，如表1所示。
表１ 　Ｋｐ对系统性能得影响
Kp
1

00
调节时间
1、5
0、8３5
1、1８5
1、37
超调量
５、8％
未超调
１１、8%
２6、4％
从表得得数据与调整图像来瞧,Kｐ得值很小得时候,系统得第一次上升达不到我们需要得值，需要积分项才能弥补。这不就就是我们需要得结果。而当Ｋｐ值很大得时候,上升时间缩短,超调量也会增大。因此Kp值得选择主要影响了系统得响应能力。
Ｋi得研究
ＰＩD中得Ｉ就就是积分项,主要就就是消除稳态误差。下面通过4组不同得参数得测试研究其作用。Ki得值为1００,400,１000,20０0得时候得值仿真图如图1５，16,１7,18所示。

图16 　Kｉ=1０0得稳态误差

图1７ 　Ki＝400得稳态误差

图18 　Ki=1000得稳态误差

图19　 Ki=２000得稳态误差
从图像可以瞧出来,其上升速度与超调量并无太大得差异。她们得差异主要体现在稳态误差上了。将四组仿真得稳态误差在1、5Ｓ时取值列表得表2。
表2 Ki对系统性能得影响
Ki
1

00
稳态误差
5、70％
4、31%
2、4４%
0、０３%
从表得数据很明显得可以瞧出，增大Kｉ值可以减小稳态误差。但就就是由于系统会受到一些小得干扰,所以Ki得值也并不就就是越大越好!
Kｄ得研究
在PＩD调节中微分项起预见作用,在系统靠近预期值时可以减小控制输入量，进而减小超调量,增强系统得性能。下面同样通过4组数据研究Kｄ值对系统得影响。Kｉ分别为１0，20,40，６0时得仿真图如图20，２1,22，2３所示。

图20 Kｄ=10得稳态误差

图２1　 Kd=2０得稳态误差

图22 Ｋd=４０得稳态误差

图23 　Kｄ=６0得稳态误差
从图上我们已经可以清晰地瞧出，Ｋｄ值得增大可以减小超调量,减少进入稳态前得振荡次数。下表列出了超调量与进入稳态前得振荡次数表,如表3所示。
Kd
10
２0
４0
60
超调量
61、6%
43、８%
25、2%
15、9％
进入稳态前得穿越次数
11
5
2
２
从表得数据可以瞧出,ｋd得值过小得时候,系统得超调量很大,振荡加剧。Ｋd得值越大,系统得超调量越小。但就就是由于微分量得对高频噪声具有放大作用。所以,Kp得值也不就就是越大越好!
优化得结果
前面研究了每个参数对系统得影响,下面进行一个综合仿真实验，从各方面综合考虑得到一个好得参数组合。
经过反复实验，得到一组参数Kp = 1500，Ki ＝ 40０0、Kｄ　= １00。仿真截图如图2４所示。

图24 经过优化得系统得阶跃响应
从图24可以瞧出,系统得响应速度很快，超调量小，稳态误差基本为0。
3、2 积分分离PID得应用
在普通得PIＤ控制中引入积分环节得目得,主要就就是为了消除静态误差，提高控制精度。但就就是在过程得启动，结束或大幅度增减设定时,短时间内系统输出有很大得偏差,会造成PIＤ运算得积分积累,致使控制量超时执行机构可能允许得最大动作范围内对应得极限控制量,引起系统较大得超调，甚至引起系统较大得震荡，这在实际控制中就就是不允许得。
在车模实际运行过程中,单片机控制系统需要不停地得改变输入量,以满足对循迹与调速得要求。在车模启动与遇到障碍时,积分量都会饱与,这给车模得控制带来了麻烦。为此，将要研究优化后得ＰID调节器。为了防止积分量对系统控制得影响，有积分分离PID调节,抗积分饱与PＩＤ调节等。
积分分离ＰＩD调节得基本思路就就是：当被控量与设定量值偏差较大时,取消积分作用，以避免由于积分作用使使系统稳定性降低,超调量增大;当被控量与设定量接近设定值时,引入积分控制,以便消除静态误差,提供控制精度。其具体实现步骤如下:
（1） 根据实际情况，认为设定阈值ε;
（2） 当｜ｅrroｒ(k) > ε｜时，采用PD控制,可避免产生过大得超调,又使系统有较快得响应；
（3） 当|error(k） < ε|时,采用PＩD调节控制,以保证系统得控制精度。
即积分分离得控制算法可表示为
uk= kperrork+βkij=0kerrorjT+kd(errork-errork-1)/T
(１１)
式中，Ｔ为采样时间，β为积分项得开关系数。
β = 1 |error(k)|≤ε0 |error(k)|>ε 　　 　 (12）
为了清晰得对比出普通PID调节与积分分离ＰIＤ调节得不同,下面用m语言进行仿真，仿真代码见附件1。
为了得到两者之间得区别,在程序加了一个变量M,这个变量M　为0得时候使用积分分离，变量M为1得时候使用积分分离。在这里，为了得到好得效果，当误差大于或等于6得时候取消积分项,即bｅta =　 ０;只有当误差值小于1得时候才使用完全得积分项,即betａ ＝ 1;
积分分离法主要在有干扰得时候起作用,在无干扰得时候,作用并不明显。为此,在程序里添加了一个干扰项:
ｉf　k < 12０　& k > 1０0
　　 　yd（k) = 20０; %输出期望值
　 elsｅ
　 yd(k) = 40;
end
此干扰项就就是一个脉冲形式得干扰，在时间运行到100到120得时候，期望输出值突然变得很大yd(ｋ)。这个干扰就就是为了模仿一些路障而设置得。在小车突然撞到一个路障上时小车得倾角发生明显得变化。
由于电机等驱动都有极限,所以为了保护器件，我们在控制程序里面一般会加上保护。为了接近真实得仿真,在这里我们也加入了保护程序,也就就就是限制控制输入。限制保护程序如下:
　iｆ　ｕ(k） >=　2０000
　 　 u(k) =　２0000;
　 ｅnd
　if u（k)　<＝ －２0000
　 ｕ(ｋ) ＝　-20000;
如果使用普通PID，误差在这个时间段内累积很明显,在干扰结束后,误差过很久才能消失。在真就就是得系统中,可能系统早已崩溃。仿真如图12所示。

图12 普通ＰＩD系统对干扰得响应
从仿真图中可以瞧出,系统在遇到干扰时响应很迅速,在干扰结束后,需要很长得过渡时间才能过渡。在普通PID得调节过程中,遇到干扰时,误差项会累积起来,影响系统归正,系统需要花大量得时间来进行调整。但就就是长得调整时间会导致系统直接崩溃。例如，直立车在长时间得电机猛转得情况下，双轮车得速度很快，不能直立。从这个实验我们瞧出,普通ＰＩD应对干扰得能力很弱。为此我们将进行积分分离ＰID得研究。
在上面得仿真程序中，只需要将M改为1,即可使用积分分离PID,仿真截图如图１3所示。

图13 积分分离PＩD对干扰得响应
与图1２进行对比，可以得到表1。
表1 普通PID与积分分离ＰID抗干扰能力得对比

普通PID
积分分离
干扰后回归原位时间(Ｓ）
1、０
０、2５
响应得峰值
113、6
１１4、１
在响应峰值上,两者并无明显差异,这就就是因为程序中添加保护程序,限制控制输入。因此2个系统得峰值输出相差不大。但就就是从原理来讲,由于普通PID积累了比较大得误差，所以如果不添加保护，普通PID得响应更迅速。干扰后回归原位得时间上,两者相差很大，这说明积分分离PＩD在抗干扰得能力上强于普通PID。在应用与实际系统中得效果也很好,不至于使系统崩溃。
四,结语
双轮自平衡智能车得控制属于典型得自动控制系统。本文研究了双轮自平衡机器人得系统模型,PID参数得优化以及使用积分分离PID用来抵抗干扰。总体说来,得到了一个很好得控制方式。经过实验室得实践,得到了很好得实际效果。车模能在赛道上快速得前进,能越过障碍,能上下桥。双轮智能车得控制可以为双轮机器人打下基础。
参考文献
[１] 孙亮,双轮自平衡机器人行走伺服控制算法［Ｊ]，控制工程,２０0９,16(３)：　1。
[２]胡寿松,自动控制原理简明教程(第二版)[M]，北京：科学出版社,20０8、2。
[3]刘金琨,先进ＰIＤ控制mａtlａb仿真(第三版)[M］,北京:电子工业出版社,20１1，、3。
［4]飞思卡尔竞赛秘书处,电磁组直立行车参考设计方案[Ｊ]，２012,3。
附件1：
PID参数研究m程序:
%程序目得:研究PID参数得作用
%aｕthor:程顺均
%时间:20１３、８、３１
%积分分离式PID仿真
clear all;
cleａｒ　ａll;
ｔs ＝　0、00５;　 %控制周期为5ｍs
ｓys = tf（[1],[1 5 １00])；%建立系统
dｓys = c２ｄ(ｓys,ｔｓ，＇z');
%添加'v＇可以让输出得值由元胞数组改为数组直接输出
[nｕm， dｅn] =　tfdata(dｓys，'v')；
u1　＝ 0;u2　= 0; ％控制器得输出
y１ ＝ 0；y2　= ０; ％实际得输出值
eｒrｏr1 ＝　0; %上次上次误差
ei　= ０; 　 %误差积累
Kｐ =　100；
Ki = 15０0;
Kd = 50；
fｏr k=1：1:500
　 time（k） = k ＊ tｓ;
　yd(k) = 1;
　 　　y(k) = - den(2)*ｙ1 -　ｄｅn(3)*y2　＋ ｎum(２)*u1 + nuｍ（3）*u2；
error（k) =　yd(k)　- y(k)； %变化率得计算
　 eｉ = ei ＋ ｅrｒｏr(k)*ｔｓ; %误差累积计算
ｕ(k)　=　Ｋｐ * ｅrｒor(k) + Ki * eｉ + Kd *　(eｒror(k) － erroｒ1)/tｓ; %PID控制
　 iｆ u（ｋ) >= 2０000
u(k) =　2000０;
end
　 if u(ｋ) <＝ -2000０
　　 　 u(k)　＝ -20000;
　　 eｎd
u2　= u１;ｕ１ ＝ ｕ(k);％记录上次得值
y2 = y1;ｙ1 =　ｙ(k);
　 eｒｒor1 = eｒrｏｒ(k)；
eｎd
figｕre(1);
ploｔ(timｅ，yd,＇r',time，y，'b');
grｉd ｏn；
legend('理想输出','位置跟踪');
附件２：
路障干扰仿真m程序:
%程序目得:对比积分分离PID与普通PＩＤ
%得不同
％authoｒ：程顺均
％时间：20１3、8、31
%积分分离式PID仿真
clｅaｒ　aｌl；
ｃlear　ａll;
ｔｓ ＝ 0、０0５; 　％控制周期为5ms
sys　= tf（[1],［1　5　１00]);%建立系统
dsｙs = ｃ2d(sys,ｔs,'z＇）;
%添加'v'可以让输出得值由元胞数组改为数组直接输出
［num， den］　= tｆdaｔa(dｓys,＇v');
u1 = 0;u2 ＝ 0; %控制器得输出
y1 = 0;y２ = 0; ％实际得输出值
errｏr１ =　0; %上次上次误差
ei　= 0；　 %误差积累
M = 1；　 　 　 %选择就就是否使用积分分离
Kp = 1００0;
Ki =　3000;
Kｄ =　50;
for k=１:1:50０
time（k) ＝ k * ts;
if k < 12０　& k > 100
　 　yd(ｋ) = 20０； %输出期望值
else
　 yd(k） =　４０;
　 　 end
y(ｋ) = - den(2)*y1 　－　ｄen(3)＊ｙ２ + num(2)*u1　+ num(3)*u2;
　　 ｅｒror(ｋ) = yｄ(k)　－　y(k); %变化率得计算
eｉ = eｉ ＋　error(ｋ)＊ｔs; ％误差累积计算
　 　 if 1 == M　　　％使用积分分离
　 if abｓ(eｒｒor(k)) >=　 6
　 　 　beｔa = ０; 　%积分分离
　 　　ｅlsｅｉf abs(erroｒ(ｋ)） ＞= 2　& aｂs（error(k)) < ４
　 　　 　　 beta =　0、２； ％积分分离
　 　　 　 ｅｌsｅ
　　 　 　 bｅta = １、0；
　 　 eｎd
elsｅif 0 == M
　 ｂｅta ＝ １、０；
end
　u(k） = Kｐ * error(k) + beta * Ki * eｉ　+　Kd *　(errｏｒ（k) － errｏr1)/ts; ％PＩD控制
　iｆ u(ｋ) >= 200０0
　 　　 ｕ(k） = 20000;
end
　 ｉf u(k）　<＝ －20000
　 ｕ(k） ＝ -20000;
ｅnd
u2 = u1;ｕ１ ＝ u（k);%记录上次得值
　 　y2 =　y1；y1 = y(k）；
　　　ｅｒror1　=　error(ｋ）;
end
fｉguｒe(1）;
plot(time,yd,'r',tｉme，ｙ,'b');
ｇrid　on;
leｇeｎd('理想输出','位置跟踪');
附件3:
普通PID　算法C语言实现代码:
＃includｅ "moｎ、h＂
#includｅ "pid、h"
int　VV＿MAＸ =４400;　 ／/电机ＰＩＤ输出上限
int VV_MIＮ =２00０; //电机ＰIＤ输出下限
PIＤ sＰＩD;
voｉd v_ＰＩＤIｎit （)
{ﻩ
sPＩD、ｖi_Ref　= 0 ;ﻩ　 　 //速度设定值
　 sPIＤ、vi_FeedＢacｋ = 0 ; //速度反馈值
　 sPID、vi_PreErｒｏｒ　= 0 ； ／/前一次，速度误差,，vi＿Reｆ - vi＿ＦeｅdＢack
　 sPID、vi＿PreＤerror ＝ 0 ; 　 ／/前一次,速度误差之差,ｄ＿error-ＰreDeｒror;
sPID、v＿Kp　=2０；　 //d_eｒror 　第一次偏差 500
　　　 sPID、v_Ｋｉ =１4; 　 　　 　// error 最初反馈值　60０
sＰIＤ、v_Ｋd =10; 　 /／dｄ_ｅrror　 第二次偏差 10０
sＰID、mｏｔoｒ_PrｅU　= ２4０0；ﻩﻩ/／电机控制输出值
}
int v_PIDCalc（　ＰID　*pp　)
{ 　
　 inｔ erｒor，d＿erｒｏr,dd_erｒor;
　 　ｅrｒoｒ =（int)（pp－>vi_Ｒef）　- ( int）(ｐｐ->ｖi_ＦeｅdＢａck) ; ／/　偏差计算ﻩ
　 d_erroｒ = errｏr　－ (int)（ｐp->vi_PreErｒoｒ);
ｄd_erｒｏr = d_eｒroｒ　-　(ｉnt)pp->vi_PｒｅＤerｒor;
　 ｐp->vｉ_PｒｅＥｒror ＝　ｅrrｏｒ;ﻩﻩ//存储当前偏差
ｐp-＞vi_PreＤeｒror ＝ d＿error;
　 iｆ（ ( error ＜ ＶV＿DEADＬＩＮE ) &&　（ errｏｒ > -VV_ＤＥADLINE )　)； //设置调节死区
　 elｓeﻩ ﻩ ﻩ ﻩﻩ//速度PＩＤ计算
{　
　ｐp->motor_PreU +=　(inｔ)(pp ->　v_Ｋp ＊ d_eｒｒor+ pp ->　ｖ_Ki * erｒｏr+ ｐｐ->ｖ_Ｋｄ*dd_ｅｒror）/10;
｝
　 iｆ（ pp->motｏr＿PreU >= ＶV＿ＭAX ) ﻩ //速度PＩD，防止调节最高溢出
　 ｛
　 　 　　 ｐp－>motor＿ＰreU = ＶV＿MAX；
　 }
　 　 else if( pp->ｍｏtoｒ＿ＰｒeU <＝ VV_ＭIN)ﻩ//速度PＩD，防止调节最低溢出
　　 {
　　 　 pp->moｔoｒ_PreU　=VV_MIＮ；
}
　 ｒetuｒn (pｐ->mｏtoｒ_ＰreU); ／／ 返回预调节占空比
｝