无人驾驶（pid算法）
【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】
对于工科的同学来说，最简单的自动控制代码其实就是PID。在现实的生产中，最难控制的往往是输入和输出之间的关系。很多时候，系统的输入和输出是很复杂的关系，根本没有办法用方程来表示，或者说无法用方程来准确表示。这个时候，PID就可以发挥用处。PID非常简单，主要就是比例系数、积分系数和微分系数。我们可以用一个简单的代码来说明这个问题，
#!/usr/bin/python
import os
import sys
import re
import time
import signal
'''
pid data
'''
Kp=0.2
Ki=0.015
Kd=0.2
Acc=0.0
PrevErr=0.0
Target=200.0
Sample=10
g_exit = 0
'''
read data from sensor, we can see the relationship between pid and perceptron is 1:1
'''
def get_from_sensor(data):
return data
'''
pid process
'''
def pid_process(data):
global Acc
global PrevErr
Acc+= Target - data
data = Kp*(Target - data) + Ki * Acc + Kd * (Target- data - PrevErr)
PrevErr = Target - data
print data
return data
'''
process data, this is just to simulate some digital to analog process
'''
def process_data(data):
pass
'''
signal process
'''
def sig_process(sig, data):
global g_exit
g_exit=1
'''
file entry starts here
'''
def main():
start = 0
signal.signal(signal.SIGINT, sig_process)
while not g_exit:
sensordata = get_from_sensor(start)
data = pid_process(sensordata)
process_data(data)
start = data
time.sleep(1.0/Sample)
if __name__ == '__main__':
main()
如果大家运行一下，你大概会得到这样的结果，
ubuntu>$ python pid.py
83.0
28.155
41.701675
41.366814875
43.8132846769
45.6667914463
47.6110882208
49.5080625426
51.3860467401
53.2396591995
55.0703418196
56.8781501683
58.663416246
60.4264117868
62.1674165018
63.8867043113
65.5845461847
67.2612096173
68.9169587865
70.5520545709
72.1667545954
73.7613132716
75.3359818373
76.8910083966
78.4266379588
79.9431124769
81.4406708862
82.919549141
84.3799802529
85.8221943268
87.2464185971
88.6528774641
90.0417925287
91.4133826278
92.7678638686
94.1054496624
95.4263507587
96.7307752781
98.018928745
99.2910141205
100.547231834
101.787779813
103.01285352
104.222645976
105.417347795
106.597147215
107.762230122
108.912780089
110.048978394
111.171004057
112.279033864
113.373242395
114.453802053
115.52088309
116.574653636
117.615279723
118.64292531
119.657752312
120.659920627
121.649588155
122.626910827
123.59204263
124.54513563
125.486339996
126.415804023
127.333674157
128.240095018
129.13520942
130.019158398
130.892081227
131.754115443
132.605396868
133.44605963
134.276236183
135.09605733
135.90565224
136.705148475
137.494672001
138.274347215
139.044296964
139.80464256
140.555503802
141.296998997
142.029244973
142.752357103
143.466449321
144.171634137
144.868022662
145.555724617
146.234848357
146.905500884
147.567787865
148.221813651
148.867681289
149.505492542
150.135347903
150.757346612
151.371586671
151.978164859
152.577176749
153.16871672
153.752877975
154.329752554
154.89943135
155.462004121
156.017559505
156.566185035
157.107967154
157.642991223
158.17134154
158.693101354
159.208352871
159.717177274
160.219654735
160.715864421
161.205884518
161.689792231
162.167663805
162.639574533
163.105598769
163.56580994
164.020280557
164.469082225
164.912285658
165.349960687
165.782176271
166.20900051
166.630500655
167.046743116
167.457793477
167.863716502
168.26457615
168.660435578
169.051357159
169.437402485
169.818632383
170.195106917
170.566885407
170.934026428
171.296587827
171.65462673
172.008199548
172.357361991
172.702169073
173.042675121
173.378933784
173.710998045
174.038920222
174.362751983
174.682544351
174.998347712
175.310211824
175.618185825
175.922318237
176.222656981
176.519249378
176.812142158
177.101381469
177.387012885
177.669081409
177.947631483
178.222706996
178.494351288
178.76260716
179.027516878
179.289122182
179.547464288
179.802583903
180.054521221
180.303315939
180.549007256
180.791633884
181.03123405
181.267845506
181.501505533
181.732250944
181.960118098
182.185142896
182.407360793
182.626806801
182.843515498
183.057521026
183.268857105
183.477557032
183.683653691
183.887179554
184.088166688
184.286646761
184.482651045
184.676210423
184.867355391
185.056116066
185.24252219
185.426603133
185.608387897
185.787905126
185.965183103
186.140249761
186.313132683
186.483859109
186.652455937
186.818949732
186.983366727
187.145732827
187.306073615
187.464414353
187.62077999
187.775195163
187.927684201
188.07827113
188.226979677
188.373833273
188.518855055
188.662067872
188.803494291
188.943156593
189.081076784
189.217276594
189.351777483
189.484600643
189.615767001
189.745297224
189.873211721
189.999530646
190.124273901
190.247461142
190.369111777
190.489244973
190.607879659
190.725034527
190.840728036
190.954978413
191.067803662
191.179221557
191.289249655
191.39790529
191.505205584
191.611167441
191.715807558
191.819142421
191.921188312
192.02196131
192.12147729
192.219751935
192.316800727
192.412638958
192.507281727
192.600743947
192.693040344
192.78418546
192.874193655
192.963079111
193.050855833
193.137537651
193.223138223
193.307671035
193.391149407
193.473586491
193.554995277
193.635388591
193.714779099
193.793179311
193.870601579
193.947058102
194.022560926
194.097121947
194.170752914
194.243465427
194.315270943
194.386180775
194.456206097
194.525357941
194.593647203
194.661084643
194.727680885
194.793446424
194.85839162
194.922526706
194.985861788
195.048406845
195.110171731
195.171166178
195.231399796
195.290882075
195.349622388
195.40762999
195.46491402
195.521483503
195.577347354
195.632514374
195.686993254
195.740792579
195.793920825
195.846386364
195.898197461
195.949362279
195.999888881
196.049785228
196.09905918
196.147718502
196.19577086
196.243223826
196.290084875
196.336361392
196.382060668
196.427189903
196.471756207
196.515766603
196.559228025
196.60214732
196.644531251
196.686386496
196.72771965
196.768537224
196.808845651
196.848651281
196.887960386
196.926779159
196.965113717
197.0029701
197.040354272
197.077272123
197.113729471
197.149732059
197.185285561
197.220395577
197.25506764
197.289307213
197.32311969
197.356510399
197.389484602
197.422047492
197.454204202
197.485959797
197.517319281
197.548287595
197.578869618
197.609070169
197.638894006
197.668345829
197.697430277
197.726151933
197.754515323
197.782524915
197.810185123
197.837500304
197.864474764
197.89111275
197.917418462
197.943396042
197.969049586
197.994383133
198.019400677
198.044106158
198.068503469
198.092596455
198.116388911
198.139884586
198.163087182
198.186000355
198.208627715
198.230972828
198.253039213
198.274830348
198.296349665
198.317600557
198.33858637
198.359310412
198.379775947
198.399986201
198.419944357
198.439653561
198.459116917
198.478337492
198.497318314
198.516062375
198.534572627
198.552851988
198.570903336
198.588729516
198.606333337
198.623717573
198.640884962
198.65783821
198.674579987
198.691112932
198.707439649
198.723562711
198.739484658
198.755207999
198.77073521
198.78606874
198.801211003
198.816164385
198.830931243
198.845513903
198.859914662
198.87413579
198.888179528
198.902048088
198.915743654
198.929268386
198.942624414
198.955813842
198.968838749
198.981701186
198.994403181
199.006946734
199.019333823
199.031566398
199.043646387
199.055575693
199.067356197
199.078989753
199.090478195
199.101823334
199.113026956
199.124090827
199.135016691通过输出，我们会发现数据会慢慢靠向200，这也是我们最终希望标定的数值。当然，这只是一个基础的逻辑，这个200可以是距离、温度、压力，或者是任何其他有意义的物理量。
整个代码的逻辑也不复杂，首先我们从get_from_sensor获取传感器值。为了简单，这里仅考虑输出量和被控制量是1:1的线性关系。接着，代码将sensordata作为入参传递给pid_process函数，而pid_process就是整个功能的主要部分。最后，从pid处理函数获得输出值，一般会传向放大机构或者执行机构，这样就可以完成所有的自动操作过程。
当然，这只是一个基础的PID控制代码，实际生产中还需要对它进行容错和优化考虑，比如如果让自动控制更快更准，而且要振荡更小。建议大家多看看下面这篇blog，https://blog.csdn.net/u010312937/article/details/53363831，肯定会对自己大有裨益。等到自己对PID比较熟悉的时候，也可以多多研究双闭环PID控制方法。
PS:
PID多用于电机控制。电机控制是嵌入式很大的一个部分，通常一般用双PID闭环或者三PID闭环来解决。如果是双PID闭环，外部循环是速度偏差，内部是电流偏差，输出PWM。如果是三PID控制，那么从外到内分别是距离、速度和电流偏差，一般把电流和扭矩看是成等效的。