本发明公开了一种可实现移动机器人自主导航与精确定位的控制系统及方法，该系统包括传感器模块、主控模块和执行模块；主控模块接收传感器模块采集的数据，计算轮式里程计数据，构建环境静态地图和动态地图，估计移动机器人的位姿，规划移动机器人的轨迹，并进行轨迹跟随，下发轨迹跟随指令至执行模块；待机器人到达目标点后，获取精准定位相机采集的地标图像数据，计算移动机器人中心与地标的位置偏差，对移动机器人的位姿进行校正，并反馈精准定位指令至执行模块，执行模块控制移动机器人朝着地标位置方向运动，最终实现毫米级定位。
1.一种可实现移动机器人自主导航与精确定位的控制系统，其特征是，包括设置在移动机器人上的传感器模块、主控模块和执行模块；
所述主控模块连接传感器模块，接收传感器模块采集的移动机器人所处环境轮廓数据、移动机器人在三维空间中的速度与位置数据、移动机器人的车轮转速数据及机器人前方图像数据，计算轮式里程计数据，构建环境静态地图和动态地图，估计移动机器人的位姿，规划移动机器人的轨迹，并进行轨迹跟随，下发轨迹跟随指令至执行模块；待机器人到达目标点后，获取传感器模块采集的地标图像数据并对其进行处理，计算移动机器人中心与地标的位置偏差，对移动机器人的位姿进行校正，并反馈精准定位指令至执行模块；
所述执行模块连接主控模块，用于接收到主控模块下发的轨迹跟随指令后驱动移动机器人行驶至目标点；接收到主控模块下发的精准定位指令后控制移动机器人朝着地标位置方向运动；
所述工控机Ⅰ，用于接收传感器模块采集的移动机器人所处环境轮廓数据、移动机器人在三维空间中的速度与位置数据、移动机器人的车轮转速数据、机器人前方图像数据以及地标图像数据，计算轮式里程计数据，构建环境静态地图和动态地图，估计移动机器人的位姿，规划移动机器人的轨迹，并将生成的轨迹传输至工控机Ⅱ；
工控机Ⅱ，用于接收传感器模块采集的移动机器人在三维空间中的速度与位置数据以及移动机器人的车轮转速数据，根据工控机Ⅰ估计的移动机器人的位姿以及生成的轨迹，进行轨迹跟随；获取传感器模块采集的地标图像数据并对其进行处理，计算移动机器人中心与地标的位置偏差，采用PID算法对移动机器人的位姿进行校正，并反馈精准定位指令至执行模块；
(1)间隔采样路径规划单元得到的路径点，计算路径长度的初始时间分配；
为加速计算，通过间隔采样减少从路径规划单元获得的密集路径点，并计算路径长度；
使用S形ACC/DEC算法来进行路径总长度的初始时间分配；根据速度减速度限制vmax/amax和公式(2)(3)表示的加速度/速度限制，计算得到路径总长度的初始时间分配s(t)，如公式(1)；
(2)对路径长度的初始时间分配进行离散化处理，采用二分法确定路径点的时间分配；
直接求解s(t)的反函数时需要的时间长，因此提出了使用二分法快速求解；首先离散化s(t)，然后使用二分法来快速查找路径点的相应时间分配；在相应的路径点之间产生的时间分配用于后续的轨迹规划；
(3)采用三次样条法对确定的路径点的时间分配进行曲线拟合，得到分段三次样条多项式组成的轨迹；
由分段三次样条多项式组成的轨迹被参数化为每个维度μ中的时间变量t，其中μ取值于x和y；每个维度上的Nth阶M段轨迹可以写成如下：其中， 是ith段轨迹的jth阶多项式系数，jth的值可以是3，T1:T2…TM是每个细分的结束时间，总时间为TM-T0；通过并三次样条多项式进行求解，得到相应的轨迹ημ(t)；
由于前一次使用S形ACC/DEC算法分配路径长度，忽略了路径的曲率变化，获得不满足动态约束的轨迹，针对该问题提出了时间重新分配方法；
在上一步骤的第一次时间分配之后，需要再次为速度和加速度超过动态限制的任何段进行时间间隔调整；
整个轨迹由多段曲线组成，对轨迹进行采样以获得轨迹中每段轨迹的最大速度和加速度的绝对值，用vm和am表示；其中vmax和amax是最大速度和加速度极限值的绝对值，并且vm和am是所生成轨迹的每个段中的最大速度和加速度的绝对值；如果某段归轨迹的vm/vmax或
(am/amax) 超过α，则认为相应的时间分配接近饱和，有必要调整分段曲线的时间分配，得到新的时间分配；如果没有，继续使用以前的时间分配；
其中，α是一个小于1的值，表示vm和vmax以及am和amax的接近程度；系数k可以有效地防止任何时间跨度过度延伸并变得过于保守；
在时间重新分配之后，再次使用三次样条方法来生成轨迹；由于路径点经过前述采样步骤，可能导致产生的轨迹与狭窄区域中的障碍物碰撞；如果轨迹与障碍物碰撞，则在碰撞处及其周围添加路径点，并再次执行轨迹规划；如果未发生碰撞，则将轨迹输出值轨迹下发单元。
2.根据权利要求1所述的可实现移动机器人自主导航与精确定位的控制系统，其特征是，所述传感器模块包括两个激光雷达、惯性测量单元、编码器、防撞传感器、感知用相机和精准定位相机；
所述激光雷达，用于扫描移动机器人所处环境区域，提取环境轮廓信息，并发送给主控模块；
所述惯性测量单元，用于测量移动机器人在三维空间中的速度与位置信息，并发送给主控模块；
所述编码器，用于测量移动机器人的车轮转速，并发送给主控模块；
所述防撞传感器，用于测量障碍物与移动机器人之间的距离，若障碍物与移动机器人之间的距离小于设定的报警距离，则发出信号至主控模块；
所述感知用相机，用于采集的机器人前方图像数据，并发送给主控模块；
所述精准定位相机，用于采集地标图像数据，并发送给主控模块。
3.根据权利要求1所述的可实现移动机器人自主导航与精确定位的控制系统，其特征是，所述工控机Ⅰ包括环境感知模块；所述环境感知模块包括地图构建单元和状态估计单元，其中：所述地图构建单元，用于利用移动机器人在三维空间中的速度与位置数据以及移动机器人的车轮转速数据计算轮式里程计数据，基于移动机器人所处环境轮廓数据、感知用相机采集的机器人前方图像数据以及轮式里程计数据，构建环境静态地图和动态地图，将得到的环境静态地图和动态地图传输给规划模块；
所述状态估计单元，用于利用移动机器人在三维空间中的速度与位置数据以及移动机器人车轮的转速数据计算轮式里程计数据，利用扩展卡尔曼滤波方法和自适应蒙特卡洛方法将移动机器人在三维空间中的速度与位置数据、移动机器人所处环境轮廓数据、感知用相机采集的机器人前方图像数据以及轮式里程计数据进行融合，得到移动机器人的位置和姿态，将得到的移动机器人的位置反馈给地图构建单元，将移动机器人的姿态反馈给工控机Ⅱ；
通过地图构建单元构建环境静态地图和小范围的环境动态地图，所述小范围的环境动态地图建立为：从机器人前后的激光雷达扫描的区域内划定一小块区域建立动态地图，用来反映环境的动态变化，动态地图与静态地图叠加用于路径规划与避障。
4.根据权利要求1所述的可实现移动机器人自主导航与精确定位的控制系统，其特征是，所述工控机Ⅰ还包括规划模块，所述规划模块包括路径规划单元、轨迹规划单元以及轨迹下发单元，其中：所述路径规划单元，用于利用环境感知模块构建的环境静态地图以及动态地图，采用搜索类方法生成无碰撞路径；
所述轨迹规划单元，用于对路径规划单元得到的路径进行处理，采用轨迹规划方法生成移动机器人执行轨迹；
所述轨迹下发单元，用于将轨迹规划单元生成的轨迹下发至工控机Ⅱ。
5.根据权利要求1所述的可实现移动机器人自主导航与精确定位的控制系统，其特征是，所述工控机Ⅱ包括地标位姿反馈模块、轨迹跟随模块和视觉精定位模块，其中：所述地标位姿反馈模块，用于获取精定位相机采集的地标图像数据，并反馈给视觉精定位模块；
所述轨迹跟随模块，用于获取移动机器人在三维空间中的速度及位置数据以及移动机器人的车轮转速数据，根据环境感知模块估计的移动机器人的位姿以及规划模块生成的轨迹，利用反演法或者模型预测控制方法进行轨迹跟随，并下发轨迹跟随指令至执行模块；
所述视觉精定位模块，用于对地标位姿反馈模块反馈的地标图像数据进行处理，利用PID算法对移动机器人的位姿进行校正，并下发定位指令至执行模块。
6.一种可实现移动机器人自主导航与精确定位的控制方法，其特征是，该方法包括以下步骤：工控机Ⅰ获取传感器模块采集的移动机器人所处环境轮廓数据、移动机器人在三维空间中的速度与位置数据、移动机器人的车轮转速数据及机器人前方图像数据，计算轮式里程计数据，构建环境静态地图和动态地图，估计移动机器人的位姿，规划移动机器人的轨迹，并传输至工控机Ⅱ；
工控机Ⅱ获取传感器模块采集的移动机器人在三维空间中的速度与位置数据以及移动机器人的车轮转速数据，根据工控机Ⅰ估计的移动机器人的位姿以及生成的轨迹，利用反演法或者模型预测控制方法进行轨迹跟随，并下发轨迹跟随指令至执行模块；
执行模块接收到主控模块下发的轨迹跟随指令后，驱动移动机器人行驶至目标点；
待机器人到达目标点后，工控机Ⅱ获取精准定位相机采集的地标图像数据并对其进行处理，计算移动机器人中心与地标的位置偏差，利用PID算法对移动机器人的位姿进行校正，并反馈定位指令至执行模块；
执行模块根据接收到的主控模块下发的定位指令控制移动机器人朝着地标位置方向运动，待移动机器人中心与地标位置偏差小于设定的偏差阈值后控制移动机器人停止运动；
间隔采样得到的路径点，使用S形ACC/DEC算法计算路径长度的初始时间分配；
对路径长度的初始时间分配进行离散化处理，采用二分法查找路径点的相应时间分配；
采用三次样条法对路径点的时间分配进行曲线拟合，得到多段轨迹；
判断每段轨迹的最大速度的绝对值与最大速度阈值的绝对值的比值或每段轨迹的加速度的绝对值与加速度阈值的绝对值的比值是否大于设定的阈值；
若大于，则调整该段轨迹曲线的时间分配，得到新的时间分配，采用三次样条方法对新的时间分配进行曲线拟合，生成新的轨迹段；否者，不调整时间分配；
判断轨迹与障碍物是否发生碰撞，若发生，则在碰撞处及其周围添加路径点，并再次执行轨迹规划；若未发生，则输出生成的轨迹。
7.根据权利要求6所述的可实现移动机器人自主导航与精确定位的控制方法，其特征是，所述环境静态地图和动态地图的构建方法为：使用catorgrapher算法建立环境静态地图；
从激光雷达扫描的机器人周围所处环境区域内划定一小块区域，建立动态地图。
8.根据权利要求6所述的可实现移动机器人自主导航与精确定位的控制方法，其特征是，所述移动机器人的位姿的估计方法为：采用扩展卡尔曼滤波算法将移动机器人在三维空间中的速度与位置数据和里程计数据进行融合，得到移动机器人初始的位姿数据；
将移动机器人所处环境轮廓数据进行融合，并将其转换到移动机器人坐标系下，将移动机器人坐标系下的环境轮廓数据转换为点云数据；
采用自适应蒙特卡洛方法将移动机器人初始的位姿数据与得到的点云数据进行融合，得到移动机器人的位姿数据。
可实现移动机器人自主导航与精确定位的控制系统及方法
技术领域
[0001] 本公开涉及移动机器人控制技术领域，具体涉及一种可实现移动机器人自主导航与精确定位的控制系统及控制方法。
背景技术
[0002] 目前只基于激光雷达、只基于视觉(相机)、或者激光雷达与视觉(相机)结合实现自主导航的移动机器人到达目标点后定位精度在厘米级，但是在某些工况下需要毫米级别的精确定位，所以亟需设计可以实现移动机器人自主导航与精确定位的控制系统及其控制方法。
[0003] 发明人在研发过程中发现，目前大多数与ROS机器人操作系统结合的控制器多用STM32等结构较为简单的控制器来响应ROS机器人操作系统端规划出来的速度指令。而ROS机器人操作系统存在实时性不强的问题，不利于进行轨迹跟随等反馈控制，故造成控制精度不高、控制频率低的问题。
发明内容
[0004] 为了克服上述现有技术的不足，本公开提供了一种可实现移动机器人自主导航与精确定位的控制系统及其控制方法，可以进行实时轨迹跟随控制，实现移动机器人自主导航与精确定位。
[0005] 本公开一方面提供的一种可实现移动机器人自主导航与精确定位的控制系统的技术方案是：
[0006] 一种可实现移动机器人自主导航与精确定位的控制系统，其特征是，包括设置在移动机器人上的传感器模块、主控模块和执行模块；
[0007] 所述主控模块连接传感器模块，接收传感器模块采集的移动机器人所处环境轮廓数据、移动机器人在三维空间中的速度与位置数据、移动机器人的车轮转速数据及机器人前方图像数据，计算轮式里程计数据，构建环境静态地图和动态地图，估计移动机器人的位姿，规划移动机器人的轨迹，并进行轨迹跟随，下发轨迹跟随指令至执行模块；待机器人到达目标点后，获取传感器模块采集的地标图像数据并对其进行处理，计算移动机器人中心与地标的位置偏差，对移动机器人的位姿进行校正，并反馈精准定位指令至执行模块；
[0008] 所述执行模块连接主控模块，用于接收到主控模块下发的轨迹跟随指令后驱动移动机器人行驶至目标点；接收到主控模块下发的精准定位指令后控制移动机器人朝着地标位置方向运动。
[0009] 本公开一方面提供的一种可实现移动机器人自主导航与精确定位的控制方法的技术方案是：
[0010] 一种可实现移动机器人自主导航与精确定位的控制方法，该方法包括以下步骤：
[0011] 工控机Ⅰ获取传感器模块采集的移动机器人所处环境轮廓数据、移动机器人在三维空间中的速度与位置数据、移动机器人的车轮转速数据及机器人前方图像数据，计算轮式里程计数据，构建环境静态地图和动态地图，估计移动机器人的位姿，规划移动机器人的轨迹，并传输至工控机Ⅱ；
[0012] 工控机Ⅱ获取传感器模块采集的移动机器人在三维空间中的速度与位置数据以及移动机器人的车轮转速数据，根据工控机Ⅰ估计的移动机器人的位姿以及生成的轨迹，利用反演法或者模型预测控制方法进行轨迹跟随，并下发轨迹跟随指令至执行模块；
[0013] 执行模块接收到主控模块下发的轨迹跟随指令后，驱动移动机器人行驶至目标点；
[0014] 待机器人到达目标点后，工控机Ⅱ获取精准定位相机采集的地标图像数据并对其进行处理，计算移动机器人中心与地标的位置偏差，利用PID算法对移动机器人的位姿进行校正，并反馈定位指令至执行模块；
[0015] 执行模块根据接收到的主控模块下发的定位指令控制移动机器人朝着地标位置方向运动，待移动机器人中心与地标位置偏差小于设定的偏差阈值后控制移动机器人停止运动。
[0016] 通过上述技术方案，本公开的有益效果是：
[0017] (1)本公开采用两台工控机、若干传感器与执行器，两台工控机之间通过网络通信协议进行通信，既利用了机器人操作系统ROS的强大的建图定位能力，又加入了运动控制器进行实时运动控制，提高了整个系统的实时性。
[0018] (2)目前基于激光雷达的自主导航、基于视觉(相机)的自主导航、或者激光雷达与视觉(相机)结合的自主导航方式可实现厘米级定位，本公开通过自主导航与视觉相结合精定位方式，实现毫米级定位。
[0019] (3)本公开结合扩展卡尔曼滤波与自适应蒙特卡洛方法的多传感器融合定位方法，实现移动机器人的准确位姿估计。
[0020] (4)本公开提出的轨迹规划方法可以生成满足动力学约束的平滑轨迹，可显著减少传动结构冲击延长移动机器人使用寿命。
附图说明
[0021] 构成本公开的一部分的说明书附图用来提供对本公开的进一步理解，本公开的示意性实施例及其说明用于解释本申请，并不构成对本公开的不当限定。
[0022] 图1是实施例一移动机器人的结构图；
[0023] 图2是实施例一可实现移动机器人自主导航与精确定位的控制系统的结构图；
[0024] 图3是实施例一主控模块的结构图一；
[0025] 图4是实施例一主控模块的结构图二；
[0026] 图5是实施例一移动机器人位姿估计方法的流程图；
[0027] 图6是实施例一轨迹规划方法的流程图；
[0028] 图7是实施例一时间重分配的流程图。
具体实施方式
[0029] 下面结合附图与实施例对本公开作进一步说明。
[0030] 应该指出，以下详细说明都是例示性的，旨在对本公开提供进一步的说明。除非另有指明，本公开使用的所有技术和科学术语具有与本公开所属技术领域的普通技术人员通常理解的相同含义。
[0031] 需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0032] 实施例一
[0033] 本实施例提供一种可实现移动机器人自主导航与精确定位的控制系统，可以进行实时轨迹跟随控制，实现移动机器人自主导航与精确定位。
[0034] 请参阅附图1，所述控制系统包括设置在移动机器人上的传感器模块、主控模块和执行模块。
[0035] 请参阅附图2，所述传感器模块包括激光雷达、惯性测量单元、编码器、防撞传感器、红外回冲单元、感知用相机和精准定位相机。
[0036] 所述激光雷达的数量为2个，分别安装在移动机器人的前侧和后侧，且位于同一对角线上，用于扫描移动机器人所处环境区域，提取环境轮廓信息，并发送给主控模块。
[0037] 所述惯性测量单元，安装在机器人底面中轴线任一位置，用于测量移动机器人前向、侧向、旋转方向三维方向上的速度与位置信息，并发送给主控模块；
[0038] 所述编码器安装在驱动移动机器人的车轮的电机上，用于测量电机轴转过的角度，从而可以得到移动机器人车轮的转速，并发送给主控模块。
[0039] 所述防撞传感器可为超声传感器，安装在移动机器人四周(图中未示)，用于测量障碍物与移动机器人之间的距离，若障碍物与移动机器人之间的距离小于设定的报警距离，则发出信号至主控模块。
[0040] 所述红外回冲单元，安装在车体后端即可(图中未示)，用于引导移动机器人去充电桩充电。
[0041] 请参阅附图1，所述感知用相机设置在移动机器人的顶部前侧，用于采集机器人前方的图像数据，可用于动态障碍物的追踪、物体识别等。
[0042] 所述精准定位相机设置在移动机器人的底部，用于采集人工地标的图像数据，如二维码，用于位姿矫正达到精确定位。
[0043] 请参阅附图4，所述主控模块包括安装有Linux系统和ROS机器人操作系统的工控机Ⅰ和安装有Windows7操作系统和Kithara RTS的工控机Ⅱ，所述工控机Ⅰ和工控机Ⅱ通过网线通信连接，实现数据的交互。
[0044] 所述工控机Ⅰ连接激光雷达和感知用相机，用于获取激光雷达提取的环境轮廓信息，感知用相机采集的机器人前方图像数据，以及工控机Ⅱ上传的惯性测量单元测量的移动机器人的速度与位置数据，编码器测量的移动机器人车轮的转速数据，精定位相机采集的人工地标图像数据，构建环境地图，估计移动机器人的位置和状态，规划移动机器人运动轨迹。
[0045] 请参阅附图2，所述工控机Ⅰ包括环境感知模块和规划模块。
[0046] 所述环境感知模块包括地图构建单元和状态估计单元，其中：
[0047] 所述地图构建单元，用于利用惯性测量单元测量的移动机器人在三维空间中的速度与位置数据和编码器测量的移动机器人车轮的转速数据计算轮式里程计数据，基于激光雷达提取的环境轮廓信息、感知用相机采集的机器人前方图像数据，惯性测量单元测量的移动机器人在三维空间中的的速度与位置数据，以及轮式里程计数据，构建环境静态地图和小范围的环境动态地图，将得到的环境静态地图和动态地图传输给规划模块。
[0048] 具体地，环境静态地图构建方法具体为：
[0049] 使用catorgrapher算法建立环境静态地图，该算法使用UKF融合多传感器数据来创建子图和闭环检测策略，从而实现5cm分辨率的精确地图。
[0050] 具体地，所述小范围的环境动态地图建立为：
[0051] 从机器人前后的激光雷达扫描的区域内划定一小块区域建立动态地图，用来反映环境的动态变化，动态地图与静态地图叠加用于路径规划与避障。
[0052] 所述状态估计单元，用于利用惯性测量单元测量的移动机器人在三维空间中的速度与位置数据和编码器测量的移动机器人车轮的转速数据计算轮式里程计数据，基于两个激光雷达提取的环境轮廓信息、感知用相机采集的机器人前方图像数据，惯性测量单元测量的移动机器人在三维空间中的速度与位置数据，以及轮式里程计数据，利用结合扩展卡尔曼滤波与自适应蒙特卡洛方法的多传感器融合定位方法将多传感器数据进行融合，确定移动机器人的位姿，包括移动机器人的位置和姿态，将得到的移动机器人的位置反馈给地图构建单元，将移动机器人的姿态反馈给工控机Ⅱ。
[0053] 由于移动机器人在平面运动，因此，所确定的移动机器人的位姿包括移动机器人的位置(x,y)和姿态(绕z轴的转角)。
[0054] 请参阅附图5，所述结合扩展卡尔曼滤波与自适应蒙特卡洛方法的多传感器融合定位方法具体为：
[0055] (1)采用扩展卡尔曼滤波将惯性测量单元测量的移动机器人在三维空间中的速度与位置数据与里程计数据融合。
[0056] 扩展卡尔曼滤波(EKF)包括运动模型与测量模型，测量模型可以使用非线性函数，运动模型使用里程计运动模型，预测移动机器人的位姿，测量模型使用likelihood模型，更新纠正移动机器人的位姿；得到新的位姿将成为下一时刻扩展卡尔曼滤波的运动模型的初始状态；采用扩展卡尔曼滤波将使用带有协方差的IMU数据与里程计数据进行融合，得到初始的位姿数据(包含位置和姿态)；
[0057] (2)将两个激光雷达提取的环境轮廓数据融合。
[0058] 由于机械结构设计的局限，本实施例在移动机器人的左前角与右后角分别安装了一个扫描范围为270°的二维激光雷达，在这里，将两个激光雷达提取的环境轮廓数据转换到移动机器人坐标系下，然后将两个激光雷达提取的环境轮廓数据转换为点云数据，融合点云数据后，以统一的格式发布；
[0059] (3)将初始的位姿数据与融合后的激光雷达数据进行融合定位。
[0060] 在本实施例中，采用自适应蒙特卡洛方法进行融合定位，其具体实现方式如下：
[0061] 首先，采用预测模型预测扩展卡尔曼滤波融合后的带有协方差的初始位姿数据，得到此时刻的预测状态；
[0062] 然后，基于观测模型，使用融合后的激光雷达数据，对预测状态进行更新修正，得到此时刻更加准确的位姿；
[0063] 最后，进行每个粒子的重采样，如更新权重，增加随机粒子等操作，现存的粒子将用于下一时刻的位姿估计。
[0064] 通过以上步骤，实现对多传感器数据进行融合，从而实现更准确的移动机器人的位置、姿态的估计。
[0065] 在本实施例中，所述轮式里程计数据的计算方法为：
[0066] 利用编码器测量的移动机器人四个车轮的转速v1、v2、v3、v4，通过逆运动学，解算出移动机器人整体x方向的线速度vx、y方向的线速度vy和z轴方向的角速度ω，其具体计算公式如下：
[0067] vx＝(v1+v2+v3+v4)/4
[0068] vy＝(v2+v4-v1-v3)/4
[0069] ω＝(v3+v4-v1-v3)/(4*(a+b))
[0070] 其中，a、b分别为移动机器人车体长和宽的一半。
[0071] 将相邻时刻Δt得到的轨迹进行累加求和，可计算得到机器人里程计数据，其表达式如下所示：
[0072] x＝x+vxΔtcos(θt)-vyΔtsin(θt)
[0073] y＝y+vxΔtsin(θt)+vyΔtcos(θt)
[0074] θt＝θt+ωΔt
[0075] 其中，θt为t时刻机器人的角度值。
[0076] 所述规划模块包括路径规划单元、轨迹规划单元以及轨迹下发单元，其中：
[0077] 所述路径规划单元，用于利用环境感知模块构建的环境静态地图以及动态地图，采用搜索类方法如A*或采样类方法比如快速拓展随机树等方法，生成无碰撞路径。
[0078] 所述轨迹规划单元，用于对路径规划单元得到的路径进行处理，生成满足移动机器人动力学约束可被执行的轨迹。
[0079] 与传统的移动机器人相比，全向移动机器人可以沿着任何轨迹移动，保持其中心的方向为零。全向移动机器人到达目标点时，将根据运输需要进行方向调整。所以，本实施例只规划x-y方向的轨迹，在机器人移动时保持方向不变。在进行轨迹规划时满足并且可以找到的速度范围vxmin≤vx≤vxmax与vymin≤vy≤vymax，其中vx、vy指的是x方向上、y方向的速度。类似的加速也有类似的限制。
[0080] 请参阅附图6，本实施例使用S形ACC/DEC算法结合路径总长度来进行路径点之间的第一次时间分配。然后使用指定的时间和相应的路径点通过三次样条拟合轨迹ημ(t)(η∈(x,y))。使用时间重新分配和碰撞检测机制来获得满足动态约束的无碰撞轨迹。
[0081] 具体地，所述轨迹规划方法具体为：
[0082] (1)间隔采样路径规划单元得到的路径点，计算路径长度的初始时间分配。
[0083] 为了加速计算，通过间隔采样减少从路径规划单元获得的密集路径点，并计算路径长度。根据其加速和减速的性质进行初始时间分配。
[0084] 本实施例使用S形ACC/DEC算法来进行路径总长度的初始时间分配。根据速度减速度限制vmax/amax和公式(2)(3)表示的加速度/速度限制，计算得到路径总长度的初始时间分配s(t)，如公式(1)。
[0085]
[0086]
[0087]
[0088] (2)对路径长度的初始时间分配进行离散化处理，采用二分法确定路径点的时间分配。
[0089] 由于直接求解s(t)的反函数，需要更多时间，因此本实施例提出了使用二分法快速求解它的方法。首先离散化s(t)，然后使用二分法来快速查找路径点的相应时间分配。在相应的路径点之间产生的时间分配用于后续的轨迹规划。
[0090] (3)采用三次样条法对确定的路径点的时间分配进行曲线拟合，得到分段三次样条多项式组成的轨迹。
[0091] 由分段三次样条多项式组成的轨迹被参数化为每个维度μ中的时间变量t，其中μ取值于x和y。每个维度上的Nth阶M段轨迹可以写成如下：
[0092]
[0093] 其中， 是ith段轨迹的jth阶多项式系数，jth的值可以是3，T1:T2…TM是每个细分的结束时间，总时间为TM-T0。通过并三次样条多项式进行求解，可以得到相应的轨迹ημ(t)。
[0094] (4)时间重新分配。
[0095] 由于前一次使用S形ACC/DEC算法分配路径长度，忽略了路径的曲率变化。可能获得不满足动态约束的轨迹。针对该问题提出了时间重新分配方法。
[0096] 在上一步骤的第一次时间分配之后，需要再次为速度和加速度超过动态限制的任何段进行时间间隔调整。
[0097] 由于整个轨迹由多段曲线组成，本实施例对轨迹进行采样以获得轨迹中每段轨迹的最大速度和加速度的绝对值，用vm和am表示。请参阅附图7，本实施例提出了一种时间重新分配机制，其中vmax和amax是最大速度和加速度极限值的绝对值，并且vm和am是所生成轨迹的每个段中的最大速度和加速度的绝对值。如果某段归轨迹的vm/vmax或(am/amax)1/2超过α，则认为相应的时间分配接近饱和，有必要调整分段曲线的时间分配，得到新的时间分配。如果没有，继续使用以前的时间分配。
[0098] 其中，α是一个小于1的值，表示vm和vmax以及am和amax的接近程度；系数k可以有效地防止任何时间跨度过度延伸并变得过于保守。
[0099] (5)再次生成轨迹。
[0100] 在时间重新分配之后，再次使用三次样条方法来生成轨迹。由于路径点经过前述采样步骤，可能导致产生的轨迹与狭窄区域中的障碍物碰撞。如果轨迹与障碍物碰撞，则在碰撞处及其周围添加路径点，并再次执行轨迹规划。如果未发生碰撞，则将轨迹输出值轨迹下发单元。
[0101] 所述轨迹下发单元，用于将轨迹规划单元生成的轨迹段下发至工控机Ⅱ。
[0102] 所述工控机Ⅱ连接惯性测量单元、编码器、防撞传感器、精定位相机、红外红冲单元以及执行模块，用于接收惯性测量单元测量的移动机器人在三维空间中的速度及位置数据，编码器检测的移动机器人四个车轮速度数据，防撞传感器检测的数据，精定位相机采集的图像数据，以及红外红冲单元检测的数据，对数据进行处理，得到跟踪轨迹，实现视觉精定位，输出运动控制指令至执行模块，实现实时运动控制。
[0103] 具体地，所述工控机Ⅱ包括和地标位姿反馈模块、轨迹跟随模块和视觉精定位模块，其中：
[0104] 所述地标位姿反馈单元，用于获取精定位相机采集的人工地标图像数据，并反馈给视觉精定位模块。
[0105] 所述轨迹跟随模块，用于结合惯性测量单元采集的移动机器人在三维空间中的速度及位置数据以及编码器检测的四个车轮速度数据，根据环境感知模块估计的移动机器人的位姿，以及规划模块生成的轨迹，基于反演法、模型预测控制等控制方法进行轨迹跟随，并下发指令至执行模块。
[0106] 具体地，轨迹跟随的具体实现过程如下：
[0107] 首先，设定轨迹跟随执行的周期，每个周期中计算规划的轨迹的位置、速度、加速度作为参考目标，利用环境感知模块反馈的移动机器人的位姿信息作为实际的位置反馈，基于以上数据，利用反演法或者模型预测控制等算法进行轨迹跟随。
[0108] 所述视觉精定位模块，用于对环境感知模块反馈的地标图像数据进行处理，利用PID算法对位姿进行校正，实现精确定位，消除移动机器人运行过程中的累计误差。
[0109] 具体地，视觉精定位模块的具体用于：
[0110] 获取环境感知模块反馈的移动机器人的地标图像数据，并处理图像，获取地标在相机坐标系的位置与角度信息；之后进行相机坐标系与移动机器人中心坐标系的坐标变换，得到移动机器人中心与地标的位置偏差；利用PID算法不断控制移动机器人朝着减小误差的方向运动，待位置偏差小于要求的偏差之后停止运动。
[0111] 在本实施例中，所述工控机Ⅱ通过工业以太网总线与执行模块连接，实现对执行模块的控制，所述执行模块执行工控机Ⅱ的运动指令，实现移动机器人的运动。请参阅附图2，所述执行模块包括依次串联的伺服驱动器Ⅰ、伺服驱动器Ⅱ、伺服驱动器Ⅲ、伺服驱动器Ⅳ和伺服驱动器Ⅴ，所述伺服驱动器Ⅰ连接伺服电机Ⅰ，所述伺服驱动器Ⅱ连接伺服电机Ⅱ，所述伺服驱动器Ⅲ连接伺服电机Ⅲ，所述伺服驱动器Ⅳ连接伺服电机Ⅳ，用于控制移动机器人的运动；所述伺服驱动器Ⅴ连接举升电机，用于控制移动机器人上平面的举升，便于货物的搬运。
[0112] 本实施例提出的实现移动机器人自主导航与精确定位的控制系统，采用两台工控机、若干传感器与执行器，两台工控机之间通过网络通信协议进行通信，既利用了机器人操作系统ROS的强大的建图定位能力，又加入了运动控制器进行实时运动控制，提高了整个系统的实时性。
[0113] 实施例二
[0114] 本实施例提供一种实现移动机器人自主导航与精确定位的控制方法，该方法是基于实施例一所述的实现移动机器人自主导航与精确定位的控制系统实现的，该方法包括以下步骤：
[0115] S101，工控机Ⅰ获取传感器数据，计算轮式里程计数据，构建环境静态地图和动态地图，估计移动机器人的位姿，规划移动机器人的轨迹，并反馈给工控机Ⅱ。
[0116] 具体地，工控机Ⅰ通过两个激光雷达扫描移动机器人所处的环境轮廓信息，通过感知用相机采集机器人前方的图像数据，接收工控机Ⅱ上传的惯性测量单元测量移动机器人前向、侧向、旋转方向的速度与位置信息；编码器测量移动机器人车轮的转速。
[0117] 具体地，所述步骤101中，轮式里程计数据的计算方法为：
[0118] 利用编码器测量的移动机器人四个车轮的转速v1、v2、v3、v4，通过逆运动学，解算出移动机器人整体x方向的线速度vx、y方向的线速度vy和z轴方向的角速度ω，其具体计算公式如下：
[0119] vx＝(v1+v2+v3+v4)/4
[0120] vy＝(v2+v4-v1-v3)/4
[0121] ω＝(v3+v4-v1-v3)/(4*(a+b))
[0122] 其中，a、b分别为移动机器人车体长和宽的一半。
[0123] 将相邻时刻Δt得到的轨迹进行累加求和，可计算得到机器人里程计数据，其表达式如下所示：
[0124] x＝x+vxΔtcos(θt)-vyΔtsin(θt)
[0125] y＝y+vxΔtsin(θt)+vyΔtcos(θt)
[0126] θt＝θt+ωΔt
[0127] 其中，θt为t时刻机器人的角度值。
[0128] 在本实施例中，所述构建环境静态地图和动态地图的方法具体为：
[0129] 使用catorgrapher算法建立环境静态地图，该算法使用UKF融合多传感器数据来创建子图和闭环检测策略，从而实现5cm分辨率的精确地图。
[0130] 从两个激光雷达扫描的移动机器人所处环境区域内划定一小块区域建立动态地图，用来反映环境的动态变化。
[0131] 具体地，所述步骤101中，移动机器人的位姿的估计方法为：
[0132] (1)采用扩展卡尔曼滤波将惯性测量单元测量的移动机器人在三维空间中的速度与位置数据与里程计数据融合。
[0133] 扩展卡尔曼滤波(EKF)包括运动模型与测量模型，测量模型可以使用非线性函数，运动模型使用里程计运动模型，预测移动机器人的位姿，测量模型使用likelihood模型，更新纠正移动机器人的位姿；得到新的位姿将成为下一时刻扩展卡尔曼滤波的运动模型的初始状态；采用扩展卡尔曼滤波将使用带有协方差的IMU数据与里程计数据进行融合，得到初步的位姿数据(包含位置和姿态)；
[0134] (2)将两个激光雷达提取的环境轮廓数据融合。
[0135] 由于机械结构设计的局限，本实施例在移动机器人的左前角与右后角分别安装了一个扫描范围为270°的二维激光雷达，在这里，将两个激光雷达提取的环境轮廓数据转换到移动机器人坐标系下，然后将两个激光雷达提取的环境轮廓数据转换为点云数据，融合点云数据后，以统一的格式发布；
[0136] (3)将初始的位姿数据与融合后的激光雷达数据进行融合定位，得到移动机器人的位置和姿态。
[0137] 在本实施例中，采用自适应蒙特卡洛方法进行融合定位，其具体实现方式如下：
[0138] 首先，采用预测模型预测扩展卡尔曼滤波融合后的带有协方差的初始位姿数据，得到此时刻的预测状态；
[0139] 然后，基于观测模型，使用融合后的激光雷达数据，对预测状态进行更新修正，得到此时刻更加准确的位姿；
[0140] 最后，进行每个粒子的重采样，如更新权重，增加随机粒子等操作，现存的粒子将用于下一时刻的位姿估计。
[0141] 通过以上步骤，实现对多传感器数据进行融合，从而实现更准确的移动机器人的位置、姿态的估计。
[0142] 具体地，所述步骤101中，移动机器人的轨迹规划方法具体为：
[0143] (1)利用构建的环境静态地图以及动态地图，采用搜索类方法如A*或采样类方法比如快速拓展随机树等方法，生成无碰撞路径；
[0144] (2)间隔采样路径规划单元得到的路径点，计算路径长度，并对其进行初始时间分配。
[0145] 为了加速计算，通过间隔采样减少从路径规划单元获得的密集路径点，并计算路径长度。根据其加速和减速的性质进行初始时间分配。
[0146] 本实施例使用S形ACC/DEC算法来进行路径总长度的初始时间分配。根据速度减速度限制vmax/amax和公式(2)(3)表示的加速度/速度限制，计算得到路径总长度的初始时间分配s(t)，如公式(1)。
[0147]
[0148]
[0149]
[0150] (3)对路径长度的初始时间分配进行离散化处理，采样二分法确定路径点的时间分配。
[0151] 由于直接求解s(t)的反函数，需要更多时间，因此本实施例提出了使用二分法快速求解它的方法。首先离散化s(t)，然后使用二分法来快速查找路径点的相应时间分配。在相应的路径点之间产生的时间分配用于后续的轨迹规划。
[0152] (4)采用三次样条法对确定的路径点的时间分配进行曲线拟合，得到分段三次样条多项式组成的轨迹。
[0153] 由分段三次样条多项式组成的轨迹被参数化为每个维度μ中的时间变量t，其中μ取值于x和y。每个维度上的Nth阶M段轨迹可以写成如下：
[0154]
[0155] 其中， 是ith段轨迹的jth阶多项式系数，jth的值可以是3，T1:T2…TM是每个细分的结束时间，总时间为TM-T0。通过并三次样条多项式进行求解，可以得到相应的轨迹ημ(t)。
[0156] (5)时间重新分配。
[0157] 由于前一次使用S形ACC/DEC算法分配路径长度，忽略了路径的曲率变化。可能获得不满足动态约束的轨迹。针对该问题提出了时间重新分配方法。
[0158] 在上一步骤的第一次时间分配之后，需要再次为速度和加速度超过动态限制的任何段进行时间间隔调整。
[0159] 由于整个轨迹由多段曲线组成，本实施例对轨迹进行采样以获得轨迹中每段轨迹的最大速度和加速度的绝对值，用vm和am表示。请参阅附图7，本实施例提出了一种时间重新分配机制，其中vmax和amax是最大速度和加速度极限的绝对值，并且vm和am是所生成轨迹的每个段中的最大速度和加速度的绝对值。如果某段归轨迹的vm/vmax或(am/amax)1/2超过α，则认为相应的时间分配接近饱和，有必要调整分段曲线的时间分配，得到新的时间分配。如果没有，继续使用以前的时间分配。
[0160] 其中，α是一个小于1的值，表示vm和vmax以及am和amax的接近程度；系数k可以有效地防止任何时间跨度过度延伸并变得过于保守；
[0161] 当Δti>α，则新的分配时间
[0162] 否则，继续使用以前的时间分配Δti。
[0163] (6)再次生成轨迹。
[0164] 在时间重新分配之后，再次使用三次样条方法来生成轨迹。由于路径点经过前述采样步骤，可能导致产生的轨迹与狭窄区域中的障碍物碰撞。如果轨迹与障碍物碰撞，则在碰撞处及其周围添加路径点，并再次返回步骤(2)进行轨迹规划。如果未发生碰撞，则输出生成的轨迹。
[0165] S102，工控机Ⅱ结合惯性测量单元测量的移动机器人在三维空间中的速度与位置数据以及编码器检测的四个车轮速度数据，根据工控机Ⅰ估计的移动机器人的位姿以及生成的轨迹，基于反演法、模型预测控制等控制方法进行轨迹跟随，并下发指令至执行模块。
[0166] S103，执行模块驱动移动机器人按照工控机Ⅱ下发的指令转动。
[0167] S104，待移动机器人到达目标点后，工控机Ⅱ通过精准定位相机采集地标的图像数据，并对图像数据进行处理，获取地标在相机坐标系的位置与角度信息，将地标的位置坐标从机坐标系变换到移动机器人中心坐标系，得到移动机器人中心与地标的位置偏差，采用PID算法对移动机器人的位姿进行校正，并反馈控制指令至执行模块。
[0168] S105，通过执行模块控制移动机器人朝着减小误差的方向运动，待移动机器人位置偏差小于要求的偏差之后停止运动。
[0169] 目前基于激光雷达的自主导航、基于视觉(相机)的自主导航、或者激光雷达与视觉(相机)结合的自主导航方式可实现厘米级定位，本实施例提出实现移动机器人自主导航与精确定位的控制方法，的通过自主导航与视觉相结合精定位方式，实现毫米级定位；结合扩展卡尔曼滤波与自适应蒙特卡洛方法的多传感器融合定位方法，实现移动机器人的准确位姿估计；采用轨迹规划方法可以生成满足动力学约束的平滑轨迹，可显著减少传动结构冲击延长移动机器人使用寿命。
[0170] 上述虽然结合附图对本公开的具体实施方式进行了描述，但并非对本公开保护范围的限制，所属领域技术人员应该明白，在本公开的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本公开的保护范围以内。
