声明：软件截图为个人所有，严禁用于商业目的和其他盈利行为。

云台控制和激光点云获取软件

第一阶段：三维点云采集可视化

第二阶段：MoblieSim仿真数据接入，sim_lms1xx_1激光。Gmapping建图实现效果图，感觉效果一般，估计有的参数还是要调整一下。

进一步工作：

（1）当前真实轨迹，当前pose；<Done>

（2）目前建图过程延迟很厉害，因为加入队列的数据RangeReading数目太多，需要进行控制；

　　思路：减速增加队列中的无效数据（不进行扫描匹配）；机器人处于停止状态不读取激光数据；时间间隔控制；

（3）图的大小不是实际的坐标值，需要对应上；<Done>

（4）ArServerModelGoto中的速度控制模式，控制运行到位置；<Done>

 

  

第三阶段：鼠标控制初步（没有局部避障和路径规划）

今天在机器人上真实环境测试了一下，首先鼠标控制和运动到位置的服务都是可用的。问题：运动过程中老是撞到障碍，有室内太拥挤的原因，因此急需增加局部避障功能。

同时考虑是不是机器人里程计提供的位姿飘的太厉害了，我看着地图控制怎么会撞呢？

机器人启动之后即产生位姿（世界，机器人原点），当开始建图时的机器人位姿（相对于机器人原点）已经不为0。但是建图用的地图原点产生了一个新的坐标系。

第四阶段：实现了VFF局部避障算法（Done 2017/6/6）

第五阶段：边界探索 （初步完成，很多问题 2017/6/8）

　　完成一个简单版本的自主探索，目前还有很多问题：

1.根据True Pose进行运算而非里程计 （Done）

2.控制根据True Pose相对方向运动 （Done，思考该方法是有问题的！）

3.自主探索开口点提取的太粗糙，需要精化 （未完成）

　　

实际环境测试，出现问题：像木马一样震颤（2017/6/20）

可能原因：

（1）

VFF算法

局部避障需要延时，建议值1s （Done）

（2）

激光的5m范围内存在过小的值，无效值1mm （Done）

（3）

服务器端需要增加配置文件，控制激光不连接 (Done)

（4）按钮实现的速度控制需要限制前进速度

（5）

优化服务端，尤其是旋转的速度要控制的低速旋转（Done，目前+-15度以内速度不控制，以外按照Heading方向10度间隔旋转）

反馈：

修改了（1）（2）（3）之后，实际环境中鼠标算是可以控制了。

 第六阶段：服务优化，算法优化

 　　（1）$t1$完成扫描匹配，得到真实位姿$T_{1}$，用$t1$时刻的真实位姿和里程计来更新$t_{2}, t_{3}, ... , t_{10}$时刻的里程计。则$t_{10}$时刻有，$T_{10}=T_{1}*Odom_{1}^{-1}*(Odom_{10})$

　　（2）服务器端的旋转都按照Heading方向相对运动，因为里程计偏差太大。根据TruePose计算的Map坐标系下的目标方向，和Odometry中相差太大。

　　（1）（2）两点都归结为相对运行，相对于机器人前一时刻的Heading进行旋转。

 

2017/6/26室外测试，没有问题，只是走廊地板的摩擦力好大，旋转不流畅，阻力很大！

2017/8/4修改：旋转角度很大时，采用匀速旋转；角度比较小时则采用旋转到角度。

视频地址：