[发明专利]一种机器人移动控制方法、系统及惯性传感控制装置

权利要求书

1.一种机器人移动控制方法，其特征在于，包括：

S1，获取惯性传感器的角速度数据；

S2，对所述角速度数据进行在线自适应滤波的预处理；

所述S2包括：

S2.1，初始化状态量和系统自适应参数，所述状态量包括角速度、角速度一阶导数和角速度二阶导数；

S2.2，建立具有系统自适应参数的状态过程模型；

S2.3，根据建立的具有系统自适应参数的状态过程模型对所述状态量进行预测，获取状态预测值以及状态协方差预测值；

S2.4，根据所述状态预测值、测量数据值和状态协方差预测值对所述状态量进行更新，获取状态估计值；

S2.5，根据所述状态估计值计算角速度二阶导数均值及角速度二阶导数估计值；

利用下式计算角速度二阶导数均值；

其中为0至k时刻角速度二阶导数均值，为k时刻的状态估计值的第三行值，k为采样时刻；并按照下式获取系统k-1时刻和k时刻的角速度二阶导数估计值

其中为k-1时刻状态估计的第三行值，为k时刻状态估计的第三行值；

S2.6，根据所述角速度二阶导数估计值对所述系统自适应参数进行修正；

根据采样时刻k值的大小，选择修正系统自适应参数α和的方法，若k小于等于4进入步骤2.6.1，若k大于4进入步骤2.6.2，

2.6.1当采样时刻k小于等于4时，因为采样数据较少，采用当前统计模型的参数取值方法，按如下方式计算系统自适应参数α和

α＝α₀其中α₀为系统自适应参数α的初值，

如果则取

如果则取

如果则取(0,10]之间的任意数，

其中，为k时刻角速度二阶导数估计值，π为圆周率，取为3.14，a_M为正的常数，取为3，a_-M为与a_M绝对值相等的负常数，取为-3；

2.6.2当采样时刻k大于4时，按下式计算系统自适应参数α和

其中b是大于1的常数，r_k(1)为k时刻角速度二阶导数向前一步相关函数，r_k-1(1)为k-1时刻角速度二阶导数向前一步相关函数，和分别为k-1时刻和k时刻角速度二阶导数估计值；r_k(0)为k-1时刻角速度二阶导数自相关函数，r_k-1(0)为k-1时刻角速度二阶导数自相关函数；

自适应参数α和可按照下式计算得到：

其中，r_k(1)为k时刻的加速度向前一步相关函数，r_k(0)为k时刻的加速度自相关函数，ln为取以e为底的对数计算；α和为系统自适应参数，T为采样间隔；

S2.7，根据所述角速度二阶导数均值和所述修正后的系统自适应参数更新所述状态过程模型，获取在线自适应滤波后的角速度数据；

S3，根据经在线自适应滤波预处理的所述角速度数据建立四元数微分方程，利用龙格-库塔法求解所述四元数微分方程，获取包括目标姿态角的姿态矩阵；

S4，将目标姿态角从载体坐标系转换为导航坐标系；

S5，将所述导航坐标系中的目标姿态角中不在阈值范围内的目标姿态角排除；

S6，根据阈值范围内的目标姿态角控制机器人动作。

2.根据权利要求1所述的方法，其特征在于，所述S3包括：

S3.1，利用滤波后的角速度数据和四元数建立四元数微分方程；

S3.2，利用四阶龙格-库塔法求解所述四元数微分方程，获取由四元数描述的姿态矩阵，通过更新四元数的元值从而更新姿态矩阵，更新目标姿态角；

其中，利用四阶龙格-库塔法求解所述四元数微分方程所需的斜率初始值由四元数的初始值确定，所述四元数的初始值由目标姿态角的初始值确定。

3.根据权利要求2所述的方法，其特征在于，所述利用龙格-库塔法求解所述四元数微分方程，获取由四元数描述的姿态矩阵如下：

其中，K为斜率，t为当前时刻，h为更新的步长，式中为三轴角速度的矩阵表达式，q(t)表示四元数。