【技术实现步骤摘要】
一种补偿足地冲击的四足机器人姿态解算方法
本专利技术涉及自动控制领域，具体涉及一种补偿足地冲击的四足机器人姿态解算方法。
技术介绍
目前基于EKF的姿态解算方法在无人机、无人车中被广泛应用，近年来随着四足机器人技术的发展，其相关姿态解算技术也逐步发展，基于传统的EKF姿态计算方法能获取实时的机器人姿态角度，但对于足式机器人来说由于其往往采用动态稳定技术需要不断摆动踏步来保持机器人稳定，足地冲击会造成本体传感器数据的噪声和运动加速度的测量，受限于目前柔顺阻抗控制技术的发展难以完全消除落足的冲击，采用传统姿态解算方法难以解决周期冲击带来的测量误差使得最终姿态测量结果发散，因此需要设计一种考虑四足机器人步态相序补偿落足冲击的新姿态解算方法。
技术实现思路
本专利技术公布了一种补偿足地冲击的四足机器人姿态解算方法，以EKF滤波器为姿态解算模块核心，通过引入步态相序信息与足底力估计结果来补偿冲击力带来的运动加速度干扰并动态调节传感器测量方差，在大冲击时降低加速度传感器的修正权重，从而提高四足机器人在步态运动中的姿态...
【技术保护点】
1.一种补偿足地冲击的四足机器人姿态解算方法，其特征在于，包括如下步骤：/n步骤1：初始化欧拉角，采用机载加速度数据求取俯仰、横滚角度：/n
【技术特征摘要】
1.一种补偿足地冲击的四足机器人姿态解算方法，其特征在于，包括如下步骤：
步骤1：初始化欧拉角，采用机载加速度数据求取俯仰、横滚角度：
步骤2：使用求取的初始俯仰、横滚角分解磁力计数据求取初始航向角度：
步骤3：采用计算得到的初始角度俯仰角α、横滚角β和航向角γ，计算初始化姿态四元数：
步骤4：初始化滤波器对应的传感器测量噪声参数，加速度测量噪声an，陀螺仪测量噪声wm，陀螺仪测量偏差噪声wn，磁力计测量偏差噪声mn，得到对应的系统噪声矩阵与测量噪声矩阵：
步骤5：采用足端里程计修正航向陀螺数据，首先计算各支撑腿移动速度引起的转向速度，计算速度矢量，以第i条支撑腿为例：
步骤6：计算末端速度矢量vi末端位置pi质心连线间的夹角
γ1＝arctan(pi,x/pi,y)
γ2＝arctan(vi,x/vi,y)
步骤7：使用该夹角对速度矢量进行分解求取转向方向的足端线速度分量
vr,i＝|v|icos(γ3)
步骤8：基于末端位置将速度矢量转换为对应的里程计角速度值
步骤9：累加各支撑腿的里程计角速度并求取均值
步骤10：采用足端里程计结果修正陀螺仪角速度测量值
步骤11：采用一阶龙格库卡法，采用陀螺仪测量数据基于时间T更新四元数状态：
步骤12：计算当前机体与世界坐标系下的旋转矩阵
步骤13：更新陀螺仪测量偏差状态：
wn，k+1＝w...
【专利技术属性】
技术研发人员：邢伯阳，刘宇飞，索旭东，李冀川，
申请(专利权)人：智能移动机器人中山研究院，
类型：发明
国别省市：广东;44
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。