一种四足机器人自适应复杂地形的方法

policy gradient(ppo)，采用离线学习的方式，并行环境数量为512个，mini_batch大小为2048，控制频率为20hz。
13.奖励函数：总奖励函数r＝r
mf
+r
dx
+r
fa
+r
cp
+r
jp
+r
bs
14.move forward reward(r
mf
)：
[0015][0016]
其中是身体的航向角(yaw)，v
x
是沿x轴方向的身体的速度
[0017]
distance from x-axis(r
dx
)：
[0018]rdx
:＝|d
x
|
[0019]
其中d
x
为身体中心离x轴的垂直距离
[0020]
feet airtime reward(r
fa
)：
[0021]rfa
:＝∑
i∈legs
|t
stance
(i)-0.5|
[0022]
其中t
stance
为处于支撑相的腿在上一个摆动相的总滞空时间
[0023]rcp
:＝|c
1-c4|+|c
2-c3|-(|c
1-c2|+|c
3-c4|+|c
1-c3|+|c
2-c4|)/6
[0024]
其中c1、c2、c3、c4分别表示左前腿、右前腿、左后腿、右后腿的触地情况
[0025]
joint position symmetry(r
jp
)：
[0026][0027]
其中ai表示第i个电机的实际角度位置
[0028]
base stability(r
bs
):
[0029][0030]
其中θ
base
是身体的俯仰角(pitch)，γ
base
是身体的横滚角(roll)
[0031]
所述的奖励函数，其特征为，使用了特殊奖励量：
[0032]
move forward reward(r
mf
)：
[0033][0034]
其中是身体的航向角z(yaw)，v
x
是沿x轴方向的身体的速度。
附图说明
[0035]
图1是本发明的总体控制框架图。
[0036]
图2是机器人小跑步态(trot)步态的说明图
[0037]
图3是仿真训练中所使用的宇树科技a1机器人模型图
[0038]
图4是运动学逆解坐标系的说明图
具体实施方式
[0039]
下面结合附图与具体实施方式对本发明作进一步详细描述：
[0040]
参照附图1，本发明公开了一种四足机器人自适应复杂地形的方法。本发明运用了深度强化学习技术训练多层感知机(mdp)控制四足机器人的运动，四足机器人每一条腿有个关节，每个足端的位置表示为三维空间坐标，多层感知机发出的指令为每个足端的坐标，
该坐标经过机器人的运动学逆解过程转换为对应该腿的3个关节的角度位置，对强化学习的动作空间进行约束，约束范围是期望的足端轨迹运动范围。
[0041]
参照附图1，可将机器人的控制过程定义为马尔科夫决策过程(mdp)，mdp是所有强化学习算法遵循的学习过程范式，它是一种在离散化的时间空间在根据状态估计值生成随机化动作的数学模型，一个马尔科夫决策过程由动作a、状态s、标量奖励函数r＝(s_t,s_(t-1))和状态过度概率p(s_(t+1)|s_t,a_t)，一个智能体(agent)可以根据状态s_t在策略π(a_t|s_t)中找到对应的动作a_t，强化学习算法的目标是找到可以使加权的总奖励在有限的时间中最大化的有效策略π^*,损失函数定义如下：
[0042][0043]
其中τ＝{(x0,a0,r0),(x1,a1,r1)
…
}是智能体采取策略π得到的一组数据轨迹。
[0044]
参照附图1在仿真环境中，机器人以20hz的频率运行，将imu传感器、足端压力传感器的数据与奖励函数值传入多层感知机，多层感知机将参数传入ppo优化算法，ppo算法输出机器人下一步的步态并且更新mlp的参数，mlp输出下一步的足端坐标，经过运动学逆解过程将足端坐标传入pd controller中，控制机器狗的下一步运动。如此不断循环训练，直至训练出能够平稳运动的模型。
[0045]
参照附图2,标准的trot(对角小跑)步态如上图，即对角腿的足端位置呈中心对称且关节角度位置相同，本发明训练出来的trot步态即与图2一致。
[0046]
对于神经网络生成的每一组动作a
t
∈r
12
，其组成为4个(x,y,z)足端坐标，神经网络的输出值归一化在(-1,1)之间，但每一个值的意义不同，因此需要对输出值进行进一步的缩放，缩放的范围根据大腿和小腿长度的不同而不同，以宇树a1机器人结构为例，肩部长为0.0838m，大腿长0.2m，小腿长0.2m，经试验合理的足端坐标限制范围为x坐标(-0.12，0.12)，y坐标(-0.1838，0.06838)，z坐标(0.1，0.4)。
[0047]
足端轨迹的前后距离最长可达到20cm左右，因此先假定一个足端的期望最大运动范围，再利用运动学逆解和递归搜索算法，算出足端在该范围内运动时出现的关节的极大极小值，在该极大极小值基础之上进一步适当扩大关节最大运动角度范围，限定的肩部关节(hip)角度范围为(-0.3，0.3)，大腿关节(thigh)角度范围为(0.0，1.3)，小腿关节(calf)角度范围为(-1.57，-0.5)，关节角度的单位采用弧度制，在urdf文档中写入被限制关节的角度转动范围。
[0048]
如附图3所示，a1机器人每条腿由三个关节组成，即肩(hip)、大腿(thigh)、小腿(calf)关节，每个关节仅在一个平面内运动，以机器人向前的朝向为x轴，肩部转动平面平行于y轴、z轴、垂直于x 轴，大腿和小腿转动平面平行于x轴、z轴、垂直于y轴，肩部关节用固定关节相连在机身上。
[0049]
如附图4所示，求解逆运动学解的空间坐标系，其中，γ表示肩部转角α表示大腿转角，β表示小腿转角。
[0050]
关于所用到的运动学逆解：已知足端坐标d(x，y，z)和关节长度(h、hu、hl)
→
求肩部、大腿、小腿的角度：γ、α、β
[0051]
旋转量偏移量
[0052][0053]
(hu+n)2+m2＝s2ꢀꢀ
(1)
[0054]
n2+m2＝hl2ꢀꢀ
(2)
[0055]
(1)式—(2)式得
[0056][0057][0058][0059][0060]
动作空间约束：对于神经网络生成的每一组动作a
t
∈r
12
，其组成为4个(x，y，z)足端坐标，神经网络的输出值归一化在(-1,1)之间，但每一个值的意义不同，因此需要对输出值进行进一步的缩放。
[0061]
在奖励函数中，本发明设计了特殊奖励量r
mf
，当机器人沿x轴d速度大于0.3且航向角的绝对值小于 0.1时，r
mf
的值固定为1，从而提高其他步态奖励量以优化步态。
[0062]
上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。

技术特征：
1.一种运用强化学习模型训练四足机器人运动的方法体系，其特征在于，包括以下步骤：步骤一：将四足机器人设置为稳定站立姿态以及欧拉角均为零的状态作为初始位姿；步骤二：利用足端压力传感器和imu传感器，读入数据，获得四足机器人的触地状态和机器人姿态的欧拉角；步骤三：将机器人的触地状态和姿态的欧拉角，以及机器人的电机角度作为强化学习模型的输入量；步骤四：电机角度和欧拉角数据用于构建多层感知机，电机角度和触地状态以及欧拉角数据用于建立奖励函数，并根据触地状态和欧拉角判断机器人是否倒地，如果倒地，则根据此训练的总奖励量是否大于目前最大总奖励量，如果大于，则更新多层感知机，如果小于，则不更新；如果没有倒地，则执行步骤五；步骤五：强化学习模型输出足部空间坐标，采用动作空间约束的方式，并使用逆运动学求解，求得目标电机角度，且保证将电机角度限制在一定范围内，控制四足机器人的下一步运动；步骤六：控制机器人电机转动到步骤五中得到的电机角度，重复步骤二至四，从而是实现对机器人的闭环训练。2.根据权利要求1所述的运用强化学习模型训练四足机器人运动的方法体系，其特征为，步骤四具体为：多层感知机：为3层神经网络，每一层分别有400、200、100个单元，其输入数量与观测量的数目保持一致，输出为12个，神经网络优化器采用adam optimizer，算法为proximal policy gradient(ppo)，采用离线学习的方式，并行环境数量为512个，mini_batch大小为2048，控制频率为20hz；奖励函数：总奖励函数r＝r
mf
+r
dx
+r
fa
+r
cp
+r
jp
+r
bs
move forward reward(r
mf
)：其中是身体的航向角(yaw)，v
x
是沿x轴方向的身体的速度distance from x-axis(r
dx
)：r
dx
:＝|d
x
|其中d
x
为身体中心离x轴的垂直距离feet airtime reward(r
fa
)：r
fa
:＝∑
i∈legs
|t
stance
(i)-0.5|其中t
stance
为处于支撑相的腿在上一个摆动相的总滞空时间r
cp
:＝|c
1-c4|+|c
2-c3|-(|c
1-c2|+|c
3-c4|+|c
1-c3|+|c
2-c4|)/6其中c1、c2、c3、c4分别表示左前腿、右前腿、左后腿、右后腿的触地情况joint position symmetry(r
jp
)：其中a
i
表示第i个电机的实际角度位置
base stability(r
bs
):其中θ
base
是身体的俯仰角(pitch)，γ
base
是身体的横滚角(roll)。3.根据权利要求2所述的奖励函数，其特征为，使用了特殊奖励量：move forward reward(r
mf
)：其中是身体的航向角z(yaw)，v
x
是沿x轴方向的身体的速度。

技术总结
本发明公开了一种控制四足十二自由度机器人稳定运动的方法。本发明根据IMU传感器、足端压力传感器的信息，约束机器人的足端轨迹运动范围，经过运动学逆解过程将机器人的足端坐标转化为关节角度，通过奖励函数及调制后的参数进行仿真训练，以在仿真软件中得到稳定的对角小跑步态，并可以在崎岖地形、台阶等复杂地形条件下稳定行走。形条件下稳定行走。


技术研发人员：塔斯图列克 温子扬 谢肖斌 刘梅
受保护的技术使用者：兰州大学
技术研发日：2022.03.12
技术公布日：2023/9/22