Tsukuba exploration rover 制御周り仕様書
- 2. Ubuntu PC 制御周りのシステムオーバービュー Arduino Motor Controleller Steering Controller Arduino IMU Controller move_base TerController Odometry cmd_vel : 500ms Odometry : 10ms 速度?ヨーレートは過去100msの平均
- 3. ステアリング制御フロー ~TerController~ フィードバック制御 (P制御) 1. move_base から ωtarget を取得． 2. PID制御式を通して，入力を計算 ? ωoutput = ωtarget + Kp * (ωtarget - ωcurrent) + Ki * ∫(ωtarget - ωcurrent) 3. ステアリング角に変換 ? ヨーレート?車速／舵角の関係から， θtmp = sin-1(Wωoutput / (2 * max(vcurrent, 0.05))) θoutput =min(max(θtmp, -18), 18) 4. Arduino に送信 制御フロー ROS TerController Arduino Steering Controller Odometry Odometry : 10ms ωInput VRx : 100ms ステアリング角入力 ROS move_base cmd_vel : 500ms ωtarget ヨーレート フィードバック W R θ θ θ モデル化 車両制御モデル 1. ヨーレートと旋回半径の関係 - v = Rω 2. ステアリング角と旋回半径の関係 - Rsinθ = W / 2 3. ヨーレート?車速／舵角の関係 - θ = sin-1(Wω / 2v) Odometryの速度?ヨーレート情報 に関しては，過去100msの平均
- 4. 速度制御フロー ~TerController~ フィードバック制御 (PI制御) 1. move_base から vtarget を取得． 2. PID制御式を通して，入力を計算 ? voutput = vtarget + Kp * (vtarget - vcurrent) + Ki * ∫(vtarget - vcurrent) 3. 1s間のターゲットパルスに変換 ? Pulseoutput = voutput / Pulse Distance 4. Arduino に送信 制御フロー ROS TerController Arduino Motor Controller Odometry Odometry : 10ms vCurrent VRx : 100ms 速度入力 ROS move_base cmd_vel : 500ms vtarget 速度 フィードバック Odometryの速度?ヨーレート情報 に関しては，過去100msの平均
- 5. オドメトリ計算 ~Odometry~ フィードバック制御 (PI制御) 位置積分計算 1. Arduino Motor Controller からPulseを，Imuからヨーレート を取得する． 2. 左右車輪に関して，dt間のパルス差分をそれぞれ求め，平均する． 3. z軸周りのヨーレートとパルス差分平均値を使って(x, y)座標値を計算． 速度計算（速度） 1. Arduino Motor Controller からパルスを取得． 2. 過去100ms のパルス増加を保持しておき，これを100msで割る ことによって時速を求める． 速度計算（ヨーレート） 1. Arduino Motor Controller からヨーレートを取得． 2. 過去100ms のヨーレートを保持しておき，これを10で割ることによって平均ヨーレートを求める． 制御フロー Odometry Arduino Imu Imu : 10ms aInput Arduino Motor Controller Whl : 10ms Pulse Count 速度計算 速度 ヨーレート 位置積分計算
- 6. 制御マイコンタスク ~ArduinoController~ 10msの周期でArduinoのタスクが実行 1. 1msタイマ割り込み 1. 車輪速パルスが動いたかどうかのチェックを実施． 2. 10msタスク 1. 車輪速カウンタメッセージ（Whl）をパブリッシュ． 3. 100msタスク 1. ROSメッセージ受信（VRx） 2. ステアリングPWM制御 3. モータPWM制御 4. ROSメッセージ送信（VTx） 5. LED更新
- 7. 各メッセージ詳細 Whl.msg (10ms) Header header uint16 wheel_pulse_rl uint16 wheel_pulse_rr uint8 cycleTime Imu.msg (10ms) Header header int16 accX int16 accY int16 accZ int16 omegaX int16 omegaY int16 omegaZ uint8 cycleTime VRx.msg (100ms) Header header bool actuation_request bool backward_request uint8 tgt_pulse_1s float32 stg_angle VTx.msg (100ms) Header header bool emergency_sw_staus uint8 motor_pwm uint8 steering_pwm uint8 cycle_time uint8 time_out_cnt float32 steering_angle