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

Ubuntu PC

制御周りのシステムオーバービュー

VRx : 100ms VTx

: 100

ms

Whl : 10ms

Imu : 10ms

Arduino

Motor ControlellerSteering Controller

Arduino

IMU Controller

move_base

TerController Odometry

Odometry : 10ms

速度

・ヨ

ーレ

ート

は過

去100ms の

平均cmd_vel : 500ms

Odometry : 10ms速度・ヨーレートは過去 100ms の平均

ステアリング制御フロー ~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ステアリング角入力

ROSmove_base

cmd_vel : 500msωtarget

ヨーレートフィードバッ

ク

W

R

θ

θθ

モデル化

車両制御モデル

1. ヨーレートと旋回半径の関係- v = Rω

2. ステアリング角と旋回半径の関係- Rsinθ = W / 2

3. ヨーレート・車速／舵角の関係- θ = sin-1(Wω / 2v)

Odometry の速度・ヨーレート情報

に関しては，過去 100ms の平均

速度制御フロー ~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 : 10msvCurrent

VRx : 100ms速度入力

ROSmove_base

cmd_vel : 500msvtarget

速度フィードバッ

クOdometry の速度・ヨーレート情報

に関しては，過去 100ms の平均

オドメトリ計算 ~Odometry~フィードバック制御 (PI 制御 )

位置積分計算1. Arduino Motor Controller から Pulse を， Imu からヨーレート

を取得する．

2. 左右車輪に関して， dt 間のパルス差分をそれぞれ求め，平均する．

3. z 軸周りのヨーレートとパルス差分平均値を使って (x, y) 座標値を計算．

速度計算（速度）4. Arduino Motor Controller からパルスを取得．

5. 過去 100ms のパルス増加を保持しておき，これを 100ms で割ることによって時速を求める．

速度計算（ヨーレート）6. Arduino Motor Controller からヨーレートを取得．

7. 過去 100ms のヨーレートを保持しておき，これを 10 で割ることによって平均ヨーレートを求める．

制御フロー

Odometry

Arduino Imu

Imu : 10msaInput

ArduinoMotor

Controller

Whl : 10msPulse Count

速度計算速度

ヨーレート位置積分計算

制御マイコンタスク　 ~ArduinoController~10ms の周期で Arduino のタスクが実行

1. 1ms タイマ割り込み1. 車輪速パルスが動いたかどうかのチェックを実施．

2. 10ms タスク1. 車輪速カウンタメッセージ（ Whl ）をパブリッシュ．

3. 100ms タスク1. ROS メッセージ受信（ VRx ）2. ステアリング PWM 制御3. モータ PWM 制御4. ROS メッセージ送信（ VTx ）5. LED 更新

各メッセージ詳細

Whl.msg (10ms)Header headeruint16 wheel_pulse_rluint16 wheel_pulse_rruint8 cycleTime

Imu.msg (10ms)Header headerint16 accXint16 accYint16 accZint16 omegaXint16 omegaYint16 omegaZuint8 cycleTime

VRx.msg (100ms)Header headerbool actuation_requestbool backward_requestuint8 tgt_pulse_1sfloat32 stg_angle

VTx.msg (100ms)Header headerbool emergency_sw_stausuint8 motor_pwmuint8 steering_pwmuint8 cycle_timeuint8 time_out_cntfloat32 steering_angle