基于图像的无人机追踪1 - 卡尔曼估计、基于势场导数的追踪控制算法、airsim仿真
文章目录
- 1. 简介
- 2. airsim平台的搭建
- 3. 分割图的读取
- 3.1 相机与图片类型
- 3.2 img api拍摄图片
- 4. 使用分割图初步估计目标的距离和方位，反推出坐标
- 4.1 距离估计
- 4.2 方位估计
- 4.3 反推坐标
- 5. 线性卡尔曼状态估计
- 5.1 状态方程
- 5.2 输出方程
- 5.3 卡尔曼五公式
- 5.4 matlab仿真
- 6. 控制算法设计
- 6.1 控制算法
- 6.2 airsim中的无人机控制函数api总结
- 6.2.1 api与airsim通信
- 6.2.2 无人机控制api
- 6.2.3 无人机获取状态api
- 6.2.4 图片api相关函数
- 6.2.5 Python相关
- 6.2.6 airsim相关小函数
- 7. 最后效果
1. 简介
这是一个初步的无人机追踪效果。具体工作是在airsim中仿真实现的，直接通过分割图来观测无人机的状态，然后通过卡尔曼来估计目标无人机的状态，最后通过追踪控制算法来控制无人机去追踪目标无人机。
工作共分为：
- airsim平台搭建与设置
- 分割图的读取，同时在分割图中识别目标无人机
- 从分割图估计观测量（通过拟合的方法）
- 线性卡尔曼公式推导
- 控制算法的设计
2. airsim平台的搭建
airsim平台的搭建可以看我之前的两篇博客：
- airsim手动控制无人机-搭建过程
- win10-airsim-Unreal 安装配置教程(踩坑)
这次项目使用经典的"Landscape Mountains"环境，“settings”文件见资源。
在settings文件中，设置2号无人机在1号无人机前面5~10米的位置。高度都设为0.
这次使用的是python api。
3. 分割图的读取
3.1 相机与图片类型
我使用的是airsim自带的simple flight，每个无人机自带5个相机，其ID 与相机分别对应如下：
0 = front_center
1 = front_right
2 = front_left
3 = bottom_center
4 = back_center
每个相机都可以拍摄8种类型的图片，对应关系如下：
0 = Scene彩色图
1 = DepthPlanner深度图
2 = DepthPerspective
3 = DepthVis
4 = DisparityNormalized
5 = Segmentation分割图
6 = SurfaceNormals三维重建图
7 = Infrared红外图
这次项目，我使用的是0号相机，拍摄分割图。通过0号相机拍摄的分割图来实现无人机抓捕。
3.2 img api拍摄图片
使用分割图的目的是为了跳过目标检测的部分，就假设已经可以实现目标检测了。所以分割图中，要把除了无人机其他所有的物体都设为同一个颜色，这样就能明显地把无人机区别出来。所用到的函数有：
found = client.simSetSegmentationObjectID("[\w]*", 12, True)
found = client.simSetSegmentationObjectID("UAV[\w]",19,True)
上面的代码第一行，将所有的物体的分割图id设置为12；第二行将所有的无人机分割ID设为19。每个ID对应一种颜色，具体是什么颜色，要参考这个列表。我选用12和19是因为这两个颜色差别还算比较大。
读取图像使用img api，部分代码如下：
imgs = client.simGetImages([
airsim.ImageRequest("0", airsim.ImageType.Segmentation, False, False)], vehicle_name = "UAV1")
img1d = np.fromstring(imgs[0].image_data_uint8, dtype=np.uint8) # get numpy array
img_rgb = img1d.reshape(imgs[0].height, imgs[0].width, 3) # reshape array to 3 channel image array H * W * 3
img_gray = cv2.cvtColor(img_rgb,cv2.COLOR_BGRA2GRAY) # gray image
最后可以把分割图变换为灰度图，并保存在矩阵中，以供后面的程序使用。
4. 使用分割图初步估计目标的距离和方位，反推出坐标
这一步是使用分割图像，得出目标相对我的距离和方位（ r , θ b , θ z r,\theta_b,\theta_z r,θb,θz）
4.1 距离估计
距离估计很简单，使用近大远小的原则，首先得到一个实验测量的表格，然后对测量得到的数据做拟合。具体就是测量在分割图中，飞机的长边占了几个像素，每相隔1米测量一次，总共测量了2~30米，每个距离都对应一个像素长度。
得到表格后，通过matlab自带的拟合工具，对数据进行拟合，最后我发现，有一个函数能够很好地拟合这个情况下的数据： y = a e b x + c e d x y=ae^{bx}+ce^{dx} y=aebx+cedx。
4.2 方位估计
3维空间的方位角有两个，我的定义是 θ b \theta_b θb和 θ z \theta_z θz。
首先定义相机坐标系：x右y下z前。 θ b \theta_b θb是目标点投影到xy平面内，其与x轴的夹角。 θ z \theta_z θz是目标与z轴的夹角。
所以 θ b \theta_b θb的取值范围是 [ 0 , 2 π ) [0,2\pi) [0,2π)，如果相机的视角是90度，那么 θ z \theta_z θz的取值范围是 [ 0 , π 4 ] [0,\dfrac{\pi}{4}] [0,4π]。
从定义可以得出两个角度的公式（这里的xyz是相机坐标系下的距离，单位是米）：
θ b = arctan ( y x ) \theta_b = \arctan(\dfrac{y}{x}) θb=arctan(xy)
θ z = arctan ( x 2 + y 2 z ) \theta_z = \arctan(\dfrac{\sqrt{x^2+y^2}}{z}) θz=arctan(zx2+y2)
所以 θ b \theta_b θb可以直接求取，因为xy换算成像素，是等比的。
而 θ z \theta_z θz就需要通过实验，得出表格，再拟合了。
4.3 反推坐标
根据定义，可以直接通过 r , θ b , θ z r,\theta_b,\theta_z r,θb,θz反推出目标无人机在相机坐标系下的坐标，公式如下：
x = r sin ( θ z ) cos ( θ b ) y = r sin ( θ z ) sin ( θ b ) z = r c o s ( θ z ) \begin{aligned} x &= r\sin(\theta_z)\cos(\theta_b) \\ y &= r\sin(\theta_z)\sin(\theta_b) \\ z & = rcos(\theta_z) \end{aligned} xyz=rsin(θz)cos(θb)=rsin(θz)sin(θb)=rcos(θz)
5. 线性卡尔曼状态估计
首先解释一下为什么使用线性卡尔曼。如果把距离和方位当做观测量，输出方程是非线性的，这个时候可以使用扩展卡尔曼，来线性化输出方程。刚开始我的确是这么做的，但是出现了奇点，当目标无人机在视野正中央的时候，输出矩阵就无法线性化了。然后使用线性卡尔曼的时候，发现效果还是不错的，所以这次就使用的线性卡尔曼状态估计。
5.1 状态方程
首先状态量的选取为： x , v x , y , v y , z , v z x, v_x, y, v_y, z, v_z x,vx,y,vy,z,vz，这是目标无人机在相机坐标系下的位置的速度。
则状态方程为：
X ( k + 1 ) = d i a g ( f , f , f ) X ( k ) \bm{X}(k+1)= diag(f,f,f)\bm{X}(k) X(k+1)=diag(f,f,f)X(k)
其中， f = [ 1 δ t 0 1 ] \bm{f} = \left[ \begin{matrix} 1 & \delta t \\ 0 & 1 \end{matrix}\right] f=[10δt1]， X ^ = [ x ^ v ^ x y ^ v ^ y z ^ v ^ z ] T \hat{\bm{X}}= \left[ \begin{matrix} \hat{x}& \hat{v}_x & \hat{y} & \hat{v}_y & \hat{z} & \hat{v}_z \end{matrix}\right]^T X^=[x^v^xy^v^yz^v^z]T.
5.2 输出方程
将观测量设置与状态量相同，所以输出矩阵 H = I \bm{H} = \bm{I} H=I就是单位阵。
观测量由4.3节给出，同时加上速度的观测量：
v x ( t ) = x ( t ) − x ( t − 1 ) d t v y ( t ) = y ( t ) − y ( t − 1 ) d t v z ( t ) = z ( t ) − z ( t − 1 ) d t \begin{aligned} v_x(t) &= \dfrac{x(t)-x(t-1)}{dt} \\ v_y(t) &= \dfrac{y(t)-y(t-1)}{dt} \\ v_z(t) &= \dfrac{z(t)-z(t-1)}{dt} \end{aligned} vx(t)vy(t)vz(t)=dtx(t)−x(t−1)=dty(t)−y(t−1)=dtz(t)−z(t−1)
5.3 卡尔曼五公式
X ^ k − = F X ^ k − 1 P k − = F P k − 1 F T + Q K k = P k − H T ( F P k − H T + R ) − 1 X ^ k = X ^ k − + K k [ M k − H X ^ k − ] P k = ( I − K k H ) P k − \begin{aligned} \hat{X}^-_k&=F\hat{X}_{k-1} \\ P^-_k&=FP_{k-1}F^T+Q \\ K_k&=P^-_kH^T(FP^-_kH^T+R)^{-1} \\ \hat{X}_k&=\hat{X}^-_k+K_k[M_k-H\hat{X}_k^-] \\ P_k&=(I-K_kH)P^-_k \end{aligned} X^k−Pk−KkX^kPk=FX^k−1=FPk−1FT+Q=Pk−HT(FPk−HT+R)−1=X^k−+Kk[Mk−HX^k−]=(I−KkH)Pk−
5.4 matlab仿真
使用的随机加速度，蓝色曲线是目标无人机的位置移动曲线，红色是估计的位置曲线，可以看到，估计的基本正确。
6. 控制算法设计
6.1 控制算法
最后的目标是追踪目标无人机，而且要以相同的速度与目标无人机保持固定的距离。也就是说，当我距离目标无人机比较远的时候，就以我的最大速度（大于目标无人机的速度）去追赶，当达到期望的距离的时候，就以相同的速度保持速度一致，距离不变。
这个目标跟多机协同的很类似，所以我设计了类似于多机协同的控制算法。
控制共分为三部分：水平速度控制、高度控制、偏航角控制。
- 水平速度控制：
v r o w = k p ( ∣ Δ p ∣ − d i s s h o o t ) Δ p + k v Δ v v = v r o w min { m a x v , ∣ v r o w ∣ } ∣ v r o w ∣ \begin{aligned} \bm{v}_{row} &= k_p(|\Delta \bm{p}| - dis_{shoot})\Delta \bm{p}+k_v\Delta v \\ \bm{v} & = \dfrac{\bm{v}_{row} \min \{max_v,|\bm{v}_{row}| \}}{|\bm{v}_{row}|} \end{aligned} vrowv=kp(∣Δp∣−disshoot)Δp+kvΔv=∣vrow∣vrowmin{maxv,∣vrow∣} - 高度控制：
h = Δ h min { m a x v h , ∣ Δ h ∣ } ∣ Δ h ∣ + h m e h = \dfrac{\Delta h \min\{max_{vh}, |\Delta h|\}}{|\Delta h|}+h_{me} h=∣Δh∣Δhmin{maxvh,∣Δh∣}+hme - 偏航角控制：
y a w = Δ y a w min { m a x v y a w , ∣ Δ y a w ∣ } ∣ Δ y a w ∣ + y a w m e yaw = \dfrac{\Delta yaw \min\{max_{vyaw}, |\Delta yaw|\}}{|\Delta yaw|}+yaw_{me} yaw=∣Δyaw∣Δyawmin{maxvyaw,∣Δyaw∣}+yawme
需要说明的是：airsim中的偏航角控制会有超调，而超调对卡尔曼的估计有非常大的影响，因为我们的算法仅适合观测相机不运动的情况，所以，一定要把最大偏航角速度设置的小一些。
6.2 airsim中的无人机控制函数api总结
6.2.1 api与airsim通信
-
python api 与airsim连接
client = airsim.MultirotorClient()
cient.confirmConnection()
6.2.2 无人机控制api
-
初始化UAV
client.enableApiControl(True,name)
client.armDisarm(True,name)
-
控制行为 - 起飞
client.takeoffAsync().join()
join()会等待当前任务完成，才继续往下执行代码
-
控制行为 - 悬停
client.hoverAsync().join()
-
控制指令 - 航路点模式
client.moveToPositionAsync(x, y, z, -5).join()
第四个数字是速度，不能设置的太大，如果速度设置的太大，会死机的
-
控制指令 - 虚拟摇杆
client.moveByManualAsync( vx_max = 1E6, vy_max = 1E6, z_min = -1E6, duration = 1E10)
client.moveByRC(rcdata = airsim.RCData( roll = 0.5, pitch = 0.0, throttle = 1.0, is_initialized = True, is_valid = True))
-
控制指令 - 虚拟摇杆模式
client.moveByAngleThrottleAsync().join()
-
控制指令 - 速度模式
client.moveByVelocityAsync(vx, vy, vz, duration).join()
-
控制指令 - 2维速度模式
client.moveByVelocityZAsync(vx, vy, z, duration).join()
-
控制函数的参数：drivetrain, yaw_mode
-
所有的控制指令都可以添加两个参数（drivetrain, yaw_mode）
-
drivetrain = airsim.DrivetrainType.MaxDegreeOfFreedom，独立控制飞机的yaw角
drivetrain = airsim.DrivetrainType.ForwardOnly，飞机的yaw角一直保持向前
-
yaw_mode = airsim.YawMode( is_rate = False, yaw_or_rate = theta) , yaw角度为theta度
yaw_mode = airsim.YawMode( is_rate = True, yaw_or_rate = phi) , yaw角速度为phi度/秒
-
注意：drivetrain为ForwardOnly时，yaw_mode.is_rate不能为True, 不然是矛盾的。
-
-
6.2.3 无人机获取状态api
-
获取状态
state = client.getMultirotorState(vehicle_name=“UAV1”)
s = pprint.pformat(state)
print(“state: %s” %s)
-
获取状态 - 位置
quad_pos = client.getMultirotorState().kinematics_estimated.position
-
获取状态 - 速度
quad_vel = client.getMultirotorState().kinematics_estimated.linear_velocity
-
获取状态 - 真实状态(相对于飞机起始点的状态)
state = client.simGetGroundTruthKinematics(vehicle_name = “UAV1”)
state.position.x_val( y_val, z_val)（float类型）
state.linear_velocity.x_val（float类型）
-
设置无人机位置（会将airsim设置为computer vision模式， 是的api无法控制无人机)
client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(x, 0, -2), airsim.to_quaternion(0, 0, 0)), True)
6.2.4 图片api相关函数
-
设置 - 分割图像号
found = client.simSetSegmentationObjectID( “UAV[\w]”, 19, True)
所有的无人机，19=[0, 53, 65], 12=[242, 107, 146]
-
设置 - 相机的姿态
client.simSetCameraOrientation(0, airsim.to_quaternion(0.261799, 0, 0))
其中，airsim.to_quaternion是将pitch、roll、yaw变为四元数。
本例的功能是将0号相机的pitch设为15度。
6.2.5 Python相关
-
暂停
import time
time.sleep(2) 暂停两秒
-
文件夹路径
import os
dir = os.getcwd() 读取当前文件夹路径
final_dir = os.path.join(dir,“picture”) 合并文件夹路径
os.makedirs(final_dir) 创建文件夹
-
opencv
import cv2
-
矩阵运算
from numpy import *
import numpy as np
pos_ori = np.mat([-3,0],[0,0]) 创建矩阵
6.2.6 airsim相关小函数
-
欧拉角转四元数
airim.to_quaternion(0.261799, 0, 0)
-
向量构造
airsim.Vector3r(0, 0, 0)
-
simSetVehiclePose函数只适用于computer vison mode，如果是无人机模式的话，会固定死无人机的位置，是的api控制失效
7. 最后效果
添加不了视频，就放一张截图吧
