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

控制架构

文章继续采用的是 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),迭代出最优轨迹和最优策略。

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

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

java抽奖系统登录下(四)

6.4 关于登录 最简单的登录: 1、web登录页填写登录信息,前端发送登录信息到后端; 2、后端接受登录信息,并校验。校验成功,返回成功结果。 这种登录会出现一个问题,用户1成功登录之后,获取到后台…

面经zijie

以下是对 C# GC 和 Lua GC 的详细分析,包括它们的原理、特性、优化方式及对比。 C# GC:详细分析 C# 的垃圾回收器 (Garbage Collector, GC) 是一个自动内存管理系统,它在程序运行时负责管理对象的分配和释放,防止内存泄漏。 1. …

利用代理IP爬取Zillow房产数据用于数据分析

引言 最近数据分析的热度在编程社区不断攀升,有很多小伙伴都开始学习或从事数据采集相关的工作。然而,网站数据已经成为网站的核心资产,许多网站都会设置一系列很复杂的防范措施,阻止外部人员随意采集其数据。为了解决这个问题&a…

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

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

Pytest-Bdd-Playwright 系列教程(14):Docstring 参数

Pytest-Bdd-Playwright 系列教程(14):Docstring 参数 前言一、什么是docstring?二、基本语法三、主要特点四、实际例子五、注意事项六、使用建议总结 前言 在自动化测试的过程中,我们经常需要处理复杂的测试数据或需要输入多行文…

Quad Remesher使用教程

为什么要拓扑? 我们知道,模型在三维软件中的表现,是由一系列的面通过不同角度组合而成的。3D模型制作层面上的拓扑,按我的理解来说,就是一个模型的面的结构分布——布线。想表现和制作一个三维模型,有无限…

智慧政务数据中台建设及运营解决方案

数据中台:政府数字化转型的引擎 数据中台作为政府数字化转型的核心驱动力,起源于美军的作战体系,强调高效、灵活与强大。它不仅促进了政府决策的科学性,还推动了政府服务的精细化与智能化。 数据中台的应用场景:数字…

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…

Android 系统应用重名install安装失败分析解决

Android 系统应用重名install安装失败分析解决 文章目录 Android 系统应用重名install安装失败分析解决一、前言1、Android Persistent apps 简单介绍 二、系统 persistent 应用直接安装需求分析解决1、系统应用安装报错返回的信息2、分析解决 三、其他1、persistent系统应用in…

3D一览通在线协同设计,助力汽车钣金件设计与制造数字化升级

汽车行业已迎来智能化的汹涌浪潮,在此背景下,零部件制造商唯有积极应对,以智能制造为核心驱动力,方能跟上行业发展步调,在激烈的市场竞争中抢占先机。作为整车制造不可或缺的核心组件之一,汽车钣金件亦需紧…

如何将你的 Ruby 应用程序从 OpenSearch 迁移到 Elasticsearch

作者:来自 Elastic Fernando Briano 将 Ruby 代码库从 OpenSearch 客户端迁移到 Elasticsearch 客户端的指南。 OpenSearch Ruby 客户端是从 7.x 版 Elasticsearch Ruby 客户端分叉而来的,因此代码库相对相似。这意味着当将 Ruby 代码库从 OpenSearch 迁…

Kafka系列教程 - Kafka 生产者 -2

1. 生产者简介 不管是把 Kafka 作为消息队列系统、还是数据存储平台,总是需要一个可以向 Kafka 写入数据的生产者和一个可以从 Kafka 读取数据的消费者,或者是一个兼具两种角色的应用程序。 使用 Kafka 的场景很多,诉求也各有不同&#xff…

动态规划:0-1背包问题 图文+举例超详细说明

一、题目描述 给定n(n<100)种物品和一个背包。物品i的重量是wi(wi<100)&#xff0c;价值为vi(vi<100)&#xff0c;背包的容量为C(C<1000)。 应如何选择装入背包中的物品&#xff0c;使得装入背包中物品的总价值最大? 在选择装入背包的物品时&#xff0c;对每种物…

实例:图片处理

目录 图片处理 Python代码展示 代码逐行注释 图片素材 运行结果 需要注意的几点&#xff1a; 运行思路 1. 导入必要的模块及类&#xff08;开头部分&#xff09; 2. 定义文件相似度检查函数&#xff08;file_similarity_checker 函数部分&#xff09; 3. 指定要比较的…

鸿蒙项目云捐助第四讲鸿蒙App应用的登陆注册页实现

根据app的操作流程可以知道&#xff0c;当启动页启动后&#xff0c;点击启动页中的页面就进入到了登录页。本讲就是针对于登录注册页的实现&#xff0c;实现的界面参考下图。 这里根据这个素材的参考实现鸿蒙Next云捐助的登录页。 一、鸿蒙Next云捐助登录页的实现 在项目中继…

大屏开源项目go-view二次开发1----环境搭建(C#)

最近公司要求做一个大屏的程序用于展示公司的产品&#xff0c;我以前也没有相关的经验&#xff0c;最糟糕的是公司没有UI设计的人员&#xff0c;领导就一句话要展示公司的产品&#xff0c;具体展示的内容细节也不知道&#xff0c;全凭借自己发挥。刚开始做时是用wpf做的&#x…

WHLUG丨deepin、华中科技大学开放原子开源俱乐部、 RustSBI 和清华大学开源操作系统训练营共话开源新生代成长之路

2024年11月30日下午&#xff0c;由 deepin&#xff08;深度&#xff09;社区联合华中科技大学开放原子开源俱乐部、 RustSBI 开源社区和清华大学开源操作系统训练营共同举办的WHLUG&#xff08;武汉Linux用户组&#xff09;线下沙龙在华中科技大学成功举办。 本次活动聚集了50余…

操作系统的基本认识

操作系统的感性认识 操作系统这个词可能或多或少听说过&#xff0c;比如windows, linux, macOS。这些其实都是工程师们经过实践后的具象化产物。而操作系统原理这六个字就是操作系统的抽象化&#xff0c;更准确的说&#xff0c;操作系统原理是很理论化的东西。举一个不是很恰当…

强化学习Q-learning及其在机器人路径规划系统中的应用研究,matlab代码

一、Q-learning 算法概述 Q-learning 是一种无模型的强化学习算法&#xff0c;它允许智能体&#xff08;agent&#xff09;在没有环境模型的情况下通过与环境的交互来学习最优策略。Q-learning的核心是学习一个动作价值函数&#xff08;Q-function&#xff09;&#xff0c;该函…

微信小程序横屏页面跳转后,自定义navbar样式跑了?

文章目录 问题原因&#xff1a;解决方案&#xff1a; 今天刚遇到的问题&#xff0c;横屏的页面完成操作后跳转页面后&#xff0c;自定义的tabbar样式乱了&#xff0c;跑到最顶了&#xff0c;真机调试后发现navbar跑到手机状态栏了&#xff0c;它正常应该跟右边胶囊一行。 知道问…