基于LOS导引律的无人船路径点跟踪控制考虑横向误差以及船舶

基于LOS导引律的无人船路径点跟踪控制考虑横向误差以及船舶

基于LOS导引律的无人船路径点跟踪控制
考虑横向误差以及船舶操纵性
MATLAB编程实现，mmg模型
无人船模型为KVLCC2

以下文字及示例代码仅供参考

1. 背景与问题

无人船（USV）在执行测绘、巡逻、搜救等任务时，需要高精度的路径点跟踪能力。传统 PID 航向控制往往忽略横向误差与船舶操纵性，导致在风浪中跟踪精度下降。本文采用经典 LOS（Line-of-Sight）导引律，显式引入横向误差 e 与船舶操纵性模型（MMG/KVLCC2），给出可直接落地的 C++ 与 Python 实现，方便后续嵌入式移植或 ROS 部署。

2. 系统模型

2.1 三自由度运动学（NED 坐标系）

x

˙

=

u

cos

⁡

ψ

−

v

sin

⁡

ψ

y

˙

=

u

sin

⁡

ψ

+

v

cos

⁡

ψ

ψ

˙

=

r

begin{aligned} dot{x} &= ucospsi – vsinpsi dot{y} &= usinpsi + vcospsi dot{psi} &= r end{aligned}

x˙y˙​ψ˙​​=ucosψ−vsinψ=usinψ+vcosψ=r​

• $(x,y,\psi )$：位置与艏向
• $(u,v,r)$：纵荡、横荡、艏摇速度

2.2 MMG 简化动力学

仅展示艏摇通道（用于舵角

δ

delta

δ 控制）：

T

r

˙

+

r

=

K

δ

+

d

Tdot{r} + r = Kdelta + d

Tr˙+r=Kδ+d

• $T,K$：船舶操纵性指数
• $d$：风浪扰动（可扩展观测器）

3. 横向误差计算

给定路径点序列

P

k

=

(

x

k

,

y

k

)

P_k=(x_k,y_k)

Pk​=(xk​,yk​)，当前船位

P

=

(

x

,

y

)

P=(x,y)

P=(x,y)：

1. 计算投影点到当前线段 $P_k{P}_{k+1}$ 的距离 $e$（横向误差）
2. 计算 LOS 期望艏向 ${\psi }_{\text{LOS}}$

LOS 导引律

ψ

LOS

=

atan2

(

y

LOS

−

y

,

x

LOS

−

x

)

psi_{text{LOS}} = text{atan2}(y_{text{LOS}}-y, x_{text{LOS}}-x)

ψLOS​=atan2(yLOS​−y,xLOS​−x)

其中 LOS 点取圆与线段交点，圆半径：

R

LOS

=

max

⁡

{

R

min

⁡

,

min

⁡

{

R

max

⁡

,

k

∣

e

∣

+

Δ

}

}

R_{text{LOS}} = maxleft{R_{min}, minleft{R_{max}, k|e|+Deltaright}right}

RLOS​=max{Rmin​,min{Rmax​,k∣e∣+Δ}}

• $k,\Delta$：可调增益，$R_{\min }=2L_{\text{ship}}$，$R_{\max }=5L_{\text{ship}}$

4. 控制器设计

• 制导回路：${\psi }_{\text{LOS}}$
• 姿态回路：PID 舵角控制

δ

=

K

p

e

ψ

+

K

i

∫

e

ψ

+

K

d

e

˙

ψ

,

e

ψ

=

ψ

LOS

−

ψ

delta = K_p e_psi + K_i int e_psi + K_d dot{e}_psi,quad e_psi = psi_{text{LOS}} – psi

δ=Kp​eψ​+Ki​∫eψ​+Kd​e˙ψ​,eψ​=ψLOS​−ψ

5. 代码实现（ROS2/C++17 为例）

5.1 数据结构

// usv_guidance.hpp
#pragma once
#include 
#include 

struct State {
    double x, y, psi, u, v, r;
};

struct Waypoint {
    double x, y;
};

class LOSGuidance {
public:
    LOSGuidance(double k, double delta, double Lship)
        : k_(k), delta_(delta), L_(Lship) {}

    double update(const State& s, const std::vectorWaypoint>& path);

private:
    double k_, delta_, L_;
    size_t idx_ = 0;

    double crossTrackError(const State& s,
                           const Waypoint& p1,
                           const Waypoint& p2);
};

5.2 LOS 核心算法

// usv_guidance.cpp
#include "usv_guidance.hpp"

double LOSGuidance::update(const State& s,
                           const std::vectorWaypoint>& path) {
    if (idx_ >= path.size()-1) return 0.0;

    const Waypoint& p1 = path[idx_];
    const Waypoint& p2 = path[idx_+1];

    // 1. 横向误差
    double e = crossTrackError(s, p1, p2);

    // 2. LOS半径
    double R = std::max(2*L_, std::min(5*L_, k_*std::fabs(e)+delta_));

    // 3. 计算LOS点
    double dx = p2.x-p1.x, dy = p2.y-p1.y;
    double segLen = std::hypot(dx,dy);
    dx /= segLen; dy /= segLen;

    double t = ((s.x-p1.x)*dx + (s.y-p1.y)*dy);
    double x_proj = p1.x + t*dx;
    double y_proj = p1.y + t*dy;

    double x_los = x_proj + R*dy*std::copysign(1.0, e);
    double y_los = y_proj - R*dx*std::copysign(1.0, e);

    // 4. 期望艏向
    double psi_d = std::atan2(y_los - s.y, x_los - s.x);

    // 5. 切换下一航段
    if (std::hypot(s.x-p2.x, s.y-p2.y)  R) ++idx_;
    return psi_d;
}

double LOSGuidance::crossTrackError(const State& s,
                                    const Waypoint& p1,
                                    const Waypoint& p2) {
    double dx = p2.x-p1.x, dy = p2.y-p1.y;
    return -(s.x - p1.x)*dy + (s.y - p1.y)*dx;
}

5.3 PID 控制器

class PID {
public:
    PID(double kp, double ki, double kd, double dt)
        : kp_(kp), ki_(ki), kd_(kd), dt_(dt) {}
    double operator()(double err) {
        double derr = (err - err_prev_)/dt_;
        integ_ += err*dt_;
        double out = kp_*err + ki_*integ_ + kd_*derr;
        err_prev_ = err;
        return out;
    }
private:
    double kp_, ki_, kd_, dt_;
    double integ_ = 0, err_prev_ = 0;
};

6. Python 轻量脚本（无 ROS 版）

# los_usv.py
import numpy as np

class LOSUSV:
    def __init__(self, k=2.0, delta=10.0, L=7.0):
        self.k, self.delta, self.L = k, delta, L
        self.idx = 0

    def update(self, x, y, psi, path):
        if self.idx >= len(path)-1:
            return None
        p1, p2 = path[self.idx], path[self.idx+1]
        dx, dy = p2[0]-p1[0], p2[1]-p1[1]
        seg = np.hypot(dx, dy)
        dx /= seg; dy /= seg

        # 横向误差
        e = -(x-p1[0])*dy + (y-p1[1])*dx
        R = max(2*self.L, min(5*self.L, self.k*abs(e)+self.delta))

        t = (x-p1[0])*dx + (y-p1[1])*dy
        proj = [p1[0]+t*dx, p1[1]+t*dy]
        los = [proj[0]+R*dy*np.sign(e), proj[1]-R*dx*np.sign(e)]

        psi_d = np.arctan2(los[1]-y, los[0]-x)
        if np.hypot(x-p2[0], y-p2[1])  R:
            self.idx += 1
        return psi_d

文章来源于互联网:基于LOS导引律的无人船路径点跟踪控制考虑横向误差以及船舶

相关推荐: Stable Diffusion 3 开源实测：12GB 显存跑出电影级 AI 图像

Stable Diffusion 3（以下简称 SD3）作为 Stability AI 最新开源文本生成图像模型，凭借其卓越的生成质量与高效的资源占用，迅速成为 AI 绘画领域的焦点。本文将从本地化部署、手部生成优化、Diffusion Transformer…