Lidar 센서를 사용해서 장애물 회피 알고리즘 설계

목표 : Lidar 센서를 통해 얻은 값을 이용해서, 앞의 장애물을 회피하도록 알고리즘을 설계한다.

라이다로 거리, 각도 테스트

측정되는 값

Setting

라이다 센서를 차량에 장착하고, 180도 부분이 앞을 보도록 배치 함.

구조물에 의해서 측정이 불가능한 구역을 없애고, 90도~ 270도 사이의 값을 사용해서 알고리즘을 설계 함.

1. 변수 설정

위 사진과 같이,
장애물을 발견했을 때 왼쪽 각도 ~ 오른쪽 각도, 거리를 변수로 설정했다.\

인식되는 범위 안에 들어왔을 경우, 각도와 거리가 측정된다.

2. 상태 머신 설계 (State Machine)

차량이 인식되는 범위를 계속 실험하면서, 크게 3가지 상태로 나누어서 설계 했다.

1. 장애물이 도로 오른 편에 있을 때
2. 장애물이 도로 왼 편에 있을 때
3. 도로 중앙에 있을 때

이렇게 설정하고 알고리즘을 제작하면, 한 가지 예외경우가 발생하게 된다.
theta_L이 왼쪽인식, theta_R이 오른쪽 인식이라고 했을 때
theta_L <= 차 중앙 <= theta_R
일 경우, 1번 2번 상태가 나오지 않는다.

그래서 다시 3번의 조건을 세분화 했다.

1. 장애물이 도로 오른 편에 있을 때
   (차 중앙 <= theta_L <= theta_R)
   

2. 장애물이 도로 왼 편에 있을 때
   (theta_L <= theta_R <= 차 중앙)
   

3. 도로 중앙에 있을 때 (theta_L <= 차 중앙 <= theta_R)
   장애물의 폭을 계산해서 통과가 가능한지 판단.
   (1) 도로 오른 편에 있을 때
   (2) 도로 왼 편에 있을 떄
   (3) 장애물 통과가 불가능한 경우
   

3. 코드 구현

우선 장애물의 폭을 측정하는 방식은 Case별로 당연히 달라진다.
인식하는 각도에 따라 Case를 나누고, 측정되는 값에 따라 삼각함수를 이용한다면 장애물의 폭이 계산가능하다.

if((objFlag == 1 && P.ang_L<=(185+Set_Angle*25)) && (P.ang_R>=(175+Set_Angle*25))){
		Obj_state = Deflection_No;
	}
else if(objFlag == 1 && P.ang_L>(183+Set_Angle*25) && P.ang_L<(Road_ang+(180+Set_Angle*25))){  //Line Scan Camera의 조향 값을 받아와서 차량 주행 방향에 맞춰서 사용 각도 범위 변환
		Obj_state = R_On_Road;
	}
else if(objFlag == 1 && P.ang_R<(177+Set_Angle*25) && P.ang_R>((180+Set_Angle*25)-Road_ang)){
		Obj_state = L_On_Road;
	}
else{
		Obj_state = No_Object;
	}