Airsim无人机状态获取getMultirotorState
getMultirotorState
此 API 一次调用即可返回车辆的状态。状态包括碰撞、估计运动学(即通过融合传感器计算的运动学)和时间戳(自纪元以来的纳秒)。这里的运动学意味着 6 个量:位置、方向、线速度和角速度、线速度和角加速度。请注意,simple_slight 目前不支持状态估计器,这意味着 simple_flight 的估计和地面实况运动学值将相同。但是,除了角加速度外,还可以为 PX4 提供估计的运动学。所有量都在 NED 坐标系中,世界坐标系中的 SI 单位除了角速度和加速度在体坐标系中。
这个状态是由传感器估计的状态,并不是无人机状态的真值。
AirSim默认的无人机底层飞控 simple_flight 并不支持状态估计,所以如果是simple_flight 飞控,此函数得到的状态与真值相同。
使用PX4 飞控可以获取估计的状态
fly_state = client.getMultirotorState()
# query vehicle state
def getMultirotorState(self, vehicle_name = ''):
"""
Args:
vehicle_name (str, optional): Vehicle to get the state of
Returns:
MultirotorState:
"""
return MultirotorState.from_msgpack(self.client.call('getMultirotorState', vehicle_name))
getMultirotorState.__annotations__ = {'return': MultirotorState}
class MultirotorState(MsgpackMixin):
collision = CollisionInfo() # 碰撞信息
kinematics_estimated = KinematicsState() # 状态信息
gps_location = GeoPoint() # GPS 信息
timestamp = np.uint64(0) # 时间戳
landed_state = LandedState.Landed # 是否是降落状态
rc_data = RCData() # 遥控器数据
ready = False
ready_message = ""
can_arm = False
KinematicsState
此API获得的是相对于起始位置的无人机状态,其中速度为起始位置xyz方向的速度。
class KinematicsState(MsgpackMixin):
position = Vector3r() # 位置
orientation = Quaternionr() # 姿态角
linear_velocity = Vector3r() # 速度
angular_velocity = Vector3r() # 机体角速率
linear_acceleration = Vector3r() # 加速度
angular_acceleration = Vector3r() # 机体角加速度
相对于全局的速度转换为无人机自身的速度
fly_state = client.getMultirotorState()
vel_x = fly_state.kinematics_estimated.linear_velocity.x_val
vel_y = fly_state.kinematics_estimated.linear_velocity.y_val
vel_z = fly_state.kinematics_estimated.linear_velocity.z_val
x = fly_state.kinematics_estimated.orientation.x_val
y = fly_state.kinematics_estimated.orientation.y_val
z = fly_state.kinematics_estimated.orientation.z_val
w = fly_state.kinematics_estimated.orientation.w_val
vel_x_self = (1-2*(y**2)-2*(z**2))*vel_x + (2*x*y - 2*z*w)*vel_y + (2*x*z + 2*y*w)*vel_z
vel_y_self = (2*x*y + 2*z*w)*vel_x + (1 - 2*(x**2) - 2*(z**2))*vel_y + (2*y*z - 2*x*w)*vel_z
vel_z_self = (2*x*z - 2*y*w)*vel_x + (2*y*z + 2*x*w)*vel_y + (1-2*(x**2)-2*(y**2))*vel_z
return vel_x_self,vel_y_self,vel_z_self