[发明专利]基于在线安全学习的四旋翼无人机路径跟踪控制方法

摘要

本发明涉及一种基于在线安全学习的四旋翼无人机路径跟踪控制方法。其技术方案是，首先建立四旋翼无人机名义模型根据残差g(X,U)的数据得到学习模型再采用四旋翼无人机在线实时避障的概率约束条件实现在线避障，然后将控制系统状态安全域融入学习过程，在线计算安全域，保证四旋翼无人机安全性，提高跟踪性能。最后采用反馈线性化方法获得线性模型，针对线性模型在相应的约束条件下设计模型预测控制器。本发明能够在线获得环境参数，自主适应巡检环境的变化，因此，能显著提高控制系统的鲁棒性和响应性能，具有稳定性高、跟踪性能好和安全性强的特点。

主权项

一种基于在线安全学习的四旋翼无人机路径跟踪控制方法，其特征在于所述控制方法包含如下步骤：步骤1、四旋翼无人机名义模型的建立构建四旋翼无人机机体坐标系，所述机体坐标系是：以北为坐标系x轴的正方向，以东为坐标系y轴的正方向，以地为坐标系z轴的正方向；建立四旋翼无人机的名义模型X·=f(X,U)=φ·a1θ·ψ·+b1U2θ·a2φ·ψ·+b2U3ψ·a3φ·θ·+b3U4z·g-U1mcosφcosθx·-U1m(sinφsinψ+cosφsinθcosψ)y·U1m(sinφcosψ-cosφsinθsinψ)---(1)]]>式(1)中：φ表示四旋翼无人机的滚转角，rad，表示四旋翼无人机的滚转角加速度，rad/s，θ表示四旋翼无人机的俯仰角，rad，表示四旋翼无人机的俯仰角加速度，rad/s，ψ表示四旋翼无人机的偏航角，rad，表示四旋翼无人机的偏航角角速度，rad/s，z表示四旋翼无人机在z轴的位置，m，表示四旋翼无人机z轴方向的线速度，m/s，x表示四旋翼无人机在x轴的位置，m，表示四旋翼无人机x轴方向的线速度，m/s，y表示四旋翼无人机在y轴的位置，m，表示四旋翼无人机y轴方向的线速度，m/s，X表示四旋翼无人机12维的状态向量，X=φφ·θθ·ψψ·zψ·xx·yy·,]]>U表示虚拟控制量，U＝[U1 U2 U3 U4]，U1表示通道z的控制量，U2表示通道φ的控制量，U3表示通道θ的控制量，U4表示通道ψ的控制量，m表示四旋翼无人机的质量，kg，g表示重力加速度常量，9.8m/s2；步骤2、四旋翼无人机的学习模型假设在k时刻，传感器测得的四旋翼无人机实际状态根据k‑1时刻的状态和k时刻的状态得到状态微分再将状态微分与k时刻名义模型的值相减，得到k时刻残差g(X(k),U(k))g(X(k),U(k))=X~·(k)-X·(k)---(2)]]>式(2)中：表示k时刻的状态微分，表示k时刻的名义模型；随着时间t的增加，得到N个残差g(X,U)的数据；然后，利用所述N个残差g(X,U)的数据，得到学习模型对四旋翼无人机的12个状态分别建立相应的高斯过程学习模型Xl表示状态向量X的第l个变量，l＝1,2,…，12，学习模型定义状态向量X的第l个学习样本结构sl＝[Xl,U]，l＝1,2,…，12，取M个时刻(200～1000)个样本，则i时刻状态向量X的第l个样本为gl(sli)，i＝1,2,…,M，l＝1,2,…,12，j时刻状态向量X的第l个样本为gl(slj)，j＝1,2,…,M，l＝1,2,…,12；假设样本的数据均值为0，i时刻状态向量X的第l个样本gl(sli)和j时刻状态向量X的第l个样本gl(slj)的协方差为：kl(sli,slj)=σlη2exp(-12(sli-slj)TMl-2(sli-slj))+δlijσlω2---(3)]]>式(3)中：σlω表示测量噪声标准差，σ2lω表示测量噪声方差，σlη表示过程噪声标准差，σ2lη表示过程噪声方差，Ml表示对角矩阵，l＝1,2,…，12，sli表示i时刻状态向量X的第l个样本，i＝1,2,…,M，l＝1,2,…,12，slj表示j时刻状态向量X的第l个样本，j＝1,2,…,M，l＝1,2,…,12，δlab表示测量噪声方差系数，l＝1,2,…,12，δlab=1,a=b0,a≠b]]>采用梯度法求取最大似然，获得测量噪声标准差σlω、过程噪声标准差σlη和对角矩证Ml；对于下一时刻的样本s*，学习模型的预测参数为：μ(s*)=k(s*)K-1g^σ2(s*)=k(s*,s*)-k(s*)K-1kT(s*)---(4)]]>式(4)中：K表示N行N列矩阵，k(si,sj)表示i时刻样本和j时刻样本的协方差，i＝1,2,…,M，j＝1,2,…,M，K(i,j)表示矩阵第i行第j列的值,K(i,j)＝k(si,sj),i＝1,2,…,M，j＝1,2,…,M，k(s*,s*)表示下一时刻的样本s*的方差，k(s*)表示下一时刻的样本s*与之前M个时刻样本分别的协方差，k(s*)＝[k(s*,s1),...,k(s*,sM)]，μ(s*)表示学习模型的预测值，σ2(s*)表示学习模型更新的协方差值；步骤3、学习模型在线更新在所述第l个学习模型的基础上，l＝1,2,…,12，依据置信度3σ(sl)建立第l个学习模型的估计的扰动集合Ω^(sl)=[E(g‾l(sl))-3σ(sl),E(g‾l(sl))+3σ(sl)]---(5)]]>式(5)中：表示第l个学习模型的估计的扰动集合，Rm表示m维空间，表示第l个学习模型期望值，l＝1,2,…,12，3σ(sl)表示置信度，l＝1,2,…,12；然后，采用下述判别函数λ(sl)，判断第l个学习模型的准确性λ(sl)=dist(g(sl),Ω^(sl)C)maxδ∈Ω^(sl)dist(δ,Ω^(sl)C)---(6)]]>式(6)中：表示第l个学习模型的估计的扰动集合，Rm表示m维空间，表示第l个估计的扰动集的补集，g(sl)表示第l个残差，l＝1,2,…,12，δ表示估计扰动集里任一点，dist表示有符号距离函数，定义为：对于给定的Rn上的范数||.||，x∈Rn到的符号距离为st(x,A)=inf{||x-y||,y∈A},x∉A-inf{||x-y||,y∈Rn/A},x∈A---(7)]]>式(7)中：Rn表示n维空间，A表示第l个估计的扰动集的补集x表示第l个残差g(sl)中的一点，l＝1,2,…,12，y表示A内的任一点，x‑y表示第l个残差g(sl)中的一点x与A内任一点y的差；当判别函数λ(sl)∈[0,1]时，实际的第l个残差gl(sl)在估计的扰动集内,l＝1,2,…,12，设定经验阈值λL∈(0,1)，当判别函数λ(sl)＞λL时，当前第l个学习模型较为精确，不用更新，否则重复步骤2和步骤3更新学习模型步骤4、在线学习的四旋翼无人机跟踪控制方法步骤4.1、四旋翼无人机在线实时避障的概率约束条件对于预测时域N，假设四旋翼无人机实际状态服从正态分布：x(k+j)~N(x^(k+j),P(k+j))---(8)]]>式(8)中：表示期望为和方差为P(k+j)的正态分布，表示k+j时刻四旋翼无人机的预测状态，j＝0,1,2,…,N，P(k+j)表示k+j时刻四旋翼无人机状态不确定性的方差，j＝0,1,2,…,N；设障碍物位置估计的不确定性w～N(0,Z)，则障碍物集合：O=∪i=1rOi⊕{w}⋐R3---(9)]]>式(9)中：Oi表示环境中的障碍物，R3表示3维空间，Z表示四旋翼无人机位置估计不确定性的方差，N(0,Z)表示期望为0和方差为Z的正态分布；若四旋翼无人机的位置为p(k)，定义四旋翼无人机占据的空间为采用高斯分布描述四旋翼无人机位置的不确定性，则四旋翼无人机和障碍物的碰撞概率小于给定值Ph的约束条件为式(10)中：表示期望为和方差为Pc(k+j)+Z的正态分布，R(p(k+j))表示k+j时刻四旋翼无人机占据的空间，j＝0,1,2,…,N，O表示障碍物集合，表示k+j时刻四旋翼无人机估计的位置，Pc(k+j)表示k+j时刻四旋翼无人机估计位置不确定性的方差，Z表示障碍物位置估计不确定性的方差；步骤4.2、四旋翼无人机状态安全域约束方法假设四旋翼无人机状态约束集为Φ、控制输入约束为Γ和一个较为保守的域Ω，使得残差g(X,U)∈Ω；根据估计的扰动集合得到四旋翼无人机的安全域可达条件V(m)＞0和四旋翼无人机的安全域Disc(m,Ω^(sl))={m∈Rn|V(m)>0}---(11)]]>式(11)中：Rn表示n维空间，m表示四旋翼无人机的状态，表示第l个学习模型的估计的扰动集合，l＝1,2,…,12；当四旋翼无人机状态满足安全域可达条件V(m)＞0且判别函数λ(sl)＞λL时，安全域采用并且以完整动力学模型的预测状态作为状态约束；当四旋翼无人机状态不满足安全域可达条件V(m)＞0或判别函数λ(sl)≤λL时，安全域依赖名义模型构建，根据名义模型得到预测状态将作为状态约束；步骤4.3、非线性模型预测控制器的设计首先采用扩展卡尔曼滤波方法估计四旋翼无人机的状态，然后将所述四旋翼无人机名义模型分为四旋翼无人机位置动力学模型和四旋翼无人机姿态动力学模型，分别采用反馈线性化方法获得线性模型，最后针对线性模型在相应的约束条件下设计模型预测控制器；考虑四旋翼无人机的位置，在满足姿态约束、在线避障约束和安全域约束的条件下，将四旋翼无人机巡检的自主控制转化为二次优化minvi(·)J=Σj=0N-1(||yi(k+j)-yir(k+j)||Q2+||vi(k+j-1)||R2)+||yi(k+N)-yir(k+N)||S---(12)]]>式(12)中：N表示预测时域，i表示四旋翼无人机位置动力子系统、四旋翼无人机姿态子系统中任一个，Q表示yi(k+j)‑yir(k+j)的误差权矩阵，R表示vi(k+j‑1)的控制权矩阵，S表示yi(k+N)‑yir(k+N)的误差权矩阵，yi(k+j)表示四旋翼无人机在k+j时刻的预测值，yir(k+j)表示四旋翼无人机在k+j时刻的参考值，yi(k+N)表示四旋翼无人机在k+j时刻的预测值，yir(k+N)表示四旋翼无人机在k+j时刻的参考值，yi(k+j)‑yir(k+j)表示四旋翼无人机在k+j时刻预测值与参考值的差，yi(k+N)‑yir(k+N)表示四旋翼无人机在k+N时刻预测值与参考值的差，vi(k+j‑1)表示四旋翼无人机在k+j‑1时刻的控制量；当安全域满足安全域可达条件V(m)＞0且学习模型满足步骤2和步骤3时，采用学习模型和最大安全域作为状态的约束条件，当安全域可达条件V(m)＞0不成立或学习模型不满足步骤2和步骤3时，则采用预先设定的较为保守的安全域和名义模型的状态作为约束条件。