tsukuba exploration rover 制御周り仕様書
TRANSCRIPT
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