Unified Approach of Unmanned Surface Vehicle Navigation in Presence of Waves

Algorithm 1

Planner pseudocode.

Set Target Point 𝑋_𝑡

Measure USV’s location 𝑋_𝑟

Measure USV’s velocity 𝑣_𝑟

Set 𝑇_safe, 𝑃_safe

for 𝑡=0 to 𝑡_final  do

 Measure 𝐻_1/3

 if  𝑋_𝑟=𝑋_𝑡  then

  Get a New Waypoint 𝑋_𝑡

 else

  for  𝑖=1 to 𝑛  do

   for  𝑗=1 to 𝑛  do

    Calculate 𝑃_{wave𝑖,𝑗}

   end for

  end for

  for  each node 𝑖=1⋯𝑛  do

   Calculate 𝑃_coll,𝑡

   if  𝑃_coll,𝑡<𝑃_safe  then

    𝜏_𝑟,𝑖(𝑡+Δ𝑡)=𝜏_𝑟,𝑖(𝑡)+𝜏_mp(𝑖)

   else

    𝜏_𝑟,𝑖(𝑡+Δ𝑡)=𝜏_𝑟,𝑖(𝑡)+𝜏_mp(max(𝑇_danger))

   end if

   Calculate 𝑤_𝑖(𝜏_𝑟,𝑖(𝑡+Δ𝑡))

  end for

  Find Min𝑤_𝑖(𝜏_𝑟,𝑖(𝑡+Δ𝑡))𝑖=1⋯𝑛

  Set 𝜏_𝑟,𝑖min(𝑡+Δ𝑡)

  Update 𝑋_𝑟

 end if

end for