一种基于状态约束的无人机非线性PID姿态控制方法与流程

一种基于状态约束的无人机非线性pid姿态控制方法
技术领域
1.本发明涉及无人机领域,特别涉及一种基于状态约束的无人机非线性pid姿态控制方法。


背景技术：

2.当前无人机被广泛应用于高压输电线路、电缆隧道巡检等领域，自主飞行是无人机未来的发展方向。无人机在环境中存在静态和动态障碍物、外界存在不可测量扰动，这使得其感知、决策系统的复杂度和控制器设计的难度均呈现出指数增长趋势，以传统感知、运动预测、决策和现代控制理论方法面临着重大挑战，其主要表现为以下几个方面：
3.(1)无人机系统运行环境的恶劣性和多样性，导致传统感知方法对周围环境的障碍物识别正确率较低，并且对动态环境下避开障碍物的运动轨迹预测泛化性较差，可能造成无人机系统不能正确感知周围环境而引发严重事故。无人机需要感知对环境障碍物的正确识别，对动态障碍物的运行轨迹进行准确预测；
4.(2)复杂无人机系统的动态环境下的控制过程基于规则的方法，往往需要投入较多的人力成本，同时无人机在动态环境下的避障算法本身的自适应与自学习能力薄弱，无人机需要在各种复杂的动态环境下，尤其是含有大量障碍物的情况下进行稳定、高效和安全的飞行。
5.无人机运行的安全可靠性，不仅需要满足基本的运动路径规划，更关键的是要引入较多规则和动力学约束条件，解决无人机自主飞行速度慢、动作生硬、运动连续性受限以及在多样性条件下的飞行不流畅等问题。


技术实现要素：

6.针对现有技术存在的上述问题，本发明要解决的技术问题是：如何控制无人机在动态环境下，特别是面对恶劣以及多样性运行环境条件下的飞行。
7.为解决上述技术问题，本发明采用如下技术方案：一种基于状态约束的无人机非线性pid姿态控制方法，包括如下步骤：
8.s100：采用四旋翼无人机的6-dof动力学模型，并定义平移期望v
*
(t)和旋转期望w
*
(t),表达式如下：
[0009][0010][0011][0012]
其中，a1和b1表示动力学模型的系数矩阵，v表示无人机运行的平移预测值，w表示无人机运行的旋转预测值，表示v的一阶导数，表示w的一阶导数，f＝[f
x
,fy,fz]
t
表示作
用在四旋翼无人机上的x、y、z三个坐标系方向上的分力向量，m表示四旋翼无人机的重量，j表示常数矩阵，m＝[u
φ
,u
θ
,u
ψ
]
t
表示输入矩阵矢量向量，f/m表示的输入，m表示的输入；
[0013]
s200：定义初始平移控制量u
t
和初始旋转控制量ur，将u
t
和ur代入四旋翼无人机的6-dof动力学模型得到初始动力模型，表达式如下：
[0014][0015]
其中，u
t
＝[f
x
/m,fy/m,fz/m]
t
，ur＝[u
φ
,u
θ
,u
ψ
]
t
，fa(
·
)＝a1v和fb(
·
)＝b1w表示模型的耦合项，a＝diag{1,1,1}和b＝j-1
表示增广矩阵；da(.)表示平移扰动，db(.)表示旋转扰动；
[0016]
s300：结合状态依赖函数和初始动力模型构建基于状态约束的无人机动力模型，表达式如下：
[0017][0018]
其中，ξv表示平移状态依赖转换函数，ξw表示旋转状态依赖转换函数，ηv和ηw表示可计算的参数，ωv和ωw分别表示平移和旋转误差变量；
[0019]
s400：通过非线性pid控制算法计算基于状态约束的无人机动力模型的系统pid控制率，系统pid控制率包括平移误差方程控制律u
′
t
和旋转误差方程控制律u
′r，表达式分别如下：
[0020]
平移误差方程控制律：
[0021][0022]
其中，平移误差方程控制律自适应pid参数为：
[0023][0024]
其中，zv表示平移跟踪误差，k
p
表示比例系数，ki表示积分系数，kd表示微分系数，γ表示设计常数，c表示设计常数，δk
p
表示时变比例系数，δki表示时变积分系数，δkd表示时变微分系数，表示自适应参数，φ(
·
)表示平移误差方程控制律的负反馈参数；
[0025]
旋转误差方程控制律：
[0026][0027]
其中，旋转误差方程控制律自适应pid参数为：
[0028][0029]
其中，zw表示旋转跟踪误差，k
p
表示比例系数，ki表示积分系数，kd表示微分系数，γ2表示设计常数，d表示设计常数，δk
p
表示时变比例系数，δki表示时变积分系数，δkd表示时变微分系数，表示自适应参数，φ
w1
(
·
)表示旋转误差方程控制律的负反馈参数；
[0030]
s500：预设状态约束条件范围，对基于状态约束的无人机动力模型进行训练，具体步骤如下：
[0031]
s510：将系统控制信号输入平移误差方程控制律u
′
t
，输出得到无人机在状态约束下运行的平移预测值ξv；将系统控制信号输入旋转误差方程控制律u
′r，输出得到无人机在状态约束下运行的旋转预测值ξw；
[0032]
s520：分别计算平移跟踪误差zv和旋转跟踪误差zw，计算表达式如下：
[0033]zv
＝ξ
v-αv[0034]zw
＝ξ
w-αw；(10)
[0035]
其中，zv表示平移跟踪误差，zw表示旋转跟踪误差，ξv表示状态约束下运行的平移预测值，ξw表示状态约束下运行的旋转预测值，αv表示平移在进行状态依赖函数转换后的期望值，αw表示旋转在进行状态依赖函数转换后的期望值，表达式如下：
[0036][0037][0038]
其中，fv(t)表示平移下界，表示平移上界，fw(t)表示旋转下界，表示旋转上界；
[0039]
s530：如果计算得到的平移跟踪误差zv和旋转跟踪误差zw均衰减到状态约束条件范围内，得到训练好的基于状态约束的无人机动力模型，则认为可以在动态环境条件下实现无人机的飞行；
[0040]
如果计算得到的平移跟踪误差zv和旋转跟踪误差zw没有衰减到状态约束条件范围内，则反向更新基于状态约束的无人机动力模型的参数，并返回s510。
[0041]
作为优选，所述s300中状态依赖函数表达式为：
[0042][0043]
其中，p＝v时表示t时刻的平移误差，p＝w时表示t时刻的旋转误差，初始状态满足
pk(0)∈dk，k＝1,2；ξk(t)表示状态依赖转换函数，fk(t)表示误差下界，表示误差上界。
[0044]
从状态依赖转换中可以发现，当初始条件满足的时候，当且仅当xk到达边界dk的时候，状态依赖函数趋于无穷；也就是说，在初始条件xk(0)在子集dk时，只要保证了状态依赖转换函数有界，就能保证系统状态xk(t)一直在约束范围之内，从而实现全状态约束，所以，全状态约束问题被巧妙地转化为保证状态依赖函数有界性的问题。
[0045]
作为优选，所述s500中状态约束条件范围为[-1.5,2.5]。
[0046]
相对于现有技术，本发明至少具有如下优点：
[0047]
1.本发明阐述了一种基于状态约束的无人机非线性pid姿态控制方法，通过非线性pid控制算法来为无人机设计状态约束下的平移控制量和旋转控制量，以pid控制算法为基础设计的平移控制率和旋转控制率可以更精确的控制无人机的飞行姿态，使得无人机的实际飞行轨迹和期望轨迹基本重合，实现在动态环境的下，特别是面对恶劣以及多样性的运行环境条件下的无人机飞行；在实际飞行过程中的表现方式是可以更精确的避开复杂环境中的障碍物。
[0048]
2.本发明所阐述的基于状态约束的无人机非线性pid姿态控制方法将无人机在复杂动态环境下的飞行问题可以转化成动态系统的状态及性能约束控制问题，通过构建状态依赖转换函数，将非线性动态约束转换为新动态系统的状态有界性控制，从而使动态环境下无人机姿态飞行这一关键科学问题转换为一个动态系统的“约束”控制问题予以解决。
[0049]
3.在状态约束状状态下所提出的控制策略能使四旋翼无人机跟随参考轨迹移动，并且时间延迟很小，具有较好的飞行稳定性。
[0050]
4.相较于传统的pid控制，该非线性pid算法能根据环境变化自动调整参数，更加方便。
附图说明
[0051]
图1为本发明中无人机的动力学模型坐标图。
[0052]
图2为本发明实验中无人机的线速度跟踪性能。
[0053]
图3为本发明实验中无人机的角速度跟踪性能。
具体实施方式
[0054]
下面对本发明作进一步详细说明。
[0055]
参见图1，一种基于状态约束的无人机非线性pid姿态控制方法，包括如下步骤：
[0056]
s100：采用四旋翼无人机的6-dof动力学模型，并定义平移期望v
*
(t)和旋转期望w
*
(t),表达式如下：
[0057][0058]
[0059][0060]
其中，a1和b1表示动力学模型的系数矩阵，v表示无人机运行的平移预测值，w表示无人机运行的旋转预测值，表示v的一阶导数，表示w的一阶导数，f＝[f
x
,fy,fz]
t
表示作用在四旋翼无人机上的x、y、z三个坐标系方向上的分力向量，m表示四旋翼无人机的重量，j表示常数矩阵，m＝[u
φ
,u
θ
,u
ψ
]
t
表示输入矩阵矢量向量，f/m表示的输入，m表示的输入；
[0061]
具体实施建模过程时，参考了经典四旋翼无人机的建模方式，由向量恒等式可知，假设机体坐标系在地面坐标系中以w旋转，对于任意向量a，满足如下等式：
[0062][0063]
公式(13)中，表示在地面坐标系下a的绝对导数，而表示在机体坐标系中a的相对导数，由动量定理可知，四旋翼无人机动量的变化等于力的冲量，满足公式(14)：
[0064][0065]
由角动量定理可知，物体角动量的变化等于力矩的冲量，得到公式(15)：
[0066]
mdt＝d(jw)；(15)
[0067]
其中，j为在机体坐标系下四旋翼无人机的转动惯量，且假定为常数，如下表示：
[0068][0069]
结合公式(13)、公式(14)、公式(15)和公式(16)，可得到经典的四旋翼无人机平移子系统的动力学模型，表达式如下：
[0070][0071]
通常，可以假设将转动惯量矩阵j的非对角项视为一个小数项，当有两个小数项相乘时，可以被忽略，值得注意的是此假设比普通无人机模型更仔细地考虑了四旋翼无人机的质量不对称性。所以，公式(17)可优化为公式(18)，表达式如下：
[0072][0073]
公式(18)中，将j看作一个常数矩阵，使之计算较为简便，将它与动力学模型结合，就能得到四旋翼无人机的6-dof动力学模型，见公式(1)；
[0074]
公式(1)中的系数矩阵以及其详细表达如下所示；
[0075]
[0076][0077][0078][0079][0080][0081]
通过选择控制量u
t
＝[f
x
/m,fy/m,fz/m]
t
和ur＝[u
φ
,u
θ
,u
ψ
]
t
，并加入系统模型的不确定性，可以得到初始动力模型，见公式(4)。
[0082]
s200：定义初始平移控制量u
t
和初始旋转控制量ur，将u
t
和ur代入四旋翼无人机的6-dof动力学模型得到初始动力模型，表达式如下：
[0083][0084]
其中，u
t
＝[f
x
/m,fy/m,fz/m]
t
，ur＝[u
φ
,u
θ
,u
ψ
]
t
，fa(
·
)＝a1v和fb(
·
)＝b1w表示模型的耦合项，a＝diag{1,1,1}和b＝j-1
表示增广矩阵；da(.)表示平移扰动，db(.)表示旋转扰动；
[0085]
s300：结合状态依赖函数和初始动力模型构建基于状态约束的无人机动力模型，状态依赖函数为现有技术，表达式如下：
[0086][0087]
其中，ξv表示平移状态依赖转换函数，ξw表示旋转状态依赖转换函数，ηv和ηw表示可计算的参数，ωv和ωw分别表示平移和旋转误差变量；
[0088]
所述s300中状态依赖函数表达式为：
[0089][0090]
其中，p＝v时表示t时刻的平移误差，p＝w时表示t时刻的旋转误差，初始状态满足pk(0)∈dk，k＝1,2；ξk(t)表示状态依赖转换函数，fk(t)表示误差下界，表示误差上界。
[0091]
状态依赖函数为现有技术，对状态依赖函数求导得到公式(25)：
[0092][0093]
其中，
[0094][0095][0096][0097][0098]
由此可得基于状态约束的无人机动力模型，见公式(5)。
[0099]
s400：通过非线性pid控制算法计算基于状态约束的无人机动力模型的系统pid控制率，系统pid控制率包括平移误差方程控制律u
′
t
和旋转误差方程控制律u
′r，非线性pid控制算法为现有技术，表达式分别如下：
[0100]
平移误差方程控制律：
[0101][0102]
其中，平移误差方程控制律自适应pid参数为：
[0103][0104]
其中，zv表示平移跟踪误差，k
p
表示比例系数，ki表示积分系数，kd表示微分系数，γ表示设计常数，c表示设计常数，δk
p
表示时变比例系数，δki表示时变积分系数，δkd表示时变微分系数，表示自适应参数，φ(
·
)表示平移误差方程控制律的负反馈参数；
[0105]
旋转误差方程控制律：
[0106][0107]
其中，旋转误差方程控制律自适应pid参数为：
[0108][0109]
其中，zw表示旋转跟踪误差，k
p
表示比例系数，ki表示积分系数，kd表示微分系数，γ2表示设计常数，d表示设计常数，δk
p
表示时变比例系数，δki表示时变积分系数，δkd表示时变微分系数，表示自适应参数，φ
w1
(
·
)表示旋转误差方程控制律的负反馈参数；
[0110]
具体实施构建非线性pid控制律时，先通过构造如下pid形式的广义误差：
[0111][0112]
其中ε1和ε2是可以自由选择的常数；
[0113]
然后将平移跟踪误差和旋转跟踪误差分别结合广义误差进行进一步转换，得到公式(28)：
[0114][0115][0116]
基于此，控制目标就变成了设计非线性pid控制律，使得跟踪误差sv和sw在时间趋于无穷时候趋于0。
[0117]
首先，针对平移误差方程设计控制律，对平移跟踪误差求导得到公式(29)：
[0118][0119]
随后，对广义误差进行求导，得到公式(30)：
[0120][0121]
选定李雅普诺夫函数对李雅普诺夫函数求导可得公式(31)
[0122][0123]
定义函数李雅普诺夫函数的求导公式可以进一步化简为公式(32)：
[0124][0125]
针对函数γ
v1
，使用rbf神经网络进行近似，得到公式(33)：
[0126][0127]
因此，李雅普诺夫函数的导数可以化简为公式(34)：
[0128][0129]
这里通过应用神经网络对多个未知项进行整体地拟合，巧妙地将多个未知项转化为神经网络的标准形式，从而将问题转化为在一个未知项的情况下，用自适应方法实现控制器的设计。根据φ
v1
(s)和δ
v1
(s)的定义，很容易得到δ
v1
(s)在紧集ωs内，并且存在上限，即由此，既可以应用杨不等式对式子进行缩放从而解决未知项的问题，具体见公式(35)：
[0130][0131]
由此，可以进一步得到公式(36)：
[0132][0133]
其中，是未知的常数，并且φ
v1
＝φ
t
(s)φ(s) 1＞0是可以计算的函数；因此，可以进一步化简李雅普诺夫函数的导数，见公式(37)：
[0134][0135]
由此，可以得到平移误差方程的pid控制，如公式(8)。
[0136]
根据pid参数可对pid控制律进行进一步化简，得到如下公式(38)
[0137][0138]
将控制律代入s
t
ηvauv项中，对其进行化简得到公式(39)：
[0139][0140]
因此，对李雅普诺夫函数的导数进一步化简，可以得到公式(40)：
[0141][0142]
进一步的，将自适应参数误差引入李雅普诺夫候选函数从而构造了新的李雅普诺夫函数其中γ＞0是一个正的常数，将更新律代入到李雅普诺夫函数的导数并进一步的处理如下，得到公式(41)：
[0143]
[0144]
其中，υ＝min{2cvλv,σ}＞0，
[0145]
基于此可以得到以下结果：
[0146]
1.不违反状态约束。当李雅普诺夫函数能保证sv在紧凑集ωv中，可知|sv|＜kb，因zv＜kb，而zv＝ξ
v-αv，根据状态约束函数的理论，能得到|ev|＜kb；对于v＝ev v
*
和|v
*
|≤av，有|v|≤|ev| |v
*
|＜kb av，又因kb＝k
v-av，可知|v|＜kv，因此，状态约束永远不会违反；
[0147]
2.所有闭环信号都是有界且连续的。通过求解微分不等式得到因此，可以得出vv(t)是有界的，所以可以进一步得到ev是有界收敛的且限制在一个小的紧集ωv，由控制律的表达式可知，u
t
,a,φv均有界；综上所述，所有的信号均有界且连续。
[0148]
对于旋转误差方程控制律设计，通过将平移误差所有相关参数改为旋转误差的相关参数，即可得到的相同的结果和可行性证明；
[0149]
s500：预设状态约束条件范围，对基于状态约束的无人机动力模型进行训练，具体实施时可以使用matlab仿真实验进行训练验证，所述s500中状态约束条件范围为[-1.5,2.5]，具体步骤如下：
[0150]
s510：将系统控制信号输入平移误差方程控制律u
′
t
，输出得到无人机在状态约束下运行的平移预测值ξv；将系统控制信号输入旋转误差方程控制律u
′r，输出得到无人机在状态约束下运行的旋转预测值ξw；
[0151]
s520：分别计算平移跟踪误差zv和旋转跟踪误差zw，计算表达式如下：
[0152]zv
＝ξ
v-αv[0153]zw
＝ξ
w-αw；(10)
[0154]
其中，zv表示平移跟踪误差，zw表示旋转跟踪误差，ξv表示状态约束下运行的平移预测值，ξw表示状态约束下运行的旋转预测值，αv表示平移在进行状态依赖函数转换后的期望值，αw表示旋转在进行状态依赖函数转换后的期望值，表达式如下：
[0155][0156][0157]
其中，fv(t)表示平移下界，表示平移上界，fw(t)表示旋转下界，表示旋转上界；
[0158]
s530：如果计算得到的平移跟踪误差zv和旋转跟踪误差zw均衰减到状态约束条件范围内，得到训练好的基于状态约束的无人机动力模型，则认为可以在动态环境条件下实现无人机的飞行；
[0159]
如果计算得到的平移跟踪误差zv和旋转跟踪误差zw没有衰减到状态约束条件范围内，则反向更新基于状态约束的无人机动力模型的参数，并返回s510。
[0160]
仿真实验验证算法的可行性：
[0161]
为了验证所阐述的基于模型的控制器的有效性和稳定性，将以悬挂载荷的四旋翼无人机为研究对象进行matlab仿真实验。本实验选择系统状态约束在(-1.5,2.5)范围之内进行仿真，参见图2和图3，由图可知在状态约束状态下，所提出的控制策略能使四旋翼跟随参考轨迹移动，并且时间延迟很小，具有较好的飞行稳定性。
[0162]
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。