{"id":257,"date":"2015-11-23T20:44:34","date_gmt":"2015-11-24T01:44:34","guid":{"rendered":"http:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/?page_id=257"},"modified":"2016-05-06T17:49:22","modified_gmt":"2016-05-06T21:49:22","slug":"planning-and-navigation-module-design-and-development","status":"publish","type":"page","link":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/system-information\/planning-and-navigation-module-design-and-development\/","title":{"rendered":"Planning and Navigation Module Design and Development"},"content":{"rendered":"

For the Fall Validation Experiment, we plan to detect the April tags mounted on the person of interest. From the depth estimate obtained from the April Tag, the robot will move towards the person and stop at a minimum distance. Following tasks have been completed in this regard:<\/span><\/p>\n

    \n
  1. IMU and Gyro Calibration of Turtlebots<\/strong><\/span>
    \n Calibration was done using the calibration scripts part of the standard TurtleBot ROS packages. This calibration involves having each Turtle-bot perform successive steps of increasingly large in-place rotations and measuring their wheel encoder and IMU output. When finished, the calibration indicates by what number the wheel and IMU odometry outputs should be scaled in order to better match the TurtleBot\u2019s actual motion. We have performed this calibration on two TurtleBots.<\/span><\/li>\n
  2. April Tag detection-<\/strong><\/span>First we used the Michael Kaess C++ library for getting the proper position estimate for the April Tags. That gave us the 6 degress of freedom for the tag and its tag id. Now we have a ROS node based on this library that performs the same task and detects the tag using webcam. We referred to the light-swarm\/april_tag library for this.\"aptag\"<\/a>Figure: April Tag Detection topic<\/strong><\/span><\/li>\n
  3. Relative Navigation: <\/strong>The Navigation controller node that runs on the turtlebot computer, subscribes to the April Tag detection topic and gets the relative pose of the person with respect to the camera. The z-axis starts from camera towards the April Tag, while the x- axis is towards the right and y axis points downwards. The navigation controller node than tries to reduce the relative z distance and the relative yaw angle between the camera and the April tag by publishing angular z and linear x velocity to the Turtlebot. A proportional controller was used to tune smoothly from stop to acceleration to steering to stopping again. The Navigation node moves the Turtlebot towards the person wearing April Tag until the z- distance or the distance of the April tag from camera is 100 cm or 1 m and the relative yaw is close to\u00a0 zero (between -5 to +5 degrees).<\/span><\/li>\n<\/ol>\n

    SPRING SEMESTER PROGRESS<\/strong><\/span><\/p>\n

      \n
    1. Overhead Localization- <\/strong>After building the single system robot navigation subsystem, the swarm navigation system requires the robots to know the position of other robots. This localization will be done using global cameras mounted on the ceiling of the lab (Advanced Agents Lab). Four wide angle lens camera are mounted on T shaped wooden mounts. These cameras are 4-5 feet away from each other such that the entire lab room is covered. They are placed such that between two consecutive cameras next to each other there is an overlap region so there are no blind spots in the room. In each of these overlap regions April tags are glued to the carpet. These April Tags serve the purpose of automated external calibration of the cameras with respect to each other such that in the event of certain changes in orientation of the cameras, one just has to launch the automated calibration file to get them calibrated extrinsically. The intrinsic calibration was done when the cameras were bought by the.\u00a0\u00a0 The cameras are calibrated intrinsically and manually by members of the lab. The overhead localization node is launched every time one turns on the ros system and with it all the cameras are initialized and extrinsically calibrated. One of the April Tags glued to the ground is considered to be the global origin and all the transforms and April tag detection regardless of the camera that detects it is calculated in the frame of this origin.\"IMG_1327\"<\/a>\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0<\/strong> Figure: Overhead camera setup<\/strong> \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 \"Screenshot<\/a><\/strong>\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 Figure: April tags detected in common region(door region is common)<\/strong><\/span><\/li>\n
    2. . <\/strong>Flocking- <\/strong>The algorithm basically is based on bio\u00ad-inspired school of fishes or a flock\u00a0<\/span>of birds. The intuition is that the birds when they fly are unaware of the entire world i.e. they do not\u00a0<\/span>know the position and velocities of all the birds in the flock but only of those around them. The birds while they flock do not collide with one another, thus they may change their velocity direction when they come too close to the other bird. The birds also move around with almost same velocity. Based on these observations, Reynolds devised a model with three functions that these birds would follow. The functions are as follows. \"130306-birds-flocking\"<\/a>\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 Figure: Bio-Inspired flocking<\/strong><\/span>
      \n Cohesion:<\/strong><\/span>
      \n The velocity and the trajectory of the agent in question is only affected by those near\u00a0<\/span>it. Thus we define a certain R_attract as a circular approximation to the area within which an\u00a0<\/span>agent could affect other agents velocity. In other words we only consider the agents within this\u00a0<\/span>circle with the agent in question as the center. \u00a0\u00a0<\/em><\/span><\/li>\n<\/ol>\n

      Function Cohesion <\/em><\/span>
      \n \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 { \u00a0<\/em><\/span>
      \n \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 agent_in_Question.Vel=0; <\/em><\/span>
      \n \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 For each agent != agent_In_Question <\/em><\/span>
      \n \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 (if dist(agent\u00adagent_In_Question)< R_attract) <\/em><\/span>
      \n \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 { <\/em><\/span>
      \n \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 agent_In_Question.Vel+=(Agent.Vel\u00ad agent_In_Question.Vel) <\/em><\/span>
      \n \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 \/\/same for y <\/em><\/span>
      \n \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 }}<\/em><\/span>
      \n Repulsion:<\/strong> <\/span>
      \n To avoid colliding with each other the agents should move away from each other if\u00a0<\/span>they happen to come too close to one another. So we define a R_Repel as the safeguard\u00a0<\/span>distance within which if the agent is found then it will experience a virtual repulsion force. So for\u00a0<\/span>each agent_In_Question<\/span><\/p>\n

      Function Repulsion <\/em><\/span>
      \n { <\/em><\/span>
      \n \u00a0\u00a0 For each agent!=agent_In_Question <\/em><\/span>
      \n \u00a0\u00a0 (if dist( agent\u00adagent_In_Question<R_Repel)) <\/em><\/span>
      \n \u00a0\u00a0\u00a0 { <\/em><\/span>
      \n \u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 agent_In_Question.vel= \u00ad agent.vel\u00a0\u00a0 <\/em><\/span>
      \n \/\/opposite velocity vecotr }<\/em><\/span>
      \n \u00a0\u00a0 <\/em><\/span>
      \n Alignment:<\/span><\/strong><\/span>
      \n In this function, if the agents are within R_attract and beyond R_Repel they tend to\u00a0<\/span>move in a similar fashion. Reynolds defined this function as each of the agent moving towards the\u00a0<\/span>center of mass of the entire flock.\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0<\/span><\/p>\n

      Function Alignment <\/em><\/span>
      \n { number_of_agent=0; <\/em><\/span>
      \n \u00a0 For each agent != agent_In_Question <\/em><\/span>
      \n \u00a0<\/em><\/span>
      \n if (dist(agent\u00ad-agent_in_Question)>R_Repel & dist(agent-\u00adagent_In_Question)<R_Attract)) <\/em><\/span>
      \n { <\/em><\/span>
      \n \u00a0\u00a0\u00a0 agent_In_Question.Vel += agent.vel <\/em><\/span>
      \n \u00a0\u00a0\u00a0 number_of_agent++ <\/em><\/span>
      \n } <\/em><\/span>
      \n \u00a0agent_in_question\/=number_of_agent;\u00a0<\/em><\/span><\/p>\n

       <\/p>\n

      \"Screenshot<\/a>\u00a0\u00a0\u00a0 Figure: Turtlebots flocking towards the person<\/strong><\/span><\/p>\n

      3. Aligning Robots around the person<\/strong><\/span><\/p>\n

      In this part, as soon as the central robot reaches a distance of 1.5 meters from the person, the flocking stops and each turtlebot is independently assigned coordinates that are at -30,0 and +30 degrees about the person at 1 meter distance. Each Turtlebot then steers itself towards the respective coordinates using continuous updates from the overhead localization.<\/span><\/p>\n

      \u00a0\"Screenshot<\/a>\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0\u00a0 Figure: Turtlebots aligning around the person of interest at -30 ,0 and 30 degrees<\/strong><\/p>\n

      \n","protected":false},"excerpt":{"rendered":"

      For the Fall Validation Experiment, we plan to detect the April tags mounted on the person of interest. From the depth estimate obtained from the April Tag, the robot will move towards the person and stop at a minimum distance. Following tasks have been completed in this regard: IMU and Gyro Calibration of Turtlebots Calibration
      + Read More<\/a><\/p>\n","protected":false},"author":39,"featured_media":0,"parent":29,"menu_order":0,"comment_status":"closed","ping_status":"closed","template":"","meta":{"footnotes":""},"class_list":["post-257","page","type-page","status-publish","hentry"],"_links":{"self":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/wp-json\/wp\/v2\/pages\/257","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/wp-json\/wp\/v2\/pages"}],"about":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/wp-json\/wp\/v2\/types\/page"}],"author":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/wp-json\/wp\/v2\/users\/39"}],"replies":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/wp-json\/wp\/v2\/comments?post=257"}],"version-history":[{"count":13,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/wp-json\/wp\/v2\/pages\/257\/revisions"}],"predecessor-version":[{"id":503,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/wp-json\/wp\/v2\/pages\/257\/revisions\/503"}],"up":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/wp-json\/wp\/v2\/pages\/29"}],"wp:attachment":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2015teamg\/wp-json\/wp\/v2\/media?parent=257"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}

      \"Boids<\/a>Figure: The 3 main functions that make agents flock with one another<\/strong><\/span><\/p>\n