基于三维成像的多足机器人复杂路况离散成落脚点方法技术

本发明专利技术提供了一种基于三维成像的多足机器人复杂路况离散成落脚点方法，步骤包括：读取加速度数据和距离数据，计算获得多足机器人的位移数据；读取运动惯性测量数据，计算获得多足机器人的姿态角度数据；读取深度断层图像数据，计算获得深度断层图像；根据位移数据和姿态角度数据确定深度断层图像数据的采集位置信息，再在相邻位置的深度断层图像之间使用滑动窗口牛顿插值法，从而实现三维成像；根据三维成像，把复杂地形离散化成一个个机器人足端的落脚点。利用三维成像将复杂地形环境离散成有效的落脚点的方法具有高度的灵活性，适用于路况复杂的野外环境，能够降低复杂地面对多足机器人运动的影响，具有较高的适应能力。

The road is divided into goal method of complex 3D imaging based on multi legged robot

【技术实现步骤摘要】
基于三维成像的多足机器人复杂路况离散成落脚点方法
本专利技术涉及一种多足机器人落脚点规划方法，尤其是一种基于三维成像的多足机器人复杂路况离散成落脚点方法。
技术介绍
山区路况存在地形不规则、崎岖不平、较高的障碍物或涉水路面等非结构环境，限制了轮式和履带式运输工具的应用。多足机器人在结构上具有多冗余自由度，因而具有较高的地形环境适应能力，在森林采伐、矿山开采、水下建筑、核工业、军事运输及探测、星球探测等领域有着非常广阔的应用前景。因此，多足机器人的相关研究一直备受各国专家学者的关注，但是如何提高多足机器人在非结构环境下的移动能力仍然是个具有挑战性的课题。对于多足机器人三维成像方法，基于图像处理获取场景信息，通过图像拼接实现三维成像，该方法算法复杂，受光照影响，可靠性低；基于激光的三维成像方法主要应用于静态对象的三维建模，激光器的摆放位置严格，设备便捷度低；基于轮式或履带式移动机器人的三维成像方法受环境障碍物影响大，无法在野外复杂路况下自由移动。对于落脚点的离散方法，基于获取的三维成像，将三维图形进行离散化，结合多足机器人自身的参数和运动轨迹，将三维图像显示的复杂地形环境离散成中国古代练习功夫梅花桩一样的离散化落脚点，可以提供给多足机器人进行安全落足。有效降低了多足机器人在复杂环境中的行走难度，提高了多足机器人的地形适应能力。
技术实现思路
本专利技术要解决的技术问题是现有的三维成像方法使用复杂，且常常由于环境障碍物影响无法全方位实现三维成像。为了解决上述技术问题，本专利技术提供的基于三维成像的多足机器人复杂路况离散成落脚点方法，包括如下步骤：步骤1，读取多足机器人惯性导航系统测量的加速度数据以及测距系统测量的距离数据，再利用互补滤波法对加速度数据和距离数据进行数据融合，从而获得多足机器人的位移数据；步骤2，读取多足机器人惯性导航系统测量的惯性测量数据，并利用卡尔曼滤波对惯性测量数据进行数据融合，获得多足机器人的姿态角度数据；步骤3，读取多足机器人前进方向上激光雷达采集的深度断层图像数据，再对深度断层图像数据进行自适应中值滤波，并进行基于位移和角度的补偿，获得深度断层图像；步骤4，根据步骤1的位移数据和步骤2的姿态角度数据确定多足机器人运动轨迹，并对运动轨迹上的相邻数据采集点的深度断层图像使用滑动窗口牛顿插值法获取复杂地形环境的三维图像；步骤5，将步骤4的获取复杂地形环境的三维图像进行离散化，结合多足机器人自身运动参数和运动轨迹，将三维图像显示的复杂地形环境离散为与机器人足端大小相近的落脚点，为多足机器人提供安全落脚点。采用测量的位移数据和姿态角度数据确定深度断层图像数据的位置，从而有效提高了激光雷达的扫描范围，扩大了三维成像的面积范围；基于获取的三维成像，将三维图形进行离散化，结合多足机器人自身的参数和运动轨迹，将三维图像显示的复杂地形环境离散成中国古代练习功夫梅花桩一样的离散化落脚点，可以提供给多足机器人进行安全落足，有效降低了多足机器人在复杂环境中的行走难度，提高了多足机器人的地形适应能力。作为本专利技术的进一步限定方案，步骤1中，获得多足机器人的位移数据的具体步骤为：步骤1.1，读取多足机器人惯性导航系统测量的加速度数据为(ax(t),ay(t),az(t))，进行二重积分得到tn-1到tn时刻的近似位移(xt,yt,zt)为：式中，(vx(tn-1),vy(tn-1),vz(tn-1))为tn-1时刻的速度；步骤1.2，读取多足机器人测距系统测量的tn-1和tn时刻的距离数据为和计算tn-1到tn时刻的近似位移(mxt,myt,mzt)为：步骤1.3，利用互补滤波法进行数据融合获得多足机器人tn时刻的位移数据为：式中，为tn-1时刻进行数据融合后的位移，Tu为超声波传感器采样周期，(τx,τy,τz)为置信程度，(ax,ay,az)为动态加权系数。由于加速度积分得到的位移累积误差大，而测距系统得到的距离数据准确度高，但是存在大量噪声，所以采用互补滤波法进行数据融合，能够得到置信度更高的多足机器人位移。作为本专利技术的进一步限定方案，步骤2中，获得多足机器人的姿态角度数据的具体步骤为：步骤2.1，多足机器人惯性导航系统测量的X轴上惯性测量数据包括倾角陀螺仪测量角速度ωgyro、陀螺仪测量噪声ωg、螺仪常值误差b、加速度计测量角度以及角速度计测量噪声ωa，多足机器人惯性导航系统的状态方程和输出方程为：步骤2.2，设多足机器人惯性导航系统的采样周期为Ts，则方程(5)离散化后为：X(k|k-1)＝AX(k-1|k-1)+BU(k)(6)式中，X(k-1|k-1)为k-1时刻的后验估计，X(k|k-1)为k时刻的先验估计，X(k|k-1)的协方差为：P(k|k-1)＝AP(k-1|k-1)AT+Q(7)式中，为陀螺仪和加速度计的过程噪声矩阵，P(k-1|k-1)为协方差在k-1时刻的后验估计，P(k|k-1)为协方差矩阵在k时刻的先验估计；步骤2.3，利用卡尔曼滤波对惯性测量数据进行数据融合，卡尔曼增益系数为：K(k)＝P(k|k-1)+HT/(HP(k|k-1))HT+R)(8)式中，H＝[10]，R为测量协方差矩阵，得到角度和角度协方差的最优估计值X(k|k)和P(k|k)为：式中，Z(k)为当前陀螺仪的测量值；步骤2.4，利用步骤2.1～步骤2.3相同的步骤计算多足机器人惯性导航系统测量的Y轴和Z轴上的惯性测量数据处理，根据多足机器人在X轴、Y轴和Z轴上的惯性测量数据计算得到多足机器人的姿态角度数据。利用卡尔曼滤波对惯性测量数据进行数据融合，使测量的惯性数据具有较高的精确性，为三维成像提供较准确的位置基础。作为本专利技术的进一步限定方案，步骤3中，获得深度断层图像的具体步骤为：步骤3.1，以角度θ为X轴，深度h为Y轴，将深度断层图像展开为二维平面上的曲线h＝f(θ)，对该曲线进行自适应中值滤波，消除曲线上的椒盐噪声；步骤3.2，设多足机器人的位移为倾角为对深度断层图像进行补偿的具体步骤如下：步骤3.2.1，多足机器人前进方向上的倾角为对深度断层图像进行补偿后展开对应的曲线方程为根据tn-1和tn时刻的倾角动态加权得到理想的深度断层图像：H1＝h't-1+st/cos(βt-βt-1)cos(αt-αt-1)(11)H2＝h't+st/tan(βt-βt-1)tan(αt-αt-1)(12)h”＝(1-cos(βt-βt-1)cos(αt-αt-1))H1+cos(βt-βt-1)cos(αt-αt-1)H2(13)式中，h't-1和h't为经过补偿后tn-1和tn时刻的深度，h”为经过位移和倾角补偿后的深度值，(αt-1,βt-1)为tn-1时刻多足机器人的其中两轴姿态角，(αt,βt)为tn时刻多足机器人的其中两轴姿态角。采用自适应中值滤波能够消除曲线上的椒盐噪声，使深度断层图像展开在二维平面上曲线更加平滑，减小偶然误差；采用对深度断层图像进行补偿，能够得到较理想的深度断层图像；采用近似计算关系能够有效减少计算量，降低方法的执行复杂度，使三维成像更加迅速。作为本专利技术的进一步限定方案，步骤4中，在相邻深度断层图像之间使用滑动窗口牛顿插值法获得三维图像的具体步骤为：步骤4.1，对ti(i＝0,1,2,...,n)本文档来自技高网...

【技术保护点】
基于三维成像的多足机器人复杂路况离散成落脚点方法，其特征在于，包括如下步骤：步骤1，读取多足机器人惯性导航系统测量的加速度数据以及测距系统测量的距离数据，再利用互补滤波法对加速度数据和距离数据进行数据融合，从而获得多足机器人的位移数据；步骤2，读取多足机器人惯性导航系统测量的惯性测量数据，并利用卡尔曼滤波对惯性测量数据进行数据融合，获得多足机器人的姿态角度数据；步骤3，读取多足机器人前进方向上激光雷达采集的深度断层图像数据，再对深度断层图像数据进行自适应中值滤波，并进行基于位移和角度的补偿，获得深度断层图像；步骤4，根据步骤1的位移数据和步骤2的姿态角度数据确定多足机器人运动轨迹，并对运动轨迹上的相邻数据采集点的深度断层图像使用滑动窗口牛顿插值法获取复杂地形环境的三维图像；步骤5，将步骤4的三维图像进行离散化，结合多足机器人自身运动参数和运动轨迹，将三维图像显示的复杂地形环境离散为与机器人足端大小相近的落脚点，为多足机器人提供安全落脚点。

【技术特征摘要】
1.基于三维成像的多足机器人复杂路况离散成落脚点方法，其特征在于，包括如下步骤：步骤1，读取多足机器人惯性导航系统测量的加速度数据以及测距系统测量的距离数据，再利用互补滤波法对加速度数据和距离数据进行数据融合，从而获得多足机器人的位移数据；步骤2，读取多足机器人惯性导航系统测量的惯性测量数据，并利用卡尔曼滤波对惯性测量数据进行数据融合，获得多足机器人的姿态角度数据；步骤3，读取多足机器人前进方向上激光雷达采集的深度断层图像数据，再对深度断层图像数据进行自适应中值滤波，并进行基于位移和角度的补偿，获得深度断层图像；步骤4，根据步骤1的位移数据和步骤2的姿态角度数据确定多足机器人运动轨迹，并对运动轨迹上的相邻数据采集点的深度断层图像使用滑动窗口牛顿插值法获取复杂地形环境的三维图像；步骤5，将步骤4的三维图像进行离散化，结合多足机器人自身运动参数和运动轨迹，将三维图像显示的复杂地形环境离散为与机器人足端大小相近的落脚点，为多足机器人提供安全落脚点。2.根据权利要求1所述的基于三维成像的多足机器人复杂路况离散成落脚点方法，其特征在于，步骤1中，获得多足机器人的位移数据的具体步骤为：步骤1.1，读取多足机器人惯性导航系统测量的加速度数据为(ax(t),ay(t),az(t))，进行二重积分得到tn-1到tn时刻的近似位移(xt,yt,zt)为：式中，(vx(tn-1),vy(tn-1),vz(tn-1))为tn-1时刻的速度；步骤1.2，读取多足机器人测距系统测量的tn-1和tn时刻的距离数据为和计算tn-1到tn时刻的近似位移(mxt,myt,mzt)为：步骤1.3，利用互补滤波法进行数据融合获得多足机器人tn时刻的位移数据为：式中，为tn-1时刻进行数据融合后的位移，Tu为超声波传感器采样周期，(τx,τy,τz)为置信程度，(ax,ay,az)为动态加权系数。3.根据权利要求1所述的基于三维成像的多足机器人复杂路况离散成落脚点方法，其特征在于，步骤2中，获得多足机器人的姿态角度数据的具体步骤为：步骤2.1，多足机器人惯性导航系统测量的X轴上惯性测量数据包括倾角陀螺仪测量角速度ωgyro、陀螺仪测量噪声ωg、螺仪常值误差b、加速度计测量角度以及角速度计测量噪声ωa，多足机器人惯性导航系统的状态方程和输出方程为：步骤2.2，设多足机器人惯性导航系统的采样周期为Ts，则方程(5)离散化后为：X(k|k-1)＝AX(k-1|k-1)+BU(k)(6)式中，X(k-1|k-1)为k-1时刻的后验估计，X(k|k-1)为k时刻的先验估计，X(k|k-1)的协方差为：P(k|k-1)＝AP(k-1|k-1)AT+Q(7)式中，为陀螺仪和加速度计的过程噪声矩阵，P(k-1|k-1)为协方差在k-1时刻的后验估计，P(k|k-1)为协方差矩阵在k时刻的先验估计；步骤2.3...

【专利技术属性】
技术研发人员：陈春林，唐开强，洪俊，王岚，王子辉，刘力锋，朱张青，辛博，陈文玉，于跃文，吴涛，
申请(专利权)人：南京大学，
类型：发明
国别省市：江苏,32