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

控制架构

文章继续采用的是 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.rhkb.cn/news/490234.html

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

相关文章

VR虚拟展厅的实时互动是如何实现的?

VR虚拟展厅的实时互动是通过一系列技术和流程实现的,这些技术和流程共同确保了用户在虚拟环境中的互动体验能够及时响应和更新。 接下来,由专业从事VR虚拟展厅制作的圆桌3D云展厅平台为大家介绍一下实现VR虚拟展厅实时互动的几个关键要素: 高…

(三)FT2232HL高速调试器的接口定义与使用配置说明

(特别声明:仅对FT2232HL_v0.2 20241125版本进行电路优化调整) 如果FT2232HL板子是V0.2版本(背面丝印FT2232HL_v0.2 20241125),类似下图这样的,说明已经对电路进行了优化调整。 1、接口定义 FT…

C++20 标准概念

1. 所有标准概念的概述 “类型和对象基本概念”表列出了类型和对象的基本概念。 “范围、迭代器和算法概念”表列出了范围、视图、迭代器和算法的概念。 “辅助概念”表列出的概念主要用作其他概念的构建块,通常不会让应用程序开发者直接使用。 头文件和命名空间 …

zookeeper的安装

zookeeper的安装 一.前言 zookeeper开源组件是为分布式应用,提供协调服务的一种解决方案。本文主要是介绍在Centos7的操作系统中,如何以单机,伪集群,集群的方式来安装部署zookeeper服务。zookeeper要求的jdk版本为1.6以上。本文假…

【经验分享】容器云运维的知识点

最近忙于备考没关注,有次点进某小黄鱼发现首页出现了我的笔记还被人收费了 虽然我也卖了一些资源,但我以交流、交换为主,笔记都是免费给别人看的 由于当时刚刚接触写的并不成熟,为了避免更多人花没必要的钱,所以决定公…

【docker】springboot 服务提交至docker

准备docker (不是docker hub或者harbor,就是可以运行docker run的服务),首先确保docker已经安装。 本文以linux下举例说明: systemctl stats docker ● docker.service - Docker Application Container EngineLoaded…

MySQL-5.7离线安装配置

说明: 因为在搭建hive和azkaban需要用到mysql数据库,所以先搭建好环境,练习自己搭建比赛会提供 环境: 在宿主机内搭建mysql服务(因为容器内搭建比较复杂) 开始安装 检查下是否已安装了mysql和mariadb&…

认识异常吧

在 Java 中,将程序执行过程中发生的不正常行为称为异常 。 异常的体系结构 1. Throwable : 是异常体系的顶层类,其派生出两个重要的子类 , Error(错误) 和 Exception(异常) 2. Error &…

C++ 【类和对象】

C是面向过程的编程语言。关注的是求解问题的过程。而c是面向对象的,关注的是对象,将一件事情拆分成不同的对象,靠对象之间的相互交互完成。 1、类 C种类是一种用户自定义的数据类型,用于封装数据成员(数据&#xff09…

蛋白研究新热点:AI 全方位剖析 DHA 与 Ferrostatin - 1 的作用密码

胰腺癌是一种非常棘手的癌症,传统化疗药物往往对它收效甚微,很难提高患者的生存率。不过,研究人员发现了一种可能的新治疗方向 —— 利用双氢青蒿素(DHA)诱导癌细胞发生铁死亡。 下面将以Dihydroartemisinin induces …

使用idea创建一个JAVA WEB项目

文章目录 1. javaweb项目简介2. 创建2.1 idea新建项目2.2 选择,命名2.3 打开2.4 选择tomcat运行2.5 结果 3. 总结 1. javaweb项目简介 JavaWeb项目是一种基于Java技术的Web应用程序,主要用于开发动态网页和Web服务。这种项目能够构建在Java技术栈之上&a…

python中向量指的是什么意思

一、向量是什么 在数学中,向量(也称为欧几里得向量、几何向量、矢量),指具有大小(magnitude)和方向的量。它可以形象化地表示为带箭头的线段。箭头所指:代表向量的方向;线段长度&am…

Axure高保真数据可视化大屏图表组件库

推出了一款高保真数据可视化大屏图表组件库,旨在为用户提供丰富的图表类型,使数据呈现更加直观、生动。本文将详细介绍该组件库中的各类图表元件,包括面积图、折线图、柱状图、条形图、圆环图、雷达图、仪表图以及综合类图表,以满…

内网是如何访问到互联网(H3C源NAT)

H3C设备NAPT配置 直接打开29篇的拓扑,之前都配置好了 「模拟器、工具合集」复制整段内容 链接:https://docs.qq.com/sheet/DV0xxTmFDRFVoY1dQ?tab7ulgil 现在是出口路由器可以直接访问61.128.1.1,下面的终端访问不了,需要做NAPT源…

hbase读写操作后hdfs内存占用太大的问题

hbase读写操作后hdfs内存占用太大的问题 查看内存信息hbase读写操作 查看内存信息 查看本地磁盘的内存信息 df -h查看hdfs上根目录下各个文件的内存大小 hdfs dfs -du -h /查看hdfs上/hbase目录下各个文件的内存大小 hdfs dfs -du -h /hbase查看hdfs上/hbase/oldWALs目录下…

海外组网之优解:外贸 SD-WAN 跨境组网专线赋能企业全球互联

在全球化商业进程中,众多企业积极拓展海外业务,设立国内外分支,然而分支组网难题也随之而来。外贸 SD-WAN 跨境专线成为解决此类问题的优选方案,为企业提供高效稳定的网络连接。 一、SD-WAN 跨境组网专线优势 (一&…

React Image Crop——在React应用中轻松实现图片裁剪功能

React Image Crop是一个用于在React应用程序中裁剪和调整图像的库。它提供了一个简单而强大的界面,允许用户选择和调整裁剪区域,并生成裁剪后的图像。 什么是React Image Crop? React Image Crop是一个开源的React组件,用于在浏览…

【2024 Dec 超实时】编辑安装llama.cpp并运行llama

首先讲一下环境 这是2024 年12月,llama.cpp 的编译需要cmake 呜呜呜 网上教程都是make 跑的。反正我现在装的时候make已经不再适用了,因为工具的版本,捣鼓了很久。 ubuntu 18 conda env内置安装。 以下是可以完美编译llama.cpp的测试工具版…

【AI知识】无监督学习之主成分分析PCA

1. 主成分分析 主成分分析(Principal Component Analysis,简称PCA)是一种常用的降维技术,旨在将数据从高维空间投影到一个新的低维空间,同时尽可能保留原始数据的结构和信息,这样做可以减少存储数据的空间…

在Liunx中安装JDK、Tomcat、mysql、lrzsz、Nginx

一.软件安装方式 在Linux系统中,安装软件的方式主要有四种,这四种安装方式的特点如下: 二.安装JDK 上述我们介绍了Linux系统软件安装的四种形式,接下来我们就通过第一种(二进制发 布包)形式来安装JDK。 在/下创建soft目录&…