一种基于渐近导引的船舶定位控制方法

摘要

本发明属于动力定位船舶控制领域，具体涉及一种基于渐近导引的船舶定位控制方法。本发明包括：（1）确定船舶的目标艏向和目标位置；（2）计算实时的期望位置和艏向；（3）根据当前的北位置、东位置和艏向，目标北位置、东位置、艏向，船舶纵向和横向的瞬时速度和顺数回转率和环境干扰产生的纵向力、横向力和力矩计算需要的纵向力、横向力、转艏时所需的力矩和北向、东向的位置偏差和艏向偏差。本发明基于渐近导引和控制设计了定位的控制方法。该方法得到的力矩和推力变化平缓，可以保证在推进器的限额范围内，从而使船舶平稳地到达目标位置，不会出现振荡，且定位精度更高。

主权项

一种基于渐近导引的船舶定位控制方法，其特征在于：(1)确定船舶的目标艏向和目标位置；(2)计算实时的期望位置和艏向：1)转艏时，计算目标艏向ψ_d和当前艏向ψ_n的差值得到船舶的相对艏向，ψ_g1＝ψ₁，相对艏向大于0时，则船舶向正向转向，相对艏向小于0时，则船舶向反向转向；船舶向正向转向时，期望艏向为ψ_gn＝ψ_g(n‑1)+K_turn·Ts，ψ_gn为在第n时刻得到的期望艏向，K_turn为上位机设定的回转率，Ts为控制系统的采样时间，如果ψ_gn‑ψ_d≤0，则ψ_gn＝ψ_d，否则重新读取目标艏向和当前艏向进行计算；船舶向反向转向时，期望艏向为ψ_gn＝ψ_g(n‑1)‑K_turn·Ts，如果ψ_gn‑ψ_d≥0，则ψ_gn＝ψ_d，否则重新读取目标艏向和当前艏向进行计算；2)向目标位置移动时，船舶在二维平面内第n时刻的当前位置为P＝[n_n,e_n]^T，第n时刻得到的期望位置为P_gn＝[n_gn,e_gn]^T，船舶的目标位置为P_d＝[n_d,e_d]^T，[n_g1,e_g1]^T＝[n₁,e₁]^T，计算船舶目标位置和当前位置在北东坐标系的夹角将标志位置为1，计算船舶当前位置和目标位置的相对距离，计算期望位置时所用的速度为v_g＝k*r_dn‑0.1,k＝0.015，方向为船舶沿着当前位置和目标位置的连线方向，计算上一时刻期望位置和目标位置的相对距离，即：$r_{gd}=\sqrt{{(e_{g(n-1)}-e_d)}^2+{(n_{g(n-1)}-n_d)}^2}$式中e_g(n‑1)为上一时刻得到的期望东位置，n_g(n‑1)为上一时刻得到的期望北位置；r_gd大于1m时，则此时刻的期望位置为：$\left[\begin{array}{c}{n}_{gn}\\ e_{gn}\end{array}\right]=\left[\begin{array}{c}{n}_{g(n-1)}+v_s*Ts*cos\left(\theta \right)\\ e_{g(n-1)}+v_s*Ts*sin\left(\theta \right)\end{array}\right]$r_gd小于等于1m，则此时刻的期望位置为：[n_gn,e_gn]^T＝[n_d,e_d]^T，同时将标志位置为2，如果纵向或是横向速度大于0.5m/s，将标志位置为3，此时的期望位置是船舶的当前时刻的位置，即：[n_gn,e_gn]^T＝[n_n,e_n]^T；船舶当前位置和目标位置的相对距离大于3m且标志位为2时，将标志位重新置为4，此时的期望位置为目标位置，即[n_g,e_g]^T＝[n_d,e_d]^T，得到新的期望位置后，将标志位再次置为1；(3)根据当前的北位置、东位置和艏向，目标北位置、东位置、艏向，船舶纵向和横向的瞬时速度和瞬时回转率和环境干扰产生的纵向力、横向力和力矩计算需要的纵向力、横向力、转艏时所需的力矩和北向、东向的位置偏差和艏向偏差：1)船舶的目标位置和目标艏向为η_d＝[n_d,e_d,ψ_d]^T时，船舶的目标位置和目标艏向为定常向量，船舶的三自由度模型为：$\begin{array}{c}\stackrel{·}{\eta }=R\left(\psi \right)v\\ M\stackrel{·}{v}+C\left(v\right)v+D\left(v\right)v=\tau +{\tau }_e\end{array},$$R\left(\psi \right)=\left[\begin{array}{ccc}cos\left(\psi \right)& -sin\left(\psi \right)& 0\\ sin\left(\psi \right)& \cos \left(\psi \right)& 0\\ 0& 0& 1\end{array}\right];$船舶的位置跟踪误差为：求导：$\stackrel{·}{\stackrel{~}{\eta }}=\stackrel{·}{\eta }-{\stackrel{·}{\eta }}_d=R\left(\psi \right)v,$在大地坐标系下的虚拟的参考速度为式中Λ∈R^3×3为正定的对角阵，在船体坐标系下的虚拟的参考速度为速度的跟踪误差为：$s=\stackrel{·}{\eta }-{\stackrel{·}{\eta }}_r=R\left(\psi \right)v+\Lambda \tilde{\eta },$船舶三自由度模型变形得：$M*\left(\eta \right)\stackrel{··}{\eta }+C*(\eta ,v)\stackrel{·}{\eta }+D*(\eta ,v)\stackrel{·}{\eta }=\tau +{\tau }_e$M*(η)＝R(ψ)MR^‑1(ψ)，$C*(\eta ,v)=R\left(\psi \right)[C\left(v\right)-{\mathrm{MR}}^{-1}\left(\psi \right)\stackrel{·}{R}\left(\psi \right)]R^{-1}\left(\psi \right),$D*(η,v)＝R(ψ)D(v)R^‑1(ψ)，$\begin{array}{c}M*\left(\eta \right)\stackrel{·}{s}\\ =-C*\left(\eta ,v\right)s-D*\left(\eta ,v\right)s+R\left(\psi \right)\left(\tau +{\tau }_e\right)-M*\left(\eta \right){\stackrel{··}{\eta }}_r-C*\left(\eta ,v\right){\stackrel{·}{\eta }}_r-D*\left(\eta ,v\right){\stackrel{·}{\eta }}_r\\ =-C*\left(\eta ,v\right)s-D*\left(\eta ,v\right)s+R\left(\psi \right)\left(\tau +{\tau }_e-M{\stackrel{·}{v}}_r-C\left(v\right)v_r+D\left(v\right)v_r\right)\end{array}$位置跟踪误差为：第一个李亚普诺夫函数式中K_P∈R^3×3为正定的对角阵；对V₁进行求导：${\stackrel{·}{V}}_1={\tilde{\eta }}^T{K}_P\stackrel{·}{\stackrel{~}{\eta }}={\tilde{\eta }}^T{K}_P(s-\Lambda \tilde{\eta })=-{\tilde{\eta }}^T{K}_P\Lambda \tilde{\eta }+s^T{K}_P\tilde{\eta }$第二个李亚普诺夫函数$V_2=\frac{1}{2}{s}^{T}M*\left(\eta \right)s+V_1$对V₂进行求导：$\begin{array}{c}{\stackrel{·}{V}}_2=s^{T}M*\left(\eta \right)\stackrel{·}{s}+\frac{1}{2}{s}^T\stackrel{·}{M}*\left(\eta \right)s+{\stackrel{·}{V}}_1\\ =-s^T\left[C*\left(\eta ,v\right)+D*\left(\eta ,v\right)\right]s+\frac{1}{2}{s}^T\stackrel{·}{M}*\left(\eta \right)s\\ +s^T\left[R\left(\psi \right)\left(\tau +{\tau }_e-M{\stackrel{·}{v}}_r-C\left(v\right)v_r+D\left(v\right)v_r\right)\right]\\ -{\tilde{\eta }}^T{K}_P\Lambda \tilde{\eta }+s^T{K}_P\tilde{\eta }\end{array}$$s^T[\stackrel{·}{M}*\left(\eta \right)-2C*(\eta ,v)]s=0,$$\begin{array}{c}{\stackrel{·}{V}}_2=-s^{T}D*(\eta ,v)s-{\tilde{\eta }}^T{K}_P\Lambda \tilde{\eta }\\ +s^T\left[R\left(\psi \right)(\tau +{\tau }_e-M{\stackrel{·}{v}}_r-C(v)v_r+D(v)v_r+R^{-1}(\psi \left)K_P\tilde{\eta }\right)\right]\end{array}$选择控制输入为：$\tau =M{\stackrel{·}{v}}_r+C\left(v\right)v_r+D\left(v\right)v_r-R^{-1}\left(\psi \right)K_P\tilde{\eta }-R^{-1}\left(\psi \right)K_D{s}_n-{\tau }_e$${\stackrel{·}{V}}_2=-s^T(D*(\eta ,v)+K_D)s-{\tilde{\eta }}^T{K}_P\Lambda \tilde{\eta }$由于V₂正定，负定，所以平衡点从而驱动船舶渐渐地向目标位置和目标艏向靠近，并使船舶保持在目标位置和目标艏向，实现高精度定位；2)船舶每一采样时刻的船舶的当前位置和当前艏向为η_n＝[n_n,e_n,ψ_n]^T时，通过步骤(2)得到的期望位置和期望艏向为η_gn＝[n_gn,e_gn,ψ_gn]^T，大地坐标系下第n时刻的位置跟踪误差：${\tilde{\eta }}_n={\eta }_n-{\eta }_{gn}=\left[\begin{array}{c}{n}_n-n_{gn}\\ e_n-e_{gn}\\ {\psi }_n-{\psi }_{gn}\end{array}\right]$在大地坐标系下的虚拟的参考速度为：则在船体坐标系下的虚拟的参考速度为：$v_{rn}=-R^{-1}\left({\psi }_n\right)\Lambda {\tilde{\eta }}_n,$速度的跟踪误差为：$s_n=R\left({\psi }_n\right)v_n+\Lambda {\tilde{\eta }}_n,$船舶需要的控制力输入为：${\tau }_n=M(v_{rn}-v_{r(n-1)})+{\mathrm{Cv}}_{rn}+{\mathrm{Dv}}_{rn}-R^{-1}\left({\psi }_n\right)K_P{\tilde{\eta }}_n-R^{-1}\left({\psi }_n\right)K_D{s}_n-{\tau }_e$τ_e为海洋环境的对船舶的作用力，根据控制输入，通过推进器驱动船舶向目标位置；M为船舶惯性矩阵,C(v)为船舶科里奥利向心力矩阵，D(v)为船舶阻尼矩阵，Ψ为船舶在大地坐标系下艏向角，τ为船舶坐标系下纵向力、横向力和回转力矩三维状态向量，η为船舶在大地坐标系下北向位置、东向位置和艏向角三维状态向量，η_r为船舶在大地坐标系下的虚拟的参考位置向量，v为船舶纵向速度、横向速度和回转角速度在船体坐标下的三维状态向量，v_n为第n时刻的船舶纵向速度、横向速度和回转角速度在船体坐标下的三维状态向量，v_r为船舶坐标系下的虚拟的参考速度向量，v_rn为第n时刻的船舶坐标系下的虚拟的参考速度向量，s为大地坐标下对虚拟参考速度的跟踪误差，s_n为第n时刻的大地坐标下对虚拟参考速度的跟踪误差，K_D为正定的对角矩阵，Ψ₁为船舶的当前艏向，n_n为船舶的当前北位置，e_n为船舶的当前东位置，n_gn为船舶的期望北位置，e_gn为船舶的期望东位置，n_d为船舶的目标北位置，e_d为船舶的目标东位置，v_s为考虑到大地坐标下对虚拟参考速度的跟踪误差的速度，η_d为船舶的目标艏向，v_r(n‑1)在船体坐标系下的虚拟的参考速度。