airsim机体坐标系下的机体速度的获取与recording函数修改

最新推荐文章于 2024-09-11 21:45:00 发布

RL_FEIWU

最新推荐文章于 2024-09-11 21:45:00 发布

阅读量1k

点赞数 1

文章标签：矩阵 python 算法

本文链接：https://blog.csdn.net/qq_42704954/article/details/122182289

版权

本文介绍如何从airsim的局部坐标系转换到机体坐标系，利用四元数解算得到机体速度vel_body。airsim的getMultirotorState API提供了无人机状态信息，包括四旋翼飞行状态、GPS、时间戳等，但不直接提供机体速度。通过旋转矩阵的逆操作，可以得到从局部到机体的转换矩阵。参考相关博客和官方源码实现这一过程。

摘要由CSDN通过智能技术生成

airsim局部坐标系转化机体坐标系
airsim官方提供的state不包含机体坐标系的速度等信息，但包含了局部坐标系下的速度和当前机体在初始点的四元数矩阵，因此可通过四元数解算得到机体坐标系下的速度vel_body
一般来说四元矩阵的表示如下所示：
在这里插入图片描述

因此构造得到的旋转矩阵如下所示：

但得到的是从机体坐标系到局部坐标系的旋转矩阵，我们想要得到的是从局部坐标系到机体坐标系的旋转矩阵
由于旋转的可逆性和旋转矩阵的本身性质（旋转矩阵的逆即为矩阵的转置）
因此，局部坐标系向机体坐标系转化的旋转矩阵为：
在这里插入图片描述

airsim代码示意:

airsim提供了getMultirotorState作为获取无人机飞行状态信息的api函数。
该函数能够获取无人机的参数信息：是否碰撞、无人机的状态（在simple飞行模式下为ue4构建的四旋翼飞行状态，以全局坐标系为基准）、GPS信息、时间戳、是否降落、遥控器状态等信息。

	 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

	因此通过该api获取无人机的全局速度与四元数，构造上述旋转矩阵即可得到无人机的局部速度（全局）向机体坐标系的转化

		fly_state = self.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