使用MPC算法控制小船运动_船舶mpc控制-CSDN博客

本文链接：https://blog.csdn.net/weixin_50467818/article/details/139471949

场景

假设小船以恒定的速度10m/s在水面上航向，不考虑风流浪等因素，速度方向始终和船艏向一致，不考虑运动姿态和动力学等因素，预设一条计划航线(在航海航线设计中，设计的航线是一段一段的直线)，要求小船在模型预测控制算法(Model predictive control，MPC)下行驶在计划航线上。

参数

小船初始位置(X=0,Y=0)，初始速度v=10m/s，船艏向C=90°，步长t=0.1s，最大转向速度10°/s

MPC模块

MPC利用一个已有的模型、系统当前的状态和未来的控制量，来预测系统未来的输出，然后与我们期望的系统输出做比较，得到代价函数，通过优化的方法，优化出未来控制量，使得代价函数最小。优化出来的控制量即算法的输出。

这里使用的是目标函数：是使小船实时航向与目标航向差最小和小船实时位置点与计划航线距离最小，约束条件是：最大最小转向速率。

def cost_function(u, *args):
    current_x, current_y, current_c, current_v, Tc, k, b = args
    #
    planned_c = current_c + u[0] * dt
    planned_x = current_x + current_v * dt * sin(planned_c)
    planned_y = current_y + current_v * dt * cos(planned_c)
    #
    dis0 = abs(k * planned_x - planned_y + b) / sqrt(k ** 2 + 1)
    dtc0 = abs(planned_c - Tc)
    cost = dis0 + dtc0
    return cost

计算转向角度模块

# 计算航向角(-180-180°), 其实应该是(000-360°)但是在控制航向时会有限制
def calcu_azimuth(x1, y1, x2, y2):
    dx = x2 - x1
    dy = y2 - y1
    if (dx >= 0) and (dy > 0):
        angle0 = atan(dx / dy)               # tan是[-90,90],第1象限负值,所以是000+(+a)
    elif (dx >= 0) and (dy < 0):
        angle0 = pi + atan(dx / dy)          # tan是[-90,90],第2象限负值,所以是180+(-a)
    elif (dx <= 0) and (dy < 0):
        angle0 = pi - atan(dx / dy)          # tan是[-90,90],第3象限正值,所以是180-(+a)
    elif (dx <= 0) and (dy > 0):
        angle0 = 0 + atan(dx / dy)           # tan是[-90,90],第4象限负值,所以是000+(-a)
    else:
        angle0 = 0
    return angle0

这里需要计算出连续两个关键转向点之间的角度，后续根据船舶实际位置距离关键转向点之间的距离来判断，船舶目标航向，以此来控制船舶安装计划航向航行在预定航线上。

转向角模块

关键转向点的设计要求是小于90°，这样才符合航海实践要求，转向角超过90°船舶需要较长的转向时间，且不安全。

# 预设航线的关键转向点
xx = [0, 400, 600, 700, 1000]
yy = [0, 100, 300, 600, 1000]

# 计算出转向点的角度angle
angles = []
for i in range(len(xx)-1):
    angle = calcu_azimuth(xx[i], yy[i], xx[i + 1], yy[i + 1])
    angles.append(angle)
    print(angle * 180 / pi)

上面给出的5个关键转向点的航向分别是 076°、045°、018°、037°，转向角为14°、31°、27°、19°转向角度皆小于90°，符合航海实际要求。