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

1.本发明属于机器人技术领域，具体涉及一种面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统。


背景技术：

2.随着机器人技术的发展，各种各样的机器人被研究与应用于各领域中。四足和其它腿式机器人可以通过多种地形和障碍相比于轮式机器人有更好的通过性。在一些狭窄且密闭的隧道环境中，需要经常检查设备的工作状态，利用智能四足机器狗进行巡检工作，能够很好的完成任务，因此在复杂的工作场景中有更大的应用需求。
3.智能四足机器狗的身上可集成多个传感器，例如摄像头、激光雷达、组合导航等，通过这些传感器再加上相应的算法，可以感知到周围的环境信息，构建全局或者局部的地图，再利用路径规划算法，规划出全局或者局部的路径，并通过相应的策略实时控制智能机器狗的速度和姿态，从而使得智能机器狗能够主动的避开障碍物，同时安全可靠地沿着规划的路径移动，从而到达目的地完成相应的任务。智能机器狗在沿着规划的路径行驶中，难免会遇到障碍物，因此机器狗的避障能力对完成任务也有十分重要的作用，在避障过程中需要实时的判断智能机器狗的姿态并进行相应的调整，以此才能安全快速的避开障碍物。
4.目前，为了获得智能机器狗的位置和姿态，可以通过gps定位系统和惯性导航系统提供精确的信息，但是在狭窄且密闭的隧道环境中因为无法接收到卫星信号导致gps定位系统无法正常工作，惯性导航系统在长运行中随着时间的增加存在累计误差，因此无法准确的获取姿态。


技术实现要素：

5.有鉴于此，本发明的目的在于提供一种面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，以实现智能四足机器狗在狭窄隧道巡检中的应用，该方法仅利用激光雷达采集的数据，计算智能机器狗的姿态变化信息，以此辅助机器狗修正姿态，为智能机器狗的控制系统提供更多的信息，能增强避障能力，使其能安全快速通过障碍物。
6.为达到上述目的，本发明提供如下技术方案：
7.面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，包括以下步骤：
8.s1：利用激光雷达采集多个时刻下不同夹角和线束到墙面的距离，然后对采集的数据进行预处理，剔除无效的数据；
9.s2：利用同一时刻下激光雷达不同夹角和不同线束测量的距离计算基于空间的姿态变化参数，并通过主成分分析算法融合基于空间的姿态变化参数；
10.s3：利用距离更新函数刻画多个时刻下机器狗到墙面距离的参数变化率，并计算参数的平均变化率，再将参数的平均变化率和距离进行迭代重新计算距离，利用重新计算的距离求出基于时间的姿态变化参数，并利用主成分分析算法融合基于时间的姿态变化参数；
11.s4：利用时空自适应融合方法融合基于时间和空间的姿态变化参数，最后得到机器狗最优姿态变化参数，以辅助修正姿态。
12.进一步，在步骤s1中，采用16线激光雷达获取不同角度和线束到墙面距离的数据，激光雷达位置为固定采集，并安装在智能四足机器狗(简称机器狗)2的背部上面中间位置，安装位置距离地面0.5-0.7米，检测角为360度，分辨率为0.18度，以每秒钟10帧的速率采集离墙面的距离，然后剔除检测距离数据中特别大或特别小的无效数据，具体数据采集和处理包括：
13.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
°
，以此采集数据；
14.s12：根据环境自适应筛选方法选取不同垂直角度的线束和不同的夹角，均以y轴为平分线采集数据；
15.s13：找出采集数据中不符合狭窄隧道场景中的数据并剔除，即将距离特别大和特别小的数据剔除。
16.进一步，在步骤s2中所述的计算和融合同一时刻不同夹角和不同线束数据的姿态变化参数包括：
17.s21：测量出不同夹角和不同线束和(其中α＝30
°
,45
°
,60
°
,90
°
,120
°
，β＝1
°
,7
°
,15
°
)与墙面的交点和交点前后各两个小角度(每个小角度为0.18
°
)的距离，再利用这5个交点的距离求平均距离以代替和到墙面的距离，计算公式如下：
[0018][0019]
其中，和表示垂直方向角度为β，激光雷达线束oa与ob夹角角度为α的直线距离，oa-2
和ob-2
表示激光雷达扫描线与墙面交点处前两个小角度到墙面的距离；oa-1
和ob-1
表示激光雷达扫描线与墙面交点处前一个小角度到墙面的距离，oa和ob表示激光雷达扫描线与墙面中间点的交点距离；oa2和ob2分别表示激光雷达扫描线与墙面交点处后两个小角度的距离，oa1和ob1分别表示激光雷达扫描线与墙面交点处后一个小角度的距离。
[0020]
s22：将和代入差和比(differenceoversummation,dos)函数计算基于空间的姿态变化参数，以衡量机器狗姿态变化情况，具体的计算公式如下：
[0021][0022]
其中，θ表示机器狗姿态变化参数，能体现机器狗姿态变化情况，如果θ＜0说明机器狗的头部朝左偏；如果θ＞0说明机器狗的头部朝右偏；若θ＝0说明机器狗与墙面平行，|θ越大表示机器狗的头部偏转越大，反之则说明偏转越小。
[0023]
s23：利用基于主成分分析的数据融合算法融合不同夹角和线束计算得到的姿态
变化参数，从而得到基于空间的最优姿态变化参数估计。
[0024]
可选地，所述步骤s23具体为：
[0025]
①
将不同夹角和不同线束计算的基于空间的姿态变化参数θ看作一个整体θ＝(θ1,θ2,...,θn)，其中n为数据的个数。设θ的协方差矩阵为d，求得特征根为λ1≥λ2≥...≥λn和相应的单位化正交特征向量为e1,e2,...,en，其中第i个主成分单位化正交特征向量为ei＝(e
1i
,e
2i
,...,e
ni
)，因此第i个主成分为：
[0026]fi
＝e
′iθ＝e
1i
θ1 ... e
ni
θn(3)
[0027]
②
计算主成分fi的方差贡献率以及前m个主成分的累计方差贡献率计算公式如下：
[0028][0029][0030]
其中，m的取值使累计方差贡献率达到适合当前情况的比例时，可省略其它的主成分。
[0031]
③
计算不同夹角和线束的综合支持度，具体公式如下：
[0032][0033]
其中，f
i*
为每个θi与之最相关的主成分，通过它们之间的相关系数确定ρ(θi,fj)，选择ρ(θi,fj)值最大的fj作为θi的最相关主成分，具体公式如下：
[0034][0035]
其中，σi为θi的标准差，e
ij
为特征向量ei的第j个分量。
[0036]
④
最后利用动态融合方法计算出基于空间的姿态变化情况结果，具体公式如下：
[0037][0038]
其中，ηi为第i组数据的二元指示变量，其取值随具体工作环境确定，当输入数据量较少的时候令m＝n，反之如果输入的数据量较大则取累计贡献率达到适合当前环境的比例进行融合。
[0039]
进一步，在步骤s3中所述的利用距离更新函数刻画不同时刻下机器狗到墙面距离的变化趋势，然后计算姿态变化的参数，具体包括：
[0040]
s31：保存as内的数据，然后将每一组的数据代入距离更新函数计算出参数和并将这些参数保存下来，其中计算的函数如下所示：
[0041][0042]
其中，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时刻距离变化的补偿参数；
[0043]
s32：利用每个时刻的参数和计算出每个时刻每组扫描线的变化率，计算公式如下所示：
[0044][0045]
其中，
△
k表示时刻k前后的时间差；表示参数a在k时刻，水平方向夹角为α，垂直方向角度为β的第一个参数；表示参数a在k时刻，水平方向夹角为α，垂直方向角度为β的第二个参数，表示参数b在k时刻，水平方向夹角为α，垂直方向角度为β的第一个参数；表示参数b在k时刻，水平方向夹角为α，垂直方向角度为β的第二个参数。因连续运动中前后两个时刻参数的变化率不会太大，所以将参数变化率较小的线束保留，剔除变化率较大的数据，然后利用保留的线束计算出参数的平均变化率，计算公式如下：
[0046][0047]
其中，和分别代表参数和的平均变化率，再将平均变化率代入公式(9)计算k时刻的和
[0048]
s33：将步骤s32计算出的和代入公式(2)计算不同夹角和不同线束的下姿态变化参数，然后基于主成分分析的数据融合算法将不同夹角和线束计算得到的姿态变化参数θ进行融合最终得到基于时间的姿态变化参数具体过程与步骤s23相同。
[0049]
进一步，在步骤s4中所述的面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，其特征在于：所述步骤s4具体为：
[0050]
利用时空自适应融合函数计算出最优的姿态变化参数具体公式如下所示：
[0051]
[0052][0053]
其中，和分别为基于空间和时间姿态变化参数的最优估计，σ1和σ2分别为基于空间和时间上的姿态变化自适应权重，θ
space
和θ
time
分别表示当前基于空间和时间姿态变化最大的值。
[0054]
本发明的有益效果在于：
[0055]
1、本发明仅采用激光雷达采集不同角度和线束离墙面的距离数据用于辅助修正智能四足机器狗在狭窄隧道的姿态，如此减少了安装惯性导航系统等实施成本，同时避免了运用点集配准算法时对庞大数据的处理。
[0056]
2、相比于现有的辅助姿态修正方法，本发明采用结合距离更新函数和主成分分析方法能更加准确、稳定地测量出姿态变化情况。
[0057]
3、利用自适应权重融合姿态变化，如此能够根据环境更好的获得姿态变化的情况。
[0058]
4、本发明利用基于空间和时间对智能四足机器狗实现姿态变化的检测，能够弥补激光雷达扫描到非墙面距离的缺陷，提升了扫描到障碍物时检测结果的稳定性、准确性以及鲁棒性。
附图说明
[0059]
为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：
[0060]
图1为本发明实施例所述辅助姿态修正方法的主流程图；
[0061]
图2为发明实施例所述的激光雷达安装位置图；
[0062]
图3为发明实施例所述的应用基于空间和时间的计算姿态变化的流程图；
[0063]
图4为发明实施例所述的扫描狭窄隧道墙面模型图。
具体实施方式
[0064]
下面将结合附图，对本发明的优选实施例进行详细的描述。
[0065]
本实施例中，选用16线激光雷达作为数据采集传感器，在ros系统中编写算法以实现面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统。
[0066]
本实施例中，如图1所示，面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统，包括以下步骤：
[0067]
s1：利用激光雷达1采集多个时刻下不同夹角和线束到墙面的距离，然后对采集的数据进行预处理，剔除无效的数据；
[0068]
s2：利用同一时刻下激光雷达不同夹角和不同线束测量的距离计算基于空间的姿态变化参数，并通过主成分分析算法融合基于空间的姿态变化参数；
[0069]
s3：利用距离更新函数刻画多个时刻下机器狗到墙面距离的参数变化率，然后计算参数的平均变化率，再将参数的平均变化率和距离进行迭代重新计算距离，利用重新计算的距离求出基于时间的姿态变化参数，最后利用主成分分析算法融合基于时间的姿态变
化参数；
[0070]
s4：利用时空自适应融合方法融合基于时间和空间的姿态变化参数，最后得到机器狗最优姿态变化参数，以辅助修正姿态。
[0071]
所述步骤s1中，激光雷达1位置如图2所示，应为固定采集，并安装在智能机器狗2的背部上面中间位置，0.5-0.7米，检测角为360度，分辨率为0.18度，以每秒钟10帧的速率采集离墙面的距离，然后剔除检测距离数据中特别大或特别小的无效数据，在狭窄隧道采集场景如图4所示，具体数据采集和处理包括：
[0072]
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
°
，以此采集数据；
[0073]
s12:根据环境自适应筛选方法选取不同垂直角度的线束和不同的夹角，均以轴为平分线采集数据；
[0074]
s13:筛选数据中不符合狭窄隧道场景中的数据并剔除，例如将距离特别大和特别小的数据剔除。
[0075]
本实施例中，所述步骤s2的计算及融合同一时刻中不同夹角和不同线束下的姿态变化参数包括：
[0076]
s21:利用不同夹角和不同线束和(其中α＝30
°
,45
°
,60
°
,90
°
,120
°
，β＝1
°
,7
°
,15
°
)测量到墙面的交点及交点前后各两个小角度的距离，利用这5个交点的距离求平均距离来代替和到墙面的距离，计算公式如下：
[0077][0078]
其中，和表示垂直方向角度为β，激光雷达线束oa与ob夹角角度为α的直线距离，oa-2
和ob-2
表示激光雷达扫描线与墙面交点处前两个小角度到墙面的距离；oa-1
和ob-1
表示激光雷达扫描线与墙面交点处前一个小角度到墙面的距离，oa和ob表示激光雷达扫描线与墙面中间点的交点距离；同理可知，oa2和ob2分别表示激光雷达扫描线与墙面交点处后两个小角度的距离，oa1和ob1分别表示激光雷达扫描线与墙面交点处后一个小角度的距离。
[0079]
s22:将和代入差和比(difference over summation,dos)函数计算基于空间的姿态变化参数，以衡量机器狗姿态变化情况，具体的计算公式如下：
[0080][0081]
其中，θ表示机器狗姿态变化参数，能体现机器狗姿态变化情况，如果θ＜0说明机器狗的头部朝左偏；如果θ＞0说明机器狗的头部朝右偏；若θ＝0说明机器狗与墙面平行，|θ|越大表示机器狗的头部偏转越大，反之则说明偏转越小。
[0082]
s23:利用基于主成分分析的数据融合算法融合不同夹角和线束计算得到的姿态
变化参数，从而得到基于空间的最优姿态变化参数估计，具体如下：
[0083]
①
将不同夹角和不同线束计算的基于空间的姿态变化参数θ看作一个整体θ＝(θ1,θ2,...,θn)，其中n为数据的个数。设θ的协方差矩阵为d，求得特征根为λ1≥λ2≥...≥λn和相应的单位化正交特征向量为e1,e2,...,en，其中第i个主成分单位化正交特征向量为ei＝(e
1i
,e
2i
,...,e
ni
)，因此第i个主成分为：
[0084]fi
＝ei′
θ＝e
1i
θ1 ... e
ni
θn(3)
[0085]
②
计算主成分fi的方差贡献率以及前m个主成分的累计方差贡献率计算公式如下：
[0086][0087][0088]
其中，m的取值使累计方差贡献率达到适合当前情况的比例时，可省略其它的主成分。
[0089]
③
计算不同夹角和线束的综合支持度，具体公式如下：
[0090][0091]
其中，f
i*
为每个θi与之最相关的主成分，通过它们之间的相关系数确定ρ(θi,fj)，选择ρ(θi,fj)值最大的fj作为θi的最相关主成分，具体公式如下：
[0092][0093]
其中，σi为θi的标准差，e
ij
为特征向量ei的第j个分量。
[0094]
④
最后利用动态融合方法计算出基于空间的姿态变化情况结果，具体公式如下：
[0095][0096]
其中，ηi为第i组数据的二元指示变量，其取值随具体工作环境确定，当输入数据量较少的时候可以令m＝n，反之如果输入的数据量较大则取累计贡献率达到适合当前环境的比例进行融合。
[0097]
本实施例中，所述步骤s3中所述的利用距离更新函数刻画不同时刻下机器狗到墙面距离的变化趋势，然后计算姿态变化的参数，具体包括：
[0098]
s31:保存as(比如：5s)内的数据，然后将每一组的数据代入距离更新函数计算出参数参数和并将这些参数保存下来，其中计算的函数如下所示：
[0099]
[0100]
其中，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时刻距离变化的补偿参数；
[0101]
s32:利用每个时刻的参数和计算出每个时刻每组线束的变化率，计算公式如下所示：
[0102][0103]
其中，
△
k表示时刻k前后的时间差；表示参数a在k时刻，水平方向夹角为α，垂直方向角度为β的第一个参数；表示参数a在k时刻，水平方向夹角为α，垂直方向角度为β的第二个参数，同理可知的含义。因连续运动中前后两个时刻参数的变化率不会太大，所以将参数变化率较小的线束保留，剔除变化率较大的数据，然后利用保留的线束计算出参数的平均变化率，计算公式如下：
[0104][0105]
其中，和分别代表参数和的平均变化率，再将平均变化率代入公式(9)计算k时刻的和
[0106]
s33:将步骤s32计算出的和代入公式(2)计算不同夹角和不同线束的下姿态变化参数，然后基于主成分分析的数据融合算法将不同夹角和线束计算得到的姿态变化参数θ进行融合最终得到基于时间的姿态变化参数具体过程与步骤s23相同。
[0107]
本实施例中，所述步骤s4的利用时空自适应融合函数计算出最优的姿态变化参数具体公式如下所示：
[0108][0109][0110]
其中，和分别为基于空间和时间姿态变化参数的最优估计，σ1和σ2分别为基于空间和时间上的姿态变化自适应权重，θ
space
和θ
time
分别表示当前基于空间和时间姿态变化最大的值。
[0111]
本实施例中，一种面向电缆隧道巡检的四足机器狗辅助姿态修正系统，包括存储
器和控制器，所述存储器内存储有计算机可读程序，所述计算机可读程序被控制器调用时，能执行如本实施例中所述的面向电缆隧道巡检的四足机器狗辅助姿态修正方法及系统的步骤。本方法实现了智能四足机器狗2在狭窄隧道巡检中的应用，该方法仅利用激光雷达1采集的数据，计算智能四足机器狗2的姿态变化信息，以此辅助机器狗修正姿态，为智能机器狗的控制系统提供更多的信息，能增强避障能力，使其能安全快速通过障碍物3。
[0112]
最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明权利要求书所限定的范围。