[Pegasus] Matlab-Ros2-Gazebo 통합 아키텍처
Gazebo / Ros2 시뮬레이터
이제 명령을 내리기위한 컨트롤 용도로 Ros2를 사용, Gazebo 시뮬레이터를 사용해 시뮬레이션 환경에서 제어를 적용해 볼 것이다.
Ros2는 사용해본 경험이 있지만, Gazebo는 경험이 없어서 설치부터 환경세팅까지 정리 한 후 로봇을 모델링했다.
로봇 모델링은 아래와 같은 모델 코드로 구현했고, 외장 디자인과 바퀴등은 인터넷 자료를 활용했다.
활용한 인터넷 자료 디자인
모델 제작 과정
모델 코드 model.sdf
<?xml version='1.0'?>
<sdf version='1.7'>
<model name='mobilebot'>
<link name="base_link">
<inertial>
<mass>102</mass>
<inertia>
<ixx>3.3668</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>5.0881</iyy>
<iyz>0</iyz>
<izz>7.8412</izz>
</inertia>
</inertial>
<collision name="collision_base">
<geometry>
<box>
<size>0.6 0.4 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual_base">
<geometry>
<box>
<size>0.6 0.4 0.05</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.3 0.3 0.3 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0.0 0.0 0.0 1</emissive>
</material>
</visual>
</link>
<link name="front_right_hinge">
<pose>0.0 -0.215 0.025 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<visual name="visual_front_right_hinge">
<geometry>
<box>
<size>0.03 0.03 0.1</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0.0 0.0 0.0 1</emissive>
</material>
</visual>
</link>
<joint name="front_right_axis_joint" type="fixed">
<child>front_right_hinge</child>
<parent>base_link</parent>
</joint>
<link name="front_left_hinge">
<pose>0.0 0.215 0.025 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.01</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.01</iyy>
<iyz>0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<visual name="visual_front_left_hinge">
<geometry>
<box>
<size>0.03 0.03 0.1</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.6 0.6 0.6 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0.0 0.0 0.0 1</emissive>
</material>
</visual>
</link>
<joint name="front_left_axis_joint" type="fixed">
<child>front_left_hinge</child>
<parent>base_link</parent>
</joint>
<link name="front_left_wheel">
<pose>0.0 0.25 0.03 1.57079 0 0</pose>
<inertial>
<mass>4.0</mass>
<inertia>
<ixx>0.0064</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0064</iyy>
<iyz>0</iyz>
<izz>0.0128</izz>
</inertia>
</inertial>
<collision name="collision_front_left_wheel">
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<length>0.05</length>
<radius>0.08</radius>
</cylinder>
</geometry>
<!--max_contacts>1</max_contacts-->
<surface>
<friction>
<ode>
<mu>20.0</mu>
<mu2>20.0</mu2>
<fdir1>1 0 0</fdir1>
<slip1>0</slip1>
<slip2>0.001</slip2>
</ode>
</friction>
<contact>
<ode>
<kp>1000000000000000000000000000.0</kp>
<kd>1000000000000000000000000000.0</kd>
</ode>
</contact>
</surface>
</collision>
<visual name="visual_front_left_wheel">
<pose>0.0 0.0 0.0 1.57079 0 0</pose>
<geometry>
<mesh>
<uri>model://mobilebot/meshes/wheel.dae</uri>
<scale>0.45 0.45 0.45</scale>
</mesh>
</geometry>
</visual>
</link>
<joint name="front_left_wheel_joint" type="revolute">
<child>front_left_wheel</child>
<parent>front_left_hinge</parent>
<axis>
<xyz>0 0 1</xyz>
</axis>
<limit>
<effort>2.0</effort>
<velocity>3.0</velocity>
</limit>
<joint_properties>
<damping>0.0</damping>
<friction>0.001</friction>
</joint_properties>
</joint>
<link name="front_right_wheel">
<pose>0.0 -0.25 0.03 1.57079 0 0</pose>
<inertial>
<mass>4.0</mass>
<inertia>
<ixx>0.0064</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0064</iyy>
<iyz>0</iyz>
<izz>0.0128</izz>
</inertia>
</inertial>
<collision name="collision_front_right_wheel">
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<length>0.05</length>
<radius>0.08</radius>
</cylinder>
</geometry>
<!--max_contacts>1</max_contacts-->
<surface>
<friction>
<ode>
<mu>20.0</mu>
<mu2>20.0</mu2>
<fdir1>1 0 0</fdir1>
<slip1>0</slip1>
<slip2>0.001</slip2>
</ode>
</friction>
<contact>
<ode>
<!--min_depth>0.001</min_depth-->
<kp>1000000000000000000000000000.0</kp>
<kd>1000000000000000000000000000.0</kd>
</ode>
</contact>
</surface>
</collision>
<visual name="visual_front_right_wheel">
<pose>0.0 0.0 0.0 1.57079 0 0</pose>
<geometry>
<mesh>
<uri>model://mobilebot/meshes/wheel.dae</uri>
<scale>0.45 0.45 0.45</scale>
</mesh>
</geometry>
</visual>
</link>
<joint name="front_right_wheel_joint" type="revolute">
<child>front_right_wheel</child>
<parent>front_right_hinge</parent>
<axis>
<xyz>0 0 1</xyz>
</axis>
<limit>
<effort>2.0</effort>
<velocity>3.0</velocity>
</limit>
<joint_properties>
<damping>0.0</damping>
<friction>0.001</friction>
</joint_properties>
</joint>
<link name="front_yaw_link">
<pose>0.25 0 -0.04 0 1.5707 1.5707</pose>
<visual name="visual_front_yaw_link">
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.01</radius>
</cylinder>
</geometry>
</visual>
<collision name="collision_front_yaw_link">
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.01</radius>
</cylinder>
</geometry>
</collision>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>5.145833333333334e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>5.145833333333334e-06</iyy>
<iyz>0</iyz>
<izz>1.0125000000000003e-04</izz>
</inertia>
</inertial>
</link>
<joint name="front_yaw_joint" type="revolute">
<child>front_yaw_link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
</axis>
<limit>
<effort>1000.0</effort>
<velocity>100.0</velocity>
</limit>
<dynamics>
<damping>0.0</damping>
<friction>0.001</friction>
</dynamics>
</joint>
<link name="front_roll_link">
<pose>0.25 0 -0.04 0 1.5707 1.5707</pose>
<visual name="visual_front_roll_link">
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.01</radius>
</cylinder>
</geometry>
</visual>
<collision name="collision_front_roll_link">
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.01</radius>
</cylinder>
</geometry>
</collision>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>5.145833333333334e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>5.145833333333334e-06</iyy>
<iyz>0</iyz>
<izz>1.0125000000000003e-04</izz>
</inertia>
</inertial>
</link>
<joint name="front_roll_joint" type="revolute">
<child>front_roll_link</child>
<parent>front_yaw_link</parent>
<axis>
<xyz>1 0 0</xyz>
</axis>
<limit>
<effort>1000.0</effort>
<velocity>100.0</velocity>
</limit>
<dynamics>
<damping>0.0</damping>
<friction>0.001</friction>
</dynamics>
</joint>
<link name="front_pitch_link">
<pose>0.25 0 -0.04 0 1.5707 1.5707</pose>
<visual name="visual_front_pitch_link">
<geometry>
<sphere>
<length>0.005</length>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<collision name="collision_front_pitch_link">
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</collision>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>5.145833333333334e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>5.145833333333334e-06</iyy>
<iyz>0</iyz>
<izz>1.0125000000000003e-04</izz>
</inertia>
</inertial>
</link>
<joint name="front_pitch_joint" type="revolute">
<child>front_pitch_link</child>
<parent>front_roll_link</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
<limit>
<effort>1000.0</effort>
<velocity>100.0</velocity>
</limit>
<dynamics>
<damping>0.0</damping>
<friction>0.001</friction>
</dynamics>
</joint>
<link name="back_yaw_link">
<pose>-0.25 0 -0.04 0 1.5707 1.5707</pose>
<visual name="visual_back_yaw_link">
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.01</radius>
</cylinder>
</geometry>
</visual>
<collision name="collision_back_yaw_link">
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.01</radius>
</cylinder>
</geometry>
</collision>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>5.145833333333334e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>5.145833333333334e-06</iyy>
<iyz>0</iyz>
<izz>1.0125000000000003e-04</izz>
</inertia>
</inertial>
</link>
<joint name="back_yaw_joint" type="revolute">
<child>back_yaw_link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
</axis>
<limit>
<effort>1000.0</effort>
<velocity>100.0</velocity>
</limit>
<dynamics>
<damping>0.0</damping>
<friction>0.001</friction>
</dynamics>
</joint>
<link name="back_roll_link">
<pose>-0.25 0 -0.04 0 1.5707 1.5707</pose>
<visual name="visual_back_roll_link">
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.01</radius>
</cylinder>
</geometry>
</visual>
<collision name="collision_back_roll_link">
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.01</radius>
</cylinder>
</geometry>
</collision>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>5.145833333333334e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>5.145833333333334e-06</iyy>
<iyz>0</iyz>
<izz>1.0125000000000003e-04</izz>
</inertia>
</inertial>
</link>
<joint name="back_roll_joint" type="revolute">
<child>back_roll_link</child>
<parent>back_yaw_link</parent>
<axis>
<xyz>1 0 0</xyz>
</axis>
<limit>
<effort>1000.0</effort>
<velocity>100.0</velocity>
</limit>
<dynamics>
<damping>0.0</damping>
<friction>0.001</friction>
</dynamics>
</joint>
<link name="back_pitch_link">
<pose>-0.25 0 -0.04 0 1.5707 1.5707</pose>
<visual name="visual_back_pitch_link">
<geometry>
<sphere>
<length>0.005</length>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<collision name="collision_back_pitch_link">
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</collision>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>5.145833333333334e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>5.145833333333334e-06</iyy>
<iyz>0</iyz>
<izz>1.0125000000000003e-04</izz>
</inertia>
</inertial>
</link>
<joint name="back_pitch_joint" type="revolute">
<child>back_pitch_link</child>
<parent>back_roll_link</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
<limit>
<effort>1000.0</effort>
<velocity>100.0</velocity>
</limit>
<dynamics>
<damping>0.0</damping>
<friction>0.001</friction>
</dynamics>
</joint>
<link name="cover">
<pose>0.0 -0.0 -0.02 0 0 1.57079</pose>
<inertial>
<mass>0.0001</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00001</iyy>
<iyz>0</iyz>
<izz>0.00001</izz>
</inertia>
</inertial>
<visual name="visual_cover">
<geometry>
<mesh>
<uri>model://mobilebot/meshes/robot.dae</uri>
<scale>0.26 0.25 0.32</scale>
</mesh>
</geometry>
</visual>
</link>
<joint name="cover_joint" type="fixed">
<child>cover</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<plugin name="libgazebo_ros_p3d" filename="libgazebo_ros_p3d.so">
<ros>
<namespace>robot</namespace>
<remapping>odom:=odom</remapping>
</ros>
<frame_name>map</frame_name>
<body_name>base_link</body_name>
<update_rate>30.0</update_rate>
<gaussian_noise>10</gaussian_noise>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
</plugin>
<plugin name="left_motor" filename="libgazebo_ros_dc_motor.so">
<motor_shaft_joint>front_left_wheel_joint</motor_shaft_joint>
<motor_wrench_frame>front_left_wheel</motor_wrench_frame>
<parameter type="bool" name="publish_encoder">true</parameter>
<parameter type="bool" name="publish_velocity">true</parameter>
<parameter type="bool" name="publish_current">false</parameter>
<parameter type="bool" name="publish_motor_joint_state">true</parameter>
<parameter type="bool" name="publish_load">false</parameter>
<parameter type="double" name="update_rate">1000</parameter>
<parameter type="double" name="motor_nominal_voltage">48</parameter>
<parameter type="double" name="moment_of_inertia">0.00001</parameter>
<parameter type="double" name="armature_damping_ratio">0.0001</parameter>
<parameter type="double" name="electromotive_force_constant">0.0322</parameter>
<parameter type="double" name="electric_resistance">4.18</parameter>
<parameter type="double" name="electric_inductance">0.012</parameter>
<parameter type="double" name="gear_ratio">25.0</parameter>
<parameter type="double" name="encoder_ppr">100</parameter>
<parameter type="double" name="velocity_noise">0.0</parameter>
<parameter type="double" name="kp_v">2.0</parameter>
<parameter type="double" name="ki_v">0.0</parameter>
</plugin>
<plugin name="right_motor" filename="libgazebo_ros_dc_motor.so">
<motor_shaft_joint>front_right_wheel_joint</motor_shaft_joint>
<motor_wrench_frame>front_right_wheel</motor_wrench_frame>
<parameter type="bool" name="publish_encoder">true</parameter>
<parameter type="bool" name="publish_velocity">true</parameter>
<parameter type="bool" name="publish_current">false</parameter>
<parameter type="bool" name="publish_motor_joint_state">true</parameter>
<parameter type="bool" name="publish_load">false</parameter>
<parameter type="double" name="update_rate">1000</parameter>
<parameter type="double" name="motor_nominal_voltage">48</parameter>
<parameter type="double" name="moment_of_inertia">0.00001</parameter>
<parameter type="double" name="armature_damping_ratio">0.0001</parameter>
<parameter type="double" name="electromotive_force_constant">0.0322</parameter>
<parameter type="double" name="electric_resistance">4.18</parameter>
<parameter type="double" name="electric_inductance">0.012</parameter>
<parameter type="double" name="gear_ratio">25.0</parameter>
<parameter type="double" name="encoder_ppr">100</parameter>
<parameter type="double" name="velocity_noise">0.0</parameter>
<parameter type="double" name="kp_v">2.0</parameter>
<parameter type="double" name="ki_v">0.0</parameter>
</plugin>
<static>0</static>
<allow_auto_disable>1</allow_auto_disable>
</model>
</sdf>
- 위 모델 코드를 사용해서 각 link별로 크기와 위치를 선정
- 각 link와 몸체 base_link를 joint를 사용해서 연결
Ros2 통신 과정
- 플러그인은 Gazebo 물리 엔진과 ROS2 노드를 이어주는 역할을 한다.
- 하드웨어 대신 Gazebo에서 동일한 인터페이스를 위해 Gazebo 플러그인을 사용했다.
- 구체적으로, 이동 로봇의 구동부는 플러그인이
/cmd_vel토픽으로 들어오는geometry_msgs/Twist메시지를 속도로 변환해주고, 센서 플러그인은 Gazeb0에서 계산된 상태를 ROS2 토픽으로 퍼블리시한다.
- 구체적으로, 이동 로봇의 구동부는 플러그인이
-
로봇의 현재 상태 정보를 얻기 위한 P3D 플러그인을 적용
<plugin name="libgazebo_ros_p3d" filename="libgazebo_ros_p3d.so"> <ros> <namespace>robot</namespace> <remapping>odom:=odom</remapping> </ros> <frame_name>map</frame_name> <body_name>base_link</body_name> <update_rate>30.0</update_rate> <gaussian_noise>0.01</gaussian_noise> <xyzOffsets>0 0 0</xyzOffsets> <rpyOffsets>0 0 0</rpyOffsets> </plugin>- 몸체 link의 위치와 자세를 읽어서 ROS2로 Pose/Odometry를 퍼블리시 해주는 플러그인이다.
<frame_name>map</frame_name>를 통해 map 기준으로 몸체 link의 위치를 퍼블리시 했다.- Gazebo는 기본적으로 world 프레임이 있는데, 그걸 ROS2 기준의 프레임으로 대응시켰다.
<update_rate>30.0</update_rate>현실적인 센서 업데이트 주기 중 일반적인 수준으로 맞췄다.<gaussian_noise>0.01</gaussian_noise>퍼블리시 되는 위치나 자세 값에 랜덤오차를 0.01 정도 표준편차를 주었다.- 0은 너무 이상적이라고 생각했다. 실세 노이즈를 어느 정도 반영한 상태에서 제어기를 설계하고 싶었다.
- 정지 상태에서는 실제 위치 x는 같다. 측정된 위치는 실제 위치에 임의로 노이즈를 더하기 때문에 값이 위아래로 튄다.
- $v = \frac{x_k - x_{k-1}}{\Delta t}$ 에서 $x_k = x + n_k$ 이 k 시점의 실제 위치에 노이즈를 더한 것이고, $v = \frac{n_k - n_{k-1}}{\Delta t}$ 가 되기 때문에 속도는 노이즈의 변화율이 된다.
- 분모의 $\Delta t$ 가 매우 작기 때문에 노이즈의 변화량이 증폭되어 정지상태여도 빠른 주행중인 것처럼 보인다.
- 따라서 위치 기반 속도 추정은 노이즈에 매우 민감하고 우리는 휠 오도메트리를 사용해서 속도를 직접 측정하면 이 증폭을 피할 수 있다. 속도를 직접 측정하니까 미분 과정이 없어서 노이즈가 순수 노이즈 크기만큼만 반영이 된다.
- 소프트웨어적으로는 위치 센서를 위치 미분해서 얻지 않고 엔코터나 IMU 쪽에서만 얻게하거나, 필터링이나 스무딩을 한다.
-
DC 모터에 파라미터를 할당하고 제어를 위한 오픈소스 플러그인을 적용
<plugin name="left_motor" filename="libgazebo_ros_dc_motor.so"> <motor_shaft_joint>front_left_wheel_joint</motor_shaft_joint> <motor_wrench_frame>front_left_wheel</motor_wrench_frame> <parameter type="bool" name="publish_encoder">true</parameter> <parameter type="bool" name="publish_velocity">true</parameter> <parameter type="bool" name="publish_current">false</parameter> <parameter type="bool" name="publish_motor_joint_state">true</parameter> <parameter type="bool" name="publish_load">false</parameter> <parameter type="double" name="update_rate">1000</parameter> <parameter type="double" name="motor_nominal_voltage">48</parameter> <parameter type="double" name="moment_of_inertia">0.00001</parameter> <parameter type="double" name="armature_damping_ratio">0.0001</parameter> <parameter type="double" name="electromotive_force_constant">0.0322</parameter> <parameter type="double" name="electric_resistance">4.18</parameter> <parameter type="double" name="electric_inductance">0.012</parameter> <parameter type="double" name="gear_ratio">25.0</parameter> <parameter type="double" name="encoder_ppr">100</parameter> <parameter type="double" name="velocity_noise">0.0</parameter> <parameter type="double" name="kp_v">2.0</parameter> <parameter type="double" name="ki_v">0.0</parameter> </plugin> <plugin name="right_motor" filename="libgazebo_ros_dc_motor.so"> <motor_shaft_joint>front_right_wheel_joint</motor_shaft_joint> <motor_wrench_frame>front_right_wheel</motor_wrench_frame> <parameter type="bool" name="publish_encoder">true</parameter> <parameter type="bool" name="publish_velocity">true</parameter> <parameter type="bool" name="publish_current">false</parameter> <parameter type="bool" name="publish_motor_joint_state">true</parameter> <parameter type="bool" name="publish_load">false</parameter> <parameter type="double" name="update_rate">1000</parameter> <parameter type="double" name="motor_nominal_voltage">48</parameter> <parameter type="double" name="moment_of_inertia">0.00001</parameter> <parameter type="double" name="armature_damping_ratio">0.0001</parameter> <parameter type="double" name="electromotive_force_constant">0.0322</parameter> <parameter type="double" name="electric_resistance">4.18</parameter> <parameter type="double" name="electric_inductance">0.012</parameter> <parameter type="double" name="gear_ratio">25.0</parameter> <parameter type="double" name="encoder_ppr">100</parameter> <parameter type="double" name="velocity_noise">0.0</parameter> <parameter type="double" name="kp_v">2.0</parameter> <parameter type="double" name="ki_v">0.0</parameter> </plugin>- 모터 스펙은 앞서 정의한 값을 넣어줬다.
update_rate는 1000HZ로 설정했다.- DC 모터는 동특성이 로봇 움직임보다 훨씬 빠르게 변하는 시스템이므로 외부 제어 주기보다 훨씬 높은 주파수로 작동하도록 했다.
- 미세하게 힘을 조절해서 정밀한 제어가 가능한 장점도 있다.
moment_of_inertia = 0.00001, armature_damping_ratio = 0.0001모터가 관성 모멘트가 없어서 무한대 가속도가 나오는 것을 방지했다.- 입력과 동시에 바로 최고 속도가 나오지 않아야 실제 응답과 비슷하다고 생각했다.
플러그인 적용 후
실제 로봇이 없어도 Gazebo 환경에서 로봇이 구동되며 실시간으로 값을 받아볼 수 있게 되었다.
- 모터에 대한 정보도 좌/우 따로 토픽으로 받을 수 있게 되었다.
- 한 쪽 바퀴에만 $1rad/s$ 에 해당하는 각속도를 넣어서 테스트 했다.
Turtlebot3 패키지 연동
터틀봇3 패키지 내에 들어있는 teleop node를 활용해서 키보드로 로봇을 제어해봤다.
- 키보드 값을 입력 받아서 topic으로
/cmd_vel로 쏘면, Control node가 구독해서 모바일봇으로 다시 퍼블리시 해주는 구조이다.
[cmd] linear : 3.0 angular : 0.0
[cur] linear : 6.957660564507382 angular : -4.336003365866704
[cmd] linear : 3.0 angular : 0.0
[cur] linear : -11.48909999033403 angular : -2.4905442053146474
[cmd] linear : 3.0 angular : 0.0
[cur] linear : 15.354914321095018 angular : 3.460247668695481
[cmd] linear : 3.0 angular : 0.0
[cur] linear : -2.017019213756047 angular : 4.682560264096943
[cmd] linear : 3.0 angular : 0.0
[cur] linear : 17.38343493116724 angular : 3.7183501743370213
[cmd] linear : 3.0 angular : 0.0
[cur] linear : -1.796576020986112 angular : 9.611881336718003
[cmd] linear : 3.0 angular : 0.0
[cur] linear : -3.5824107926800606 angular : -13.074454957071762
[cmd] linear : 3.0 angular : 0.0
[cur] linear : -13.872253149371469 angular : -0.642756694489865
[cmd] linear : 3.0 angular : 0.0
[cur] linear : 2.166972600360915 angular : 7.851183712573045
[cmd] linear : 3.0 angular : 0.0
[cur] linear : -19.907223824591874 angular : 20.055677520612377
[cmd] linear : 3.0 angular : 0.0
[cur] linear : 3.1564870765572213 angular : -9.889990927134317
- 정상적인 주행 상황에서 가끔씩 값이 크게 튀는 로그가 발견되었다. 실제 로봇은 부드럽게 움직였지만 로그의 속도와 각속도는 그렇지 않았다.
튀는 로그 분석
cmd 는 정상적으로 3과 0으로 유지되는데 cur 값은 한 번씩 값이 크게 튀는 현상이 있었다. 노이즈 값이 크게 반영되었거나 속도이기 때문에 위치를 시간으로 나누는 과정에서 값이 튀었을 것으로 예상하고 원인을 찾아보았다.
- 후보
- 차분의 분모 dt가 작아져서 값이 순간적으로 크게 보이는 경우
- now() 로 dt를 잡고, 데이터는 stamp 기반일때 시간축이 섞여서
- 측정이 찍힌 시간은 stamp로, 콜백이 실행된 시간을 now()로 얻게 되면 위치변화는 stamp 흐름을 따라가는데 dt는 현재 시간으로 재면, 두 시간축이 같은 리듬이 아니기 때문에 dt 범위가 달라져서 변동이 심할 수 있다. 이럴 경우 stamp 기준으로 계산하는 것이 안정적이다.
- 메세지가 네트워크/큐 때문에 실제 간격과 다르게 한꺼번에 연속으로 콜백되어서 어느 순간에 거의 2개가 동시 처리되면 dt 감소
- dt를 콜백 호출 간격 기반으로 계산하면 어느 순간 한 번에 빠르게 실행되는 경우 dt가 1회성으로 매우 커질 수 있다.
- now() 로 dt를 잡고, 데이터는 stamp 기반일때 시간축이 섞여서
- 플러그인의 실험을 위해 가우시안 노이즈를
<gaussian_noise>10</gaussian_noise>10으로 설정해두어서
- 차분의 분모 dt가 작아져서 값이 순간적으로 크게 보이는 경우
- 결론
- 코드를 직접 확인한 결과,
- cur 값은 control_robot.py 에서 출력하는데, 실제로는 control_robot.py 로 구독한
/robot/odom의 Odometry.twist 를 그대로 읽어온 값이다. 이 노드 안에서는 dt로 나눠서 속도를 계산하는 코드가 없다. 따라서 dt 관련 후보는 문제가 아니다. /robot/odm은 모델 SDF에서 P3D 플러그인이 퍼블리시 하는데,- 플러그인 설정의 가우시간 노이즈가 테스트를 위해 10으로 높여 놓았는데, 되돌리지 않아서 표준 편차가 10으로 한 번씩 크게 튀는 값이 나오게 되었다.
- 테스트
-
가우시안 노이즈 값을
0.01 로 두고 다시 제어를 시작해봤다.[cmd] linear : -1.5 angular : 0.0 [cur] linear : 0.07788550724647324 angular : -0.01895389213057958 [cmd] linear : -2.0 angular : 0.0 [cur] linear : 0.1195186484726249 angular : -0.0062722198927030355 [cmd] linear : -2.0 angular : 0.0 [cur] linear : 0.1530365989391994 angular : -0.00861613546351606 [cmd] linear : -2.5 angular : 0.0 [cur] linear : 0.1709270954028069 angular : -0.003100883572690438 [cmd] linear : -3.0 angular : 0.0 [cur] linear : 0.20240741083392372 angular : -0.009781222284346372 [cmd] linear : -3.0 angular : 0.0 [cur] linear : 0.23607689382020272 angular : -0.003795920321737263 [cmd] linear : -3.0 angular : 0.0 [cur] linear : 0.23694622727228343 angular : -0.014323942045737757 [cmd] linear : -3.4999999999999996 angular : 0.0 [cur] linear : 0.2519330011736734 angular : 0.014088135430624676 [cmd] linear : -3.9999999999999996 angular : 0.0 [cur] linear : 0.2424767872855163 angular : -0.014139599319290787 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.31398173326248 angular : 0.010898859279758845 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.3217806859406334 angular : 0.0033362062504576884 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.3175554819382802 angular : -0.005200905808602651 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.3348905921220147 angular : 0.010606165652519546 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.3207792354489992 angular : -0.00294466189502181 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.31710844723395365 angular : -6.023304307044801e-05 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.33231914549753977 angular : -0.019434537821900286 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.3261831338532742 angular : 0.004577647788463024 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.31274331797214155 angular : -0.00504699603976933 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.3017519289845812 angular : 0.011928417773003512 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.31792136174318825 angular : -0.00200434917624646 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.3153826457173677 angular : -0.0027232532408877717 [cmd] linear : -4.0 angular : 0.0 [cur] linear : 0.3140751681233642 angular : -0.0161248175875797 angular값이 0.01~0.02 사이에서 움직이는 정상적인 범위가 되었다.cmd는/cmd_vel을 휠 속도 명령으로 바꾸기 위해 임의 게인을 곱한 ‘제어입력’이고,cur은 Gazebo가 계산한 ‘실제 차체 속도’ 라서 단위나 의미 자체가 다르기 때문에 값이 같게 보일 필요가 없다.- Gazebo에서 차량을 제어할 때 휠 반지름/차폭으로 변환식을 써야하지만, 차량 속도가 느려 체감 속도를 맞추려고 임의의 게인을 곱해
cmd를 계산했다.
- Gazebo에서 차량을 제어할 때 휠 반지름/차폭으로 변환식을 써야하지만, 차량 속도가 느려 체감 속도를 맞추려고 임의의 게인을 곱해
- 현실에서는 관성이나 마찰 때문에 명령을 즉시 그대로 못 따라가기 때문에 매 순간
cur = cmd가 성립하지는 않는다.
-
Simulink 통신 연결
매트랩과 simulink에서 만든 제어기를 Gazebo에 적용하기 위해서 UDP 통신 환경을 구축했다.
Instrument Control Toolbox 애드온을 적용하여 UDP recieve/send 블록을 사용한다.
- UDP(User Datagram Protocol) 통신이란
- 인터넷에서 데이터를 주고받는 전송 계층 프로토콜로 TCP와 다르게 비연결형 방식으로 상대방이 수신중이라는 응답을 하지 않아도 ros2의 토픽처럼 패킷을 그냥 던지는 방식이다.
- 중간에 패킷이 손실되거나 순서가 바뀌거나 중복이 될 수도 있지만 보내는 사람은 알 수 없다.
- 하지만 전송 속도가 매우 빠르기 때문에 조금 유실 되어도 크게 문제가 없다.
- 하나의 IP 안에서도 여러 프로그램이 동시에 UDP를 쓸 수 있도록 포트 번호로 구분한다.
Simulink 통신 테스트
- Solver가 Auto 방식이면
- 보통 가변 스텝 솔버를 선택하기 때문에 신호의 변화가 적을 때는 step을 크게 건너뛰고, 변화가 급격할 때만 잘게 쪼갠다.
- 계산이 단순할 때, 10초 분량의 데이터를 몇 번의 계산으로 끝내기 때문에 결과를 한 번에 뿌려준다.
- Solver가 Fixed-step 방식이면
- 정해진 간격마다 무조건 멈추기 때문에 10초 시뮬레이션이라면 step이 0.01 일때 총 1000번의 계산을 강제로 수행한다.
- 매 step마다 데이터를 주고받는 과정에서 대기 시간이 발생해서 시뮬레이션 속도가 물리적인 실제 시간과 비슷하게 느려지게 된다.
- 완전한 실시간을 사용하고 싶으면 simulate - run 아래 화살표 - simulation pacing 기능을 켜면 실제 시간과 동기화 된다.
- IP 주소 설정은
- 외부 pc가 아닌 내 컴퓨터로 데이터를 보내기 때문에 로컬을 의미하는 127.0.0.1을 사용했다.
- 내 pc에 어떤 통로로 들어오든 포트 7777로 오는 것은 다 받게 하기 위해 로컬 주소를 0.0.0.0을 사용했다.
- Byte-order는
- 숫자를 저장하거나 보낼 때 큰 단위부터 차례대로 보내는 방식이다.
- 123을 1-2-3 순서로 보냈으면 받을 때도 1-2-3 순서로 받기 위해서 big-endian으로 맞춰주었다.
- 이 설정이 서로 다르다면 데이터가 깨지거나 말도 안 되는 다른 값이 나오게 된다.
Ros2 통신 연결
이제 Ros2에서도 UDP 통신을 사용하여 Server와 Client 노드를 만들어서 서로 데이터를 잘 주고 받는지 환경 테스트를 해야한다.
- 간단하게 udp_server 노드와 udp_client 노드를 만들어서 둘 사이에 통신이 잘 되는지 확인했다.
- 테스트로 0.11과 0.22를 송신하고, 서버로부터 그 절반인 0.06과 0.11이 계산되어 나오는 것을 볼 수 있다.
- 클라이언트와 서버 모두 송수신이 정상적으로 작동한다.
Ros2 상호간에 통신이 잘 되는 것을 확인하였고, 우리의 현재 흐름은 아래와 같다.
명령(입력) 경로
- MATLAB 제어 알고리즘 → UDP로 (linear, angular) 송신 → ROS2 udp_server가 수신 → ROS2 /cmd_vel(Twist)로 publish → control_robot가
/cmd_velsubscribe → 휠 명령 publish → Gazebo 모터 플러그인 → 로봇이 움직임
상태(출력) 경로
- Gazebo → gazebo_ros_p3d가
/robot/odom publish→ ROS2에서/robot/odom subscribe→ UDP로 상태($v$, $w$, pose)를 MATLAB으로 송신 → MATLAB이 UDP receive