一种四足机器人模型参数的实时校正方法与流程

1.本发明涉及机器人控制技术领域，具体涉及一种四足机器人模型参数的实时校正方法。


背景技术：

2.基于四足机器人具备较强的灵活性、优秀的环境适应性的特性，其在灾后救援、野外勘探以及军事物资运输等领域都有较明朗的应用场景。并且近年来已经陆续开始应用于巡检，物流配送等各个领域。
3.机器人模型参数一般只会在出厂时进行一次测量，但是机器人的模型参数会随着在机器人身上加装设备或者存放货物后有较大的变化，进而影响到控制算法的力矩计算结果，最终会导致机器人对运动轨迹跟随的误差较大或者导致运动性能大大减弱从而摔倒，使得四足机器人的实际使用效果不佳。


技术实现要素：

4.本发明的目的就在于解决上述背景技术的问题，而提出一种四足机器人模型参数的实时校正方法，实现根据四足机器人当前的运动状态数据和运动力矩，实时计算四足机器人动力学模型的实时参数，对动力学模型的历史参数进行实时矫正，保证四足机器人的实际使用效果。
5.本发明的目的可以通过以下技术方案实现：
6.本发明实施例提供了一种四足机器人模型参数的实时校正方法，所述方法包括：
7.当接收到用于控制四足机器人运动状态的控制指令，将当前所述四足机器人的传感器模块采集的数据，送入预设的状态估计算法，得到所述四足机器人当前的运动状态数据；
8.根据所述控制指令，计算所述四足机器人当前的运动力矩；所述运动力矩用于控制所述四足机器人的关节电机，以使所述四足机器人运动；
9.将所述四足机器人当前的所述运动状态数据和所述运动力矩，代入所述四足机器人的动力学模型，计算出所述动力学模型的实时参数；
10.使用所述实时参数校正所述动力学模型的历史参数。
11.可选地，在根据所述控制指令，计算所述四足机器人当前的运动力矩之前，所述方法还包括：
12.使用模型预测控制mpc算法对所述四足机器人进行数学建模，得到所述动力学模型。
13.可选地，根据所述控制指令，计算所述四足机器人当前的运动力矩，包括：
14.根据所述控制指令，预测所述四足机器人的期望轨迹状态数据；
15.根据所述动力学模型，得到所述四足机器人的运动状态方程；
16.根据所述运动状态方程，得到所述四足机器人的运动状态离散迭代方程；所述运
动状态离散迭代方程用于根据四足机器人当前的运动状态数据预测四足机器人未来若干时刻的运动状态数据；
17.将使用所述运动状态离散迭代方程预测的运动状态数据与所述期望轨迹状态数据进行拟合，求解得到所述四足机器人当前的运动力矩。
18.可选地，将使用所述运动状态离散迭代方程预测的运动状态数据与所述期望轨迹状态数据进行拟合，求解得到所述四足机器人当前的运动力矩，包括：
19.将使用所述运动状态离散迭代方程预测的运动状态数据与所述期望轨迹状态数据采用最小二乘法进行拟合，并添加摩擦力约束，拟合方程组为：
20.其中，xi表示当前时刻四足机器人的运动状态数据，x
i 1
表示预测的未来时刻四足机器人的运动状态数据，x
i 1，ref
表示该未来时刻四足机器人的期望轨迹运动状态数据，ui表示运动力矩，qi和ri为权重比例，ai和bi为常量矩阵，ci为摩擦系数，ci和为摩擦力上下边界；
21.求解拟合方程组得到所述四足机器人当前的运动力矩。
22.可选地，所述动力学模型包括所述四足机器人的线速度微分方程和角速度微分方程；
23.所述线速度微分方程为：
24.所述角速度微分方程为：
25.其中，是所述四足机器人的线速度，fi是地面对所述四足机器人的足端的反作用力，即所述运动力矩，m是所述四足机器人的质量，g是重力加速度，i是所述四足机器人的转动惯量，ω是所述四足机器人的角速度，ri是所述四足机器人的质心到足端的向量。
26.可选地，所述运动状态数据包括所述四足机器人的角加速度、线加速度和所述四足机器人的质心到足的向量。
27.可选地，将所述四足机器人当前的所述运动状态数据和所述运动力矩，代入所述四足机器人的动力学模型，计算出所述动力学模型的实时参数，包括：
28.将所述运动力矩和所述线加速度，代入所述线速度微分方程，得到最小二乘公式一：
[0029][0030]
当最小二乘公式一的结果最小时，解得m，作为所述四足机器人的质量参数；
[0031]
将所述运动力矩、所述线加速度和所述四足机器人的质心到足的向量，代入所述
角速度微分方程，得到最小二乘公式二：
[0032][0033]
当最小二乘公式二的结果最小时，解得i，作为所述四足机器人的转动惯量参数。
[0034]
可选地，使用所述实时参数校正所述动力学模型的历史参数，包括：
[0035]
根据所述实时参数采用一个线性的学习率校正所述动力学模型的历史参数，经过数次迭代，直到所述历史参数近似于所述实时参数。
[0036]
本发明的有益效果：
[0037]
基于本发明实施例提供的四足机器人模型参数的实时校正方法，当接收到用于控制四足机器人运动状态的控制指令，将当前四足机器人的传感器模块采集的数据，送入预设的状态估计算法，得到四足机器人当前的运动状态数据；根据控制指令，计算四足机器人当前的运动力矩；运动力矩用于控制四足机器人的关节电机，以使四足机器人运动；将四足机器人当前的运动状态数据和运动力矩，代入四足机器人的动力学模型，计算出动力学模型的实时参数；使用实时参数校正动力学模型的历史参数。实现了根据四足机器人当前的运动状态数据和运动力矩，实时计算四足机器人动力学模型的实时参数，对动力学模型的历史参数进行实时矫正，保证四足机器人的实际使用效果。
附图说明
[0038]
下面结合附图对本发明作进一步的说明。
[0039]
图1为本发明实施例提供的一种四足机器人模型参数的实时校正方法的流程图；
[0040]
图2为本发明实施例提供的另一种四足机器人模型参数的实时校正方法的流程图；
[0041]
图3为本发明实施例提供的另一种四足机器人模型参数的实时校正方法的流程图。
具体实施方式
[0042]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
[0043]
现有技术中，机器人模型参数一般只会在出厂时进行一次测量，但是机器人的模型参数会随着在机器人身上加装设备或者存放货物后有较大的变化，进而影响到控制算法的力矩计算结果，最终会导致机器人对运动轨迹跟随的误差较大或者导致运动性能大大减弱从而摔倒，使得机器人的实际使用效果不佳。
[0044]
针对上述问题，本发明实施例提供了一种四足机器人模型参数的实时校正方法。参见图1，图1为本发明实施例提供的一种四足机器人模型参数的实时校正方法的流程图。该方法可以包括以下步骤：
[0045]
s101，当接收到用于控制四足机器人运动状态的控制指令，将当前四足机器人的传感器模块采集的数据，送入预设的状态估计算法，得到四足机器人当前的运动状态数据。
[0046]
s102，根据控制指令，计算四足机器人当前的运动力矩。
[0047]
s103，将四足机器人当前的运动状态数据和运动力矩，代入四足机器人的动力学模型，计算出动力学模型的实时参数。
[0048]
s104，使用实时参数校正动力学模型的历史参数。
[0049]
运动力矩用于控制四足机器人的关节电机，以使四足机器人运动。
[0050]
基于本发明实施例提供的四足机器人模型参数的实时校正方法，实现了根据四足机器人当前的运动状态数据和运动力矩，实时计算四足机器人动力学模型的实时参数，对动力学模型的历史参数进行实时矫正，保证四足机器人的实际使用效果。
[0051]
一种实现方式中，工作人员可以通过智能终端向四足机器人发送控制指令，以使该四足机器人按路径规划的照期望轨迹运动。
[0052]
一种实现方式中，四足机器人的机身上可以安装陀螺仪、加速度计和电机编码器等传感器模块。
[0053]
一种实现方式中，预设的状态估计算法可以由技术人员根据实际需求选择，在此不做限定。例如，可以使用卡尔曼滤波、扩展卡尔曼滤波和互补滤波中的任意一个作为预设的状态估计算法。
[0054]
一种实现方式中，可以使用模型优化方法计算动力学模型的实时参数，例如，可以使用最小二乘法，或者，可以使用梯度下降法等优化方法，在此不作限定。
[0055]
在一个实施例中，参见图2，在图1的基础上，在步骤s102之前，该方法还包括：
[0056]
s105，使用模型预测控制mpc算法对四足机器人进行数学建模，得到动力学模型。
[0057]
一种实现方式中，可以使用主流的mpc算法对四足机器人进行数学建模，可以不受机器人复杂结构的影响，适用于各种各样大小的四足机器人。
[0058]
一种实现方式中，在对四足机器人进行数学建模的过程中，首先简化四足机器人的模型构造，忽略腿的质量，假设质量全部集中在机身上。
[0059]
在一个实施例中，动力学模型包括四足机器人的线速度微分方程和角速度微分方程。
[0060]
线速度微分方程为：
[0061][0062]
角速度微分方程为：
[0063][0064]
其中，是四足机器人的线速度，fi是地面对四足机器人的足端的反作用力，即运动力矩，m是四足机器人的质量，g是重力加速度，i是四足机器人的转动惯量，ω是四足机器人的角速度，ri是四足机器人的质心到足端的向量。
[0065]
在一个实施例中，运动状态数据包括四足机器人的角加速度、线加速度和四足机器人的质心到足的向量。
[0066]
在一个实施例中，参见图3，在图2的基础上，步骤s102包括以下步骤：
[0067]
s1021，根据控制指令，预测四足机器人的期望轨迹状态数据。
[0068]
s1022，根据动力学模型，得到四足机器人的运动状态方程。
[0069]
s1023，根据运动状态方程，得到四足机器人的运动状态离散迭代方程。
[0070]
s1024，将使用运动状态离散迭代方程预测的运动状态数据与期望轨迹状态数据进行拟合，求解得到四足机器人当前的运动力矩。
[0071]
运动状态离散迭代方程用于根据四足机器人当前的运动状态数据预测四足机器人未来若干时刻的运动状态数据。
[0072]
一种实现方式中，因此可以简化公式(2)，得到公式(3)：
[0073][0074]
而欧拉角与角速度有如下关系：
[0075][0076]
根据公式(1)、(3)和(4)可以得到四足机器人的运动状态方程为：
[0077][0078]
公式(5)也可以写成如下的微分方程：
[0079][0080]
其中，ac(ψ)和bc(r1,
…
,rn,ψ)分别表示关于ψ和r1,...,rn,ψ的矩阵。
[0081]
根据公式(5)可知，四足机器人的动力学模型主要受三类参数影响，即机器人的质量参数、转动惯量参数和质心到足端的向量。其中质心到足端的向量可以通过状态估计算法得到，所以四足机器人的动力学模型主要影响来源于机器人的质量参数和转动惯量参数。
[0082]
由于r1,...,rn,ψ可以根据状态估计器求出，所以ac(ψ)和bc(r1,
…
,rn,ψ)可以写作常量矩阵a和b。则公式(6)可以变形为：
[0083][0084]
再将x(t)和u(t)合并在一起，公式(7)继续变形成为：
[0085][0086]
公式(8)是一个典型的线性定常齐次状态方程，求解该方程得到：
[0087][0088]
其中，x0＝x(t＝0),y0＝y(t＝0)代表初始状态。
[0089]
令t＝t可以得到一个离散公式，公式(9)变形成：
[0090][0091]
展开公式(10)得到公式(11)，其中在一个离散时间段中u不变，及u(t)＝u(0)：
[0092]
x(t)＝e
at
x(0) e
bt
u(0)
ꢀꢀꢀ
(11)
[0093]
由(11)抽象出离散的迭代公式(12)(13)：
[0094][0095]
x
i 1
＝aixi biuiꢀꢀꢀ
(13)
[0096]
其中，
[0097]
在一个实施例中，步骤s1024包括：
[0098]
步骤一，将使用运动状态离散迭代方程预测的运动状态数据与期望轨迹状态数据采用最小二乘法进行拟合，并添加摩擦力约束，拟合方程组为：
[0099][0100]
其中，xi表示当前时刻四足机器人的运动状态数据，x
i 1
表示预测的未来时刻四足机器人的运动状态数据，x
i 1，ref
表示该未来时刻四足机器人的期望轨迹运动状态数据，ui表示运动力矩，qi和ri为权重比例，ai和bi为常量矩阵，ci为摩擦系数，ci和为摩擦力上下边界。
[0101]
步骤二，求解拟合方程组得四足机器人当前的到运动力矩。
[0102]
一种实现方式中，以四足机器人的质心为坐标原点，四足机器人的正前方为x轴，正上方为z轴，正左方为y轴建立坐标系。其中，f
ix
、f
iy
和f
iz
是足端反作用力在三维坐标x轴、y轴和z轴三个方向的分量，且这三个分量满足
[0103]
其中，μ为摩擦系数，整理上述三个方程得到：
[0104][0105]
令
[0106]
令
[0107]
得到：
[0108][0109]
对任意时刻都如公式(16)的摩檫力约束，对于离散化依然成立：
[0110][0111]
在一个实施例中，步骤s103包括：
[0112]
将运动力矩和线加速度，代入线速度微分方程，得到最小二乘公式一：
[0113][0114]
当最小二乘公式一的结果最小时，解得m，作为四足机器人的质量参数。
[0115]
将运动力矩、线加速度和四足机器人的质心到足的向量，代入角速度微分方程，得到最小二乘公式二：
[0116][0117]
当最小二乘公式二的结果最小时，解得i，作为四足机器人的转动惯量参数。
[0118]
一种实现方式中，小二乘公式一和小二乘公式二可以根据qpoases等解算库解算出结果。
[0119]
在一个实施例中，步骤s104具体为：
[0120]
根据实时参数采用一个线性的学习率校正动力学模型的历史参数，经过数次迭代，直到历史参数近似于实时参数。
[0121]
一种实现方式中，为了使动力学模型的参数能够逐渐变化，避免由模型参数剧烈变化，可能导致的四足机器人运动产生的副作用，可以采用一种线性的迭代公式校正动力学模型的模型参数。通过迭代也可以消除计算出的实时参数中的白噪声。
[0122]mnew
＝(1-k)*m
old
k*m
estimate
ꢀꢀꢀ
(20)
[0123]
其中，m
new
为动力学模型校正后当前的实际参数，m
old
动力学模型校正前的历史参
数，m
estimate
为计算出的实时参数，k为学习率。k可以由技术人员根据经验进行设置，在此不作限定。
[0124]
可以使用公式(20)根据计算的实时参数校正动力学模型的历史参数。
[0125]
以上对本发明的一个实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之内。