AirSim学习日志 9-三维空间航路点跟踪

之前已经使用Carrot-Chasing方法实现了二维平面内的航路点跟踪，但是为实现其控制，需利用二维平面内的几何关系，计算大量夹角，如果将该控制器推广到三维平面内，计算推导会更加复杂，而且直线与轴或平面的夹角也需要重新考虑。今天学习了一种新的控制方法，可以避免复杂的计算，也更加易于理解。

这一方法的参考来源为《多旋翼飞行器设计与控制》 P284 路径规划中的直线路径跟踪部分，其基本思想与Carrot-Chasing一样，将两个航路点连接成线，要求无人机跟踪这条直线。

1.控制原理

首先将多旋翼进行如下的建模：
{p˙=vv˙=u\left\{ \begin{aligned} \dot{p}=v \\\dot{v}=u \end{aligned} \right. {p˙=vv˙=u
p,v,u∈R3p,v,u\in \R^3p,v,u∈R3分别表示无人机的位置、速度和加速度。假设其要跟随的路径为Pwp,last−PwpP_{wp,last}-P_{wp}Pwp,last−Pwp，如下图所示，图片是书中讲解该部分的插图：

计算垂足Pwp,prepP_{wp,prep}Pwp,prep为
Pwp,prep=Pwp+(Pwp,last−Pwp)(P−Pwp)T(Pwp,last−Pwp)∣∣Pwp−Pwp,last∣∣2P_{wp,prep}=P_{wp}+(P_{wp,last}-P_{wp})\frac{(P-P_{wp})^T(P_{wp,last}-P_{wp})}{||P_{wp-P_{wp,last}}||^2} Pwp,prep=Pwp+(Pwp,last−Pwp)∣∣Pwp−Pwp,last∣∣2(P−Pwp)T(Pwp,last−Pwp)
则
KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ P-P_{wp,prep}&…
控制器输入为
u=−1k2satgd(k0P^wp+k1AP^wp,a0)−1k2vu=-\frac{1}{k_2}sat_{gd}(k_0\hat P_{wp}+k_1A\hat{P}_{wp},a0)-\frac{1}{k_2}v u=−k21satgd(k0P^wp+k1AP^wp,a0)−k21v
其中，P^wp=P−Pwp\hat P_{wp}=P-P_{wp}P^wp=P−Pwp，satgt(m,a)sat_{gt}(m,a)satgt(m,a)函数的作用为限制控制器输出幅度，其表示形式为
satgt(m,a)={m,if∣∣m∣∣∞≤aam∣∣m∣∣∞,if∣∣m∣∣∞>asat_{gt}(m,a)= \left\{ \begin{aligned} m,\quad&if\quad||m||_\infty \leq a \\a\frac{m}{||m||_\infty},\quad&if \quad||m||_\infty>a \end{aligned} \right. satgt(m,a)=⎩⎨⎧m,a∣∣m∣∣∞m,if∣∣m∣∣∞≤aif∣∣m∣∣∞>a
该控制器的稳定性可以通过李雅普诺夫稳定性证明，书中有详细证明。

这一控制器同时考虑到了三个因素：

一是无人机当前的速度，其符号与控制器输出的符号相反，则无人机当前速度与目标速度的方向差距越大，控制器的响应就越大；

二是无人机与目标航点的距离，即P^wp\hat{P}_{wp}P^wp，对应控制器参数k0k_0k0，该参数越大，无人机飞向目标航点的趋势就越大。

三是无人机与目标轨迹的距离，即AP^wp=P−Pwp,prepA\hat{P}_{wp}=P-P_{wp,prep}AP^wp=P−Pwp,prep，对应控制器参数k1k_1k1，该参数越大，无人机飞向目标轨迹的趋势就越大。

2.实现效果

设置参数k0=0,k1=1,k2=0.8k_0=0,k_1=1,k_2=0.8k0=0,k1=1,k2=0.8，初始状态为P=[1,1,1],v=[1,1,1]P=[1,1,1],v=[1,1,1]P=[1,1,1],v=[1,1,1]，跟踪直线为[0,0,0]−[10,15,15][0,0,0]-[10,15,15][0,0,0]−[10,15,15]，得到的效果如下，无人机直接飞向目标轨迹，飞向目标航点的趋势很小。

设置参数k0=1,k1=0,k2=0.8k_0=1,k_1=0,k_2=0.8k0=1,k1=0,k2=0.8，得到的效果如下，无人机飞向目标轨迹的趋势很小，直接飞向目标航点，在最后才与目标轨迹产生相交。

设置参数k0=1,k1=2,k2=0.8k_0=1,k_1=2,k_2=0.8k0=1,k1=2,k2=0.8，得到的效果如下。

为实现跟踪多个航路点组成的路径，增加参数δ\deltaδ，当Pwp,prepP_{wp,prep}Pwp,prep与PwpP_{wp}Pwp距离小于δ\deltaδ时，开始对下一个航路点进行跟踪。设置参数k0=1,k1=2,k2=0.8,δ=0.5k_0=1,k_1=2,k_2=0.8,\delta=0.5k0=1,k1=2,k2=0.8,δ=0.5，，得到的效果如下。

将代码修改后迁移到AirSim中，跟踪航路点为

points = [airsim.Vector3r(5, 0, -3),airsim.Vector3r(3, 10, -1),airsim.Vector3r(8, 12, -7),airsim.Vector3r(-5, 9, -2)]

设置参数为

K0=1.5, K1=4, K2=0.6, a0=1, delta=0.7

a0为控制器限幅参数，这里与二维的控制器一个很重要的不同点在于，二维要求固定飞行器速率，而这里无法设置飞行器速率，速度由控制器计算得到，我们只能进行控制输出的限幅。设置控制器限幅的范围越大，飞行器速度可能越大。

这里的跟踪效果如下：

还是实现了比较好的航路点跟踪，但是在zzz轴上的误差比较大。这与四旋翼控制方法是有关的，由于控制器输出为加速度，而AirSim中没有相关的接口，这里依然使用了DLQR方法中用过的加速度控制方法，需先通过加速度解算出位姿，再通过AirSim中的姿态角控制接口进行控制。但是AirSim中提供的相关接口不能实现zzz轴方向的加速度控制，在zzz轴上只能进行位置控制或高度油门控制。这里采用了简单的高度位置控制，认为目标高度为

z_cmd = P[2] + (V[2] + U_cmd[2] * dt) * dt

然后多次尝试、设置合理的控制器参数k2k_2k2，使四旋翼轨迹与目标轨迹的误差尽可能小。

3.代码

python代码实现：

def move_by_path_3d(P, V, Path, K0=2, K1=2, K2=1, dt=0.1, a0=2, delta=0.5):def distance(A, B):return math.sqrt((A[0] - B[0]) ** 2 +(A[1] - B[1]) ** 2 +(A[2] - B[2]) ** 2)P = np.matrix(P).Tpos_record = [P]e_record = []V = np.matrix(V).TI3 = np.matrix([[1, 0, 0],[0, 1, 0],[0, 0, 1]])for i in range(len(Path) - 1):Wa = np.matrix(Path[i]).TWb = np.matrix(Path[i + 1]).TA = I3 - (Wa - Wb).dot((Wa - Wb).T) / (distance(Wa, Wb) ** 2)Pt = P - Wbe = np.linalg.norm(A.dot(Pt))e_record.append(e)d = np.linalg.norm(Pt - A.dot(Pt))print('i,', 'Start:', Path[i], ',Aim:', Path[i + 1])print('\tP:', P, 'V:', V, 'e:', e)while d >= delta:Pt = P - WbU1 = K0 * Pt + K1 * A.dot(Pt)if np.linalg.norm(U1, ord=np.inf) > a0:U1 = U1 * a0 / np.linalg.norm(U1, ord=np.inf)U = -(U1 + V) / K2V = V + U * dtP = P + V * dte = np.linalg.norm(A.dot(Pt))e_record.append(e)d = np.linalg.norm(Pt - A.dot(Pt))print('\tP:', P, 'V:', V, 'e:', e)pos_record.append(P)pos_record = np.array(pos_record).T[0]path_plot = np.array(Path).Tax = plt.subplot(projection='3d')ax.plot(path_plot[0], path_plot[1], path_plot[2])ax.plot(pos_record[0], pos_record[1], pos_record[2], '--')plt.show()plt.plot(e_record)plt.show()if __name__ == "__main__":Path = [[0, 0, 0], [10, 15, 15], [15, 20, 20], [20, 5, 5]]move_by_path_3d([1, 1, 1], [1, 1, 1], Path, K0=1, K1=2, K2=0.8)

迁移至AirSim中：

def get_state(client):# 获取无人机状态DIG = 6State = client.getMultirotorState()kinematics = State.kinematics_estimatedstate = {"timestamp": str(State.timestamp),"position": [round(ele, DIG) for i, ele inenumerate(kinematics.position.to_numpy_array().tolist())],"orientation": [round(i, DIG) for i in airsim.to_eularian_angles(kinematics.orientation)],"linear_velocity": [round(i, DIG) for i in kinematics.linear_velocity.to_numpy_array().tolist()],"linear_acceleration": [round(i, DIG) for i in kinematics.linear_acceleration.to_numpy_array().tolist()],"angular_velocity": [round(i, DIG) for i in kinematics.angular_velocity.to_numpy_array().tolist()],"angular_acceleration": [round(i, DIG) for i in kinematics.angular_acceleration.to_numpy_array().tolist()]}return statedef move_by_acceleration_horizontal(client, ax_cmd, ay_cmd, az_cmd, z_cmd, duration=1):# 读取自身yaw角度state = get_state(client)angles = state['orientation']yaw_my = angles[2]g = 9.8  # 重力加速度sin_yaw = math.sin(yaw_my)cos_yaw = math.cos(yaw_my)A_psi = np.array([[cos_yaw, sin_yaw], [-sin_yaw, cos_yaw]])A_psi_inverse = np.linalg.inv(A_psi)angle_h_cmd = 1 / (g - az_cmd) * np.dot(A_psi_inverse, np.array([[ax_cmd], [ay_cmd]]))theta = math.atan(angle_h_cmd[0, 0])phi = math.atan(angle_h_cmd[1, 0] * math.cos(theta))# client.moveToZAsync(z_cmd, vz).join()client.moveByRollPitchYawZAsync(phi, theta, 0, z_cmd, duration).join()def move_by_path_3d(client, Path, K0=1.5, K1=4, K2=0.6, dt=0.5, a0=1, delta=0.7):def distance(A, B):return math.sqrt((A[0] - B[0]) ** 2 +(A[1] - B[1]) ** 2 +(A[2] - B[2]) ** 2)state = get_state(client)P = state['position']V = state['linear_velocity']Wb = PWb_m = np.matrix(Wb).TP_m = np.matrix(P).TV_m = np.matrix(V).TI3 = np.matrix([[1, 0, 0],[0, 1, 0],[0, 0, 1]])for i in range(len(Path)):Wa = WbWb = [Path[i].x_val, Path[i].y_val, Path[i].z_val]Wa_m = Wb_mWb_m = np.matrix(Wb).TA = I3 - (Wa_m - Wb_m).dot((Wa_m - Wb_m).T) / (distance(Wa_m, Wb_m) ** 2)Pt = P_m - Wb_me = np.linalg.norm(A.dot(Pt))d = np.linalg.norm(Pt - A.dot(Pt))print('i,', i, 'Start:', Wa, ',Aim:', Wb)print('\tP:', P, 'V:', V, 'e:', e)while d >= delta or \(i == len(Path) - 1and ((P[0] - Wb[0]) * (Wb[0] - Wa[0]) < 0or (P[1] - Wb[1]) * (Wb[1] - Wa[1]) < 0or (P[2] - Wb[2]) * (Wb[2] - Wa[2]) < 0)):Pt = P_m - Wb_mU1 = K0 * Pt + K1 * A.dot(Pt)if np.linalg.norm(U1, ord=np.inf) > a0:U1 = U1 * a0 / np.linalg.norm(U1, ord=np.inf)U = -(U1 + V_m) / K2U_cmd = np.array(U)[:, 0]z_cmd = P[2] + (V[2] + U_cmd[2] * dt) * dtmove_by_acceleration_horizontal(client, U_cmd[0], U_cmd[1], U_cmd[2], z_cmd, dt)e = np.linalg.norm(A.dot(Pt))d = np.linalg.norm(Pt - A.dot(Pt))print('\tP:', P, 'V:', V, 'e:', e, 'U:', U_cmd)# 画图plot_p1 = [airsim.Vector3r(P[0], P[1], P[2])]state = get_state(client)P = state['position']V = state['linear_velocity']P_m = np.matrix(P).TV_m = np.matrix(V).Tplot_p2 = [airsim.Vector3r(P[0], P[1], P[2])]client.simPlotArrows(plot_p1, plot_p2, arrow_size=8.0, color_rgba=[0.0, 0.0, 1.0, 1.0])client.simPlotLineStrip(plot_p1 + plot_p2, color_rgba=[1.0, 0.0, 0.0, 1.0], is_persistent=True)client = airsim.MultirotorClient()  # 创建连接
client.confirmConnection()          # 检查连接
client.reset()
client.enableApiControl(True)       # 获取控制权
client.armDisarm(True)              # 电机启转
client.takeoffAsync().join()        # 起飞
client.moveToZAsync(-3, 1).join()   # 上升到3米高度
client.simSetTraceLine([1, 0, 0, 1], thickness=5)
client.simFlushPersistentMarkers()  # 清空画图# 三维航路点跟踪
points = [airsim.Vector3r(5, 0, -3),airsim.Vector3r(3, 10, -1),airsim.Vector3r(8, 12, -7),airsim.Vector3r(-5, 9, -2)]
client.simPlotPoints(points, color_rgba=[0, 1, 0, 1], size=30, is_persistent=True)
client.simPlotLineStrip(points, color_rgba=[0, 1, 0, 1], thickness=5, is_persistent=True)
move_by_path_3d(client, points)