-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathatom.xml
1980 lines (1770 loc) · 388 KB
/
atom.xml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
<?xml version="1.0" encoding="utf-8"?>
<feed xmlns="http://www.w3.org/2005/Atom">
<title>梦的小窝</title>
<link href="/atom.xml" rel="self"/>
<link href="https://rm-rf.moe/"/>
<updated>2018-05-25T06:22:25.000Z</updated>
<id>https://rm-rf.moe/</id>
<author>
<name>Hsmouc</name>
</author>
<generator uri="http://hexo.io/">Hexo</generator>
<entry>
<title>水下机器人航向/轨迹跟踪控制系统设计--滑模控制</title>
<link href="https://rm-rf.moe/2018/05/25/2018-05-25-SMC/"/>
<id>https://rm-rf.moe/2018/05/25/2018-05-25-SMC/</id>
<published>2018-05-25T04:24:12.000Z</published>
<updated>2018-05-25T06:22:25.000Z</updated>
<content type="html"><![CDATA[<p>本文给出滑模控制在水下机器人运动控制方面的两个实例，主要参考JJE Slotine的著作<em>Applied Nonlinear Control</em>。</p>
<a id="more"></a>
<h1 id="基础知识">基础知识</h1>
<h2 id="流形的选择与证明">流形的选择与证明</h2>
<p>使<span class="math inline">\(\widetilde{x} = x - x_d\)</span>作为变量<span class="math inline">\(x\)</span>的跟踪误差，并使 <span class="math display">\[\widetilde{x}=x-x_d=
\left[
\begin{matrix}
\widetilde{x} & \dot{\widetilde{x}}& \cdots & \widetilde{x}^{(n-1)} \\
\end{matrix}
\right]^T\]</span> 定义一个在状态空间<span class="math inline">\(R^{(n)}\)</span>中时变的平面<span class="math inline">\(s(x;t)=0\)</span>，其中： <span class="math display">\[s(x;t) = (\frac{d}{dt}+\lambda)^{n-1}\widetilde{x}\]</span> 式中的<span class="math inline">\(\lambda\)</span>是一个正常数。若<span class="math inline">\(x_d(0)=x(0)\)</span>，那么使得<span class="math inline">\(x \equiv x_d\)</span>的跟踪问题等价于对任意<span class="math inline">\(t > 0\)</span>，状态变量保持在流形<span class="math inline">\(s(t)\)</span>上。即<span class="math inline">\(n\)</span>阶向量<span class="math inline">\(x_d\)</span>的跟随问题可以被简化为使标量<span class="math inline">\(s\)</span>保持为0。且系统一旦到达流形，系统就将保持在流形上。<br>
以一流形为例： <span class="math display">\[s = \dot{\widetilde{x}}+\lambda\widetilde{x}\]</span> 系统到达流形上后，由<span class="math inline">\(s\equiv0\)</span>，则有 <span class="math display">\[\dot{\widetilde{x}} = -\lambda \widetilde{x}\]</span> 上式表明系统将渐进趋近零点。<br>
另外，标量<span class="math inline">\(s\)</span>的界可以转化为对跟踪误差<span class="math inline">\(\widetilde{x}\)</span>的界，所以可认为标量<span class="math inline">\(s\)</span>提供了一个对跟踪性能的可靠量度。特别的，假设<span class="math inline">\(\widetilde{x}(0)=0\)</span>，则有： <span class="math display">\[
\forall t \geq 0, |s(t)| \leq \Phi \quad \Longrightarrow \quad
\forall t \geq 0,|\widetilde{x}^{(i)}(t)| \leq (2\lambda)^i \varepsilon \qquad \qquad \qquad i=0,\cdots, n-1
\]</span><br>
<strong>证明：</strong> <span class="math display">\[y_1(t) = \int_0^t e^{-\lambda(t-T)}s(T)dT\]</span> <img src="https://ws1.sinaimg.cn/large/005WMcFzly1frnhncvas9j30na03gmx8.jpg" alt="辅助说明"> 由<span class="math inline">\(|s|\leq \Phi\)</span>，可得： <span class="math display">\[|y_1 (t)|\leq \Phi \int_0^t e^{-\lambda(t-T)}dT =
(\frac{\Phi}{\lambda})(1-e^{-\lambda t})
\leq \frac{\Phi}{\lambda}\]</span> 依次类推，可以得到： <span class="math display">\[|\widetilde{x}| \leq \frac{\Phi}{\lambda^{n-1}}=\varepsilon \]</span> 用类似的方式可以得到<span class="math inline">\(\widetilde{x}^{(i)}\)</span>与<span class="math inline">\(s\)</span>的关系。假定标量<span class="math inline">\(s\)</span>通过了<span class="math inline">\(n-i-1\)</span>个低通滤波器得到了变量<span class="math inline">\(v_1\)</span>，则按上述证明可知： <span class="math display">\[|v_1| \leq \frac{\Phi}{\lambda^{n-i-1}} \]</span> 构造滤波器<span class="math inline">\(G_f\)</span>：<span class="math inline">\(\frac{r}{r+\lambda}\)</span>，<span class="math inline">\(v_1\)</span>经过一次<span class="math inline">\(G_f\)</span>得到的结果<span class="math inline">\(v_2\)</span>。上述过程可记为： <span class="math display">\[v_2 (t) = v_1 (t) - v_1 (t)\lambda \int_0^t e^{-\lambda(t-T)}dT\]</span> 由此可得： <span class="math display">\[v_2 (t) = (1+\frac{\lambda}{\lambda}) v_1 (t) \]</span> 经过<span class="math inline">\(i\)</span>次<span class="math inline">\(G_f\)</span>，将得到<span class="math inline">\(\widetilde{x}^*(i)\)</span>，因此联立两式可得： <span class="math display">\[|\widetilde{x^i}| \leq (\frac{\Phi}{\lambda^{n-1-i}})(1+\frac{\lambda}{\lambda})^i = (2\lambda)^i \varepsilon \]</span></p>
<h2 id="有限时间到达流形的证明">有限时间到达流形的证明</h2>
<p>条件： <span class="math display">\[\frac{1}{2}\frac{d}{dt} s^2 \leq -\eta |s| \]</span> 其中<span class="math inline">\(\eta\)</span>是一个严格大于零的常数。满足以上条件则表明系统将在有限时间到达流形。 <strong>证明：</strong><br>
令: <span class="math display">\[v = s^2 (s \neq 0)\]</span> 则： <span class="math display">\[\dot{v} = 2s\dot{s} < -2\eta |s| = -2\eta\sqrt{v}\]</span> 等价于： <span class="math display">\[\frac{dv}{\sqrt{v}} < -2\eta dt\]</span> 对上式积分可得： <span class="math display">\[\int_{v(0)}^{v(t)}\frac{dv}{v} < -2\eta \int_0^t dt\]</span> 结果为： <span class="math display">\[\sqrt{v(t)}-\sqrt{v(0)} < -\eta t\]</span> 由上式可得： <span class="math display">\[0 < \sqrt{v(t)} < \sqrt{v(0)}- \eta t\]</span> 由此可以推导出： <span class="math display">\[\sqrt{v(0)}-\eta t > 0 \Longrightarrow t < \frac{\sqrt{v(0)}}{\eta}\]</span></p>
<h1 id="被控对象模型">被控对象模型</h1>
<h2 id="单自由度艏摇模型">单自由度艏摇模型</h2>
<p>水下机器人的运动模型可用以下形式表述： <span class="math display">\[\ddot{x} = -\frac{c}{m}\dot{x}|\dot{x}| + \frac{1}{m}u\]</span> 对航向控制来说，<span class="math inline">\(u\)</span>表示推进器的输入扭矩，<span class="math inline">\(x\)</span>表示当前指向的角度。 ## 平面三自由度模型 在进行平面轨迹跟踪控制系统设计之前，首先要将前文所述的单自由度模型扩展到平面三自由度，如下图： <img src="https://ws1.sinaimg.cn/mw690/005WMcFzly1frni2t0s2cj30nr0km3zp.jpg" width="300" alt="三自由度示意图" align="center"></p>
<p>图中的<span class="math inline">\(p_1,p_2\)</span>代表推进器，平面内大地坐标和体坐标的速度转换关系如下： <span class="math display">\[
\left\{ \begin{array}{l}
\dot{x} = v_x cos(\theta) - v_y sin(\theta) \\
\dot{y} = v_x sin(\theta) + v_y cos(\theta) \\
\dot{\theta} = \omega
\end{array} \right. \label{cons:cvtnb}
\]</span> 式中的坐标<span class="math inline">\((x,y)\)</span>表示水下机器人质心在大地坐标系下的位置，<span class="math inline">\(\theta\)</span>是相对于大地坐标系机器人的转向角。<span class="math inline">\(v_x,v_y,\omega\)</span>分别是其体坐标系下的纵荡，横荡与艏摇速度。平面内机器人运动除有前文已导出的模型，还有： <span class="math display">\[
\left \{ \begin{array}{l}
M_{x}\dot{v_x} - M_{y}v_y \omega + c_x v_x |v_x| = u \\
M_{y}\dot{v_y} + M_{x}v_x \omega + c_y v_y |v_y| = 0 \\
\end{array} \right. \label{cons:3dof}
\]</span> 式中，<span class="math inline">\(M_{x}\)</span>与<span class="math inline">\(M_{y}\)</span>都包括刚体质量和附加质量，尽管在纵荡方向和垂荡方向水下机器人质量没有变化，但二者附加质量的差异导致了<span class="math inline">\(M_{x} \neq M_{y}\)</span>。其纵荡和横荡的阻尼也存在差异。</p>
<h1 id="控制律推导">控制律推导</h1>
<p>由改写的水下机器人动力学方程，二阶系统可描述为： <span class="math display">\[
\ddot{x} = f + bu
\]</span> 式中<span class="math inline">\(f,b\)</span>均不能精确测得，但是均已知参数摄动范围。其中<span class="math inline">\(b\)</span>的范围是： <span class="math display">\[
0 < b_{min} \leq b \leq b_{max}
\]</span> 且认为<span class="math inline">\(b\)</span>的最优估计是: <span class="math display">\[
\widehat{b} = \sqrt{b_{min} b_{max}}
\]</span> 所以<span class="math inline">\(b\)</span>的边界条件也可写作： <span class="math display">\[
\beta^{-1} \leq \frac{\widehat{b}}{b} \leq \beta
\]</span> <span class="math inline">\(f(x;t)\)</span>的摄动可认为被已知函数<span class="math inline">\(F(x;t)\)</span>限制，且设对动态的<span class="math inline">\(f\)</span>最优估计为<span class="math inline">\(\widehat{f}\)</span>： <span class="math display">\[
|\widehat{f}-f|\leq F
\]</span> 对输入跟踪问题<span class="math inline">\(x(t) \equiv x_d(t)\)</span>，定义一个流形： <span class="math display">\[
s = (\frac{d}{dt}+\lambda)\widetilde{x} = \dot{\widetilde{x}}+\lambda \widetilde{x} \label{cons:yawsf}
\]</span> 可以推出： <span class="math display">\[
\dot{s} = \ddot{\widetilde{x}}+\lambda \dot{\widetilde{x}} = \ddot{x}-\ddot{x}_d+\lambda \dot{\widetilde{x}}
\]</span> 又由<span class="math inline">\(\ddot{x} = f + bu\)</span>，则： <span class="math display">\[
\dot{s} = f + bu - \ddot{x}_d + \lambda \dot{\widetilde{x}}
\]</span> 因此，使得<span class="math inline">\(\dot{s}=0\)</span>的最优估计<span class="math inline">\(\widehat{u}\)</span>为： <span class="math display">\[
\widehat{u} = \widehat{b}^{-1}(-\widehat{f}+\ddot{x}_d-\lambda\dot{\widetilde{x}}) \label{cons:estu}
\]</span> 为满足滑动模态存在条件，在输入<span class="math inline">\(\widehat{u}\)</span>上需要叠加一个非线性输入。记为： <span class="math display">\[
u = \widehat{b}^{-1}[\widehat{u}-k sgn(s)] \label{cons:u}
\]</span> 式中<span class="math inline">\(sgn(s)\)</span>为符号函数。 <span class="math display">\[
\left\{ \begin{array}{ll}
sgn(s) = +1 \qquad\qquad if \quad s>0 \\
sgn(s) = -1 \qquad\qquad if \quad s<0
\end{array} \right.
\]</span> 需选择足够大的<span class="math inline">\(k\)</span>才能保证满足滑动模态存在条件。</p>
<p>联立，得： <span class="math display">\[
\dot{s} = f + b\widehat{b}^{-1}(-\widehat{f}+\ddot{x}_d-\lambda\dot{\widetilde{x}}-k sgn(s))-\ddot{x}_d+\lambda \dot{\widetilde{x}}
\]</span> 整理得： <span class="math display">\[
\dot{s} = f-b\widehat{b}^{-1}\widehat{f}+(1-b\widehat{b}^{-1})(-\ddot{x}_d+\lambda \dot{\widetilde{x}})-b\widehat{b}^{-1}k sgn(s)
\]</span> 又因为： <span class="math display">\[
s\dot{s} = [f-b\widehat{b}^{-1}\widehat{f}+(1-b\widehat{b}^{-1})(-\ddot{x}_d+\lambda \dot{\widetilde{x}})]s-b\widehat{b}^{-1}k|s| \leq -\eta |s|
\]</span> 所以<span class="math inline">\(k\)</span>必须满足： <span class="math display">\[
k \geq |\widehat{b}b^{-1}f-\widehat{f}+(\widehat{b}b^{-1}-1)(-\ddot{x}_d+\lambda \dot{\widetilde{x}})|+\eta\widehat{b}b^{-1}
\]</span> 由<span class="math inline">\(\beta > \widehat{b}b^{-1}\)</span>： <span class="math display">\[
\begin{array}{ll}
\Longrightarrow k \geq |\beta f - \widehat{f} + (\beta -1)(-\ddot{x}_d+\lambda\dot{\widetilde{x}})|+\eta \beta\\
\Longrightarrow k \geq |f + (\beta -1)f - \widehat{f}+(\beta -1)(-\ddot{x}_d+\lambda\dot{\widetilde{x}})|+\eta \beta\\
\Longrightarrow k \geq |(f - \widehat{f})+(\beta -1)f +(\beta -1)(-\ddot{x}_d+\lambda\dot{\widetilde{x}})|+\eta \beta
\end{array}
\]</span> 又因为<span class="math inline">\(|f-\widehat{f}|\leq F\)</span>，可导出： <span class="math display">\[
k \geq |F+(\beta -1)f +(\beta -1)(-\ddot{x}_d+\lambda\dot{\widetilde{x}})|+\eta \beta
\]</span> 对于前文所述的水下机器人动力学模型：<span class="math inline">\(f = -\frac{c}{m}\dot{x}|\dot{x}|\)</span>，由于流体阻力与机器人运动方向相反，所以<span class="math inline">\(f\leq 0\)</span>。又<span class="math inline">\(\beta-1 \geq 0\)</span>，所以： <span class="math display">\[
k \geq (F+\beta \eta) + (\beta -1)|\ddot{x}_d-\lambda \dot{\widetilde{x}}| \label{cons:k}
\]</span> 对水下机器人运动学方程： <span class="math display">\[
\left\{\begin{array}{ll}
f = -\frac{\widehat{c}}{\widehat{m}}\dot{x}|\dot{x}| \\
b = \frac{1}{\widehat{m}}
\end{array}\right.
\]</span> 代入前式，得到航向角滑模控制律应为： <span class="math display">\[
u = \widehat{c}\dot{x}|\dot{x}|+\widehat{m}(\ddot{x}_d-\lambda\dot{\widetilde{x}})- \widehat{m}ksgn(s) \label{cons:sf1}
\]</span> 其中<span class="math inline">\(k\)</span>应该满足： <span class="math display">\[
k \geq (F+\beta \eta) + (\beta -1)|\ddot{x}_d-\lambda \dot{\widetilde{x}}| \label{cons:sf2}
\]</span> 类似地，导出轨迹跟踪滑模控制律为：<br>
流形： <span class="math inline">\(s=\widetilde{v}_x + \lambda \int_0^t \widetilde{v}_x(\tau)d\tau\)</span>： <span class="math display">\[
u = -\widehat{M}_y v_y \omega + c_x v_x |v_x| - \widehat{M}_x\dot{s_r} - ksgn(s)
\]</span> 其中： <span class="math display">\[
k = \beta_y |v_y \omega| + F + \beta_x |\dot{s_r}| + \widehat{M}_x\eta
\]</span></p>
<h1 id="仿真">仿真</h1>
<h2 id="航向角控制">航向角控制</h2>
<p><img src="https://ws1.sinaimg.cn/large/005WMcFzly1frnimh3gs5j312m0j7411.jpg" alt="heading control"> <figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">function</span> <span class="title">u</span> = <span class="title">smc</span><span class="params">(k, ddot_x_d, e_dot, dot_x, s, lambda)</span></span></div><div class="line"> u = <span class="number">1.352</span>*(ddot_x_d-lambda*e_dot)+<span class="number">0.87</span>*dot_x*<span class="built_in">abs</span>(dot_x)<span class="number">-1.352</span>*k*<span class="built_in">sign</span>(s)<span class="comment">%sat(s,20); </span></div><div class="line"><span class="comment">%#codegen</span></div></pre></td></tr></table></figure></p>
<h2 id="轨迹跟踪控制">轨迹跟踪控制</h2>
<p><img src="https://ws1.sinaimg.cn/large/005WMcFzly1frnimwj71wj30vh0j50vj.jpg" alt="trajectory tracking control"> <figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">function</span> <span class="title">u</span> = <span class="title">smc_surge</span><span class="params">(v_y,omega,v_x_d,v_x_dd,x_d,lambda,k,x,v_x)</span></span></div><div class="line"><span class="comment">%#codegen</span></div><div class="line"> u = <span class="number">-42.951</span>*v_y*omega+<span class="number">25</span>*v_x^<span class="number">2</span>+<span class="number">5.379</span>*v_x<span class="number">-34.675</span>*(-v_x_dd+lambda*(v_x-v_x_d))-k*<span class="built_in">sign</span>(v_x-v_x_d+lambda*(x-x_d));</div></pre></td></tr></table></figure></p>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">function</span> <span class="title">u</span> = <span class="title">fcnK</span><span class="params">(v_y,omega,v_x_dd,v_x_d,eta,lambda,v_x)</span></span></div><div class="line">u = <span class="number">1.2</span>*<span class="built_in">abs</span>(v_y*omega)+v_x^<span class="number">2</span>+<span class="number">1.2</span>*<span class="built_in">abs</span>(-v_x_dd+lambda*(v_x-v_x_d))+<span class="number">34.675</span>*eta;</div></pre></td></tr></table></figure>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">function</span> <span class="params">[v_x,v_y]</span> = <span class="title">fcnT</span><span class="params">(theta,v)</span></span></div><div class="line">v_x = v*<span class="built_in">cos</span>(theta);</div><div class="line">v_y = v*<span class="built_in">sin</span>(theta);</div></pre></td></tr></table></figure>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">function</span> <span class="title">y</span> = <span class="title">fcnD</span><span class="params">(dx,dy)</span></span></div><div class="line"><span class="comment">%#codegen</span></div><div class="line">y = <span class="built_in">sqrt</span>(dx^<span class="number">2</span>+dy^<span class="number">2</span>);</div></pre></td></tr></table></figure>
<h2 id="跟踪轨迹设置">跟踪轨迹设置</h2>
<p>对给定的时变跟踪轨迹曲线可用如下参数方程描述： <span class="math display">\[
\left\{ \begin{array}{l}
x = \phi(t) \\
y = \psi(t)
\end{array} \right.
\]</span> 纵荡滑模控制的设定曲线为弧长<span class="math inline">\(l\)</span>与时间<span class="math inline">\(t\)</span>的关系，有： <span class="math display">\[
ds = \sqrt{(dx)^2+(dy)^2} = \sqrt{\dot{\phi}^2 (t)+\dot{\psi}^2 (t)} dt
\]</span> 弧长为： <span class="math display">\[
s = \int_0^t \sqrt{\dot{\phi}^2 (t)+\dot{\psi}^2 (t)} dt
\]</span> 航向角滑模控制的设定曲线为角度<span class="math inline">\(\theta\)</span>与时间<span class="math inline">\(t\)</span>的关系，有： <span class="math display">\[
\theta = arctan(\frac{dy}{dx})
\]</span> <figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div><div class="line">15</div><div class="line">16</div><div class="line">17</div></pre></td><td class="code"><pre><div class="line"><span class="comment">%% Input Signal Generator</span></div><div class="line"><span class="comment">% By heshiming</span></div><div class="line"><span class="comment">%</span></div><div class="line"><span class="comment">%% for generate input signal</span></div><div class="line">t = <span class="built_in">linspace</span>(<span class="number">0</span>,<span class="number">10</span>,<span class="number">15</span>);</div><div class="line">input = <span class="number">2</span>*<span class="built_in">ones</span>(<span class="number">1</span>,<span class="number">15</span>);</div><div class="line"><span class="keyword">for</span> num = <span class="built_in">linspace</span>(<span class="number">1</span>,<span class="number">15</span>,<span class="number">15</span>)</div><div class="line"> input(num) = <span class="number">0.1</span>*t(num);</div><div class="line"><span class="keyword">end</span></div><div class="line">c = spcrv([input(<span class="number">1</span>) input input(end)],<span class="number">3</span>,<span class="number">50000</span>)';</div><div class="line">time = <span class="built_in">linspace</span>(<span class="number">0</span>,<span class="number">10</span>,<span class="built_in">length</span>(c))';</div><div class="line">T = <span class="number">10</span>/<span class="built_in">length</span>(c);tau = <span class="number">0.3</span>;</div><div class="line">a = T/tau;</div><div class="line">x_filt = filter(a, [<span class="number">1</span> a<span class="number">-1</span>],c);</div><div class="line">y_filt = <span class="built_in">cos</span>(x_filt.^<span class="number">2</span>)<span class="number">-1</span>;</div><div class="line">x_t = [time(<span class="number">3</span>:end),x_filt(<span class="number">3</span>:end)];</div><div class="line">y_t = [time(<span class="number">3</span>:end),y_filt(<span class="number">3</span>:end)];</div></pre></td></tr></table></figure></p>
<h2 id="参数摄动下的航向角控制">参数摄动下的航向角控制</h2>
<h3 id="航向角随时间的变化情况">航向角随时间的变化情况</h3>
<figure>
<img src="https://ws1.sinaimg.cn/large/005WMcFzly1frnk34jq2dj30oi0f1aav.jpg" alt="航向角响应曲线"><figcaption>航向角响应曲线</figcaption>
</figure>
<h3 id="控制器输出">控制器输出</h3>
<p>当控制律中含符号函数时，控制器的输出情况： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1frnk9bjvgfj30ol0f474s.jpg" alt="含符号函数控制器的输出"> 将符号函数更换为饱和函数，以消除控制器抖动： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1frnkaqtvzzj30oi0f1aak.jpg" alt="含饱和函数控制器的输出"></p>
<h2 id="轨迹跟踪情况">轨迹跟踪情况</h2>
<figure>
<img src="https://ws1.sinaimg.cn/large/005WMcFzly1frnk5i0wryj30og0f00ti.jpg" alt="轨迹跟踪"><figcaption>轨迹跟踪</figcaption>
</figure>
]]></content>
<summary type="html">
<p>本文给出滑模控制在水下机器人运动控制方面的两个实例，主要参考JJE Slotine的著作<em>Applied Nonlinear Control</em>。</p>
</summary>
<category term="Control Theory" scheme="https://rm-rf.moe/categories/Control-Theory/"/>
<category term="Sliding Mode Control" scheme="https://rm-rf.moe/tags/Sliding-Mode-Control/"/>
</entry>
<entry>
<title>解决在macOS Sierra(10.12)上MATLAB的mex问题</title>
<link href="https://rm-rf.moe/2017/12/01/2017-12-01-fix-matlab/"/>
<id>https://rm-rf.moe/2017/12/01/2017-12-01-fix-matlab/</id>
<published>2017-12-01T03:27:18.000Z</published>
<updated>2018-05-25T06:26:28.000Z</updated>
<content type="html"><![CDATA[<p>最近设计滑模控制器需要用MATLAB Simulink中的User-Defined Function，写好Function后编译报错。报错信息为： <figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">No supported compiler or SDK was found.</div></pre></td></tr></table></figure></p>
<p>在<a href="https://cn.mathworks.com/matlabcentral/answers/246135-is-matlab-compatible-with-mac-os-x-10-11-el-capitan" target="_blank" rel="external">MathWorks论坛</a>上有这个问题的解决方案。大致就是去App Store上安装Xcode就可以了，然而我并不想安装臃肿的Xcode且我已经安装了Xcode command line tool。这篇文章将教你如何花式配置mex。<br>
<a id="more"></a></p>
<h2 id="更改mex的xml配置">更改mex的XML配置</h2>
<p>打开MATLAB，并在其Command Line下输入： <figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">cd( fullfile( matlabroot, 'bin', 'maci64', 'mexopts' ) );</div></pre></td></tr></table></figure></p>
<p>这里MATLAB左侧的Current Folder中应有三个文件，分别是<code>clang_maci64.xml</code>，<code>clang++_maci64.xml</code>和<code>gfortan.xml</code>，分别对应了c文件，cpp文件和fortan的编译配置。由于Simulink只需要编译c文件，所以仅更改<code>clang_maci64.xml</code>就可以了。<code>clang++_maci64.xml</code>的修改方法与<code>clang_maci64.xml</code>类似，就不多加说明了。<br>
直接使用MATLAB打开<code>clang_maci64.xml</code>，找到以下部分，并将其用<code><!--> <--></code>注释掉。这将会使编译器在<code>/usr/include</code>中搜索头文件，而非Xcode的路径。 <figure class="highlight xml"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div><div class="line">15</div><div class="line">16</div><div class="line">17</div><div class="line">18</div><div class="line">19</div><div class="line">20</div><div class="line">21</div><div class="line">22</div><div class="line">23</div><div class="line">24</div><div class="line">25</div><div class="line">26</div><div class="line">27</div><div class="line">28</div><div class="line">29</div><div class="line">30</div><div class="line">31</div><div class="line">32</div><div class="line">33</div></pre></td><td class="code"><pre><div class="line"><span class="tag"><<span class="name">ISYSROOT</span>></span></div><div class="line"> <span class="tag"><<span class="name">and</span>></span></div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"xcode-select -print-path"</span>/></span></div><div class="line"> <span class="tag"><<span class="name">or</span>></span></div><div class="line"> <span class="tag"><<span class="name">dirExists</span> <span class="attr">name</span>=<span class="string">"$$/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.9.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">dirExists</span> <span class="attr">name</span>=<span class="string">"$$/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.10.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">dirExists</span> <span class="attr">name</span>=<span class="string">"$$/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.11.sdk"</span> /></span> </div><div class="line"> <span class="tag"><<span class="name">dirExists</span> <span class="attr">name</span>=<span class="string">"$$/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.12.sdk"</span> /></span> </div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"find $$ -name MacOSX10.9.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"find $$ -name MacOSX10.10.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"find $$ -name MacOSX10.11.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"find $$ -name MacOSX10.12.sdk"</span> /></span></div><div class="line"> <span class="tag"></<span class="name">or</span>></span></div><div class="line"> <span class="tag"></<span class="name">and</span>></span></div><div class="line"> <span class="tag"></<span class="name">ISYSROOT</span>></span></div><div class="line"> <span class="tag"><<span class="name">SDKVER</span>></span></div><div class="line"> <span class="tag"><<span class="name">and</span>></span></div><div class="line"> <span class="tag"><<span class="name">and</span>></span></div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"xcode-select -print-path"</span>/></span></div><div class="line"> <span class="tag"><<span class="name">or</span>></span></div><div class="line"> <span class="tag"><<span class="name">dirExists</span> <span class="attr">name</span>=<span class="string">"$$/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.9.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">dirExists</span> <span class="attr">name</span>=<span class="string">"$$/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.10.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">dirExists</span> <span class="attr">name</span>=<span class="string">"$$/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.11.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">dirExists</span> <span class="attr">name</span>=<span class="string">"$$/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.12.sdk"</span> /></span> </div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"find $$ -name MacOSX10.9.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"find $$ -name MacOSX10.10.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"find $$ -name MacOSX10.11.sdk"</span> /></span></div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"find $$ -name MacOSX10.12.sdk"</span> /></span></div><div class="line"> <span class="tag"></<span class="name">or</span>></span></div><div class="line"> <span class="tag"></<span class="name">and</span>></span></div><div class="line"> <span class="tag"><<span class="name">cmdReturns</span> <span class="attr">name</span>=<span class="string">"echo $$ | rev | cut -c1-10 | rev | egrep -oh '[0-9]+\.[0-9]+'"</span> /></span></div><div class="line"> <span class="tag"></<span class="name">and</span>></span></div><div class="line"> <span class="tag"></<span class="name">SDKVER</span>></span></div></pre></td></tr></table></figure></p>
<p>回到MATLAB的Command Window，并输入 <figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">mex --setup</div></pre></td></tr></table></figure></p>
<p>此时应该输出 <figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div></pre></td><td class="code"><pre><div class="line">MEX configured to use 'Xcode with Clang' for C language compilation.</div><div class="line">Warning: The MATLAB C and Fortran API has changed to support MATLAB</div><div class="line"> variables with more than 2^32-1 elements. In the near future</div><div class="line"> you will be required to update your code to utilize the</div><div class="line"> new API. You can find more information about this at:</div><div class="line"> http://www.mathworks.com/help/matlab/matlab_external/upgrading-mex-files-to-use-64-bit-api.html.</div></pre></td></tr></table></figure></p>
<p>这表明此时mex已经找到并使用gcc了，但是Simulink编译仍然会报以下错误： <figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">invalid version number in '-mmacosx-version-min='</div></pre></td></tr></table></figure></p>
<p>这是因为在<code>clang_maci64.xml</code>33行处，还有其他的一些行 <figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">CFLAGS="-fno-common -arch x86_64 -mmacosx-version-min=$SDKVER -fexceptions -isysroot $ISYSROOT"</div></pre></td></tr></table></figure></p>
<p>引用了我们刚才注释掉的内容<code>$SDKVER</code>和<code>$ISYSROOT</code>。<br>
需要注意的是此时并不应该再修改这个文件，需要在Command Window中输入 <figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">cd( prefdir );</div></pre></td></tr></table></figure></p>
<p>打开<code>mex_C_maci64.xml</code>继续修改。需要将文件中所有的<code>$SDKVER</code>更改为<code>10.11</code>，并将<code>ISYSROOT</code>更改为<code>/usr/bin</code>。<br>
至此XML文件修改完毕，然而打开Simulink还是不能编译通过。</p>
<h2 id="复制头文件">复制头文件</h2>
<p>Simulink报错的内容大概是在<code>simulink/include/</code>文件夹下找不到头文件，手动将<code>/usr/include/</code>中的所有文件复制到<code>/Applications/MATLAB_R2014a.app/simulink/include/</code>下即可。现在重新编译Simulink的model应该就可以顺利通过了。</p>
<h2 id="总结">总结</h2>
<p>之前在解决mex问题的时候搜索到<a href="https://stackoverflow.com/questions/40052388/matlab-mex-without-xcode-but-with-standalone-command-line-tools/40083012#40083012" target="_blank" rel="external">stackoverflow</a>上的这个答案，不过按照回答者提出的方案仍然报错，于是自己动手修改了一些细节上的内容，希望可以帮助到被这个问题困扰的朋友们。</p>
]]></content>
<summary type="html">
<p>最近设计滑模控制器需要用MATLAB Simulink中的User-Defined Function，写好Function后编译报错。报错信息为： <figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">No supported compiler or SDK was found.</div></pre></td></tr></table></figure></p>
<p>在<a href="https://cn.mathworks.com/matlabcentral/answers/246135-is-matlab-compatible-with-mac-os-x-10-11-el-capitan">MathWorks论坛</a>上有这个问题的解决方案。大致就是去App Store上安装Xcode就可以了，然而我并不想安装臃肿的Xcode且我已经安装了Xcode command line tool。这篇文章将教你如何花式配置mex。<br />
</summary>
<category term="BugFix" scheme="https://rm-rf.moe/categories/BugFix/"/>
<category term="MATLAB" scheme="https://rm-rf.moe/tags/MATLAB/"/>
<category term="bug" scheme="https://rm-rf.moe/tags/bug/"/>
</entry>
<entry>
<title>谈笑风生又一年</title>
<link href="https://rm-rf.moe/2017/10/11/2017-10-11-summary/"/>
<id>https://rm-rf.moe/2017/10/11/2017-10-11-summary/</id>
<published>2017-10-11T11:31:28.000Z</published>
<updated>2017-10-11T12:41:40.000Z</updated>
<content type="html"><![CDATA[<p>混吃混喝->坐吃等死的大四生活开始啦(｡･ω･｡) <a id="more"></a></p>
<h2 id="大三下学期的一些微小的工作">大三下学期的一些微小的工作</h2>
<ul>
<li>继续完成了平衡小车的制作，并意外的获得了山东省光电平衡组第一名的成绩。不过在全国赛表现不佳，很不适应现场的光线，坡道也跑不起来，速度一快起来车子就起飞。算法还是naive。</li>
<li>大约一个月的时间完成了一个较简单的水下机器人的控制部分，前进后退上浮下沉都没什么问题，不过定深方面因为压力传感器莫名其妙的坏掉所以没有完成。主要时间并没有用在控制算法的调试与创新上，而是浪费在了调上位机上，上位机是<code>Python+Tk</code>搞出来的，总体来说界面很naive。</li>
</ul>
<p><img src="https://ws1.sinaimg.cn/large/005WMcFzly1fkejj1btr2j30qo0f0grz.jpg" alt="奔跑的小车"> <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fkejyxau05j30vk0notu7.jpg" alt="下沉中的水下机器人"> <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fkek5m7au8j30sf0lxae0.jpg" alt="简陋的图形界面"></p>
<h2 id="大四的生活">大四的生活</h2>
<h3 id="推免生复试">推免生复试</h3>
<p>国庆节前去了一趟杭州，参加了浙江大学的推免生复试，顺便去逛了一逛西湖，吃了生煎包和糖醋排骨。虽然通过了复试但认识到自己与优秀控制系学生之间的差距，控制理论的基础不扎实，认识也很浅薄。电路方面更是需要花时间去弥补。 <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fkekelnxjwj31qw1qw1kx.jpg" alt="西湖一隅"></p>
<h3 id="未来的打算">未来的打算</h3>
<p>听取未来导师的建议，坚持学习线性代数知识；培养高效阅读论文的能力；慢慢学习了解水动力学以及水下机器人的控制。认真把毕业设计做好，掌握水下机器人的建模方法，实现鲁棒控制并测试性能。</p>
<h2 id="关于域名">关于域名</h2>
<p>之前的域名快要到期了，昨天在浏览网站的时候意外发现<code>.moe</code>，而且似乎目前注册的人数也不多，价格也不算太贵，于是挑选了一个比较有意义的<code>rm -rf</code>注册了。<br>
目前在<a href="https://cloudflare.com" target="_blank" rel="external">CloudFlare</a>部署了简单的<code>Page Rules</code>来不完全解决301重定向的问题。目前的<code>rules</code>是：</p>
<ul>
<li>https://hsmouc.com/* 301-> https://rm-rf.moe/</li>
<li>https://www.hsmouc.com/* 301-> https://rm-rf.moe/</li>
<li>https://en.hsmouc.com/* 301-> https://en.rm-rf.moe/</li>
</ul>
<p>因为不是氪金用户所以只能添加三条，所以如果用户直接访问http的站就没有办法重定向到新域名。<br>
另外，在我的个人服务器上配置<code>Nginx</code>的<code>server name</code>，并重新申请了SSL证书，来解决不安全连接的问题。</p>
]]></content>
<summary type="html">
<p>混吃混喝-&gt;坐吃等死的大四生活开始啦(｡･ω･｡)
</summary>
<category term="Life" scheme="https://rm-rf.moe/categories/Life/"/>
<category term="summary" scheme="https://rm-rf.moe/tags/summary/"/>
</entry>
<entry>
<title>我又双叒叕写了个电子钟</title>
<link href="https://rm-rf.moe/2017/07/16/2017-07-16-sopcclock/"/>
<id>https://rm-rf.moe/2017/07/16/2017-07-16-sopcclock/</id>
<published>2017-07-16T13:55:13.000Z</published>
<updated>2017-07-26T06:03:20.000Z</updated>
<content type="html"><![CDATA[<p>这篇文章基本上是SoPC的课程设计报告，要求是使用SoPC EDA实验板开发一个电子钟，使用OLED显示时间，并支持按键修改时间。本文包括SoPC系统硬件消抖器的开发，定时中断与硬件中断的实现等内容。最终实现效果如下图所示 <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx8t3ke7ej32io1w07vj.jpg" alt="电子钟显示效果"><a id="more"></a></p>
<h1 id="课程eda实验平台介绍">课程EDA实验平台介绍</h1>
<h2 id="edasopc概述">EDA,SoPC概述</h2>
<p>作为可编程逻辑应用的核心方向，EDA和SoPC是目前电子设计的两个热门领域。EDA是电子设计自动化(Electronic Design Automation)的缩写。EDA技术就是以计算机为工具，设计者在EDA软件平台上，用硬件描述语言HDL完成设计文件，然后由计算机自动地完成逻辑编译、化简、分割、综合、优化、布局、布线和仿真，直至对于特定目标芯片的适配编译、逻辑映射和编程下载等工作，最后通过这些芯片演示出所需要的设计结果。<br>
而SoC(System on Chip)一般来说被称为系统级芯片，也称为片上系统。它是一个有专用目标的集成电路，包含完整系统并有嵌入软件的全部内容。而SoPC(System on a Programmable chip)技术则可以使设计人员充分利用FPGA的逻辑单元以及植入FPGA内部的储存模块，并使用FPGA制造厂商提供的软核处理器，设计出可灵活裁剪、扩充、可升级的嵌入式处理系统。</p>
<h2 id="eda实验板功能">EDA实验板功能</h2>
<p>EDA实验板采用了Cyclone II 2C35 FPGA。它具有672个引脚，33,216 个逻辑单元 。FPGA引脚连接各个功能模块，方便对各个模块的控制和通信。对于简单的实验，在EDA实验板上开发了四个拨动开关，四个按键，四个LED灯，旋转编码器。对于用于比较系统的实验，EDA实验板提供了SDRAM、FLASH、128*64点阵OLED显示屏、日历时钟芯片。 对于需要处理各种通信的实验，EDA实验板提供了各种标准化的接口方案，如RS-232、RS-485、PS/2。对于较大较高级的设计项目，EDA实验板提供了USB2.0主从设备、10/100M以太网、红外端口、SD记忆卡、CAN总线。对于娱乐需求，EDA实验板还提供了音频编解码器，8位AD/DA，FM芯片。最后，其它扩展变可通过EDA实验板上的两个40针的扩展槽连接。<br>
## EDA实验板硬件说明 1. FPGA - Cyclone II EP2C35F484C8 FPGA - EPCS16串行配置芯片 2. I/O设备 - RS-232、RS-485、PS/2 - 10/100以太网、USB主从控制器 - IrDA 、CAN - FM、音频解码芯片、8-BIT ADC/DAC 3. 开关、LED、显示、时钟 - 实时日历时钟芯片 - OLED显示屏 4. 储存器 - 8-Mbyte Flash - 32-Mbyte SDRAM <span class="math inline">\(\times\)</span> 2 - SD Card</p>
<p>EDA实验板硬件设计如下图所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhp9tj8ipgj30cx0ao7bb.jpg" alt="EDA实验板硬件设计示意图"></p>
<h2 id="电子钟设计要求和系统硬件资源">电子钟设计要求和系统硬件资源</h2>
<h3 id="电子钟设计要求">电子钟设计要求</h3>
<p>基于课程EDA实验平台，开展基于SoPC技术的电子钟设计：</p>
<ul>
<li>在OLED显示屏上显示日期和时间，走时准确；</li>
<li>可区分闰年，各月天数无错误；</li>
<li>配置3个按键，用以调整时间；</li>
<li>调整时间时闪烁当前调整的位；</li>
</ul>
<h3 id="系统硬件资源">系统硬件资源</h3>
<p>电子钟所需的硬件资源可分为外围器件和SoPC硬件系统模块两个部分，下面分别叙述：</p>
<h4 id="系统所需外围硬件">系统所需外围硬件</h4>
<table>
<thead>
<tr class="header">
<th align="center">外围器件</th>
<th align="left">作用</th>
</tr>
</thead>
<tbody>
<tr class="odd">
<td align="center">按键</td>
<td align="left">电子钟设置功能键</td>
</tr>
<tr class="even">
<td align="center">OLED屏幕</td>
<td align="left">电子钟显示屏幕</td>
</tr>
<tr class="odd">
<td align="center">FLASH</td>
<td align="left">储存软、硬件程序</td>
</tr>
<tr class="even">
<td align="center">SDRAM</td>
<td align="left">程序运行内存</td>
</tr>
</tbody>
</table>
<h4 id="sopc硬件系统模块">SoPC硬件系统模块</h4>
<p>SoPC硬件系统模块包括以下部分：Nios II CPU、定时器、按键PIO、OLED接口、AVALON三态桥、外部SDRAM接口，外部Flash接口、EPCS串行Flash控制器、JTAG UART。</p>
<h1 id="系统硬件设计">系统硬件设计</h1>
<p>这部分主要设计基于FPGA的SoPC整体硬件系统，主要包括PIO接口模块，存储模块、配置模块等。外围接口电路主要包括系统的OLED屏幕接口电路，储存系统接口电路，晶振电路、按键电路等部分。该系统的整体单元结构如下图所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx61dgafej30j80ert9j.jpg" alt="电子钟硬件系统整体结构框图"></p>
<h2 id="键盘接口电路设计">键盘接口电路设计</h2>
<p>在本例中键盘接口电路比较简单，并未通过串电容的方式来进行硬件消抖。之后将详细介绍基于FPGA设计D触发器消抖电路的方法，EDA实验板上的基础键盘电路如下所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx66nn1f9j311i0a0gmr.jpg" alt="SoPC系统与键盘接口电路图"></p>
<h2 id="oled接口电路设计">OLED接口电路设计</h2>
<p>LCD接口电路连接采用SSD1305Z型OLED模块，显示的内容为<span class="math inline">\(128\times64\)</span>，它属于点阵型液晶模块，能够显示数字、字母和一些符号。有对比度、背光调节等功能。SoPC系统与LCD接口电路如下图所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx67rb866j30we0cfwha.jpg" alt="SoPC系统与LCD接口电路图"></p>
<h2 id="基于nios-ii软核和fpga的嵌入式sopc系统配置与调试">基于Nios II软核和FPGA的嵌入式SoPC系统配置与调试</h2>
<h3 id="nios-2系统简介">Nios 2系统简介</h3>
<p>Nios II是一种新型嵌入式处理器。具体来说，它是CPU软核，为Altera公司的FPGA提供了一种嵌入式系统解决方案，几乎可以用在所有Altera公司设计制造的FPGA芯片上。Nios II处理器及其外设模块程序大部分是用硬件描述语言(HDL)或C语言进行编程设计。<br>
Nios II处理器包括Nios II CPU模块、Avalon交换总线，系统外设和片内用户逻辑，系统中的外设，如SDRAM控制器、程序储存器、数据储存器、UART、通用I/O等都是由FPGA内部的逻辑和RAM资源实现的。<br>
Nios II是可以灵活配置的软核处理器，可以灵活地设计或者配置外设接口，满足设计和应用要求，使用嵌入式软核Nios II有以下特点：</p>
<ul>
<li>CPU可以根据需要定制，灵活便捷；</li>
<li>有丰富的外围接口，接口的类型和数量可以灵活定制；</li>
<li>总线的连接不同于传统的CPU，其总线分布在FPGA的内部，因此不存在PCB布线中由于总线拥挤而难以走线；</li>
<li>采用Avalon总线结构，其结构不同于传统意义上的共享式总线，在要连接的每一个主、从端口之间都建立了点到点的连接关系，则不同主、从端口之间可以同时进行通信，提高了Avalon总线的性能和效率。</li>
<li>可以在同一块FPGA内定制多个Nios II软核进行多处理器并行工作，提高处理性能。</li>
</ul>
<h3 id="基于fpga的键盘消抖硬件设计">基于FPGA的键盘消抖硬件设计</h3>
<p>前文已经提到，由于按键本身机械性能并不优异，导致在按键按下或松开时抖动较为严重，很容易出现误操作的情况，影响用户体验。为了避免以上问题，必须对机械按键进行消抖处理。<br>
常见的消抖方法有硬件和软件消抖两种；SoPC系统的优势就在于用户不但可以自由设计软件，还能最大程度上定制自己需要的硬件，减少代码量，提高系统运行效率。消抖即为消除在一段时间按键的反复作用。考虑到D触发器配合合适的时钟频率即可滤去一些在短时间反复跳变的电平，而长时间存在的电平会被保留下来。综上所述，可通过FPGA构造相应的D触发器按键消抖电路。具体做法如下：<br>
首先需要为D触发器电路提供合适的时钟信号。而为Nios II软核提供的是<span class="math inline">\(50MHz\)</span>时钟，必须对其进行分频。由于大部分人的按键速度都在<span class="math inline">\(0.1s\)</span>以上，所以选择<span class="math inline">\(400Hz\)</span>时钟信号，使用四个D触发器串行连接，对一个按键作用的电平连续校验四次，将四次校验的结果输入<span class="math inline">\(xor\)</span>逻辑，若四次校验的结果相同则认为按键产生了一个可靠动作。<br>
根据以上描述，直接在Block Diagram中放置响应的元件，组成D触发器消抖阵列，并通过VHDL语言设计分频器；D触发器消抖电路如下图所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx6e2l6k3j30i505nmxb.jpg" alt="使用D触发器的消抖电路"> 分频器代码如下： <figure class="highlight vhdl"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div><div class="line">15</div><div class="line">16</div><div class="line">17</div><div class="line">18</div><div class="line">19</div><div class="line">20</div><div class="line">21</div><div class="line">22</div><div class="line">23</div><div class="line">24</div><div class="line">25</div><div class="line">26</div><div class="line">27</div><div class="line">28</div><div class="line">29</div><div class="line">30</div><div class="line">31</div></pre></td><td class="code"><pre><div class="line"><span class="keyword">library</span> IEEE;</div><div class="line"><span class="keyword">use</span> IEEE.STD_LOGIC_1164.<span class="keyword">all</span>;</div><div class="line"><span class="keyword">use</span> IEEE.NUMERIC_STD.<span class="keyword">all</span>;</div><div class="line"></div><div class="line"><span class="keyword">entity</span> frqdivide <span class="keyword">is</span></div><div class="line"> <span class="keyword">port</span> (</div><div class="line"> clk_50Mhz : <span class="keyword">in</span> <span class="built_in">std_logic</span>;</div><div class="line"> rst : <span class="keyword">in</span> <span class="built_in">std_logic</span>;</div><div class="line"> clk_400Hz : <span class="keyword">out</span> <span class="built_in">std_logic</span>);</div><div class="line"><span class="keyword">end</span> frqdivide;</div><div class="line"></div><div class="line"><span class="keyword">architecture</span> Behavioral <span class="keyword">of</span> frqdivide <span class="keyword">is</span></div><div class="line"> <span class="keyword">signal</span> prescaler : <span class="built_in">unsigned</span>(<span class="number">23</span> <span class="keyword">downto</span> <span class="number">0</span>);</div><div class="line"> <span class="keyword">signal</span> clk_400Hz_i : <span class="built_in">std_logic</span>;</div><div class="line"> <span class="keyword">begin</span></div><div class="line"> gen_clk : <span class="keyword">process</span> (clk_50Mhz, rst)</div><div class="line"> <span class="keyword">begin</span></div><div class="line"> <span class="keyword">if</span> rst = <span class="string">'1'</span> <span class="keyword">then</span></div><div class="line"> clk_400Hz_i <= <span class="string">'0'</span>;</div><div class="line"> prescaler <= (<span class="keyword">others</span> => <span class="string">'0'</span>);</div><div class="line"> <span class="keyword">elsif</span> rising_edge(clk_50Mhz) <span class="keyword">then</span></div><div class="line"> <span class="keyword">if</span> prescaler = X<span class="string">"186A"</span> <span class="keyword">then</span></div><div class="line"> prescaler <= (<span class="keyword">others</span> => <span class="string">'0'</span>);</div><div class="line"> clk_400Hz_i <= <span class="keyword">not</span> clk_400Hz_i;</div><div class="line"> <span class="keyword">else</span></div><div class="line"> prescaler <= prescaler + <span class="string">"1"</span>;</div><div class="line"> <span class="keyword">end</span> <span class="keyword">if</span>;</div><div class="line"> <span class="keyword">end</span> <span class="keyword">if</span>;</div><div class="line"><span class="keyword">end</span> <span class="keyword">process</span> gen_clk;</div><div class="line">clk_400Hz <= clk_400Hz_i;</div><div class="line"><span class="keyword">end</span> Behavioral;</div></pre></td></tr></table></figure></p>
<h3 id="nios-ii软核配置">Nios II软核配置</h3>
<p>创建Nios II系统模块需要SoPC Builder，它是Quartus II中的一个工具，使用SoPC Builder可以创建一个Nios II系统模块，或者创建多主从设备SoPC模块。</p>
<ul>
<li><p>加入CPU核Nios II Processor Nios II CPU有三种不同的类型可以选择，分别是经济型，标准型和快速型，在本设计中，板载的FPGA芯片完全支持快速型Nios II核，资源足够，所以选快速型。</p></li>
<li>加入定时器Timer 将定时器的组件名称设置为timer0，这个定时器是为了实现时钟的核心功能加入的。在之后的软件设计中，将使用定时器中断，每<span class="math inline">\(1s\)</span>触发一次。 <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx6lq4fmhj30mu0ggabu.jpg" alt="Altera Nios II核设置页面"></li>
<li>加入PIO功能 在电子钟设计中，使用按键来调整时间。由于需要使用按键中断，必须选择Generate IRQ，并选择下降沿中断。这样可以有利于后期软件层面设计的代码简化，即可以方便地在中断服务函数里实现按键功能。</li>
<li>加入外部Flash 在组件中加入Flash Memory Interface(CFI)，选择地址线宽度为23，数据线宽度为8，并设置处理器对Flash的读写时序；加入EPCS Serial Flash Controller，可用于CPU对EPCS Flash存储器的读写，可通过此控制器将编译的生成的SOF文件和CPU运行的其他代码存在EPCS器件中，以减少系统的整体硬件组成。</li>
<li>加入用户定制外设 OLED模块为用户自定义外设，需要添加以作为电子钟的显示屏幕。</li>
<li><p>加入Avalon三态总线桥 Nios II CPU与SDRAM，Flash，和用户自定义组件相连接都需要Avalon三态总线桥。<br>
配置完成的Nios II CPU内核如下图所示：<br>
<img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx6np6wrxj30ky0e3wgi.jpg" alt="配置完成的Nios II核"> 在Quartus II中将已经配置好的Nios II核加入设计的顶层设计文件，并连接外设与CPU的接口。 <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx6pl4xjaj30l80l4myt.jpg" alt="电子钟系统顶层原理图设计"></p></li>
</ul>
<h1 id="系统软件设计">系统软件设计</h1>
<h2 id="代码框架">代码框架</h2>
<p>系统的软件可分为两个部分，分别是Nios II EDS辅助构建的板级支持包，指的是使所给操作系统能够适配于所给设备主板的一套特定的支持代码（软件）实现。它通常包含了以基础支持代码来加载操作系统的引导程序（英语：bootloader），以及主板上所有设备的驱动程序}，和我编写的电子钟应用层代码。应用层代码分为三块，分别为电子钟时间计算服务；电子钟时间显示服务；电子钟按键功能服务；代码的框架结构如下所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx6tw839ij30pd0b20ts.jpg" alt="电子钟系统代码结构设计"> ## 代码数据结构 在电子钟代码设计中对多种变量进行了封装，便于提高代码可读性与规范性，结构化的数据使得数据的引用变得简单。考虑到电子钟设计中主要考虑时间计算与设置问题，于是针对时间计算和时间设置功能构造两个结构体，在time.h文件中定义，两个结构体的定义如下：<br>
<strong>时间结构体：</strong>由年、月、日、时、分、秒六个变量构成，均为无符号整型； <figure class="highlight c"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div></pre></td><td class="code"><pre><div class="line"><span class="keyword">typedef</span> <span class="keyword">struct</span> {</div><div class="line"> <span class="keyword">unsigned</span> <span class="keyword">int</span> year; </div><div class="line"> <span class="keyword">unsigned</span> <span class="keyword">int</span> month;</div><div class="line"> <span class="keyword">unsigned</span> <span class="keyword">int</span> day;</div><div class="line"> <span class="keyword">unsigned</span> <span class="keyword">int</span> hour;</div><div class="line"> <span class="keyword">unsigned</span> <span class="keyword">int</span> minute;</div><div class="line"> <span class="keyword">unsigned</span> <span class="keyword">int</span> second;</div><div class="line">}clockTypeDef;</div><div class="line">clockTypeDef clock;</div></pre></td></tr></table></figure></p>
<p><strong>时间设置结构体：</strong>由是否设置时间标志，和设置时间位置两个变量构成，为无符号整型和整型； <figure class="highlight c"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div></pre></td><td class="code"><pre><div class="line"><span class="keyword">typedef</span> <span class="keyword">struct</span> {</div><div class="line"> <span class="keyword">unsigned</span> <span class="keyword">int</span> isTimeSet;</div><div class="line"> <span class="keyword">int</span> blinkPos;</div><div class="line">}setTypeDef;</div><div class="line">setTypeDef <span class="built_in">set</span>;</div></pre></td></tr></table></figure></p>
<h2 id="代码核心功能设计">代码核心功能设计</h2>
<p>由于板级支持包的代码由Nios II EDS自动生成，在此不再展开。以下将从电子钟走时实现和时间调整功能两个方面介绍电子钟系统的代码。 ### 走时实现 电子钟实现走时通常有以下两种途径：第一种是通过软件延时实现的，根据CPU的晶振频率计算每秒理论可执行的指令数，使用循环的方法占用CPU资源以达到定时器的效果，这种方法在软件上易于实施，不用配置中断，但是缺点在于耗费CPU资源，计时时间不准。另一种是用过中断实现的，通过配置CPU定时器中断并编写中断服务函数，这样即可实现精准计时的效果。在本设计中采用定时器中断方法实现走时。<br>
电子钟定时中断配置函数如下，在主函数中调用，写寄存器设定周期为1秒，运行，并注册中断。需要注意的是注册中断函数在Nios II EDS 11.0版本中已经由alt_isr_register更改为alt_ic_isr_register，函数结构改变，传入的参数发生了一些变化。 <figure class="highlight c"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">void</span> <span class="title">initTimer</span><span class="params">(<span class="keyword">void</span>)</span> </span>{</div><div class="line"> IOWR_ALTERA_AVALON_TIMER_STATUS(TIMER0_BASE, <span class="number">0x00</span>);</div><div class="line"> IOWR_ALTERA_AVALON_TIMER_PERIODL(TIMER0_BASE,<span class="number">50000000</span>);</div><div class="line"> IOWR_ALTERA_AVALON_TIMER_PERIODH(TIMER0_BASE, <span class="number">50000000</span> >> <span class="number">16</span>);</div><div class="line"> IOWR_ALTERA_AVALON_TIMER_CONTROL(TIMER0_BASE, <span class="number">0x07</span>);</div><div class="line"> alt_ic_isr_register(TIMER0_IRQ_INTERRUPT_CONTROLLER_ID,</div><div class="line"> TIMER0_IRQ, isrTimer0, <span class="literal">NULL</span>,<span class="number">0x0</span>);</div><div class="line">}</div></pre></td></tr></table></figure></p>
<p>定时中断的服务函数主要作用是调用时间计算函数，如下所示： <figure class="highlight c"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">void</span> <span class="title">isrTimer0</span><span class="params">()</span> </span>{</div><div class="line"> timeCalculate();</div><div class="line"> timeDisplay();</div><div class="line"> IOWR_ALTERA_AVALON_TIMER_STATUS(TIMER0_BASE, <span class="number">0x00</span>);</div><div class="line">}</div></pre></td></tr></table></figure></p>
<p>时间计算函数较长，参见附录完整代码，其效果为每调用一次，时间结构体中的秒递增1，递增至59时清零，同时分钟递增1，之后同理。在此函数中也考虑了不同月份天数不同，闰年二月天数改变等细节问题。<br>
时间显示函数包括了两个模块，分别为闪烁功能和时间的逐位显示。特别地，闪烁功能是基于定时中断实现的，每进入一次中断，闪烁标志都会取反，根据闪烁标志的0，1来确定显示空白字符还是时间。另外通过时间设置功能结构体中的闪烁位置成员计算OLED显示数组中需要被置为空白字符的数组元素，代码如下所示，省略了整数取位显示和变量赋值： <figure class="highlight c"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div><div class="line">15</div><div class="line">16</div><div class="line">17</div><div class="line">18</div><div class="line">19</div><div class="line">20</div><div class="line">21</div><div class="line">22</div><div class="line">23</div><div class="line">24</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">void</span> <span class="title">timeDisplay</span><span class="params">()</span> </span>{</div><div class="line"> ...</div><div class="line"> <span class="keyword">if</span>(<span class="built_in">set</span>.isTimeSet) {</div><div class="line"> isBlink = isBlink><span class="number">0</span>?<span class="number">0</span>:<span class="number">1</span>;</div><div class="line"> }</div><div class="line"> <span class="keyword">else</span> isBlink = <span class="number">0</span>;</div><div class="line"> ...</div><div class="line"> <span class="keyword">if</span>(isBlink) {</div><div class="line"> <span class="keyword">if</span>(!<span class="built_in">set</span>.blinkPos) {</div><div class="line"> <span class="keyword">for</span>(i = <span class="number">0</span>; i < <span class="number">4</span>; i++)</div><div class="line"> numDate[i] = <span class="number">1</span>;</div><div class="line"> }</div><div class="line"> <span class="keyword">else</span> {</div><div class="line"> numDate[<span class="built_in">set</span>.blinkPos*<span class="number">3</span>+<span class="number">2</span>] = <span class="number">1</span>;</div><div class="line"> numDate[<span class="built_in">set</span>.blinkPos*<span class="number">3</span>+<span class="number">3</span>] = <span class="number">1</span>;</div><div class="line"> <span class="keyword">if</span>(<span class="built_in">set</span>.blinkPos > <span class="number">2</span>) {</div><div class="line"> numTime[(<span class="built_in">set</span>.blinkPos<span class="number">-3</span>)*<span class="number">3</span>] = <span class="number">1</span>;</div><div class="line"> numTime[(<span class="built_in">set</span>.blinkPos<span class="number">-3</span>)*<span class="number">3</span>+<span class="number">1</span>] = <span class="number">1</span>;</div><div class="line"> }</div><div class="line"> }</div><div class="line"> }</div><div class="line"> Show_String(<span class="number">1</span>,numDate,<span class="number">2</span>,<span class="number">50</span>);</div><div class="line"> Show_String(<span class="number">1</span>,numTime,<span class="number">4</span>,<span class="number">50</span>);</div><div class="line">}</div></pre></td></tr></table></figure></p>
<h3 id="时间调整实现">时间调整实现</h3>
<p>电子钟的时间调整是通过按键实现的，而按键功能通常也有两种实现方式，第一种为循环查询方式，主要做法是在主函数循环中不断读按键寄存器捕捉电平跳变，捕捉到跳变则调用相应功能函数。这种循环查询方式的优势在于可以方便地设计消抖功能，无需硬件消抖，但缺点仍然是占用CPU资源，从嵌入式开发的角度不够合理。而第二种方式为中断查询方式，通过PIO中断来捕捉边沿，在中断服务函数中部署功能，这种做法节省CPU资源，同时我也为此方式提供了硬件消抖电路。在本设计中采取第二种方式。<br>
按键的中断配置函数与定时中断类似，通过以下代码写PIO中断寄存器，配置中断为边沿触发，注册中断，在本例中使用了三个按键，在硬件资源足够的情况下注册了三个不同的硬件中断： <figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div></pre></td><td class="code"><pre><div class="line">void initKey() {</div><div class="line"> IOWR_ALTERA_AVALON_PIO_IRQ_MASK(KEY0_BASE, 0xf);</div><div class="line"> IOWR_ALTERA_AVALON_PIO_EDGE_CAP(KEY0_BASE, 0x0);</div><div class="line"> alt_ic_isr_register(KEY0_IRQ_INTERRUPT_CONTROLLER_ID,</div><div class="line"> KEY0_IRQ,isrKey0,NULL,0x0);</div><div class="line">}</div></pre></td></tr></table></figure></p>
<p>按键的中断服务函数有三个，分别针对时间调整加，时间调整减，时间调整开关。时间调整按钮按下后，时间设置结构体中的时间设置标志置位，并将成员闪烁位步进1，直至闪烁位到5，也就是“秒”，将闪烁位置-1以备下次调整时间使用，清时间设置标志，相当于关时间设置功能。核心代码如下： <figure class="highlight c"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">void</span> <span class="title">isrKey0</span><span class="params">(<span class="keyword">void</span>* context, <span class="keyword">int</span> id)</span> </span>{</div><div class="line"> <span class="built_in">set</span>.isTimeSet = <span class="number">1</span>;</div><div class="line"> <span class="built_in">set</span>.blinkPos++;</div><div class="line"> <span class="keyword">if</span>(<span class="built_in">set</span>.blinkPos > <span class="number">5</span>) {</div><div class="line"> <span class="built_in">set</span>.isTimeSet = <span class="number">0</span>;</div><div class="line"> <span class="built_in">set</span>.blinkPos = <span class="number">-1</span>;</div><div class="line"> }</div><div class="line"> IOWR_ALTERA_AVALON_PIO_EDGE_CAP(KEY0_BASE, <span class="number">1</span>);</div><div class="line">}</div></pre></td></tr></table></figure></p>
<p>时间调整功能的实现使用了switch语句，通过当前的闪烁位置来决定具体增减哪部分的值，时间调整加示例代码如下，时间调整减代码类似： <figure class="highlight c"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">void</span> <span class="title">isrKey1</span><span class="params">(<span class="keyword">void</span>* context, alt_u32 id)</span> </span>{</div><div class="line"> <span class="keyword">if</span>(<span class="built_in">set</span>.isTimeSet == <span class="number">1</span>) {</div><div class="line"> <span class="keyword">switch</span>(<span class="built_in">set</span>.blinkPos) {</div><div class="line"> <span class="keyword">case</span> <span class="number">0</span>: clock.year++; <span class="keyword">break</span>;</div><div class="line"> ...</div><div class="line"> <span class="keyword">default</span>: <span class="keyword">break</span>;</div><div class="line"> }</div><div class="line"> IOWR_ALTERA_AVALON_PIO_EDGE_CAP(KEY1_BASE, <span class="number">1</span>);</div><div class="line"> }</div><div class="line">}</div></pre></td></tr></table></figure></p>
<p>需要特别注意的是，在中断服务函数中需要清中断标志位，Altera官方文档说明寄存器写0清除。事实上，按官方文档上的写法并不能有效跳出中断，必须写1清除。 # 总结 ## 最终实现效果 电子钟最终在实验板上的显示效果如下图所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx8t3ke7ej32io1w07vj.jpg" alt="电子钟显示效果"> 由图可见，电子钟从默认的时间-1970年1月1日开始走时，经过五小时的测试误差小于1秒，说明了时钟还是比较准确的。<br>
<strong>为何从1970-01-01开始</strong><br>
作为一个使用UNIX和Linux的用户，当然要使用UNIX(POSIX)时间来致敬此伟大的计算机系统。 ## 一个bug 在电子钟上电时会自动进入时间设置功能，这是因为按键中断是用上升沿触发的。也就是说上电时的动作相当于按键从按下到抬起产生了一个上升沿。这个问题可通过在CPU的PIO中断配置中将其更改为下降沿触发解决。</p>
]]></content>
<summary type="html">
<p>这篇文章基本上是SoPC的课程设计报告，要求是使用SoPC EDA实验板开发一个电子钟，使用OLED显示时间，并支持按键修改时间。本文包括SoPC系统硬件消抖器的开发，定时中断与硬件中断的实现等内容。最终实现效果如下图所示 <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhx8t3ke7ej32io1w07vj.jpg" alt="电子钟显示效果" />
</summary>
<category term="Embedded System" scheme="https://rm-rf.moe/categories/Embedded-System/"/>
<category term="C" scheme="https://rm-rf.moe/tags/C/"/>
</entry>
<entry>
<title>单自由度弹性关节机器人控制系统分析与设计</title>
<link href="https://rm-rf.moe/2017/05/15/2017-05-15-flexiblejoint/"/>
<id>https://rm-rf.moe/2017/05/15/2017-05-15-flexiblejoint/</id>
<published>2017-05-15T11:21:18.000Z</published>
<updated>2017-07-16T09:27:13.000Z</updated>
<content type="html"><![CDATA[<ul>
<li>这篇文章是现代控制理论课程设计的前半部分，主要研究了单自由度弹性关节系统的工程背景，动力学建模；控制系统方面包括状态方程建立，状态转移矩阵计算，零输入给定初值<code>MATLAB</code>与<code>Python Tkinter</code>模型仿真，状态方程与传递函数之间的转换，系统能观性和能控性的讨论。</li>
<li>单自由度弹性关节示意图如下： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1ffn2m7gikuj30by06pq3b.jpg" alt="单自由度弹性关节"></li>
</ul>
<p><em>P.S.</em> 这篇文章的原排版系统为<code>LaTeX</code>，在<a href="https://www.sharelatex.com/project/5915c87ed1d7c39a6d050f86" target="_blank" rel="external">ShareLaTeX</a>上可以导出<code>PDF</code>格式的报告，同时<code>LaTeX Source</code>也开放了公共阅读权限。 <a id="more"></a></p>
<h1 id="工程背景">工程背景</h1>
<h2 id="工业机器人概述">工业机器人概述</h2>
<p>工业机器人按照ISO8373定义<code>\footnote{ISO Standard 8373:1994, Manipulating Industrial Robots – Vocabulary}</code>，它是面向工业领域的多关节机械手或多自由度的机器人。工业机器人是自动执行工作的机器装置，是靠自身动力和控制能力来实现各种功能的一种机器。它可以接受人类指挥，也可以按照预先编排的程序运行，现代的工业机器人还可以根据人工智能技术制定的原则纲领行动。<br>
工业机器人的重要特性是三维空间运动的空间机构，这也是其区别与数控机床的特征。机器人的机构本质上为空间多体系统。当考虑系统元件的弹性时，为多柔性体系统。机器人的主要机构有：关节、连杆、伺服电机、减速机等。</p>
<ul>
<li><strong>关节：</strong>机器人系统中将运动副称为关节。在关节之中，凡单独驱动的称为主动关节，反之称从动关节。工业机器人手臂的关节常为单自由度主动关节，即每一个关节均有一个驱动器驱动。</li>
<li><strong>连杆：</strong>连杆机构是传递机械能的一种装置，通常是由刚体构件用转动副、移动副、球面副、球销副、圆柱副或螺旋副中的一种或几种联结而成的机械机构，因为上述联接副均属于低副，连杆机构也称为低副机构。通过不同的设计与计算，连杆机构可实现转动、直线移动、往复运动和平面或空间的复杂函数运动轨迹。</li>
<li><strong>伺服电机：</strong>伺服电机是对用于使用伺服机构的电动机总称。伺服系统，就是依照指示命令动作所构成的控制装置，应用于马达的伺服控制，将感测器装在马达与控制对象机器上，侦测结果会返回伺服放大器与指令值做比较。伺服马达的动作特性是进行位置定位控制和动作速度控制，其主要特点为转速可以精确控制，速度控制范围广。</li>
<li><strong>减速机：</strong>减速机一般用于低转速大扭矩的传动设备，把电动机，内燃机或其它高速运转的动力通过减速机的输入轴上的齿数少的齿轮啮合输出轴上的大齿轮来达到减速的目的，减速机是一种动力传达机构，利用齿轮的速度转换器，将马达的回转数减速到所要的回转数，并得到较大转矩的机构。</li>
</ul>
<h2 id="机器人柔性设计">机器人柔性设计</h2>
<p>机器人的柔顺性是实现未知约束环境下人机安全物理交互，开展复杂作业的 重要前提。针对机器人关节的顺应性控制问题，传统工业中普遍应用“刚性设计，控制实现安全”的模式，然而这种设计会造成操作者的不适，且有可能对操作者或机器人接触的物体造成损伤。为了解决这个问题，可以在驱动机构和负载端之间串联弹性元件，将负载输出和电机惯量隔离。在人机物理交互的场合中，可确保接触力控制在合理范围内，其实现模式为“设计实现安全，控制提高性能<code>\footnote{浙江大学硕士学位论文：串联弹性关节控制与交互刚度辨识 吕铖杰 2015}</code>”。但是弹性介质的引入增加了系统动态结构的复杂度，设计稳定和可靠的控制算法成为使用该类驱动器的挑战之一。</p>
<h1 id="动力学建模">动力学建模</h1>
<p>单自由度弹性关节的使用场景一般是低速大力矩情形下，往往电机和负载之间会安装减速器，为了分析方便，在理想状态下不考虑减速器摩擦与额外转动惯量的影响，以下为单自由度弹性关节示意图： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1ffn2m7gikuj30by06pq3b.jpg" alt="单自由度弹性关节"> 其中：<span class="math inline">\(J_m\)</span>为电动机的转动惯量，<span class="math inline">\(J_l\)</span>为负载的转动惯量，<span class="math inline">\(\beta_m\)</span>为电动机的阻尼，可认为是转动摩擦与空气阻力等，<span class="math inline">\(\beta_l\)</span>为负载的阻尼，<span class="math inline">\(\theta_m\)</span>为电动机旋转的角度，<span class="math inline">\(\theta_l\)</span>为负载旋转的角度，<span class="math inline">\(k\)</span>为扭力弹簧常数，<span class="math inline">\(u\)</span>为电动机的输入转矩。<br>
首先对电动机进行受力分析：电动机输入转矩与弹簧扭转力矩，自身转动惯量产生的转矩，阻尼产生的转矩之和相平衡。<br>
弹簧扭转力矩公式为：<span class="math inline">\(F_1=k\times\Delta\theta\)</span>；<br>
转动惯量产生的转矩为：<span class="math inline">\(F_2=J\times\omega\)</span>；<br>
阻尼产生的转矩为：<span class="math inline">\(F_3 = \beta\times\alpha\)</span>；<br>
综上，可导出电动机转动时的转矩平衡公式：<br>
<span class="math display">\[u = k(\theta_m-\theta_l)+J_m\ddot{\theta}_m+\beta_m \dot{\theta}_m\]</span><br>
同理可导出负载转动时的转矩平衡公式：<br>
<span class="math display">\[k(\theta_l-\theta_m)=-J_l\ddot{\theta}_l-\beta_l\dot{\theta}_l\]</span><br>
整理两式，单自由度弹性关节的模型为：<br>
<span class="math display">\[
\left\{ \begin{array}{ll}
J_l\ddot{\theta}_l+\beta_l\dot{\theta}_l+k(\theta_l-\theta_m)=0 \\
J_m\ddot{\theta}_m+\beta_m\dot{\theta}_m-k(\theta_l-\theta_m)=u
\end{array} \right.
\]</span></p>
<h1 id="控制系统分析">控制系统分析</h1>
<h2 id="微分方程转化状态方程">微分方程转化状态方程</h2>
<p>首先确定状态变量为：<span class="math inline">\(\theta_l,\dot{\theta}_l,\theta_m,\dot{\theta}_m\)</span>；<br>
根据前文导出的单自由度弹性关节模型，可导出系统的状态方程为：</p>
<p><span class="math display">\[
\left\{ \begin{array}{ll}
\dot{x_1} = x_2 \\
\dot{x_2} = -\frac{k}{J_l}x_1-\frac{\beta_l}{J_l}x_2+\frac{k}{J_l}x_3 \\
\dot{x_3} = x_4 \\
\dot{x_4} = \frac{k}{J_m}x_1-\frac{\beta_m}{J_m}x_4-\frac{k}{J_m}x_3+\frac{1}{J_m}u
\end{array} \right.
\]</span> ## 状态转移矩阵 线性定常系统的状态空间表达式为：<br>
<span class="math display">\[
\left\{ \begin{array}{ll}
\dot{X} = AX+Bu \\
y = CX
\end{array} \right.
\]</span><br>
其中：<br>
<span class="math display">\[A =
\begin{bmatrix}
0 & 1 & 0 & 0 \\
-\frac{k}{J_l} & -\frac{\beta_l}{J_l} & \frac{k}{J_l} & 0 \\
0 & 0 & 0 & 1 \\
\frac{k}{J_m} & 0 & -\frac{k}{J_m} & -\frac{\beta_m}{J_m}
\end{bmatrix};\qquad B =
\begin{bmatrix}
0 \\ 0 \\ 0 \\ \frac{1}{J_m}
\end{bmatrix}; \qquad C =
\begin{bmatrix}
1 & 0 & 0 & 0
\end{bmatrix}
\]</span><br>
线性定常系统在没有控制作用时，由初始条件引起的运动为自由运动：对于线性定常系统，状态转移矩阵为： <span class="math display">\[\Phi(t-t_0)=e^{A(t-t_0)}\]</span></p>
<p><strong>MATLAB求解</strong><br>
代入系统已知参数，使用</p>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">var = expm(A*t)</div></pre></td></tr></table></figure>
<p>或者<br>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">var = ilaplace(inv(s*<span class="built_in">diag</span>([<span class="number">1</span> <span class="number">1</span> <span class="number">1</span> <span class="number">1</span>])-A))</div></pre></td></tr></table></figure></p>
<p>即可解出结果。</p>
<h2 id="零输入给定初值状态方程仿真">零输入给定初值状态方程仿真</h2>
<p>单自由度弹性关节机器人系统仿真所需参数如下表所示:</p>
<table>
<thead>
<tr class="header">
<th align="center">物理量</th>
<th align="center">数值</th>
<th align="center">单位</th>
</tr>
</thead>
<tbody>
<tr class="odd">
<td align="center"><span class="math inline">\(J_m\)</span></td>
<td align="center">1</td>
<td align="center"><span class="math inline">\(kg\cdot m^2\)</span></td>
</tr>
<tr class="even">
<td align="center"><span class="math inline">\(J_l\)</span></td>
<td align="center">1</td>
<td align="center"><span class="math inline">\(kg\cdot m^2\)</span></td>
</tr>
<tr class="odd">
<td align="center"><span class="math inline">\(\beta_m\)</span></td>
<td align="center">0.1</td>
<td align="center"><span class="math inline">\(N\)</span></td>
</tr>
<tr class="even">
<td align="center"><span class="math inline">\(\beta_l\)</span></td>
<td align="center">0.1</td>
<td align="center"><span class="math inline">\(N\)</span></td>
</tr>
<tr class="odd">
<td align="center"><span class="math inline">\(k\)</span></td>
<td align="center">60</td>
<td align="center"><span class="math inline">\(N/rad\)</span></td>
</tr>
</tbody>
</table>
<h3 id="ode45方法求解基于matlab">ODE45方法求解(基于MATLAB)</h3>
<p>使用ode45求解高阶常微分方程需要做降阶处理，使用该系统的状态方程构造函数，代码如下：</p>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">function</span> <span class="title">dydt</span> = <span class="title">odefcn</span><span class="params">(t,y,matrixA,matrixB)</span></span></div><div class="line"> dydt = <span class="built_in">zeros</span>(<span class="number">4</span>,<span class="number">1</span>);</div><div class="line"> dydt = matrixA*[y(<span class="number">1</span>);y(<span class="number">2</span>);y(<span class="number">3</span>);y(<span class="number">4</span>)]+matrixB*<span class="number">0</span>;</div></pre></td></tr></table></figure>
<p>其中，已知输入<span class="math inline">\(u\)</span>为零。<br>
代入矩阵参数，初值，时间求微分方程的数值解，并绘制<span class="math inline">\(\theta_l,\dot{\theta}_l,\theta_m,\dot{\theta}_m\)</span>关于变量<span class="math inline">\(t\)</span>的曲线，代码如下：</p>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div></pre></td><td class="code"><pre><div class="line">clear all</div><div class="line">syms s t</div><div class="line">J_m = <span class="number">1</span>; J_l = <span class="number">1</span>; B_m = <span class="number">0.1</span>; B_l = <span class="number">0.1</span>; k = <span class="number">60</span>;</div><div class="line">A = [<span class="number">0</span> <span class="number">1</span> <span class="number">0</span> <span class="number">0</span>;</div><div class="line"> -k/J_l -B_l/J_l k/J_l <span class="number">0</span>;</div><div class="line"> <span class="number">0</span> <span class="number">0</span> <span class="number">0</span> <span class="number">1</span>;</div><div class="line"> k/J_m <span class="number">0</span> -k/J_m -B_m/J_m]</div><div class="line">b = [<span class="number">0</span>;<span class="number">0</span>;<span class="number">0</span>;<span class="number">1</span>/J_m]</div><div class="line">tspan = <span class="built_in">linspace</span>(<span class="number">0</span>,<span class="number">100</span>,<span class="number">500</span>)</div><div class="line">y0 = [<span class="number">0.1</span>;<span class="number">0</span>;<span class="number">0</span>;<span class="number">0</span>];</div><div class="line">[tode,y] = ode45(@(t,y) odefcn(t,y,A,b), tspan, y0)</div><div class="line">plot(tode,y(:,<span class="number">1</span>),<span class="string">'g+'</span>,tode,y(:,<span class="number">2</span>),<span class="string">'b*'</span>,tode,y(:,<span class="number">3</span>),<span class="string">'k-.'</span>,tode,y(:,<span class="number">4</span>),<span class="string">'r'</span>)</div><div class="line">legend(<span class="string">'y1'</span>,<span class="string">'y2'</span>,<span class="string">'y3'</span>,<span class="string">'y4'</span>)</div><div class="line">title(<span class="string">'Solutions of Differential Equation(ode45)'</span>)</div></pre></td></tr></table></figure>
<h3 id="矩阵指数方法求解基于matlab">矩阵指数方法求解(基于MATLAB)</h3>
<p>和上一种方案的矩阵参数，初值，时间相同，使用拉氏反变换法求解矩阵指数，并绘制<span class="math inline">\(\theta_l,\dot{\theta}_l,\theta_m,\dot{\theta}_m\)</span>关于变量<span class="math inline">\(t\)</span>的曲线。<br>
矩阵指数的计算公式：<br>
<span class="math display">\[e^At=\mathcal{L}^{-1}[(sI-A)^{-1}]\]</span><br>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div></pre></td><td class="code"><pre><div class="line">X = ilaplace(inv(s*<span class="built_in">diag</span>([<span class="number">1</span> <span class="number">1</span> <span class="number">1</span> <span class="number">1</span>])-A))*y0</div><div class="line">plot(tspan,subs(X(<span class="number">1</span>),t,tspan),<span class="string">'-b+'</span>,tspan,subs(X(<span class="number">2</span>),t,tspan),<span class="string">'-g*'</span>,tspan,subs(X(<span class="number">3</span>),t,tspan),<span class="string">'k-.'</span>,tspan,subs(X(<span class="number">4</span>),t,tspan),<span class="string">'r'</span>)</div><div class="line">legend(<span class="string">'y1'</span>,<span class="string">'y2'</span>,<span class="string">'y3'</span>,<span class="string">'y4'</span>)</div><div class="line">title(<span class="string">'Solutions of Differential Equation(Matrix exponential)'</span>)</div></pre></td></tr></table></figure></p>
<h3 id="matlab程序运行结果">MATLAB程序运行结果</h3>
<p>运行以上MATLAB程序，可得到如下图所示的图像；当前给负载角度初值为<span class="math inline">\(0.1rad\)</span>,弹簧处于紧绷状态，系统零输入启动后，电机与负载产生相反方向的加速度，最终达到稳态。 <img src="https://ws1.sinaimg.cn/large/005WMcFzly1ffn3i6ix0wj30fk0dpdgl.jpg" alt="ODE45"> <img src="https://ws1.sinaimg.cn/large/005WMcFzly1ffn3i6k4l2j30fk0dp0tg.jpg" alt="矩阵指数"></p>
<h3 id="系统仿真动画基于python-tkinter">系统仿真动画(基于Python Tkinter)</h3>
<p>首先使用Python scipy模块求解系统的状态方程，得到<span class="math inline">\(\theta_l,\dot{\theta}_l,\theta_m,\dot{\theta}_m\)</span>关于变量<span class="math inline">\(t\)</span>的变化情况。借由matplotlib模块绘制曲线。同时，使用Tkinter创建图形用户界面，并根据上述求解的值绘制动画，实现数据可视化。程序运行效果如下图所示：<br>
<img src="https://ws1.sinaimg.cn/large/005WMcFzly1ffn3jpjjb2j30hs0f9jrq.jpg" alt="GUI"> <img src="https://ws1.sinaimg.cn/large/005WMcFzly1ffn3jqufepj30hs0f4tb2.jpg" alt="matplotlib绘图"></p>
<h4 id="代码功能">代码功能：</h4>
<p>单自由度弹性关节机器人系统的仿真程序，支持自定义输入，初值，系统参数；可以通过图形用户界面观察该系统的运行状态。</p>
<h4 id="运行环境">运行环境：</h4>
<p>兼容Windows/macOS/Linux，需要Python环境：</p>
<ul>
<li>Python 2.7.11<br>
</li>
<li>matplotlib <span class="math inline">\(\geq\)</span> 1.5.1<br>
</li>
<li>numpy <span class="math inline">\(\geq\)</span> 1.12.1<br>
</li>
<li>scipy <span class="math inline">\(\geq\)</span> 0.19.0</li>
</ul>
<h4 id="代码说明">代码说明:</h4>
<p>首先导入需要使用的模块，相关代码块如下所示：<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div></pre></td><td class="code"><pre><div class="line"><span class="keyword">import</span> numpy <span class="keyword">as</span> np</div><div class="line"><span class="keyword">import</span> matplotlib</div><div class="line">matplotlib.use(<span class="string">"TkAgg"</span>)</div><div class="line"><span class="keyword">import</span> matplotlib.pyplot <span class="keyword">as</span> plt</div><div class="line"><span class="keyword">import</span> math</div><div class="line"><span class="keyword">import</span> time</div><div class="line"><span class="keyword">from</span> Tkinter <span class="keyword">import</span> *</div><div class="line"><span class="keyword">from</span> scipy.integrate <span class="keyword">import</span> odeint</div></pre></td></tr></table></figure></p>
<p>变量赋值部分，根据实际物理系统给转动惯量，阻尼，劲度系数等变量赋值，同时确定画布大小，确定代表电机/负载的矩形位置。<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div></pre></td><td class="code"><pre><div class="line">WIDTH,HEIGHT,SIDE = <span class="number">400</span>,<span class="number">400</span>,<span class="number">100</span></div><div class="line">CANVAS_MID_X,CANVAS_MID_Y = WIDTH/<span class="number">2</span>,HEIGHT/<span class="number">2</span></div><div class="line">J_m, J_l, B_m, B_l, k = <span class="number">1</span>,<span class="number">1</span>,<span class="number">0.1</span>,<span class="number">0.1</span>,<span class="number">1</span></div><div class="line">square_m = np.array([</div><div class="line"> [CANVAS_MID_X - SIDE/<span class="number">2</span>, CANVAS_MID_Y - SIDE/<span class="number">2</span>],</div><div class="line"> [CANVAS_MID_X + SIDE/<span class="number">2</span>, CANVAS_MID_Y - SIDE/<span class="number">2</span>],</div><div class="line"> [CANVAS_MID_X + SIDE/<span class="number">2</span>, CANVAS_MID_Y + SIDE/<span class="number">2</span>],</div><div class="line"> [CANVAS_MID_X - SIDE/<span class="number">2</span>, CANVAS_MID_Y + SIDE/<span class="number">2</span>]]</div><div class="line">)</div><div class="line">square_l = square_m + <span class="number">100</span>*np.ones((<span class="number">4</span>,<span class="number">2</span>))</div></pre></td></tr></table></figure></p>
<p>以下为数据初始化函数，规范数据格式：<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">def</span> <span class="title">data_init</span><span class="params">(initial_conditions, model_params)</span>:</span></div><div class="line"> y0 = initial_conditions[<span class="string">'y0'</span>]</div><div class="line"> A = model_params[<span class="string">'A'</span>]</div><div class="line"> b = model_params[<span class="string">'b'</span>]</div><div class="line"> <span class="keyword">return</span> [y0,A,b]</div></pre></td></tr></table></figure></p>
<p>以下为ode求解所需的降阶函数，输入为初始状态，时间，系统状态空间描述系数矩阵，输出重构为单维数组(Python odeinit解法不支持高阶输入)<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">def</span> <span class="title">f</span><span class="params">(state, t, matrixA, matrixb)</span>:</span></div><div class="line"> xdot = matrixA*state.reshape(<span class="number">4</span>,<span class="number">1</span>) + matrixb*np.exp(-t)</div><div class="line"> <span class="keyword">return</span> np.squeeze(np.asarray(xdot))</div></pre></td></tr></table></figure></p>
<p>以下为矩形旋转坐标运算函数，输入为矩形四点坐标，需要旋转的角度(弧度制)，旋转中心坐标；返回值为旋转后的矩形四点坐标。<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">def</span> <span class="title">rotate</span><span class="params">(points, angle, center)</span>:</span></div><div class="line"> cos_val = math.cos(angle)</div><div class="line"> sin_val = math.sin(angle)</div><div class="line"> cx, cy = center</div><div class="line"> new_points = []</div><div class="line"> <span class="keyword">for</span> x_old, y_old <span class="keyword">in</span> points:</div><div class="line"> x_old -= cx</div><div class="line"> y_old -= cy</div><div class="line"> x_new = x_old * cos_val - y_old * sin_val</div><div class="line"> y_new = x_old * sin_val + y_old * cos_val</div><div class="line"> new_points.append([x_new + cx, y_new + cy])</div><div class="line"> <span class="keyword">return</span> new_points</div></pre></td></tr></table></figure></p>
<p>以下为绘图函数，输入为几何元素canvas，ode解法得到的角度list，使用i索引list元素，无返回值。遍历解出的角度list即可画出旋转矩形；为实现动画效果，需使用定时任务，在该函数中使用了time模块的sleep()函数<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">def</span> <span class="title">draw_square</span><span class="params">(cv,ans,i)</span>:</span></div><div class="line"> center_l = (CANVAS_MID_X+<span class="number">100</span>, CANVAS_MID_Y+<span class="number">100</span>)</div><div class="line"> center_m = (CANVAS_MID_X, CANVAS_MID_Y)</div><div class="line"> points_l = rotate(square_l, ans[i,<span class="number">0</span>], center_l)</div><div class="line"> points_m = rotate(square_m, ans[i,<span class="number">2</span>], center_m)</div><div class="line"> cv.create_polygon(points_l, fill=<span class="string">"blue"</span>)</div><div class="line"> cv.create_polygon(points_m, fill=<span class="string">"red"</span>)</div><div class="line"> cv.update()</div><div class="line"> time.sleep(<span class="number">0.01</span>)</div><div class="line"> cv.delete(<span class="string">"all"</span>)</div></pre></td></tr></table></figure></p>
<p>以下为开始按钮的事件函数，输入为几何元素canvas，ode解法得到的角度list，Tk的界面对象。<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">def</span> <span class="title">draw_start</span><span class="params">(canvas,ans,root)</span>:</span></div><div class="line"> <span class="keyword">for</span> i <span class="keyword">in</span> range(<span class="number">0</span>, <span class="number">500</span>):</div><div class="line"> draw_square(canvas,ans,i)</div><div class="line"> <span class="keyword">if</span> i == <span class="number">499</span>:</div><div class="line"> root.destroy()</div><div class="line"> root.mainloop()</div></pre></td></tr></table></figure></p>
<p>以下为数据过程可视化函数，输入为ode解法得到的角度list。封装canvas元素，label元素(用以显示解释性文字)，buttom元素(开始绘图功能支持)。<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">def</span> <span class="title">visualize_process</span><span class="params">(ans)</span>:</span></div><div class="line"> root = Tk()</div><div class="line"> canvas = Canvas(root, bg=<span class="string">"black"</span>, height=HEIGHT, width=WIDTH)</div><div class="line"> canvas.pack()</div><div class="line"> Label_l = Label(root, text=<span class="string">"blue--Load"</span>, font=<span class="string">"Times 16 bold"</span>)</div><div class="line"> Label_l.pack()</div><div class="line"> Label_m = Label(root, text=<span class="string">"red--motor"</span>, font=<span class="string">"Times 16 bold"</span>)</div><div class="line"> Label_m.pack()</div><div class="line"> btn = Button(root, text=<span class="string">"run"</span>, command=<span class="keyword">lambda</span>:draw_start(canvas,ans,root))</div><div class="line"> btn.pack()</div></pre></td></tr></table></figure></p>
<p>以下为数据图表可视化函数，输入为ode解法得到的角度list。使用matplotlib模块中的plot函数绘制<span class="math inline">\(\theta_l,\dot{\theta}_l,\theta_m,\dot{\theta}_m\)</span>关于变量<span class="math inline">\(t\)</span>的曲线。<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div></pre></td><td class="code"><pre><div class="line"><span class="function"><span class="keyword">def</span> <span class="title">visualize_figure</span><span class="params">(ans)</span>:</span></div><div class="line"> t_ = np.linspace(<span class="number">0</span>, <span class="number">100</span>, np.size(ans[:, <span class="number">0</span>]))</div><div class="line"> thetal,= plt.plot(t_, ans[:,<span class="number">0</span>],label=<span class="string">'thetal'</span>)</div><div class="line"> thetal_dot,= plt.plot(t_, ans[:,<span class="number">1</span>],label=<span class="string">'thetal_dot'</span>)</div><div class="line"> thetam, = plt.plot(t_, ans[:,<span class="number">2</span>],label=<span class="string">'thetam'</span>)</div><div class="line"> thetam_dot, = plt.plot(t_, ans[:,<span class="number">3</span>],label=<span class="string">'thetam_dot'</span>)</div><div class="line"> plt.legend(handles=[thetal, thetal_dot, thetam, thetam_dot])</div><div class="line"> plt.show()</div></pre></td></tr></table></figure></p>
<p>以下为主函数，初始化数据，求解模型的状态方程，并分别调用过程可视化函数，图表可视化函数。<br>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div></pre></td><td class="code"><pre><div class="line"><span class="keyword">if</span> __name__ == <span class="string">'__main__'</span>:</div><div class="line"> tspan = np.linspace(<span class="number">0</span>, <span class="number">100</span>, <span class="number">500</span>)</div><div class="line"> initial_conditions = {<span class="string">'y0'</span>: [<span class="number">0</span>,<span class="number">0</span>,<span class="number">0</span>,<span class="number">0</span>]}</div><div class="line"> model_params = {<span class="string">'A'</span>: np.mat([<span class="number">0</span>, <span class="number">1</span>, <span class="number">0</span>, <span class="number">0</span>,</div><div class="line"> -k / J_l, -B_l / J_l, k / J_l, <span class="number">0</span>,</div><div class="line"> <span class="number">0</span>, <span class="number">0</span>, <span class="number">0</span>, <span class="number">1</span>,</div><div class="line"> k / J_m, <span class="number">0</span>, -k / J_m, -B_m / J_m</div><div class="line"> ]).reshape(<span class="number">4</span>,<span class="number">4</span>),</div><div class="line"> <span class="string">'b'</span>: np.mat([<span class="number">0</span>, <span class="number">0</span>, <span class="number">0</span>, <span class="number">1</span> / J_m]).reshape(<span class="number">4</span>,<span class="number">1</span>)</div><div class="line"> }</div><div class="line"> data = data_init(initial_conditions, model_params)</div><div class="line"> y = odeint(f,data[<span class="number">0</span>],tspan,args=(data[<span class="number">1</span>], data[<span class="number">2</span>]))</div><div class="line"> visualize_process(y)</div><div class="line"> visualize_figure(y)</div></pre></td></tr></table></figure></p>
<h3 id="状态方程转化为传递函数">状态方程转化为传递函数</h3>
<p>传递函数可用来描述线性非时变系统的特性。一个连续线性非时变系统可通过以下方法将状态方程转化为传递函数。<br>
首先对下式进行拉氏变换：<br>
<span class="math display">\[ \dot{X} = AX + Bu\]</span> 可以得到：<br>
<span class="math display">\[sX(s) = AX(s) + Bu(s)\]</span> 再针对<span class="math inline">\(X(s)\)</span>进行化简可得到：<br>
<span class="math display">\[(sI-A)X(s)=Bu(s)\]</span><br>
<span class="math display">\[X(s) = (sI-A)^{-1}Bu(s)\]</span><br>
用上式替换以下输出方程中的<span class="math inline">\(X(s)\)</span>：<br>
<span class="math display">\[y(s) = CX(s)+Du(s)\]</span><br>
结果如下：<br>
<span class="math display">\[y(s) = C((sI-A)^{-1}Bu(s))+Du(s)\]</span><br>
由传递函数的定义<span class="math inline">\(G(s) = \frac{y(s)}{u(s)}\)</span>：<br>
可导出：<br>
<span class="math display">\[G(s) = C(sI-A)^{-1}B + D\]</span><br>
<strong>传递函数</strong> 使用系统的状态方程的系数矩阵求解，求解出的结果为：<br>
<span class="math display">\[G(s) = \frac{k}{J_lJ_ms^4 + (B_lJ_m+ B_mJ_l)s^3 + (B_lB_m+J_lk+J_mk)s^2 + (B_lk+ B_mk)s }\]</span></p>
<p>代入已知系数，该系统的传递函数为：<br>
<span class="math display">\[G(s) = \frac{6000}{100s^4+20s^3+12001s^2+1200s}\]</span></p>
<h3 id="能观性与能控性讨论">能观性与能控性讨论</h3>
<h4 id="能控性讨论">能控性讨论</h4>
<p><strong>定理：</strong> 系统<span class="math inline">\(\Sigma = (A,B)\)</span>，即<br>
<span class="math display">\[\dot{X} = AX+Bu\]</span><br>
<span class="math display">\[y = CX+Du\]</span><br>
状态完全能控的充分必要条件是其能控性矩阵<br>
<span class="math display">\[Q_k = \lbrack B \vdots AB \vdots \cdots \vdots A^{n-1}B \rbrack\]</span><br>
满秩，即<br>
<span class="math display">\[rank\lbrack B \vdots AB \vdots \cdots \vdots A^{n-1}B \rbrack = n\]</span><br>
由系统的状态方程系数矩阵<span class="math inline">\(A,B\)</span>求出其能控性矩阵为：<br>
<span class="math display">\[Q_k =
\begin{bmatrix}
0 & 0 & 0 & \frac{k}{J_lJ_m} \\
0 & 0 & \frac{k}{J_lJ_m} & -\frac{B_lk}{J_mJ_l^2} - \frac{B_mk}{J_lJ_m^2} \\
0 & \frac{1}{J_m} & -\frac{B_m}{J_m^2} &-\frac{k}{J_m^2} - \frac{B_m^2}{J_m^3} \\
\frac{1}{J_m} & -\frac{B_m}{J_m^2} & -\frac{k}{J_m^2} -\frac{B_m^2}{J_m^3} & \frac{B_mk}{J_m^3} + \frac{B_mk}{J_m^3} - \frac{B_m^3}{J_m^4} \\
\end{bmatrix}
\]</span><br>
该矩阵的行列式可以按下式求解：<br>
<span class="math display">\[det(Q_k) =
-\frac{k}{J_lJ_m} \times \frac{k}{J_lJ_m} \times
\begin{vmatrix}
0 & \frac{1}{J_m} \\ \frac{1}{J_m} & -\frac{B_m}{J_m^2} \\
\end{vmatrix}
=\frac{k^2}{J_l^2J_m^2}\frac{1}{Jm^2}
\]</span><br>
由于<span class="math inline">\(J_l,J_m\neq 0\)</span>，所以当且仅当<span class="math inline">\(k = 0\)</span>时系统不能控。</p>
<h4 id="能观性讨论">能观性讨论</h4>
<p><strong>定理：</strong>系统<span class="math inline">\(\Sigma = (A,C)\)</span>，即<br>
<span class="math display">\[\dot{X} = AX\]</span><br>
<span class="math display">\[y = CX\]</span><br>
状态完全能观测的充分必要条件是其能能观测性矩阵<br>
<span class="math display">\[Q_g = \lbrack C \vdots A^TC^T \vdots \cdots \vdots (A^T)^{n-1}C^T \rbrack\]</span><br>
满秩，即<br>
<span class="math display">\[Q_g =
\begin{bmatrix}
C \\
CA \\
\vdots \\
CA^{n-1} \\
\end{bmatrix}
\]</span> 满秩。<br>
由系统的状态方程系数矩阵<span class="math inline">\(A,C\)</span>求出其能观测性矩阵为：<br>
<span class="math display">\[Q_g =
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
-\frac{k}{J_l} & -\frac{B_l}{J_l} & \frac{k}{J_l} & 0 \\
\frac{B_lk}{J_l^2} & \frac{B_l^2}{J_l^2} - \frac{k}{J_l} & -\frac{B_lk}{J_l^2} & \frac{k}{J_l}\\
\end{bmatrix}
\]</span> 该矩阵的行列式可以按下式求解：<br>
<span class="math display">\[det(Q_g)=
1\times 1 \times
\begin{vmatrix}
\frac{k}{J_l} & 0 \\
-\frac{B_lk}{J_l^2} & \frac{k}{J_l} \\
\end{vmatrix}
= \frac{k^2}{J_l^2}
\]</span> 由于<span class="math inline">\(J_l\neq 0\)</span>，所以当且仅当<span class="math inline">\(k = 0\)</span>时系统不能观测。</p>
<h3 id="能控规范型和能观测规范型">能控规范型和能观测规范型</h3>
<h4 id="单输入-单输出系统的能控规范型">单输入-单输出系统的能控规范型</h4>
单自由度弹性关节机器人系统的能控性矩阵： <span class="math display">\[Q_k =
\begin{bmatrix}
0 & 0 & 0 & \frac{k}{J_lJ_m} \\
0 & 0 & \frac{k}{J_lJ_m} & -\frac{B_lk}{J_mJ_l^2} - \frac{B_mk}{J_lJ_m^2} \\
0 & \frac{1}{J_m} & -\frac{B_m}{J_m^2} &-\frac{k}{J_m^2} - \frac{B_m^2}{J_m^3} \\
\frac{1}{J_m} & -\frac{B_m}{J_m^2} & -\frac{k}{J_m^2} -\frac{B_m^2}{J_m^3} & \frac{B_m k}{J_m^3} + \frac{B_m k}{J_m^3} - \frac{B_m^3}{J_m^4} \\
\end{bmatrix}
\]</span><br>
为非奇异，则存在非奇异变换： <span class="math display">\[\widehat{X}=PX\]</span><br>
或<br>
<span class="math display">\[X=P^{-1}\widehat{X}\]</span><br>
系统可化为能控规范型，即 <span class="math display">\[P_1=
\begin{bmatrix}
0 & 0 & 0 & 1 \\
\end{bmatrix}Q_k^{-1}
\]</span> 求得 <span class="math display">\[P_1 =
\begin{bmatrix}
\frac{J_lJ_m}{k} & 0 & 0 & 0\\
\end{bmatrix}
\]</span> 变换矩阵为 <span class="math display">\[ P =
\begin{bmatrix}
P_1 \\
P_1A \\
P_1A^2 \\
P_1A^3 \\
\end{bmatrix}
=
\begin{bmatrix}
\frac{J_lJ_m}{k} & 0 & 0 & 0 \\
0 & \frac{J_lJ_m}{k} & 0 & 0 \\
-J_m & -\frac{B_lJ_m}{k} & J_m & 0\\
\frac{B_l J_m}{J_l} & \frac{J_m B_L^2}{J_l k}-J_m & -\frac{B_l J_m}{J_l} & J_m \\
\end{bmatrix}
\]</span> 因此 <span class="math display">\[ \widehat{A} = PAP^{-1}=
\begin{bmatrix}
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
0 & -\frac{k(B_l + B_m)}{J_l J_m} & -\frac{(J_l k + J_m k + B_l B_m)}{(J_l k + J_m k + B_l B_m)} & -\frac{B_l J_m + B_m J_l}{J_l*J_m} \\
\end{bmatrix} \qquad \widehat{B} = PB =
\begin{bmatrix}
0 & 0 & 0 & 1 \\
\end{bmatrix}^T
\]</span> 故可将状态方程化为能控规范性 {
<span class="math display">\[\begin{array}{ll}
\dot{\widehat{X}} = \widehat{A}\widehat{X}+\widehat{B}u \\
y = \widehat{C}\widehat{X}
\end{array}\]</span>
<p>.</p>
<h4 id="单输入-单输出系统的能观测规范型">单输入-单输出系统的能观测规范型</h4>
<p>单自由度弹性关节机器人系统的能观测性矩阵： <span class="math display">\[Q_g^T =
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
-\frac{k}{J_l} & -\frac{B_l}{J_l} & \frac{k}{J_l} & 0 \\
\frac{B_lk}{J_l^2} & \frac{B_l^2}{J_l^2} - \frac{k}{J_l} & -\frac{B_lk}{J_l^2} & \frac{k}{J_l}\\
\end{bmatrix}
\]</span><br>
为非奇异，则存在非奇异变换： <span class="math display">\[X(t)=T\widehat{X}(t)\]</span><br>
或 <span class="math display">\[\widehat{X}(t)=T^{-1}X(t)\]</span> 系统可化为能控规范型，即 <span class="math display">\[T_1 = (Q_g^T)^{-1}
\begin{bmatrix}
0 & 0 & 0 & 1 \\
\end{bmatrix}^T
\]</span></p>
求得 <span class="math display">\[T_1 =
\begin{bmatrix}
0 & 0 & 0 & \frac{J_l}{k} \\
\end{bmatrix}^T
\]</span><br>
变换矩阵为 <span class="math display">\[T =
\begin{bmatrix}
T_1 & A T_1 & A^2 T_1 & A^3 T_1 \\
\end{bmatrix} =
\begin{bmatrix}
0 & 0 & 0 & 1 \\
0 & 0 & 1 & -\frac{B_l}{J_l}-\frac{B_m}{J_m}\\
0 & \frac{J_l}{k} & -\frac{B_m J_l}{J_m*k} & \frac{B_m^2 J_l}{J_m^2 k}-\frac{J_l}{J_m} \\
\frac{J_l}{k} & -\frac{B_m J_l}{J_m k} & \frac{B_m^2 J_l}{J_m^2 k}-\frac{J_l}{J_m} & \frac{k B_m J_l + k B_m J_m^2 - J_m B_m^3}{k J_m^3} \\
\end{bmatrix}
\]</span><br>
因此 <span class="math display">\[\widehat{A} = T^{-1}AT =
\begin{bmatrix}
0 & 0 & 0 & 0 \\
1 & 0 & 0 & -\frac{k(B_l + B_m)}{J_l J_m} \\
0 & 1 & 0 & -\frac{(J_l k + J_m k + B_l B_m}{J_l J_m} \\
0 & 0 & 1 & -\frac{B_l J_m + B_m J_l}{J_l J_m}]
\end{bmatrix} \qquad \widehat{C} = CT =
\begin{bmatrix}
0 & 0 & 0 & 1 \\
\end{bmatrix}
\]</span> 故可将状态方程化为能观测规范性 {
<span class="math display">\[\begin{array}{ll}
\dot{\widehat{X}} = \widehat{A}\widehat{X}+\widehat{B}u \\
y = \widehat{C}\widehat{X}
\end{array}\]</span>
<p>.</p>
<h1 id="控制系统设计">控制系统设计</h1>
<p>系统给出的参数为<span class="math inline">\(J_m=2,J_l=10,B_m=0.5,B_l=1,k=111\)</span> ## 开环阶跃响应仿真 使用MATLAB将系统的状态空间方程转换为传递函数形式，并观察阶跃响应，代码如下：<br>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div></pre></td><td class="code"><pre><div class="line">[num_1,den_1] = ss2tf(A,B,C(<span class="number">1</span>,:),D);sys_1 = tf(num_1,den_1);</div><div class="line">[num_2,den_2] = ss2tf(A,B,C(<span class="number">2</span>,:),D);sys_2 = tf(num_2,den_2);</div><div class="line">[num_3,den_3] = ss2tf(A,B,C(<span class="number">3</span>,:),D);sys_3 = tf(num_3,den_3);</div><div class="line">[num_4,den_4] = ss2tf(A,B,C(<span class="number">4</span>,:),D);sys_4 = tf(num_4,den_4);</div><div class="line">t = (<span class="number">0</span>:<span class="number">0.1</span>:<span class="number">40</span>)';</div><div class="line">[y1, ~, ~, ysd1] = step(sys_1,t);[y2, ~, ~, ysd2] = step(sys_2,t);</div><div class="line">[y3, ~, ~, ysd3] = step(sys_3,t);[y4, ~, ~, ysd4] = step(sys_4,t);</div><div class="line">subplot(<span class="number">2</span>,<span class="number">1</span>,<span class="number">1</span>)</div><div class="line">plot(t, y1, <span class="string">':b'</span>,t, y3, <span class="string">'r'</span>);legend(<span class="string">'\theta_l'</span>,<span class="string">'\theta_m'</span>);</div><div class="line">title(<span class="string">'Step Response(\theta)'</span>)</div><div class="line">subplot(<span class="number">2</span>,<span class="number">1</span>,<span class="number">2</span>)</div><div class="line">plot(t, y2, <span class="string">':m'</span>,t, y4, <span class="string">'y'</span>);legend(<span class="string">'\omega_l'</span>,<span class="string">'\omega_m'</span>);</div><div class="line">title(<span class="string">'Step Response(\omega)'</span>)</div></pre></td></tr></table></figure></p>
<p><em>Notes: 这里将输出矩阵<span class="math inline">\(C\)</span>改为了四阶单位矩阵，这样可以观察系统各状态的阶跃响应情况。</em></p>
<p><strong>阶跃响应曲线</strong> <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhlu3fkavcj30ns0jywg5.jpg" alt="单自由度弹性关节阶跃响应曲线"></p>
<p>由阶跃响应曲线可以看出，电机和负载的转速基本保持同步，但二者相互影响；从角速度曲线上可以看出二者的角速度都存在锯齿，一段时间后保持稳定。电机和负载的转速均线性增加。</p>
<h2 id="状态反馈控制器设计">状态反馈控制器设计</h2>
<h3 id="根据性能指标要求确定系统主导极点">根据性能指标要求确定系统主导极点</h3>
这是一个主导极点问题。在稳定的高阶系统中，对于其时间响应起主导作用的闭环极点为主导极点，远离原点的极点所对应的响应影响很小，离虚轴最近且邻近没有零点的极点，其对应的暂态响应分量衰减最慢，幅值也大。正是如此，将本例中讨论的四阶系统近似为二阶系统分析。<br>
系统要求的性能指标为超调量<span class="math inline">\(\sigma \le 5\%\)</span>;调节时间$ t_s 0.5s<span class="math inline">\(; 系统频宽\)</span>BW 10$。<br>
由超调量定义公式得到欠阻尼典型二阶系统的超调量为: <span class="math display">\[\sigma \% = \frac{c(t_p)-1}{1}\times 100\% = e^{-\pi \xi/\sqrt{1-\xi^2}}\times 100\%\]</span><br>
超调量<span class="math inline">\(\sigma \le 5\%\)</span>，取超调量为<span class="math inline">\(1.52\%\)</span>,解得<span class="math inline">\(\xi = 0.8\)</span>。<br>
由调节时间公式: <span class="math display">\[t_s \approx \frac{4}{\xi \omega_n}\]</span><br>
调节时间间$ t_s 0.5s$，取调节时间为<span class="math inline">\(0.5s\)</span>,解得<span class="math inline">\(\omega_n = 10\)</span><br>
由典型二阶系统的带宽频率公式： <span class="math display">\[BW = \omega_n\sqrt{(1-2\xi^2)+\sqrt{(1-2\xi^2)^2+1}}\]</span><br>
验证得出系统的带宽为<span class="math inline">\(8.7090\)</span>。 由典型二阶欠阻尼系统的关系式： <span class="math display">\[s_{1,2}=-\xi\omega_n=\pm j\omega_n\sqrt{1-\xi^2}\]</span> <span class="math display">\[s^2+2\xi\omega_n s+\omega_n^2=0\]</span><br>
得出配置的闭环主导极点为： <span class="math display">\[s_{1,2} = -8\pm 6j\]</span><br>
远极点应选择使它和原点的距离大于<span class="math inline">\(5\mid s_1 \mid\)</span>的点，所以确定希望的极点为：<br>
{
<span class="math display">\[\begin{array}{ll}
s_1 = -8 + 6j \\
s_2 = -8 - 6j \\
s_3 = -50 \\
s_4 = -60 \\
\end{array}\]</span>
<p>.</p>
<h3 id="确定状态反馈矩阵widehatk">确定状态反馈矩阵<span class="math inline">\(\widehat{K}\)</span></h3>
<p>受控系统的特征多项式为 <span class="math display">\[f(s)=s^4+0.35s^3+66.625s^2+8.325s;\qquad a_1=0.35\quad a_2=66.625\quad a_3=8.325\quad a_4 =0\]</span> 由希望的极点构成的特征多项式为 <span class="math display">\[f^*(s)=(s+50)(s+60)(s+8-6j)(s+8+6j) = s^4 + 126s^3 + 4860s^2 + 59000s + 300000\]</span> <span class="math display">\[a_1^* = 126 \quad a_2^* = 4860 \quad a_3^* = 59000 \quad a_4^* = 300000
\]</span><br>
于是状态反馈矩阵<span class="math inline">\(\widehat{K}\)</span>为 <span class="math display">\[K=
\begin{bmatrix}
300000 & 58991.675 & 4793.375 & 125.65\\
\end{bmatrix}\]</span> 若需将其转化为能控规范性反馈矩阵可按<span class="math inline">\(\widehat{K} = KP\)</span></p>
<h3 id="确定输入放大系数l">确定输入放大系数<span class="math inline">\(L\)</span></h3>
<p>已知闭环传递函数为 <span class="math display">\[W_K(s)=\frac{5.55L}{s^4 + 126 s^3 + 4860 s^2 + 59000 s + 300000}\]</span><br>
要求跟踪信号阶跃信号的误差<span class="math inline">\(e_p = 0\)</span>，有<br>
<span class="math display">\[e_p = 0 = \lim\limits_{t\rightarrow{\infty}}[1-y(t)]=\lim\limits_{s\rightarrow{\infty}}s[\frac{1}{s}-\frac{W_K(s)}{s}]=\lim\limits_{s\rightarrow{\infty}}[1-W_K(s)] = \frac{300000-5.55L}{300000}=0\]</span><br>
解得<br>
<span class="math display">\[L = 54055\]</span></p>
<h3 id="matlab阶跃响应仿真">MATLAB阶跃响应仿真</h3>
<p>使用MATLAB进行极点配置及绘制单位阶跃响应曲线的代码如下：<br>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div></pre></td><td class="code"><pre><div class="line">point = [<span class="number">-8</span>+<span class="number">6</span>i <span class="number">-8</span><span class="number">-6</span>i <span class="number">-50</span> <span class="number">-60</span>];</div><div class="line">k =place(A,B,point);</div><div class="line">A = A - B*k;</div><div class="line">[num,den] = ss2tf(A,B,C,D);</div><div class="line">sys = tf(<span class="number">54055</span>*num,den);</div><div class="line">t = (<span class="number">0</span>:<span class="number">0.01</span>:<span class="number">1</span>)';</div><div class="line">[y, ~, ~, ysd] = step(sys,t);</div><div class="line">plot(t, y, <span class="string">'*r'</span>);axis([<span class="number">0</span> <span class="number">1</span> <span class="number">0</span> <span class="number">1.2</span>]);</div><div class="line">title(<span class="string">'Step Response(\theta)'</span>)</div></pre></td></tr></table></figure></p>
<p>绘制出的阶跃响应曲线如下图所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhlu82s1pqj30fk0dpgmi.jpg" alt="系统配置极点后的阶跃响应曲线"></p>
<p>并使用MATLAB计算超调量及调节时间，可用以下代码： <figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div></pre></td><td class="code"><pre><div class="line">overshoot = max(y) - y(<span class="keyword">end</span>)</div><div class="line">ts = t(max(<span class="built_in">find</span>(<span class="built_in">abs</span>(y<span class="number">-1</span>)><span class="number">0.05</span>)))</div></pre></td></tr></table></figure></p>
<p>解出超调量为<span class="math inline">\(1.46\%\)</span>，调节时间为<span class="math inline">\(0.37s\)</span>，符合之前设置的性能指标要求。</p>
<h2 id="带观测器状态反馈闭环系统">带观测器状态反馈闭环系统</h2>
由于并非所有的状态变量都能够被测量到。因此，为了实现状态反馈控制律，就必须利用已知信息<span class="math inline">\((y,u)\)</span>，通过模型来对系统的状态变量进行估计。 ### 观测器极点位置选择 观测器极点的选取通常使观测器的响应比系统的响应要快得多，一个经验法则是选择观测器的响应至少为系统响应的<span class="math inline">\(2\sim 5\)</span>倍。观测器的极点位于所期望的闭环极点的左边，所以后者在响应中起主导作用。 根据以上规律，选择观测器极点为：<br>
{
<span class="math display">\[\begin{array}{ll}
s_1 = -70 \\
s_2 = -71 \\
s_3 = -72 \\
s_4 = -73 \\
\end{array}\]</span>
<p>.</p>
<h3 id="确定观测器方程">确定观测器方程</h3>
<p>设观测器增益为<span class="math inline">\(G = [g_2 g_1]^T\)</span>，观测器期望的多项式为 <span class="math display">\[a^*(s)=(s+70)(s+71)(s+72)(s+73)\]</span> 观测器的特征多项式为： <span class="math display">\[det[sI-(A-GC)]\]</span> 可求观测器方程： <span class="math display">\[\dot{\widehat{x}}=(A-G C)\widehat{x}+B u+G y\]</span> 带观测器的状态反馈系统可以用分块矩阵表示： <span class="math display">\[
\begin{pmat}[{.}]
\dot{x} \cr \-
\dot{x} - \dot{\widetilde{x}} \cr
\end{pmat}=
\begin{pmat}[{|}]
A-B K & BK \cr \-
0 & A-G C \cr
\end{pmat}
\begin{pmat}[{.}]
X \cr \-
X - \widetilde{X} \cr
\end{pmat}
+
\begin{pmat}[{.}]
B \cr \-
0 \cr
\end{pmat}
u
\]</span> <span class="math display">\[y=
\begin{pmat}[{|}]
C & 0 \cr
\end{pmat}
\begin{pmat}[{.}]
X \cr \-
X - \bar{X} \cr
\end{pmat}
\]</span> ### MATLAB阶跃响应仿真 用MATLAB分别计算状态反馈矩阵和观测器反馈矩阵，构造分块矩阵并绘制阶跃响应曲线，代码如下：<br>
<figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div></pre></td><td class="code"><pre><div class="line">pointK = [<span class="number">-8</span>+<span class="number">6</span>i <span class="number">-8</span><span class="number">-6</span>i <span class="number">-50</span> <span class="number">-60</span>];pointG = [<span class="number">-70</span> <span class="number">-71</span> <span class="number">-72</span> <span class="number">-73</span>];</div><div class="line">K =place(A,B,pointK);G = place(A',C',pointG)';</div><div class="line">A_ = [A-B*K B*K;zeros(size(A)) A-G*C];B_ = [B;zeros(size(B))];</div><div class="line">C_ = [C zeros(size(C))];D_ = <span class="number">0</span>;</div><div class="line">sys = ss(A_,<span class="number">54055</span>*B_,C_,D_);</div><div class="line">t = (<span class="number">0</span>:<span class="number">0.01</span>:<span class="number">1</span>)';u = <span class="built_in">ones</span>(<span class="built_in">size</span>(t));</div><div class="line">[y,x] = lsim(sys,u,t);</div><div class="line">plot(t, y,<span class="string">'*b'</span>);axis([<span class="number">0</span> <span class="number">1</span> <span class="number">0</span> <span class="number">1.2</span>]);</div><div class="line">title(<span class="string">'Step Response(\theta)'</span>);</div><div class="line">overshoot = max(y) - y(<span class="keyword">end</span>)</div><div class="line">t_ = t(max(<span class="built_in">find</span>(<span class="built_in">abs</span>(y<span class="number">-1</span>)><span class="number">0.05</span>)))</div></pre></td></tr></table></figure></p>
<div class="figure">
<img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhlub1yra0j30fk0dpjsb.jpg" alt="带观测器状态反馈闭环系统阶跃响应曲线">
<p class="caption">带观测器状态反馈闭环系统阶跃响应曲线</p>
</div>
<p>这个响应结果几乎和之前没有采用观测器时的结果完全一致。这是因为我们假设的观测器模型与实际系统模型相同（包括有相同的初始条件）。所以，在最小的控制信号代价下，所有的设计要求都得到了满足，无需再次设计。</p>
<h2 id="观测器期望极点比较性验证">观测器期望极点比较性验证</h2>
<p>在本例中，真实状态是无法观测的，但由于在MATLAB仿真中设置的观测器模型与系统模型完全相同，初值也相同，所以任意配置极点都能保证系统状态的准确估计。为了验证观测器极点位置对观测器逼近被估计状态速度的影响，可设置一个观测器初值误差，观察系统的响应情况。<br>
由此。将观测器初值设置为<span class="math inline">\([1\quad0 \quad 0 \quad 0]\)</span>，即负载转角初值未测准，误差<span class="math inline">\(1rad\)</span>。 分别配置观测器极点为</p>
P_1 = {
<span class="math display">\[\begin{array}{ll}
s_1 = -12 \\
s_2 = -13 \\
s_3 = -14 \\
s_4 = -15 \\
\end{array}\]</span>
. P_2 = {
<span class="math display">\[\begin{array}{ll}
s_1 = -42 \\
s_2 = -43 \\
s_3 = -44 \\
s_4 = -45 \\
\end{array}\]</span>
. P_3 = {
<span class="math display">\[\begin{array}{ll}
s_1 = -72 \\
s_2 = -73 \\
s_3 = -74 \\
s_4 = -75 \\
\end{array}\]</span>
<p>.</p>
<p>使用以下代码配置初值，并分析阶跃响应情况： <figure class="highlight matlab"><table><tr><td class="gutter"><pre><div class="line">1</div></pre></td><td class="code"><pre><div class="line">[y,x] = lsim(A_,<span class="number">54055</span>*B_,C_,D_,u,t,[<span class="number">0</span>;<span class="number">0</span>;<span class="number">0</span>;<span class="number">0</span>;<span class="number">1</span>;<span class="number">0</span>;<span class="number">0</span>;<span class="number">0</span>]);</div></pre></td></tr></table></figure></p>
<p>得到的观测器响应曲线如下图所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhlucs81b1j30fk0dpab5.jpg" alt="观测器响应曲线"></p>
<p>根据经典控制理论，我们知道闭环极点离开虚轴的距离越大，系统的调节时间越短。以上仿真的结果和理论是一致的，由图可以看出配置<span class="math inline">\(P_3\)</span>极点，观测器的估计收敛速度最快。</p>
<p><strong>系统的阶跃响应情况</strong> <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhludzffqsj30fk0dp400.jpg" alt="电机和负载的转角变化情况"> <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fhluecdlyij30fk0dpgn5.jpg" alt="电机和负载的角速度变化情况"></p>
<p>在初值误差相同的情况下，观测器闭环极点距离虚轴越远，响应速度越快，跟随性能越好。但是相对地，超调量也会增加。这同时也表征了观测器快速性和抗干扰性的矛盾。观测器的最大响应速度受到控制系统中的噪声和灵敏性的限制。为了保证系统有足够的抗扰性能，通常设计<span class="math inline">\(G\)</span>使观测器比被估系统稍微快些就可以了。</p>
]]></content>
<summary type="html">
<ul>
<li>这篇文章是现代控制理论课程设计的前半部分，主要研究了单自由度弹性关节系统的工程背景，动力学建模；控制系统方面包括状态方程建立，状态转移矩阵计算，零输入给定初值<code>MATLAB</code>与<code>Python Tkinter</code>模型仿真，状态方程与传递函数之间的转换，系统能观性和能控性的讨论。</li>
<li>单自由度弹性关节示意图如下： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1ffn2m7gikuj30by06pq3b.jpg" alt="单自由度弹性关节" /></li>
</ul>
<p><em>P.S.</em> 这篇文章的原排版系统为<code>LaTeX</code>，在<a href="https://www.sharelatex.com/project/5915c87ed1d7c39a6d050f86">ShareLaTeX</a>上可以导出<code>PDF</code>格式的报告，同时<code>LaTeX Source</code>也开放了公共阅读权限。
</summary>
<category term="Robot" scheme="https://rm-rf.moe/categories/Robot/"/>
<category term="Python" scheme="https://rm-rf.moe/tags/Python/"/>
<category term="MATLAB" scheme="https://rm-rf.moe/tags/MATLAB/"/>
</entry>
<entry>
<title>神经网络库Keras解决Regression问题</title>
<link href="https://rm-rf.moe/2017/04/01/2017-04-01-regressionbykeras/"/>
<id>https://rm-rf.moe/2017/04/01/2017-04-01-regressionbykeras/</id>
<published>2017-04-01T13:11:18.000Z</published>
<updated>2017-08-27T07:05:59.000Z</updated>
<content type="html"><![CDATA[<blockquote>
<p>Can machines think? –AM Turing</p>
</blockquote>
<ul>
<li>写在文前：最近在学机器学习的相关知识，由于相关文档和更新速度的关系选择了当前较为流行的<a href="https://keras.io/" target="_blank" rel="external">Keras</a>，一个基于python的深度学习框架。学校正在进行一个基于深度学习/强化学习(reinforcement learning)控制水下滑翔机的项目，我学习了一下Keras，并做了一些练习。这篇文章将水下滑翔机作为问题背景介绍一下用Keras和Sequential模型处理回归问题。</li>
</ul>
<a id="more"></a>
<h2 id="问题背景">问题背景</h2>
<p>水下滑翔机(Underwater glider)是通过两个方向上的可动质量块改变横滚角度和航向角度的，同时角度和滑翔机的深度和油量也有关系。现在我们想利用已知的可动质量块/深度／油量数据预测滑翔机的姿态。我们可以建立一个三输入两输出的Sequential模型，利用已有数据和神经网络算法解决这个问题。下面用两个例子说明怎么使用Keras较好地做回归。</p>
<h2 id="regression的两个例子">Regression的两个例子</h2>
<p>下面的内容包括利用Keras拟合一个单输入单输出的函数和一个双输入单输出的函数。</p>
<h3 id="单输入单输出函数拟合">单输入单输出函数拟合</h3>
<p>这里使用的函数为：<br>
<span class="math display">\[y=cos(arctan(x))\]</span><br>
<code>Sequential</code>是多个网络层的线性堆叠。在这个问题中，我们输入量是一个一阶张量，所以可以通过<code>input_dim</code>来指定输入维度。接下来我们可以建立一个最简单的全连接网络，最后一层的<code>Dense</code>维度同样为1。同时由于输出量有正有负，使用<code>tanh</code>作为激活函数。全部代码如下：</p>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div><div class="line">15</div><div class="line">16</div><div class="line">17</div><div class="line">18</div><div class="line">19</div><div class="line">20</div><div class="line">21</div><div class="line">22</div><div class="line">23</div><div class="line">24</div><div class="line">25</div><div class="line">26</div><div class="line">27</div><div class="line">28</div></pre></td><td class="code"><pre><div class="line"><span class="keyword">import</span> keras</div><div class="line"><span class="keyword">from</span> keras.layers <span class="keyword">import</span> Input,Dense,Activation,Reshape,Flatten,Dropout</div><div class="line"><span class="keyword">from</span> keras.models <span class="keyword">import</span> Sequential,Model</div><div class="line"><span class="keyword">import</span> numpy <span class="keyword">as</span> np</div><div class="line"><span class="keyword">import</span> matplotlib.pyplot <span class="keyword">as</span> plt</div><div class="line"></div><div class="line">X_in = np.linspace(<span class="number">-10.</span>,<span class="number">10.</span>,<span class="number">1000</span>)</div><div class="line"><span class="keyword">print</span> X_in</div><div class="line">Y_in = np.cos(np.tanh(X_in))</div><div class="line"></div><div class="line">X_verify = np.linspace(<span class="number">-10.</span>,<span class="number">10.</span>,<span class="number">1000</span>)</div><div class="line">Y_verify = np.cos(np.tanh(X_verify))</div><div class="line"></div><div class="line">m = Sequential() </div><div class="line">m.add(Dense(<span class="number">128</span>,kernel_initializer=<span class="string">'uniform'</span>, input_dim=<span class="number">1</span>))</div><div class="line">m.add(Activation(<span class="string">'relu'</span>))</div><div class="line">m.add(Dense(<span class="number">64</span>,activation=<span class="string">'relu'</span>))</div><div class="line">m.add(Dense(<span class="number">1</span>,activation=<span class="string">'tanh'</span>))</div><div class="line">m.summary()</div><div class="line">m.compile(optimizer=<span class="string">'sgd'</span>,loss=<span class="string">'mean_absolute_error'</span>, metric=[<span class="string">'accuracy'</span>])</div><div class="line">m.fit(X_in,Y_in,epochs=<span class="number">1000</span>,batch_size=<span class="number">64</span>)</div><div class="line"></div><div class="line">y_verify = m.predict(X_verify)</div><div class="line">x = np.linspace(<span class="number">1</span>,len(y_verify),len(y_verify))</div><div class="line">line_predict, = plt.plot(x,y_verify,label=<span class="string">"perdict value"</span>)</div><div class="line">line_theoretical, = plt.plot(x,Y_verify,label=<span class="string">"theoretical value"</span>)</div><div class="line">plt.legend(handles=[line_predict,line_theoretical])</div><div class="line">plt.show()</div></pre></td></tr></table></figure>
<p>代码大约分为三个部分，首先使用<code>np.linspace</code>生成数据，接下来将生成的<code>x</code>和计算得到的<code>y</code>扔进建好的模型里并<code>fit</code>。最后使用<code>matplotlib</code>绘图，观察预测值和真实值。得到的图像如下： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fe7k69ci03j30qh0iz0u0.jpg" alt="运行结果"></p>
<p><em>Tips</em>：在做这个的时候发现了几个小问题：</p>
<ol style="list-style-type: decimal">
<li><code>batch_size</code>不宜过小，不然在数据集上不收敛</li>
<li>要有足量的数据用于训练，否则容易欠拟合，误差极大</li>
<li>准确来说应该分训练集和测试集验证模型的泛化性能，我没干是因为懒</li>
</ol>
<h3 id="双输入单输出函数拟合">双输入单输出函数拟合</h3>
<p>这里使用的函数是：<br>
<span class="math display">\[z=sin(\sqrt{x^2+y^2})\]</span> 解决这个问题我们需要引入一个<code>Merge</code>层将多个<code>Sequential</code>合并到一个输出，结构如下图所示，图片来自<a href="https://keras-cn.readthedocs.io/en/latest/getting_started/sequential_model/" target="_blank" rel="external">Keras中文文档</a> <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fe7kqqgy8vj30cq0730su.jpg"> 在这个问题中，我们要将两个一维张量输入模型，需添加一个<code>LSTM</code>层，但是我发现这个所谓的<code>LSTM</code>层并不能传入一个一维张量，要求数据的<code>shape</code>必须是二维或以上，不知道是我的打开方式不对还是其他原因，这里存在疑问。为了解决这个问题，我就把输入的两个一维张量<code>reshape</code>了。下面是代码：</p>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div><div class="line">15</div><div class="line">16</div><div class="line">17</div><div class="line">18</div><div class="line">19</div><div class="line">20</div><div class="line">21</div><div class="line">22</div><div class="line">23</div><div class="line">24</div><div class="line">25</div><div class="line">26</div><div class="line">27</div><div class="line">28</div><div class="line">29</div><div class="line">30</div><div class="line">31</div><div class="line">32</div><div class="line">33</div><div class="line">34</div><div class="line">35</div><div class="line">36</div><div class="line">37</div><div class="line">38</div><div class="line">39</div><div class="line">40</div><div class="line">41</div><div class="line">42</div><div class="line">43</div><div class="line">44</div><div class="line">45</div><div class="line">46</div></pre></td><td class="code"><pre><div class="line"><span class="keyword">import</span> keras</div><div class="line"><span class="keyword">from</span> keras.layers <span class="keyword">import</span> Input,Flatten,Dense,LSTM,merge</div><div class="line"><span class="keyword">from</span> keras.models <span class="keyword">import</span> Sequential,Model</div><div class="line"><span class="keyword">import</span> numpy <span class="keyword">as</span> np</div><div class="line"><span class="keyword">import</span> matplotlib <span class="keyword">as</span> mpl</div><div class="line"><span class="keyword">from</span> mpl_toolkits.mplot3d <span class="keyword">import</span> Axes3D</div><div class="line"><span class="keyword">import</span> matplotlib.pyplot <span class="keyword">as</span> plt</div><div class="line"></div><div class="line">fig = plt.figure()</div><div class="line">ax = fig.gca(projection=<span class="string">'3d'</span>)</div><div class="line"></div><div class="line">x = np.arange(<span class="number">-10.</span>,<span class="number">10.</span>,<span class="number">0.01</span>)</div><div class="line">y = np.arange(<span class="number">-10.</span>,<span class="number">10.</span>,<span class="number">0.01</span>)</div><div class="line">z = np.sin(np.sqrt(x**<span class="number">2</span>+y**<span class="number">2</span>))</div><div class="line"></div><div class="line">x = x.reshape(<span class="number">-1</span>,<span class="number">1</span>,<span class="number">1</span>)</div><div class="line">y = y.reshape(<span class="number">-1</span>,<span class="number">1</span>,<span class="number">1</span>)</div><div class="line"></div><div class="line">x_train_a=Input(shape=(<span class="number">1</span>,<span class="number">1</span>))</div><div class="line">x_train_b=Input(shape=(<span class="number">1</span>,<span class="number">1</span>))</div><div class="line">shared_lstm = LSTM(<span class="number">64</span>)</div><div class="line"></div><div class="line">encoded_a = shared_lstm(x_train_a)</div><div class="line">encoded_b = shared_lstm(x_train_b)</div><div class="line"></div><div class="line"></div><div class="line">merged_vector = merge([encoded_a,encoded_b],mode=<span class="string">'concat'</span>,concat_axis=<span class="number">-1</span>)</div><div class="line"></div><div class="line">model = Dense(<span class="number">128</span>,activation=<span class="string">'relu'</span>)(merged_vector)</div><div class="line">model = Dense(<span class="number">64</span>,activation=<span class="string">'relu'</span>)(model)</div><div class="line">predictions = Dense(<span class="number">1</span>,activation=<span class="string">'tanh'</span>)(model)</div><div class="line">m = Model(input=[x_train_a,x_train_b],output = predictions)</div><div class="line">m.compile(optimizer=<span class="string">'sgd'</span>,loss=<span class="string">'mean_absolute_error'</span>,metric=[<span class="string">'accuracy'</span>])</div><div class="line">m.summary()</div><div class="line"></div><div class="line">m.fit([x,y],z,epochs=<span class="number">1600</span>,batch_size=<span class="number">256</span>)</div><div class="line"></div><div class="line">z_verify = m.predict([x,y])</div><div class="line">z_verify = z_verify.reshape(<span class="number">-1</span>)</div><div class="line">x = x.reshape(<span class="number">-1</span>)</div><div class="line">y = y.reshape(<span class="number">-1</span>)</div><div class="line">line_predict, = ax.plot(x,y,z_verify,label=<span class="string">'predict value'</span>)</div><div class="line">line_theoretical, = ax.plot(x,y,z,label=<span class="string">'theoretical value'</span>)</div><div class="line">plt.legend(handles=[line_predict,line_theoretical])</div><div class="line"></div><div class="line">plt.show()</div></pre></td></tr></table></figure>
<p>这个模型和前面提到的是一样的，实现的效果基本上令人满意。程序结构和上一个基本相同，分为三个部分：生成数据，训练数据，验证数据。下面是程序运行的结果图，使用<code>Matplotlib</code>绘制： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fe7l7uflunj30r10joq4t.jpg" alt="运行结果"></p>
<h3 id="单输入双输出函数拟合">单输入双输出函数拟合</h3>
<p>这里使用了两个函数：<br>
<span class="math display">\[y=cos(arctan(x))\]</span> <span class="math display">\[y=sin(x)\]</span><br>
在这个问题中我们需要增加一个输出层，代码如下：</p>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div><div class="line">15</div><div class="line">16</div><div class="line">17</div><div class="line">18</div><div class="line">19</div><div class="line">20</div><div class="line">21</div><div class="line">22</div><div class="line">23</div><div class="line">24</div><div class="line">25</div><div class="line">26</div><div class="line">27</div><div class="line">28</div><div class="line">29</div><div class="line">30</div><div class="line">31</div><div class="line">32</div><div class="line">33</div><div class="line">34</div><div class="line">35</div><div class="line">36</div><div class="line">37</div><div class="line">38</div><div class="line">39</div><div class="line">40</div><div class="line">41</div><div class="line">42</div><div class="line">43</div><div class="line">44</div></pre></td><td class="code"><pre><div class="line"><span class="keyword">import</span> keras</div><div class="line"><span class="keyword">from</span> keras.layers <span class="keyword">import</span> Input,Dense,Activation,Reshape,Flatten,Dropout</div><div class="line"><span class="keyword">from</span> keras.models <span class="keyword">import</span> Sequential,Model</div><div class="line"><span class="keyword">from</span> keras.callbacks <span class="keyword">import</span> EarlyStopping</div><div class="line"><span class="keyword">import</span> numpy <span class="keyword">as</span> np</div><div class="line"><span class="keyword">import</span> matplotlib.pyplot <span class="keyword">as</span> plt</div><div class="line"></div><div class="line">X_in = np.linspace(<span class="number">-10.</span>,<span class="number">10.</span>,<span class="number">2000</span>)</div><div class="line">Y_inA = np.cos(np.tanh(X_in))</div><div class="line">Y_inB = np.sin(X_in)</div><div class="line"></div><div class="line">Y_verifyA = np.cos(np.tanh(X_in))</div><div class="line">Y_verifyB = np.sin(X_in)</div><div class="line"></div><div class="line">X_in = X_in.reshape(<span class="number">-1</span>,<span class="number">1</span>,<span class="number">1</span>)</div><div class="line">Y_inA = Y_inA.reshape(<span class="number">-1</span>,<span class="number">1</span>,<span class="number">1</span>)</div><div class="line">Y_inB = Y_inB.reshape(<span class="number">-1</span>,<span class="number">1</span>,<span class="number">1</span>)</div><div class="line"></div><div class="line">inputs = Input(shape=(<span class="number">1</span>,<span class="number">1</span>))</div><div class="line">m = Dense(<span class="number">64</span>,activation=<span class="string">'relu'</span>)(inputs)</div><div class="line">m = Dense(<span class="number">64</span>,activation=<span class="string">'relu'</span>)(m)</div><div class="line">outputA = Dense(<span class="number">1</span>,activation=<span class="string">'tanh'</span>)(m)</div><div class="line">outputB = Dense(<span class="number">1</span>,activation=<span class="string">'tanh'</span>)(m)</div><div class="line"></div><div class="line">m = Model(inputs=[inputs], outputs=[outputA, outputB])</div><div class="line">m.compile(optimizer=<span class="string">'adam'</span>,loss=<span class="string">'mean_absolute_error'</span>,metric=[<span class="string">'accuracy'</span>])</div><div class="line">m.summary()</div><div class="line">early_stopping = EarlyStopping(monitor=<span class="string">'loss'</span>, patience=<span class="number">15</span>)</div><div class="line">m.fit(X_in,[Y_inA,Y_inB],epochs=<span class="number">1000</span>,batch_size=<span class="number">128</span>,callbacks=[early_stopping])</div><div class="line"></div><div class="line">[y_verifyA,y_verifyB] = m.predict(X_in)</div><div class="line">y_verifyA = y_verifyA.reshape(<span class="number">-1</span>)</div><div class="line">y_verifyB = y_verifyB.reshape(<span class="number">-1</span>)</div><div class="line"></div><div class="line">x = np.linspace(<span class="number">1</span>,len(y_verifyA),len(y_verifyA))</div><div class="line">p1 = plt.subplot(<span class="number">211</span>)</div><div class="line">p2 = plt.subplot(<span class="number">212</span>)</div><div class="line">line_predictA, = p1.plot(x,y_verifyA,label=<span class="string">"predict value A"</span>)</div><div class="line">line_theoreticalA, = p1.plot(x,Y_verifyA,label=<span class="string">"theoretical value A"</span>)</div><div class="line">p1.legend(handles=[line_predictA,line_theoreticalA])</div><div class="line">line_predictB, = p2.plot(x,y_verifyB,label=<span class="string">"predict value B"</span>)</div><div class="line">line_theoreticalB, = p2.plot(x,Y_verifyB,label=<span class="string">"theoretical value B"</span>)</div><div class="line">p2.legend(handles=[line_predictB,line_theoreticalB])</div><div class="line">plt.show()</div></pre></td></tr></table></figure>
<p>这个说起来并没有什么难的，值的一提的是，在解决这个问题的时候顺便学会了<code>early_stopping</code>的使用。两个参数一个是<code>monitor</code>，监视<code>loss</code>就可以；另一个参数<code>patience</code>填入一个整数<code>x</code>，若训练过程中<code>monitor</code>监视的值不减小<code>x</code>次，那么就提前结束训练。这样<code>epoch</code>就可以往大了设置了。<br>
程序运行的结果如下图所示： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fe99tifihfj30sp0ngq51.jpg" alt="运行结果"></p>
<h2 id="小结">小结</h2>
<p>嗯，没什么总结的，就这样。愚人节快乐(/ω＼)</p>
]]></content>
<summary type="html">
<blockquote>
<p>Can machines think? –AM Turing</p>
</blockquote>
<ul>
<li>写在文前：最近在学机器学习的相关知识，由于相关文档和更新速度的关系选择了当前较为流行的<a href="https://keras.io/">Keras</a>，一个基于python的深度学习框架。学校正在进行一个基于深度学习/强化学习(reinforcement learning)控制水下滑翔机的项目，我学习了一下Keras，并做了一些练习。这篇文章将水下滑翔机作为问题背景介绍一下用Keras和Sequential模型处理回归问题。</li>
</ul>
</summary>
<category term="Computer Science" scheme="https://rm-rf.moe/categories/Computer-Science/"/>
<category term="Python" scheme="https://rm-rf.moe/tags/Python/"/>
<category term="Keras" scheme="https://rm-rf.moe/tags/Keras/"/>
</entry>
<entry>
<title>热电偶学习笔记--原理与使用</title>
<link href="https://rm-rf.moe/2017/03/21/2017-03-21-thermocouple/"/>
<id>https://rm-rf.moe/2017/03/21/2017-03-21-thermocouple/</id>
<published>2017-03-21T03:36:18.000Z</published>
<updated>2017-05-16T18:07:43.000Z</updated>
<content type="html"><![CDATA[<p>这学期<strong>过程控制</strong>课介绍了热电偶的相关知识，相对于<strong>检测技术</strong>课程更偏向于热电偶的实际应用。这篇文章将会介绍热电偶的基本原理和使用方法，介绍如何使用K型热电偶的正向/反向公式，并用MATLAB绘制SEEBACK曲线。</p>
<a id="more"></a>
<h2 id="热电偶原理简介">热电偶原理简介</h2>
<h3 id="基本原理">基本原理</h3>
<ul>
<li>以下内容参考 <em>过程控制系统/清华大学出版社</em></li>
</ul>
<blockquote>
<p>热电偶是一种感温元件，是一次仪表，它直接测量温度，并把温度信号转换成电动势信号。热电偶是由两根不同的导体组成的，当两端温度不同时就会有<strong>热电势</strong>产生。</p>
</blockquote>
<p><em>P.S：</em>热电势包括接触电势和温差电势两部分，由于温差电势比接触电势小得多，故主要考虑接触电势。<br>
<em>P.S：</em>接触电势是指两种不同材质的导体A，B接触时两边自由电子的密度不同，在交界面上产生电子相互扩散的电势。</p>
<h3 id="三个结论">三个结论</h3>
<ul>
<li>热电偶的两个热电极必须是两种不同材料的均质导体，否则热电偶回路的总电动势为零。</li>
<li>热电偶两接合点的温度必须不等，否则热电偶回路的总热电势为零。</li>
<li>热电偶A，B产生的热电势只与两个接点温度有关，而与中间温度无关，与热电偶的材料有关，和热电偶的尺寸形状无关。</li>
</ul>
<h2 id="热电偶的应用">热电偶的应用</h2>
<p>下面这张图是一个较为完整的热电偶测温系统，是我根据课堂笔记在<a href="https://www.draw.io" target="_blank" rel="external">Draw App</a>上面绘制的。这张图漏画了一个低通滤波器，主要是为了解决10KHz的射频干扰问题。 <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fe19lcaky2j30ho088jrm.jpg" alt="测温系统"> ### 补偿导线 因为热电偶一般都是贵金属，在远距离测量时为了节约成本所以需要补偿导线。补偿导线在一段温度范围内与所匹配的热电偶热电势标称值相同，而且价格比热电偶本身便宜很多，延长了热电偶的冷端。使用补偿导线的理论基础是中间温度定律。 ### 滤波及放大器 - 上图测温系统应该有两处滤波，第一处在运放之前，使用低通滤波器消除射频干扰，经由运放后需要滤波器滤去工频50Hz的杂波干扰。 - 使用的差分放大器需要满足<strong>高输入阻抗</strong>和<strong>低偏置电流</strong>两个特点。（大高输入阻抗可以有效减小流过热电偶的电流，这样就避免了热电偶自发热对测温准确度的影响）</p>
<h3 id="热电偶分度表和正反向公式使用">热电偶分度表和正/反向公式使用</h3>
<p>首先推荐一个很有用的网站<a href="https://srdata.nist.gov/its90/download/download.html" target="_blank" rel="external">NIST</a>在这里可以查看各种型号的热电偶温度与电压的对应表。表格的样子如下：<br>
<img src="https://ws1.sinaimg.cn/large/005WMcFzly1fe19m7d06jj30hz0fuafl.jpg" alt="表格"> 网站还给出了两个公式如下： <img src="https://ws1.sinaimg.cn/large/005WMcFzly1fe19mqb8rkj30ku0jumze.jpg" alt="公式"> 我们可以用MATLAB绘制“温度-输出热电势”，“输出热电势-温度”，“赛贝克(seeback)系数曲线”曲线。下面是MATLAB程序：</p>
<figure class="highlight python"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div><div class="line">9</div><div class="line">10</div><div class="line">11</div><div class="line">12</div><div class="line">13</div><div class="line">14</div><div class="line">15</div><div class="line">16</div><div class="line">17</div><div class="line">18</div><div class="line">19</div><div class="line">20</div><div class="line">21</div><div class="line">22</div><div class="line">23</div><div class="line">24</div><div class="line">25</div><div class="line">26</div><div class="line">27</div><div class="line">28</div><div class="line">29</div><div class="line">30</div><div class="line">31</div></pre></td><td class="code"><pre><div class="line">a0 = <span class="number">0.118597600000E+00</span></div><div class="line">a1 = <span class="number">-0.118343200000E-03</span></div><div class="line">a2 = <span class="number">0.126968600000E+03</span></div><div class="line">c_i_1 = [<span class="number">0.000000000000E+00</span> <span class="number">0.394501280250E-01</span> <span class="number">0.236223735980E-04</span> <span class="number">-0.328589067840E-06</span> <span class="number">-0.499048287770E-08</span> <span class="number">-0.675090591730E-10</span> <span class="number">-0.574103274280E-12</span> <span class="number">-0.310888728940E-14</span> <span class="number">-0.104516093650E-16</span> <span class="number">-0.198892668780E-19</span> <span class="number">-0.163226974860E-22</span>]</div><div class="line">c_i_2 = [<span class="number">-0.176004136860E-01</span> <span class="number">0.389212049750E-01</span> <span class="number">0.185587700320E-04</span> <span class="number">-0.994575928740E-07</span> <span class="number">0.318409457190E-09</span> <span class="number">-0.560728448890E-12</span> <span class="number">0.560750590590E-15</span> <span class="number">-0.320207200030E-18</span> <span class="number">0.971511471520E-22</span> <span class="number">-0.121047212750E-25</span>]</div><div class="line">t_1 = <span class="number">-270</span>:<span class="number">0</span></div><div class="line">t_2 = <span class="number">1</span>:<span class="number">1370</span></div><div class="line">E_1 = <span class="number">0</span></div><div class="line">E_2 = <span class="number">0</span></div><div class="line"><span class="keyword">for</span> i = <span class="number">1</span>:length(c_i_1)</div><div class="line"> E_1 = E_1 + c_i_1(i) * t_1.^(i<span class="number">-1</span>)</div><div class="line">end</div><div class="line"></div><div class="line"><span class="keyword">for</span> i = <span class="number">1</span>:length(c_i_2)</div><div class="line"> E_2 = E_2 + c_i_2(i) * t_2.^(i<span class="number">-1</span>)</div><div class="line">end</div><div class="line"></div><div class="line">E_2 = E_2 + a0*exp(a1 * (t_2 - a2).^<span class="number">2</span>)</div><div class="line">t = [t_1 t_2]</div><div class="line">E = [E_1 E_2]</div><div class="line"></div><div class="line"><span class="keyword">for</span> i = <span class="number">1</span>:length(E)<span class="number">-1</span></div><div class="line"> err(i) = E(i+<span class="number">1</span>)-E(i)</div><div class="line">end</div><div class="line"></div><div class="line">T = <span class="number">-270</span>:<span class="number">1369</span></div><div class="line">plot(T,err)</div><div class="line">plot(t,E)</div><div class="line">xlabel(<span class="string">'TEMPERATURE(?C)'</span>)</div><div class="line">ylabel(<span class="string">'SEEBECK COEFFICIENT -?V/?C'</span>)</div><div class="line">title(<span class="string">'THERMOCUPLE SEEBECK COEFFICIENT VERSUS TEMPERATURE'</span>)</div></pre></td></tr></table></figure>
<p>以上是正向公式的使用和绘制SEEBACK曲线，下面是反向公式的使用：</p>
<figure class="highlight plain"><table><tr><td class="gutter"><pre><div class="line">1</div><div class="line">2</div><div class="line">3</div><div class="line">4</div><div class="line">5</div><div class="line">6</div><div class="line">7</div><div class="line">8</div></pre></td><td class="code"><pre><div class="line">d_i = [-1.318058E+02 4.830222E+01 -1.646031E+00 5.464731E-02 -9.650715E-04 8.802193E-06 -3.110810E-08 0.000000E+00 0.000000E+00 0.000000E+00]</div><div class="line">t = 0</div><div class="line">E = 20.644:0.1:54.886</div><div class="line">E = 31.213</div><div class="line">for i = 1:10</div><div class="line"> t = t + d_i(i)*E.^(i-1) </div><div class="line">end</div><div class="line">plot(E,t)</div></pre></td></tr></table></figure>
<p>这里给出的电压区间是<code>20.644V~54.886V</code>，其他区间程序同理，相应地更改<code>d_i</code>就可以了。</p>
]]></content>
<summary type="html">
<p>这学期<strong>过程控制</strong>课介绍了热电偶的相关知识，相对于<strong>检测技术</strong>课程更偏向于热电偶的实际应用。这篇文章将会介绍热电偶的基本原理和使用方法，介绍如何使用K型热电偶的正向/反向公式，并用MATLAB绘制SEEBACK曲线。</p>
</summary>
<category term="Embedded System" scheme="https://rm-rf.moe/categories/Embedded-System/"/>
<category term="MATLAB" scheme="https://rm-rf.moe/tags/MATLAB/"/>
<category term="Sensor" scheme="https://rm-rf.moe/tags/Sensor/"/>
</entry>
<entry>
<title>加速度计和陀螺仪数据融合算法</title>
<link href="https://rm-rf.moe/2017/02/06/2017-02-06-pi/"/>
<id>https://rm-rf.moe/2017/02/06/2017-02-06-pi/</id>
<published>2017-02-06T03:16:18.000Z</published>
<updated>2017-05-16T18:07:30.000Z</updated>
<content type="html"><![CDATA[<p>在这篇文章里我用Python实现了一个简单的互补滤波器来做角度融合，实现姿态解算。用到的传感器是<a href="https://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/" target="_blank" rel="external">MPU-6050</a>，使用树莓派的I2C总线读取传感器的底层数据。</p>
<a id="more"></a>
<ul>
<li>这篇文章的大部分内容来源于我这学期的期末作业，题目是<em>平衡车角度融合算法探究</em>。当时的方案是：<strong>单片机+模拟加速度陀螺仪传感器，C语言单片机编程</strong>。考虑到当时没有上位机所以无法观察滤波的效果，所以我尝试用<code>smbus</code>使用I2C总线读取数据，用<code>matplotlib</code>绘图检验滤波器的性能。</li>
</ul>
<h3 id="系统概述">系统概述</h3>
<ul>
<li>Raspberry 3 Model B ／ Ubuntu 16.04（ARM）</li>
<li>MPU-6050</li>
</ul>
<div class="figure">