1.狭窄巷道小车巡检的无人机UWB增强定位图优化方法,其特征在于,针对目标巷道,以巷道中预先测绘的各测绘点作为无人机的悬停点位,执行如下步骤S1‑步骤S6,完成无人机UWB测量精度的优化:步骤S1:针对位于初始位置的巡检小车,无人机飞至巡检小车所在初始位置上方悬停,采集巡检小车当前时刻的状态向量;并基于巡检小车当前时刻的状态向量,获得巡检小车下一时刻的预测位姿;
步骤S2:分别针对巡检小车所处环境的激光点云中的各激光点,将当前时刻的激光点云与历史时刻所采集的激光点云进行配准,并更新巡检小车的状态向量;
步骤S3:使无人机悬停在测绘点上方,计算无人机与巡检小车之间的距离,包括测量距离和计算距离;
步骤S4:基于巡检小车的预测位姿、激光点云配准过程、巡检小车与无人机之间的距离计算残差,基于图优化方法,构建当前测绘点的无人机定位综合残差,并构建无人机定位综合残差的优化目标函数;
步骤S4的具体步骤如下:
步骤S4.1:构建状态向量集合如下式:
;
其中, 表示状态向量集合, 表示巡检小车的状态向量序列,N为巡检小车的状态向量总数, 表示无人机的测绘点位置序列,M为无人机的测绘点总数;在巷道中预先测绘获得;
步骤S4.2:构建无人机定位综合残差,具体如下式:;
其中, 表示无人机定位综合残差, 表示惯性测量单元预积分残差,表示激光点云匹配残差, 表示UWB测距残差;I表示惯性测量单元,L表示激光雷达,U表示UWB模块;i表示i时刻,k表示第k个激光点,j表示j时刻;
步骤S4.3:构建无人机定位综合残差的优化目标函数如下式:;
其中, 表示惯性测量单元预积分残差, 表示激光点云匹配残差, 表示UWB测距残差;
、 表示巡检小车在i时刻、i+1时刻的状态向量;
激光点云匹配残差 的计算如下式:
;
其中,表示巡检小车i时刻的位姿,表示巡检小车j时刻的位姿, 表示巡检小车从i时刻到j时刻的相对位姿变换;
UWB测距残差 表示无人机与巡检小车之间的计算距离与测量距离之差,具体如下:;
其中, 表示无人机与巡检小车之间的计算距离,表示无人机与巡检小车之间的测量距离,表示巡检小车j时刻的状态向量, 表示无人机的第m个测绘点位置;
步骤S5:基于巡检小车当前位置,在预设范围内寻找更优的测绘点,进行无人机测绘点位置的切换,直至巡检小车在完成巷道中完成巡检;
步骤S6:基于几何精度因子,通过调节测绘点在巷道中的位置、密度、数量,使几何精度因子最小化,完成对巷道中测绘点的分布优化。
2.根据权利要求1所述的狭窄巷道小车巡检的无人机UWB增强定位图优化方法,其特征在于,步骤S1的具体步骤如下:步骤S1.1:采集巡检小车当前时刻的状态向量如下式:;
其中,表示巡检小车的状态向量,表示巡检小车在世界坐标系下的三维位置坐标;
表示巡检小车在世界坐标系下的三维速度向量;表示巡检小车在世界坐标系下的姿态四元数;巡检小车搭载惯性测量单元,包含加速度计和陀螺仪; 表示加速度计偏置; 表示陀螺仪偏置,T表示转置;
步骤S1.2:计算巡检小车下一时刻的预测位姿,包括预测位置、预测速度、预测姿态,具体如下式:;
;
;
其中, 为巡检小车的位置变化率,用于表征巡检小车的预测位置, 为巡检小车的速度变化率,用于表征巡检小车的预测速度;表示巡检小车的三维速度向量; 表示惯性测量单元中加速度计的原始测量值, 是加速度计偏置, 是姿态旋转矩阵,是重力向量; 为姿态四元数变化率,用于表征巡检小车的预测姿态,为姿态四元数, 表示惯性测量单元中陀螺仪的原始测量值, 是陀螺仪偏置。
3.根据权利要求2所述的狭窄巷道小车巡检的无人机UWB增强定位图优化方法,其特征在于,步骤S2的具体步骤如下:步骤S2.1:针对巡检小车所处环境的激光点云中的激光点 ,采用最近邻点搜索方法,搜索参考点云中的最近邻点,具体如下式:;
其中, 是参考平面的法向量, 是参考平面上的参考点, 是姿态旋转矩阵,是平移向量, 表示激光点 到参考平面的距离;
步骤S2.2:对 进行加权处理,然后通过迭代扩展卡尔曼滤波更新状态向量,状态向量更新公式为:;
其中, 表示流形加法,是卡尔曼增益矩阵,是加权后的 ;表示巡检小车的状态向量。
4.根据权利要求3所述的狭窄巷道小车巡检的无人机UWB增强定位图优化方法,其特征在于,步骤S3的具体步骤如下:步骤S3.1:无人机搭载UWB模块,其中集成UWB测距模型,计算悬停在测绘点上方的无人机与巡检小车之间的测量距离 :;
其中,c为光速, 、 分别为无人机的收发时间戳, 、 分别为巡检小车的收发时间戳;
步骤S3.2:在无人机四个机翼角搭载锚点,在巡检小车上搭载相对应的标签,无人机与巡检小车之间的计算距离 具体如下式:;
其中, 为时间戳t下无人机与巡检小车之间的计算距离,角标中的z表示无人机上搭载的锚点, 为无人机本体到世界坐标的变换矩阵, 为时间戳t下无人机本体到标签的固定变换矩阵, 为无人机本体坐标系下的原点位置, 为锚点z在世界坐标系下的位置,为测量噪声。
5.根据权利要求4所述的狭窄巷道小车巡检的无人机UWB增强定位图优化方法,其特征在于,步骤S5的具体步骤如下:步骤S5.1:基于巡检小车当前位置,在预设范围内寻找更优的测绘点,若存在更优的测绘点,则执行步骤S5.2,否则执行步骤S5.3;
步骤S5.2:无人机飞行到更优的测绘点上方悬停,完成无人机的位置切换,并更新无人机定位综合残差;
步骤S5.3:无人机保持当前位置,提供当前位置的无人机定位综合残差。
6.根据权利要求5所述的狭窄巷道小车巡检的无人机UWB增强定位图优化方法,其特征在于,更优的测绘点的判断依据为:存在另一个测绘点,该测绘点与巡检小车之间的非遮挡距离小于当前测绘点与巡检小车之间的非遮挡距离。
7.根据权利要求6所述的狭窄巷道小车巡检的无人机UWB增强定位图优化方法,其特征在于,步骤S6的具体方法如下:计算几何精度因子如下式:
;
其中, 表示几何精度因子,表示测绘点与巡检小车位置之间的雅可比矩阵,其行数为测绘点数量,列数为巡检小车三维位置误差的维度加上时间误差的维度; 表示迹;
、 、 分别为巡检小车在x、y、z三个方向上的位置误差, 为时间误差;
通过调节巷道中测绘点的数量、位置分布,以及弯道处测绘点的密度,使几何精度因子最小化,完成对巷道中测绘点的分布优化。