面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统与流程

技术特征：
1.面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，其特征在于：包括以下步骤：s1：利用激光雷达采集多个时刻下不同夹角和线束到墙面的距离，然后对采集的数据进行预处理，剔除无效的数据；s2：利用同一时刻下激光雷达不同夹角和不同线束测量的距离计算基于空间的姿态变化参数，并通过主成分分析算法融合基于空间的姿态变化参数；s3：利用距离更新函数刻画多个时刻下机器狗到墙面距离的参数变化率，并计算参数的平均变化率，再将参数的平均变化率和距离进行迭代重新计算距离，利用重新计算的距离求出基于时间的姿态变化参数，并利用主成分分析算法融合基于时间的姿态变化参数；s4：利用时空自适应融合方法融合基于时间和空间的姿态变化参数，得到机器狗最优姿态变化参数，以辅助修正姿态。2.根据权利要求1所述的面向电缆隧道的四足机器狗辅助姿态修正方法，其特征在于，所述步骤s1中，对激光雷达数据的采集和处理包括：s11：以激光雷达的x轴为机器狗的正前行驶方向，y轴为垂直于机器狗身体以右向左为正方向，从下向上为z轴的正方向，原点为o；不同夹角是指以激光雷达垂直角度0
°
为水平面，并以y轴为平分线；设oa
30
°
与ob
30
°
的夹角为30
°
、oa
45
°
与ob
45
°
的夹角为45
°
、oa
60
°
与ob
60
°
的夹角为60
°
、oa
90
°
与ob
90
°
的夹角为90
°
、oa
120
°
与ob
120
°
的夹角为120
°
，以此采集数据；s12：根据环境自适应筛选方法选取不同垂直角度的线束和不同的夹角，以y轴为平分线采集数据；s13：筛选数据中不符合狭窄隧道场景中的数据并剔除。3.根据权利要求2所述的面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，其特征在于，所述步骤s2中，计算及融合同一时刻中不同夹角和不同线束下的姿态变化参数包括：s21：利用不同夹角和不同线束的和其中α＝30
°
,45
°
,60
°
,90
°
,120
°
，β＝1
°
,7
°
,15
°
，测量到墙面的交点及交点前后各两个小角度的距离，再利用这5个交点的距离求平均距离以代替和到墙面的距离，计算公式如下：其中，和表示垂直方向角度为β，激光雷达线束oa与ob夹角角度为α的直线距离，oa-2
和ob-2
表示激光雷达扫描线与墙面交点处前两个小角度到墙面的距离；oa-1
和ob-1
表示激光雷达扫描线与墙面交点处前一个小角度到墙面的距离，oa和ob表示激光雷达扫描线与墙面中间点的交点距离；oa2和ob2分别表示激光雷达扫描线与墙面交点处后两个小角度的距离，oa1和ob1分别表示激光雷达扫描线与墙面交点处后一个小角度的距离；s22：将和代入差和比函数计算基于空间的姿态变化参数，以衡量机器狗姿态变化情况，具体的计算公式如下：
其中，θ表示机器狗姿态变化参数，能体现机器狗姿态变化情况，如果θ＜0说明机器狗的头部朝左偏；如果θ＞0说明机器狗的头部朝右偏；若θ＝0说明机器狗与墙面平行，|θ|越大表示机器狗的头部偏转越大，反之则说明偏转越小；s23：利用基于主成分分析的数据融合算法融合不同夹角和线束计算得到的姿态变化参数，从而得到基于空间的最优姿态变化参数估计。4.根据权利要求3所述的面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，其特征在于：所述步骤s23具体为：
①
将不同夹角和不同线束计算的基于空间的姿态变化参数θ看作一个整体θ＝(θ1,θ2,...,θ
n
)，其中n为数据的个数；设θ的协方差矩阵为d，求得特征根为λ1≥λ2≥...≥λ
n
和相应的单位化正交特征向量为e1,e2,...,e
n
，其中第i个主成分单位化正交特征向量为e
i
＝(e
1i
,e
2i
,...,e
ni
)，因此第i个主成分为：f
i
＝e
′
i
θ＝e
1i
θ1 ... e
ni
θ
n
(3)
②
计算主成分f
i
的方差贡献率以及前m个主成分的累计方差贡献率计算公式如下：下：
③
计算不同夹角和线束的综合支持度，具体公式如下：其中，f
i*
为每个θ
i
与之最相关的主成分，通过它们之间的相关系数确定ρ(θ
i
,f
j
)，选择ρ(θ
i
,f
j
)值最大的f
j
作为θ
i
的最相关主成分，具体公式如下：其中，σ
i
为θ
i
的标准差，e
ij
为特征向量e
i
的第j个分量；
④
最后利用动态融合方法计算出基于空间的姿态变化情况结果，具体公式如下：其中，η
i
为第i组数据的二元指示变量，其取值随具体工作环境确定，当输入数据量较少的时候令m＝n，反之如果输入的数据量较大则取累计贡献率达到适合当前环境的比例进行融合。5.根据权利要求4所述的面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，其特征在于：步骤s3中利用距离更新函数刻画不同时刻下机器狗到墙面距离的变化趋势，
然后计算姿态变化的参数，具体包括：s31：保存as内的数据，然后将每一组的数据代入距离更新函数计算出参数和并将这些参数保存下来，其中计算函数如下所示：其中，w
k-1
和v
k-1
分别表示第k-1时刻激光雷达的量测噪声，其中k≥5；表示表示表示表示中第k-3时刻到k-2时刻距离变化的补偿参数，表示中第k-4时刻到k-3时刻距离变化的补偿参数；表示中第k-3时刻到k-2时刻距离变化的补偿参数，表示中第k-4时刻到k-3时刻距离变化的补偿参数；s32：利用每个时刻的参数和计算出每个时刻每组扫描线的变化率，计算公式如下所示：其中，
△
k表示时刻k前后的时间差；表示参数a在k时刻，水平方向夹角为α，垂直方向角度为β的第一个参数；表示参数a在k时刻，水平方向夹角为α，垂直方向角度为β的第二个参数，表示参数b在k时刻，水平方向夹角为α，垂直方向角度为β的第一个参数；表示参数b在k时刻，水平方向夹角为α，垂直方向角度为β的第二个参数；因连续运动中前后两个时刻参数的变化率不会太大，所以将参数变化率较小的线束保留，剔除变化率较大的数据，然后利用保留的线束计算出参数的平均变化率，计算公式如下：其中，和分别代表参数和的平均变化率，再将平均变化率代入公式(9)计算k时刻的和s33：将步骤s32计算出的和代入公式(2)计算不同夹角和不同线束的下姿态变化参数，然后基于主成分分析的数据融合算法将不同夹角和线束计算得到的姿态变化参数θ进行融合最终得到基于时间的姿态变化参数6.根据权利要求5所述的面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，其特征在于，所述步骤s4具体为：
利用时空自适应融合函数计算出最优的姿态变化参数具体公式如下所示：具体公式如下所示：其中，和分别为基于空间和时间姿态变化参数的最优估计，σ1和σ2分别为基于空间和时间上的姿态变化自适应权重，θ
space
和θ
time
分别表示当前基于空间和时间姿态变化最大的值。7.一种面向电缆隧道巡检的四足机器狗辅助姿态修正系统，其特征在于：包括存储器和控制器，所述存储器内存储有计算机可读程序，所述计算机可读程序被控制器调用时，能执行如权利要求1至6任一所述的面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统的步骤。

技术总结
本发明涉及一种面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，包括：利用激光雷达垂直方向不同夹角线束测量到墙面距离，利用距离数据计算姿态变化的参数，利用主成分分析方法融合基于空间的姿态变化参数；获取激光雷达不同时刻不同夹角线束到墙面的距离，代入距离更新函数中计算出参数的变化率和平均变化率，将距离数据和参数的平均变化率代入距离更新函数中迭代计算每组的距离，通过新的距离求出姿态变化参数，利用主成分分析方法融合基于时间的姿态变化参数；利用时空自适应融合方法融合基于空间和时间的姿态变化参数，得到智能四足机器狗姿态变化参数的最优估计值。本发明能够弥补智能四足机器狗在行驶中无法判断自身姿态变化的缺点。断自身姿态变化的缺点。断自身姿态变化的缺点。


技术研发人员：杜勇 朱浩 米冀
受保护的技术使用者：重庆览辉信息技术有限公司
技术研发日：2022.05.27
技术公布日：2022/9/2