四元数船舶领域Quaternion ship domain

最新推荐文章于 2023-04-08 15:34:40 发布

知识它进不去脑子啊

最新推荐文章于 2023-04-08 15:34:40 发布

阅读量2.1k

点赞数 5

分类专栏：避碰和路径规划

本文链接：https://blog.csdn.net/weixin_41352742/article/details/117289708

版权

避碰和路径规划专栏收录该内容

1 篇文章 5 订阅

订阅专栏

四元数船舶领域(Quaternion ship domain, QSD)由确定的四个元素构成：
$R_{fore},R_{aft},R_{starb},R_{port}$

$R_{fore}$ 和 $R_{aft}$ 为船前部和船尾部， $R_{port}$ 和 $R_{starb}$ 为船左舷和右舷。

$Q S D$ 是由连接这四个元素和内部区域的闭合曲线定义的区域，可以描述如下：

$=\lbrace(x,y)|f(x,y;Q)\leq 1, Q= \lbrace R_{fore},R_{aft},R_{starb},R_{port}\rbrace\rbrace \quad(1)$

其中 $f ()$ 是 $Q S D 的$ 边界， $Q$ 是四元数。

由于多边形和椭圆形在以前的船域中经常使用。因此在不失普遍性的前提下，QSD的边界可以用多边形和椭圆两种形状表示。

根据 Eq.(1), $Q S D$ 的边界可以表示如下：

多边形：
$QSD_q =\lbrace(x,y)|f_q(x,y;Q)\leq 1, Q= \lbrace R_{fore},R_{aft},R_{starb},R_{port}\rbrace\rbrace \quad(2)$

椭圆形：
$QSD_{ce} =\lbrace(x,y)|f_{ce}(x,y;Q)\leq 1, Q= \lbrace R_{fore},R_{aft},R_{starb},R_{port}\rbrace\rbrace \quad(3)$

其中边界函数 $f_{q}$ 和 $f_{ce}$ 可以用下式表示：

$f_q(x,y;Q) = \frac{2x}{(1+sgn x)R_{fore} - (1-sgn x)R_{aft}} + \frac{2y}{(1+sgn y)R_{starb} - (1-sgn y)R_{port}}\quad(4)$

$f_{ce}(x,y;Q) = \left(\frac{2x}{(1+sgn x)R_{fore} - (1-sgn x)R_{aft}}\right)^2 + \left(\frac{2y}{(1+sgn y)R_{starb} - (1-sgn y)R_{port}}\right)^2 \quad(5)$

其中 $s g n$ 是如numpy.sign类的符号函数：
$\begin{cases} 1 & x \geq 0 \\ -1, & x < 0 \\ \end{cases} \quad(6)$
多边形和椭圆形四元数船舶领域
根据 Eq.(4)和(5)可以发现， $Q S D$ 的边界形状可以用一个参数 $k$ 控制，含有参数 $k$ 的 $Q S D$ 可以写作如下：

$QSD_k =\lbrace(x,y)|f_k(x,y;Q)\leq 1, Q= \lbrace R_{fore},R_{aft},R_{starb},R_{port}\rbrace, k\geq1 \rbrace \quad(7)$

$f_{k}(x,y;Q) = \left(\frac{2x}{(1+sgn x)R_{fore} - (1-sgn x)R_{aft}}\right)^k + \left(\frac{2y}{(1+sgn y)R_{starb} - (1-sgn y)R_{port}}\right)^k \quad(8)$

幂 $k$ 决定了 $Q S D$ 的形状，而四元数 $Q(i.e. R_{fore},R_{aft},R_{starb},R_{port})$ 标识了船域大小。
因此可以通过参数 $Q$ 和 $k$ 合理地对 $Q S D$ 建模。

$Kijima^{[1]}$ 等人用阻塞区参数估计公式(Estimation formulae for parameters of the blocking area)估计 $Q$ 的横向和纵向半径。可以用下式表示：

$\begin{cases} R_{fore} = \left(1+1.34 \sqrt{k^2_{AD}+(k_{DT}/2)^2 } \right) L\\ R_{aft} = \left(1+0.67 \sqrt{k^2_{AD}+(k_{DT}/2)^2 } \right) L \\ R_{starb} = (0.2 + k_{DT})L \\ R_{port}= (0.2 + 0.75k_{DT})L\\ \end{cases} \quad(9)$

其中 $L$ 是本船长度， $k_{AD}$ 是进距距离(Advance)的增益, $k_{DT}$ 是旋回直径的增益。并且可以被定义为如下：

$\begin{cases} k_{AD} = A_D/L = 10^{0.3591 \lg {V_{o} + 0.0952} }\\ k_{DT} = D_T/L = 10^{0.5441 \lg{ V_{o} - 0.0795}}\\ \end{cases} \quad(10)$

其中 $V_o$ 是本船速度，用节表示。

旋回初径 $D_T$ ：3~6倍船长

相对旋回初径 $D_T/L$

旋回直径 $D$ : 旋回初径的 0.9~1.2倍

进距 $A_D$ :旋回初径的 0.6~1.2倍

船舶旋回运动

# Python 实现后续补充
import numpy as np
import math
import matplotlib.pyplot as plt

L = 0.0863  # 以海里nm为单位 1节=1海里/小时=1.852公里/小时=0.514444m/s
DT = 5 * L # 相对旋回初径
D = 1.2 * DT # 旋回直径
AD = 1.2 * DT # 进距

# 进距和旋回直径增益
def Gain_k(v):
    k_ad = 10**(0.3591*math.log10(v)+0.0952)
    k_dt = 10**(0.5441*math.log10(v)-0.0795)
    return k_ad, k_dt

# 计算四元数 
def Quaternion(k_ad,k_dt):
    r_fore = (1+1.34*math.sqrt(k_ad**2+(k_ad/2)**2))*L
    r_aft = (1+0.67*math.sqrt(k_ad**2+(k_ad/2)**2))*L
    r_starb = (0.2+k_dt)*L
    r_port = (0.2+0.75*k_dt)*L
    return r_fore, r_aft, r_starb, r_port

v = 20

k_ad, k_dt = Gain_k(v)
r_fore, r_aft, r_starb, r_port = Quaternion(k_ad,k_dt)

Q = [r_fore, r_aft, r_starb, r_port]

x = [[0,r_starb],[r_starb,0],[0,-r_port],[-r_port,0]]
y = [[r_fore,0],[0,-r_aft],[-r_aft,0],[0,r_fore]]

plt.figure(dpi=128)
plt.plot(x,y,color = 'r')
plt.scatter(0,0,marker='*')
plt.grid(True)
plt.grid(color='black', linestyle='--',linewidth=0.3,alpha=0.3)
plt.xlabel('x(n mile)')
plt.ylabel('y(n mile)')
plt.xlim(-1,1)
plt.ylim(-1,1)
plt.show()

在这里插入图片描述

Refer

ence
[1]: Kijima, K. and Furukawa, Y. (2003). Automatic collision avoidance system using the concept of blocking area. Proceeding of IFAC Conference on Manoeuvring and Control of Marine Craft, Girona, Spain.
[2]: Wang N. An intelligent spatial collision risk based on the quaternion ship domain[J]. The Journal of Navigation, 2010, 63(4): 733.