Jetson Nano의 ROS 시작하기 - - SLAM의 Gmapping 매핑 및 경로 계획

---

머리말

SLAM(Simultaneous Localization And Mapping)은 로봇이나 모바일 장치와 같은 자율 이동 시스템의 이동 중에 환경 지도를 동시에 구축하고 실시간으로 자신의 위치를 ​​파악하는 기술을 말합니다. SLAM 기술은 무인 운전, 로봇 내비게이션, 가상 현실 및 기타 분야에서 널리 사용되었습니다.

SLAM 기술에서 로봇은 라이더, 카메라, 관성 측정 장치 등과 같은 자체 센서를 통해 환경 정보를 얻은 다음 알고리즘을 통해 자율 이동 및 위치 추정을 구현하고 동시에 환경 정보를 지도. SLAM 기술은 프런트엔드와 백엔드의 두 가지 주요 부분으로 나눌 수 있습니다.

프런트 엔드는 센서 데이터에서 특징점 또는 랜드마크를 추출하고 다양한 알고리즘을 통해 이러한 정보를 추적 및 일치시켜 로봇의 실시간 위치 지정 및 매핑을 실현합니다. 일반적으로 사용되는 프런트엔드 알고리즘에는 라이더 기반 알고리즘, 비전 기반 알고리즘 및 관성 측정 단위 기반 알고리즘이 있습니다.

백엔드는 프런트엔드에서 얻은 데이터를 처리 및 최적화하고, 여러 측정에서 얻은 위치 정보를 병합하고, 보다 정확한 지도를 생성하고, 로봇의 궤적을 수정하고 최적화하는 역할을 합니다. 일반적으로 사용되는 백엔드 알고리즘에는 그래프 최적화 기반 알고리즘과 필터링 기반 알고리즘이 있습니다.

일반적으로 사용되는 라이다 기반 SLAM 알고리즘으로는 고전적인 Gmapping 알고리즘과 Hector SLAM 알고리즘이 있으며, 일반적으로 사용되는 비전 기반 SLAM 알고리즘으로는 ORB-SLAM, LSD-SLAM, DSO 등이 있다.

---

1. 지매핑 알고리즘

GMapping 알고리즘은 모바일 로봇 제작에 널리 사용되는 고전적인 레이저 SLAM 알고리즘으로 입자 필터를 사용하여 환경에서 모바일 로봇의 위치를 ​​추정하고 동시에 환경 맵을 작성합니다.

1. Gmapping 알고리즘 처리 원리

SLAM 문제는 모바일 로봇의 전원이 켜질 때부터 시간 t까지의 일련의 센서 측정 데이터 $1\_:t$ (물론 /scan here) 및 일련의 제어 데이터$1시에:t$ 조건 (여기서는 /odom으로 간주)에서 동시에 맵$m$ , 로봇 포즈$\times 1:t$ 에 의해 만들어진 추정은

입자 필터는 시간에 따라 변하는 상태 수량을 추정하기 위한 Monte Carlo 방법을 기반으로 하는 확률 필터입니다. 상태 공간에서 임의의 입자 집합을 샘플링하여 현재 상태의 불확실성을 나타내고 이러한 입자에 가중치를 부여하여 상태의 사후 확률 분포를 계산합니다. 로봇 위치 파악 및 매핑 문제에서 파티클 필터는 환경에서 로봇의 자세와 맵의 특성을 추정하는 데 자주 사용됩니다.

너 ${유}_{}$시간 tt 에서 로봇을 나타냅니다.$t$ ~$티+1$ , zt z_t사이의 모션 모델${지}_{}$시간 tt 에서 로봇을 나타냅니다.$t$ 의 센서 측정 결과 . 입자 필터는 일련의 입자${에스}_{티}^{[1:남}$시간 tt 에서 로봇을 나타내기 위해$t$ 의 위치 와 지도 기능의 불확실성, 여기서$M은$ 입자의 수를 나타내며, 각 입자는 포즈 벡터를 포함합니다.${엑스}_{티}^{[m}$및 지도 특징 벡터 ${중}_{티}^{[m}$。

예측 단계에서 각 입자에 대해 ${엑스}_{티}^{[m}$, 로봇의 동작 모델을 기반으로 예측을 수행하려면:

$${\overline{엑스}}_{티+1}^{[m}={유}_{}\left({엑스}_{티}^{[m}\right)+{ϵ}_{티}^{[m}$$

여기서 ${\overline{엑스}}_{티+1}^{[m}$다음 순간의 예측 포즈 상태를 나타냅니다. ${유}_{}({엑스}_{티}^{[m})는$ 모션 모델에 따른 로봇의 예측된 상태를 나타내며,${ϵ}_{티}^{[m}$예측 오차를 나타냅니다. 여기서 예측 오차는 제로 평균 가우스 분포, 즉 ${ϵ}_{티}^{[m}~엔(0,{에스}_{})$ , 여기서${에스}_{}$예측 오차의 공분산 행렬입니다. 측정 업데이트 단계에서 각 입자의 포즈에 대해 ${엑스}_{티+1}^{[m}$, 무게 계산 ${승}_{티+1}^{[m}$, 현재 입자의 사후 확률 분포를 나타냅니다.

$${승}_{티+1}^{[m}=P(z_{티+}|{엑스}_{티+1}^{[m},{중}_{티}^{[m})\cdot 피({엑스}_{티+1}^{[m}|{엑스}_{티}^{[m},{유}_{})$$

其中$P(z_{티+}|{엑스}_{티+1}^{[m},{중}_{티}^{[m})$ 는 센서 측정 결과와 맵 피쳐의 일치 정도를 나타내며,${중}_{티}^{[m}$현재 맵의 특성을 나타냅니다. $피({엑스}_{티+1}^{[m}|{엑스}_{티}^{[m},{유}_{})는$ 시간 tt에서 로봇을 의미합니다.$t$ ~$티+1$ 사이의 모션 모델의 확률 밀도 함수

리샘플링 단계에서 입자는 가중치에 따라 리샘플링됩니다. 구체적으로, 각 입자에 대해 ${에스}_{티}^{[m}$, 무게에 따라 ${승}_{티}^{[m}$그것을 샘플링하여 새로운 입자 집합을 얻습니다. ${에스}_{티+1}^{[1:남}$, 더 큰 가중치를 가진 입자가 샘플링될 확률이 더 높아집니다.

마지막으로 모든 입자의 평균을 가중하여 시간 t + 1 t+1 에서 로봇을 얻을 수 있습니다.$티+1$ 에 대한 위치 및 지도 기능의 추정치

$${\stackrel{^^}{에스}}_{티+}=\sum _{m=1}{승}_{티+1}^{[m}{에스}_{티+1}^{[m}$$

위의 단계를 지속적으로 반복하면 입자 필터가 점진적으로 올바른 로봇 포즈 및 맵 기능으로 수렴할 수 있습니다.

2. 지맵핑 지도 구축 실습

먼저 터미널을 열고 ROS 버전에 해당하는 Gmapping 기능 패키지, 키보드 컨트롤 패키지, 지도 서비스 패키지를 설치합니다.

sudo apt-get install ros-melodic-gmapping
sudo apt-get install ros-melodic-teleop_twist_keyboard
sudo apt install ros-melodic-map-server

gmapping 기능 패키지의 핵심 노드는 slam_gmapping이며 이 노드의 구독 주제, 게시 주제, 서비스 및 관련 매개변수의 세부 정보는 다음과 같습니다.

2.1 Subscribed Topic
tf(tf/tfMessage): 레이더, 섀시 및 주행 거리계 간의 좌표 변환 메시지에 사용됩니다.
scan(sensor_msgs/LaserScan): SLAM에 필요한 레이더 정보입니다.

2.2에서 발표된 주제 map_metadata(nav_msgs/MapMetaData)
: 지도 너비, 높이, 해상도 등을 포함한 지도 메타데이터, 이 메시지는 정기적으로 업데이트됩니다.
map(nav_msgs/OccupancyGrid): 맵 래스터 데이터, 일반적으로 rviz에서 그래픽으로 표시됩니다.
~entropy(std_msgs/Float64): 로봇 포즈 분포 엔트로피 추정(값이 클수록 불확실성이 커짐).

2.3 서비스
dynamic_map(nav_msgs/GetMap): 지도 데이터를 얻기 위해 사용됩니다.

2.4 파라미터
~base_frame(string, default: "base_link"): 로봇의 베이스 좌표계.
~map_frame(문자열, 기본값: "map"): 지도 좌표계.
~odom_frame(문자열, 기본값: "odom"): odometry 프레임.
~map_update_interval(float, default: 5.0): 지도 업데이트 빈도, 업데이트 간격은 지정된 값에 따라 설계됩니다.
~maxUrange(float, 기본값: 80.0): 레이저 감지에 사용할 수 있는 최대 범위(이 임계값 초과, 잘림).
~maxRange(float): 레이저 감지의 최대 범위.

작업 공간에 새 실행 폴더 및 실행 파일을 생성하고 다음 실행 파일 매개변수 구성을 복사합니다.

<launch>
<param name="use_sim_time" value="true"/> <!--使用仿真时间-->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <remap from="scan" to="scan"/>
      <param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
      <param name="odom_frame" value="odom"/> <!--里程计坐标系-->
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz" />
</launch>

그런 다음 Gazebo 시뮬레이션 환경을 시작하고 로봇 모델을 로드한 다음 roslaunch 명령을 사용하여 시작 파일을 실행합니다.

roslaunch 包名 launch文件名

rviz 구성 표시줄에서 RobotModel, Map, LaserScan 및 기타 플러그인을 로드하고 터미널에서 키보드 제어 노드를 시작합니다.

rosrun teleop_twist_keyboard teleop_twist_keyboard

그런 다음 키보드를 이용하여 로봇의 움직임을 제어하고 실시간으로 지도를 작성하며, 획득한 그리드 지도는 map_server 노드를 통해 저장할 수 있습니다.

터미널에서 rosrun 명령을 실행하여 map_server 노드를 실행하여 그리드 맵을 지정된 경로에 저장합니다.

rosrun map_server map_saver map:=/<Map Topic> -f PATH_TO_YOUR_FILE/mymap

2. AMCL 몬테카를로 포지셔닝

1. 적응형 몬테카를로 포지셔닝 알고리즘의 원리

AMCL(Adaptive Monte Carlo Localization)은 센서 데이터를 기반으로 환경에서 로봇의 포즈를 추정하는 것이 주요 목표인 입자 필터 기반 로봇 위치 파악 방법입니다.

우리의 목표는 xt x_t를 사용하여 시간 t에서 로봇의 포즈를 추정하는 것입니다.${엑스}_{}$익스프레스, ${유}_{}$시간 t-1에서 t, zt z_t 까지의 모션 모델을 나타냅니다.${지}_{}$시간 t의 관측 데이터를 나타냅니다. 베이지안 정리(Bayesian theorem)에 따르면 모션 모델과 관찰 데이터의 조건 하에서 로봇 포즈가 다음과 같이 추정된다고 표현할 수 있습니다.

$$피({엑스}_{}|{지}_{1:},{유}_{1:})=\frac{피({지}_{}|{엑스}_{},{지}_{1:티-},{유}_{1:})\cdot 피({엑스}_{}|{지}_{1:티-},{유}_{1:}}{피({지}_{}|{지}_{1:티-},{유}_{1:})}$$

p( xt ∣ z 1 : t − 1 , u 1 : t ) p(x_t | z_{1:t-1}, u_{1:t})를 계산 하려면$피({엑스}_{}|{지}_{1:티-},{유}_{1:})$ , 조건부 확률$피({엑스}_{}|{엑스}_{t-},{유}_{})$ 말했다. 주어진 마지막 순간에${엑스}_{t-}$그리고 현재 모션 모델 ${유}_{}$다음으로 로봇의 현재 포즈 ${엑스}_{}$확률 분포. 총확률공식을 적용하면 로봇의 고유한 동작특성에 따른 동작모델을 표현할 수 있다.

$$피({엑스}_{}|{지}_{1:티-},{유}_{1:})=\int 피({엑스}_{}|{엑스}_{t-},{유}_{})\cdot 피({엑스}_{t-}|{지}_{1:티-},{유}_{1:티-})디{엑스}_{t-}$$

为了计算$피({지}_{}|{엑스}_{},{지}_{1:티-},{유}_{1:})$ , 관찰 모델을 도입해야 합니다.$피({지}_{}|{엑스}_{})$ 말했다. 이 모델은 주어진 현재 위치${엑스}_{}$다음으로 관측 데이터 ${지}_{}$확률 분포.

몬테카를로 포지셔닝 방법은 파티클 필터(Particle Filter)를 이용하여 위의 확률분포를 근사화하고 계산한다. 입자 집합 ${엑스}_{}={엑스}_{티}^{(1},{엑스}_{티}^{(2},\cdots ,{엑스}_{티}^{(남}$시간 t, MM 에서 포즈 분포를 나타내기 위해$M$ 은 입자의 수입니다.

모션 모델에 따르면 입자 세트 ${엑스}_{t-}$모션 모델 ${유}_{}$시간 t X_t에서 입자 집합 X t를 예측합니다.${엑스}_{}$, 각 입자에 대해 ${엑스}_{t-1}^{(나는}$, 모션 모델 $피({엑스}_{}|{엑스}_{t-1}^{(나는},{유}_{})$ 새로운 입자를 얻기 위한 샘플링${엑스}_{티}^{(나는}$.
관찰 모델에 따라 각각의 새로운 입자 ${엑스}_{티}^{(나는}$무게 ${승}_{티}^{(나는}$, 각 입자에 대해 ${엑스}_{티}^{(나는}$, 관측 데이터 ${지}_{}$우도 $피({지}_{}|{엑스}_{티}^{(나는})$ , 그리고 그것을 입자의 무게로 사용하십시오. 새로운 입자의 무게에 따라${승}_{티}^{(나는}$리샘플링을 수행하여 새 입자 세트 X t X_t 생성${엑스}_{}$. 이 과정에서 가중치가 큰 입자는 여러 번 샘플링될 확률이 높고 가중치가 작은 입자는 폐기될 수 있습니다. 이 프로세스는 확률이 높은 영역에 입자 수집을 집중시켜 위치 파악 정확도를 향상시키는 데 도움이 됩니다.

AMCL의 주요 기능은 입자 중량의 유효 표본 크기(ESS)에 따라 적응적으로 조정되는 리샘플링 빈도를 적응적으로 조정하는 것입니다.ESS는 다음과 같이 계산됩니다.

$$N_{에프}=\frac{}{{\sum }_{나는=1}^{}{{승}_{티}^{(나는}}^2}$$

当$N_{에프}$임계값(예: 총 입자 수의 절반) 미만에서는 리샘플링 단계가 수행됩니다. 이는 조기 리샘플링으로 인한 입자 세트 저하를 방지하고 계산 오버헤드를 줄입니다. AMCL Monte Carlo 지역화 방법은 입자 필터와 적응형 리샘플링 전략을 통해 로봇 위치를 추정하며, 로봇 지역화 문제에서 우수한 견고성과 실시간 성능을 보여줍니다.

2. AMCL 포지셔닝 연습

먼저 터미널을 열고 amcl 포지셔닝 기능이 포함된 ROS 버전에 해당하는 내비게이션 기능 패키지를 설치합니다.

sudo apt-get install ros-melodic-navigation

amcl 기능 패키지의 핵심 노드는 amcl입니다. 이 노드의 구독 항목, 게시 항목, 서비스 및 관련 매개 변수는 다음과 같습니다.

3.1 구독 주제
스캔(sensor_msgs/LaserScan): 라이다 데이터.
tf(tf/tfMessage): 좌표 변환 메시지.
initialpose(geometry_msgs/PoseWithCovarianceStamped): 입자 필터의 평균 및 공분산을 초기화하는 데 사용됩니다.
map(nav_msgs/OccupancyGrid): 지도 데이터를 가져옵니다.

3.2에서 릴리스된 항목 amcl_pose(geometry_msgs/PoseWithCovarianceStamped)
: 맵에서 로봇의 포즈 추정입니다.
particlecloud(geometry_msgs/PoseArray): rviz의 PoseArray에서 구독한 다음 로봇의 포즈 추정 컬렉션을 그래픽으로 표시할 수 있는 포즈 추정 컬렉션입니다.
tf(tf/tfMessage): odom에서 지도로의 변환을 게시합니다.

3.3 서비스
global_localization(std_srvs/Empty): 글로벌 포지셔닝 서비스를 초기화합니다.
request_nomotion_update(std_srvs/Empty): 업데이트를 수동으로 수행하고 업데이트된 파티클을 게시하는 서비스입니다.
set_map(nav_msgs/SetMap): 새로운 맵과 포즈를 수동으로 설정하는 서비스.
static_map(nav_msgs/GetMap): 이 서비스를 호출하여 지도 데이터를 가져옵니다.

3.5 매개변수
~odom_model_type(문자열, 기본값: "diff"): 주행 모델 선택: "diff", "omni", "diff-corrected", "omni-corrected"(diff 차동, 옴니 전방향 휠) ~ odom_frame_id(문자열
, 기본값: "odom"): 주행 거리 측정 좌표계입니다.
~base_frame_id(문자열, 기본값: "base_link"): 로봇 극좌표계.
~global_frame_id(문자열, 기본값: "map"): 지도 좌표계.

작업 공간의 실행 폴더 아래에 새 실행 파일을 생성하고 다음 실행 파일 매개변수 구성을 복사합니다.

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 -->
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="30"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.05"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.2"/>
  <param name="odom_alpha2" value="0.2"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.8"/>
  <param name="odom_alpha4" value="0.2"/>
  <param name="laser_z_hit" value="0.5"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_model_type" value="likelihood_field"/>
  <!-- <param name="laser_model_type" value="beam"/> -->
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>

  <param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
  <param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
  <param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->

  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

실행 스크립트로 새 실행 파일을 생성하고 위의 amcl 실행 구성 파일을 포함합니다.

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
    <!-- include一下上面amcl的launch配置文件，启动AMCL节点 -->
    <include file="$(find mycar_nav)/launch/amcl.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz"/>
</launch>

그런 다음 Gazebo 시뮬레이션 환경을 시작하고 로봇 모델을 로드한 다음 roslaunch 명령을 사용하여 시작 파일을 실행합니다.

roslaunch 包名 launch文件名

rviz 구성 표시줄에서 RobotModel, Map, PoseArray 및 기타 플러그인을 로드하고 터미널에서 키보드 제어 노드를 시작합니다.

rosrun teleop_twist_keyboard teleop_twist_keyboard

그런 다음 키보드를 사용하여 로봇의 움직임을 제어하고 로봇 주변의 빨간색 화살표가 로봇의 실시간 위치 추정을 나타내는 것을 관찰하고 화살표의 밀도가 높을수록 로봇이 나타날 확률이 높아집니다.

3. move_base 경로 계획

move_base는 로봇이 맵의 목표 위치로 안전하게 이동할 수 있는 방법을 제공하는 섀시 이동 기능 패키지입니다. move_base는 전역 경로 플래너와 로컬 경로 플래너를 결합하여 완전한 탐색 프레임워크를 형성합니다.

1. 경로 계획 알고리즘 소개

move_base는 전역 경로 계획 및 로컬 경로 계획을 포함하여 다양한 경로 계획 및 탐색 알고리즘을 사용합니다. 전역 경로 계획은 로봇의 현재 위치에서 알려진 지도의 목표 위치까지 최적의 경로를 찾는 데 주로 사용되며, 로컬 경로 계획은 전역적으로 계획된 경로에 따라 실시간으로 로봇의 움직임을 조정하여 그것은지도에서 알려지지 않은 경로 장애물을 피할 수 있습니다.

일반적으로 사용되는 전역 경로 계획 알고리즘은 다음과 같습니다.：

A 알고리즘 : A*는 최적성(Dijkstra의 알고리즘)과 휴리스틱 정보(Greedy Best-First-Search 알고리즘)를 결합하여 최단 경로를 찾는 휴리스틱 검색 알고리즘입니다. A* 알고리즘의 핵심 아이디어는 각 단계에서 시작점에서 목표점까지의 최소 비용을 추정하고 검색 트리에서 최소 추정 비용이 있는 노드를 선택하여 확장하는 것입니다.

Dijkstra의 알고리즘 : Dijkstra의 알고리즘은 소스 노드에서 그래프의 다른 모든 노드까지의 최단 경로를 찾는 데 사용되는 검색 알고리즘이기도 합니다. 각 단계에서 현재 처리되지 않은 노드 중 거리 값이 가장 작은 노드를 선택하고 이 노드의 이웃 노드에 따라 거리 값을 업데이트합니다.

RRT(Rapidly-exploring Random Trees) 알고리즘 : RRT는 무작위 샘플링을 기반으로 하는 증분 경로 계획 알고리즘입니다. 시작점에서 목표점까지의 경로를 찾을 때까지 트리 형태로 검색 공간에서 빠르게 확장됩니다. RRT 알고리즘은 고차원적이고 복잡한 환경을 처리하는 데 적합합니다.

로컬 경로 계획 알고리즘은：

DWA(Dynamic Window Approach) : DWA는 속도 공간 검색 기반의 로컬 경로 계획 알고리즘입니다. 로봇의 속도 공간에서 실행 가능한 속도 집합을 검색한 다음 전역 경로 및 로컬 장애물에 따라 로봇의 동작을 제어하기 위한 최적의 속도를 선택합니다.

TEB(Timed Elastic Band) : TEB는 최적화 기반의 로컬 경로 계획 알고리즘입니다. 전체 경로를 시간 종속 구성(위치 및 속도)의 시퀀스로 나타내고 탄성 밴드로 취급합니다. 탄성 밴드의 형상을 최적화함으로써 동적 구속 조건을 만족하면서 국부적 장애물을 피할 수 있습니다.

2. 가격표 소개

코스트맵(Costmap)은 로봇 경로 계획 및 장애물 회피를 위해 ROS에서 일반적으로 사용되는 맵 유형으로, 다양한 위치와 방향에서 로봇의 이동 비용을 나타냅니다. 비용 맵은 일반적으로 그리드를 기준으로 표현되며, 각 그리드는 맵에서 작은 영역을 나타내며 해당 값은 해당 영역을 이동하는 비용을 나타냅니다.

move_base의 비용 맵은 다음과 같은 방법으로 구성할 수 있습니다.：

staticLayer Static map layer : 정적 지도는 로봇이 실행되기 전에 라이더나 깊이 카메라와 같은 센서에 의해 수집된 지도이며 ROS의 map_server 소프트웨어 패키지를 통해 게시할 수 있습니다. move_base는 정적 맵 메시지를 직접 구독하여 비용 맵으로 변환할 수 있습니다. 정적 맵 레이어는 단순히 회색조 이미지의 픽셀 값을 ros 비용 맵의 비용 값으로 변환합니다.

ObstacleLayer 장애물 맵 레이어 : 로봇이 움직이기 시작하기 전에 move_base는 장애물 맵 레이어를 추가합니다.이 레이어는 사람, 차량, 가구 등과 같은 로봇 주변의 장애물로 구성됩니다. 라이더 등 로봇이 충돌을 피할 수 있도록 센서가 실시간으로 업데이트됩니다.

InflationLayer 확장 레이어 : move_base는 장애물 맵 레이어를 기반으로 확장 레이어를 추가합니다.이 레이어는 로봇 주변의 장애물을 안전한 거리로 확장할 수 있으므로 비용 맵은 로봇의 사용 가능한 공간을 보다 명확하게 표시하고 피할 수 있습니다. 로봇의 외피 또는 실수로 튀어나와 장애물에 부딪히는 행위

move_base의 비용 맵을 기반으로 한 비용 계산 방법：

위 비용곡선에서 가로축은 로봇 중심으로부터의 거리, 세로축은 Cost Map에서 그리드의 회색값임을 알 수 있으며, 매개변수 설정 시 비용 값을 지정해야 합니다. 비용 맵의 각 그리드에 대한 계산 공식, 비용 맵의 정확성과 적응성을 보장하기 위한 장애물, 경사 및 기타 요인의 가중치

(1) 치명적 장애물: 그리드 값은 254,
(2) 내접 장애물: 그리드 값은 253,
(3) 외접 장애물: 그리드 값은 [128,252],
(4) 자유 공간: 그리드 값은 (0,127],
(5) 자유 영역: 그리드 값은 0,
(6) 알 수 없는 영역: 그리드 값은 255,

2. Move_base 경로 계획 실습

move_base 기능 패키지의 핵심 노드는 move_base이며 이 노드의 작업, 구독 주제, 게시 주제, 서비스 및 관련 매개변수의 세부 정보는 다음과 같습니다.

2.1 액션
move_base/goal(move_base_msgs/MoveBaseActionGoal): move_base의 모션 계획 목표.
move_base/cancel(actionlib_msgs/GoalID): 목표를 취소합니다.
move_base/feedback(move_base_msgs/MoveBaseActionFeedback): 로봇 섀시 좌표를 포함한 지속적인 피드백 정보.
move_base/status(actionlib_msgs/GoalStatusArray): move_base로 전송되는 목표 상태 정보입니다.
move_base/결과(move_base_msgs/MoveBaseActionResult)

2.2 구독 주제
move_base_simple/goal(geometry_msgs/PoseStamped): 동작 계획 목표(동작에 비해 지속적인 피드백이 없으며 로봇의 실행 상태를 추적할 수 없음).

2.3에서 릴리스된 항목
cmd_vel(geometry_msgs/Twist) : 로봇 섀시에 모션 제어 메시지 출력.

2.4 서비스
~make_plan(nav_msgs/GetPlan): 주어진 대상의 계획된 경로를 얻기 위해 이 서비스를 요청하지만 경로 계획은 실행되지 않습니다.
~clear_unknown_space(std_srvs/Empty): 사용자가 로봇 주변의 알 수 없는 공간을 직접 지울 수 있습니다.
~clear_costmaps(std_srvs/Empty): 로봇이 장애물과 충돌할 수 있는 비용 맵의 장애물을 제거할 수 있습니다. 주의하여 사용하십시오.

함수 패키지 아래에 새 param 디렉터리를 만들고 param 디렉터리 아래에 costmap_common_params.yaml, local_costmap_params.yaml, global_costmap_params.yaml 및 base_local_planner_params.yaml과 같은 비용 맵 구성 파일을 만듭니다.

costmap_common_params.yaml

#机器人几何参数，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物


#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

global_costmap_params.yaml

global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_footprint #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 1.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

local_costmap_params.yaml

local_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_footprint #机器人坐标系

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: false  #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 3 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

base_local_planner_params

TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目标公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移动机器人
  holonomic_robot: false

# Forward Simulation Parameters，前进模拟参数
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

그런 다음 시작 파일을 작성하고 move_base 노드, amcl 포지셔닝 노드 및 맵 서비스 노드를 구성합니다.

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find mycar_nav)/launch/amcl.launch" />

    <!-- 运行move_base节点 -->
    <include file="$(find mycar_nav)/launch/path.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />

</launch>

그런 다음 Gazebo 시뮬레이션 환경을 시작하고 로봇 모델을 로드한 다음 roslaunch 명령을 사용하여 시작 파일을 실행합니다.

roslaunch 包名 launch文件名

rviz 구성 표시줄에서 RobotModel, 지도, 오도메트리, 경로 및 기타 플러그인을 로드하고 도구 모음의 2D 탐색 목표에서 목적지를 설정하여 경로 계획을 실현합니다.

---

요약하다

이상은 SLAM의 Gmapping 매핑 및 경로 계획 연구 노트의 전체 내용입니다. Gmapping 알고리즘은 로봇 센서 데이터의 실시간 분석 및 처리를 통해 고정밀 환경 지도를 구축할 수 있으며 환경 지도의 실시간 업데이트를 실현할 수 있어 경로 계획에 중요한 기반을 제공합니다. 경로 계획 알고리즘을 적용하여 로봇이 환경에서 최단 경로를 찾도록 돕고 경로 계획 결과를 실시간으로 업데이트하여 환경 변화에 적응할 수 있습니다.