1 高機能機械制御研究室事例研究リアルタイム制御プログラムと

高機能機械制御研究室事例研究
リアルタイム制御プログラムと RoboCar の制御
2015 年 10 月 20 日
野中謙一郎
1. リアルタイム制御プログラム
リアルタイム制御とは，指定された時間までに指定された制御処理を完了することです．リアルタイム制御プ
ログラムとは，このリアルタイム制御を行うプログラムです．例として，車輪にモータと速度センサを装備した
移動ロボットを考えましょう．コンピュータを用いて，移動ロボットを目標の速度に制御するためには下記の処
理を行います．
1.
速度センサで走行速度を計測
2.
目標速度との誤差を計算し，その誤差に応じて，モータに出力させる速度を計算
3.
モータに計算した速度の出力を指示
この 3 段階の処理 1 サイクルとして繰り返し行うことによって，目標の速度で走行できます． 1s でこのサイク
ルを何回実行できるかを表す制御周波数(Hz)を高めれば即応性が良くなり，目標位置が時々刻々と変化する場合
にも素早く応答できます．機械系なら 1kHz 以上で実行することが多いでしょう．上記の処理を while などの繰
り返し文によって実行すれば，素早い動きを実現できます．
しかし，このような単なる繰り返し処理では以下の項目で問題が生じます．
(i)
コンピュータの性能によって，1 サイクルに要する時間が異なる．
別のコンピュータで実行すると，制御周波数が変化するために制御性能が異なります．
(ii) 時々刻々と変化する位置の画面表示など，高負荷の処理を並列実行する．
画面表示は時間のかかる処理です．繰り返しループに画面表示命令を入れると，制御周波数が低下します．
(iii) 一定間隔毎にデータを保存したい．
性能を検証するためには，厳密に一定間隔毎のデータを保存する必要があります．
リアルタイム制御プログラムではこれらの問題を解決することができます．
2. リアルタイム制御プログラムの長所
リアルタイム制御プログラムでは，指定された時間に直ちに処理を実行します．前述の移動ロボットも 1～3
の処理をなるべく高い制御周波数で繰り返し実行します．この際に制御周期を指定して，一定時間毎に実行する
ことをタイマー割り込みと呼びます．すると前節の問題点(i)では，スペックの異なるコンピュータを用いても同
じ制御性能を実現できます．さらに，この処理の際に余った時間を用いて，(ii)で挙げた現在の角度やトルクを表
示することが可能になります．画面の表示周波数は数 Hz 程度で十分です．また，一定時間毎に発生する割り込
みを利用してデータを保存すれば，(iii)は実現できます．
優先度
優先度
プリエンプション
制御スレッド
制御スレッド
復帰
高
画面表示などのスレッド
低
画面表示などのスレッド
時間
10ms
10ms
10ms
10ms
10ms
10ms
10ms
10ms
10ms
10ms
時間
10ms
10ms
10ms
10ms
10ms
10ms
10ms
10ms
10ms
10ms
10ms
10ms
図 1 左は非リアルタイム処理，右はリアルタイム・マルチスレッド処理：左では，制御が長時間実行されない期間が
生じる．右では，優先度の高い制御スレッドはサンプリング周期(10ms)毎に必ず実行され，その隙間の時間で，優先
度の低い画面表示などの他の処理が行われる．
1
プログラム作成手順（2 名一組）
1.
非リアルタイムのタイマー：usleep()
※サンプルプログラム：Core_0_nonRT.cpp
100s 間を 1s 毎にカウントアップし，キーボードが押されたら停止するプログラムを理解する．
usleep() 指定時間[us]だけ実行停止．指定時間の範囲は 0～1000000us(=1s)
kbhit()
キーが押された場合は 0 以外を返し，押されない場合は 0 を返す．
次の処理で 1s 毎に時間を表示できる．
double rt_time = 0.0;
while(kbhit()==0){
printf(“現在%f 秒”,rt_time);
rt_time += 1.0;
usleep(1000000);//1000000us=1s 停止
}
演習
キーボードを入力してから停止までの時間が遅いことを確認せよ．さらに，時間表示間隔を 0.1s に
短くすると時間がずれることを確認せよ．
2.
リアルタイムのタイマー：clock_nanosleep()
※サンプルプログラム：Core_1_RT.cpp
10s 間をカウント，制御周期は 100ms のプログラム．
clock_gettime(CLOCK_MONOTONIC,&t);
struct timespec t に現在の時刻（コンピュータ起動からの経過時間）を取得
clock_nanosleep(CLOCK_MONOTONIC,TIMER_ABSTIME,&t,NULL);
struct timespec t で指定した時間まで実行停止．ここで timespec 構造体の定義は下記の通り．
struct timespec {
time_t tv_sec;
/* seconds */
long
/* nanoseconds [0 .. 999999999] */
tv_nsec;
};
次の処理で 100ms 毎に進めることができる．
（while で tv_nsec のオーバーフローを回避している）
double start, now;
clock_gettime(CLOCK_MONOTONIC,&t);
t.tv_sec++;//1s 後にループを開始
start = t.tv_sec + t.tv_nsec * 1.0E-9;// ループ開始時刻[s]（CPU が起動してからの経過時間）
while(kbhit()==0){
clock_nanosleep(CLOCK_MONOTONIC,TIMER_ABSTIME,&t,NULL);
clock_gettime(CLOCK_MONOTONIC,&t);
t.tv_nsec += 100000000;//100ms 進める
while(t.tv_nsec >= NSEC_PER_SEC){//ns が 1s を超えた場合は繰り上げ
t.tv_nsec -= NSEC_PER_SEC;
t.tv_sec++;
}
now = t.tv_sec + t.tv_nsec * 1.0E-9;// 現在時刻[s]
rt_time = now – start;
}
2
演習
10s で終了するようにプログラムを変更せよ．さらに，10ms 刻みで while を繰返すようにして，停
止までの反応が良くなるように変更せよ．
3.
マルチスレッドの利用：pthread_create()
※サンプルプログラム：Core_2_Multi.cpp
次のプログラムでは，画面表示の処理 Display()を別のスレッドとして処理して，1s 毎に時間を表示する．
pthread_t ThreadRtDisplay;
int flag=1;
double rt_time = 0.0;
void *Display(void *)
{
while(flag){
printf("rt_time=%f[s]\n",rt_time);//ループの経過時間
rt_time += 1.0;
sleep(1);
}
flag = 2;
pthread_exit((void *)0);
}
int main(){
pthread_create(&ThreadRtDisplay,NULL,Display,(void *)NULL);//Display()をスレッドとして実行
sleep(10);//10s 経過を待つ
flag=0;
while(flag==0) sleep(1);//Display()のスレッド終了の合図を待つ
sleep(1);//少し待つ
return 0;
}
Core_2_Multi.cpp を参考に Core_1_RT.cpp をマルチスレッドにして現在の正確な時間を約 1s 毎に表示せよ．
プログラムの転送と起動方法
1.
Core_?_*.cpp のファイル名を Core.cpp に変更して，適宜編集
2.
send－???.bat でアップロード
3.
TeraTerm でコマンドを入力してビルド（コンパイル）と実行
cd current
←フォルダを移動
※TeraTerm でログインした最初の 1 回だけ
make
←コンパイル：バグがあるとエラーメッセージが出る
./my_app
←実行
データファイル encoder.mat のダウンロード方法：
receive－???.bat でダウンロード
3
4.
車速の制御
※サンプルプログラム：Core_3_Vehicle.cpp
停止状態から 10s 毎に速度を 100mm/s 増加し，100s で再び停止するプログラムを作成する．
次の関数を用いて，車輪を浮かせた状態で確認しなさい．
SetDriveSpeed( int speed) 駆動速度の設定．Speed は mm/s 単位で，－2800～+2800
例）
RcControl rrc;
// RoboCar ハード制御用変数（グローバル定義済）
int speed = 0;
// 指示速度格納用変数（グローバル定義済）
…
speed = 200;
rrc.SetDriveSpeed(speed); // 200mm/s に駆動速度を設定する．
5.
操舵の制御
10s 周期で±20 度で操舵を変化させる．
車輪を浮かせた状態で確認しなさい．
SetSteerAngle(float angle) 操舵角の設定．angle は 0.1 度単位で，－30～+30 度
例）
6.
rrc.SetSteerAngle(23.4); // 23.4 度に操舵角度を設定する．
一定操舵の旋回走行実験
操舵角度を 15 度に固定して，停止状態から 10s 毎に速度を 200mm/s 増加し，60s で再び停止．
暴走して事故を起こす可能性があるので，車両を浮かせた状態で先輩の確認を受けてから実験する．
7.
データの保存
前両輪速度の平均値を 10ms 毎に配列に保存．保存間隔をサンプリングタイムとよぶ．
ファイルに保存するデータ：時間[s]，速度[mm/s]，指示速度[mm/s]
void GetSensorInfoReq(SENSOR_VALUE *)
例）
SENSOR_VALUE
センサ情報の取得
// センサーの値を格納する構造体変数（定義済）
sensor;
rrc.GetSensorInfoReq(&sensor);
// ジャイロ/加速度/エンコーダ情報取得
printf(“速度=%f\n”,sensor.enc_1);//左前輪の速度[mm/s]：右前は enc_2，
左後 enc_3，
右後 enc_4
8.
速度の同定実験と速度応答モデルの同定
操舵角度を適切な角度に固定して，停止状態から 10s 毎に速度を 200mm/s 増加し，60s で再び停止させる．
走行データ encoder.mat は receive-22?.bat でダウンロードできる．保存したデータは MaTX のプログラム
CompPlot.mm で確認できる．さらに，2 次遅れ系
2
のモデルのパラメータ , ,
比較しながら，最も適合する
を設定すると，実験結果とモデルの応答を重ね合わせて表示できる．グラフを
, ,
を求めなさい．このように入力(指示速度)と出力(走行速度)のデータ
からシステムのモデルを求めることをシステム同定とよぶ．
0.98,0.2,0.65
課題：速度の同定実験の結果を A4 で 2 頁（両面）のレポートに纏めよ．実験結果のグラフ（指令速度，計測速
度，モデルの速度応答）を示し，グラフを比較して考察せよ．
以上
4

Download Report