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其中α0为系统自适应参数α的初值,如果 则取
如果 则取
如果 则 取(0,10]之间的任意数,
其中, 为k时刻角速度二阶导数估计值,π为圆周率,取为3.14,aM为正的常数,取为
3,a-M为与aM绝对值相等的负常数,取为-3;
2.6.2当采样时刻k大于4时,按下式计算系统自适应参数α和其中b是大于1的常数,rk(1)为k时刻角速度二阶导数向前一步相关函数,rk-1(1)为k-1时刻角速度二阶导数向前一步相关函数, 和 分别为k-1时刻和k时刻角速度二阶导数估计值;rk(0)为k-1时刻角速度二阶导数自相关函数,rk-1(0)为k-1时刻角速度二阶导数自相关函数;
自适应参数α和 可按照下式计算得到:
其中,rk(1)为k时刻的加速度向前一步相关函数,rk(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)表示四元数。
4.根据权利要求1至3任一项所述的方法,其特征在于,所述目标姿态角包括俯仰角和航向角;所述俯仰角的阈值范围的绝对值为二十至五十度,所述航向角的阈值范围为二十度至四十度。
5.一种机器人移动控制系统,其特征在于,包括:采集单元,用于获取惯性传感器的角速度数据;预处理单元,用于对所述角速度数据进行在线自适应滤波的预处理;
所述预处理单元具体用于:
初始化状态量和系统自适应参数,所述状态量包括角速度、角速度一阶导数和角速度二阶导数;
建立具有系统自适应参数的状态过程模型;
根据建立的具有系统自适应参数的状态过程模型对所述状态量进行预测,获取状态预测值以及状态协方差预测值;
根据所述状态预测值、测量数据值和状态协方差预测值对所述状态量进行更新,获取状态估计值;
根据所述状态估计值计算角速度二阶导数均值及角速度二阶导数估计值;
利用下式计算角速度二阶导数均值;
其中 为0至k时刻角速度二阶导数均值, 为k时刻的状态估计值 的第三行值,k为采样时刻;并按照下式获取系统k-1时刻和k时刻的角速度二阶导数估计值其中 为k-1时刻状态估计 的第三行值, 为k时刻状态估计 的第三行值;
根据所述角速度二阶导数估计值对所述系统自适应参数进行修正;
根据采样时刻k值的大小,选择修正系统自适应参数α和 的方法,若k小于等于4进入步骤2.6.1,若k大于4进入步骤2.6.2,
2.6.1当采样时刻k小于等于4时,因为采样数据较少,采用当前统计模型的参数取值方法,按如下方式计算系统自适应参数α和α=α0其中α0为系统自适应参数α的初值,如果 则取
如果 则取
如果 则 取(0,10]之间的任意数,
其中, 为k时刻角速度二阶导数估计值,π为圆周率,取为3.14,aM为正的常数,取为
3,a-M为与aM绝对值相等的负常数,取为-3;
2.6.2当采样时刻k大于4时,按下式计算系统自适应参数α和其中b是大于1的常数,rk(1)为k时刻角速度二阶导数向前一步相关函数,rk-1(1)为k-1时刻角速度二阶导数向前一步相关函数, 和 分别为k-1时刻和k时刻角速度二阶导数估计值;rk(0)为k-1时刻角速度二阶导数自相关函数,rk-1(0)为k-1时刻角速度二阶导数自相关函数;
自适应参数α和 可按照下式计算得到:
其中,rk(1)为k时刻的加速度向前一步相关函数,rk(0)为k时刻的加速度自相关函数,ln为取以e为底的对数计算;α和 为系统自适应参数,T为采样间隔;
根据所述角速度二阶导数均值和所述修正后的系统自适应参数更新所述状态过程模型,获取在线自适应滤波后的角速度数据;
处理单元,用于根据经在线自适应滤波预处理的所述角速度数据建立四元数微分方程,利用龙格-库塔法求解所述四元数微分方程,获取包括目标姿态角的姿态矩阵;
坐标系转换单元,用于将目标姿态角从载体坐标系转换为导航坐标系;
筛选单元,用于将所述导航坐标系中的目标姿态角中不在阈值范围内的目标姿态角排除;
控制单元,用于根据阈值范围内的目标姿态角控制机器人动作。
6.根据权利要求5所述的系统,其特征在于,所述处理单元具体用于:利用滤波后的角速度数据和四元数建立四元数微分方程;
利用四阶龙格-库塔法求解所述四元数微分方程,获取由四元数描述的姿态矩阵,通过更新四元数的元值从而更新姿态矩阵,更新目标姿态角;
其中,利用四阶龙格-库塔法求解所述四元数微分方程所需的斜率初始值由四元数的初始值确定,所述四元数的初始值由目标姿态角的初始值确定。
7.根据权利要求5至6任一项所述的系统,其特征在于,所述利用龙格-库塔法求解所述四元数微分方程,获取由四元数描述的姿态矩阵如下:其中,K为斜率,t为当前时刻,h为更新的步长,式中 为三轴角速度的矩阵表达式,q(t)表示四元数。
8.一种惯性传感控制装置,其特征在于,包括权利要求5至7任一项所述的机器人移动控制系统,所述惯性传感控制装置与机器人无线通信。