一种基于力觉的落足点选择方法技术

本发明专利技术属于自动控制技术领域，具体涉及一种基于力觉的落足点选择方法，所述方法基于机器人本体传感器数据构建力觉感知与检测模型，通过雅克比动力学求解足端力结合摆动轨迹完成对碰撞的检查，并在出现碰撞后进行落足点的重规划和摆动轨迹的重规划，本发明专利技术基于足端位置

【技术实现步骤摘要】
一种基于力觉的落足点选择方法


[0001]本专利技术属于自动控制
，具体涉及一种基于力觉的落足点选择方法
。

技术介绍

[0002]四足机器人越障中非常容易出现由于没有感知或者感知盲区，导致腿部摆动与地形发生碰撞和干涉
。
由于传统控制策略仅采用固定摆动高度和摆动轨迹，则在发生碰撞后会导致机器人踏空和摔倒
。
因此，在发生上述问题时对摆动行为的重规划十分重要，需要通过对落足点重新选择，进一步完成二次摆动轨迹规划，实现更加可靠的越障
。

技术实现思路

[0003](
一
)
要解决的技术问题
[0004]本专利技术要解决的技术问题是：如何提供一种基于力觉的落足点选择方法，以解决四足机器人无感知
/
弱感知越障中的摆动干涉问题
。
[0005](
二
)
技术方案
[0006]为解决上述技术问题，本专利技术提供一种基于力觉的落足点选择方法，所述方法包括如下步骤：
[0007]步骤
S1
：基于机器人结构参数与单腿构型
、
输入关节角度
、
速度
、
扭矩，获取足端在全局坐标系下的位置
、
速度和力；
[0008]步骤
S2
：基于机器人当前摆动轨迹
、
足端位置
、
速度和力，构建带有速度门限的判别权重函数
W
i
，当加权值大于阈值时，判断为当前摆动中出现碰撞并进一步触发重规划；
[0009]步骤
S3
：基于判别权重函数
W
i
的计算结果，设计反向史密斯触发器设定上下限阈值
W
L
,W
H
，当
W
i
高于
W
H
时判别结果
W
check
为1，并进行保持，直到当
W
i
低于
W
L
时判别结果
W
check
为0，当加权值大于阈值时判断为当前摆动中出现碰撞并进一步触发重规划；
[0010]步骤
S4
：基于判别结果
W
check
，当判别结果
W
check
为1时触发摆动落足点和轨迹的重规划，在执行过程中不再进行二次碰撞的检查，上述流程将在二次摆动结束后重置；则重规划过程首先获取当前碰撞点的位置，进一步对倒立摆计算的落足点进行修正，采用修正后的落足点位置结合碰撞点位置实现摆动轨迹重规划
。
[0011]其中，所述步骤
S1
的具体步骤如下：
[0012]步骤
S1
‑1：使用关节角度反馈值，通过运动学正解计算各支撑腿足端在胯关节坐标系
{H}
下的三维位置：
[0013]P
iH
＝
f(c1,c2,c3),i
＝
1...4
[0014]其中，
P
iH
为第
i
条支撑腿足端在胯关节坐标系
{H}
下的三维位置；
f(c1,c2,c3)
为对应单腿构型的运动学正解计算函数：
[0015][0016]其中，
c1为机器人大腿角度，
c2为小腿角度，
c3为胯关节角度；
L1为大腿长度，
L2为小腿长度，
L3为侧摆电机相对跨关节的偏差，
L
end
为足端到跨关节的直线距离；
[0017]步骤
S1
‑2：将各支撑腿足端在胯关节坐标系
{H}
下的三维位置转换为机体坐标系
{B}
下的三维位置：
[0018][0019]其中，
P
iB
为第
i
条支撑腿足端在机体坐标系
{B}
下的三维位置，
W
为机器人左右两侧支撑腿胯关节间距，
H
为机器人前后两侧支撑腿胯关节间距；
[0020]步骤
S1
‑3：基于机器人机载
IMU
测量得到的姿态四元数，获取当前全局坐标系
{N}
与机体坐标系
{B}
间的转换矩阵，并将各支撑腿足端在机体坐标系
{B}
下的三维位置转换为全局坐标系
{N}
下的三维位置：
[0021][0022][0023]其中，为全局坐标系
{N}
与机体坐标系
{B}
间的转换矩阵，
q0、q1、q2、q3为机器人当前姿态四元数；为的转置；
P
iN
为第
i
条支撑腿足端在全局坐标系
{N}
下的三维位置；
[0024]步骤
S1
‑4：定义第
i
条腿当前的雅克比矩阵为
J
i
,
为输入的关节速度，则基于雅克比动力学原理，将输入的关节角速度换算为机体坐标系
{B}
下的足端速度
V
iB
：
[0025][0026]进一步转换得到全局坐标系
{N}
的足端速度：
[0027][0028]步骤
S1
‑5：定义第
i
条腿当前的雅克比矩阵为
J
i
，
τ
i
为输入的关节扭矩，则基于虚功原理，将输入的关节扭矩换算为机体坐标系
{B}
下的足端力
F
iB
：
[0029][0030]进一步转换得到全局坐标系
{N}
的足端力：
[0031][0032]其中，所述步骤
S2
的具体步骤如下：
[0033]步骤
S2
‑1：定义第
i
条腿当前摆动完成比例为
α
i
,0
＜
α
i
＜1，则构建摆动碰撞判别窗口上下界
α
L
,
α
H
，仅在该窗口内进行后续碰撞检测；
[0034]步骤
S2
‑2：定义如下的判别权重函数
W
i
，其采用摆动轨迹误差权重
、
速度权重和力
阈值权重进行加权，具体计算方式如下；
[0035][0036]其中，
ω1为摆动轨迹权重系数，
ω2为速度权重系数，
ω3为力权重系数，为当前本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种基于力觉的落足点选择方法，其特征在于，所述方法包括如下步骤：步骤
S1
：基于机器人结构参数与单腿构型
、
输入关节角度
、
速度
、
扭矩，获取足端在全局坐标系下的位置
、
速度和力；步骤
S2
：基于机器人当前摆动轨迹
、
足端位置
、
速度和力，构建带有速度门限的判别权重函数
W
i
，当加权值大于阈值时，判断为当前摆动中出现碰撞并进一步触发重规划；步骤
S3
：基于判别权重函数
W
i
的计算结果，设计反向史密斯触发器设定上下限阈值
W
L
,W
H
，当
W
i
高于
W
H
时判别结果
W
check
为1，并进行保持，直到当
W
i
低于
W
L
时判别结果
W
check
为0，当加权值大于阈值时判断为当前摆动中出现碰撞并进一步触发重规划；步骤
S4
：基于判别结果
W
check
，当判别结果
W
check
为1时触发摆动落足点和轨迹的重规划，在执行过程中不再进行二次碰撞的检查，上述流程将在二次摆动结束后重置；则重规划过程首先获取当前碰撞点的位置，进一步对倒立摆计算的落足点进行修正，采用修正后的落足点位置结合碰撞点位置实现摆动轨迹重规划
。2.
如权利要求1所述的基于力觉的落足点选择方法，其特征在于，所述步骤
S1
的具体步骤如下：步骤
S1
‑1：使用关节角度反馈值，通过运动学正解计算各支撑腿足端在胯关节坐标系
{H}
下的三维位置：
P
iH
＝
f(c1,c2,c3),i
＝
1...4
其中，
P
iH
为第
i
条支撑腿足端在胯关节坐标系
{H}
下的三维位置；
f(c1,c2,c3)
为对应单腿构型的运动学正解计算函数：其中，
c1为机器人大腿角度，
c2为小腿角度，
c3为胯关节角度；
L1为大腿长度，
L2为小腿长度，
L3为侧摆电机相对跨关节的偏差，
L
end
为足端到跨关节的直线距离；步骤
S1
‑2：将各支撑腿足端在胯关节坐标系
{H}
下的三维位置转换为机体坐标系
{B}
下的三维位置：其中，
P
iB
为第
i
条支撑腿足端在机体坐标系
{B}
下的三维位置，
W
为机器人左右两侧支撑腿胯关节间距，
H
为机器人前后两侧支撑腿胯关节间距；步骤
S1
‑3：基于机器人机载
IMU
测量得到的姿态四元数，获取当前全局坐标系
{N}
与机体坐标系
{B}
间的转换矩阵，并将各支撑腿足端在机体坐标系
{B}
下的三维位置转换为全局坐标系
{N}
下的三维位置：
其中，为全局坐标系
{N}
与机体坐标系
{B}
间的转换矩阵，
q0、q1、q2、q3为机器人当前姿态四元数；为的转置；
P
iN
为第
i
条支撑腿足端在全局坐标系
{N}
下的三维位置；步骤
S1
‑4：定义第
i
条腿当前的雅克比矩阵为
J
i
,
为输入的关节速度，则基于雅克比动力学原理，将输入的关节角速度换算为机体坐标系
{B}
下的足端速度
V
iB
：进一步转换得到全局坐标系
{N}
的足端速度：步骤
S1
‑5：定义第
i
条腿当前的雅克比矩阵为
J
i
，
τ
i
为输入的关节扭矩，则基于虚功原理，将输入的关节扭矩换算为机体坐标系
{B}
下的足端力
F
iB
：进一步转换得到全局坐标系
{N...

【专利技术属性】
技术研发人员：邢伯阳，刘宇飞，王志瑞，许鹏，梁振杰，赵建新，邱天奇，苏波，江磊，
申请(专利权)人：中兵智能创新研究院有限公司，
类型：发明
国别省市：