imu去除零偏和降低零漂
低通滤波原理解析
该代码实现了一个 IMU 数据处理类 Imu，主要功能包括：
- 传感器零偏校准（加速度计 & 陀螺仪）
- 低通滤波（抑制高频噪声）
- 姿态解算（通过四元数积分角速度）
其中要点是低通滤波和姿态解算。具体如下：
低通滤波：
姿态解算：
去零偏和零漂算法实现(python实现)
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from std_msgs.msg import Header
import numpy as np
import matplotlib.pyplot as plt
import time
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
#import debugpy
class ImuDataLogger(Node):
def __init__(self,flag):
super().__init__('imu_data_logger')
self.angular_velocity = []
self.linear_acceleration = []
self.timestamps = [] # 存储时间戳，用于计算采样频率
self.angular_velocity_processed=[]
self.linear_acceleration_processed=[]
self.angular_velocity_bias=np.array([0.0,0.0,0.0])
self.linear_acceleration_bias=np.array([0.0,0.0,0.0])
self.last_angular_velocity=np.array([0.0,0.0,0.0])
self.last_linear_acceleration=np.array([0.0,0.0,0.0])
self.qvalue=np.array([1.0,0.0,0.0,0.0])
# self.start_time = time.time()
self.start_time =0.0
self.frequency = 0.0
self.last_time = 0.0
self.biasFlag=0
if flag=="topic":
qos_profile = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT, # 与 SensorDataQoS 相同
history=HistoryPolicy.KEEP_LAST,
depth=20 # 保留20条消息
)
self.subscription = self.create_subscription(
Imu,
'/imu', # IMU话题
self.imu_callback,
qos_profile) # 设置保留20条消息的历史记录
self.publisher_ = self.create_publisher(Imu, '/imu_processed', qos_profile)
else:
self.fileRead()
# angular_velocity = np.array(self.angular_velocity)
# linear_acceleration = np.array(self.linear_acceleration)
# dataDict=dict[str,np.ndarray]()
# dataDict['angular_velocity_x'] = angular_velocity[:,0]
# dataDict['angular_velocity_y'] = angular_velocity[:,1]
# dataDict['angular_velocity_z'] = angular_velocity[:,2]
# dataDict['linear_acceleration_x'] = linear_acceleration[:,0]
# dataDict['linear_acceleration_y'] = linear_acceleration[:,1]
# dataDict['linear_acceleration_z'] = linear_acceleration[:,2]
# self.compute_sampling_rate_and_plot(dataDict)
self.process_and_display_data()
# self.raw_display()
def raw_display(self):
# 转换为numpy数组
angular_velocity = np.array(self.angular_velocity)
linear_acceleration = np.array(self.linear_acceleration)
# # 计算均值和方差
# ang_vel_mean = np.mean(angular_velocity, axis=0)
# ang_vel_var = np.var(angular_velocity, axis=0,ddof=1)
# lin_acc_mean = np.mean(linear_acceleration, axis=0)
# lin_acc_var = np.var(linear_acceleration, axis=0,ddof=1)
# 打印结果
# self.get_logger().info(f'Angular Velocity Mean: {ang_vel_mean}')
# self.get_logger().info(f'Angular Velocity Variance: {ang_vel_var}')
# self.get_logger().info(f'Linear Acceleration Mean: {lin_acc_mean}')
# self.get_logger().info(f'Linear Acceleration Variance: {lin_acc_var}')
# 绘制图像
# self.plot_data(angular_velocity, linear_acceleration)
def fileRead(self):
with open('imu_data.txt', 'r') as f:
lines = f.readlines()
flag=0
for line in lines:
if line.strip() == 'end':
flag=1
continue
values = line.strip().split()
assert(len(values)==4)
if flag==0:
self.angular_velocity.append([float(values[0]), float(values[1]), float(values[2])])
self.timestamps.append(float(values[3]))
print("time is ",values[3])
else:
self.linear_acceleration.append([float(values[0]), float(values[1]), float(values[2])])
assert(len(self.angular_velocity)==len(self.linear_acceleration))
assert(len(self.linear_acceleration)==len(self.timestamps))
last_velocity=[]
last_linear_acceleration=[]
for i in range(len(self.timestamps)):
if len(last_velocity)==0:
angular_np_array=np.array(self.angular_velocity[i])
linear_np_array=np.array(self.linear_acceleration[i])
angular_np_array-=self.angular_velocity_bias
linear_np_array-=self.linear_acceleration_bias
self.angular_velocity_processed.append(list(angular_np_array))
self.linear_acceleration_processed.append(list(linear_np_array))
last_velocity=list(angular_np_array)[:]
last_linear_acceleration=list(linear_np_array)[:]
else:
angular_np_array=np.array(self.angular_velocity[i])
linear_np_array=np.array(self.linear_acceleration[i])
angular_np_array-=self.angular_velocity_bias
linear_np_array-=self.linear_acceleration_bias
angular_np_array=np.array([last_velocity,angular_np_array])
linear_np_array=np.array([last_linear_acceleration,linear_np_array])
processed_angular_data,processed_linear_data=self.low_pass_filter(angular_np_array,linear_np_array)
self.angular_velocity_processed.append(list(processed_angular_data))
self.linear_acceleration_processed.append(list(processed_linear_data))
last_velocity=list(processed_angular_data)[:]
last_linear_acceleration=list(processed_linear_data)[:]
def imu_callback(self, msg: Imu):
if self.biasFlag==0:
if self.start_time == 0.0:
self.start_time=(float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec) / 1e9)
current_time=(float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec) / 1e9)
# current_time = time.time()
# self.get_logger().info(f'Frequency is : {current_time - self.start_time}')
if current_time - self.start_time < 10.0: # 记录10秒
# 记录角速度和线性加速度
self.angular_velocity.append([
msg.angular_velocity.x,
msg.angular_velocity.y,
msg.angular_velocity.z])
self.linear_acceleration.append([
msg.linear_acceleration.x,
msg.linear_acceleration.y,
msg.linear_acceleration.z])
self.timestamps.append(current_time)
else :
self.biasFlag=1
# # 转换为numpy数组
angular_velocity = np.array(self.angular_velocity)
linear_acceleration = np.array(self.linear_acceleration)
#计算均值和方差
ang_vel_mean = np.mean(angular_velocity, axis=0)
ang_vel_var = np.var(angular_velocity, axis=0,ddof=1)
lin_acc_mean = np.mean(linear_acceleration, axis=0)
lin_acc_var = np.var(linear_acceleration, axis=0,ddof=1)
time_deltas = np.diff(self.timestamps) # 计算时间间隔
avg_sampling_period = np.mean(time_deltas) # 平均采样周期
sampling_rate = 1 / avg_sampling_period # 采样频率
self.frequency=sampling_rate
# self.frequency=20.0
# # 打印结果
self.angular_velocity_bias=ang_vel_mean
self.linear_acceleration_bias=lin_acc_mean
self.get_logger().info(f'Frequency is : { self.frequency}')
self.get_logger().info(f'Angular Velocity Mean: {self.angular_velocity_bias}')
self.get_logger().info(f'Angular Velocity Variance: {ang_vel_var}')
self.get_logger().info(f'Linear Acceleration Mean: {self.linear_acceleration_bias}')
self.get_logger().info(f'Linear Acceleration Variance: {lin_acc_var}')
self.get_logger().info(f'imu bias complete')
else :
self.output_data(msg)
# current_time = time.time()
# if current_time - self.start_time < 100: # 记录100秒
# # 记录角速度和线性加速度
# self.angular_velocity.append([
# msg.angular_velocity.x,
# msg.angular_velocity.y,
# msg.angular_velocity.z])
# self.linear_acceleration.append([
# msg.linear_acceleration.x,
# msg.linear_acceleration.y,
# msg.linear_acceleration.z])
# self.timestamps.append(current_time)
# self.get_logger().info(f'Time is: {current_time - self.start_time}')
# else:
# # 当超过100秒，停止订阅并处理数据
# self.subscription.destroy()
# with open('imu_data.txt', 'w') as f:
# z=0
# for i in self.angular_velocity:
# for j in i:
# f.write(str(j)+' ')
# f.write(str(self.timestamps[z]))
# z+=1
# f.write("\n")
# f.write("end\n")
# z=0
# for i in self.linear_acceleration:
# for j in i:
# f.write(str(j)+' ')
# f.write(str(self.timestamps[z]))
# z+=1
# f.write("\n")
# angular_velocity = np.array(self.angular_velocity)
# linear_acceleration = np.array(self.linear_acceleration)
# dataDict=dict[str,np.ndarray]()
# dataDict['angular_velocity_x'] = angular_velocity[:,0]
# dataDict['angular_velocity_y'] = angular_velocity[:,1]
# dataDict['angular_velocity_z'] = angular_velocity[:,2]
# dataDict['linear_acceleration_x'] = linear_acceleration[:,0]
# dataDict['linear_acceleration_y'] = linear_acceleration[:,1]
# dataDict['linear_acceleration_z'] = linear_acceleration[:,2]
# self.compute_sampling_rate_and_plot(dataDict)
# self.process_and_display_data()
# 定义四元数乘法
def quat_multiply(self,q1, q2):
# 四元数乘法公式
w1, x1, y1,z1 =q1
w2, x2, y2, z2=q2
return np.array([
w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
])
# 定义角速度转换为四元数的函数
def gyro_to_quaternion(self,gyro, dt):
"""
根据陀螺仪数据计算四元数增量。
:param gyro: 角速度 (rad/s), 一个包含三个元素的数组 [gx, gy, gz]
:param dt: 时间间隔 (秒)
:return: 更新后的四元数增量
"""
gx, gy, gz = gyro
# 将角速度转换为四元数形式 (角速度为向量部分，实部为0)
q_gyro = np.array([0, gx, gy, gz])
# 四元数增量公式: dq = 1/2 * q * gyro * dt
q_delta = 0.5 * self.quat_multiply(self.qvalue, q_gyro) * dt
return q_delta
def output_data(self,msg: Imu):
timeNow=(float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec) / 1e9)
if self.last_time==0.0:
dt=0.0
self.last_time=timeNow
else:
dt=timeNow-self.last_time
self.last_time=timeNow
# self.get_logger().info(f"dt is: {dt} s")
msgSend=Imu()
if len(self.last_angular_velocity)==0:
self.last_angular_velocity=np.array([msg.angular_velocity.x,msg.angular_velocity.y,msg.angular_velocity.z])
self.last_linear_acceleration=np.array([msg.linear_acceleration.x,msg.linear_acceleration.y,msg.linear_acceleration.z])
self.last_angular_velocity-=self.angular_velocity_bias
self.last_linear_acceleration-=self.linear_acceleration_bias
msgSend.angular_velocity.x=self.last_angular_velocity[0]
msgSend.angular_velocity.y=self.last_angular_velocity[1]
msgSend.angular_velocity.z=self.last_angular_velocity[2]
msgSend.linear_acceleration.x=self.last_linear_acceleration[0]
msgSend.linear_acceleration.y=self.last_linear_acceleration[1]
msgSend.linear_acceleration.z=self.last_linear_acceleration[2]
else:
now_angular_velocity=np.array([msg.angular_velocity.x,msg.angular_velocity.y,msg.angular_velocity.z])
now_linear_acceleration=np.array([msg.linear_acceleration.x,msg.linear_acceleration.y,msg.linear_acceleration.z])
now_angular_velocity-=self.angular_velocity_bias
now_linear_acceleration-=self.linear_acceleration_bias
# 按垂直方向（行顺序）堆叠数组构成一个新的数组
# 堆叠的数组需要具有相同的维度
input_angular_velocity=np.vstack((self.last_angular_velocity,now_angular_velocity))
input_linear_acceleration=np.vstack((self.last_linear_acceleration,now_linear_acceleration))
processed_angular_velocity,processed_linear_acceleration=self.low_pass_filter(input_angular_velocity,input_linear_acceleration)
# msgSend.angular_velocity.x=now_angular_velocity[0]
# msgSend.angular_velocity.y=now_angular_velocity[1]
# msgSend.angular_velocity.z=now_angular_velocity[2]
# msgSend.linear_acceleration.x=now_linear_acceleration[0]
# msgSend.linear_acceleration.y=now_linear_acceleration[1]
# msgSend.linear_acceleration.z=now_linear_acceleration[2]
msgSend.angular_velocity.x=processed_angular_velocity[0]
msgSend.angular_velocity.y=processed_angular_velocity[1]
msgSend.angular_velocity.z=processed_angular_velocity[2]
msgSend.linear_acceleration.x=processed_linear_acceleration[0]
msgSend.linear_acceleration.y=processed_linear_acceleration[1]
msgSend.linear_acceleration.z=processed_linear_acceleration[2]
if abs( msgSend.angular_velocity.x)<0.001 :
msgSend.angular_velocity.x=0.0
if abs( msgSend.angular_velocity.y)<0.001 :
msgSend.angular_velocity.y=0.0
if abs( msgSend.angular_velocity.z)<0.001 :
msgSend.angular_velocity.z=0.0
if abs( msgSend.linear_acceleration.x)<0.01 :
msgSend.linear_acceleration.x=0.0
if abs( msgSend.linear_acceleration.y)<0.001 :
msgSend.linear_acceleration.y=0.0
if abs( msgSend.linear_acceleration.z)<0.001 :
msgSend.linear_acceleration.z=0.0
# self.last_angular_velocity=processed_angular_velocity[:]
# self.last_linear_acceleration=processed_linear_acceleration[:]
self.last_angular_velocity=np.array([msgSend.angular_velocity.x,msgSend.angular_velocity.y,msgSend.angular_velocity.z])
self.last_linear_acceleration=np.array([msgSend.linear_acceleration.x,msgSend.linear_acceleration.y,msgSend.linear_acceleration.z])
# 更新四元数
q_delta = self.gyro_to_quaternion([msgSend.angular_velocity.x,msgSend.angular_velocity.y,msgSend.angular_velocity.z],dt)
self.qvalue += q_delta
self.qvalue=self.qvalue/np.linalg.norm(self.qvalue)
msgSend.header = Header()
msgSend.header.stamp = msg.header.stamp
msgSend.header.frame_id = 'imu_link'
# msgSend.orientation.x = msg.orientation.x
# msgSend.orientation.y = msg.orientation.y
# msgSend.orientation.z = msg.orientation.z
# msgSend.orientation.w = msg.orientation.w
msgSend.orientation.x = self.qvalue[1]
msgSend.orientation.y = self.qvalue[2]
msgSend.orientation.z = self.qvalue[3]
msgSend.orientation.w = self.qvalue[0]
self.publisher_.publish(msgSend)
def compute_sampling_rate_and_plot(self,dictData):
# 计算采样频率
if len(self.timestamps) > 1:
time_deltas = np.diff(self.timestamps) # 计算时间间隔
avg_sampling_period = np.mean(time_deltas) # 平均采样周期
sampling_rate = 1 / avg_sampling_period # 采样频率
self.get_logger().info(f"Estimated Sampling Rate: {sampling_rate:.2f} Hz")
else:
sampling_rate = 1.0 # 如果没有足够时间戳，假设采样频率为1Hz
for key in dictData.keys():
self.get_logger().info(f"Length of {key}: {len(dictData[key])}")
# 计算频谱
data = dictData[key]
fft_result = np.fft.fft(data)
frequencies = np.fft.fftfreq(len(fft_result), d=1/sampling_rate)
positive_freq_indices = frequencies > 0
fft_amplitude = np.abs(fft_result[positive_freq_indices])
positive_frequencies = frequencies[positive_freq_indices]
# 绘制频谱图
plt.figure(figsize=(10, 6))
plt.plot(positive_frequencies, fft_amplitude)
plt.title("Frequency Spectrum of IMU Data {}".format(key))
plt.xlabel("Frequency (Hz)")
plt.ylabel("Amplitude")
# plt.xlim(0, 1000) # 设置频率范围为0到1000Hz
plt.grid()
plt.show(block=False)
def process_and_display_data(self):
# 转换为numpy数组
angular_velocity = np.array(self.angular_velocity_processed)
linear_acceleration = np.array(self.linear_acceleration_processed)
#计算均值和方差
ang_vel_mean = np.mean(angular_velocity, axis=0)
ang_vel_var = np.var(angular_velocity, axis=0,ddof=1)
lin_acc_mean = np.mean(linear_acceleration, axis=0)
lin_acc_var = np.var(linear_acceleration, axis=0,ddof=1)
# 打印结果
self.get_logger().info(f'Angular Velocity Mean: {ang_vel_mean}')
self.get_logger().info(f'Angular Velocity Variance: {ang_vel_var}')
self.get_logger().info(f'Linear Acceleration Mean: {lin_acc_mean}')
self.get_logger().info(f'Linear Acceleration Variance: {lin_acc_var}')
# 绘制图像
self.plot_data(np.array(self.angular_velocity), np.array(self.linear_acceleration),angular_velocity,linear_acceleration)
def low_pass_filter(self, angular_velocity, linear_acceleration):
# 进行低通滤波
cutoff_frequency = 2.5 # 希望保留的频率范围
frequency=self.frequency
filter_ratio=1.0/(1.0+frequency/(2*3.14159265*cutoff_frequency))
angular_velocity[1,:]=angular_velocity[1,:]*filter_ratio+(1-filter_ratio)*angular_velocity[0,:]
linear_acceleration[1,:]=linear_acceleration[1,:]*filter_ratio+(1-filter_ratio)*linear_acceleration[0,:]
return angular_velocity[1,:],linear_acceleration[1,:]
def plot_data(self, angular_velocity, linear_acceleration,angular_velocity_processed,linear_acceleration_processed):
time_range = np.linspace(0, 100, len(angular_velocity))
fig, axs = plt.subplots(2, 1, figsize=(10, 8))
# 角速度图像
# axs[0].plot(time_range, angular_velocity[:, 0], label='X')
# axs[0].plot(time_range, angular_velocity_processed[:, 0], label='XP')
# axs[0].plot(time_range, angular_velocity[:, 1], label='Y')
# axs[0].plot(time_range, angular_velocity_processed[:, 1], label='YP')
axs[0].plot(time_range, angular_velocity[:, 2], label='Z')
axs[0].plot(time_range, angular_velocity_processed[:, 2], label='ZP')
axs[0].set_title('Angular Velocity')
axs[0].set_xlabel('Time (s)')
axs[0].set_ylabel('Angular Velocity (rad/s)')
axs[0].legend()
# 加速度图像
# axs[1].plot(time_range, linear_acceleration[:, 0], label='X')
# axs[1].plot(time_range, linear_acceleration_processed[:, 0], label='XP')
# axs[1].plot(time_range, linear_acceleration[:, 1], label='Y')
# axs[1].plot(time_range, linear_acceleration_processed[:, 1], label='YP')
axs[1].plot(time_range, linear_acceleration[:, 2], label='Z')
axs[1].plot(time_range, linear_acceleration_processed[:, 2], label='ZP')
axs[1].set_title('Linear Acceleration')
axs[1].set_xlabel('Time (s)')
axs[1].set_ylabel('Linear Acceleration (m/s²)')
axs[1].legend()
plt.tight_layout()
plt.show()
def main(args=None):
# debugpy.listen(('localhost', 5678))
# debugpy.wait_for_client()
rclpy.init(args=args)
node = ImuDataLogger("topic")
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
扫一扫
266
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
