ros2中的磁力记如何校准

获取数据

磁力计由于自身精度与环境的影响都是有误差的，磁力计的X，Y，Z轴的数值在实际中是椭球的形状，下面代码可以获取ros2中的磁力计数值保存为一个名为data.txt的文件

import rclpy
from rclpy.node import Node

from nav_msgs.msg import Path,Odometry

from sensor_msgs.msg import NavSatFix,Imu
from geometry_msgs.msg import PoseStamped,Quaternion,Vector3,Pose,PoseWithCovariance,PoseArray,Point,Pose
from visualization_msgs.msg import Marker

import pyproj
import numpy as np

from tf_transformations import euler_from_quaternion,quaternion_from_euler
import math
import matplotlib.pyplot as plt

class controlNode(Node):
    def __init__(self):
        super().__init__('compass_node')  
        self.imu = self.create_subscription(
            Vector3,
            'compass',
            self.compass_callback,
            10)
        self.data = []
        timer_period = 0.1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)


    def timer_callback(self):
        # print(len(self.data))
        # if len > 100:
        pass

    def compass_callback(self,msg):
        x = int(msg.x)
        y = int(msg.y)
        z = int(msg.z)
        # print(x,y,z)

        with open(file="./data.txt",mode="a",encoding="utf-8") as f:
            data=f.write(str(x) + ' ' + str(y) + ' '  + str(z) + '\r')
            print(data)

        # self.data.append([x,y,z])
        # print(x)

def main(args=None):
    rclpy.init(args=args)
    minimal_subscriber = controlNode()
    rclpy.spin(minimal_subscriber)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

数据的格式看起来是这个样子的

拟合球体

使用最小二乘法拟合磁力计的圆心位置，在python中有可以直接使用的工具scipy.optimize.leastsq，下面代码调用刚刚获取的磁力计数值，然后拟合出圆心的位置与直径

import numpy as np
from scipy.optimize import leastsq
import matplotlib.pyplot as plt
import threading
import time
import rclpy
from rclpy.node import Node

import compassAdjust


with open(file="./data.txt",mode="r",encoding="utf-8") as f:
    lines = f.read().split()

numbers = []
for line in lines:
    number = int(line)
    numbers.append(number)
arr = np.array(numbers).reshape(-1,3)
x = arr[:,0]
y = arr[:,1]
z = arr[:,2]

def sphere_func(params, x, y, z):
    x0, y0, z0, r = params
    return (((x - x0) ** 2 + (y - y0) ** 2 + (z - z0) ** 2) - r ** 2)

initial_params = np.array([0.0, 0.0, 0.0, 10.0])
# 拟合
params, covariance = leastsq(lambda p: sphere_func(p, x, y, z), initial_params)
print("拟合参数:", params)
print("误差:", covariance)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(x,y,z)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')

plt.show()

将磁力计数据绘制成了一个椭球，并且输出了圆心坐标与半径


将磁力计数值分别减去拟合的参数，重新获取数据然后拟合这个时候圆心的坐标已经很接近0了

方位角

磁力计是在每一个轴上的磁力数值，传感器正常使用肯定会有一点倾斜的，肯定会有roll和pitch角。只要飞机倾斜，磁力计测得的磁场强度就不是真正的水平X，Y轴上的分量了，下面代码head是方位角，ypr是传感器放置的yaw,pitch,row角度，error是刚拟合的圆心数值，这样就求出了准确的方位角了

void IMU::headCalcu(float *head,sensors_vec_t compass,float *ypr,float *error){
    float xh,yh;

    float x = compass.x - error[0];
    float y = compass.y - error[1];
    float z = compass.z - error[2];

    xh = x*cos(ypr[1]) + y*sin(ypr[2])*sin(ypr[1]) - z*cos(ypr[2])*sin(ypr[1]);
    yh = y*cos(ypr[2]) + z*sin(ypr[2]);

    *head = atan2(yh, xh) ;
}