MEMS传感器机械臂姿态检测【附代码】
✨ 长期致力于MEMS传感器、机械臂、惯性测量单元、数据融合、姿态检测系统研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》
（1）设计基于ICM20948的惯性测量单元硬件与卡尔曼滤波器数据融合：
IMU包含三轴加速度计、陀螺仪与磁力计，采样率设为两百赫兹。硬件电路以STM32F103C8T6为主控，通过nRF52832蓝牙将数据上传至上位机。针对加速度计与陀螺仪的数据融合，设计离散卡尔曼滤波器，状态向量为四元数，状态转移矩阵基于陀螺仪角速度积分。观测向量由加速度计与磁力计计算的重力方向与地磁方向得到。过程噪声协方差矩阵对角元素分别设为零点零零一与零点零零零五，观测噪声协方差设为零点一。在Matlab中对水平静止、水平运动与往复运动三种工况进行仿真，卡尔曼滤波的俯仰角均方根误差为零点七四度，横滚角零点四六度，优于互补滤波的一点二度。实际传感器静态测试俯仰角、偏航角、横滚角均方根误差分别为零点七四一度、零点八八八度与零点四六二度，均小于一度。
（2）开发基于C语言的上位机测试软件与机械臂末端实时姿态解算：
上位机采用Visual Studio C++编写，通过蓝牙接收数据包，数据包格式为帧头、三轴加速度、三轴角速度、三轴磁力计、校验位。解算时先对原始数据进行校准，加速度计校准采用六位置法，陀螺仪采用静止平均法消除零偏。卡尔曼滤波算法在STM32中实现以减轻上位机负担。滤波周期十毫秒，姿态更新频率一百赫兹。通过正运动学验证，将末端姿态与关节编码器推算值对比，在机械臂以每秒三十度速度运动时，姿态角误差小于零点三度。静态测试中，将IMU固定在机械臂末端，连续采集十分钟，横滚角漂移小于零点零五度每小时。
（3）建立机械臂运动学模型辅助MEMS传感器动态误差补偿：
利用机械臂正运动学模型，根据各关节角度实时计算末端理论姿态，作为卡尔曼滤波的外部观测值。当机械臂运动速度高于每秒五十度时，陀螺仪积分漂移增大，引入运动学模型辅助更新，观测噪声协方差自适应减小。反之，低速时信任IMU。通过实验采集六组不同速度下的运动数据，融合运动学辅助后，动态姿态误差均方根从零点八五度降低至零点三七度。在往复运动中，最大峰值误差从一点三度降至零点六度。最终系统集成在UR5机械臂上，末端安装IMU模块，实测抓取任务中姿态检测精度满足定位要求，定位偏差小于两毫米。
import numpy as np from filterpy.kalman import KalmanFilter class QuaternionKalman: def __init__(self, dt): self.kf = KalmanFilter(dim_x=4, dim_z=3) self.dt = dt self.kf.F = np.eye(4) # 简化状态转移，实际由角速度更新 self.kf.H = np.eye(4)[:3,:] # 观测为欧拉角转换后的四元数 self.kf.Q = np.eye(4) * 0.001 self.kf.R = np.eye(3) * 0.1 self.kf.P = np.eye(4) * 0.1 self.q = np.array([1,0,0,0]) # 初始四元数 def predict_with_gyro(self, gyro): # gyro: [wx, wy, wz] rad/s wx, wy, wz = gyro q0, q1, q2, q3 = self.q omega = np.array([ [0, -wx, -wy, -wz], [wx, 0, wz, -wy], [wy, -wz, 0, wx], [wz, wy, -wx, 0] ]) q_dot = 0.5 * omega @ self.q self.q = self.q + q_dot * self.dt self.q = self.q / np.linalg.norm(self.q) self.kf.x = self.q def update_with_acc_mag(self, acc, mag): # 从加速度计与磁力计计算期望四元数 # 简化：假设roll,pitch从acc计算，yaw从mag ax, ay, az = acc pitch = np.arcsin(-ax / 9.8) roll = np.arctan2(ay, az) yaw = np.arctan2(mag[1], mag[0]) # 欧拉角转四元数 cy = np.cos(yaw*0.5); sy = np.sin(yaw*0.5) cp = np.cos(pitch*0.5); sp = np.sin(pitch*0.5) cr = np.cos(roll*0.5); sr = np.sin(roll*0.5) q_ref = np.array([ cr*cp*cy + sr*sp*sy, sr*cp*cy - cr*sp*sy, cr*sp*cy + sr*cp*sy, cr*cp*sy - sr*sp*cy ]) self.kf.update(q_ref[:3]) # 观测前三维 self.q = self.kf.x def six_position_calibration(acc_samples): # 六位置法校准加速度计偏置与尺度因子 # acc_samples: 6个位置的平均值 # 简化返回偏置 bias = np.mean(acc_samples, axis=0) return bias if __name__ == '__main__': dt = 0.01 kf_imu = QuaternionKalman(dt) # 模拟数据 gyro_data = np.array([0.01, 0.02, 0.005]) acc_data = np.array([0.1, 0.2, 9.8]) mag_data = np.array([0.3, 0.4, 0.5]) kf_imu.predict_with_gyro(gyro_data) kf_imu.update_with_acc_mag(acc_data, mag_data) print(f'四元数: {kf_imu.q}') # 校准示例 sample_data = np.random.randn(6,3) + [0.05, 0.03, 9.81] bias = six_position_calibration(sample_data) print(f'偏置估计: {bias}')