基于大疆 MSDK 的无人机视觉引导自适应降落实现

速度分量计算:

val bearing = calculateBearing(currentLat, currentLon, targetLat, targetLon)
val bearingRad = Math.toRadians(bearing)
// 使用 GROUND 坐标系 (地面坐标系)
val navParam = VirtualStickFlightControlParam().apply {
    rollPitchCoordinateSystem = FlightCoordinateSystem.GROUND
    verticalControlMode = VerticalControlMode.POSITION
    yawControlMode = YawControlMode.ANGLE
    rollPitchControlMode = RollPitchControlMode.VELOCITY
    // 将速度分解为南北和东西分量
    pitch = NAVIGATION_SPEED * Math.cos(bearingRad) // 南北分量 (5m/s)
    roll = NAVIGATION_SPEED * Math.sin(bearingRad) // 东西分量 (5m/s)
    yaw = bearing // 让机头指向目标
    verticalThrottle = targetAlt
}

• GROUND 坐标系是绝对方向,不受无人机朝向影响
• pitch 控制南北，roll 控制东西。

第二步：判断何时到达目标点上方附近

持续监测距离

每 100ms 检查一次当前位置与目标的距离，距离小于预期值 ARRIVAL_THRESHOLD,就认为无人机已到达目标点上方附近，停止导航，开始降落:

val navTask = object : Runnable {
    override fun run() {
        val currentLoc = getAircraftLocation()
        val remainingDistance = calculateDistance(
            currentLoc.latitude,
            currentLoc.longitude,
            targetLat,
            targetLon
        )
        if (remainingDistance < ARRIVAL_THRESHOLD) { // 10 米内
            // 到达目标，停止导航，开始降落
            isNavigating = false
            startDynamicAdjustment()
        } else {
            // 继续飞行
            sendNavigationCommand()
            virtualStickHandler?.postDelayed(this, 100)
        }
    }
}

第三步：精确降落到指定点

无人机虽然到了目标附近 (10 米内),但有以下问题：

1. GPS 精度有限(±3 米),不够精确。
2. 风力影响,有时候受风的影响，无人机会偏离。

解决方案：视觉识别 + 位置调整

工作原理:

1. 无人机摄像头识别地面的特定图像(如二维码、标记点)
2. 视觉算法计算偏移量(X 轴左右,Y 轴前后,Z 轴距图像距离)
3. 将偏移量传给无人机
4. 无人机调整位置，边降落边对准

数据结构:

private var xOffset: Double = 0.0 // X 轴偏移 (米),正=右，负=左
private var yOffset: Double = 0.0 // Y 轴偏移 (米),正=前，负=后
private var zDistance: Double = 0.0 // Z 轴距离 (米),距降落点高度

外部接口:

// 视觉识别系统调用这些方法更新偏移量 (~1Hz)
fun setXOffset(offset: Double) { xOffset = offset }
fun setYOffset(offset: Double) { yOffset = offset }
fun setZDistance(distance: Double) { zDistance = distance }

采用自适应策略，一边降落一遍调整

关键点:
在不同的高度，我们允许的偏移量阈值不同的，高度较高的时候，偏移量就算比较大也可以下降，随着高度降低，我们允许的偏移量阈值会不断缩小（要求越来越向中间对齐）

真实偏移超出偏移量阈值的 2 倍就停止下降，只进行对齐调整；
真实偏移超出偏移量的 1 倍，就以 0.1m/s 的慢速一边降落一边调整；
在偏移量范围内，且高度> 20m，以 0.5m/s 的速度快速下降；
在偏移量范围内，且高度在 5m-20m 之间，以 0.2m/s 的速度下降；
在偏移量范围内，且高度< 5m，以 0.2m/s 速度下降；

实现:

// 1. 根据高度动态计算允许的误差
private fun getOffsetThreshold(altitude: Double): Double {
    return when {
        altitude > 50.0 -> 1.0 // 高空：允许 1 米偏移误差
        altitude > 20.0 -> 0.5 // 中空：允许 0.5 米偏移误差
        altitude > 5.0 -> 0.3 // 低空：允许 0.3 米偏移误差
        else -> 0.2 // 极低空：要求 0.2 米精度
    }
}

// 2. 根据高度和偏移量动态计算下降速度
private fun getDescentSpeed(altitude: Double, xOffset: Double, yOffset: Double): Double {
    val threshold = getOffsetThreshold(altitude)
    return when {
        xOffset > threshold * 2 || yOffset > threshold * 2 -> 0.0 // 偏移太大：停止下降
        xOffset > threshold || yOffset > threshold -> 0.1 // 偏移较大：慢降
        altitude > 20.0 -> 0.5 // 中高空：快降
        altitude > 5.0 -> 0.2 // 低空：慢降
        else -> 0.2 // 极低空：极慢降
    }
}

控制逻辑:

• 大于 50m / 20-50m / 5-20m / 小于 5m：高度判断
• 偏移大于阈值的 2 倍：停止下降，只调整
• 偏移大于阈值：慢降 0.1m/s 并且调整
• 偏移小于阈值：快降并且微调
• 高度小于等于 0.1m：着陆完成

第四步：处理避障，降落后停桨。

问题：下视避障会阻止降落

无人机的下视避障系统会将地面识别为障碍物，在接近地面时自动停止下降，我们在高度为 5m 的时候关闭下视避障，落到地面后调用 KeyStartAutoLanding 进行停桨。

低空时关闭下视避障

var downwardObstacleDisabled = false //确保关闭下视避障操作只成功执行一次
// 高度<5m 时关闭下视避障
if (currentAltitude <= 5.0 && !downwardObstacleDisabled) {
    downwardObstacleDisabled = true
    setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
}

//关闭下视避障调用方法
private fun setObstacleAvoidanceEnable(enabled: Boolean, direction: PerceptionDirection) {
    if (direction == null) {
        Log.e("Perception", "方向参数为空，无法设置避障")
        return
    }
    PerceptionManager.getInstance().setObstacleAvoidanceEnabled(
        enabled,
        direction,
        object : CommonCallbacks.CompletionCallback {
            override fun onSuccess() {
                toastResult?.postValue(DJIToastResult.success("成功设置【${direction.name}】方向的避障为：${if(enabled)"开启"else"关闭"}"))
                Log.i("Perception", "成功设置【${direction.name}】方向的避障为：${if(enabled)"开启"else"关闭"}")
            }
            override fun onFailure(error: IDJIError) {
                downwardObstacleDisabled = false
                toastResult?.postValue(DJIToastResult.failed("设置【${direction.name}】方向的避障失败：$error"))
                Log.e("Perception", "设置【${direction.name}】方向的避障失败：$error")
            }
        }
    )
}

第五步：降落循环完整逻辑

private fun startDynamicAdjustment() {
    isAdjusting = true
    virtualStickHandler = Handler(Looper.getMainLooper())
    val adjustTask = object : Runnable {
        override fun run() {
            if (!isAdjusting) return
            // 1. 获取当前状态
            val currentAltitude = FlightControllerKey.KeyAltitude.create().get(0.0)
            val currentXOffsetAbs = Math.abs(xOffset)
            val currentYOffsetAbs = Math.abs(yOffset)
            // 2. 检查是否着陆
            if (currentAltitude <= 0.1) {
                stopLanding()
                return
            }
            // 3. 低空时关闭下视避障
            if (currentAltitude <= 5.0 && !downwardObstacleDisabled) {
                downwardObstacleDisabled = true
                setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
            }
            // 4. 计算自适应参数
            val offsetThreshold = getOffsetThreshold(currentAltitude)
            val descentSpeed = getDescentSpeed(currentAltitude, currentXOffsetAbs, currentYOffsetAbs)
            // 5. 构建虚拟摇杆指令
            val adjustParam = VirtualStickFlightControlParam().apply {
                rollPitchCoordinateSystem = FlightCoordinateSystem.BODY
                verticalControlMode = VerticalControlMode.VELOCITY
                rollPitchControlMode = RollPitchControlMode.VELOCITY
                // 水平调整
                roll = if (currentXOffsetAbs > offsetThreshold) {
                    if (xOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
                } else 0.0
                pitch = if (currentYOffsetAbs > offsetThreshold) {
                    if (yOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
                } else 0.0
                // 垂直下降
                verticalThrottle = -descentSpeed
            }
            // 6. 发送指令
            VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(adjustParam)
            // 7. 100ms 后再次执行 (10Hz)
            virtualStickHandler?.postDelayed(this, 100)
        }
    }
    virtualStickHandler?.post(adjustTask)
}

以上，就实现了一整套视觉引导的自适应降落方案

安全注意事项

WARNING

1. 必须在空旷、安全环境测试
2. 建议先用 DJI 模拟器测试
3. 视觉识别必须持续更新 (~1Hz)
4. 准备好随时手动接管

代码示例

/**
 * One-key return to vehicle function (using Virtual Stick instead of FlyTo)
 * 1. Get aircraft current location
 * 2. Calculate distance to vehicle using Haversine formula
 * 3. If distance > 500m, reject with error
 * 4. Use Virtual Stick to navigate to vehicle location
 * 5. Switch to precision adjustment when close enough
 */
fun returnToVehicle(callback: CommonCallbacks.CompletionCallback) {
    // Get aircraft current location
    val aircraftLocation = getAircraftLocation()
    if (aircraftLocation == null || !isLocationValid(aircraftLocation.latitude, aircraftLocation.longitude)) {
        callback.onFailure(DJICommonError.FACTORY.build("无法获取无人机位置信息"))
        return
    }
    // Vehicle coordinates (hardcoded for now, will be replaced with API later)
    // TODO: Replace with actual vehicle GPS coordinates from API
    val vehicleLatitude = 22.579 // Example coordinates
    val vehicleLongitude = 113.941 // Example coordinates
    // Calculate distance using Haversine formula
    val distance = calculateDistance(
        aircraftLocation.latitude,
        aircraftLocation.longitude,
        vehicleLatitude,
        vehicleLongitude
    )
    // Distance validation: reject if > 500m
    if (distance > 500) {
        callback.onFailure(DJICommonError.FACTORY.build("距离过远：${String.format("%.2f", distance)}m, 超出 500m 限制"))
        return
    }
    // Start virtual stick navigation to vehicle location
    toastResult?.postValue(DJIToastResult.success("开始飞向车辆位置"))
    //TODO 这个 targetAlt 需要后期经过计算算出来。
    navigateToTarget(vehicleLatitude, vehicleLongitude, 100.0, callback)
}

/**
 * Navigate to target location using Virtual Stick
 */
private fun navigateToTarget(
    targetLat: Double,
    targetLon: Double,
    targetAlt: Double,
    callback: CommonCallbacks.CompletionCallback
) {
    VirtualStickManager.getInstance().enableVirtualStick(object : CommonCallbacks.CompletionCallback {
        override fun onSuccess() {
            VirtualStickManager.getInstance().setVirtualStickAdvancedModeEnabled(true)
            isNavigating = true
            startNavigation(targetLat, targetLon, targetAlt, callback)
        }
        override fun onFailure(error: IDJIError) {
            callback.onFailure(error)
        }
    })
}

/**
 * Start navigation loop using Virtual Stick
 */
private fun startNavigation(
    targetLat: Double,
    targetLon: Double,
    targetAlt: Double,
    callback: CommonCallbacks.CompletionCallback
) {
    virtualStickHandler = Handler(Looper.getMainLooper())
    val navTask = object : Runnable {
        override fun run() {
            if (!isNavigating) return
            val currentLoc = getAircraftLocation()
            if (currentLoc == null) {
                virtualStickHandler?.postDelayed(this, 100)
                return
            }
            // Calculate remaining distance
            val remainingDistance = calculateDistance(
                currentLoc.latitude,
                currentLoc.longitude,
                targetLat,
                targetLon
            )
            println("targetLat:" + targetLat + " targetLon:" + targetLon + " currentLoc.latitude:" + currentLoc.latitude + " currentLoc.longitude:" + currentLoc.longitude + " remainingDistance:" + remainingDistance)
            // Check if arrived
            if (remainingDistance < ARRIVAL_THRESHOLD) {
                // Arrived at target, stop navigation
                isNavigating = false
                virtualStickHandler?.removeCallbacksAndMessages(null)
                callback.onSuccess()
                toastResult?.postValue(DJIToastResult.success("已到达车辆位置，开始精确定位"))
                //开始调节云台角度，俯仰角为 -90°，旋转时间 1s
                startGimbalAngleRotation(GimbalAngleRotationMode.ABSOLUTE_ANGLE, -90.0, 0.0, 0.0, 1.0)
                // Start precision adjustment
                startDynamicAdjustment()
            } else {
                // Continue navigation
                val bearing = calculateBearing(
                    currentLoc.latitude,
                    currentLoc.longitude,
                    targetLat,
                    targetLon
                )
                val navParam = VirtualStickFlightControlParam().apply {
                    rollPitchCoordinateSystem = FlightCoordinateSystem.GROUND // Use ground coordinate system
                    verticalControlMode = VerticalControlMode.POSITION
                    yawControlMode = YawControlMode.ANGLE
                    rollPitchControlMode = RollPitchControlMode.VELOCITY
                    // Calculate velocity components based on bearing
                    val bearingRad = Math.toRadians(bearing)
                    pitch = NAVIGATION_SPEED * Math.sin(bearingRad) // North-South component
                    roll = NAVIGATION_SPEED * Math.cos(bearingRad) // East-West component
                    yaw = bearing // Point towards target
                    verticalThrottle = targetAlt // Target altitude
                }
                VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(navParam)
                virtualStickHandler?.postDelayed(this, 100)
            }
        }
    }
    virtualStickHandler?.post(navTask)
}

fun startGimbalAngleRotation(
    mode: GimbalAngleRotationMode,
    pitch: Double,
    yaw: Double,
    roll: Double,
    duration: Double
) {
    val rotation = GimbalAngleRotation().apply {
        setMode(mode)
        setPitch(pitch)
        setYaw(yaw)
        setRoll(roll)
        setDuration(duration)
    }
    KeyManager.getInstance().performAction(
        KeyTools.createKey(GimbalKey.KeyRotateByAngle),
        rotation,
        object : CommonCallbacks.CompletionCallbackWithParam<EmptyMsg> {
            override fun onSuccess(result: EmptyMsg?) {
                toastResult?.postValue(DJIToastResult.success("云台旋转成功"))
                Log.i("Gimbal", "云台旋转成功：yaw:${rotation.yaw},pitch:${rotation.pitch},roll:${rotation.roll}")
            }
            override fun onFailure(error: IDJIError) {
                toastResult?.postValue(DJIToastResult.failed("云台旋转失败,$error"))
                Log.e("Gimbal", "云台旋转失败，$error")
            }
        }
    )
}

/**
 * Start dynamic position adjustment loop with adaptive descent
 * Adjusts position while descending, with stricter requirements at lower altitudes
 */
private fun startDynamicAdjustment() {
    isAdjusting = true
    virtualStickHandler = Handler(Looper.getMainLooper())
    // Send adjustment commands at 10Hz
    val adjustTask = object : Runnable {
        override fun run() {
            if (!isAdjusting) return
            // TODO 获取脚本检测出的 z 轴距离
            val currentAltitude = FlightControllerKey.KeyAltitude.create().get(0.0)
            val currentXOffsetAbs = Math.abs(xOffset)
            val currentYOffsetAbs = Math.abs(yOffset)
            //关闭降落保护，下视避障失效
            if (currentAltitude <= 5 && !downwardObstacleDisabled) {
                downwardObstacleDisabled = true
                setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
            }
            // 检查是否落地
            if (currentAltitude <= 0.1) {
                stopLanding()
                return
            }
            // Get adaptive thresholds based on altitude
            val offsetThreshold = getOffsetThreshold(currentAltitude)
            val descentSpeed = getDescentSpeed(currentAltitude, currentXOffsetAbs, currentYOffsetAbs)
            // Log for debugging
            println("自动调整 - 高度:%.2fm, x 偏移:%.2fm,y 偏移:%.2fm, 阈值:%.2fm, 下降速度:%.2fm/s".format(
                currentAltitude,
                currentXOffsetAbs,
                currentYOffsetAbs,
                offsetThreshold,
                descentSpeed
            ))
            // Calculate adjustment parameters
            val adjustParam = VirtualStickFlightControlParam().apply {
                rollPitchCoordinateSystem = FlightCoordinateSystem.BODY
                verticalControlMode = VerticalControlMode.VELOCITY
                yawControlMode = YawControlMode.ANGULAR_VELOCITY
                rollPitchControlMode = RollPitchControlMode.VELOCITY
                // Calculate roll value based on offset
                // Positive offset (need to move forward) -> positive roll
                // Negative offset (need to move backward) -> negative roll
                if (currentXOffsetAbs > offsetThreshold) {
                    // Need adjustment
                    roll = if (xOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
                } else {
                    // Within threshold, no adjustment needed
                    roll = 0.0
                }
                if (currentYOffsetAbs > offsetThreshold) {
                    pitch = if (yOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
                } else {
                    pitch = 0.0
                }
                yaw = 0.0
                verticalThrottle = -descentSpeed // Descend at adaptive speed
            }
            VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(adjustParam)
            virtualStickHandler?.postDelayed(this, 100)
        }
    }
    virtualStickHandler?.post(adjustTask)
    toastResult?.postValue(DJIToastResult.success("开始动态位置调整"))
}

/**
 * Stop landing and cleanup
 */
private fun stopLanding() {
    virtualStickHandler?.removeCallbacksAndMessages(null)
    //调用 KeyStartAutoLanding 进行停桨
    FlightControllerKey.KeyStartAutoLanding.create().action(
        {
            toastResult?.postValue(DJIToastResult.success("桨叶动力关闭"))
            Log.i("stopLanding", "桨叶动力关闭成功")
        },
        {
            toastResult?.postValue(DJIToastResult.failed("桨叶动力关闭失败"))
            Log.i("stopLanding", "桨叶动力关闭失败！！")
        }
    )
    cleanupVirtualStick()
    toastResult?.postValue(DJIToastResult.success("降落完成"))
}

/**
 * Get offset threshold based on current altitude
 * Higher altitude allows larger offset, lower altitude requires stricter precision
 */
private fun getOffsetThreshold(altitude: Double): Double {
    return when {
        altitude > HIGH_ALTITUDE -> 1.0 // High altitude: allow 1m offset
        altitude > MID_ALTITUDE -> 0.5 // Mid altitude: allow 0.5m offset
        altitude > LOW_ALTITUDE -> 0.4 // Low altitude: allow 0.3m offset
        else -> 0.3 // Very low altitude: require 0.2m precision
    }
}

/**
 * Get descent speed based on current altitude and offset
 * Larger offset or lower altitude results in slower descent
 */
private fun getDescentSpeed(altitude: Double, xOffset: Double, yOffset: Double): Double {
    val threshold = getOffsetThreshold(altitude)
    return when {
        xOffset > threshold * 2 || yOffset > threshold * 2 -> 0.0 // Offset too large: stop descending
        xOffset > threshold || yOffset > threshold -> 0.1 // Offset large: slow descent
        altitude > MID_ALTITUDE -> 0.5 // Mid-high altitude: fast descent
        altitude > LOW_ALTITUDE -> 0.2 // Low altitude: slow descent
        else -> 0.1 // Very low altitude: very slow descent
    }
}