[发明专利]一种月面巡视探测器的三维定姿与局部定位方法

权利要求书

1.一种月面巡视探测器的三维定姿与局部定位方法，其特征在于步骤如下：

(1)当月面巡视探测器处于静态时，利用三轴加速度计敏感确定滚动和俯仰角；

(2)利用太阳敏感器的输出及上述步骤(1)获得的滚动和俯仰角确定偏航角姿态；

(3)以三轴姿态和陀螺偏差作为状态量，三轴加速度计确定的滚动、俯仰角和由太阳敏感器确定的偏航角，及三个陀螺输出作为测量信息，建立状态方程和测量方程，利用扩展卡尔曼滤波估计三轴姿态和陀螺偏差：

(4)当月面巡视探测器运动时，利用由步骤(3)估计得到的陀螺偏差对陀螺输出进行补偿后，计算月面巡视探测器的姿态变化，完成陀螺姿态预估，实现姿态更新，获得由导航坐标系到本体坐标系的姿态矩阵；

(5)采集月面巡视探测器各驱动轮转速、转向轮转角、左右摇臂转角信息，利用移动子系统的正运动学关系获得探测器在本体坐标系中的位置增量；

(6)将步骤(5)获得月面探测器在本体坐标系中的位置增量转换成导航坐标系中的位置增量，求解月面巡视探测器在导航坐标系三个方向上的速度和位置信息。

2.根据权利要求1所述的月面巡视探测器的三维定姿与局部定位方法，其特征在于：所述的步骤(1)利用三轴加速度计敏感确定滚动和俯仰角如下：利用三轴加速度计敏感当地重力加速度，经转换后获得滚动和俯仰角度，即设沿本体坐标系正交安装的三个加速度计的输出分别为[f_mx，f_my，f_mc]，则滚动角和俯仰角θ_m可按下式求解：

gm=fmx2+fmy2+fmz2]]>

θ_m＝arcsin(-f_mx/g_m)。