无人机SET_POSITION_TARGET_LOCAL_NED控制方法转向异常排查

存在的问题 #

1. 坐标系选择错误：当前使用MAV_FRAME_BODY_FRD（机体坐标系），该坐标系下的yaw参数是相对于无人机自身机头的偏移量，而非绝对世界坐标系航向角，飞控无法识别这种方式传递的转向指令，导致转向失效。
2. 指令时间戳缺失：构造SetPositionTargetLocalNed时未设置timeBootMs，部分飞控依赖该时间戳验证指令有效性，可能导致指令被忽略。
3. 初始航向 fallback 逻辑不合理：当缓存中无yaw数据时直接用0作为初始航向，可能导致转向基准错误。

修改步骤 #

1. 更换为局部NED坐标系 #

将coordinateFrame从MavFrame.MAV_FRAME_BODY_FRD改为MavFrame.MAV_FRAME_LOCAL_NED，此时设置的yaw值会被飞控识别为世界坐标系下的绝对航向角，符合转向需求。

2. 补充指令时间戳 #

添加timeBootMs参数，优先从缓存获取无人机开机时间戳，无缓存时通过系统时间与无人机开机时间计算，确保飞控能正确处理指令。

3. 优化初始航向获取逻辑 #

当缓存无yaw数据时，尝试从无人机实时状态获取航向，避免直接使用0导致的基准错误。

修改后的完整代码 #

public void droneControl(String sn, float x, float y, float h, float w) throws Exception {
    isConnection(sn);
    
    setOldUavMode(sn, 1);  // 切换到offboard模式
    
    float currentYawRad = 0.0f;
    UavDataCacheManager cacheManager = UavDataCacheManager.getInstance();
    Map<String, Object> uavDataBySn = cacheManager.getUavDataBySn(sn);
    
    // 优先读取缓存中的航向数据
    if (ObjectUtil.isNotEmpty(uavDataBySn) && ObjectUtil.isNotEmpty(uavDataBySn.get("yaw"))) {
        currentYawRad = (float) uavDataBySn.get("yaw");
    } else {
        // 缓存无数据时，尝试从无人机实时状态获取（需根据实际实现调整）
        MavlinkUav uav = getMavlinkUav(sn);
        if (uav.getState() != null && uav.getState().getYaw() != null) {
            currentYawRad = uav.getState().getYaw();
        }
    }
    
    // 处理转向指令
    if (w != 0) {
        float deltaYaw = (float) Math.toRadians(-w); // 弧度转换，符号需根据飞控转向逻辑验证
        currentYawRad += deltaYaw;
        // 归一化航向到[-π, π]区间
        currentYawRad = (float) ((currentYawRad + Math.PI) % (2 * Math.PI) - Math.PI);
    }
    
    MavlinkUav uav = getMavlinkUav(sn);
    
    // 获取开机时间戳
    long timeBootMs = 0;
    if (ObjectUtil.isNotEmpty(uavDataBySn) && ObjectUtil.isNotEmpty(uavDataBySn.get("timeBootMs"))) {
        timeBootMs = (long) uavDataBySn.get("timeBootMs");
    } else {
        timeBootMs = System.currentTimeMillis() - uav.getBootTime();
    }

    SetPositionTargetLocalNed message = SetPositionTargetLocalNed.builder()
            .timeBootMs(timeBootMs) // 添加指令时间戳
            .targetSystem(uav.getTargetSystem())
            .targetComponent(uav.getTargetComponent())
            .coordinateFrame(MavFrame.MAV_FRAME_LOCAL_NED) // 更换为局部NED坐标系
            .typeMask(PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE,
                    PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE,
                    PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                    PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE,
                    PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE,
                    PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE,
                    PositionTargetTypemask.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)
            .x(x)
            .y(y)
            .z(-h)
            .vx(0.0f)
            .vy(0.0f)
            .vz(0.0f)
            .afx(0.0f)
            .afy(0.0f)
            .afz(0.0f)
            .yaw(currentYawRad)
            .yawRate(0.0f)
            .build();
    
    MavlinkConnection connection = uav.getMavlinkConnection();
    logger.info("The command sent by the command flight is", message);
    connection.send2(
            uav.getTargetSystem(),
            uav.getTargetComponent(),
            message);
}

额外验证点 #

• 确认deltaYaw的符号是否符合飞控转向逻辑：若输入的w为顺时针转向角度，负号可能需要调整，需结合飞控文档验证。
• 检查setOldUavMode(sn, 1)是否正确切换到OFFBOARD模式：部分飞控的模式枚举值不同，需确保参数对应正确模式。

内容的提问来源于stack exchange，提问作者Junjie Kang