一种未知路况下机器人环境感知与路径寻优系统技术方案

本发明专利技术公开一种未知路况下机器人环境感知与路径寻优系统，实现车身在未知环境下能够进行精确环境感知和高精度定位，考虑车身的约束使用改进A*算法进行路径规划与优化。控制系统控制车身未知环境下运动，车身通过搭载的多传感器采集周围的环境信息，再通过无迹卡尔曼滤波器进行信息融合获取车身的位姿信息并校正，同时进行精确定位与导航；通过改进的A*算法和车身尺寸约束进行路径规划和路径优化，提高路径的合理性和准确性。高路径的合理性和准确性。高路径的合理性和准确性。

【技术实现步骤摘要】
一种未知路况下机器人环境感知与路径寻优系统


[0001]本专利技术涉及一种测量导航
，特别涉及一种未知路况下机器人环境感知与路径寻优系统。

技术介绍

[0002]智能机器人在近几年随着科学技术的进步得到了飞速的发展，其能实现在未知路况下借助视觉、激光等传感器进行实时感知周围环境进行建图，通过建立的实时地图进行定位和导航，也可以通过卫星地图提供的实时路径借助机器人的导航系统，诸如捷联惯导系统(INS)、全球导航卫星系统(GNSS)进行自主导航。按照工作环境可以分为地下工作机器人、地面工作机器人、高空工作机器人以及水下工作机器人。这四类机器人均实现在不同环境下的巡检、勘探等工作。有效的降低了人工成本和人工工作存在的安全隐患。
[0003]现有技术的缺点是：由于工作环境的不确定性，对于一些特殊场景下的工作环境，比如GNSS拒止环境下，水下等特殊环境，部分传感器存在工作性能不稳定的情况，同时，针对大部分自主机器人的路径规划算法中，很少考虑到其车身本身的尺寸约束，可能导致在实际项目工程运行中，存在一些不可控的错误进导致机器人及其车身无法通过，出现较大的工程隐患。

技术实现思路

[0004]针对现有技术的问题，本专利技术的目的在于提供一种高精度导航同时考虑车身本身的尺寸约束的一种未知路况下机器人环境感知与路径寻优系统。
[0005]为达到上述目的，本专利技术的技术方案如下：一种未知路况下机器人环境感知与路径寻优系统，包括安装在车身上的控制系统、车载处理器和驱动机构，控制系统经车载处理器控制驱动机构，其关键在于：还设置有中央处理器与控制系统无线连接，该中央处理器还与车身上的路径寻优系统无线连接；
[0006]所述路径寻优系统设置有16线激光雷达、捷联惯导IMU、GPS，其中所述捷联惯导IMU输出端、GPS输出端、16线激光雷达输出端分别连接组合滤波器IMU输入端、GPS输入端、雷达输入端；
[0007]所述捷联惯导IMU输出端和GPS输出端分别连接无迹卡尔曼滤波器的IMU输入端、GPS输入端；
[0008]所述无迹卡尔曼滤波器输出端与组合滤波器输出端分别连接所述算法器，所述算法器经无线通信模块与所述中央处理器双向连接，该算法器的输出端还连接所述车载处理器；
[0009]所述16线激光雷达、捷联惯导IMU、GPS用于采集地面信息数据，所述无线通讯模块用于连接所述路径寻优系统与所述中央处理器。
[0010]作为优选：所述算法器还分别连接有函数库和车身尺寸库；所述车身尺寸库连接有尺寸录入单元。
[0011]作为优选：所述16线激光雷达置于车身车头且高于地面30cm处，捷联惯导IMU置于车身底盘的车身正中间，GPS双天线分别为主从天线，从天线置于车头，主天线置于车尾，且主天线尽可能保持与捷联惯导IMU安装位置在垂直方向上重合，目的是减少杆臂效应对组合导航系统精度的影响；所述16线激光雷达、捷联惯导IMU的坐标轴中Y轴指向车身车头方向，X轴垂直Y轴指向车身右侧方向，Z轴指向垂直车身向上方向，所述GPS坐标轴方向任意。
[0012]作为优选：所述中央处理器无线连接有n个车身上的控制系统和算法器。
[0013]中央处理器用于绘制全局地面地图、调配适宜的车身进行工作。
[0014]一种未知路况下机器人环境感知与路径优化方法，其关键在于：包括绘制地图的流程和路径规划流程，其中绘制地图的流程按如下步骤进行：
[0015]A
‑
1：控制系统控制车载处理器运行，由车载处理器控制车身在未知路况下运动；
[0016]A
‑
2：16线激光雷达、捷联惯导IMU和GPS在车身运动过程中采集点云数据并分别送入组合滤波器；
[0017]A
‑
3：采集的点云数据经组合滤波器后送入算法器，算法器从函数库中调取算法对滤波后的点云数据进行融合、优化、建立地图并不断修正出地图最新数据，同时算法器将建立的地图通过无线模块传送至中央处理器，中央处理器将m个车身建立的地图进行汇总得到全局地面地图；
[0018]所述路径规划的流程按如下步骤进行：
[0019]B
‑
1：中央处理器设置终点及其位姿；
[0020]B
‑
2：捷联惯导IMU和GPS采集的点云数据分别送入无迹卡尔曼滤波器，获得最优位姿估计值，实现未知路况下的高精度定位和导航；
[0021]B
‑
3：通过尺寸录入单元录入相关车身尺寸至车身尺寸库，算法器从车身尺寸库获取车身的尺寸数据；
[0022]B
‑
4：算法器获取中央处理器发送的全局地面地图、终点及其位姿、结合车身尺寸及当前位置与位姿信息，利用A*算法完成路径规划与路径优化，获得最优路径；
[0023]B
‑
5：车载处理器按最优路径控制驱动机构，驱动机构控制车身运动。
[0024]作为优选：步骤A
‑
2中组合滤波器中先后经过条件滤波、体素滤波和半径滤波，其具体步骤为：
[0025]A
‑2‑
1：采用条件滤波过滤原始点云数据，设定原始点云数据的任意一点对应的欧式距离大于一个临近阈值且小于一个上限阈值，上限阈值由人为设定，除去原始点云数据中的野值点，得到合法点云数据；
[0026]A
‑2‑
2：体素滤波，输入合法点云数据并创建一个体素网格，每个体素中所包含的合法点云数据以其质心取近似，即下采样；
[0027]A
‑2‑
3：半径滤波：设定在某个合法点云数据以0.8m范围内有至少10个合法点云数据，否则舍去点云信息，以达到滤除一些不必要的点云数据，增强点云数据的可靠性。
[0028]作为优选：步骤A
‑
3使用hdl_graph_slam算法完成数据融合，建立地面检测优化约束并建图，具体如下：
[0029]A
‑3‑
1：步骤A
‑
2滤波后的合法点云数据采用NDT算法进行点云匹配，获得前端里程计，具体为：扫描所占据的空间并细分为单位网格，计算每个单位网格中的概率密度函数表示为单位网格内表面点的生成过程，得
[0030][0031]其中D表示维度；
[0032]由计算每个格子里的平均向量和协方差矩阵∑：
[0033][0034][0035]其中表示包含在单元格中的参考扫描点的位置；
[0036]假设空间变换函数空间的点通过T变换，可得最佳姿态为最大化似然函数Ψ：
[0037][0038]其中，Ψ表示最佳姿态的最大概率；
[0039]A
‑3‑
2：得到前端里程计的同时进行地面检测，并完成后端图优化的约束：
[0040]设在世界坐标系XYZ中，全局地面在世界坐标系的一般方程：
[0041]Ax+By+Cz+D＝0
[0042]其中，Y轴指向车身车头方向，X轴本文档来自技高网...

【技术保护点】

【技术特征摘要】
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：体素滤波，输入合法点云数据并创建一个体素网格，每个体素中所包含的合法点云数据以其质心取近似。7.根据权利要求5所述的一种未知路况下机器人环境感知与路径优化方法，其特征在于：步骤A
‑
3使用hdl_graph_slam算法完成数据融合，建立地面检测优化约束并建图，具体如下：A
‑3‑
1：步骤A
‑
2滤波后的合法点云数据采用NDT算法进行点云匹配，获得前端里程计，具体为：扫描所占据的空间并细分为单位网格，计算每个单位网格中的概率密度函数表示为单位网格内表面点的生成过程，得的生成过程，得其中D表示维度；由计算每个格子里的平均向量和协方差矩阵∑：和协方差矩阵∑：其中表示包含在单元格中的参考扫描点的位置；假设空间变换函数空间的点通过T变换，可得最佳姿态为最大化似然函数Ψ：其中，Ψ表示最佳姿态的最大概率；A
‑3‑
2：地面检测，并完成后端图优化的约束：设在世界坐标系XYZ中，全局地面在世界坐标系的一般方程：Ax+By+Cz+D＝0其中，Y轴指向车身车头方向，X轴垂直Y轴指向车身右侧方向，Z轴指向垂直车身向上方向；设激光雷达坐标系为X'Y'Z'，X'Y'Z'与XYZ坐标方向一致，世界坐标系到激光雷达坐标系的变换为T0；假设全局地面上一点Q＝[x
Q
,y
Q
,z
Q
]
T
，全局地面的法向量其中x
Q
,y
Q
,z
Q
为全局地面下一个点的坐标，x
n
,y
n
,z
n
为全局地面下法向量的坐标，全局地面
的参数方程：x
n
x+y
n
y+z
n
z
‑
(x
n
x
Q
+y
n
y
Q
+z
n
z
Q
)＝0所述法向量根据雷达姿态T1＝{R,t}，R是旋转矩阵，t是平移向量，得全局地面在雷达坐标系下的参数方程：由雷达姿态T1求p'＝Rp+t，得全局地图在雷达坐标系下的参数方程(1)：x
n
'x+y
n
'y+z
n
'z
‑
(x
n
'x
Q
'+y
n
'y
Q
'+z
n
'z
Q
')＝0其中，x
Q
'、y
Q
'、z
Q
'为雷达坐标系下的点坐标；x
n
'、y
n
'、z
n
'为雷达坐标系下法向量的坐标；使用RANSAC进行地面检测，得到激光雷达检测的地面参数方程(2)：x
d
x+y
d
y+z
d
z+D
d
＝0其中[x
d
,y
d
,z
d
]
T
表示地面法向量，D
d
为任意常数；参数方程(1)对应的法向量转到x轴，旋转矩阵R作用于参数方程(2)：参数方程(1)对应的法向量转到x轴，旋转矩阵R作用于参数方程(2)：其中[x
’
d
,y
’
d
,z
’
d
]
T
表示[x
d
,y
d
,z
d
]
T
旋转后得到的法向量；旋转后，获得两个法向量，其中[1,0,0]
T
与x轴重合，[x
’
d
,y
’
d
,z
’
d
]
T
与xy平面形成一个夹角，定义与xy平面形成的夹角为β，β投影到xy平面与x轴形成的夹角为α，具体为：α＝arctan2(y
d
',x
d
')两个平面之间可能存在平行，衡量两平面在Z轴的截距，记:D'＝
‑
(x
n
'x
p
'+y
n
'y
p
'+z
n
'z
p
')整个误差函数以作后端图优化约束为：e
rr
＝[α,β,D'
‑
D
d
]
T
e＝e
rrT
Ωe
rr
其中，e
rr
表示两个平面之间的误差；Ω表示信息矩阵；完成数据融合，建立地面检测优化约束并完成建图。8.根据权利要求5所述的一种未知路况下机器人环境感知与路径优化方法，其特征在
于：步骤B
‑
2中无迹卡尔曼滤波器的具体工作为：以松耦合方式的INS/GNSS组合导航系统作为车身的实时导航定位系统，以捷联惯导IMU的误差方程为组合导航系统的状态方程，以捷联惯导IMU、GPS输出信息为量测使...

【专利技术属性】
技术研发人员：欧阳奇，张兴兰，张鑫益，
申请(专利权)人：重庆理工大学，
类型：发明
国别省市：