双机领航-跟踪是最简单的无人机分布式控制，是实现大规模集群分布式控制的基础，本文主要记录对如何实现双机跟踪护航控制的思考以及验证实验。
无人机的位置控制模型可以建立为：
{
p
˙
=
v
v
˙
=
u
−
>
x
˙
(
t
)
=
A
x
(
t
)
+
B
u
(
t
)
,
x
(
t
)
=
[
p
(
t
)
v
(
t
)
]
,
A
=
[
0
1
0
0
]
B
=
[
0
1
]
\left\{\begin{array}{l} \dot{p}=v \\ \dot{v}=u \end{array}\right.->\dot{x}(t)=Ax(t)+Bu(t),x(t)= \left[ \begin{matrix} p(t) \\ v(t) \end{matrix} \right], A=\left[ \begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix} \right] B=\left[ \begin{matrix} 0 \\ 1 \end{matrix} \right]
{p˙=vv˙=u−>x˙(t)=Ax(t)+Bu(t),x(t)=[p(t)v(t)],A=[0010]B=[01]
领航机和跟随机的相关变量可分别用下标{0,1}来标记，领航机的控制协议
u
0
u_0
u0不受跟随机影响，仅仅受期望路线和自身状态反馈影响，假设其期望飞行轨迹为
f
0
(
t
)
=
[
p
d
0
(
t
)
,
v
d
0
(
t
)
]
T
f_0(t)=[p_{d0}(t),v_{d0}(t)]^T
f0(t)=[pd0(t),vd0(t)]T，那么其控制协议可以设计为：
u
0
=
K
(
f
0
(
t
)
−
x
0
)
+
v
˙
d
0
(1)
u_0=K(f_0(t)-x_0)+\dot{v}_{d0} \tag{1}
u0=K(f0(t)−x0)+v˙d0(1)
其中 K = [ k 1 , k 2 ] K=[k_1,k_2] K=[k1,k2]，只要保证参数 k 1 , k 2 k_1,k_2 k1,k2均是正数，即可确保领航机对轨迹 f 0 ( t ) f_0(t) f0(t)的跟踪。
对于跟随机而言，由于要跟踪领航机，其期望的位置应当为相对于领航机的某一位置，可以表示为：
f
1
(
t
)
=
s
1
(
t
)
+
x
0
(
t
)
f_1(t)=s_1(t)+x_0(t)
f1(t)=s1(t)+x0(t)
其中
s
1
(
t
)
=
[
p
s
1
(
t
)
,
v
s
1
(
t
)
]
T
s_1(t)=[p_{s1}(t),v_{s1}(t)]^T
s1(t)=[ps1(t),vs1(t)]T表示相对于领航机的期望位置差和速度差，类似的，其控制协议可设计为：
u
1
=
K
(
f
1
(
t
)
−
x
1
)
+
v
˙
d
1
=
K
(
s
1
(
t
)
+
x
0
(
t
)
−
x
1
(
t
)
)
+
(
v
˙
s
1
+
v
˙
0
)
(2)
\begin{aligned} u_1&=K(f_1(t)-x_1)+\dot{v}_{d1} \\ &=K(s_1(t)+x_0(t)-x_1(t))+(\dot{v}_{s1}+\dot{v}_0) \end{aligned} \tag{2}
u1=K(f1(t)−x1)+v˙d1=K(s1(t)+x0(t)−x1(t))+(v˙s1+v˙0)(2)
可开展时变编队实验和时不变编队实验：
（1）时不变编队，
s
1
(
t
)
s_1(t)
s1(t)为恒定值，在三维方向上可取
s
1
x
(
t
)
=
[
−
5
,
0
]
T
,
s
1
y
(
t
)
=
[
0
,
0
]
T
,
s
1
z
(
t
)
=
[
0
,
0
]
T
s_{1x}(t)=[-5,0] ^T, s_{1y}(t)=[0,0]^T , s_{1z}(t)=[0,0]^T
s1x(t)=[−5,0]T,s1y(t)=[0,0]T,s1z(t)=[0,0]T
即跟随机始终在维持在距离领航机X负方向5m的地方。
（2）时变编队，
s
1
(
t
)
s_1(t)
s1(t)为变化值，我们可以构造跟随机在水平方向上绕领航机旋转的绕圆函数，圆的半径为
r
=
5
m
r=5m
r=5m，绕圆线速度为
v
1
=
1.1
m
/
s
v_1=1.1m/s
v1=1.1m/s，则绕圆角速度为
w
1
=
v
/
r
=
0.22
rad/s
w_1=v/r=0.22 \text{rad/s}
w1=v/r=0.22rad/s，则
s
1
(
t
)
s_1(t)
s1(t)在三维方向上可取
s
1
x
(
t
)
=
[
5
cos
(
0.22
t
)
,
−
1.1
sin
(
0.22
t
)
]
T
,
s
1
y
(
t
)
=
[
5
sin
(
0.22
t
)
,
1.1
cos
(
0.22
t
)
]
T
,
s
1
z
(
t
)
=
[
0
,
0
]
T
s_{1x}(t)=[ 5\text{cos}(0.22t),-1.1\text{sin}(0.22t)] ^T, s_{1y}(t)=[5\text{sin}(0.22t),1.1 \text{cos}(0.22t)] ^T, s_{1z}(t)=[0,0] ^T
s1x(t)=[5cos(0.22t),−1.1sin(0.22t)]T,s1y(t)=[5sin(0.22t),1.1cos(0.22t)]T,s1z(t)=[0,0]T
领航机和跟随机的初始状态可分别取为：
x
0
x
(
0
)
=
[
0
,
0
]
T
,
x
0
y
(
0
)
=
[
0
,
0
]
T
,
x
0
z
(
0
)
=
[
5
,
0
]
T
x
1
x
(
0
)
=
[
6
,
0
]
T
,
x
1
y
(
0
)
=
[
6
,
0
]
T
,
x
1
z
(
0
)
=
[
5
,
0
]
T
\begin{aligned} &x_{0x}(0)=[0,0]^T,x_{0y}(0)=[0,0]^T ,x_{0z}(0)=[5,0]^T\\ &x_{1x}(0)=[6,0]^T,x_{1y}(0)=[6,0]^T ,x_{1z}(0)=[5,0]^T \end{aligned}
x0x(0)=[0,0]T,x0y(0)=[0,0]T,x0z(0)=[5,0]Tx1x(0)=[6,0]T,x1y(0)=[6,0]T,x1z(0)=[5,0]T
领航机的期望飞行轨迹可设为一个边长为10m的正方形轨迹，最大速度为$v_{max}=
0.4
m
/
s
，
最
大
加
速
度
为
0.4m/s，最大加速度为
0.4m/s，最大加速度为a_{max}=0.4\text{m/s}$ ，首先可列出期望轨迹方程为：
z
(
t
k
+
1
)
=
{
fh
=
{
fhan
(
z
1
(
t
k
)
−
L
(
t
k
)
,
z
2
(
t
k
)
,
a
m
a
x
,
3
Δ
T
)
,
∣
v
d
0
(
t
k
)
∣
<
v
m
a
x
0
,
∣
v
d
0
(
t
k
)
∣
≥
v
m
a
x
z
1
(
t
k
+
1
)
=
z
1
(
t
k
)
+
z
2
(
t
k
)
Δ
T
z
2
(
t
k
+
1
)
=
z
2
(
t
k
)
+
fh
Δ
T
z(t_{k+1})=\left\{\begin{array}{l} \text{fh}=\left\{\begin{array}{l} \text{fhan} (z_{1}(t_k)-L(t_k),z_{2}(t_k),a_{max},3\Delta T),&|v_{d0}(t_k)|<v_{max} \\ 0,&|v_{d0}(t_k)|\geq v_{max} \end{array}\right.\\ z_{1}(t_{k+1})=z_{1}(t_k)+z_{2}(t_k)\Delta T\\ z_{2}(t_{k+1})=z_{2}(t_k)+\text{fh}\Delta T \end{array}\right.
z(tk+1)=⎩⎪⎪⎨⎪⎪⎧fh={fhan(z1(tk)−L(tk),z2(tk),amax,3ΔT),0,∣vd0(tk)∣<vmax∣vd0(tk)∣≥vmaxz1(tk+1)=z1(tk)+z2(tk)ΔTz2(tk+1)=z2(tk)+fhΔT
f 0 ( t k + 1 ) = [ f 0 x ( t k + 1 ) f 0 y ( t k + 1 ) f 0 z ( t k + 1 ) ] = z ( t k + 1 ) [ v e c x ( n ) v e c y ( n ) [ 0 , 0 ] T ] + [ [ p x ( n ) , 0 ] T [ p y ( n ) , 0 ] T [ 5 , 0 ] T ] , n = 0 , 1 , 2 , 3 f_{0}(t_{k+1})=\left[ \begin{matrix} f_{0x}(t_{k+1}) \\ f_{0y}(t_{k+1}) \\ f_{0z}(t_{k+1}) \end{matrix} \right]=z(t_{k+1})\left[ \begin{matrix} vec_x (n) \\ vec_y(n)\\ [0,0]^T \end{matrix} \right]+\left[ \begin{matrix} [p_{x}(n),0]^T \\ [p_{y}(n),0]^T \\ [5,0] ^T \end{matrix} \right],n=0,1,2,3 f0(tk+1)=⎣⎡f0x(tk+1)f0y(tk+1)f0z(tk+1)⎦⎤=z(tk+1)⎣⎡vecx(n)vecy(n)[0,0]T⎦⎤+⎣⎡[px(n),0]T[py(n),0]T[5,0]T⎦⎤,n=0,1,2,3
其中，
v
e
c
(
n
)
=
[
v
e
c
x
(
n
)
,
v
e
c
y
(
n
)
]
T
vec(n)=[vec_x(n),vec_y(n)]^T
vec(n)=[vecx(n),vecy(n)]T表示无人机在XY平面上每一条边的速度单位向量,
p
(
n
)
=
[
p
x
(
n
)
,
p
y
(
n
)
]
T
p(n)=[p_x(n),p_y(n)]^T
p(n)=[px(n),py(n)]T表示正方形每一个顶点处的位置坐标：
{
v
e
c
(
0
)
=
[
1
,
0
]
T
,
v
e
c
(
1
)
=
[
0
,
1
]
T
,
v
e
c
(
2
)
=
[
1
,
0
]
T
,
v
e
c
(
3
)
=
[
0
,
1
]
T
p
(
0
)
=
[
0
,
0
]
T
,
p
(
1
)
=
[
10
,
0
]
T
,
p
(
2
)
=
[
10
,
10
]
T
,
p
(
3
)
=
[
0
,
10
]
T
\left\{\begin{array}{l} vec(0)=[1,0]^T,vec(1)=[0,1]^T,vec(2)=[1,0]^T,vec(3)=[0,1]^T\\ p(0)=[0,0]^T,p(1)=[10,0]^T,p(2)=[10,10]^T,p(3)=[0,10]^T \end{array}\right.
{vec(0)=[1,0]T,vec(1)=[0,1]T,vec(2)=[1,0]T,vec(3)=[0,1]Tp(0)=[0,0]T,p(1)=[10,0]T,p(2)=[10,10]T,p(3)=[0,10]T
因为每一段边长需要的飞行时间为： T = 2 × v m a x / a m a x + ( 10 − v m a x 2 / a ) / v m a x = 12 s T=2×v_{max}/a_{max}+(10-v_{max}^2/a)/v_{max}=12 \text{s} T=2×vmax/amax+(10−vmax2/a)/vmax=12s，飞行策略可设为每飞12s停歇1s：
{ L ( t k ) = 10 , i f : 13 n ≤ t k < 13 n + 12 , n = 0 , 1 , 2 , 3 L ( t k ) = 0 , o t h e r w i s e \left\{\begin{array}{l} L(t_k)=10, if :13n\leq t_k<13n+12,n= 0,1,2,3\\ L(t_k)=0 ,otherwise \end{array}\right. {L(tk)=10,if:13n≤tk<13n+12,n=0,1,2,3L(tk)=0,otherwise
仿真结果如下：
（1）时不变跟踪仿真：
实飞实验开始前，需要先修改好相应的飞控程序及地面站程序，主要包括：
原点标定主要是为了确保每架无人机以某个相同的经纬度作为坐标原点，此外因为无人机都以北-东-地（NED）为XYZ正方向，这样无人机就能在同一个坐标系下工作运行了。具体方法为：
由于PX4飞控自带有设置home点功能，我们也可以将home点设置为参考原点，即给每架无人机都设置相同的home点：
void
MavlinkReceiver::handle_message_set_my_command(mavlink_message_t *msg)
{
/* command */
mavlink_set_my_command_t cmd_mavlink;
mavlink_msg_set_my_command_decode(msg, &cmd_mavlink);
struct vehicle_command_s vcmd;
vcmd.timestamp = hrt_absolute_time();
vcmd.param5 = NAN;
vcmd.param6 = NAN;
/* minimum pitch */
vcmd.param1 = NAN;
vcmd.param2 = NAN;
vcmd.param3 = NAN;
vcmd.param4 = NAN;
vcmd.param7 = NAN;
vcmd.target_system = 1;
vcmd.target_component = 1 ;
vcmd.source_system = 1;
vcmd.source_component =1;
vcmd.confirmation = false;
vcmd.from_external = true;
switch (cmd_mavlink.data1)
{
case 0://设置原点
vcmd.param1 = 1;
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_HOME;
break;
case 1://设置锁定
vcmd.param1 = 1;
vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM ;
break;
case 2://起飞
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
break;
case 3://降落
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
break;
...
}
可以在QGC飞控里添加相应的指令发送功能：
注释：我对QGC地面站的FlightDisplayView页面进行了一些改进，使之可以适合于多机状态显示和指令的发送。
我们关心的多机状态显示主要是姿态和位置：
关于领导者发送状态信息的问题，我们在构造这个mavlink消息时，需要考虑以下数据：
由此总共需要的字节数为2*6+3=15个字节，对应的mavlink消息结构体变量如下：
#define MAVLINK_MSG_ID_agent_state 272
MAVPACKED(
typedef struct __mavlink_agent_state_t {
int16_t x; /*< unit:dm 1E3*/
int16_t y; /*< unit:dm 1E3*/
int16_t z; /*< unit:dm 1E3*/
int16_t vx; /*< unit:cm/s 1E3*/
int16_t vy; /*< unit:cm/s 1E3*/
int16_t vz; /*< unit:cm/s 1E3*/
int8_t ax; /*< unit:dm/s 1E3*/
int8_t ay; /*< unit:dm/s 1E3*/
int8_t az; /*< unit:dm/s 1E3*/
}) mavlink_agent_state_t;
#define MAVLINK_MSG_ID_agent_state_LEN 15
实飞实验的步骤为：
在进行机间通信时，发现了一些错误：
无人机的编号（sysyid）必须和Xbee模块上的跟随机编号-MacId 一一对应，这一段主要体现在QGC代码中的：
static inline void select_des_address(mavlink_message_t* msg,uint8_t vehicleID)
{
switch (vehicleID)
{
case 1:
msg->addr64 = Addr_QuadRotor1;
break;
case 2:
msg->addr64 = Addr_QuadRotor2;
break;
default:
msg->addr64=0x000000000000FFFF;
break;
}
}
因此在新增通信模块时，一定要在地面站增加该模块的通信地址ID。在收到地面站开始领航跟踪指令后，领航机按照预定规划的路线飞行即可，而跟随机在位置控制器里要加入编队分量和领航机传递的状态分量 。
我们采用的是F450的机架（耐摔神机，如下）：
实际飞行视频已上传到腾讯视频：双机领航-跟踪实验视频
这次实验只做了时不变编队的领航-跟踪实验，尚未开展时不变编队实验。主要是发现跟随机的跟踪效果不是非常理想，在实验现场看起来较领航机要抖振的多，在查看飞行数据后，感觉主要原因在于：
后续计划考虑采用一些滤波器对跟踪机接收到的领航机数据进行滤波。