详解APM的开方控制器sqrt_controller

Ardupilot姿态控制详解(完结篇) 本文主要是对过去写的一些APM中姿态控制流程及函数博文的汇总。 按照之前写的博文以及个人认为的学习顺序,给出APM姿态控制器的学习顺序如下: APM姿态旋转理论基础 Ardupilot姿态控制器 PID控制流程(整定后的P控制器) 详解APM的开方控制器sqrt_controller Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析 Ardupilot倾转分离函数thrust_heading_rotation_angles Ardupilot四元数姿态控制 阅读详情
AI摘要

本文深入解析了开方控制器(sqrt_controller)的工作原理及其在飞行控制器中的应用。详细介绍了如何通过控制P项,实现误差响应的平滑过渡,并利用MATLAB绘制了控制器特性曲线。

前言

前面说过,sqrt_controller是对P项进行整定用途的,目的就是让P项的控制响应“软”下来,实际上就是一个经过改进的P控制器。读懂了sqrt_controller,那么你对APM的P控制器就了解得差不多了。

仅讨论P控制器,sqrt_controller的作用就是设定一个量linear_dist,以这个量的正负值为分界线划分出一个线性区域范围,在这个范围内表示误差量还很小,遵循Kp*error计算方式输出控制量。

超过linear_dist后,表明误差较大,此时不能由着误差单纯按照线性方式乘以Kp输出控制量,需要按照开方形式,使用另外的方式计算出实际控制量,此处后面补充。


在开始讲解源码之前,先来补充个前提:

了解飞控的应该都清楚,一般是使用串级PID来进行姿态等的控制。在APM中,外环角度环就是接收期望与实际的角度误差,然后输入到P控制器中得到期望的角速率。也就是说,这个控制过程可以表示为:
w = K p ∗ e r r o r θ w=K_p*error_θ w=Kp​∗errorθ​

而我们同样知道,假定小角度内速率恒定,那么可知 w = e r r o r θ / d t w=error_θ/dt w=errorθ​/dt,而由此可直接定义
K p = 1 / d t Kp=1/dt Kp=1/dt

而实际上APM中广泛采用了这个方式,因此,后续过程中我们谈论到Kp表明的都是时间的倒数。

 

源码阅读

数学公式的原理放在后面讲解,这边先放源码

// Proportional controller with piecewise sqrt sections to constrain second derivative
// 翻译:具有分段sqrt截面以约束二阶导数的比例控制器
// error:输入参数,误差量
// p:输入参数,Kp值
// second_ord_lim:输入参数,二阶限制,对Kp进行二次限制,通常是加速度最大值输入
// dt:输入参数,时间
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim, float dt)
{
   
   
    float correction_rate;	// 最后需要返回的期望速率
    if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
   
   
        // 二阶限制是否为负或者零
		// 是的话表明开方控制器不起作用,按照正常方式计算Kp*error
        correction_rate = error * p;
    } else if (is_zero(p)) {
   
   
        // Kp=0但是我们开启了二阶限制则采用如下计算方式
        if (is_positive(error)) {
   
   
            correction_rate = safe_sqrt(2.0f * second_ord_lim * (error));
        } else if (is_negative(error)) {
   
   
            correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error));
        } else {
   
   
            correction_rate = 0.0f;
        }
    } else {
   
   
        // 如果同时定义了Kp和二阶限制
		// 一般我们用了sqrt_controller都是进的这个函数
		// 计算获取分界点linear_dist
		// 根据error与linear_dist的相对大小采用不同公式
        float linear_dist = second_ord_lim / sq(p);
        if (error > linear_dist) {
   
   
            correction_rate = safe_sqrt(2.0f * second_ord_lim * (error - (linear_dist / 2.0f)));
        } else if (error < -linear_dist) {
   
   
            correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error - (linear_dist / 2.0f)))
手把手推导APM的 sqrt_controller控制器 前言 玩过无人机的朋友对APM应该都不会陌生,如果是搞过多旋翼飞控且读过代码,肯定遇到过sqrt_controller控制器,它频繁的在位置、姿态控制中出现,所以非常有必要搞清楚这个控制原理。 下面以高度控制器为例来分析。 高度控制 APM的高度控制采用PID串级3环控制,即外环位置控制,中间速度控制,内环速度控制,如下图所示。 今天要讨论的sqrt_controller就是应用于位置控制器中。位置控制器的输出是速度控制器的参考reference输入,如果采用单P控制,其输出vel_ref = pos_e 阅读详情

相关推荐

APM飞控中的sqrt_controller:如何用分段开方优化PID响应(附MATLAB仿真代码)

本文深入解析了APM飞控中sqrt_controller分段开方控制器的设计原理与应用。该控制器通过动态切换线性与开方控制律,在保证小误差响应精度的同时,利用最大加速度约束平滑大误差响应,有效解决了传统P控制器在大指令下的抖动与超调问题。文中提供了详细的算法推导、代码解析及MATLAB仿真代码,助力开发者优化无人机姿态控制。

Jerry的专栏 802

ArduPilot开方控制器sqrt_controller源码解析与调参实战

飞控系统中,位置环和姿态环的控制逻辑决定了飞行器的稳定与精准。纯比例控制在误差较大时容易产生超调,而误差较小时又难以兼顾灵敏度。开方控制器通过平方根映射实现“大误差温和、小误差精准”的非线性响应,并模拟了恒定减速度刹停的物理过程。ArduPilot(APM飞控固件)中的sqrt_controller正是这一思想的核心实现,广泛用于位置误差转期望速度、角度误差转期望角速度等关键链路。本文结合源码逐段拆解其线性区、平方根区及安全锁设计,并深入探讨PSC_POSXY_P、ATC_ANG_RLL_P等参数与加速度限

weixin_32349093的博客 320

APM飞控中的sqrt_controller:如何用分段开方函数优化PID响应(附MATLAB仿真)

本文深入解析了APM飞控中的sqrt_controller分段开方函数如何优化PID响应。通过MATLAB仿真,展示了该控制器在小误差时保持线性快速响应,大误差时通过非线性衰减避免超调的设计优势,有效提升无人机飞控的稳定性和机动性。

weixin_29034963的博客 170

ArduPilot sqrt_controller原理与调参:从刹车距离到源码实战

无人机飞控中的位置控制常面临大误差响应慢、小误差易超调的矛盾。纯比例控制受限于固定增益,PD控制又对噪声敏感。sqrt_controller基于运动学中的刹车距离公式,通过平方根关系将位置误差映射为速度指令,在物理加速度约束下实现近似时间最优的速度规划。它既保证了大幅偏差下快速接近目标,又在近端通过线性段平滑过渡,避免误差零点附近的控制抖动。该控制器广泛用于ArduPilot的姿态环与位置环,是GPS定点、自主飞行稳定性的关键。理解其数学推导、源码实现及rate_p、smax、hz_dist三个参数的配合,

weixin_29194817的博客 240

无人机飞控系统实战解析:IMU融合、GPS误差与串级PID调参

飞控系统是无人机实现稳定悬停与精准导航的核心实时闭环控制器,其本质并非传感器数据采集终端,而是基于IMU高频动态感知与GPS低频全局校正的多源融合决策中枢。理解加速度计重力对齐原理、七层嵌套式GPS误差(含多径、电离层、PDOP)及其对定位可信度的影响,是解决‘yaw慢漂’‘姿态发散’‘GPS跳变’等典型故障的前提。技术价值在于通过EKF2卡尔曼滤波实现时空对齐,支撑串级PID控制架构——内环500Hz保障响应带宽,外环50Hz抑制噪声误判。广泛应用于航拍测绘、电力巡检、农业植保等场景,本文聚焦F4飞控平台

weixin_34203832的博客 561

ASP.NET Core性能优化全链路实践:从代码到架构的高并发调优指南

在构建高并发Web应用时,性能优化是确保系统稳定性和用户体验的核心技术。其原理在于通过减少资源消耗、提升处理效率来优化吞吐量和延迟,技术价值体现在支撑更高并发访问、降低服务器成本并提升业务响应能力。应用场景广泛覆盖电商秒杀、实时通信、大数据处理等高负载场景。本文聚焦ASP.NET Core框架,深入探讨异步编程、内存管理等关键热词,通过代码级优化、GC调优和缓存策略等工程实践,系统解决高并发下的性能瓶颈问题。

weixin_34221332的博客 537

apm姿态模式详解

详细介绍apm飞控飞行姿态的几种模式,以及在对应模式下需要注意的事项

不止于算力:深入鲲鹏920的片上网络(NoC)与一致性协议HCCS,看华为如何优化数据流转

本文深入解析了鲲鹏920处理器的片上网络(NoC)架构和一致性协议HCCS,揭示了华为如何通过创新的环总线设计和私有协议优化数据流转效率。文章详细介绍了鲲鹏920在多die、多芯片环境下的高效协同机制,包括环总线架构、HCCS协议精妙设计及物理实现创新,为云服务场景提供了高性能解决方案。

weixin_30681121的博客 254

用Origin快速拟合荧光寿命、PL Decay (TRPL)数据分析处理

用Origin快速拟合荧光寿命、PL Decay (TRPL)数据分析处理

A20802013的博客 2万+

避坑指南:Windows下OpenCV摄像头索引混乱问题的3种解决之道

本文详细解析了Windows下OpenCV摄像头索引混乱问题的3种解决方案,包括GUID查询法、DirectShow接口集成和注册表固化技术。针对工业视觉和智能监控场景,提供实战代码和性能优化建议,帮助开发者稳定识别多路摄像头设备,有效解决VideoCapture接口的索引漂移问题。

weixin_30836759的博客 407

体素坐标(voxel_coors)在mmdetection3d中的理解

体素坐标(voxel_coors)在mmdetection3d中的理解

weixin_38362784的博客 4219

逆向解析M团外卖mtgsig与waimai_sign加密参数实战指南⊙ω⊙

本文详细解析了M团外卖App中mtgsig与waimai_sign两个核心加密参数的逆向分析与生成方法。通过抓包定位、静态逆向与Frida动态调试,拆解了waimai_sign的RSA签名逻辑与mtgsig的复杂参数组装加密流程,并提供了Python模拟实现的框架与调试心得,为安全研究及合规数据分析提供实战指南。

weixin_29327525的博客 1286

OpenClaw实战系列·进阶特辑02:多智能体协同——打造企业级AI中台

**企业级AI中台面临多Agent协同的挑战,本文提出基于多智能体协同的解决方案。通过分析单Agent的局限性(上下文溢出、专业度稀释等)和多Agent的核心优势(任务专注度、故障隔离等),提出三种协作架构模式(指挥型、联邦型、点对点型),并强调人格化设计的重要性。文章详细介绍Agent间通讯机制(同步/异步会话、文件系统协作)和"身脑分离"的解耦架构设计,提供可落地的企业级AI中台实施方案。

PeterPan的博客 495

高通平台开发系列讲解(USB篇)MBIM协议详解

📢本文主要介绍高通平台USB网卡MBIM协议。

内核笔记 5441

YOLOv11涨点改进 | Conv篇/小目标改进 | YOLOv11引入SPDConv卷积特征无损下采样模块,增加YOLOv11小目标P2检测层(助力小目标任务高效涨点)

本文提出了一种创新的SPDConv模块来优化YOLOv11模型,通过空间到深度(SPD)层和非步幅卷积层实现特征无损下采样。该方法有效解决了传统CNN中因跨步卷积和池化层导致的细粒度信息丢失问题,特别提升了小目标检测性能。实验表明,SPDConv在目标检测和图像分类任务中显著优于现有SOTA模型。文章详细介绍了模块原理、核心代码实现,并提供了YOLOv11集成SPDConv的具体方法,包括配置文件修改和P2层增强等优化技巧。该技术为计算机视觉任务提供了一种高效的特征保留方案。

Ai缝合怪、助力小伙伴高效发论文 592

迈瑞BS系列全自动生化分析仪LIS协议接口手册(内含HL7和ASTM)

迈瑞BS系列全自动生化分析仪LIS协议接口手册,内含HL7和ASTM)

最新的sdio 4.0 spec 协议标准 带书签 高清完整PDF版

Part-E1-SDIO-Specification-Ver4.00-Final-120220.pdf:最新的sdio 4.0 spec 协议标准 带书签 高清完整PDF版,包括SDIO 接口的外设,SDIO 接口官方协议等。

win10 跳过U8环境检测的IE WebControls.rar

win10 安装u8的时候 提示IE WebControls 不通过,这个工具可以欺骗u8的检查。 跳过U8环境检测的IE WebControls。提示导入失败后,重新检测通过。

SAP B1中文培訓教材_SDK

公司内部培训资料,对开发工具 ,DIAPI, UIAPI,Add-ON有详细解说

上一篇: RoboWare Studio安装及ROS支持的IDE
下一篇: Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析
moumde
博客等级 码龄10年 454粉丝 40原创
评论 3
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值