选题要求

功能需求:
基本需求：机器人需要从起点处去往，从平台上取得货物，并将其放在指定货架的空货窗内，最后返回起点
拓展需求：两台（或两台以上）机器人同时进行补货操作，可以一次取多件货物
图2 超市补货机器人升级版场地示意图
【基本任务】单机器人补货操作
机器人需要在仓库的平台上取得货物，并将其放在指定货架的空货窗内，每放置正确1件货物得10分，放错货架或放入非空的货窗，则扣5分。
【挑战任务】多机器人协作
两台（或两台以上）机器人同时进行补货操作（需要考虑避障；可以一次取多件货物），参加挑战任务的项目组有额外的加分。
性能分析：
路径规划：两台机器人在协同工作时，能找到一条从起点到终点适当的运动路径，使机器人在运动过程中能安全、无碰撞地绕过所有障碍物，若机器人规划路径产生冲突，其中一台机器人应及时避让
机器视觉：机器人能够准确识别分类货物，同时识别货架，判断货架是否存放有货物，能为货物正确匹配货架

抓取及路径最优化：机器手设计部分，采用较为简单的二连杆或者四连杆结构，对货物的起落进行精确控制。
约束及性能评判：
约束明确：由于实物搭接的不便，本项目采用webots进行机器人建模与仿真模拟
1.由于图像处理算法过于复杂，采用现成的recognition API实现对于相关代码的省略化，使仿真专注于建模与运动算法的开发
2.机器人具体结构建模，考虑利用world搭建里现有的E-puck机器人为基础，进行进一步的搭接
3.对于系统某些参数的设定与调整，我们尽量采用world里现有的进行调用或者模仿，避免出现调参消耗大量时间

基于webots平台下机器人选型与模型搭建

机器人结构选型设计

移动方案：采用四轮机器人模型，轮子使用例程youbot的麦克纳姆轮

抓取方案：以例程pioneer3_gripper中的机器爪模块为原型进行修改

而经过项目的推进与设计，我们还在已有基础上，搭载了红外传感器，以进行避障功能，结构如下：

具体细节：

麦克纳姆轮：麦克纳姆轮技术的全方位运动设备可以实现前行、横移、斜行、旋转及其组合等运动方式。此处采用youbot中的proto文件进行直接引入，简化操作。

机器臂结构设计

原版pioneer3的gripper太小，不能满足取货放货的需求，因此考虑调节机器臂的比例，再对其进行组合

机器人传感器设计

电机力传感：

机器爪gripper不能通过代码后台直接调节手抓开合大小抓取物体，因此可以考虑在机械抓上加装力传感器，反馈电机转速来逐渐抓紧物体

视觉传感：

机器人主体上使用前后置两个摄像头实时反馈机器人前后方图像，机械臂上采用一个顶部摄像头反馈机械臂位置。

罗盘及GPS：

机器人使用一个gps反馈机器人位置，和一个罗盘反馈机器人姿态。
对于避障环节，我们初步考虑的是，不去采用epuck机器人类似的车身八个的红外传感器进行避障，
原因是，觉得在利用了罗盘和GPS，且在固定空间下运作的情况下，利用sensor进行避障处理会更加麻烦（之后如果采用多机器人协作，可以适当考虑利用传感器进行进一步保底设计）。

但经过讨论，在项目最终进行阶段，还是考虑采用epuck机器人类似的车身红外传感进行避障功能的实现。

世界搭建

具体细节不过多阐述，可以参考官网学习文档中提供的tutorial，此处需要注意的细节是，在构建货架等非透视项目时，要在node里勾选，不然在视觉处理中，会在扫描第一列货架时，后排的货物会出现重叠覆盖，导致不能准确抓取。

机器人控制器设计与实现

webots采用的为其自带的c++，所以状态机的设置主要通过构建枚举量进行，同时也方便状态之间进行跳转

整体项目开法

状态机代码流程：：

1. Initial Pose

整个流程，从最开始的Init_Pose开始，对所有机构进行初始化，同时，利用set_posture，将机器人置于第一个顶点，即fixed_posture_findempty的第一行元素，此时车尾对着货架， 顶部摄像头调用Find_Empty进行空货架识别，若调用失败，则货架+1，巡航点下标+1，设置fin_target_posture
并进入前往下一个货架状态

2. Recognize_Empty

在此状态，判断是否到达下一个货架，若到达，则状态重新转为Recognize_Empty进行空货架识别，，若未到，则巡航点下标+1，直到其与所设置的四个空货架定位点相等，系统跳转至Recognize_Empty.

3. up_to_top & return_initial

若摄像头识别到了空货架，则状态转至环绕取货堆运动，但是此时不方便直接开始绕取货堆运动，考虑利用一新状态up_to_top使得所有位置运动到最上层(利用set_posture()的第二个参数修改成-5)，再进入下一状态return_initial，写回归起点，若未回到(1.05,1.05,2*PI)的位姿，则利用
double initial进行更进一步地从最顶层往右侧返回初始点的操作

4. Arround_Moving

若判断回到起点，则转入下一状态Arround_Moving，写环绕取货堆进行取货操作。
在Arround_Moving状态中，若没有对准物体，则率先判断物体是否已经到达上个状态结束时的位置（即初始位置(1.05,1.05,2*PI)），若未到达，则移动至该位置，到达则进行接下操作，令gp_travel_points_sum累加，并打印当前货架信息，通过遍历整个取货路径的位姿，直至Find_Goods到达指定位置，使其达到“恰好”状态若恰好在当前位置面对物品则转入下一阶段Grab_Item进行物体抓取。通过函数Aim_and_Grasp(),进行对准与抓取操作，若操作成功，返回真值，进行状态的进一步推进，即进入Back_Moving状态。

5. Back_Moving

Back_Moving状态里，先判断物体是否已经到达上个状态结束时的位置(即待取货物位置的机器人姿态)，若未到达，则移动至该位置，到达则进行接下操作，即令下标gp_Travel_Point_Index不断累加，进行巡航，直至到达起点，即gp_Travel_Point_Index等于11时，进入下一状态back_to_shelf
若此时的fin_target_posture为之前寻找到的空货架的定点的位置，则整个状态转移到TurnBack_To_LoadItem,进行转身装货过程，
若未到达，则利用一个temp作为临时变量，在每一次进入时进行自增操作，直至其到达原定货架位置。

之后进入TurnBack_To_LoadItem，通过gps计算和上货点的相对位移，通过compass计算姿态，对load_target_posture进行赋值，
最后转入上货状态 Item_Loading进行上货操作，通过按着最开始设置的步长进行步进，达到稳定上货的目的，在上完货之后，控制机器臂手抓开合与升降，对状态再次进行更换，重新回Init_Pose

注意：Travel_Point_Index下标通过整个货架巡航轨迹，一直与fixed_posture_findempty的四个定点匹配，因此在最后填充完货物后，
进行下一个货架的检索并不受影响。

视觉处理功能及抓取功能

//前部摄像头校准并抓取
bool Aim_and_Grasp(int *grasp_state, WbDeviceTag camera, int objectID)
{int number_of_objects = wb_camera_recognition_get_number_of_objects(camera);const WbCameraRecognitionObject *objects = wb_camera_recognition_get_objects(camera);for (int i = 0; i < number_of_objects; ++i){if (objects[i].id == objectID) //自左往右自下往上搜寻第一个物体{if (*grasp_state == 0) //调整位置{//调整高度，放置抓取错误，出现world is too complex等报错if (!strcmp("cereal box red", objects[i].model) || !strcmp("cereal box", objects[i].model))lift(height = 0.05);else if (!strcmp("water bottle", objects[i].model))lift(height = 0.080);else lift(height = 0.01);moveFingers(width = objects[i].size[0] / 1.5);get_gps_values(gps_values);get_compass_angle(&compass_angle);double grasp_target_posture[3];double grasp_dis_set = Grasp_dis_set[name2index(objects[i].model)];grasp_target_posture[0] = (gps_values[0] - sin(compass_angle) * objects[i].position[0] + cos(compass_angle) * (objects[i].position[2] -  grasp_dis_set) * 0.6);grasp_target_posture[1] = gps_values[1] - cos(compass_angle) * objects[i].position[0] - sin(compass_angle) * (objects[i].position[2] -  grasp_dis_set) * 0.6;grasp_target_posture[2] = compass_angle;Moveto_CertainPoint1(grasp_target_posture, 0.005);double grasp_threshold = 0.002;if (fabs(objects[i].position[0]) < grasp_threshold && fabs(objects[i].position[2] - grasp_dis_set) < grasp_threshold){*grasp_state += 1;printf("抓手已对准\n");base_reset();printf("物体大小: %lf %lf\n", objects[i].size[0], objects[i].size[1]);moveFingers(width = objects[i].size[0] / 2);wb_robot_step(30000 / TIME_STEP);}}else if (*grasp_state == 1) //抓{double grasp_force_threshold = 83.0;if (wb_motor_get_force_feedback(gripper_motors[1]) > -grasp_force_threshold)moveFingers(width -= 0.0003); //步进else{wb_robot_step(30000 / TIME_STEP); if (wb_motor_get_force_feedback(gripper_motors[1]) <= -grasp_force_threshold){printf("爪宽：%.4f\n", width);*grasp_state += 1;printf("GoodsonShelf[%d][%d] need %s\n", CurrentShelf, TargetIndex, index2name(TargetGood));if (!strcmp("water bottle", objects[i].model))//水杯特殊高度lift(height = 0.12);else if (!strcmp("cereal box red", objects[i].model))//红大盒子特殊高度lift(height = 0.50);else if (!strcmp("cereal box", objects[i].model))lift(height = 0.30);else if (TargetIndex < 8)lift(height = 0.05);else if (CurrentShelf % 2 == 0) //矮柜0.23lift(height = 0.23);else if (CurrentShelf % 2 == 1) //高柜0.43lift(height = 0.43);printf("升降臂已举起height = %.3f\n", height);wb_robot_step(10000 / TIME_STEP);}}}else if (*grasp_state == 2) //举{return true;}break;}}return false;
}//寻找空货架 给四个定点GPS 摄像头看四面墙 返回货架位置和一个商品种类
bool Find_Empty(WbDeviceTag camera)
{int number_of_objects = wb_camera_recognition_get_number_of_objects(camera);// printf("识别到 %d 个物体.\n", number_of_objects);const WbCameraRecognitionObject *objects = wb_camera_recognition_get_objects(camera);int GoodsonShelf[4][16];      //货架上的物品ID号 先下后上 先左后右for (int i = 0; i < 4; i++)for (int j = 0; j < 16; j++)GoodsonShelf[i][j] = -1;//不知道为啥写在定义的时候不成功for (int i = 0; i < number_of_objects; ++i){int Shelfx = max(0, floor((objects[i].position[0] + 0.84) * 4.17 + 0.5)); //左右 平均间隔0.24（架子宽度0.25）右移后对应一个系数 四舍五入int Shelfy = (objects[i].position[1] < -0.2) ? 0 : 1;             //上下层 -0.20  为上下分界GoodsonShelf[CurrentShelf][Shelfy * 8 + Shelfx] = name2index(objects[i].model);printf(" 物体 %s 对应编号 %d 写入[%d] 写入编号为%d\n", objects[i].model, name2index(objects[i].model), Shelfy * 8 + Shelfx, GoodsonShelf[CurrentShelf][Shelfy * 8 + Shelfx]);}//检测完毕 判断下一个要取的货物类型int Empty_Flag = 0;for (int j = 0; j < 16; j++)printf("GoodsonShelf[%d][%d] = %d\n", CurrentShelf, j, GoodsonShelf[CurrentShelf][j]);for (int j = 0; j < 16; j++){// if (strlen(GoodsonShelf[CurrentShelf][j]) == 0) // 这种判断可能会crash//   Empty_Flag = 1;if (GoodsonShelf[CurrentShelf][j] == -1){Empty_Flag = 1;TargetIndex = j;//寻找邻近货物 判断应该取的货物类型//直接覆盖 假装已经放上去了int TargetFloor = 0;if (j > 7)TargetFloor += 8; //层数无关if (j % 8 < 4)for (int k = 0; k < 8; k++)//从左往右{if (GoodsonShelf[CurrentShelf][TargetFloor + k] != -1){// strcpy(TargetGood, GoodsonShelf[CurrentShelf][TargetFloor + k]);TargetGood = GoodsonShelf[CurrentShelf][TargetFloor + k];break;}}elsefor (int k = 7; k >= 0; k--)//从右往左{if (GoodsonShelf[CurrentShelf][TargetFloor + k] != -1){// strcpy(TargetGood, GoodsonShelf[CurrentShelf][TargetFloor + k]);TargetGood = GoodsonShelf[CurrentShelf][TargetFloor + k];break;}}printf("GoodsonShelf[%d][%d] need %s\n", CurrentShelf, j, index2name(TargetGood));break;}}if (Empty_Flag)return true;elsereturn false;
}

避障功能

//GPS运动到指定位姿，返回bool值反馈是否到达，默认精度0.05
bool Moveto_CertainPoint1(double fin_posture[], double reach_precision)
{if (targetdist_reached(fin_posture, reach_precision) && targetpos_reached(fin_posture, reach_precision)){// printf("到达目标位置！\n");// base_reset();return true;}else{caculate_tmp_target(tmp_target_posture, fin_posture);base_goto_set_target(tmp_target_posture[0], tmp_target_posture[1], tmp_target_posture[2]);// printf("Target:%s\n", point_name[point_index]);// printf("initial target： %.3f  %.3f  %.3f\n", initial_posture[0], initial_posture[1], initial_posture[2]);// printf("tmp target： %.3f  %.3f  %.3f\n", tmp_target_posture[0], tmp_target_posture[1], tmp_target_posture[2]);// printf("final target： %.3f  %.3f  %.3f\n\n", fin_posture[0], fin_posture[1], fin_posture[2]);base_goto_run();return false;}
}
bool Moveto_CertainPoint(double fin_posture[], double reach_precision)
{if (targetdist_reached(fin_posture, reach_precision) && targetpos_reached(fin_posture, reach_precision)){// printf("到达目标位置！\n");// base_reset();return true;}else{int i, bizhang_flag;double ds[10];bizhang_flag = 0;for (i = 0; i < 10; i++){ds[i] = wb_distance_sensor_get_value(distanse_sensor[i]);if ((i == 0 || i == 7 ) && main_state != Arround_Moving && main_state != Back_Moving){ds[i] = ds[i] * 1;}if ((i == 8 || i == 9) && (main_state == Arround_Moving || main_state == Back_Moving || main_state == RunTo_NextShelf)){ds[i] =  ds[i] * 0.4;}if (ds[i] < 0)ds[i] = 0;if (ds[i] < 350){bizhang_flag = 1;}}if (bizhang_flag == 1 && main_state != Grab_Item && main_state != Item_Loading && main_state != RunTo_NextShelf && main_state != TurnBack_To_LoadItem){//printf("正在避障\n");bizhang(fin_posture, ds);}else{caculate_tmp_target(tmp_target_posture, fin_posture);base_goto_set_target(tmp_target_posture[0], tmp_target_posture[1], tmp_target_posture[2]);}// printf("Target:%s\n", point_name[point_index]);// printf("initial target： %.3f  %.3f  %.3f\n", initial_posture[0], initial_posture[1], initial_posture[2]);// printf("tmp target： %.3f  %.3f  %.3f\n", tmp_target_posture[0], tmp_target_posture[1], tmp_target_posture[2]);// printf("final target： %.3f  %.3f  %.3f\n\n", fin_posture[0], fin_posture[1], fin_posture[2]);base_goto_run();return false;}
}void bizhang(double fin_posture[], double ds[])
{	//最小的ds，当成为机器人移动方向//base_reset();int i;double move_direction_x = 0;double move_direction_y = 0;double move_time = 0.5;for (i = 0; i < 10; i++){move_direction_x = move_direction_x + (1000 - ds[i]) / 1000 * bizhang_x[i];move_direction_y = move_direction_y + (1000 - ds[i]) / 1000 * bizhang_y[i];}if (move_direction_y < 0){base_backwards();passive_wait(abs(move_direction_y)* move_time);}else{base_forwards();passive_wait(abs(move_direction_y) * move_time);}if (move_direction_x < 0){base_strafe_left();passive_wait(abs(move_direction_x) * move_time);}else{base_strafe_right();passive_wait(abs(move_direction_x) * move_time);}/*if (ds[8] < min_distance || ds[9] < min_distance){if (ds[0] > ds[7]){base_strafe_left();passive_wait(0.5 * move_time);base_backwards();passive_wait(0.5 * move_time);}else{base_strafe_right();passive_wait(0.5 * move_time);base_backwards();passive_wait(0.5 * move_time);}}else if (ds[0] < min_distance || ds[1] < min_distance){base_strafe_right();passive_wait(0.5 * move_time);base_backwards();passive_wait(0.5 * move_time);}else if (ds[2] < min_distance || ds[3] < min_distance){base_strafe_right();passive_wait(0.5 * move_time);base_forwards();passive_wait(0.5 * move_time);}else if (ds[4] < min_distance || ds[5] < min_distance){base_strafe_left();passive_wait(0.5 * move_time);base_forwards();passive_wait(0.5 * move_time);}else if (ds[6] < min_distance || ds[7] < min_distance){base_strafe_left();passive_wait(0.5 * move_time);base_backwards();passive_wait(0.5 * move_time);}else{}*/
}

总结

本仿真完成参考了往届学长的思路流程并在其中调整了货堆摆放位置、修改了视觉处理及循迹策略，同时HIA增加了障碍物与避障功能等一系列改进。