Tsukuba Exploration Rover の制御周りの仕様書

1. 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