system: [SYSTEM]

user: 2. 四旋翼建模 在主要的控制器设计和算法给出之前，有必要分析一下四旋翼编队模型。首先，引出四旋翼的数学模型，介绍其参数含义、动态模型以及坐标变换关系；然后，将描述四旋翼系统的感知拓扑结构以及编队队形。 2.1. 四旋翼数学模型 图1. X型四旋翼示意图 本文的实验采用的是基于Pixhawk飞控、轴距为240 mm的X型四旋翼，其外形和螺旋桨转向如图1所示。分别记机体坐标系上的 x b 轴、 y b 轴和 z b 轴方向上的转矩为 τ x 、 τ y 和 τ z ，其表达式如式1。四旋翼的空间位置信息在四旋翼大地坐标系 O x e y e z e 中采集， x e ， y e ， z e 为四旋翼在大地坐标系下的位置，而姿态角和受力情况则在机体坐标系 O x b y b z b 中测量， x b ， y b ， z b 为四旋翼大地坐标系下的位置，本实验采用的四旋翼主要参数见表1。 Table 1 Parameters and their descriptio 参数名 说明 数值 C T 旋翼升力系数 2.08 × 10 −5 (N/rad/s) C D 旋翼阻力系数 2.72 × 10 −6 (N/rad/s) I x 四旋翼绕x轴的转动惯量 1.14 × 10 −2 (kg∙m 2 ) I y 四旋翼绕y轴的转动惯量 1.14 × 10 −2 (kg∙m 2 ) I z 四旋翼绕z轴的转动惯量 1.58 × 10 −2 (kg∙m 2 ) m 四旋翼的总质量 350 (g) l 四旋翼质心到转轴距离 120 (mm) g 经纬度(119˚E， 32˚N)重力加速度 9.8 (m/s 2 ) 表1. 参数及其含义 { τ x = 2 C T l ( − ω 1 2 + ω 2 2 + ω 3 2 − ω 4 2 ) / 2 τ y = 2 C T l ( ω 1 2 − ω 2 2 + ω 3 2 − ω 4 2 ) / 2 τ z = C D ( + ω 1 2 + ω 2 2 − ω 3 2 − ω 4 2 ) (1) 其中， ω   i ( i = 1 ， 2 ， 3 ， 4 ) 为第i号电机的转速。 一般地，不计空气阻力，记 F sum = C T ( ω 1 2 + ω 2 2 + ω 3 2 + ω 4 2 ) 为四个螺旋桨总拉力，四个旋翼的转速代数和为 ω sum = − ω 1 − ω 2 + ω 3 + ω 4 ，可得如下动态模型 [ 17 ] 如式2： { x ¨ e = F sum ( cos φ sin θ cos ψ + sin φ sin ψ ) / m y ¨ e = F sum ( cos φ sin θ sin ψ + sin φ cos ψ ) / m z ¨ e = F sum cos φ cos θ / m − g φ ¨ = ( I y − I z ) θ ˙ ψ ˙ / I x + τ x / I x θ ¨ = ( I z − I x ) φ ˙ ψ ˙ / I y + τ y / I y ψ ¨ = ( I x − I y ) φ ˙ θ ˙ / I z + τ z / I z (2) 其中 φ 、 θ 、 ψ 为四旋翼的横滚角、俯仰角和偏航角。 由于本文是在具有一定高度的水平面上对四旋翼进行位置控制，当圆形编队位置控制器计算出位置目标加速度 x ¨ e 和 y ¨ e 后，需要进行姿态反解，计算出机体的目标姿态欧拉角 φ 和 θ ，进而控制四旋翼系统在水平面内运动。 由式3将大地坐标系下的加速度 x ¨ e 和 y ¨ e 的转换为机体坐标系下的加速度 x ¨ b 和 y ¨ b 。 [ x ¨ b y ¨ b ] = [ cos ψ − sin ψ sin ψ cos ψ ] [ x ¨ e y ¨ e ] (3) 由式2、式3可求解机体坐标系下的目标欧拉角 φ 和 θ 如式4： { φ = arctan ( − ( x ¨ e cos ψ + y ¨ e sin ψ ) / g ) θ = arctan ( ( − x ¨ e sin ψ + y ¨ e cos ψ ) cos φ / g ) (4) 随后，将通过控制各四旋翼的姿态角度 φ 和 θ ，得到四旋翼的稳定姿态控制，进而得到四旋翼在大地坐标系下的空间圆形编队位置控制。 2.2. 系统感知拓扑结构及队形 有了以上的四旋翼数学模型，下面将介绍四旋翼系统的感知拓扑结构以及编队队形的描述。 本文考虑四旋翼的实际体积，各四旋翼与相邻四旋翼在空间中需要避免碰撞，如图2所示的投影平面上有两个四旋翼a和b，定义相邻四旋翼之间的最小安全距离为 R 1 ，即四旋翼a和b避免碰撞的极限情况距离，需要满足中心距离 ‖ p a − p b ‖ > R 1 。接着定义避免碰撞距离为 R 2 ，即当 R 1 < ‖ p a − p b ‖ < R 2 时，需要进行避免碰撞措施。 图2. (a) 最小安全距离；(b) 避免碰撞距离 本文考虑一组由N个四旋翼组成的系统，每个四旋翼可由编号i表示，其中 i ∈ I n = { 1 ， 2 ， ⋯ ， N } ，四旋翼i的位置 p i = [ x e i ， y e i ， z e i ] T ，圆形编队的圆心位置 p 0 = [ x 0 ， y 0 ， z 0 ] T ，则各四旋翼关于目标圆心的相对位置 p ¯ i 可分别用直角坐标系和柱坐标系方式表示为式5和式6： p ¯ i = p i − p 0 = [ x ¯ i y ¯ i z ¯ i ] T ， i ∈ I n (5) p ¯ i = [ ρ i cos α i ρ i sin α i z ¯ i ] ， i ∈ I n (6) 其中，在四旋翼空间系统的垂直投影水平面上， ρ i = x ¯ i 2 + y ¯ i 2 为四旋翼i中心相对于目标中心的距离， α i ∈ [ 0 ， 2 π ) 为各四旋翼对于目标点的方位角。 图3. 四旋翼空间系统编号的俯视图 各四旋翼由相对目标圆心的方位角 α i 的递增顺序决定编号顺序，而对于同一方位角，则编号按与目标点的距离递增排序，如图3所示。由此可以得到与四旋翼i相邻的四旋翼编号为式7： i + 1 = { i + 1 ， i = 1 ， 2 ， ⋯ ， N − 1 1 ， i = N 和 i − 1 = { i − 1 ， i = 2 ， 3 ， ⋯ ， N N ， i = 1 (7) 本文考虑每个四旋翼只能获得相对目标圆心和相邻编号的四旋翼水平位置信息，可将四旋翼系统感知拓扑描述为 G = ( γ ， ε ) ，其中 γ = I n = { 1 ， 2 ， ⋯ ， N } 表示四旋翼集合，而 ε = { ( 1 ， 2 ) ， ( 2 ， 3 ) ， ⋯ ， ( N − 1 ， N ) ， ( N ， 1 ) } 表示相邻关系集合。各四旋翼自身可感知的拓扑结构信息如式8和图4所示。 { p ^ i = p i + 1 − p i p ^ i − 1 = p i − p i − 1 ， i ∈ I n (8) 其中，在四旋翼空间垂直投影水平面上， p ^ i 为四旋翼i中心到四旋翼 i + 1 中心的相对距离。并且定义 α ^ i 为从四旋翼i到 i + 1 的角距离，为目标圆心延伸到四旋翼i的直线经过逆时针旋转至四旋翼 i + 1 而形成的角度；同理可以获得四旋翼 i − 1 到四旋翼i的相对距离 p ^ i − 1 和角距离 α ^ i − 1 。 接着，本文考虑期望的四旋翼空间圆形编队的队形由高度h、半径r和圆形相对位置分布d描述。设 d i 代表从i到 i + 1 的期望水平角距离，而每个四旋翼i只利用信息 d i 和 d i − 1 ，那么可由下式9表示期望的圆形相对位置分布。 d = [ d 1 ， d 2 ， ⋯ ， d N ] T ， ∑ i = 1 N d i = 2 π (9) 图4. 各四旋翼自身可感知的拓扑结构信息俯视图 图5. (a) 空间编队轨迹；(b) 四旋翼空间轨迹及期望队形描述俯视图 最后，该空间圆形编队形成过程可由图5的轨迹表示：四旋翼由初始位置起飞，控制飞至设定高度，同时盘旋着向圆形收敛，并调整圆上四旋翼的相对位置分布，最终形成期望的队形并不断盘旋。 因此，本文的思路是将其空间圆形编队控制器建模为如下单积分模型式10： p ¯ ˙ i = [ x ¯ ˙ e i y ¯ ˙ e i z ¯ ˙ e i ] = [ u x i u y i u z i ] ， i ∈ I n (10) 其中 u x i ， u y i ， u z i 为将被设计的控制输入，即 x e ， y e ， z e 轴方向的期望速度。为了便于下文叙述论证，提出如下关于空间圆形编队问题的定义。 定义(空间圆形编队问题)：给定一个在空间上用高度h、半径r和图形分布d描述的空间圆形编队，设计分布式控制律 u i = u i ( p ¯ i ， p ^ i ， p ^ i − 1 ， h ， r ， d i ， d i − 1 ) ，在各四旋翼按空间分布逆时针排序的条件下，式10的解将趋于平衡点 p i * ，将满足下式11： { z i * = h ( 高 度 控 制 ) ρ i * = r ( 目 标 盘 旋 ) α ^ i * = d i ( 间 距 调 整 ) ，     i ∈ I n (11) 由定义，空间圆形编队问题被转换成仅使用四旋翼与目标之间的相对位置与相邻四旋翼之间的相对位置来设计所有四旋翼的自身控制器，使得所有四旋翼控制在设定高度，形成设定半径的圆形分布，同时在有限时间内形成期望的圆形相对位置分布，能够保证四旋翼之间的避免碰撞，形成围绕目标旋转的编队队形。