[发明专利]一种未知路况下机器人环境感知与路径寻优系统

权利要求书

1.一种未知路况下机器人环境感知与路径寻优系统，包括安装在车身上的控制系统、车载处理器和驱动机构，控制系统经车载处理器控制驱动机构，其特征在于：还设置有中央处理器与控制系统无线连接，该中央处理器还与车身上的路径寻优系统无线连接；

所述路径寻优系统设置有16线激光雷达、捷联惯导IMU、GPS，其中所述捷联惯导IMU输出端、GPS输出端、16线激光雷达输出端分别连接组合滤波器IMU输入端、GPS输入端、雷达输入端；

所述捷联惯导IMU输出端和GPS输出端分别连接无迹卡尔曼滤波器的IMU输入端、GPS输入端；

所述无迹卡尔曼滤波器输出端与组合滤波器输出端分别连接所述算法器，所述算法器经无线通信模块与所述中央处理器双向连接，该算法器的输出端还连接所述车载处理器。

2.根据权利要求1所述的一种未知路况下机器人环境感知与路径寻优系统，其特征在于：所述算法器还分别连接有函数库和车身尺寸库；所述车身尺寸库连接有尺寸录入单元。

3.根据权利要求1所述的一种未知路况下机器人环境感知与路径寻优系统，其特征在于：所述16线激光雷达置于车身车头且高于地面30cm处，捷联惯导IMU置于车身底盘的车身正中间，GPS双天线分别为主从天线，从天线置于车头，主天线置于车尾；所述16线激光雷达、捷联惯导IMU的坐标轴中Y轴指向车身车头方向，X轴垂直Y轴指向车身右侧方向，Z轴指向垂直车身向上方向。

4.根据权利要求1所述的一种未知路况下机器人环境感知与路径寻优系统，其特征在于：所述中央处理器无线连接有n个车身上的控制系统和算法器。

5.一种未知路况下机器人环境感知与路径优化方法，其特征在于：

包括绘制地图的流程和路径规划流程，其中绘制地图的流程按如下步骤进行：

A-1：控制系统控制车载处理器运行，由车载处理器控制车身在未知路况下运动；

A-2：16线激光雷达、捷联惯导IMU和GPS在车身运动过程中采集点云数据并分别送入组合滤波器；

A-3：采集的点云数据经组合滤波器后送入算法器，算法器从函数库中调取算法对滤波后的点云数据进行融合、优化、建立地图并不断修正出地图最新数据，同时算法器将建立的地图通过无线模块传送至中央处理器，中央处理器将m个车身建立的地图进行汇总得到全局地面地图；

所述路径规划的流程按如下步骤进行：

B-1：中央处理器设置终点及其位姿；

B-2：捷联惯导IMU和GPS采集的点云数据分别送入无迹卡尔曼滤波器，获得最优位姿估计值，实现未知路况下的高精度定位和导航；

B-3：通过尺寸录入单元录入相关车身尺寸至车身尺寸库，算法器从车身尺寸库获取车身的尺寸数据；

B-4：算法器获取中央处理器发送的全局地面地图、终点及其位姿、结合车身尺寸及当前位置与位姿信息，利用A*算法完成路径规划与路径优化，获得最优路径；

B-5：车载处理器按最优路径控制驱动机构，驱动机构控制车身运动。

6.根据权利要求5所述的一种未知路况下机器人环境感知与路径优化方法，其特征在于：步骤A-2中组合滤波器中先后经过条件滤波、体素滤波，其具体步骤为；

A-2-1：采用条件滤波过滤原始点云数据，设定原始点云数据的任意一点对应的欧式距离大于一个临近阈值且小于一个上限阈值，除去原始点云数据中的野值点，得到合法点云数据；

A-2-2：体素滤波，输入合法点云数据并创建一个体素网格，每个体素中所包含的合法点云数据以其质心取近似。