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

控制架构

文章继续采用的是 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.mzph.cn/web/63020.shtml

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

相关文章

【Flink-scala】DataStream编程模型之延迟数据处理

DataStream API编程模型 1.【Flink-Scala】DataStream编程模型之数据源、数据转换、数据输出 2.【Flink-scala】DataStream编程模型之 窗口的划分-时间概念-窗口计算程序 3.【Flink-scala】DataStream编程模型之水位线 4.【Flink-scala】DataStream编程模型之窗口计算-触发器-…

2024告别培训班 数通、安全、云计算、云服务、存储、软考等1000G资源分享

大类有:软考初级 软考中级 软考高级 华为认证 华三认证: 软考初级: 信息处理技术员 程序员 网络管理员 软考中级: 信息安全工程师 信息系统监理师 信息系统管理工程师 嵌入式系统设计时 数据库系统工程师 电子商务设…

《操作系统 - 清华大学》8 -1:进程的组成

文章目录 1. 进程的组成2. 进程与程序的联系3. 进程与程序的区别4. 进程与程序关系 1. 进程的组成 进程具体包含哪些东西: 首先要执行相应的代码,所以执行代码需要放到内存中代码执行需要处理数据,数据需要放到内存中需要知道现在要执行哪条…

【Java】String类API

创建字符串 字符串字面量"Hello"高效,常量池复用常见、简单的字符串创建 new 关键字new String("Hello")每次创建新对象,性能开销较高显式创建新对象 字符数组new String(char[])转换字符数组字符数组转字符串 StringBuilder/St…

数据结构初阶---二叉树---堆

一、树 1.树的概念 树是一种非线性的数据结构,由n(n≥0)个有限结点组成的一个有层次关系的集合。形状类似一棵倒挂的树,根朝上,分支向下。 根结点没有前驱结点,可以有n(n≥0)个后继结点。 其余结点被分为M个互不相交的集合&am…

C语言 字符串输入输出函数、scanf(“%[^\n]“,)可输入空格 、fgets删除换行符

字符串输入函数: scanf("%s",数组名) gets(数组名) fgets() --- 文件流输入函数 函数原型: int scanf( const char *format, ...…

深度学习camp-第J4周:ResNet与DenseNet结合探索

🍨 本文为🔗365天深度学习训练营 中的学习记录博客🍖 原作者:K同学啊 本周任务: 探索ResNet和DenseNet的结合可能性本周任务较难,我们在chatGPT的帮助下完成 一、网络的构建 设计一种结合 ResNet 和 Den…

「iOS」通过CoreLocation Framework深入了解MVC架构

「iOS」通过CoreLocation Framework重新了解多界面传值以及MVC架构 文章目录 「iOS」通过CoreLocation Framework重新了解多界面传值以及MVC架构前言CoreLocation了解根据需求建模设计属性方法设计协议传值Block传值KVONotification通知方式 总结参考文章 前言 在这个学期的前…

ArrayList源码分析、扩容机制面试题,数组和List的相互转换,ArrayList与LinkedList的区别

目录 1.java集合框架体系 2. 前置知识-数组 2.1 数组 2.1.1 定义: 2.1.2 数组如何获取其他元素的地址值?(寻址公式) 2.1.3 为什么数组索引从0开始呢?从1开始不行吗? 3. ArrayList 3.1 ArrayList和和…

【C++】- 掌握STL List类:带你探索双向链表的魅力

文章目录 前言:一.list的介绍及使用1. list的介绍2. list的使用2.1 list的构造2.2 list iterator的使用2.3 list capacity2.4 list element access2.5 list modifiers2.6 list的迭代器失效 二.list的模拟实现1. list的节点2. list的成员变量3.list迭代器相关问题3.1…

Docker--Docker Container(容器) 之容器实战

对docker容器的前两篇文章 Docker–Docker Container(容器) 之 操作实例 Docker–Docker Container(容器) Mysql容器化安装 我们可以先在Docker Hub上查看对应的Mysql镜像,拉取对应的镜像: 拉取mysql5.7版本的镜像: docker pull mysql:5.7…

【汇编语言】内中断(二) —— 安装自己的中断处理程序:你也能控制0号中断

文章目录 前言1. 编程处理0号中断1.1 效果演示1.2 分析所要编写的中断处理程序1.2.1 引发中断1.2.2 中断处理程序1.2.3 中断处理程序do0应该存放的位置1.2.4 中断向量表的修改1.2.5 总结 1.3 程序框架1.4 注意事项1.5 从CPU的角度看中断处理程序1.6 一些问题的思考与解答 2. 安…

VS2019中无法跳转定义_其中之一情况

我习惯了使用VS2019看stm的代码; 遇到的问题,在导入代码后,发现有些函数调用不能跳转到定义; 问题描述步骤 1、导入代码 2、跳转,无法跳转 1、中文路径 2、删除.vs文件 和网上查的都没办法解决 最后发现是VS不支持 …

让 Win10 上网本 Debug 模式 QUDPSocket 信号槽 收发不丢包的方法总结

在前两篇文章里,我们探讨了不少UDP丢包的解决方案。经过几年的摸索测试,其实方法非常简单, 无需修改代码。 1. Windows 下设置UDP缓存 这个方法可以一劳永逸解决UDP的收发丢包问题,只要添加注册表项目并重启即可。即使用Qt的信号与槽&#…

Elasticsearch:ES|QL 中的全文搜索 - 8.17

细心的开发者如果已经阅读我前两天发布的文章 “Elastic 8.17:Elasticsearch logsdb 索引模式、Elastic Rerank 等”,你就会发现在 8.17 的发布版中,有一个重要的功能发布。那就是 ES|QL 开始支持全文搜索了。在今天的文章中我们来尝试一下。…

SQL和Python 哪个更容易自学?

SQL和Python不是一个物种,Python肯定更难学习。如果你从事数据工作,我建议先学SQL、有余力再学Python。因为SQL不光容易学,而且前期的投入产出比更大。 SQL是数据查询语言,场景限于数据查询和数据库的管理,对大部分数据…

【unity】从零开始制作平台跳跃游戏--界面的认识,添加第一个角色!

在上一篇文章中,我们已经完成了unity的环境配置与安装⬇️ 【Unity】环境配置与安装-CSDN博客 接下来,让我们开始新建一个项目吧! 新建项目 首先进入unityHub的项目页面,点击“新项目”: 我们这个系列将会以2D平台…

怎么禁用 vscode 中点击 go 包名时自动打开浏览器跳转到 pkg.go.dev

本文引用怎么禁用 vscode 中点击 go 包名时自动打开浏览器跳转到 pkg.go.dev 在 vscode 设置项中配置 gopls 的 ui.navigation.importShortcut 为 Definition 即可。 "gopls": {"ui.navigation.importShortcut": "Definition" }ui.navigation.i…

Unity3D实现抽象类的应用场景例子

系列文章目录 unity知识点 文章目录 系列文章目录👉前言👉一、示例👉二、使用步骤👉三、抽象类和接口的区别👉3-1、抽象类👉3-2、接口类👉壁纸分享👉总结👉前言 假设我们正在制作一个游戏,游戏中有多种不同类型的角色,这些角色都有一些共同的行为(比如移…

数据仓库工具箱—读书笔记01(数据仓库、商业智能及维度建模初步)

数据仓库、商业智能及维度建模初步 记录一下读《数据仓库工具箱》时的思考,摘录一些书中关于维度建模比较重要的思想与大家分享🤣🤣🤣 博主在这里先把这本书"变薄"~有时间的小伙伴可以亲自再读一读,感受一下…