基于PX4飞控的双机领航-跟踪的理论分析与实验验证_px4编队飞行

双机领航-跟踪是最简单的无人机分布式控制，是实现大规模集群分布式控制的基础，本文主要记录对如何实现双机跟踪护航控制的思考以及验证实验。

一、理论基础

无人机的位置控制模型可以建立为：
{ 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=[00​10​]B=[01​]

领航机和跟随机的相关变量可分别用下标{0,1}来标记，领航机的控制协议$u_0$不受跟随机影响，仅仅受期望路线和自身状态反馈影响，假设其期望飞行轨迹为$f_0(t)=[p_{d0}(t),v_{d0}(t){]}^T$，那么其控制协议可以设计为：
$$u_0=K(f_0(t)-x_0)+{\dot{v}}_{d0}\text{\hspace{1em}(1)}$$

其中$K=[k_1,k_2]$，只要保证参数$k_1,k_2$均是正数，即可确保领航机对轨迹$f_0(t)$的跟踪。

对于跟随机而言，由于要跟踪领航机，其期望的位置应当为相对于领航机的某一位置，可以表示为：
$$f_1(t)=s_1(t)+x_0(t)$$

其中$s_1(t)=[p_{s1}(t),v_{s1}(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_{1x}(t)=[-5,0{]}^T,s_{1y}(t)=[0,0{]}^T,s_{1z}(t)=[0,0{]}^T$$

即跟随机始终在维持在距离领航机X负方向5m的地方。

（2）时变编队，$s_1(t)$为变化值，我们可以构造跟随机在水平方向上绕领航机旋转的绕圆函数，圆的半径为$r=5m$，绕圆线速度为$v_1=1.1m/s$，则绕圆角速度为$w_1=v/r=0.22\text{rad/s}$，则$s_1(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$$

领航机和跟随机的初始状态可分别取为：
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.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​)∣≥vmax​​z1​(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

其中，$vec(n)=[ve{c}_x(n),ve{c}_y(n){]}^T$表示无人机在XY平面上每一条边的速度单位向量, $p(n)=[p_x(n),p_y(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\times v_{max}/a_{max}+(10-v_{max}^2/a)/v_{max}=12\text{s}$，飞行策略可设为每飞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）时不变跟踪仿真：

图1 时不变跟踪仿真 （2）时变跟踪仿真：

图2 时变编队跟踪仿真

三、实飞实验

实飞实验开始前，需要先修改好相应的飞控程序及地面站程序，主要包括：

• 原点标定
• 多机主要状态显示
• 领导者发送状态信息到跟随者
• 跟随者接收信息的测试

原点标定主要是为了确保每架无人机以某个相同的经纬度作为坐标原点，此外因为无人机都以北-东-地（NED）为XYZ正方向，这样无人机就能在同一个坐标系下工作运行了。具体方法为：

• 在飞行场地随便选择一个点作为候选原点
• 取一架无人机在此点处开机，待收到GPS信号后，地面站发送指令设置该点经纬度为参考原点的经纬度；
• 其它无人机也在此点开机，并将该点设置为原点。

由于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;		
             ...
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

可以在QGC飞控里添加相应的指令发送功能：

图3 设置原点

注释：我对QGC地面站的FlightDisplayView页面进行了一些改进，使之可以适合于多机状态显示和指令的发送。

我们关心的多机状态显示主要是姿态和位置：

图4 主要状态显示

关于领导者发送状态信息的问题，我们在构造这个mavlink消息时，需要考虑以下数据：

• 自身位置数据： x,y,z ，可取单位为m，精确到小数点后1位，即分米级(室外GPS定位大致也就这个级别的精度)，我们可以用16位int型两字节数据来表示，可表示的范围为：-3276.3m～3276.3米，即我们初步考虑无人机的飞行范围不超过以原点为圆心，半径为3276.3m的球型区域（对于实验来说，这个范围应当已经足够了）；
• 自身速度数据，Vx,Vy,Vz，同样的可取单位为m，精确到小数点后2位，即厘米级，如果用8位一个字节存储，表示范围只能到-1.27～1.27m/s，显然不够，所以最低也只能两个字节，可表示范围为-327.63m/s~327.63m/s
• 加速度数据，这个可以由领导者的编队指令给出，也应包括x,y,z，但我们可只精确到分米级，所以仅用一个字节的数据即可，范围为-12.7~12.7m/s

由此总共需要的字节数为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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

实飞实验的步骤为：

• 两架无人机同时起飞至悬停状态（在同时到达悬停状态之前，不启动领导-跟随模式）
• 领导者可一直发送自身状态信息给跟随者
• 跟随者检测是否收到领导者信息，若检测到，可发信息给地面站，使能领导-跟随模式
• 地面站选择时不变编队跟踪模式，领导者开始绕矩形轨迹运动，跟随者则始终在距其X方向-5m处进行跟踪，矩形绕完后保持悬停
• 点击降落命令，无人机开始降落

在进行机间通信时，发现了一些错误：
无人机的编号（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;

        }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

因此在新增通信模块时，一定要在地面站增加该模块的通信地址ID。在收到地面站开始领航跟踪指令后，领航机按照预定规划的路线飞行即可，而跟随机在位置控制器里要加入编队分量和领航机传递的状态分量 。

我们采用的是F450的机架（耐摔神机，如下）：

图5 实验无人机

实际飞行视频已上传到腾讯视频：双机领航-跟踪实验视频

这次实验只做了时不变编队的领航-跟踪实验，尚未开展时不变编队实验。主要是发现跟随机的跟踪效果不是非常理想，在实验现场看起来较领航机要抖振的多，在查看飞行数据后，感觉主要原因在于：

• 由领航机发送过来的位置、速度状态数据对于跟随机来说是非平滑的轨迹曲线，既有自身测量噪声的原因，也有间歇通信（毕竟只能25Hz的频率传输数据）的原因，而领航机的轨迹是自己用轨迹生成函数自动生成的，不会存在不平滑的问题。所以跟随机的飞行效果要远逊于领航机。

后续计划考虑采用一些滤波器对跟踪机接收到的领航机数据进行滤波。