技术领域
[0001] 本
发明属于
机器人控制领域，更具体地，涉及一种四旋翼无人机轨迹跟踪控制方法。
背景技术
[0002] 随着现代控制理论和旋翼无人机技术研究的发展，旋翼无人机可以实现稳定
悬停，定点
起飞和降落等优良特性，旋翼无人机作为实现未来空中物流交通运输的主流发展方向，激励了大量科研人员从事旋翼无人机控制系统相关课题的研究工作。
[0003] 然而用于单旋翼无人机在实际应用中往往存在着控制难度高
稳定性差，针对应用领域的稳定性简易性，研究人员也开始将研究对象转向四旋翼无人机系统。另一方面，因存在输入变量更多，四旋翼往往比单旋翼无人机更加灵活、易于控制且稳定性更好，可以在飞行时，同时完成某些高要求任务，如航拍。因而，四旋翼无人机更符合实际应用需求。
[0004] 在实际应用中，可能会要求四旋翼无人机系统在固定时间完成轨迹跟踪任务，而目前主流控制方法无法实现固定时间轨迹跟踪，为满足实际应用需求，固定时间跟踪方法尤为重要。另外，同大多机械刚体结构一样，四旋翼无人机的动态特性可通过由其机械参数表示的数学模型表达，但前提是无人机的结构已知，且机械参数已知。实际上，无人机在工作过程中，受工况以及外部干扰的影响，其部分机械参数往往无法精确测得。故在对无人机进行数学建模时，通常需要考虑参数不确定性。
[0005] 因此，结合以上，在任务空间中考虑扰动及模型参数不确定性的四旋翼无人机系统的有限时间轨迹跟踪控制具有重要意义。
发明内容
[0006] 本发明提供一种四旋翼无人机轨迹跟踪控制方法，用以解决现有四旋翼无人机轨迹跟踪控制中因在建模时机械参数无法精确获得且无法固定轨迹跟踪时间而导致在实际应用中无法实现对无人机在时间和轨迹上的高
精度跟踪控制的技术问题。
[0007] 本发明解决上述技术问题的技术方案如下：一种四旋翼无人机轨迹跟踪控制方法，包括：
[0008] 基于期望
位置及其对应的速度和
加速度以及实时采集的实际位置及其对应的速度和
角度，采用位置速度控制环，实时估计该环路的扰动，并基于该扰动的估计量，计算无人机的合
力控制量以及参考第一角度及其对应的
角速度和
角加速度，其中，所述第一角度为
横滚角度和
俯仰角度，该环路的扰动估计误差在固定时间内收敛到零；
[0009] 基于所述参考第一角度及其对应的角速度和角加速度、期望第二角度其对应的角速度和角加速度以及实际角度及其对应的角速度，采用
姿态角控制环，实时估计该环路的扰动，并基于该扰动的估计量，分别计算无人机绕
机体坐标系x、y、z轴转动的力矩控制量，其中，所述第二角度为
偏航角度，该环路的扰动估计误差在固定时间内收敛到零；
[0010] 基于所述合力控制量和所述力矩控制量，控制无人机轨迹跟踪，其中，无人机的角度、角速度误差以及位置、速度误差均在固定时间内收敛到零。
[0011] 本发明的有益效果是：本方法是一种考虑扰动及模型参数不确定性的固定时间轨迹跟踪控制方法，通过控制位置速度环中的扰动估计误差以及姿态角控制环中的扰动估计误差均在固定时间内收敛到零，进而实现位置速度控制环在固定时间内使得无人机的角度、角速度误差收敛到零以及实现姿态角控制环在孤独时间内使得无人机的位置、速度误差在固定时间内收敛到零。具体实现中，可考虑四旋翼无人机模型中的动力学参数不确定性以及外部扰动，选用适当的控制参数来满足对机器人的工作速度和工作精度的要求，使得所涉及控制方法在实际应用中可实现四旋翼无人机系统在固定时间完成轨迹跟踪任务，时效性好，实用性更强，实现在实际应用中对无人机在时间和轨迹上的高精度跟踪控制。
[0012] 上述技术方案的
基础上，本发明还可以做如下改进。
[0013] 进一步，所述位置速度控制环包括用于使得该环路的扰动估计误差在第一固定时间T1内收敛到零的状态估计器，表示为：
[0014]
[0015] 其中， 为该环路的实际扰动D1的估计量，在初始时刻t0，该环路的实际扰动量D1为零，L1为该环路的实际扰动各分量变化率的上界；ki,i＝1,2,3为控制增益常数；v为无人机的实际速度矢量，vd为无人机的期望速
度矢量，g为
重力加速度，u1为所述合力控制量，ez表示大地坐标系下z轴的单位矢量，m为无人机
质量，R∈R3×3为依次以机体坐标系的z、y、x顺序旋转的旋转矩阵，ad为无人机的期望加速度矢量；
[0016] 则所述第一固定时间T1表示为：
[0017]
[0018] 进一步，所述合力控制量u1表示为：
[0019]
[0020]
[0021]
[0022] 其中，αi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝1,2，mi,ni,pi,qi均为正奇数；p为无人机的实际位置，pd为无人机的期望位置，ev为无人机的速度误差，S1为该环路的预设滑模面。
[0023] 进一步，所述参考第一角度的计算表达式为：
[0024]
[0025] 其中，ψ为大地坐标系下的偏航角度。
[0026] 进一步，所述位置速度控制环控制的位置和速度误差在第四固定时间T4内收敛于零，表达式为：
[0027]
[0028] 本发明的进一步有益效果是：本发明采用上述位置速度控制环中状态估计器和控制律进行协同作用，由第四固定时间的公式可知，只需要根据实际需要有效设置参数，即可有效实现位置和速度误差在固定时间内收敛于零，实用性强。
[0029] 进一步，所述姿态角控制环节包括用于使得该环路的扰动估计误差在第二固定时间T2内收敛到零的间状态估计器表示为：
[0030]
[0031] 其中， 为WD2的估计量，D2为该环路的实际扰动，L2为该环路的实际扰动各分量变化率的上界，在初始时刻t0，该环路的实际扰动量D2为零，W为坐标系变换矩阵，I为单位矩阵，Ω为机体坐标系下的实际角速度矢量， 为在大地坐标系下的无人机的期望角速度矢量，τ'为该环的滑模控制律；ki,i＝4,5,6为控制增益常数，
[0032] 则所述第二固定时间T2表示为：
[0033]
[0034] 进一步，所述无人机绕机体坐标系x、y、z轴转动的力矩控制量分别为u2、u3、u4，表示为：[u2,u3,u4]T＝τ；τ＝I-1Wτ'；
[0035]
[0036]
[0037] 其中，αi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝3,4，mi,ni,pi,qi均为正奇数；Θ为在大地坐标系下的无人机的实际角度矢量，Θd为在大地坐标系下的无人机的期望角度矢量，为在大地坐标系下的无人机的期望角加速度矢量， 为W的导数， S2为该环路的预设滑模面。
[0038] 进一步，所述姿态角控制环控制的角度、角速度误差在第三固定时间T3内收敛于零，T3的表达式为：
[0039]
[0040] 本发明的进一步有益效果是：本发明采用上述姿态角控制环中状态估计器和控制律进行协同作用，由第三固定时间的公式可知，只需要根据实际需要有效设置参数，即可有效实现角度、角速度误差在固定时间内收敛于零，实用性强。
[0041] 进一步，所述轨迹跟踪总的收敛时间T≤max(T1,T2)+T3+T4。
[0042] 本发明的进一步有益效果是：角度、角速度误差先收敛，然后位置和速度误差再收敛，并且扰动收敛时间T1、T2与控制输入量无关，基于此，确定的轨迹跟踪总的收敛时间，精确度高，实用性强。
[0043] 本发明还提供一种存储介质，所述存储介质中存储有指令，当计算机读取所述指令时，使所述计算机执行如上述任一种四旋翼无人机轨迹跟踪控制方法。
附图说明
[0044] 图1为本发明
实施例提供的一种四旋翼无人机轨迹跟踪控制方法的流程
框图；
[0045] 图2为本发明实施例提供的四旋翼无人机轨迹跟踪双环控制示意图；
[0046] 图3为本发明实施例提供的四旋翼无人机模型图；
[0047] 图4为本发明实施例提供的位置速度控制环的结构图；
[0048] 图5为本发明实施例提供的姿态角控制环的结构图；
[0049] 图6为本发明实施例提供的位置速度控制环和姿态角控制环中扰动估计量和实际扰动量之间的对比图；
[0050] 图7为本发明实施例提供的任务空间中位置和姿态角的跟踪轨迹图；
[0051] 图8为本发明实施例提供的无人机的控制输入量示意图；
[0052] 图9为本发明实施例提供的任务空间中位置和姿态角轨迹跟踪图。
具体实施方式
[0053] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0054] 实施例一
[0055] 一种四旋翼无人机轨迹跟踪控制方法100，如图1所示，包括：
[0056] 步骤110、基于期望位置及其对应的速度和加速度以及实时采集的实际位置及其对应的速度和角度，采用位置速度控制环，实时估计该环路的扰动，并基于该扰动的估计量，计算无人机的合力控制量以及参考第一角度及其对应的角速度和角加速度，其中，第一角度为横滚角度和俯仰角度，该环路的扰动估计误差在固定时间内收敛到零；
[0057] 步骤120、基于参考第一角度及其对应的角速度和角加速度、期望第二角度其对应的角速度和角加速度以及实际角度及其对应的角速度，采用姿态角控制环，实时估计该环路的扰动，并基于该扰动的估计量，分别计算无人机绕机体坐标系x、y、z轴转动的力矩控制量，其中，第二角度为偏航角度，该环路的扰动估计误差在固定时间内收敛到零；
[0058] 步骤130、基于合力控制量和力矩控制量，控制无人机轨迹跟踪，其中，无人机的角度、角速度误差以及位置、速度误差均在固定时间内收敛到零。
[0059] 需要说明的是，本方法首先考虑扰动，对无人机进行动力学和运动学建模；然后把无人机的轨迹跟踪控制转换为双环控制，外环是位置速度控制环，内环是姿态角环控制；如图2所示，位置速度控制环输入期望的位置和速度以及实际的位置和速度，输出给
电机的部分控制输入量以及给姿态角控制环的参考期望的角度和角速度；姿态角控制环输入期望的角度和角速度以及实际的角度和角速度，输出给电机的另一部分控制输入量。接下来，进行内外环
控制器设计，具体包括：位置速度控制器设计，包括固定时间状态估计器和固定时间滑模控制器设计，其中状态估计器可以在固定时间后准确估计位置速度控制环的扰动，在准确补偿位置速度控制环的扰动后，另外一个固定时间后在所提出的滑模控制律的控制下可以跟踪期望的位置速度轨迹；姿态角控制器设计，包括固定时间状态估计器和固定时间滑模控制器设计，其中状态估计器可以在固定时间后准确估计姿态角控制环的扰动，在准确补偿姿态角控制环的扰动后，另外一个固定时间后在所提出的滑模控制律的控制下可以跟踪期望的角度和角速度。其中，状态估计器的估计误差收敛于0的时间由它的参数唯一决定；补偿扰动后，滑模控制器控制位置速度收敛到期望的位置速度值的时间由它的参数唯一决定。基于该系统设计，进行上述四旋翼无人机轨迹跟踪控制。其中，横滚角度、俯仰角度和偏航角度均为在大地坐标系下的角度。
[0060] 本方法是一种考虑扰动及模型参数不确定性的固定时间轨迹跟踪控制方法，通过控制位置速度环中的扰动估计误差以及姿态角控制环中的扰动估计误差均在固定时间内收敛到零，进而实现位置速度控制环在固定时间内使得无人机的角度、角速度误差收敛到零以及实现姿态角控制环在孤独时间内使得无人机的位置、速度误差在固定时间内收敛到零。具体实现中，可考虑四旋翼无人机模型中的动力学参数不确定性以及外部扰动，选用适当的控制参数来满足对机器人的工作速度和工作精度的要求，使得所涉及控制方法在实际应用中可实现四旋翼无人机系统在固定时间完成轨迹跟踪任务，时效性好，实用性更强。
[0061] 为保证前述方法的结果正确性，将上述方法建立在以下三项条件基础上：
[0062] (1)位置速度控制环的扰动满足： 即位置速度控制环的扰动初始值为0，而且其一次导存在且有界；
[0063] (2)姿态角控制环的扰动满足： 即姿态角控制环的扰动初始值为0，而且其一次导存在且有界；
[0064] (3)四旋翼无人机旋转角度满足
[0065] 动力学和运动学建模模型为：
[0066]
[0067] 如图3所示的四旋翼无人机模型图，图中，E＝{ex,ey,ez}表示大地坐标系，B＝{bx,by,bz}表示无人机
机身坐标系，p＝[x,y,z]T∈R3表示无人机在世界坐标系中的位置，v＝T 3 T 3[vx,vy,vz]∈R为无人机在世界坐标系中的速度，Θ＝[φ,θ,ψ] ∈R为无人机在世界坐标系中的欧拉角，Ω＝[ωx,ωy,ωz]T∈R3为无人机在机身坐标系中的角速度，g,m分别为重力加速度和无人机质量，ez为z方向的单位向量，D1＝[dx,dy,dz]T∈R3,D2＝[dφ,dθ,dψ]T∈R3为扰动；I＝diag(Ixx,Iyy,Izz)∈R3×3表示惯性矩阵；τ＝[u2,u3,u4]T∈R3表示系统输入力矩，u1
3×3 3×3
表示系统输入无人机总升力；R∈R ,W∈R 表示旋转矩阵，C·＝cos(·),S·＝sin(·)，另外：
[0068]
[0069]
[0070] 跟踪目标的数学表达式为： 该表达式即为当前跟踪目标，pd＝[xd,yd,zd]T∈R3，vd＝[vx_d,vy_d,vz_d]T∈R3，ad＝[ax_d,ay_d,az_d]T∈R3分别表示跟踪目标的位置状态、速度、加速度。
[0071] 四旋翼无人机的物理参数如下表1所示：
[0072] 表1四旋翼无人机物理参数
[0073]
[0074]
[0075] 结合目标的位置、速度、加速度，根据
牛顿第二定律，可以得到位置速度控制方程：
[0076]
[0077] 其中，T4是收敛时间的上界，由设计参数决定的常数, 是扰动的估计值。由此可以得到 令 则u1, 都可以确定，
[0078] 目标的角度Θd＝[φd,θd,ψd]T∈R3，可以得到：
[0079]
[0080] ψd可以自行设计，一般设为0；对Θds求导数可得目标角速度T 3
aΩ_d＝[aωx_d,aωy_d,aωz_d]∈R目标角加速度，aΩ_d是控制器设计参数决定的，θ为横滚角度，φ为俯仰角度，ψ为偏航角度。
[0081] 结合目标的角度、角速度、加速度，可以得到姿态角控制环的控制方程：
[0082]
[0083] 其中，T3是收敛时间的上界，由设计参数决定的常数, 是扰动的估计值。由此可以得到τ，则u2,u3,u4都可以确定。
[0084] 优选的，上述位置速度控制环包括用于使得该环路的扰动估计误差在第一固定时间T1内收敛到零的状态估计器，表示为：
[0085]
[0086] 其中， 为该环路的实际扰动D1的估计量，在初始时刻t0，该环路的实际扰动量D1为零，L1为该环路的实际扰动各分量变化率的上界；ki,i＝1,2,3为控制增益常数；v为无人机的实际速度矢量，vd为无人机的期望速
度矢量，g为重力加速度，u1为所述合力控制量，ez表示大地坐标系下z轴的单位矢量，m为无人机质量，R∈R3×3为依次以机体坐标系的z、y、x顺序旋转的旋转矩阵，ad为无人机的期望加速度矢量；则上述第一固定时间T1表示为：
[0087]
[0088] 优选的，上述合力控制量u1表示为：
[0089]
[0090]
[0091]
[0092] 其中，αi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝1,2，mi,ni,pi,qi均为正奇数；p为无人机的实际位置，pd为无人机的期望位置，ev为无人机的速度误差，S1为该环路的预设滑模面。
[0093] 优选的，上述位置速度控制环控制的位置和速度误差在第四固定时间T4内收敛于零，表达式为：
[0094]
[0095] 如图4所示，位置速度控制环的结构包括状态估计器和控制律，图中：
[0096]
[0097]
[0098]
[0099] 优选的，上述姿态角控制环节包括用于使得该环路的扰动估计误差在第二固定时间T2内收敛到零的间状态估计器表示为：
[0100]
[0101] 其中， 为WD2的估计量，D2为该环路的实际扰动，L2为该环路的实际扰动各分量变化率的上界，在初始时刻t0，该环路的实际扰动量D2为零，W为坐标系变换矩阵，I为单位矩阵，Ω为机体坐标系下的实际角速度矢量， 为在大地坐标系下的无人机的期望角速度矢量，τ'为该环的滑模控制律；ki,i＝4,5,6为控制增益常数，
[0102] 则上述第二固定时间T2表示为：
[0103]
[0104] 各估计器的参数的选取具体可见如下表2，扰动设置为：
[0105] di＝0.2sin(5t),i＝x,y,z,φ,θ,ψ；
[0106] 表2估计器的参数
[0107]参数名称 参数取值 参数名称 参数取值
k1 3 k4 3
k2 0.5 k5 0.5
k3 4 k6 4
μ1 0.5 μ2 0.5
λ1 2 λ2 2
[0108] 优选的，上述无人机绕机体坐标系x、y、z轴转动的力矩控制量分别为u2、u3、u4，表示为：[u2,u3,u4]T＝τ；τ＝I-1Wτ'；
[0109]
[0110]
[0111] 其中，αi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝3,4，mi,ni,pi,qi均为正奇数；Θ为在大地坐标系下的无人机的实际角度矢量，Θd为在大地坐标系下的无人机的期望角度矢量，为在大地坐标系下的无人机的期望角加速度矢量， 为W的导数， S2为该环路的预设滑模面。
[0112] 优选的，上述姿态角控制环控制的角度、角速度误差在第三固定时间T3内收敛于零，T3的表达式为：
[0113]
[0114] 优选的，上述轨迹跟踪总的收敛时间T≤max(T1,T2)+T3+T4。
[0115] 如图5所示，姿态角控制环的结构图，包括状态估计器和控制律，图中：
[0116]
[0117]
[0118]
[0119]
[0120] 各控制律中的控制参数的选取具体可见如下表3，目标轨迹设置为：
[0121]
[0122]
[0123] zd(t)＝0.25,ψd(t)＝0；
[0124] 表3控制参数
[0125]
[0126]
[0127] 为了更好的说明本发明，现证明状态估计器是固定时间收敛的，如下：
[0128] 定义状态观测器估计误差 对位置速度控制环状态估计器求导，得：
[0129] 分为以下两种情况，对所述的状态估计器进行分析，得到以下分析结果：
[0130] (1)当||Δ1||≠0时，两边同时左乘 得：
[0131] 所述的状态估计器数学表达式满足：
[0132]
[0133] 令s＝||Δ1||，
[0134] 构造李雅普诺夫函数 求导可得：
[0135]
[0136] 所以渐进稳定。
[0137] 当V＞1时， V将在 时收敛到1；
[0138] 当V≤1时， V将在 时收敛到0。
[0139] 所以经过时间 后，V,s,Δ1都会收敛于0，因为经过t1+t2时间后
[0140] (2)当||Δ1||＝0时， 收敛到0的时间 其中，经过时间T1，
[0141] 状态观测器估计值误差 收敛到0。
[0142] 同理经过总时间T2状态观测器估计值误差 收敛到0。
[0143]
[0144] 如图6所示，左图对应位置速度控制环，右图对应姿态角控制环，位置速度控制环和姿态角控制环扰动估计量和实际的扰动值，在所设计的状态估计器的作用下，位置和姿态角扰动估计量的每个分量都在固定时间收敛于其对应的扰动值。
[0145] 下面证明所设计的控制率 可以保证位置速度是固定时间收敛的：
[0146] 对滑模面S1求导，可得：
[0147]
[0148] 把动力学和运动学建模模型中 的数学表达式和设计的 的表达式带入上式，可得： 其中 是估计误差。
[0149] 根据前述，经过时间T1， 得：
[0150] 构造李雅普诺夫函数 求导可得：
[0151]
[0152] V1＞1时， V1将在 后收敛到1。
[0153] V1≤1时， V1将在 后收敛到0。
[0154] 所以经过时间 后，V1,S1都会收敛于0。
[0155] 由S1＝0，得 令ep＝p-pd，ev＝v-vd，则
[0156] 同理经过时间 后，ep和ev都会收敛于0。
[0157] 位置速度控制环收敛时间为：
[0158]
[0159] 同理姿态角控制环收敛时间为：
[0160]
[0161] 由于角度、角速度误差先收敛，然后位置和速度误差才会收敛，并且扰动收敛时间T1、T2与控制输入量无关，则所述轨迹跟踪收敛总时间表示为：
[0162] T≤max(T1,T2)+T3+T4。
[0163] 如图7所示，任务空间中位置和姿态角的跟踪轨迹图，在所设计的基于估计器的固定时间控制器的作用下，任务空间中位置和姿态角都在固定时间(8秒内)收敛于其对应的跟踪目标轨迹。基于图6和图7，得到无人机的控制输入，包括合力控制量和所有力矩控制量，如图8所示，最终得到任务空间中位置和姿态角轨迹跟踪图，如图9所示，图9整体和图7仿真的结果很接近，仅仅因为
传感器精度问题造成位置和姿态角跟踪有微小的误差，除去这一因素带来的误差，位置和姿态角都可以在固定时间(8秒内)收敛于其对应的跟踪目标轨迹，其结果可
支撑本实施例方法的控制效果。
[0164] 实施例二
[0165] 一种存储介质，存储介质中存储有指令，当计算机读取所述指令时，使所述计算机执行上述如实施例一所述的一种四旋翼无人机轨迹跟踪控制方法。
[0166] 相关技术方案同实施例一，在此不再赘述。
[0167] 本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何
修改、等同替换和改进等，均应包含在本发明的保护范围之内。
