一种小型高机动救援机器人行走及作业稳定控制方法技术

本发明专利技术公开了一种小型高机动救援机器人行走及作业稳定控制方法，包括：基于位姿估计器获得小型高机动救援机器人的路径稳定性指标；获得小型高机动救援机器人的受力；基于路径稳定性指标以及受力指标获得小型高机动救援机器人的力角稳定裕度指标；基于小型高机动救援机器人的路径稳定性指标以及力角稳定裕度指标建立路径的稳定性代价函数，从而获得路径执行的总时间；或者建立路径规划超图模型，在路径规划超图模型中选取所需的起点和终点，从而获得基于稳定型的最优路径和次优路径，小型高机动救援机器人根据需要选择最优路径或次优路径。还公开了对应的控制装置、电子设备以及计算机可读存储介质，提高了稳定控制精度和效果。和效果。和效果。

【技术实现步骤摘要】
一种小型高机动救援机器人行走及作业稳定控制方法


[0001]本专利技术属于智能救援机器人控制
，尤其涉及一种小型高机动救援机器人行走及作业稳定控制方法。

技术介绍

[0002]高机动救援机器人从工程角度上需要在救援过程中模仿人类固有的动态行为，如步行、学习功能等，并且由于面临复杂的灾害救援环境，其行走和作业过程中的稳定度高于其他类型的机器人。
[0003]智能控制采用各种智能技术从而达到对复杂系统控制的目标，其产生和发展反映当代自动控制以及科学技术的发展趋势。目前具有四种基本的机器人行走及作业稳定控制方法，并且有相互之间融合的趋势，包括：
[0004](1)机器人变结构控制：具有完全和理想鲁棒性，尽管含有不确定性，但系统在滑动模态时仍具有对外部环境的不变性，因此适合机器人的控制。变结构控制不需要精确的系统模型，只需要获知模型中参数的误差范围或变化范围即可。对于有界干扰和参数变化具有不敏感性，从而消除由于哥氏力及粘性摩擦力的作用而产生的影响。控制算法简单，容易在线实现，但是抖振现象阻碍了其在救援机器人上的实际应用，目前提出的动态调整滑模参数、在线估计滑模参数的削弱抖振改进算法效果都不好。
[0005](2)机器人模糊控制：用机器模拟人对系统的控制，包括模糊化、模糊决策和精确化计算，模糊系统为不依赖模型的估计器，主要依赖模糊规则和模糊变量的隶属度函数，无需知道输入和输出之间的数学依赖关系，是解决不确定系统控制的有效方法，但是对信息进行简单的模糊处理会导致被控系统的精度降低，动态品质变差，而提高系统精度必然增加量化等级，导致规则迅速增多，影响规则库的最佳生成，且增加系统的复杂和推理时间。
[0006](3)机器人分层递阶控制：实现传统控制功能外的规划、决策和学习等智能功能，上层作用为模仿人的行为功能，主要基于知识的系统，所实现的规划决策、学习、数据的存取、任务的协调主要是对知识进行处理；下层的作用是执行机器人具体的控制任务。
[0007](4)机器人的神经网络控制：复杂非线性系统的识别和控制，能够充分逼近任意复杂的非线性系统，学习与适应不确定系统的动态特性，具有很强的鲁棒性和容错性。然而防灾救援环境多变，因此在机器人神经网络动力学控制方法中，对于多自由度机器人控制，输入参数多，学习时间长，实时性较低，对于计算力矩控制和分解运动加速度控制不友好，因此无法在高机动救援机器人中应用。

技术实现思路

[0008]本专利技术的目的是提供一种小型高机动救援机器人行走及作业稳定控制方法和装置，通过位姿估计和以力角稳定裕度为基础的稳定性计算获得基本稳定性路径规划，作为优化的方式，可以基于图优化的方法进行路径规划，并且使用动态稳定性补偿策略获得进一步的行走及作业稳定控制效果。
[0009]本专利技术一方面提供了一种小型高机动救援机器人行走及作业稳定控制方法，包括：
[0010]步骤1，基于位姿估计器获得所述小型高机动救援机器人的路径稳定性指标；
[0011]步骤2，获得所述小型高机动救援机器人的受力；
[0012]步骤3，基于所述路径稳定性指标以及所述受力指标获得所述小型高机动救援机器人的力角稳定裕度指标；
[0013]步骤4，基于所述小型高机动救援机器人的路径稳定性指标以及所述力角稳定裕度指标建立路径的稳定性代价函数，对所述稳定性代价函数实施非平坦地形下的运动学路径约束，从而获得所述路径执行的总时间。
[0014]优选的，所述步骤4还可以为：基于所述小型高机动救援机器人的路径稳定性指标以及所述力角稳定裕度指标建立路径规划超图模型，在所述路径规划超图模型中选取所需的起点和终点，从而获得基于稳定型的最优路径和次优路径，所述小型高机动救援机器人根据需要选择所述最优路径或所述次优路径。
[0015]优选的，还包括步骤5，基于所述小型高机动救援机器人的机械臂运动对履带式移动底盘的稳定型影响，采用针对所述小型高机动救援机器人的机械臂的稳定型补偿策略。
[0016]优选的，所述步骤1包括：
[0017]步骤11，定义路径，使用n个位姿序列和n
‑
1个时间序列对路径进行描述，路径点的位姿表示为s
i
＝[x
i
,y
i
,θ
i
]T
，其中，x
i
,y
i
为路径点在平面中的位置，θ
i
为路径点的航向角，路径的位姿的序列为：
[0018]P
s
＝{s
i
}i＝1,2...,n
ꢀꢀ
(1)；
[0019]路径的时间序列表示相邻两个位姿之间的时间间隔为：
[0020]T
s
＝{Δt
i
}i＝1,2...,n
‑1ꢀꢀꢀ
(2)；
[0021]综上，可定义路径为：Q:＝(P
s
,T
s
)
ꢀꢀꢀ
(3)；
[0022]步骤12，设计位姿估计器，包括：
[0023]获取稳定性地形图，利用激光雷达、CCD等获取灾后环境的数字高程图：
[0024]H＝f
dem
(x,y) (4)
[0025]其中，f
dem
为数字高程模型，H为地形高度，x为正东方向坐标，y为正北方向坐标；救援机器人的支撑多边形为长为L，宽为H的矩形，单个履带的宽度为d；记救援机器人的中心坐标在平面上的投影为(x
c
,y
c
,0)，救援机器人的航向角为θ；与正北方向夹角，偏东方向为正，定义(
‑
180,180；将坐标原点处航向角为0的救援机器人首先进行旋转，再进行平移，得到任意位置的支撑多边形投影；原点处的支撑多边形坐标分别为：Pbase1(
‑
H/2,L/2，0)Pbase2(H/2,L/2，0)Pbase3(H/2,
‑
L/2，0)Pbase4(
‑
H/2,
‑
L/2，0)，得到：
[0026][0027]则经过旋转后坐标为：
[0028]P
base
'＝Rot(θ)P
base (6)；
[0029]平移矩阵为：
[0030]因此可得到指定位置支撑多边形的投影坐标：P＝Rot(θ)P
base
+T
ꢀꢀꢀ
(8)；
[0031]根据数字高程模型可得到救援机器人与地面支撑点的近似高度：
[0032]P
z
＝f
DEM
(P) (9)；
[0033]利用支撑点近似高度，可估计出救援机器人的俯仰角和横滚角分别为：
[0034]α＝arctan((z1‑
z4+z2‑
z3)/(2L))
[0035]β＝arcta本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种小型高机动救援机器人行走及作业稳定控制方法，其特征在于，包括：步骤1，基于位姿估计器获得所述小型高机动救援机器人的路径稳定性指标；步骤2，获得所述小型高机动救援机器人的受力；步骤3，基于所述路径稳定性指标以及所述受力指标获得所述小型高机动救援机器人的力角稳定裕度指标；步骤4，基于所述小型高机动救援机器人的路径稳定性指标以及所述力角稳定裕度指标建立路径的稳定性代价函数，对所述稳定性代价函数实施非平坦地形下的运动学路径约束，从而获得所述路径执行的总时间；或者基于所述小型高机动救援机器人的路径稳定性指标以及所述力角稳定裕度指标建立路径规划超图模型，在所述路径规划超图模型中选取所需的起点和终点，从而获得基于稳定型的最优路径和次优路径，所述小型高机动救援机器人根据需要选择所述最优路径或所述次优路径。2.根据权利要求1所述的一种小型高机动救援机器人行走及作业稳定控制方法，其特征在于，还包括步骤5，基于所述小型高机动救援机器人的机械臂运动对履带式移动底盘的稳定型影响，采用针对所述小型高机动救援机器人的机械臂的稳定型补偿策略。3.根据权利要求1所述的一种小型高机动救援机器人行走及作业稳定控制方法，其特征在于，所述步骤1包括：步骤11，定义路径，使用n个位姿序列和n
‑
1个时间序列对路径进行描述，路径点的位姿表示为s
i
＝[x
i
，y
i
，θ
i
]
T
，其中，x
i
，y
i
为路径点在平面中的位置，θ
i
为路径点的航向角，路径的位姿的序列为：P
s
＝{s
i
}i＝1，2...，n
ꢀꢀꢀꢀ
(1)；路径的时间序列表示相邻两个位姿之间的时间间隔为：T
s
＝{Δt
i
}i＝1，2...，n
‑1ꢀꢀꢀꢀ
(2)；综上，可定义路径为：Q:＝(P
s
，T
s
)
ꢀꢀꢀ
(3)；步骤12，设计位姿估计器，包括：获取稳定性地形图，利用激光雷达、CCD等获取灾后环境的数字高程图：H＝f
dem
(x，y)
ꢀꢀꢀꢀ
(4)其中，f
dem
为数字高程模型，H为地形高度，x为正东方向坐标，y为正北方向坐标；救援机器人的支撑多边形为长为L，宽为H的矩形，单个履带的宽度为d；记救援机器人的中心坐标在平面上的投影为(x
c
，y
c
，0)，救援机器人的航向角为θ；与正北方向夹角，偏东方向为正，定义(
‑
180，180；将坐标原点处航向角为0的救援机器人首先进行旋转，再进行平移，得到任意位置的支撑多边形投影；原点处的支撑多边形坐标分别为：Pbase1(
‑
H/2，L/2，0)Pbase2(H/2，L/2，0)Pbase3(H/2，
‑
L/2，0)Pbase4(
‑
H/2，
‑
L/2，0)，得到：则经过旋转后坐标为：P
base
'＝Rot(θ)P
base
ꢀꢀꢀꢀ
(6)；
平移矩阵为：因此可得到指定位置支撑多边形的投影坐标：P＝Rot(θ)P
base
+T
ꢀꢀꢀꢀ
(8)；根据数字高程模型可得到救援机器人与地面支撑点的近似高度：P
z
＝f
DEM
(P)
ꢀꢀꢀꢀ
(9)；利用支撑点近似高度，可估计出救援机器人的俯仰角和横滚角分别为：α＝arctan((z1‑
z4+z2‑
z3)/(2L))β＝arctan((z2‑
z1+z3‑
z4)/(2H))
ꢀꢀꢀꢀ
(10)使用位姿估计器，由路径的位姿s
i
＝[x
i
，y
i
，θ
i
]
T
和数字高程模型f
dem
得到救援机器人的完整位姿：步骤13，计算路径稳定性指标，包括：基于位姿估计器得到的救援机器人的横滚角、俯仰角分别为α，β；救援机器人的重力在重力坐标系下的向量为G
g
＝[0 0
ꢀ‑
mg]
T
，m为总质量，则在机体坐标系下重力向量为：f
grav
＝R
y，β
R
x，α
G
g
ꢀꢀꢀꢀ
(12)；其中，相邻的位姿为S
i
，S
i+1
，相邻位姿之间的时间间隔为Δt
i
，得到近似速度：同理，由相邻速度可得到近似加速度：其中，a
tran
，a
rota
为平动加速度和转动加速度；由加速度a
i
和救援机器人的质量和转动惯量，得到惯性力和惯性力矩为：f
inertial
＝m
·
a
tran
其中，J
x
，J
y
，J
z
为救援机器人机体坐标系下的转动惯量；通过在通过安装在机器人上的力传感器获得负载力；通过各力或力矩获得救援机器人的稳定性。
4.根据权利要求1所述的一种小型高机动救援机器人行走及作业稳定控制方法，其特征在于，所述步骤3包括：步骤31，定义力角稳定裕度：β
i
＝d
i
θ
i
|f
r
|，i＝1，2
ꢀꢀꢀꢀ
(16)在这个定义中，稳定裕度是力、距离、角度三者的乘积；角度θ
i
反映的是质心的高度，质心越高则θ
i
越小，稳定性越差；距离d
i
反映有效合力的力矩对稳定性的影响，当d
i
接近0时系统达到倾翻的临界状态；力f
r
反映出系统的敏感度，当f
r
很小时一旦有外力干扰就会导致失稳；当β
i
＝0时，系统处于临界稳定状态；步骤32，计算力角稳定裕度的计算履带式救援机器人的履带与地面的接触投影为矩形，将矩形的顶点定义为接触点将矩形的边定义为倾翻边，则倾翻边为：e
i
＝P
i+1
‑
P
i i＝1，2，3e4＝P1‑
P4ꢀꢀꢀꢀ
(17)；定义履带式救援机器人的重心位置为Pc，定义重心到倾翻轴的法线为倾覆法线，则可得到...

【专利技术属性】
技术研发人员：秦绪坤，陈彤，张新，李兰芸，杨玲，宋黎，
申请(专利权)人：新兴际华集团有限公司，
类型：发明
国别省市：