Moos-IvP 实验室7 Helm Autonomy 任务详解

任务1 Alpha返回任务

这个任务包括修改alpha示例任务，实现通过在pMarineViewer左键点击界面以确定vehicle的返回路径点。主要目标为

• 修改alpha.moos和alpha.bhv这两个文件，从而实现接受用户在pMarineViewer中左键点击所确定的返回位置。该修改应能使用户点击时，立即发送一个标识为“return_point”的点发布至pMarineViewer窗口，vehicle完成其路线点调查后，将返回该点。

提示：您需要使用为所有行为定义的updates参数。return home waypoint行为已经配置了一个updates参数，将MOOS变量命名为RETURN_UPDATE。您的鼠标点击需要将结果发布到此变量。此发布的内容应该是一个字符串，其中该字符串是替换行为配置块中的任何配置行。

更多updates参数可以在这里找到Properties of Helm Behaviors

修改alpha.moos文件

找到配置块ProcessConfig = pMarineViewer，添加以下代码

left_context[return-point] = RETURN_UPDATE = points=$(XPOS),$(YPOS)

该代码将鼠标所点击的坐标位置字符串信息points=(x坐标,y坐标)发布到RETURN_UPDATE变量中。

MOOSDB自定义左右键的Poking操作基本格式为

    left_context[<key>]  = <var-data-pair>
    right_context[<key>] = <var-data-pair>

$(XPOS)和 $(YPOS)为本地坐标系的x，y坐标
$(LAT)和 $(LON)为经纬度
$(IX)为poke次数

返回路径点的bhv配置块为

Behavior=BHV_Waypoint
{
  name       = waypt_return
  pwt        = 100
  condition  = RETURN = true
  condition  = DEPLOY = true
  perpetual  = true
  updates    = RETURN_UPDATE
  endflag    = RETURN = false
  endflag    = DEPLOY = false
  endflag    = MISSION = complete
	
  speed = 2.0
  capture_radius = 2.0
  slip_radius = 8.0
  points = 0,-20
 }

由于其updates参数所设定的变量名称RETURN_UPDATE与左键点击所发布的变量名称相同，从而更新该行为的points参数为左键点击坐标点坐标。

通过修改alpha.moos文件中下列语句数值大小，来修改程序运行速度。

MOOSTimeWarp = 10

运行测试

首先开始alpha任务

左键点击屏幕中某一点，然后点击RETURN按钮，实现对所点击位置的返回

任务2 alpha的立即返回任务

该任务需要在DEPLOY过程中，左键进行点击，能够立刻返回至所点击的点。

该任务仅需要在alpha.moos文件ProcessConfig = pMarineViewer配置块中添加

left_context[return-point] = RETURN=true

从而更新RETURN变量，即可实现立即返回任务。

任务3 建立第一个自治任务—Bravo任务

在今天实验室的第二部分，目标是从零开始创建一个自治任务。我们将主要使用Loiter行为，但是我们也将有机会使用Waypoint、ConstantDepth、ConstantSpeed和Timer行为。我们将从为水面航行器构建一个任务开始，但很快将迁移到为水下航行器配置。

任务3.1 Bravo Loiter任务

在此部分，我们将：

• 从头开始准备一个新的任务
• 熟悉Loiter行为
• 引入行为运行状态的概念
• 理解定义在所有行为上的“perpetual”标志

Bravo任务应该具备以下功能：

• 它应该有一个Loiter行为，它在初始部署时为活动的。Loiter行为位置重心应为x=100,y=-75，半径为30米，loiter多边形有8个顶点。它的转速设置为2.5 m/s。它应该是逆时针的。
• 它应该有一个Waypoint行为，当变量RETURN=true时，简单地将vehicle返回到车辆的起始位置x=0,y=0，就像在alpha任务中一样。
• Loiter行为应该利用duration参数在150秒后自动“完成”，触发返回路径点行为。

为实现上述功能，在.bhv文件添加以下行为：

1.Loiter行为

该行为使vehicle航行轨迹为多边形

Loiter行为代码如下

Behavior=BHV_Loiter
{
  name = zhuan_quanquan
  pwt  = 100

  condition = DEPLOY = true
  condition = RETURN = false
  endflag = RETURN = true
  
  duration = 100
  updates    = LOITER_UPDATE
  perpetual  = true

  acquire_dist = 10
  capture_radius = 3
  center_activate = false
  clockwise = true   // true best false
  slip_radius = 15
  speed = 2 
  spiral_factor = -2
  polygon = format=radial, x=100, y=-75, radius=30, pts=8, snap=1, label=Lima
  post_suffix = HENRY 


}

重要参数

• duration：行为完成前，行为所能运行的时间
• perpetual：决定行为完成后，该行为是否继续运行

上述两参数为所有行为的公共参数，具体其它参数详见The Loiter Behavior

2.Waypoint行为

该行为设定任务完成的返回点，代码如下：

Behavior=BHV_Waypoint
{
  name       = waypt_return
  pwt        = 100
  condition  = RETURN = true
  condition  = DEPLOY = true
  perpetual  = true
  updates    = RETURN_UPDATE
  endflag    = RETURN = false
  endflag    = DEPLOY = false
  endflag    = MISSION = complete
	
           speed = 2.0
  capture_radius = 2.0
     slip_radius = 8.0
          points = 0,-20
 }

添加pA对应的.moos文件，运行结果如下

首先进行Loiter行为，在时间达到duration所设定的100后，开始返回。

任务3.2 Bravo Double Loiter任务

在此部分，我们将：

• 探索一个不连续的任务-一个在切换模式的任务
• 熟悉 duration, endflag, perpetual等行为参数
• 学习Timer行为

我们将在Bravo任务中增加第二个Loiter任务，使得vehicle能够周期性的在两个地点切换，第二个Bravo任务将配置以下功能：

• 新的bravo任务应该有第二个Loiter行为，它在初始部署时不是活动的。它的位置应该是x=160,y=-50，半径为20米，Loiter多边形应该有8个顶点。它的转速应设置为2.5 m/s。
• 利用condition参数使两个loiter行为互斥，如condition=LOITER_REGION=west
• 利用duration, endflag, perpetual参数完成周期性切换，将每个持续时间设置为150s，在每个行为中使用两个endflag参数从而
  -（1）触发另一个Loiter行为的condition
  -（2）取消行为结束的condition（笔者未添加）
  将perpetual参数设置为true，使行为永久运行，等待它的condition再次得到满足。
• 注意：您可能需要了解 duration timer是如何工作的。回想一下，duration特性是为所有行为定义的。默认情况下，一旦倒计时结束，计时器立即重新启动，同时perpetual设置为true，所以说在不满足行为condition时，该行为的计时器也在持续运行中。您可能希望您的行为等待，直到它再次运行(逻辑条件得到满足)，然后再恢复持续时间计时器倒计时。在这种情况下，需要将duration_idle_decay设置为false。默认值为true

在3.1中.bhv的基础上：
1.修改第一个Loiter行为

Behavior=BHV_Loiter
{
  name = zhuan_quanquan
  pwt  = 100

  condition = DEPLOY = true
  condition = RETURN = false
  condition = CHANGE = 1
  endflag = CHANGE = 2

  duration = 50
duration_idle_decay = false

  updates    = LOITER_UPDATE
  perpetual  = true

  acquire_dist = 10
  capture_radius = 3
  center_activate = false

  clockwise = true   // true best false
  slip_radius = 15
  speed = 2.5
  spiral_factor = -2

  polygon = format=radial, x=100, y=-75, radius=30, pts=8, snap=1, label=Lima

  post_suffix = HENRY 


}

新增condition CHANGE，当CHANGE为1时，为第一个多边形，同时新增结束标志为CHANGE=2，当该行为完成后，进入第二个多边形。
新增duration_idle_decay = false

2.添加第二个Loiter行为

Behavior=BHV_Loiter
{
  name = zhuan_sss
  pwt  = 100

  condition = DEPLOY = true
  condition = RETURN = false
  condition = CHANGE = 2
  
  endflag = CHANGE = 1

  duration = 50 
  duration_idle_decay = false

  updates    = LOITER_UPDATE2
  perpetual  = true

  acquire_dist = 10
  capture_radius = 3
  center_activate = false

  clockwise = true   // true best false
  slip_radius = 15
  speed = 2.5
  spiral_factor = -2

  polygon = format=radial, x=160, y=-50, radius=20, pts=8, snap=1, label=Lima

  post_suffix = HENRY2

}

注：若此行为名称起为 zhuan_quanquan2，运行时会报错

3.修改.moos文件

由于Loiter新增加condition=CHANGE的条件，故在ProcessConfig = pMarineViewer配置块中，将DEPLOY按钮新增CHANGE=1的消息发布，即：

button_one = DEPLOY # DEPLOY=true # CHANGE = 1

运行结果

第一个Loiter行为

第二个Loiter行为

任务3.3 Bravo UUV任务

我们的下一步是改变Bravo任务来模拟UUV而不是surface vehicle。我们需要修改uSimMarine、pMarinePID和pHelmIvP中的一些配置。当然，我们也会在任务中加入一些行为组件，这样UUV就可以俯冲了。

在此部分，我们将：

• 当使用UUV时，如何扩展pMarinePID、 uSimMarine、 pHelmIvP
• 学习使用ConstantDepth行为
• 学会将行为联系在一起，协调运作
• 学习当使用水下航行器时，如何使用 pMarineViewer

第一步是对到目前为止我们的示例中使用的几个MOOS应用程序进行修改，以支持深度。

修改pMarinePID配置
为了拥有一个模拟的水下航行器，在你的任务文件中添加以下代码到你的pMarinePID配置块中。注意，需要在depth_control=true的情况下，为此您可能要替换现有的行。

depth_control = true

   //Pitch PID controller
   pitch_pid_kp                   = 1.5
   pitch_pid_kd                   = 0.3
   pitch_pid_ki                   = 0.004
   pitch_pid_integral_limit       = 0

   //ZPID controller
   z_to_pitch_pid_kp              = 0.12
   z_to_pitch_pid_kd              = 0.1
   z_to_pitch_pid_ki              = 0.004
   z_to_pitch_pid_integral_limit  = 0.05

   maxpitch     = 15
   maxelevator  = 13

修改uSimMarine配置
添加如下代码至配置块中

   buoyancy_rate        = 0.15
   max_depth_rate       = 5
   max_depth_rate_speed = 2.0
   default_water_depth  = 400

修改了pHelmIvP的结构
下面增加的Helm决策空间，包括101个可能的深度决策。在较深的水中，可以使用不同的配置。这一行需要添加到您的任务文件中的pHelmIvP配置块。

domain = depth:0:100:101

修改pNodeReporter配置

下面对pNodeReporter的修改主要是修饰性的。它将vehicle类型更改为“UUV”，这样您就可以在模拟器潜水中看到UUV图标，而不是独木舟。这一行可以在任务文件的pNodeReporter配置块中找到。

platform_type = UUV

第三个Bravo任务应该配置以下功能:

• 在行为文件中添加一对ConstantDepth行为，并将其配置为vehicle在西部行驶时行驶深度为30米，在东部行驶时行驶深度为10米。

在pMarineViewer中，您可以通过反复点击“n”键来更改呈现在vehicle旁边的标签。在操作时增加vehicle的深度信息是很有帮助的。你可以配置pMarineViewer在这种模式下出现通过设置:

vehicles_name_mode = names+depth

您可以暂时不指定peakwidth, basewidth, and summitdelta参数，使用它们的默认值。注意:ConstantDepth行为必须将其持续时间参数设置为非零值，或者设置为duration=no-time-limit。
为了将深度行为与Loiter行为联系起来，只需使它们的运行condition相同即可。

为实现上述功能，增加两个ConstantDepth行为，从而保证能够有深度的Loiter行为

 Behavior = BHV_ConstantDepth
 {

   name         = const_fdep  
   pwt          = 100                
  condition = DEPLOY = true
  condition = RETURN = false
  condition = CHANGE = 1
   updates      = CONST_DEP_UPDATES  


   depth = 30           
   depth_mismatch_var = DEPTH_DIFF   
   duration=no-time-limit 

 }

 Behavior = BHV_ConstantDepth
 {

   name         = const_sdep  
   pwt          = 100                
  condition = DEPLOY = true
  condition = RETURN = false
  condition = CHANGE = 2
   updates      = CONST_DEP_UPDATES2


   depth = 10           
   depth_mismatch_var = DEPTH_DIFF   
   duration=no-time-limit 

 }

运行结果

任务3.4 Bravo UUV Surface任务

这项任务包括对Bravo示例任务的最后修改。在这次任务中，航行器将周期性地浮出水面，以零速度在水面等待数秒，然后俯冲并继续执行任务。这大概是为了模拟偶尔需要浮出水面进行GPS定位以重新设置导航解决方案的UUV所发生的情况。你的目标是:

• 利用Helm的Timer行为来增加你的任务，让你的vehicle周期性的停下来，然后浮出水面(每200秒一次)。利用另一个计时器行为开始时，vehicle在表面，等待60秒之前，让vehicle再次潜水。

增加第一个Timer行为
通过增加第一个Timer行为，使得能周期性停止下来

 Behavior = BHV_Timer
 {
   // General Behavior Parameters
   // ---------------------------
   name         = bhv_timer           
   condition = DEPLOY = true
   condition = RETURN = false
   condition = STA = water   
   perpetual  = true
   duration     = 100                
   
   duration_idle_decay = false

   endflag      = STA = surface         
   updates      = TIMER_UPDATES        

   // Parameters specific to this behavior
   // ------------------------------------
   // None
 }

同时为Loiter行为与对应的ConstantDepth行为增加condition，以区分水下与水面模式

  condition = STA = water  

增加第二个Timer行为

通过添加第二个Timer行为，使得vehicle能够在上浮时开始计时，计时结束返回原工作状态。

 Behavior = BHV_Timer
 {
   // General Behavior Parameters
   // ---------------------------
   name         = bhv_timer           
   condition = DEPLOY = true
   condition = RETURN = false
   condition = STA = surface   
   perpetual  = true
   duration     = 60                
   
   duration_idle_decay = false

   endflag      = STA = water         
   updates      = TIMER_UPDATES        

   // Parameters specific to this behavior
   // ------------------------------------
   // None
 }