如何对小型固定翼无人机进行最优的路径跟随控制?

news2025/4/5 13:08:40

控制架构

文章继续采用的是 ULTRA-Extra无人机,相关参数如下:

在这里插入图片描述

这里用于guidance law的无人机运动学模型为:
{ x ˙ p = V a cos ⁡ γ cos ⁡ χ + V w cos ⁡ γ w cos ⁡ χ w y ˙ p = V a cos ⁡ γ sin ⁡ χ + V w cos ⁡ γ w sin ⁡ χ w z ˙ p = V a sin ⁡ γ + V w sin ⁡ γ w χ ˙ = g tan ⁡ ϕ / V a γ ˙ = g ( n z cos ⁡ ϕ − cos ⁡ γ ) / V a \begin{cases} \dot{x}_p = V_a\cos\gamma\cos\chi + V_w\cos\gamma_w\cos\chi_w \\ \dot{y}_p = V_a\cos\gamma\sin\chi + V_w\cos\gamma_w\sin\chi_w \\ \dot{z}_p = V_a\sin\gamma + V_w\sin\gamma_w \\ \dot{\chi} = g\tan\phi/V_a \\ \dot{\gamma} = g(n_z\cos\phi-\cos\gamma)/V_a \end{cases} x˙p=Vacosγcosχ+Vwcosγwcosχwy˙p=Vacosγsinχ+Vwcosγwsinχwz˙p=Vasinγ+Vwsinγwχ˙=gtanϕ/Vaγ˙=g(nzcosϕcosγ)/Va
其中状态量为 ( x p , y p , z p , γ , χ ) (x_p,y_p,z_p,\gamma,\chi) (xp,yp,zp,γ,χ),控制量为 ( V a , n z , ϕ ) (V_a,n_z,\phi) (Va,nz,ϕ)。在自动驾驶仪(Autopilot)中,采用 Successive-Loop-Closure (SLC)实现参考量 ( V a m , n z m , ϕ m ) (V_{a_m},n_{z_m},\phi_m) (Vam,nzm,ϕm)的信号跟踪:

在这里插入图片描述

自动驾驶仪中依然采用横纵向通道的SLC实现控制,相应的控制逻辑如下:

在这里插入图片描述

Path Following 最优控制器

对运动学模型进行二阶求导可以得到:
( x ˙ p y ˙ p z ˙ p χ ˙ γ ˙ x ¨ p y ¨ p z ¨ p χ ¨ γ ¨ V ˙ a ϕ ˙ n ˙ z ) = ( O 5 × 5 I 5 O 5 × 3 − V a cos ⁡ γ sin ⁡ χ − V a sin ⁡ γ cos ⁡ χ V a cos ⁡ γ cos ⁡ χ − V a sin ⁡ γ sin ⁡ χ O 5 × 5 O 5 × 3 0 V a cos ⁡ γ O 5 × 3 0 0 0 g sin ⁡ γ V a O 3 × 13 ) ( x p y p z p χ γ x ˙ p y ˙ p z ˙ p χ ˙ γ ˙ V a ϕ n z ) + ( O 5 × 3 cos ⁡ γ cos ⁡ χ 0 0 cos ⁡ γ sin ⁡ χ 0 0 sin ⁡ γ 0 0 − g tan ⁡ ϕ V a 2 g V a cos ⁡ 2 ϕ 0 g ( cos ⁡ γ − n z cos ⁡ ϕ ) V a 2 − g n z sin ⁡ ϕ V a g cos ⁡ ϕ V a I 3 ) ( V ˙ a ϕ ˙ n ˙ z ) \left( \begin{matrix} {{{\dot{x}}}_{p}} \\ {{{\dot{y}}}_{p}} \\ {{{\dot{z}}}_{p}} \\ {\dot{\chi }} \\ {\dot{\gamma }} \\ {{{\ddot{x}}}_{p}} \\ {{{\ddot{y}}}_{p}} \\ {{{\ddot{z}}}_{p}} \\ {\ddot{\chi }} \\ {\ddot{\gamma }} \\ \dot{V}_a\\ \dot{\phi} \\ \dot{n}_z\\ \end{matrix} \right)=\left( \begin{matrix} {{O}_{5\times 5}} & {} & {{I}_{5}} & {} & O_{5\times 3} \\ {} & {} & -{{V}_{a}}\cos \gamma \sin \chi & -{{V}_{a}}\sin \gamma \cos \chi \\ {} & {} & {{V}_{a}}\cos \gamma \cos \chi & -{{V}_{a}}\sin \gamma \sin \chi \\ {{O}_{5\times 5}} & {{O}_{5\times 3}} & 0 & {{V}_{a}}\cos \gamma & O_{5\times 3}\\ {} & {} & 0 & 0 \\ {} & {} & 0 & \frac{g\sin \gamma }{V_{a}^{{}}} \\ {} & {} & {} & O_{3 \times 13} \end{matrix} \right)\left( \begin{matrix} {{x}_{p}} \\ {{y}_{p}} \\ {{z}_{p}} \\ \chi \\ \gamma \\ {{{\dot{x}}}_{p}} \\ {{{\dot{y}}}_{p}} \\ {{{\dot{z}}}_{p}} \\ {\dot{\chi }} \\ {\dot{\gamma }} \\ V_a\\ \phi \\n_z \end{matrix} \right)+\left( \begin{matrix} {} & {{O}_{5\times 3}} & {} \\ \cos \gamma \cos \chi & 0 & 0 \\ \cos \gamma \sin \chi & 0 & 0 \\ \sin \gamma & 0 & 0 \\ -\frac{g\tan \phi }{V_{a}^{2}} & \frac{g}{{{V}_{a}}{{\cos }^{2}}\phi } & 0 \\ \frac{g(\cos \gamma -{{n}_{z}}\cos \phi )}{V_{a}^{2}} & -\frac{g{{n}_{z}}\sin \phi }{V_{a}^{{}}} & \frac{g\cos \phi }{V_{a}^{{}}} \\ & I_{3} &\\ \end{matrix} \right)\left( \begin{align} & {{{\dot{V}}}_{a}} \\ & {\dot{\phi }} \\ & {{{\dot{n}}}_{z}} \\ \end{align} \right) x˙py˙pz˙pχ˙γ˙x¨py¨pz¨pχ¨γ¨V˙aϕ˙n˙z = O5×5O5×5O5×3I5VacosγsinχVacosγcosχ000VasinγcosχVasinγsinχVacosγ0VagsinγO3×13O5×3O5×3 xpypzpχγx˙py˙pz˙pχ˙γ˙Vaϕnz + cosγcosχcosγsinχsinγVa2gtanϕVa2g(cosγnzcosϕ)O5×3000Vacos2ϕgVagnzsinϕI30000Vagcosϕ V˙aϕ˙n˙z
这里设 ρ = ( γ , χ , V a , ϕ , n z ) T \rho=(\gamma,\chi,V_a,\phi,n_z)^T ρ=(γ,χ,Va,ϕ,nz)T x = ( x p , y p , z p , χ , γ , x ˙ p , y ˙ p , z ˙ p , χ ˙ , γ ˙ , V a , ϕ , n z ) T x=(x_p,y_p,z_p,\chi,\gamma,\dot{x}_p,\dot{y}_p,\dot{z}_p,\dot{\chi},\dot{\gamma},V_a,\phi,n_z)^T x=(xp,yp,zp,χ,γ,x˙p,y˙p,z˙p,χ˙,γ˙,Va,ϕ,nz)T u = ( V ˙ a , ϕ ˙ , n ˙ z ) T u=(\dot{V}_a,\dot{\phi},\dot{n}_z)^T u=(V˙a,ϕ˙,n˙z)T,得到:
x ˙ = A v ( ρ ) x + B v ( ρ ) u \dot{x}=A_v(\rho)x+B_v(\rho)u x˙=Av(ρ)x+Bv(ρ)u

假设要跟踪的量为 r = ( x r , y r , z r ) T r=(x_r,y_r,z_r)^T r=(xr,yr,zr)T,构造跟踪向量 e = ( x r − x p , y r − y p , z r − z p ) T = r − ( x p , y p , z p ) T e=(x_r-x_p,y_r-y_p,z_r-z_p)^T=r-(x_p,y_p,z_p)^T e=(xrxp,yryp,zrzp)T=r(xp,yp,zp)T e ˙ = r ˙ − ( x ˙ p , y ˙ p , z ˙ p ) T = r ˙ − C x \dot{e} = \dot{r} - (\dot{x}_p,\dot{y}_p,\dot{z}_p)^T=\dot{r}-Cx e˙=r˙(x˙p,y˙p,z˙p)T=r˙Cx,有:
( x ˙ e ˙ ) = ( A v ( ρ ) O 13 × 3 − C O 3 × 3 ) ( x e ) + ( B v ( ρ ) O 3 × 3 ) u + ( O 13 × 1 r ˙ ) \begin{pmatrix} \dot{x} \\ \dot{e} \end{pmatrix} = \begin{pmatrix} A_v(\rho) &O_{13 \times 3} \\ -C & O_{3 \times 3} \end{pmatrix}\begin{pmatrix} x \\ e \end{pmatrix} +\begin{pmatrix} B_v(\rho)\\O_{3 \times 3} \end{pmatrix}u+\begin{pmatrix} O_{13\times 1} \\\dot{r} \end{pmatrix} (x˙e˙)=(Av(ρ)CO13×3O3×3)(xe)+(Bv(ρ)O3×3)u+(O13×1r˙)
上市被描述为:
x ˙ e = A e ( ρ ) x e + B e ( ρ ) u + c e \dot{x}_{e}=A_e(\rho)x_e + B_e(\rho)u + c_e x˙e=Ae(ρ)xe+Be(ρ)u+ce
其中,
C = ( O 3 × 5 ∣ I 3 ∣ O 3 × 5 ) C=\begin{pmatrix} O_{3\times 5} | I_3 |O_{3\times 5} \end{pmatrix} C=(O3×5I3O3×5)

利用4阶Runge-Kutta法可以将上式可以离散化为一个LPV状态空间方程(linear parameter varying state-space representation):
x e , k + 1 = A e ( ρ k ) x e , k + B e ( ρ k ) u e , k + c r , k x_{e,k+1} = A_e(\rho_k)x_{e,k}+B_e(\rho_k)u_{e,k}+c_{r,k} xe,k+1=Ae(ρk)xe,k+Be(ρk)ue,k+cr,k
其中, T s T_s Ts是采样时间,
A e ( ρ k ) = 1 24 A e ( ρ k ) 4 T s 4 + 1 6 A e 3 ( ρ k ) T s 3 + 1 2 A e ( ρ k ) 2 T s 2 + A e ( ρ k ) T s + I B e ( ρ k ) = 1 24 A e ( ρ k ) 3 B e ( ρ k ) T s 4 + 1 6 A e 2 ( ρ k ) B e ( ρ k ) T s 3 + 1 2 A e ( ρ k ) B e ( ρ k ) T s 2 + B e ( ρ k ) T s A_e(\rho_k)=\frac{1}{24}A_e(\rho_k)^4T_s^4+\frac{1}{6}A^3_e(\rho_k)T_s^3+\frac{1}{2}A_e(\rho_k)^2T_s^2+A_e(\rho_k)T_s+I \\ B_e(\rho_k)=\frac{1}{24}A_e(\rho_k)^3B_e(\rho_k)T_s^4+\frac{1}{6}A^2_e(\rho_k)B_e(\rho_k)T_s^3+\frac{1}{2}A_e(\rho_k)B_e(\rho_k)T_s^2+B_e(\rho_k)T_s Ae(ρk)=241Ae(ρk)4Ts4+61Ae3(ρk)Ts3+21Ae(ρk)2Ts2+Ae(ρk)Ts+IBe(ρk)=241Ae(ρk)3Be(ρk)Ts4+61Ae2(ρk)Be(ρk)Ts3+21Ae(ρk)Be(ρk)Ts2+Be(ρk)Ts

上述轨迹跟踪问题可以转化为:
min ⁡ u ( t ) J [ u ( t ) ] = ∫ t 0 t f 1 + x ( t ) T Q x ( t ) + u ( t ) T R u ( t ) d t x ˙ ( t ) = A v ( ρ ) x ( t ) + B v ( ρ ) u ( t ) x ( t 0 ) = x 0 , E x ( t f ) = ( x r , y r , z r ) T d min ⁡ ≤ D x ( t ) ≤ d max ⁡ \min_{u(t)}J[u(t)]=\int_{t_0}^{t_f}1+x(t)^TQx(t)+u(t)^TRu(t)dt \\ \dot{x}(t)=A_v(\rho)x(t) + B_v(\rho)u(t) \\ x(t_0)=x_0,Ex(t_f)=(x_r,y_r,z_r)^T\\ d_{\min} \leq Dx(t) \leq d_{\max} u(t)minJ[u(t)]=t0tf1+x(t)TQx(t)+u(t)TRu(t)dtx˙(t)=Av(ρ)x(t)+Bv(ρ)u(t)x(t0)=x0,Ex(tf)=(xr,yr,zr)TdminDx(t)dmax
其中: E = ( I 3 , O 3 × 10 ) E=(I_3,O_{3\times 10}) E=(I3,O3×10), D = ( O 3 × 10 , I 3 ) D = (O_{3\times 10},I_3) D=(O3×10,I3) Q = Q T ≥ 0 , R = R T ≥ 0 Q=Q^T\geq 0,R=R^T\geq 0 Q=QT0,R=RT0 d min ⁡ = ( V a min ⁡ , ϕ a min ⁡ , n z min ⁡ ) T d_{\min}=(V_{a\min},\phi_{a\min},n_{z\min})^T dmin=(Vamin,ϕamin,nzmin)T, d max ⁡ = ( V a max ⁡ , ϕ a max ⁡ , n z max ⁡ ) T d_{\max}=(V_{a\max},\phi_{a\max},n_{z\max})^T dmax=(Vamax,ϕamax,nzmax)T​。令 ∂ H ∂ u = 2 R u + B v ( ρ ) T λ = 0 \frac{\partial H}{\partial u}=2Ru + B_v(\rho)^T\lambda = 0 uH=2Ru+Bv(ρ)Tλ=0,得到:
u = − 1 2 R − 1 B v ( ρ ) T λ u = -\frac{1}{2}R^{-1}B_v(\rho)^T\lambda u=21R1Bv(ρ)Tλ
构造Hamilton函数 H = 1 + x T Q x + u T R u + λ T [ A v ( ρ ) x + B v ( ρ ) u ] H=1+x^TQx+u^TRu+\lambda^T [A_v(\rho)x+B_v(\rho)u] H=1+xTQx+uTRu+λT[Av(ρ)x+Bv(ρ)u],令 ρ = x \rho =x ρ=x
{ λ ˙ = − ∂ H ∂ x = − ( 2 Q x + λ T ∂ ∂ x ( A v ( ρ ) x + B v ( ρ ) u ) ) x ˙ = ∂ H ∂ λ = A v ( ρ ) x + B v ( ρ ) u \begin{cases} \dot{\lambda}=-\frac{\partial H}{\partial x}=-(2Qx+\lambda^T\frac{\partial}{\partial x}(A_v(\rho)x+B_v(\rho)u)) \\ \dot{x} =\frac{\partial H}{\partial \lambda}= A_v(\rho)x + B_v(\rho)u \end{cases} {λ˙=xH=(2Qx+λTx(Av(ρ)x+Bv(ρ)u))x˙=λH=Av(ρ)x+Bv(ρ)u
其中,
∂ ∂ x [ A v ( ρ ) x ] = ? ∂ ∂ x [ B v ( ρ ) u ] = − 1 2 ∂ ∂ x [ B v ( ρ ) R − 1 B v ( ρ ) T λ ] = ? \frac{\partial}{\partial x}[A_v(\rho)x] = ?\\ \frac{\partial }{\partial x}[B_v(\rho)u] = -\frac{1}{2}\frac{\partial }{\partial x}[B_v(\rho)R^{-1}B_v(\rho)^T\lambda] = ? x[Av(ρ)x]=?x[Bv(ρ)u]=21x[Bv(ρ)R1Bv(ρ)Tλ]=?
其中 H ( t f ) = 0 H(t_f)=0 H(tf)=0,应该采用打靶法得到 t f t_f tf λ 0 \lambda_0 λ0,能使得:
∣ ∣ E x ( t f ) − ( x r , y r , z r ) T ∣ ∣ ≤ ε 1 ∣ ∣ H ( t f ) ∣ ∣ ≤ ε 2 d min ⁡ ≤ D x ( t ) ≤ d max ⁡ ||Ex(t_f)-(x_r,y_r,z_r)^T|| \leq \varepsilon_1 \\ ||H(t_f)||\leq \varepsilon_2\\ d_{\min} \leq Dx(t) \leq d_{\max} ∣∣Ex(tf)(xr,yr,zr)T∣∣ε1∣∣H(tf)∣∣ε2dminDx(t)dmax
获取上述的量后,如何就可以用Matlab的ode45函数,或者直接采用bvp4c将上述两点边值问题(BVP),迭代出最优轨迹和最优策略。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/2259947.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

基于Redis实现令牌桶算法

基于Redis实现令牌桶算法 令牌桶算法算法流程图优点缺点 实现其它限流算法 令牌桶算法 令牌桶是一种用于分组交换和电信网络的算法。它可用于检查数据包形式的数据传输是否符合定义的带宽和突发性限制(流量不均匀或变化的衡量标准)。它还可以用作调度算…

学习threejs,局部纹理刷新,实现图片分块加载

👨‍⚕️ 主页: gis分享者 👨‍⚕️ 感谢各位大佬 点赞👍 收藏⭐ 留言📝 加关注✅! 👨‍⚕️ 收录于专栏:threejs gis工程师 文章目录 一、🍀前言1.1 ☘️Texture 贴图 二、&#x1…

超标量处理器设计笔记(10) 寄存器重命名过程的恢复、分发

重命名 寄存器重命名过程的恢复使用 Checkpoint 对 RAT 进行恢复使用 WALK 对 RAT 进行恢复使用 Architecture State 对 RAT 进行恢复总结 分发(Dispatch) 寄存器重命名过程的恢复 当发生异常、分支预测失败时,指令占用 RAT、ROB 和 Issue …

海康萤石摄像机接入EasyNVR流程:开启RTSP-》萤石视频添加到EasyNVR-》未来支持海康SDK协议添加到EasyNVR

EasyNVR目前支持GB28181、RTSP、ONVIF、RTMP(推流)这几种协议接入,目前正在增加海康HIKSDK、大华DHSDK等几种SDK的接入,我们今天就介绍一下萤石摄像机怎么通过RTSP接入到EasyNVR。 第一步:萤石摄像机开启 萤石设备默…

Qt编写的文件传输工具

使用QT编写的文件传输工具 文件传输工具通过发送udp广播消息将IP广播给其他开启该程序的局域网机器 文件传输工具 通过发送udp广播消息将IP广播给其他开启该程序的局域网机器 收到的广播消息可以显示在IP地址列表中,点击IP地址可以自动填充到IP地址栏内 选择文件…

【潜意识Java】深入理解 Java 面向对象编程(OOP)

目录 什么是面向对象编程(OOP)? 1. 封装(Encapsulation) Java 中的封装 2. 继承(Inheritance) Java 中的继承 3. 多态(Polymorphism) Java 中的多态 4. 抽象&…

PWM调节DCDC参数计算原理

1、动态电压频率调整DVFS SOC芯片的核电压、GPU电压、NPU电压、GPU电压等,都会根据性能和实际应用场景来进行电压和频率的调整。 即动态电压频率调整DVFS(Dynamic Voltage and Frequency scaling),优化性能和功耗。 比如某SOC在…

OpenCV相关函数

一、二值化函数(threshold) 功能:将灰度图像转换为二值图像,通常用于图像分割。通过设置阈值,把图像中低于阈值的像素设为0,高于阈值的像素设为1。 参数: src:输入图像。 thresh&a…

bean后处理器的作用

这是beanFactory中常见的一些后处理器: 其中这俩个属于bean后处理器: internalAutowiredAnnotationProcessor解析Autowired、Value internalCommonAnnotationProcessor解析Resource、PostConstruct、PreDestroy Bean后处理器的作用:为Bean…

YOLOv11融合[CVPR2024]Starnet中的star block取模块

YOLOv11v10v8使用教程: YOLOv11入门到入土使用教程 YOLOv11改进汇总贴:YOLOv11及自研模型更新汇总 《Rewrite the Stars》 一、 模块介绍 论文链接:https://arxiv.org/abs/2403.19967 代码链接:https://github.com/ma-xu/Rewri…

日常灵感:听劝是一种天赋

希望这段分享能给你提供一些新的角度,让你在自己的工作和生活中更好地利用这份“听劝”的天赋! 父与子的救赎:听劝的天赋 学霸爸爸李先生是一个典型的"别人家的父母"。 他从小就是学霸,凭借过硬的学习能力从重点高中一…

The Rise and Potential of Large Language ModelBased Agents:A Survey---讨论

讨论 论法学硕士研究与Agent研究的互利性 近年来,随着激光诱导金属化技术的发展,激光诱导金属化与化学剂交叉领域的研究取得了长足的进步,促进了这两个领域的发展。在此,我们期待着LLM研究和Agent研究相互提供的一些益处和发展机…

React 第十六节 useCallback 使用详解注意事项

useCallback 概述 1、useCallback 是在React 中多次渲染缓存函数的 Hook,返回一个函数的 memoized的值; 2、如果多次传入的依赖项不变,那么多次定义的时候,返回的值是相同的,防止频繁触发更新; 3、多应用在 父组件为函…

Transformer: Attention Is All You Need (2017) 翻译

论文:Attention Is All You Need 下载地址如下: download: Transformer Attention Is All you need Attention Is All You Need 中文 《Attention Is All You Need》是《Transformer》模型的开创性论文,提出了一种全新的基于注意力机制的架构&#xf…

Git-分支(branch)常用命令

分支 我们在做项目开发的时候,无论是软件项目还是其他机械工程项目,我们为了提高效率以及合理的节省时间等等原因,现在都不再是线性进行,而是将一个项目抽离出诸进行线,每一条线在git中我们就叫做分支,bran…

【AIGC进阶-ChatGPT提示词副业解析】反向心理学在沟通中的运用:激将法的艺术

引言 在日常沟通和管理中,直接的表达方式并不总能达到预期效果。反向心理学,特别是其中的激将法,作为一种独特的沟通技巧,往往能在看似消极的表达中激发出积极的反应。本文将深入探讨反向心理学中激将法的运用技巧、实施策略及其…

360智脑张向征:共建可信可控AI生态 应对大模型安全挑战

发布 | 大力财经 人工智能的加速发展,有力推动了社会的数智化转型;与此同时,带来的相关安全风险也日益凸显。近日,在北京市举办的通明湖人工智能开发与应用大会上,360智脑总裁张向征以“大模型安全研究与实践”为主题&…

lc46全排列——回溯

46. 全排列 - 力扣(LeetCode) 法1:暴力枚举 总共n!种全排列,一一列举出来放入list就行,关键是怎么去枚举呢?那就每次随机取一个,然后删去这个,再从剩下的数组中继续去随机选一个&a…

在线设计平台:Axure新手的在线设计助手

Axure是一款在产品设计、用户体验和交互设计领域广泛使用的强大原型设计工具。它使设计师和产品经理能够迅速构建高保真原型,验证设计和功能,并进行用户测试。结合在线设计平台的协作特性,团队可以更高效地创建、分享和优化原型,加…

Burp与小程序梦中情缘

前言 在日常渗透工作中,有时需要对微信小程序进行抓包渗透,通过抓包,我们可以捕获小程序与服务器之间的通信数据,分析这些数据可以帮助我们发现潜在的安全漏洞,本文通过讲述三个方法在PC端来对小程序抓包渗透 文章目…