执行定位算法得到即时位置坐标，判断位置坐标是否在预定轨迹范围内，如果在预定的轨迹范围运行，则保持此姿态继续行走，如果超出预定的轨迹范围，则调整运行姿态。调整运行姿态的具体方法是若向左超出预定轨迹，则左轮速度加大，若向右超出预测范围，则右轮速度加大；以偏离中心的位移量和当前的航向角为输入，以左右轮的速度差为输出，实现在线模糊控制。
[0034]如附图3所示，所述第四步中执行定位算法得到即时位置坐标的具体步骤是:
[0035]第一步，在取餐处对里程计定位位置进行初始化，消除在每一次送餐过程中的累计误差。
[0036]第二步，采用里程计定位算法得到即时送餐机器人的位置增量和位置坐标；
[0037]第三步，然后通过机器人运动速度和位移判断超宽带定位系统工作是否正常，若机器人速度大于零，且位移与上个控制周期相比发生变化，但超宽带定位系统返回到各参考节点的距离没变，则判断为超宽带定位工作不正常，则采用里程计得到的位置增量和上次位置信息为当前即时位置坐标;若超宽带信号工作正常，则采用超宽带定位的结果与里程计定位结果通过卡尔曼滤波算法进行融合得到当前即时位置坐标。
[0038]本实施例中左轮电机3和右轮电机4实现机器人的运行速度和转向控制；电机驱动器2将24V直流电压变换成可调电压控制左轮电机3和右轮电机4的运转，同时计算电机旋转的圈数，通过串口接收控制板I的控制指令并发送电机旋转的圈数。定位系统采用超宽带和里程计定位融合的方法，超宽带定位算法在定位引擎服务器503中运算；定位引擎服务器503同时通过无线数传方式接收控制板传来的左右电机的里程信息并做基于里程计的定位运算;然后通过融合算法得到机器人的实时位置坐标，并将定位结果通过无线数传方式传给机器人上的控制板I。红外和超声波模块7实现障碍物检测，能够实时检测出不同高度的障碍物，遇到障碍物时机器人停止运动，并语音播报。
[0039]进一步，本实施例采用具体型号按照上述方法连接及控制，左轮电机3和右轮电机4选用100W无刷直流电机，配100W双路电机驱动器2，电机驱动器2通过RS-232与控制板I通讯，可以通过指令控制电机起停和运行速度，同时可以返回电机旋转的圈数用于计算行走位移。控制板I采用STM32系统实现，带有RS-232和无线数传方式通讯接口。红外和超声波模块7通过1口送入控制板I用于避障控制。第一电池模块8供电和第二电池模块9采用24V锂电池。超宽带定位系统5采用四个固定节点和一个移动节点，定位算法在服务器中实现，位置刷新频率为2?30Hz可选。
[0040]超宽带定位系统5的四个固定参考节点501分别安放在20X 20m的空间的四个角，安装高度为2.5m。移动节点安放在轮式机器人上，安装高度为1.3m。机器人两轮之间间距为390mm，轮子直径为200mm。超宽带定位系统5的位置刷新频率取1Hz，里程计采样周期取50Hz，轨迹控制周期取200ms;电机转速设置为lOOOrpm，换算成机器人运行速度为0.28m/s。当机器人沿直线X= 1m行走时(从Y= Im沿着Y增大的方向运行)，单独采用UWB定位方法和采用UWB与里程计相结合的定位方法的定位曲线如附图4所示。
[0041]
[0042]从图4中可以看出，当单独采用超宽带定位系统5定位时，定位结果波动较大，基本在±10cm内波动，这可能会导致机器人轨迹控制的误操作，从而引起机器人控制平稳性和循迹精度变差。当采用基于UWB和里程计融合算法实现定位时，定位误差波动较小，基本在± 4cm以内，完全满足送餐机器人轨迹控制的要求。
【主权项】
1.一种送餐机器人控制系统，其特征在于:它包括控制板，所述控制板通过串口与带有里程计算功能的电机驱动器连接，电机驱动器能够驱动机器人的左轮电机和右轮电机，所述控制板连接有超宽带定位系统、姿态传感器和红外和超声波模块； 所述超宽带定位系统包括参考节点、机器人上的移动节点和定位引擎服务器，参考节点和机器人上的移动节点之间通过超宽带通信，服务器和机器人上的移动节点之间通过无线数传方式传输数据，服务器通过无线数传方式接收控制板传来的左右电机的里程信息。2.如权利要求1所述的一种送餐机器人控制系统，其特征在于:所述控制板，机器人上的移动节点和姿态传感器，红外和超声波模块及电机驱动器通过电池模块供电。3.上述任一项权利要求所述的送餐机器人控制系统的运动轨迹控制方法，其特征在于，它包括以下几个步骤: (1)根据餐厅布局确定各餐桌的位置坐标和送餐机器人行走轨迹的坐标区间； (2)机器人根据操作人员选定的目标桌位确定目的坐标，然后根据目的坐标选定行走路径； (3)从姿态传感器读取运动姿态信息，调整运动姿态后开始运动； (4)执行定位算法得到即时位置坐标，判断位置坐标是否在预定轨迹范围内，如果在预定的轨迹范围运行，则保持此姿态继续行走，如果超出预定的轨迹范围，则调整运行姿态。4.如权利要求3所述的送餐机器人控制系统的运动轨迹控制方法，其特征在于:所述步骤(4)中调整运行姿态的具体方法是若向左超出预定轨迹，则左轮速度加大，若向右超出预测范围，则右轮速度加大；以偏离中心的位移量和当前的航向角为输入，以左右轮的速度差为输出，实现在线模糊控制。5.如权利要求3所述的送餐机器人控制系统的运动轨迹控制方法，其特征在于:所述步骤(4)中执行定位算法得到即时位置坐标的具体步骤是: (a)在取餐处对里程计定位位置进行初始化，消除在每一次送餐过程中的累计误差。 (b)采用里程计定位算法得到即时送餐机器人的位置增量和位置坐标； (c)然后通过机器人运动速度和位移判断超宽带定位系统工作是否正常，若机器人速度大于零，且位移与上个控制周期相比发生变化，但超宽带定位系统返回到各参考节点的距离没变，则判断为超宽带定位工作不正常，则采用里程计得到的位置增量和上次位置信息为当前即时位置坐标;若超宽带信号工作正常，则采用超宽带定位的结果与里程计定位结果通过卡尔曼滤波算法进行融合得到当前即时位置坐标。
【专利摘要】一种送餐机器人控制系统及其运动轨迹控制方法，它包括控制板，所述控制板通过串口与带有里程计算功能的电机驱动器连接，电机驱动器能够驱动机器人的左轮电机和右轮电机，所述控制板连接有超宽带定位系统、姿态传感器和红外和超声波模块；本发明具有精度高、稳定性好、性价比高的优点。其不需要铺设导航轨道，可以实现按预设的路径自主行走，当餐厅布局改变时，该方法不要施工，只需要在软件中修改预设轨迹即可工作。
【IPC分类】B25J9/16
【公开号】CN105563485
【申请号】CN201510973459
【发明人】李传江
【申请人】上海师大资产经营有限责任公司, 李传江
【公开日】2016年5月11日
【申请日】2015年12月21日