이동하기(SET_POSITION_TARGET_LOCAL_NED)

저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo

Previous이륙하기(MAV_CMD_NAV_TAKEOFF)Next이동하기(SET_POSITION_TARGET_GLOBAL_INT)

Last updated 8 months ago

이동하기(SET_POSITION_TARGET_LOCAL_NED)

저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo

개요

드론의 이동 명령은 아래 3가지 MAV_CMD가 사용될 수 있으며, GUIDED 모드(ArduPilot) 또는 OFFBOARD 모드(PX4) 에 있는 동안 드론의 위치, 속도 또는 자세를 제어하는 데 사용할 수 있습니다. 본 교재에서는 와 대해 학습 합니다. ( 은 잘 사용되지 않음)

SET_POSITION_TARGET_LOCAL_NED

차량의 목표 위치(EKF 원점에서 NED의 오프셋으로), 속도, 가속도, 방향 또는 선회율을 설정합니다. 메시지 정의는 다음과 같습니다.

명령 필드

설명

time_boot_ms

부팅 이후 보낸 사람의 시스템 시간(밀리초)

target_system

차량의 시스템 ID

target_component

비행 컨트롤러의 구성 요소 ID 또는 0

coordinate_frame

사용할 수 있는 MAV_FRAME은 다음과 같습니다.

type_mask

차량에서 무시 해야 하는 필드를 나타내는 비트 마스크 (POSITION_TARGET_TYPEMASK 열거형 참조)

bit1:PosX, bit2:PosY, bit3:PosZ, bit4:VelX, bit5:VelY, bit6:VelZ, bit7:AccX, bit8:AccY, bit9:AccZ, bit11:yaw, bit12:yaw 비율

Pos, Vel 및/또는 Accel을 제공할 때 3축을 모두 제공해야 합니다. Pos, Vel 및 Accel 중 하나 이상을 제공해야 합니다(예: Yaw 또는 YawRate만 제공하는 것은 지원되지 않음).

사용 위치 : 0b110111111000 / 0x0DF8 / 3576(십진수)
사용 속도 : 0b110111000111 / 0x0DC7 / 3527(십진수)
가속 사용 : 0b110000111111 / 0x0C3F / 3135 (십진수)
Pos+Vel 사용 : 0b110111000000 / 0x0DC0 / 3520(십진수)
Pos+Vel+Accel 사용 : 0b110000000000 / 0x0C00 / 3072(십진수)
요 사용 : 0b100111111111 / 0x09FF / 2559(십진수)
요레이트 사용 : 0b010111111111 / 0x05FF / 1535(십진수)

미터 단위의 X 위치(양수는 전방 또는 북쪽)

미터 단위의 Y 위치(양수는 오른쪽 또는 동쪽)

미터 단위의 Z 위치(양수는 아래)

m/s 단위의 X 속도(양수는 전방 또는 북쪽)

Y 속도(m/s)(양수는 오른쪽 또는 동쪽)

Z 속도(m/s)(양수는 아래)

afx

m/s/s 단위의 X 가속(양수는 전방 또는 북쪽)

afy

Y 가속도(m/s/s)(양수는 오른쪽 또는 동쪽)

afz

m/s/s 단위의 Z 가속도(양수는 다운)

yaw

yaw 또는 라디안 방향(0은 전방 또는 북쪽)

yaw_rate

요레이트(rad/s)

필드 `coordinate_frame`는 다음 값을 사용합니다.

프레임

설명

MAV_FRAME_LOCAL_NED(1)

위치는 NED 프레임에서 차량의 EKF 원점을 기준으로 합니다.

즉 x=1,y=2,z=3은 원점에서 북쪽 1m, 동쪽 2m, 아래쪽 3m입니다.

EKF 원점 은 차량이 처음으로 위치 추정을 달성했을 때의 차량 위치입니다.

속도와 가속도는 NED 프레임에 있습니다.

MAV_FRAME_LOCAL_OFFSET_NED(7)

위치는 차량의 현재 위치를 기준으로 합니다.

즉 x=1,y=2,z=3은 현재 위치에서 북쪽으로 1m, 동쪽으로 2m, 아래로 3m입니다.

속도와 가속도는 NED 프레임에 있습니다.

MAV_FRAME_BODY_NED(8)

위치는 NED 프레임의 EKF 원점을 기준으로 합니다.

즉 x=1,y=2,z=3은 원점에서 북쪽 1m, 동쪽 2m, 아래쪽 3m입니다.

속도 및 가속도는 현재 차량 방향에 상대적입니다. 이를 사용하여 앞으로, 오른쪽 및 아래로(또는 음수 값을 사용하는 경우 반대) 속도를 지정합니다.

MAV_FRAME_BODY_OFFSET_NED(9)

위치는 차량의 현재 위치 및 방향에 상대적입니다.

즉 x=1,y=2,z=3은 현재 위치에서 앞으로 1m, 오른쪽으로 2m, 아래로 3m

속도 및 가속도는 현재 차량 방향에 상대적입니다. 이를 사용하여 앞으로, 오른쪽 및 아래로(또는 음수 값을 사용하는 경우 반대) 속도를 지정합니다.

차량 요가 변경되지 않도록 요율을 0으로 지정

팁

프레임에서 _OFFSET_"차량 위치 기준"을 의미하고 _LOCAL_"홈 위치 기준"을 의미합니다( 속도 방향에 영향을 미치지 않음). _BODY_속도 구성 요소가 NED 프레임이 아닌 차량의 방향에 상대적임을 의미합니다.

메모

속도나 가속도 명령을 보내면 1초마다 다시 보내야 함(명령을 받지 않으면 3초 후 차량 정지)

예

다음은 이 명령을 테스트하기 위해 MAVProxy(일명 SITL)에 복사하여 붙여넣을 수 있는 몇 가지 예제 명령입니다. 이 명령을 실행하기 전에 다음을 실행하시오.

module load message
GUIDED 모드로 변경
암 스로틀
이륙 10m

MAVProxy/SITL 명령의 예

설명

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 3576 100 0 -10 0 0 0 0 0 0 0 0

EKF 원점에서 북쪽으로 100m, 위쪽 으로 10m 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 7 3576 10 0 0 0 0 0 0 0 0 0 0

현재 위치에서 북쪽으로 10m 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 3576 10 0 0 0 0 0 0 0 0 0 0

현재 위치에서 10m 전방 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 3527 0 0 0 1 0 0 0 0 0 0 0

북쪽으로 1m/s로 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 3527 0 0 0 1 0 0 0 0 0 0 0

1m/s로 앞으로 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 1479 0 0 0 0 1 0 0 0 0 0 0

0의 요율로 1m/s로 오른쪽으로 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 3135 0 0 0 0 0 0 1 0 0 0 0

북쪽으로 1m/s로 가속

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 3135 0 0 0 0 0 0 1 0 0 0 0

1m/s로 앞으로 가속

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 1087 0 0 0 0 0 0 0 1 0 0 0

0의 요율로 1m/s에서 오른쪽으로 가속

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 2503 0 0 0 0 0 0 0 0 0 0.7854 0

북동쪽으로 회전(Yaw 대상 + 속도 0)

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 2503 0 0 0 0 0 0 0 0 0 0.7854 0

오른쪽으로 45도 회전(Yaw target + 0의 속도)

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 1479 0 0 0 0 0 0 0 0 0 0 0.174

시계 방향으로 10deg/sec로 회전(속도 0)

실습

set_position_local_ned 메시지를 이용하여 드론을 이동하는 코드를 작성해 보겠습니다.

# set_position_target_local_ned - move to north 30m, alt 20m
the_connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_local_ned_message(10, the_connection.target_system, the_connection.target_component, 
                        mavutil.mavlink.MAV_FRAME_LOCAL_NED, int(0b110111111000), 30, 0, -20, 0,0,0,0,0,0,0,0))

# check response NAV_CONTROLLER_OUTPUT
while 1:
    msg = the_connection.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
    print(msg)

전체 코드는 아래와 같습니다. movement.py 파일을 만들고 아래 코드를 입력 합니다.

from pymavlink import mavutil
import time

# UDP 14550 for primary GSC
#the_connection = mavutil.mavlink_connection('udpin:localhost:14550')
#for ArduPilot
#the_connection = mavutil.mavlink_connection('tcp:localhost:5763')
#for PX4-SITL
the_connection = mavutil.mavlink_connection('udp:localhost:14540')

# Wait for the first heartbeat 
#   This sets the system and component ID of remote system for the link
the_connection.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % 
      (the_connection.target_system, the_connection.target_component))

# change MODE to GUIDED into COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 
                                     4, 0, 0, 0, 0, 0)

# check response COMMAND_ACK
msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
print(msg)

# Arming COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0)

# check response COMMAND_ACK
msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
print(msg)

# Takeoff COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 10)

# check response COMMAND_ACK
msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
print(msg)

time.sleep(10)

# set_position_target_local_ned - move to north 30m, alt 20m
the_connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_local_ned_message(10, the_connection.target_system, the_connection.target_component, 
                        mavutil.mavlink.MAV_FRAME_LOCAL_NED, int(0b110111111000), 30, 0, -20, 0,0,0,0,0,0,0,0))

# check response NAV_CONTROLLER_OUTPUT
while 1:
    msg = the_connection.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
    print(msg)

# # check response NAV_CONTROLLER_OUTPUT
# while 1:
#     msg = the_connection.recv_match(type='LOCAL_POSITION_NED', blocking=True)
#     print(msg)

이제 터미널 창을 열고 실행을 해 봅시다.

$ python3 movement.py 
Heartbeat from system (system 1 component 0)
COMMAND_ACK {command : 176, result : 0, progress : 0, result_param2 : 0, target_system : 255, target_component : 0}
COMMAND_ACK {command : 400, result : 0, progress : 0, result_param2 : 0, target_system : 255, target_component : 0}
COMMAND_ACK {command : 22, result : 0, progress : 0, result_param2 : 0, target_system : 255, target_component : 0}
LOCAL_POSITION_NED {time_boot_ms : 25645233, x : 5.907643795013428, y : -0.05608998239040375, z : -19.999507904052734, vx : -4.659669876098633, vy : -0.022126592695713043, vz : 0.00591622618958354}
LOCAL_POSITION_NED {time_boot_ms : 25645484, x : 4.77339506149292, y : -0.05335403233766556, z : -19.99872398376465, vx : -4.309316635131836, vy : -0.026195377111434937, vz : 0.006196418311446905}

드론이 고도 20m, 북쪽 방향으로 30m 전진하는 것을 볼 수 있습니다.

Previous이륙하기(MAV_CMD_NAV_TAKEOFF)Next이동하기(SET_POSITION_TARGET_GLOBAL_INT)

Last updated 8 months ago

개요

SET_POSITION_TARGET_LOCAL_NED

차량의 목표 위치(EKF 원점에서 NED의 오프셋으로), 속도, 가속도, 방향 또는 선회율을 설정합니다. 메시지 정의는 다음과 같습니다.

명령 필드

설명

time_boot_ms

부팅 이후 보낸 사람의 시스템 시간(밀리초)

target_system

차량의 시스템 ID

target_component

비행 컨트롤러의 구성 요소 ID 또는 0

coordinate_frame

사용할 수 있는 MAV_FRAME은 다음과 같습니다.

= 1, = 7,

= 8,

= 9

type_mask

차량에서 무시 해야 하는 필드를 나타내는 비트 마스크 (POSITION_TARGET_TYPEMASK 열거형 참조)

bit1:PosX, bit2:PosY, bit3:PosZ, bit4:VelX, bit5:VelY, bit6:VelZ, bit7:AccX, bit8:AccY, bit9:AccZ, bit11:yaw, bit12:yaw 비율

사용 위치 : 0b110111111000 / 0x0DF8 / 3576(십진수)
사용 속도 : 0b110111000111 / 0x0DC7 / 3527(십진수)
가속 사용 : 0b110000111111 / 0x0C3F / 3135 (십진수)
Pos+Vel 사용 : 0b110111000000 / 0x0DC0 / 3520(십진수)
Pos+Vel+Accel 사용 : 0b110000000000 / 0x0C00 / 3072(십진수)
요 사용 : 0b100111111111 / 0x09FF / 2559(십진수)
요레이트 사용 : 0b010111111111 / 0x05FF / 1535(십진수)

미터 단위의 X 위치(양수는 전방 또는 북쪽)

미터 단위의 Y 위치(양수는 오른쪽 또는 동쪽)

미터 단위의 Z 위치(양수는 아래)

m/s 단위의 X 속도(양수는 전방 또는 북쪽)

Y 속도(m/s)(양수는 오른쪽 또는 동쪽)

Z 속도(m/s)(양수는 아래)

afx

m/s/s 단위의 X 가속(양수는 전방 또는 북쪽)

afy

Y 가속도(m/s/s)(양수는 오른쪽 또는 동쪽)

afz

m/s/s 단위의 Z 가속도(양수는 다운)

yaw

yaw 또는 라디안 방향(0은 전방 또는 북쪽)

yaw_rate

요레이트(rad/s)

필드 `coordinate_frame`는 다음 값을 사용합니다.

프레임

설명

MAV_FRAME_LOCAL_NED(1)

위치는 NED 프레임에서 차량의 EKF 원점을 기준으로 합니다.

즉 x=1,y=2,z=3은 원점에서 북쪽 1m, 동쪽 2m, 아래쪽 3m입니다.

EKF 원점 은 차량이 처음으로 위치 추정을 달성했을 때의 차량 위치입니다.

속도와 가속도는 NED 프레임에 있습니다.

MAV_FRAME_LOCAL_OFFSET_NED(7)

위치는 차량의 현재 위치를 기준으로 합니다.

즉 x=1,y=2,z=3은 현재 위치에서 북쪽으로 1m, 동쪽으로 2m, 아래로 3m입니다.

속도와 가속도는 NED 프레임에 있습니다.

MAV_FRAME_BODY_NED(8)

위치는 NED 프레임의 EKF 원점을 기준으로 합니다.

즉 x=1,y=2,z=3은 원점에서 북쪽 1m, 동쪽 2m, 아래쪽 3m입니다.

속도 및 가속도는 현재 차량 방향에 상대적입니다. 이를 사용하여 앞으로, 오른쪽 및 아래로(또는 음수 값을 사용하는 경우 반대) 속도를 지정합니다.

MAV_FRAME_BODY_OFFSET_NED(9)

위치는 차량의 현재 위치 및 방향에 상대적입니다.

즉 x=1,y=2,z=3은 현재 위치에서 앞으로 1m, 오른쪽으로 2m, 아래로 3m

속도 및 가속도는 현재 차량 방향에 상대적입니다. 이를 사용하여 앞으로, 오른쪽 및 아래로(또는 음수 값을 사용하는 경우 반대) 속도를 지정합니다.

차량 요가 변경되지 않도록 요율을 0으로 지정

팁

프레임에서 _OFFSET_"차량 위치 기준"을 의미하고 _LOCAL_"홈 위치 기준"을 의미합니다( 속도 방향에 영향을 미치지 않음). _BODY_속도 구성 요소가 NED 프레임이 아닌 차량의 방향에 상대적임을 의미합니다.

메모

속도나 가속도 명령을 보내면 1초마다 다시 보내야 함(명령을 받지 않으면 3초 후 차량 정지)

예

module load message
GUIDED 모드로 변경
암 스로틀
이륙 10m

MAVProxy/SITL 명령의 예

설명

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 3576 100 0 -10 0 0 0 0 0 0 0 0

EKF 원점에서 북쪽으로 100m, 위쪽 으로 10m 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 7 3576 10 0 0 0 0 0 0 0 0 0 0

현재 위치에서 북쪽으로 10m 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 3576 10 0 0 0 0 0 0 0 0 0 0

현재 위치에서 10m 전방 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 3527 0 0 0 1 0 0 0 0 0 0 0

북쪽으로 1m/s로 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 3527 0 0 0 1 0 0 0 0 0 0 0

1m/s로 앞으로 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 1479 0 0 0 0 1 0 0 0 0 0 0

0의 요율로 1m/s로 오른쪽으로 비행

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 3135 0 0 0 0 0 0 1 0 0 0 0

북쪽으로 1m/s로 가속

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 3135 0 0 0 0 0 0 1 0 0 0 0

1m/s로 앞으로 가속

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 1087 0 0 0 0 0 0 0 1 0 0 0

0의 요율로 1m/s에서 오른쪽으로 가속

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 2503 0 0 0 0 0 0 0 0 0 0.7854 0

북동쪽으로 회전(Yaw 대상 + 속도 0)

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 9 2503 0 0 0 0 0 0 0 0 0 0.7854 0

오른쪽으로 45도 회전(Yaw target + 0의 속도)

message SET_POSITION_TARGET_LOCAL_NED 0 0 0 1 1479 0 0 0 0 0 0 0 0 0 0 0.174

시계 방향으로 10deg/sec로 회전(속도 0)

실습

set_position_local_ned 메시지를 이용하여 드론을 이동하는 코드를 작성해 보겠습니다.

# set_position_target_local_ned - move to north 30m, alt 20m
the_connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_local_ned_message(10, the_connection.target_system, the_connection.target_component, 
                        mavutil.mavlink.MAV_FRAME_LOCAL_NED, int(0b110111111000), 30, 0, -20, 0,0,0,0,0,0,0,0))

# check response NAV_CONTROLLER_OUTPUT
while 1:
    msg = the_connection.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
    print(msg)

전체 코드는 아래와 같습니다. movement.py 파일을 만들고 아래 코드를 입력 합니다.

from pymavlink import mavutil
import time

# UDP 14550 for primary GSC
#the_connection = mavutil.mavlink_connection('udpin:localhost:14550')
#for ArduPilot
#the_connection = mavutil.mavlink_connection('tcp:localhost:5763')
#for PX4-SITL
the_connection = mavutil.mavlink_connection('udp:localhost:14540')

# Wait for the first heartbeat 
#   This sets the system and component ID of remote system for the link
the_connection.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % 
      (the_connection.target_system, the_connection.target_component))

# change MODE to GUIDED into COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 
                                     4, 0, 0, 0, 0, 0)

# check response COMMAND_ACK
msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
print(msg)

# Arming COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0)

# check response COMMAND_ACK
msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
print(msg)

# Takeoff COMMAND_LONG
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component, 
                                     mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 10)

# check response COMMAND_ACK
msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
print(msg)

time.sleep(10)

# set_position_target_local_ned - move to north 30m, alt 20m
the_connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_local_ned_message(10, the_connection.target_system, the_connection.target_component, 
                        mavutil.mavlink.MAV_FRAME_LOCAL_NED, int(0b110111111000), 30, 0, -20, 0,0,0,0,0,0,0,0))

# check response NAV_CONTROLLER_OUTPUT
while 1:
    msg = the_connection.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
    print(msg)

# # check response NAV_CONTROLLER_OUTPUT
# while 1:
#     msg = the_connection.recv_match(type='LOCAL_POSITION_NED', blocking=True)
#     print(msg)

이제 터미널 창을 열고 실행을 해 봅시다.

$ python3 movement.py 
Heartbeat from system (system 1 component 0)
COMMAND_ACK {command : 176, result : 0, progress : 0, result_param2 : 0, target_system : 255, target_component : 0}
COMMAND_ACK {command : 400, result : 0, progress : 0, result_param2 : 0, target_system : 255, target_component : 0}
COMMAND_ACK {command : 22, result : 0, progress : 0, result_param2 : 0, target_system : 255, target_component : 0}
LOCAL_POSITION_NED {time_boot_ms : 25645233, x : 5.907643795013428, y : -0.05608998239040375, z : -19.999507904052734, vx : -4.659669876098633, vy : -0.022126592695713043, vz : 0.00591622618958354}
LOCAL_POSITION_NED {time_boot_ms : 25645484, x : 4.77339506149292, y : -0.05335403233766556, z : -19.99872398376465, vx : -4.309316635131836, vy : -0.026195377111434937, vz : 0.006196418311446905}

드론이 고도 20m, 북쪽 방향으로 30m 전진하는 것을 볼 수 있습니다.

개요

SET_POSITION_TARGET_LOCAL_NED

필드 coordinate_frame는 다음 값을 사용합니다.

팁

메모

예

실습

개요

SET_POSITION_TARGET_LOCAL_NED

필드 coordinate_frame는 다음 값을 사용합니다.

팁

메모

예

실습

필드 `coordinate_frame`는 다음 값을 사용합니다.

필드 `coordinate_frame`는 다음 값을 사용합니다.