[发明专利]测量物体位置和方位的方法

摘要

本发明公开了一种测量物体位置和方位的方法，物体的方位首先由三轴陀螺仪的测量结果计算出第一方位。三轴加速度传感器仅在物体处于静止状态时计算第二方位。三轴地磁传感器在物体处于静止状态时、或者在物体持续运动一定时间内计算第三方位。第二方位、第三方位是对第一方位的修正。传统的测量物体方位的方法仅使用三轴陀螺仪，这会产生累计误差而导致长时间测量的结果不准确。本发明在使用三轴陀螺仪的基础上，又采用三轴加速度传感器和三轴地磁传感器对三轴陀螺仪的方位测量结果进行修正，从而可以获得较为准确的物体方位。

主权项

一种测量物体位置和方位的方法，所述物体上包括三轴陀螺仪、三轴加速度传感器和三轴地磁传感器，其特征是，其中，测量物体位置的方法是：三轴加速度传感器测量物体的加速度并输出给处理器，处理器将该加速度中剔除掉重力加速度分量和向心加速度分量，得到线性加速度分量；处理器将该线性加速度分量与时间进行两次积分运算得到物体在该时间内的线性位移；处理器将物体的初始位置与该线性位移相加，得到物体的当前位置；其中，测量物体方位的方法是：三轴陀螺仪测量物体的角速度并输出给处理器，处理器将该角速度与时间进行积分运算得到物体在该时间内的角位移，处理器将物体的初始方位与该角位移相加称为第一方位；处理器至少给出四元数形式和欧拉角形式表示的第一方位；当物体处于静止状态时，三轴加速度传感器测量得到的物体的加速度就是该物体的重力加速度，处理器根据此时的物体的加速度值计算物体的欧拉角形式的俯仰角和滚动角；当计算出的俯仰角的绝对值小于或等于80度时，将计算出的俯仰角、滚动角以及上述第一方位的偏航角作为第二方位；当计算出的俯仰角的绝对值大于80度时，将计算出的俯仰角、上述第一方位的滚动角和偏航角作为第二方位；处理器给出四元数形式表示的第二方位；当物体处于静止状态时、或者当物体处于运动状态且持续运动的时间在预设值之内时，三轴地磁传感器测量地磁信号并输出给处理器，处理器根据该地磁信号计算物体的欧拉角形式的偏航角，并将俯仰角和滚动角分别设为欧拉角形式的上述第一方位的俯仰角和滚动角，作为第三方位；处理器给出四元数形式表示的第三方位；当物体处于静止状态时，处理器计算第二方位与第一方位的差值作为补偿量一，还计算第三方位与第一方位的差值作为补偿量二，处理器将第一方位与补偿量一、补偿量二相加作为物体的方位；当物体处于运动状态且持续运动的时间在预设值之内，处理器计算第三方位与第一方位的差值作为补偿量二，处理器将第一方位与补偿量二相加作为物体的方位；当物体处于运动状态且持续运动的时间超出预设值，处理器将第一方位作为物体的方位。