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

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

概述

最初需求：想要无人机在执行完航线任务后，一键落到一个指定的位置，简化人工控制。

实现一套完整的无人机自主降落功能,通过虚拟摇杆控制使无人机飞向指定位置，再利用视觉识别引导无人机精确降落到具体位置。本文中采用自适应降落策略,根据高度动态调整精度要求和下降速度,以实现安全、精确的降落。

核心点:

  • 虚拟摇杆导航替代FlyTo功能
  • 双轴(X/Y)位置偏移实时调整
  • 高度自适应降落策略
  • 视觉识别引导定位
  • 智能避障管理

系统架构

整体流程

graph TD A[用户触发Return to Vehicle] --> B[获取无人机GPS位置] B --> C[计算与目标点距离] C --> D[启动虚拟摇杆导航] D --> E[飞向目标位置 5m/s] E --> F{距离小于10m?} F -->|否| E F -->|是| G[开始自适应降落] G --> H[视觉识别系统] H --> I[计算X/Y偏移量] I --> J[更新偏移量到ViewModel] J --> K[自适应降落循环] K --> L{高度分段判断} L -->|高于50m| M[高空模式] L -->|20-50m| N[中空模式] L -->|5-20m| O[低空模式] L -->|低于5m| P[极低空模式] M --> Q[计算调整速度和下降速度] N --> Q O --> Q P --> Q Q --> R{偏移大于阈值2倍?} R -->|是| S[停止下降只调整] R -->|否| T[边调整边下降] S --> U{高度小于5m?} T --> U U -->|是| V[关闭下视避障] U -->|否| K V --> W{高度小于等于0.1m?} W -->|否| K W -->|是| X[着陆完成清理资源]

技术实现思路

第一步:让无人机飞到目标位置?

问题分析

遥控器控制的无人机在执行完航线任务之后，飞到给定降落点（汽车或其他载具上）。最初的想法是使用DJI SDK提供的FlyTo功能,直接指定目标GPS坐标让无人机飞过去。但在实际测试中，发现部分机型（如M3E）并不支持FlyTo功能。

机型是否支持FlyTo功能参考文档：https://developer.dji.com/doc/mobile-sdk-tutorial/cn/tutorials/intelligent-flight.html

解决方案:虚拟摇杆导航

既然FlyTo功能不可用,那就用虚拟摇杆功能进行模拟。

思路:

  1. 计算当前位置到目标位置的方位角(bearing)
  2. 将方位角转换为速度分量(南北/东西)
  3. 持续发送虚拟摇杆指令,让无人机朝目标飞行
  4. 实时监测距离,接近目标时停止

方位角计算:

private fun calculateBearing(latA: Double, lonA: Double, latB: Double, lonB: Double): Double {
    val lat1 = Math.toRadians(latA)
    val lat2 = Math.toRadians(latB)
    val dLon = Math.toRadians(lonB - lonA)
    
    val y = Math.sin(dLon) * Math.cos(lat2)
    val x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(dLon)
    
    var bearing = Math.toDegrees(Math.atan2(y, x))
    bearing = (bearing + 360) % 360  // 归一化到0-360度
    
    return bearing  // 0°=正北, 90°=正东, 180°=正南, 270°=正西
}

速度分量计算:

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控制东西。

虚拟摇杆参数含义：https://developer.dji.com/doc/mobile-sdk-tutorial/cn/basic-introduction/basic-concepts/flight-controller.html#虚拟摇杆

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

持续监测距离

每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                                                // 极低空:极慢降
    }
}

控制逻辑:

graph TD A[获取当前高度和偏移量] --> B{高度判断} B -->|大于50m| C[偏离阈值1m] B -->|20-50m| D[偏离阈值0.5m] B -->|5-20m| E[偏离阈值0.3m] B -->|小于5m| F[偏离阈值0.2m] C --> G{偏移判断} D --> G E --> G F --> G G -->|偏移大于阈值的2倍| H[停止下降,只调整] G -->|偏移大于阈值| I[慢降0.1m/s并且调整] G -->|偏移小于阈值| J[快降并且微调] H --> K[发送虚拟摇杆指令] I --> K J --> K K --> L{高度小于等于0.1m?} L -->|否| A L -->|是| M[着陆完成]

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

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

无人机的下视避障系统会将地面识别为障碍物,在接近地面时自动停止下降，我们在高度为5m的时候关闭下视避障，落到地面后调用KeyStartAutoLanding进行停桨。
参考文档： https://sdk-forum.dji.net/hc/zh-cn/articles/14578693771033-如何使用虚拟摇杆降落

低空时关闭下视避障

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(    //调用大疆MSDK方法关闭下视避障
            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.ANGLE
                    
                    // 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
        }
    }
posted @ 2025-12-31 15:24  深紫色的三北六号  阅读(89)  评论(0)    收藏  举报