Si2024 simulator

[yasuohayashibara]

SI2024用のシミュレータ検証ソフトウェア

[yasuohayashibara]

p3dでgazeboシミュレータ上での正解の位置を出力するようにした．

[yasuohayashibara]

経路追従の挙動を計測するプログラムを作成した．
以下のスクリプトで軌道のデータを抽出する．

python3 line_follower.py
ros2 bag record /p3d/odom

ros2 bag play rosbag2_2024_09_18-13_11_16_0.db3
ros2 topic echo /p3d/odom > odom_data.txt
grep "position:" odom_data.txt -A 2 | awk '/x:/ {x=$2} /y:/ {y=$2; print x "," y}' > odom_data.csv

[yasuohayashibara]

１）運営が提供したシミュレータのモデル
ros2 launch simulator gazebo_simulator.launch.py

２）タイヤの摩擦係数を大きくしたモデル
摩擦係数が0.001→0.7

aiformula/simulator/simulator/models/ai_car1/model.sdf

Lines 212 to 213 in 1f47452

	<mu>0.001</mu>
	<mu2>0.001</mu2>

３）実車の挙動に近づけたモデル
ros2 launch simulator gazebo_simulator_caster_trail.launch.py
実車にはキャスタの向きを戻すバネがあるが，それに関しては省略した．
-1.0 0 -0.02 1.57 0 1.57 → -0.85 0 -0.02 1.57 0 1.57

aiformula/simulator/simulator/models/ai_car_caster_trail/model.sdf

Line 201 in 1f47452

<pose>-1.0 0 -0.02 1.57 0 1.57</pose>

aiformula/simulator/simulator/models/ai_car_caster_trail/model.sdf

Line 209 in 1f47452

<pose>-1.0 0 -0.02 1.57 0 1.57</pose>

aiformula/simulator/simulator/models/ai_car_caster_trail/model.sdf

Line 222 in 1f47452

<pose>-1.0 0 -0.02 1.57 0 1.57</pose>

aiformula/simulator/simulator/models/ai_car_caster_trail/model.sdf

Line 253 in 1f47452

<pose>-1.0 0 -0.02 1.57 0 1.57</pose>

４）トレイルを大きくしたモデル
ros2 launch simulator gazebo_simulator_caster_trail.launch.py

５）サーボモータによりヨー軸と駆動する機構
ros2 launch simulator gazebo_simulator_active_caster.launch.py

[yasuohayashibara]

rviz2を使用して経路に対するロボットの挙動を観察した．
/gnss_pathと/tfのframe_idが異なるため，以下を実行してからrviz2で観察した．

ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map odom

[yasuohayashibara]

ros2 bag record /gnss_path /tf
ros2 bag play rosbag2_2024_09_29-23_47_53_0.db3
ros2 topic echo /gnss_path > gnss_path.txt
grep "position:" gnss_path.txt -A 2 | awk '/x:/ {x=$2} /y:/ {y=$2; print x "," y}' > gnss_path.csv
ros2 topic echo /tf > tf.txt
grep "child_frame_id: base_footprint" tf.txt -A 5 | grep translation -A 2 | awk '/x:/ {x=$2} /y:/ {y=$2; print x "," y}' > tf.csv

目標軌道とロボットの軌道を描いたらずれていた．

ただし，x方向に4m, 回転方向に-3degロボットの軌道を移動させたらほぼ一致する．
おそらく計算のどこかでこのずれを生じる計算がされていると考える．

生データ
gnss_path.xlsx

[yasuohayashibara]

トレイルを0.05mずつ変更した場合の挙動

モビリティの動力学パラメータなどを正確に計測することが難しいため定性的にはなるが，トレイルを短くすることで動きに遅れが生じて軌道を速やかに追従できていない様子が見られる．

図　トレイルと経路追従の軌道の様子（バネを含まないモデル）

経路追従のソフトウェアはこちら
https://github.com/open-rdc/aiformula/blob/si2024_simulator/simulator/simulator/simulator/line_follower.py

生データ
integration.xlsx

[yasuohayashibara]

キャスタのヨー軸の目標角度を求め，計測した角度との違いが速度を0.2rad以下だと速度を0.1m/s(通常は0.5m/s)としたときの挙動

結果
目標角度からの誤差が大きい場合に低速となるが，行き過ぎを抑えながら経路に追従する軌道となった．
できるだけ速度を低下させずに経路追従するためには動力学モデルを考慮した制御則が必要

aiformula/simulator/simulator/simulator/line_follower_caster.py

Lines 85 to 93 in 83b6497

	# caster steering の角度による補正
	wheelbase = 0.65
	target_caster_steering_angle = -asin(max(min(wheelbase * angular_velocity / self.linear_velocity, 1.0), -1.0))
	diff_steering_angle = target_caster_steering_angle - self.caster_steering_angle
	print("target: "+str(target_caster_steering_angle)+", current: "+str(self.caster_steering_angle)+", diff_steering_angle: "+str(diff_steering_angle))
	if abs(diff_steering_angle) > 0.2:
	self.linear_velocity = 0.1
	else:
	self.linear_velocity = 0.5

トレイル　0.05m

トレイル　0.10m

[yasuohayashibara]

実行の方法は「３）実車の挙動に近づけたモデル」で変更した部分をすべて変更
-1.0 0 -0.02 1.57 0 1.57　の-1.0を　トレイル　0.05 -> -0.85, 0.10 -> -0.90, 0.15 -> -0.95, 0.2 -> -1.0 とする．

実行コマンド

ros2 launch simulator gazebo_simulator_caster_trail.launch.py
python3 line_follower_caster.py & ros2 bag record /p3d/odom
ros2 bag play rosbag2_2024_10_20-23_25_22_0.db3 & ros2 topic echo /p3d/odom > odom_data.txt
grep "position:" odom_data.txt -A 2 | awk '/x:/ {x=$2} /y:/ {y=$2; print x "," y}' > odom_data.csv

なお，上記を実行するとプロセスが残ることがあるので，`kill -9‘でプロセスを終了することが必要になる．