CN101344391B - 基于全功能太阳罗盘的月球车位姿自主确定方法 - Google Patents

Abstract

本发明是一种用于深空探测科学及地球观测领域的基于全功能太阳罗盘的月球车位姿自主确定方法。该方法是以观测设备、嵌入式PC104+计算机及内置于广角太阳相机的嵌入式TMS320计算机为基础完成的。本发明利用在嵌入式系统中的广角太阳相机、IMU及高速激光雷达接口程序获取数据；并将月球车位姿确定任务交与系统中的智能太阳识别与处理、月球垂直陀螺、初始位姿校准、高精度太阳罗盘导航、位姿补偿方法、基于地图重建与即时定位(SLAM)月球车速度估计、具有速度修正的月球车位姿确定、基于太阳连测的经纬校准的各方法模块并行运行，整个系统中所执行的处理方法共同完成了月球车位姿的确定。该方法为月球车提供适应月球环境的自主位姿确定提供了高可靠性的方案。

Description

基于全功能太阳罗盘的月球车位姿自主确定方法

技术领域

基于全功能太阳罗盘的月球车位姿自主确定方法是以月球探测车(简称月球车)为应用对象的天文与惯性联合导航方法，属深空探测科学及地球观测科学与工程领域。可广泛应用于月球车、火星车及地面作战机器人的导航与控制系统当中。

背景技术

作为月球车的导航技术面临三项重要环境约束：(1)月球无地磁，现有的磁罗盘技术不能应用；(2)月球是慢自旋的(相对地球自转慢了近30倍)，陀螺寻北技术不能应用于月球北向确定；(3)尽管现代飞行器及航天器广泛利用太阳及恒星进行位置确定，但在月球车巡游探测时，采用太阳敏感器或星敏感器，都易受粉尘污染，致使使用寿命很短。另一方面，月球离地球很远，通信及数据处理延时一般高于8秒甚至达数十分钟，要求月球车具有自主位姿确定能力。因而，需要根据月球无地磁、慢自旋、粉尘污染的环境特点提出新的位姿自主确定方法，以满定日益增长的月球巡视探测任务的需要。

现有的月球与火星车的自主位姿确定方法尚未形成，美国火星车自主位姿确定利用了火星GPS系统，目前尚未见到相关专利与文献对位姿方法进行系统分析与证明。美国及日本在月球与火星车的位姿确定方面都有自己的方案，但不是自主式的、系统的绝对位姿确定方法，即不能提供相对月球车经纬度、相对地平指北系的航向及月球车相对地平系的速度。现有的技术方案不能满足对月球制图、月球资源普查等科学任务的需要。

发明内容

本发明的目的在于：为月球车提供适应月球环境，特别是无地磁、慢自旋、粉尘污染环境(除月球车运行过程中产生的粉尘，还有由于高低温环境产生的粉尘环境，该机理尚无科学解释)的月球车自主位姿确定方法。

1.技术方案与系统结构

本发明的技术方案参见系统结构框图1和工作过程图2，本发明是由观测设备、嵌入式PC104+计算机及内置于广角太阳相机1.1的嵌入式TMS320计算机为基础完成的，其中与嵌入式PC104+计算机集成的观测设备有：广角太阳相机1.1、称为IMU1.2的惯性单元、时钟1.3及高速激光雷达1.4；在广角太阳相机1.1内置的嵌入式TMS320计算机中包含的模块有智能太阳识别与处理方法模块2；在PC104+计算机中包括的模块有：通信与任务调度模块10、月球车自主位姿确定任务模块；月球车自主位姿确定任务模块包含：月球垂直陀螺方法模块3、初始位姿校准方法模块4、高精度太阳罗盘导航方法模块5、位姿补偿方法模块6、基于地图重建与即时定位(SLAM)月球车速度估计方法摸块7、具有速度修正的月球车位姿确定方法模块8、基于太阳连测的经纬校准方法模块9。

基于全功能太阳罗盘的月球车位姿自主确定方法步骤如下：

(1)数据获取，由在嵌入式PC104+计算机中开发的通信与任务调度模块完成，它包含太阳罗盘接口任务、IMU接口任务及高速激光雷达系统接口任务；太阳罗盘接口任务通过CAN通信访问智能相机，包括设置智能太阳识别与处理方法模块所需的参数，最终获取智能太阳识别与处理方法模块得到的太阳入射角及方向角

日出时间t_s(0)等；IMU接口任务通过RS232通信访问IMU，获得三轴加速度a_s(k)、三轴角速度ω_s(k)；高速激光雷达系统接口任务通过PCL总线通信访问激光雷达，获得测量的深度h_ij、光强I_ij数据流；

(2)系统初始化，嵌入式PC104+计算机上电后系统初始化程序工作，对智能相机、IMU、激光雷达进行初始设置，并启动嵌入式PC104+计算机中月球车自主位姿确定任务模块及嵌入式TMS320计算机中智能太阳识别与处理方法模块；

(3)嵌入式PC104+计算机中月球车位姿确定任务模块包括：月球垂直陀螺方法模块、智能太阳识别与处理方法模块、初始位姿校准方法模块、高精度太阳罗盘导航方法模块、位姿补偿方法模块、基于地图重建与即时定位(SLAM)月球车速度估计方法模块、具有速度修正的月球车位姿确定方法模块、基于太阳连测的经纬校准方法模块，各处理方法之间并行运行，共同完成月球车绝对位姿、相对位姿、平动速度确定；在广角太阳相机内置的嵌入式系统TMS320计算机中包含的模块有智能太阳识别与处理方法模块；

下面对各方法模块实现方法及流程进行描述：

由智能太阳识别与处理方法模块识别日出或太阳并判断太阳升起时间t_s0及太阳入射角θ_(k)、太阳方向角

该模块首先应对广角镜头进行校准，然后拍摄天空图像，提取太阳圆盘，按圆度、面积、顔色范围来判别由图像提取得到的简单曲线是否为太阳；并计算太阳圆盘中心，根据计算太阳圆盘中心像点坐标求太阳入射角θ_(k)及太阳方向角

由月球垂直陀螺方法模块由系统获得的月球车三轴加速度a_(k)、角速度ω_(k)为输入，经加速度a_(k)反馈校正四元数，进行四元数计算迭代得月球车俯仰角α_(k)与横滚角β_(k)；

由太阳入射角θ_(k)及太阳方向角及月球车俯仰角α_(k)与横滚角β_(k)计算太阳高度角Z_(k)及月球车前向与太阳的夹角ξ_(k)；

由初始位姿校准模块根据太阳高度角Z_(k)及月球车前向与太阳的夹角ξ_(k)通过高精度太阳罗盘导航模块及基于太阳连测的经纬校准模块计算月球车经纬度λ_(k)，δ_0(k)及太阳方位角θ_(k)，从而计算月球车航向角Φ_(k)；或者，由日出时按(t_s0-t_m)ω_l计算角度再换算为经度；其中t_m，ω_l分别是Sinus medii中央湾时间、月球自转角速度；

由高精度太阳罗盘导航方法模块应用于月球车初始位姿校准模块及基于太阳连测的经纬校准方法模块；它们的输入都是太阳高度角Z_(k)及观测时间；其中包含了由观测时间计算任一时刻的太阳及月赤道与黄道的升交点经度过程；二者不同在于高精度太阳罗盘导航方法模块是一次观测，基于太阳连测的经纬校准方法模块是多次观测即连测的过程；但后者要为前者提供月球车经度，从而可通过前者连续计算太阳方位角，高精度太阳罗盘导航方法模块以校准的经度及实时测量的太阳高度角Z_(k)及观测时间连续计算太阳方位角，从而获得月球车航向角Φ_(k)；

由基于地图重建与即时定位(SLAM)月球车速度估计方法模块根据测量的三轴加速度a_s(k)、三轴角速度ω_s(k)及由高程图的数据即粒子通过粒子滤波得到月球车的速度V_(k)；

由基于速度修正的月球车位姿确定方法模块输入为月球车速度V_(k)、月球车三轴加速度a_(k)、角速度ω_(k)、月球车初始经纬度λ_(k)，δ_0(k)及月球车航向角Φ_(k)，输出是连续的月球车连续速度V_(k+i)、经纬度λ_(k+i)，δ_0(k+i)及航向角Φ_(k+i)。

所述的智能太阳识别与处理方法模块的任务运行于广角太阳相机嵌入式系统当中；该方法模块完成两部分任务：即，太阳识别及太阳单位矢量计算与日出识别及日出时间计算，该方法模块的输出结果为：日出时间、太阳单位矢量；

参见图3鱼眼镜头成像示意图，所述的广角太阳相机采用鱼目镜头，其摸型为公式

r＝fθ    (1)

其中：OcXcYcZc为相机坐标系，r为图像点p到鱼眼图像中心Oc的距离，f为鱼眼镜头的焦距，θ为入射光线与鱼眼镜头光轴的夹角，即测量高度角，

为像点在坐标系OcXcYcZc的方向角。则对应于入射角及方向角为

的空间点Pt的图像点p在摄像机坐标系下的坐标(x，y)满足：

将(1)带入(2)得

由于镜头加工工艺和其他因素，鱼眼镜头通常很难严格满足成像模型(1)，利用三次多项式来拟合该鱼眼镜头的成像模型，即

r＝κ₁θ+κ₂θ³    (4)

则对应的成像变换为

畸变校正模型按式(6)进行。

成像过程用6个参数来描述鱼眼镜头成像的模型：

通过校准板，拍摄其图像，计算m_u、m_v、u₀、v₀、κ₁、κ₂六个校正参数。其中m_u、m_v为测量高度角与像素坐标关联常数，u₀、v₀为像素坐标偏差，κ₁、κ₂为测量高度角校正常数；

太阳方向计算根据鱼眼图像中某点的像素坐标(u，v)，求该点在摄像机坐标系下的入射角及方向角

其中

可以利用根据像素坐标直接求解出来；对于θ，将m_u、m_v、u₀、v₀、κ₁、κ₂和

带入式(7)得到关于θ的一元三次方程，求解后去除不合理的解到太阳入射角θ。相应k时刻的太阳入射角及太阳方向角分别记为

太阳识别及太阳单位矢量计算的方法如图4左所示：首先获取天空图像；进行滤噪，去除小颗粒粉尘图像；进行轮廓提取，并进行简单曲线几何参数计算，包括面积，质心，圆度计算；同时计算太阳平均色彩；根据太阳圆度、面积、色彩范围检测太阳；如果判断是太阳则计算太阳圆盘中心；根据太阳罗盘光学参数计算相对太阳罗盘相平面的单位矢量；

日出时间计算判别过程如图4左所右，首先获取天空图像；滤噪，去除小颗粒粉尘图像；进行轮廓提取，并进行简单曲线几何参数计算，包括面积，质心，圆度计算；同时计算平均色彩；根据太阳弧段参数、色彩范围检测太阳；若是太阳则估计太阳圆盘中心；由太阳罗盘光学参数计算相对太阳罗盘相平面的单位矢量；根据日出过程的数据记录求解日出时间得月球车所在经度。

所述的月球垂直陀螺方法模块方法中需适应月球重力常数、月球半径等环境等点，通过重力反馈进行四元数修正的方法实现月球垂直陀螺的俯仰角与滚动角解算；

根据捷联导航方程即式(8-9)

${\stackrel{\·}{q}}_{\left(k\right)}=\frac{1}{2}{q}_{\left(k\right)}\⊕{\mathrm{\ω}}_{\left(k\right)}---\left(8\right)$

其中q_(k)＝{λ₀，λ₁，λ₂，λ₃}^T，是四元数表示月球车姿态：

${\underset{\‾}{Q}}_{\left(k\right)}=\left[\begin{array}{ccc}2({\mathrm{\λ}}_{0\left(k\right)}^2+{\mathrm{\λ}}_{1\left(k\right)}^2)-1& 2({\mathrm{\λ}}_{1\left(k\right)}{\mathrm{\λ}}_{2\left(k\right)}-{\mathrm{\λ}}_{0\left(k\right)}{\mathrm{\λ}}_{3\left(k\right)})& 2({\mathrm{\λ}}_{1\left(k\right)}{\mathrm{\λ}}_{3\left(k\right)}+{\mathrm{\λ}}_{0\left(k\right)}{\mathrm{\λ}}_{2\left(k\right)})\\ 2({\mathrm{\λ}}_{1\left(k\right)}{\mathrm{\λ}}_{2\left(k\right)}+{\mathrm{\λ}}_{0\left(k\right)}{\mathrm{\λ}}_{3\left(k\right)})& 2({\mathrm{\λ}}_{0\left(k\right)}^2+{\mathrm{\λ}}_{2\left(k\right)}^2)-1& 2({\mathrm{\λ}}_{2\left(k\right)}{\mathrm{\λ}}_{3\left(k\right)}+{\mathrm{\λ}}_{0\left(k\right)}{\mathrm{\λ}}_{1\left(k\right)})\\ 2({\mathrm{\λ}}_{1\left(k\right)}{\mathrm{\λ}}_{3\left(k\right)}-{\mathrm{\λ}}_{0\left(k\right)}{\mathrm{\λ}}_{2\left(k\right)})& 2({\mathrm{\λ}}_{2\left(k\right)}{\mathrm{\λ}}_{3\left(k\right)}+{\mathrm{\λ}}_{0\left(k\right)}{\mathrm{\λ}}_{1\left(k\right)})& 2({\mathrm{\λ}}_{0\left(k\right)}^2+{\mathrm{\λ}}_{3\left(k\right)}^2)-1\end{array}\right]---\left(9\right)$

运用基于加速度反馈的捷联导航四元数迭代的修正方程

a_(k)＝Q_(k)a_s(k)，Δω_(k)＝a_(k)×g/||g_m||²    (10)

ω_(k)＝ω_s(k)+kQΔω_(k)                      (11)

解算探测月球车相对地平坐标系的俯仰角与横滚角，由三轴加速度a_s(k)及角速度ω_s(k)，通过式(9-11)计算

，得俯仰角α_(k)与横滚角β_(k)；尽管式(8-9)是已有的公式，但与式(10-11)构成了捷联式的垂直陀螺系统方程。

所述的月球车初始位姿校准方法模块工作流程如图5左所示。

前述的月球车初始位姿校准分两种：一种是通过观测太阳，确定日出时间t_s(0)来计算月球车初始经度λ_(k)，从而利用纬度及太阳方位角方程解算月球车纬度λ_(k)及太阳方位角ψ_(k)。利用垂直陀螺输出的俯仰角α_(k)与横滚角β_(k)、太阳相机获得的太阳方向角及太阳与月球车前向夹角即太阳方向角

及太阳相机安装姿态计算太阳在自由地平系下的太阳高度角Z_(k)及太阳相对月球车前向的角度ξ_(k)，从而计算出月球车航向角θ_(k)，该过程参见高精度太阳罗盘导航模块5。另一校准方法是对太阳连续观测解算月球车经度λ_(k)，从而利用太阳罗盘原理得到月球车航向角θ_(k)。该过程参见基于太阳连测的经纬校准方法模块9。

所述的高精度太阳罗盘导航方法模块是：依据日-地初始经度运用日地自转及公转速度及时间计算们任一时刻的经纬度，根据球面三角建立太阳罗盘导航方程，进行基于太阳高度及方向的解唯一性判定，以此保证该太阳罗盘导航的高精度。

高精度太阳罗盘导航方法流程框图如图6所示。该方法包括：日-地在月球体系下的经度及升交点经度计算、太阳高度角与方向角获取、基于球面天文学的太阳罗盘导航方程求解、月球车航向计算。由于月球车经度由太阳连续观测或日出校准得到，在时间上有一定的滞后，但由于短时间内月球车经纬度变化很小，故对太阳方位角计算精度影响极小。

下面对高精度太阳罗盘原理进行说明。如图7所示，月球体系定义：原点为月球质量中心Q_M，X轴经过Sinus medii环形坑即中央湾，Z轴为月球自旋轴，Y轴由右手系确定，基中Z轴正向为北天极，Z轴负向为南天极；月球赤道(简称月赤道)与黄道交角记为ε；太阳罗盘观测的离散时刻记为k，其O点为t_m时刻；在t_m时刻的太阳及升交点经度记为t_S0，t_γ0；太阳在月球体系下的经度与纬度分别记为t_(k)，δ_(k)；月球探测月球车(观测)位置记为t_0(k)，δ_0(k)；太阳入射角δ_(k)，太阳方向角

太阳相对月球车前向的角度为ξ_(k)，亦称太阳方向角；太阳在当地地平系下与自转北向的夹角即太阳方向角记为ψ_(k)；由太阳入射角θ_(k)，太阳方向角

及月球车俯仰角α_(k)与横滚角β_(k)计算太阳高度角Z_(k)及月球车前向与太阳的夹角ξ_(k)。

高精度太阳罗盘导航方法包括：太阳在月球体系下的经度及升交点经度计算方法、太阳罗盘导航方程。

所述的日-地在月球体系下的经度及升交点经度计算是由在t_m时刻的太阳及升交点经度t_S0，t_γ0，计算相对t_m观测离散时刻k的在月球体系下的经度t及升交点经度t_γ，它们随时间作周期性变化；通常是根据儒略日及J2000的日-地-月的轨道根数计算太阳经纬度、黄白升交点经度、等，但过程繁杂，不适合月球车嵌入式系统的计算。故将各t_m时刻的太阳及升交点经度t_S0，t_γ0序列事先根据天体动力学计算并保存于嵌入式计算机中，该计算过程具有非常高的精度且计算量较小。

记地球自转角速度ω_e、地球绕日公转角速度ω_E、月球自转角速度ω_l、月球绕地公转角速度ω_L、月赤道与黄道交角ε、地赤道与黄道交角ε_e、地赤道与月赤道交角ε_le；在短时间内将月-地视为匀速转动来计算短期内任一观测时刻k的太阳经纬度t_γ(k)，δ_(k)及黄白升交点经度t_γ(k)。

所述的高精度太阳罗盘导航方法是：根据事先计算的定期的日-地-月星历数据保存于月球车嵌入式系统中，根据式下述公式(1215)完成适合月球车嵌入式系统进行任一时刻的太阳及月赤道与黄道的升交点经度；

$f(C,a)=\left\{\begin{array}{ccc}\mathrm{ar}\tan \left(\cos C\tan a\right)+\mathrm{\π}/2& \mathrm{if}& C\∈[\mathrm{i\π}-\mathrm{\π}/2,\mathrm{i\π}+\mathrm{\π}/2),i=\mathrm{1,2}...\\ -\mathrm{ar}\tan \left(\cos C\tan a\right)-\mathrm{\π}/2& \mathrm{if}& C\∈[\mathrm{i\π}+\mathrm{\π}/2,(i+1)\mathrm{\π}+\mathrm{\π}/2),i=\mathrm{1,2}...\end{array}\right.---\left(12\right)$

t_γ(k)＝t_s0+f(ω_ek，ε_el)+f(ω_lk，ε_l)                                   (13)

t_(k)＝t_γ0+f(ω_ek，ε_el)+f(ω_lk，ε_l)+f(f(ω_Ek，ε_e)，ε_l)+f(ω_Lk，ε_l)  (14)

δ_(k)＝Ar tan(sin(t_(k)-t_γ(k))tanε)                                     (15)

其中：t_s0-太阳经度、t_r0-白黄升交点经度、ω_e-地球自转角速度、ω_E-地球绕日公转角速度、ω_l-月球自转角速度、ω_L-月球绕地公转角速度、ε_l-白道与黄道交角、ε_e-地赤道与黄道交角、ε_le-地赤道与月赤道交角；t_(k)，δ_(k)-太阳经纬度、t_γ(k)-黄白升交点经度，k月球零经度离散的标准时；

然后由计算的任一时刻的太阳经度t_(k)及月赤道与黄道的升交点经度t_γ(k)运用公式(16-19)即

${\mathrm{\λ}}_{(k+2)}=\mathrm{ar}\tan \left(\frac{(C_{t(k+1)}-C_{t(k+2)})(C_{Z(k+2)}-C_{Z(k+3)})-(C_{t(k+2)}-C_{t(k+3)})(C_{Z(k+1)}-C_{Z(k+2)})}{(S_{t(k+1)}-S_{t(k+2)})(C_{Z(k+2)}-C_{Z(k+3)})-(S_{t(k+2)}-S_{t(k+3)})(C_{Z(k+1)}-C_{Z(k+2)})}\right)---\left(16\right)$

${\mathrm{\δ}}_{0\left(k\right)}=2\mathrm{ar}\tan \left(\frac{C_{\mathrm{\δ}\left(k\right)}\±\sqrt{S_{Z\left(k\right)}^2-S_{\mathrm{\δ}\left(k\right)}^2{S}_{(t\left(k\right)-t_{0\left(k\right)})}^2}}{C_{Z\left(k\right)}-S_{\mathrm{\δ}\left(k\right)}{C}_{(t\left(k\right)-t_{0\left(k\right)})}}\right)---\left(17\right)$

$\mathrm{\ψ}\left(k\right)=\mathrm{ar}\tan \left(\frac{C_{{\mathrm{\δ}}_{0\left(k\right)}}{S}_{\mathrm{\δ}\left(k\right)}{S}_{(t\left(k\right)-t_{0\left(k\right)})}}{C_{Z\left(k\right)}{S}_{{\mathrm{\δ}}_{0\left(k\right)}}-C_{\mathrm{\δ}\left(k\right)}}\right)---\left(18\right)$

月球车航向角计算公式

Φ_(k)＝ψ_(k)-ξ_(k)    (19)

计算月球车的经纬度λ_(k)，δ_0(k)及月球车航向角θ_(k)。

式(16-18)是以太阳连测的太阳纬度变化极小即δ(k+1)≈δ(k+2)≈δ(k+3)为前提，根据球面三角导航方程即

$C_Z=S_{\mathrm{\δ}}{S}_{{\mathrm{\δ}}_0}+C_{\mathrm{\δ}}{C}_{{\mathrm{\δ}}_0}{C}_{(t-t_{\text{0}})}$

$S_Z{S}_{\mathrm{\ψ}}=C_{\mathrm{\δ}}{S}_{(t-t_0)}$

$S_Z{C}_{\mathrm{\ψ}}=-S_{\mathrm{\δ}}{C}_{{\mathrm{\δ}}_0}+C_{\mathrm{\δ}}{S}_{{\mathrm{\δ}}_0}{C}_{(t-t_0)}$

推理得到的。

所述的太阳罗盘位姿补偿方法是：依据现有天文知识，对太阳罗盘经纬度进行大气折射补偿、绕日运动补偿、光行差补偿、周日视差补偿、观测视差补偿、月球及其被绕行星的岁差及章动补偿，从而保证高精度太阳罗盘导航方法的高精度。

所述的基于地图重建与即时定位(SLAM)月球车速度估计方法是：利用高程图及其对应的障碍图数据特征通过粒子滤波技术实现月球车速度估计，以保证月球车速度估计的收敛性与可靠性；

由垂直陀螺系统提供俯仰角与滚动角，由激光雷达测量环境深度图，经坐标系变换得到高程图，按式(20)求ij网格点求四向平均梯度从而星到障碍图，用于地图重建与即时定位(SLAM)使用，即为SLAM提供粒子；

$G_{\mathrm{ij}}=\frac{1}{l}\underset{k=1}{\overset{l}{\mathrm{\Σ}}}\left(\right|h_{(i-k)j}-h_{\mathrm{ij}}|+|h_{(i+k)j}-h_{\mathrm{ij}}|+|h_{i(j-k)}-h_{\mathrm{ij}}|+|h_{i(j+k)}-h_{\mathrm{ij}}\left|\right)/\left(4k\right)>G_0---\left(20\right)$

其中i＞k，j＞k，i，j，k为整数，h_ij为高度，G_ij为ij网格点的梯度，G₀为设置的阈值；经四向梯度障碍提取得到障碍图。建立地图重建与即时定位(SLAM)的导航方程，利用粒子滤波实现环境数据与航位推算过程的融合。激光雷达精度很高，达1-4mm。由于粒子数的限制，只能利用月球车当前环境样本进行滤波，故利用障碍距离修正航位推算过程能够使系统稳定，但仅能保证月球车平动速度具有较高的精度，而相对位置在长时间运行之后会出现较大的误差累积，故仍需要外绝对位置校准。基于地图重建与即时定位(SLAM)月球车速度估计方法如图9所示。过程算法流程如图10所示，该过程由式(21-24)分7步完成：

(1)由惯性单元(加速度计)在地平面下的加速度(a_S(k)，a_N(k))及月球车航向角θ_(k)，经二次积分得到该单步过程的位置增量ΔX_k，其中ΔX_k＝(x_k，y_k)即是月球车的相对位置；

(2)结合上一步最优位姿估计值X_k-1 ⁺给出当前的先验位姿估计X_k ^-

$X_k^{-}=X_{k-1}^{+}+\mathrm{\Δ}{X}_k---\left(21\right)$

(3)假定过程b的误差模型服从某一分布(如高斯分布)，由此确定粒子采样所依据的建议分布，并进行粒子采样ⁱx_k ^-～N(X_k ^-，σ)(σ为事先估计的噪声分布方差)；

(4)读取当前激光雷达的环境测量数据z_k，并与之前所创建的地图特征

配准。配准过程采用ICP(Iterated Closed Points)算法。这样对于每个粒子得到一个最佳的位姿估计：

$x_k^{+}{}_i=\arg \max \{p\left(z_k\right|x_k^{-}{}_i,{\hat{m}}_{k-1})\·p\left(x_k^{-}{}_i\right|z_{1:k-1},\mathrm{\Δ}{X}_{0:k})\}---\left(22\right)$

在上述配准过程中，同时对环境观测量和地图之间的似然程度进行评价，据此计算每个粒子的重要性权值系数w_k ⁱ，并进行规一化处理 ${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{w}_k^i.$

(5)由此计算等效粒子数 $N_{\mathrm{eff}}=1/\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\tilde{w}}_k^i.$ 若该值小于设定阈值N_eff＜T，重新对粒子群采样。

(6)月球车位姿状态的最优值估计应为所有粒子的加权和：

$X_k^{+}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\tilde{w}}_k^{i}x_k^{+}{}_i---\left(23\right)$

(7)加入当前的环境观测数据，对地图进行更新：

${\hat{m}}_k={\hat{m}}_{k-1}\∪(X_k^{+}\⊕z_k)---\left(24\right)$

所述的基于太阳连测的经纬校准方法是：以高精度太阳罗盘导航方法为基础，通过基于地图重建与即时定位(SLAM)月球车速度估计提供的月球车速度进行航位计算，并利用高精度太阳罗盘导航方法实时地计算出任一时刻的月球车经纬度及航向，再根据太阳罗盘的太阳方位角方程计算太阳方位，该方法参见图11所示。它是通过多次测量(至少3次)按式下面公式(25-27)迭代计算，最后按式(27)计算月球车的经纬度及太阳方位角，其前提是连测时月球车保持静止；

${\mathrm{\λ}}_{(k+2)}=\mathrm{ar}\tan \left(\frac{(C_{t(k+1)}-C_{t(k+2)})(C_{Z(k+2)}-C_{Z(k+3)})-(C_{t(k+2)}-C_{t(k+3)})(C_{Z(k+1)}-C_{Z(k+2)})}{(S_{t(k+1)}-S_{t(k+2)})(C_{Z(k+2)}-C_{Z(k+3)})-(S_{t(k+2)}-S_{t(k+3)})(C_{Z(k+1)}-C_{Z(k+2)})}\right)---\left(25\right)$

${\mathrm{\δ}}_{0\left(k\right)}=2\mathrm{ar}\tan \left(\frac{C_{\mathrm{\δ}\left(k\right)}\±\sqrt{S_{Z\left(k\right)}^2-S_{\mathrm{\δ}\left(k\right)}^2{S}_{(t\left(k\right)-t_{0\left(k\right)})}^2}}{C_{Z\left(k\right)}-S_{\mathrm{\δ}\left(k\right)}{C}_{(t\left(k\right)-t_{0\left(k\right)})}}\right)---\left(26\right)$

$\mathrm{\ψ}\left(k\right)=\mathrm{ar}\tan \left(\frac{C_{{\mathrm{\δ}}_{0\left(k\right)}}{S}_{\mathrm{\δ}\left(k\right)}{S}_{(t\left(k\right)-t_{0\left(k\right)})}}{C_{Z\left(k\right)}{S}_{{\mathrm{\δ}}_{0\left(k\right)}}-C_{\mathrm{\δ}\left(k\right)}}\right)---\left(27\right)$

记连续观测太阳N次，0≤k≤N-2，得到N-1次测量结果。，对它们分别求平均得最终的测量结果

$\mathrm{\λ}=\underset{0}{\overset{N-2}{\mathrm{\Σ}}}{\mathrm{\λ}}_{(k+2)}/(N-1),{\mathrm{\δ}}_0=\underset{0}{\overset{N-2}{\mathrm{\Σ}}}{\mathrm{\δ}}_{0(k+2)}/(N-1),\mathrm{\ψ}=\underset{0}{\overset{N-2}{\mathrm{\Σ}}}{\mathrm{\ψ}}_{(k+2)}/(N-1)---\left(27\right)$

所述的基于太阳连测的经纬校准方法是建立太阳连测导航方程，通过太阳连测，并进行月球车经纬度的更新计算，提高了经纬度计算精度，同时也保证了嵌入式计算的实时性，确保在线位姿的准确性。

基于速度修正的月球车位姿确定方法是：在高精度太阳罗盘导航方法的基础上，通过基于地图重建与即时定位(SLAM)月球车速度估计提供的月球车速度进行航位计算，并利用高精度太阳罗盘导航方法实时地计算出任一时刻的月球车经纬度及航向，并根据太阳罗盘的太阳方位角方程计算太阳方位。由于在航位推算过程中月球车航向是根据更新的经纬确定的，保证了月球车航向的高精度，从而大大提高了经纬度的计算精度。该方法参见图12所示。该方法利用了成熟的卡尔曼滤波技术实现月球车速度估计、高精度太阳罗盘位姿确定及月球车捷联系统的融合。

本发明与现有太阳罗盘及相关技术相比具有突出的本质区别：

现有的月球车位姿主要通过星敏感器利用位置圆原理进行确定，其缺点是：

(1)星敏感器对工作环境清洁度要求极高，在月球表面，极易受粉尘污染，致使寿命很短；

(2)星敏感器造价高，重量相对太阳敏感器要高得多，从而大大增加了制造及发射成本；

(3)星敏感器工作时要求背对太阳，要进行位置确定还需转动以观测不同的恒星，因此在月球车使用星敏感器进行位姿确定就要安装两个自由度的高精密云台，从而保证星敏感器背着太阳并保证星敏感器的测量精度。这样星敏感器在月球车上应用就遇到了很大困难，一方面增加了制造与发射成本，同时也大大降低了可靠性。

美国火星车使用了太阳敏感器，但是火星有较强的地磁、自转周期较小，故通过磁罗盘及捷联平台就能完成位姿确定功能。火星车上的太阳敏感器要求已知火星车的大概经纬度，才能确定探测车的方向。

本发明的主要特点是：

(1)适应月球重力环境；(2)适应月球慢自旋环境：(3)适应月球粉尘环境；(4)适应月球无地磁环境；(5)能够同时完成较高精度的经纬度及航向确定功能：(6)系统质量较低，有利于降低月球车整体质量及研制与运营成本；(7)高可靠性，可以满足长时间的位姿自主确定需求。

附图说明

图1基于全功能太阳罗盘的月球车位姿自主确定系统结构框图；标号如下：

观测设备1、广角太阳相机1.1、惯性单元IMU 1.2、时钟1.3、及高速激光雷达1.4，智能太阳识别与处理方法模块2、月球垂直陀螺方法模块3、初始位姿校准方法模块4、高精度太阳罗盘导航方法模块5、位姿补偿方法模块6、基于地图重建与即时定位(SLAM)月球车速度估计方法模块7、具有速度修正的月球车位姿确定方法模块8、基于太阳连测的经纬校准方法模块9、通信与任务调度模块10；

图2基于全功能太阳罗盘的月球车位姿自主确定方法工作过程图；

图3鱼眼镜头成像示意图；

图4基于全功能太阳罗盘的月球车位姿自主确定方法流程图；

图5月球车位姿校准模式及垂直陀螺工作方法流程图；

图6高精度太阳罗盘导航方法流程图；

图2-6符号如下：

表示任务；

表示数据输入；

表示数据保存至内存；

表示函数；

表示条件判断；

表示任务或程度终结；

图7基于球面天文学的太阳罗盘原理图；

符号表示如下：

月球体系O_M-XYZ定义：原点为月球质量中心O_M，X轴经过Sinus medii环形坑，Z轴为月球自旋轴，Y轴由右手系确定，基中Z轴正向为北天极，Z轴负向为南天极；

Sinus medii-是月球正面的环形坑，即中央湾，被国际天文联合会定义为月球体系的0经度经的地方；

N_p-月球自转轴北向，即月球北天极；

S_p-月球自转轴南向，即月球南天极；

S_n-太阳中心；

t_n-太阳所在纬度线与月赤道在无穷远处的交点；

E-月球车所有位置的东向；

W-月球车所有位置的西向；

γ₀-升交点，即月赤道与黄道在无穷远处的交点；

t_m-是月球车系统计算时刻，是系统计算太阳经度、升交点经度的参考时刻，它与系统采用北京地方时一致；

Ut_m-是月球车系统计算时刻对应的格林威治时间，即国际标准时；

Z_r-月球车所在位置天的方向，即天顶，与重力方向基本一致，但方向指向天空；

δ-O_MS_n与月赤道的夹角，它随时间按一定规律变化，可用球面三角公式计算；

t_S0-在t_m时刻的太阳经度，是太阳在月赤道投影与月球体系x轴的夹角；

t_γ0-在t_m时刻的升交点经度，是S_nγ₀与x轴的夹角；

ε-白道(月球赤道)与黄道交角记为ε，是白道面与黄道面法向构成的交角；

t-O_Mt_n与x轴的交角；

δ-太阳在月球体系下的纬度，月球体系原点O_M与太阳构成的矢量与月赤道的夹角；

t₀-月球车(观测)经度；

δ₀-月球车(观测)纬度；

Z_r-太阳罗盘观测的高度角，即月球体系原点O_M与太阳构成的矢量与太阳太阳罗盘所在位置的天顶所张的角度；

t_r-太阳罗盘观测太阳的观测方向角，即O_MS_n在太阳罗盘所在位置的自由地平系投影与该系x轴所张的角；

ψ-太阳方向角，即O_MS_n在太阳罗盘所在位置的地平指系投影与北向所张的角；

图8基于球面天文学的太阳罗盘导航方程显式求解可视化结果示意图；

(a)观测纬度解算结果

(b)太阳方位算结果；

图9基于地图重建与即时定位(SLAM)的月球车速度估计模块流程图；

符号如下：

表示任务；

表示数据输入；

表示数据保存至内存；

表示函数；

表示条件判断；表示任务或程度终结；

图10具有速度修正的月球车位姿确定算法流程图

X表示月球车位姿(包括平面上的二维位置坐标和航向角)，下标″k″表示第k个单步，上标″-″表示先验估计，″+″表示后验估计；

ⁱx_k表示当前粒子群中某个粒子i所携带的位姿，w_k ⁱ表示每个粒子i的重要性权值，

表示规一化后的权值；

z_k表示当前的环境观测量；

表示当前地图的状态估计；

n为粒子群中包含的粒子个数；

图11基于太阳连测的经纬校准方法流程图；

图12；具有速度修正的月球车位姿确定方法流程图

图11-12符号如下：

表示任务；

表示数据输入；

表示数据保存至内存；

表示函数表示条件判断；

表示任务或程度终结；

其它符号说明如下：

°/deg-角度单位，度；′-角度单位-分；″-角度单位-秒；

Hr-时间单位，小时；min-时间单位，分；s-时间单位，秒；ms-时间单位，毫秒；

g_l-月球重力加速度，1.62m/s²；m-长度单位，米；mm-长度单位，毫米；

图13蝴蝶、蜣螂利用偏振光进行导航及它们的眼睛结构示意图；

图14基于全功能太阳罗盘的位姿自主确定与SLAM的联合定位仿真1；

图15基于全功能太阳罗盘的位姿自主确定与SLAM的联合定位仿真2。

本发明技术方案的技术思路来源

(1)来源于生物位姿确定的仿生原理如图12所示，蝴蝶、蜣螂利用偏振光进行导航。发明人发现借助日光或月光导航的生物的习性如下：a.在春分及秋分时期十分活跃，猜测它们位姿确定在该时期应更准确，否则它们应该感到紧张或行为变得不活跃。b.它们在清晨、傍晚时十分活跃，猜测此时它们的位姿确定也很准确的。发明人提出的高精度太阳罗盘导航方法的分析结果也证明了现有的猜测。图8仿真结果表明，在夏至及冬至时期的清晨与傍晚时段航向确定不准确或不能确定的，正午航向确定是不精确的或不可能的，此时必需借助其它位姿确定方法。该季节许多依据太阳导航的生物在清晨、傍晚及正午也是不活跃的。在春分与秋分至附近时附件在清晨、傍晚时经纬度确定及航向确定都是精确的。而在此季节依据太阳导航的生物在清晨、傍晚时是非常活跃的，而仅在正午经纬度确定是不精确的，此时依据太阳导航的生物行为也是不活跃的。本发明为解释依据太阳导航的生物行为提供了理论基础。同时，借助日光或月光导航的生物一般以日光或月光进行粗略的定位与精确的定向，而精确的定位是以视景图像为基础的。

(2)本发明思路也来源于手机及古地磁探测采用的太阳罗盘定向原理。其过程是：使用者将手机或古地磁太阳罗盘的指示太阳或0度线与真实太阳对准，并将手机或古地磁太阳罗盘保持水平，从而用太阳罗盘指出北向角度。它是利用球面三角建立的太阳方位方程进行计算的。但是使用者必须为它提供经纬度，或以告知所在城市名称的形式来提供，本质上提供了近似的经纬度。它的缺点是：使用者要提供经纬度及水平位置，同时北向精度确定很差，只能用于粗略的北向确定，而远远不能满足月球车航向确定需求。

具体实施方式

本发明中所选用的部分方法模块的技术构成及技术指标说明如下：

1、观测设备由广角太阳相机、IMU组成、高速激光雷达及时钟组成。

广角太阳相机：视场角为185°；分辩率1024×1098；采用CAN通信方式。在该相机中专门开发了智能太阳识别与处理系统，用于粉尘过滤、太阳识别并计算单位矢量、识别日出并输出日出时间；

IMU：加速度计范围±5g；加速度计线性度1.5％；光纤陀螺：游走

光纤陀螺速度范围100°/s，光纤陀螺线性度1.5％；通信方式采用RS232。输出：三轴加速度、三轴角速度、系统温度；

高速激光雷达：深度检测范围16m；速率5万点/秒；视场120°；具有两个自由度，通过高速激光梭镜方向伺服控制，实现对三维空间进行高速扫描测量。通信方式采用PCI总线。输出：被测点的深度值、激光强度、雷达系统温度、行场位置编码；输出数据经光强及温度补偿后得到高精度深度值。并利用月球车俯仰角、横滚角、雷达行场编码建立高精度环境高程图。

时钟：由嵌入式计算机根据星历预报日出时间，在日出前一日进行时钟设置。在日出前30分钟产生时钟中断，从而启动太阳相机中的日出识别及处理任务。

2、垂直陀螺是在IMU(三轴微机械加速度计、三轴光纤陀螺)基础上进行二次开发而成，即完成垂直陀螺的俯仰角、滚动角解算。采用光纤陀螺主要是因为：质量轻，抗冲击性能好，同时具有较高的检测精度，适应月球车工作环境的需要。应用CrossBow垂直陀螺的三轴加速度与角速度解算的俯仰角与滚动角与CrossBow垂直陀螺输出结果基本一致。加速度小于0.2g时，俯仰角与滚动角误差在5′以下。月球垂直陀螺参数与地球上的垂直陀螺不同，主要表现于：重力常数，同时应考虑月球重力异常(重力瘤)、月球半径、重力反馈四元数系数等。因而现有的地球垂直陀螺不能直接应用于月球车，必需了解垂直陀螺原理才能开发月球垂直陀螺。

本发明的主要实施步骤如下：

步骤1系统构建根据月球车自主位姿确定指标选择适合的IMU，其中IMU的光纤陀螺线性度好于2％、游走

加速度线性优于1％，检测精度优于10^-4g。激光雷达测距精度优于3mm，速度优于4万点/秒。相机分辨率优于1024×1024，相机嵌入式系统计算速度高于3200MIPS；广角镜头形变率低于0.2％，镜头模型为r＝fθ，视场可根据需要选择；开发相应接口程度。从而可以获得月球车三轴加速度a_(k)、角速度ω_(k)。

步骤2智能太阳识别与处理模块开发。首先对广角镜头进行校准，通过拍摄校正板图案并按校正板图案像点位置参照式(7)进行，求解m_u，m_v，k₁，k₂，u₀，v₀。开发太阳检测程序，提取太阳圆盘，按圆度、面积、顔色范围来判别由图像提取得到的简单曲线是否为太阳。并计算太阳圆盘中心，根据计算太阳圆盘中心像点坐标参照式(7)求太阳入射角θ，太阳方向角

由太阳圆盘中心像点坐标直接计算。相应k时刻的太阳入射角及太阳方向角分别记为

步骤3月球垂直陀螺方法模块开发。建立捷联系统方程(8-9)，以获取月球车三轴加速度a_(k)、角速度ω_(k)为输入，并按(10)计算加速度反馈用的角速度，加入式(11)中进行四元数计算迭代得月球车俯仰角α_(k)与横滚角β_(k)。

步骤4由步骤2得到的太阳入射角θ_(k)，太阳方向角

及步骤3得到的月球车俯仰角α_(k)与横滚角β_(k)计算太阳高度角Z_(k)及月球车前向与太阳的夹角ξ_(k)。

步骤5月球车初始位姿校准模块开发。由智能太阳识别与处理模块识别太阳并判断太阳升起时间t_s0，由(t_s0-t_m)ω_l计算角度再换算为经度。其中t_m，ω_l分别是Sinus medii中央湾时间、月球自转角速度。或由太阳连测的经纬校准方法进行经度测量。该方法参考式(24-27)及太阳连测的经纬校准方法一节，即将步骤4得到的太阳高度角Z_(k)代入公式(24-27)求得λ_(k)，δ_0(k)，ψ_(k)。由式(19)计算月球车航向角Φ_(k)。

步骤6基于地图重建与即时定位(SLAM)月球车速度估计模块开发。获取环境深度图并投影至水平面。根据式(20)求场景中每像点的四向平均梯度，并判断是否大于阈值，若成立则视为障碍点，于是得环境障碍图。由步骤5得到的Φ_(k)及步骤1得到的月球车三轴加速度a_(k)、角速度ω_(k)进行航位推算，得月球车单步位移增量ΔX_k，将之及激光测得深度值ⁱx_k ^-的代入式(21-24)进行粒子滤波，获得月球车速度V_(k)。

步骤7基于速度修正的月球车位姿确定模块开发首先建立捷联平台及具有车体速度观测的状态方程捷联导航方程进行迭代计算，同时加入式(10)即垂直陀螺产生的校正角速度。由输入即月球车速度V_(k)、月球车三轴加速度a_(k)、角速度ω_(k)、月球车初始经纬度λ_(k)，δ_0(k)及月球车方向Φ_(k)，对捷联导航方程进行四元数迭代求解。从而得连续的月球车连续速度V_(k+i)、经纬度λ_(k+i)，δ_0(k+i)及航向角θ_(k+i)。

步骤8月球车连续运行一段时间(根据实验测的误差范围来确定)后，重复步骤2-6，即完成再次太阳连测校准后进行月球车位姿自主确定。

本发明的实施过程及结果已由工程实验得到验证。具体如下：

应用例1应用发明人研制的基于全功能太阳罗盘的位姿确定方法，对太阳进行了观测。

测试环境与条件：时间2008.6.6全天；地点北京工业大学综合楼前广场；天气晴；天空有少量云；风4级。测试设备：发明人开发的太阳罗盘+垂直陀螺VG700、校准设备：

GPS，定位精度4米，方向精度0.15°。

测试人员：王亮、居鹤华(北京工业大学电控学院教师)

测试结果：参见表1

表1测试结果

测试结论：由上面的测试结果可知，证明了本发明正确性。尽管与GPS测量结果对比有一定误差，但自主位姿确定功能是可靠的，并且不依赖地磁、不需检测地球转动并且对粉尘污染影响很小。主要误差原因是：太阳相机测量精度较低，同时测量设备没有很好校准，通过提高设备校准精度后，预计经纬及太阳方位角测量精度优于1-5′。

应用例2将本发明应用于月球车导航中，将基于全功能太阳罗盘的位姿自主确定方法与地图重建及即时定位技术相结合用于月球车相对定位。

测试环境与条件：时间2008.6.9-11；地点 北京工业大学综合楼821；在个人计算机上进行仿真分析；仿真中IMU基本参数为：加速度计线性度1.5％，漂移

陀螺：游走

陀螺线性度1.5％；通过测量地形高程精度10mm；

测试人员：王亮、居鹤华(北京工业大学电控学院教师)

测试结果：根据上述测试条件进行基于全功能太阳罗盘的位姿自主确定与SLAM的联合定位仿真，得图14-15所示的仿真结果，其中上图为使用惯性单元的航位推算结果、基于太阳罗盘与垂直陀螺的位姿自主确定与SLAM的联合定位误差曲线，下图为月球车运行轨迹及用于位置校准的样本点集。由图14-15可知，在仅使用IMU进行航位推算时很快就变得发散，而使用基于太阳罗盘与垂直陀螺的位姿自主确定与SLAM的联合定位后，相对位置是收敛的，并具有较高的精度。该实验中，使用了太阳罗盘与垂直陀螺一起确定的月球车航向，即航向可以精确检测，并且不依赖于磁罗盘及惯导平台。

测试结论：仿真结果表明本发明对月球车相对定位精度的提高具有非常重要的意义。表现于：本发明能够适应无地磁、慢自旋、粉尘污染的环境要求，同时航向精度很高，在静态时精度能够提高至太阳观测相机的次像素级，即可达到1-5′左右，而动态精度可达到0.2°。而绝对位置定位精度同样可确定到相同的数量级。

最后应说明的是：以上实施例仅用以说明本发明而并非限制本发明所描述的技术方案；因此，尽管本说明书参照上述的各个实施例对本发明已进行了详细的说明，但是，本领域的普通技术人员应当理解，仍然可以对本发明进行修改或等同替换；而一切不脱离发明的精神和范围的技术方案及其改进，其均应涵盖在本发明的权利要求范围当中。

Claims (5)

1.基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，它是以观测设备、嵌入式PC104+计算机及内置于广角太阳相机(1.1)的嵌入式TMS320计算机为基础完成的，其中与嵌入式PC104+计算机集成的观测设备有：广角太阳相机(1.1)、惯性单元——以下称为IMU(1.2)、时钟(1.3)及高速激光雷达(1.4)；在广角太阳相机(1.1)内置的嵌入式TMS320计算机中包含的模块有智能太阳识别与处理方法模块(2)；在PC104+计算机中包括的模块有：通信与任务调度模块(10)、月球车自主位姿确定任务模块；月球车自主位姿确定任务模块包含：月球垂直陀螺方法模块(3)、初始位姿校准方法模块(4)、高精度太阳罗盘导航方法模块(5)、位姿补偿方法模块(6)、基于地图重建与即时定位月球车速度估计方法模块(7)、具有速度修正的月球车位姿确定方法模块(8)、基于太阳连测的经纬校准方法模块(9)；

基于全功能太阳罗盘的月球车位姿自主确定方法步骤如下：

(1)数据获取，由在嵌入式PC104+计算机中开发的通信与任务调度模块完成，它包含太阳罗盘接口任务、IMU接口任务及高速激光雷达系统接口任务；太阳罗盘接口任务通过CAN通信访问智能相机，包括设置智能太阳识别与处理方法模块所需的参数，最终获取智能太阳识别与处理方法模块得到的太阳入射角及太阳方向角 

日出时间t_s(0)；IMU接口任务通过RS232通信访问IMU，获得三轴加速度a_s(k)、三轴角速度ω_s(k)；高速激光雷达系统接口任务通过PCL总线通信访问激光雷达，获得测量的深度h_ij、光强I_ij数据流；

(2)系统初始化，嵌入式PC104+计算机上电后系统初始化程序工作，对智能相机、IMU、激光雷达进行初始设置，并启动嵌入式PC104+计算机中月球车自主位姿确定任务模块及嵌入式TMS320计算机中智能太阳识别与处理方法模块；

(3)嵌入式PC104+计算机中月球车位姿确定任务模块包括：月球垂直陀螺方法模块、初始位姿校准方法模块、高精度太阳罗盘导航方法模块、位姿补偿方法模块、基于地图重建与即时定位月球车速度估计方法模块、具有速度修正的月球车位姿确定方法模块、基于太阳连测的经纬校准方法模块，各处理方法之间并行运行，共同完成月球车绝对位姿、相对位姿、平动速度确定；在广角太阳相机(1.1)内置的嵌入式系统TMS320计算机中包含的模块有智能太阳识别与处理方法模块(2)；

下面对各方法模块实现方法及流程进行描述：

由智能太阳识别与处理方法模块识别日出或太阳并判断太阳升起时间t_s0及太阳入射 角θ_(k)、太阳方向角 

该模块首先应对广角镜头进行校准，然后拍摄天空图像，提取太阳圆盘，按圆度、面积、颜色范围来判别由图像提取得到的简单曲线是否为太阳；并计算太阳圆盘中心，根据计算太阳圆盘中心像点坐标求太阳入射角θ_(k)及太阳方向角 

由月球垂直陀螺方法模块由系统获得的月球车三轴加速度a_(k)、角速度ω_(k)为输入，经加速度a_(k)反馈校正四元数，进行四元数计算迭代得月球车俯仰角α_(k)与横滚角β_(k)；

由太阳入射角θ_(k)及太阳方向角 

及月球车俯仰角α_(k)与横滚角β_(k)计算太阳高度角Z_(k)及月球车前向与太阳的夹角ξ_(k)；

由初始位姿校准模块根据太阳高度角Z_(k)及月球车前向与太阳的夹角ξ_(k)通过高精度太阳罗盘导航模块及基于太阳连测的经纬校准模块计算月球车经纬度λ_(k)，δ_0(k)及太阳方位角 从而计算月球车航向角Φ_(k)；

由高精度太阳罗盘导航方法模块应用于月球车初始位姿校准模块及基于太阳连测的经纬校准方法模块；它们的输入都是太阳高度角Z_(k)及观测时间；其中包含了由观测时间计算任一时刻的太阳及月赤道与黄道的升交点经度过程；二者不同在于高精度太阳罗盘导航方法模块是一次观测，基于太阳连测的经纬校准方法模块是多次观测即连测的过程；但后者要为前者提供月球车经度，从而可通过前者连续计算太阳方位角，高精度太阳罗盘导航方法模块以校准的经度及实时测量的太阳高度角Z_(k)及观测时间连续计算太阳方位角，从而获得月球车航向角Φ_(k)；

高精度太阳罗盘导航方法是：根据事先计算的定期的日-地-月星历数据保存于月球车嵌入式系统中，根据式下述公式(1-4)完成适合月球车嵌入式系统进行任一时刻的太阳及月赤道与黄道的升交点经度；

t_γ(k)＝t_s0+f(ω_ek，ε_el)+f(ω_lk，ε_l)                                      (2)

t_(k)＝t_γ0+f(ω_ek，ε_el)+f(ω_lk，ε_l)+f(f(ω_Ek，ε_e)，ε_l)+f(ω_Lk，ε_l)  (3)

δ_(k)＝Ar tan(sin(t_(k)-t_γ(k))tanε)                                        (4)

其中：t_s0-太阳经度、t_r0-白黄升交点经度、ω_e-地球自转角速度、ω_E-地球绕日公转角 速度、ω_l-月球自转角速度、ω_L-月球绕地公转角速度、ε_l-白道与黄道交角、ε_e-地赤道与黄道交角、ε_le-地赤道与月赤道交角；t_(k)，δ_(k)-太阳经纬度、t_γ(k)-黄白升交点经度，k月球零经度离散的标准时；

然后由计算的任一时刻的太阳经度t_(k)及月赤道与黄道的升交点经度t_γ(k)运用公式(5-8)即

月球车航向角计算公式

Φ_(k)＝ψ_(k)-ξ_(k)                                   (8)

计算月球车的经纬度λ_(k)，δ_0(k)及月球车航向角θ_(k)；

式(5-7)是以太阳连测的太阳纬度变化极小即δ(k+1)≈δ(k+2)≈δ(k+3)为前提，根据球面三角导航方程即

推理得到的；

基于太阳连测的经纬校准方法是：以高精度太阳罗盘导航方法为基础，通过基于地图重建与即时定位月球车速度估计提供的月球车速度进行航位计算，并利用高精度太阳罗盘导航方法实时地计算出任一时刻的月球车经纬度及航向，再根据太阳罗盘的太阳方位角方程计算太阳方位；它是通过多次测量按式(9-11)迭代计算，最后按式(11)计算月球车的经纬度及太阳方位角，其前提是连测时月球车保持静止； 

记连续观测太阳N次，0≤k≤N-2，得到N-1次测量结果，对它们分别求平均得最终的测量结果

所述的基于太阳连测的经纬校准方法是建立太阳连测导航方程，保证在线位姿确定；

由基于地图重建与即时定位月球车速度估计方法模块根据测量的三轴加速度a_s(k)、三轴角速度ω_s(k)及由高程图及其对应的障碍图数据特征通过粒子滤波得到月球车的速度V_(k)，以保证月球车速度估计的收敛性与可靠性；

由垂直陀螺系统提供俯仰角与滚动角，由激光雷达测量环境深度图，经坐标系变换得到高程图，按式(13)求ij网格点求四向平均梯度从而得到障碍图，用于地图重建与即时定位使用，即为地图重建与即时定位提供粒子；

其中i＞k，j＞k，i，j，k为整数，h_ij为高度，G_ij为ij网格点的梯度，G₀为设置的阈值；经四向梯度障碍提取得到障碍图；建立地图重建与即时定位的导航方程，利用粒子滤波实现环境数据与航位推算过程的融合；该过程由式(14-17)分7步完成；

(1)由惯性单元测量地平系下的加速度a_S(k)，a_N(k)及月球车航向角θ_(k)，经二次积分得到该单步过程的位置增量ΔX_k，其中ΔX_k＝(x_k，y_k)即是月球车的相对位置；

(2)结合上一步最优位姿估计值X_k-1 ⁺给出当前的先验位姿估计X_k ^-

(3)假定过程b的误差模型服从高斯分布，由此确定粒子采样所依据的建议分布，并进行粒子采样ⁱx_k ^-～N(X_k ^-，σ)，其中σ为事先估计的噪声分布方差；

(4)读取当前激光雷达的环境测量数据z_k，并与之前所创建的地图特征 配准；配准过程采用ICP(Iterated Closed Points)算法；这样对于每个粒子得到一个最佳的位姿估计：

在上述配准过程中，同时对环境观测量和地图之间的似然程度进行评价，据此计算每个粒子的重要性权值系数w_k ⁱ，并进行规一化处理

(5)由此计算等效粒子数

若该值小于设定阈值N_eff＜T，重新对粒子群采样；

(6)月球车位姿状态的最优值估计应为所有粒子的加权和：

(7)加入当前的环境观测数据，对地图进行更新：

由基于速度修正的月球车位姿确定方法模块的原理是：在高精度太阳罗盘导航方法的基础上，通过基于地图重建与即时定位月球车速度估计提供的月球车速度进行航位计算，并利用高精度太阳罗盘导航方法实时地计算出任一时刻的月球车经纬度及航向，并根据太阳罗盘的太阳方位角方程计算太阳方位，其输入为月球车速度V_(k)、月球车三轴加速度a_k、角速度ω_k、月球车初始经度λ_(k)、纬度δ_0(k)及月球车航向角Φ_(k)，输出是连续的月球车连续速度V_(k+i)、经纬度λ_(k+i)，δ_0(k+i)及航向角Φ_(k+i)。

2.根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的智能太阳识别与处理方法模块的任务运行于广角太阳相机嵌入式系统当中；该方法模块完成两部分任务：即，太阳识别及太阳单位矢量计算与日出识别及日出时间计算，该方法模块的输出结果为：日出时间、太阳单位矢量；

广角太阳相机采用鱼目镜头，其模型为公式(18) 

r＝fθ                                            (18)

其中：OcXcYcZc为相机坐标系，r为图像点p到鱼眼图像中心Oc的距离，f为鱼眼镜头的焦距，θ为入射光线与鱼眼镜头光轴的夹角，即测量高度角， 

为像点在坐标系OcXcYcZc的方向角；则对应于入射角及方向角为 的空间点P_t的图像点p在摄像机坐标系下的坐标(x，y)满足：

将(18)带入(19)得

由于镜头加工工艺和其他因素，鱼眼镜头通常很难严格满足成像模型(18)，利用三次多项式来拟合该鱼眼镜头的成像模型，即

r＝κ₁θ+κ₂θ³                                   (21)

则对应的成像变换为

畸变校正模型按式(23)进行；

成像过程用6个参数来描述鱼眼镜头成像的模型：

通过校准板，拍摄其图像，计算m_u、m_v、u₀、v₀、κ₁、κ₂六个校正参数，其中m_u、m_v为测量高度角与像素坐标关联常数，u₀、v₀为像素坐标偏差，κ₁、κ₂为测量高度角校正常数；

太阳方向计算根据鱼眼图像中某点的像素坐标(u，v)，求该点在摄像机坐标系下的入射角及方向角 其中 

可以利用根据像素坐标直接求解出来；对于θ，将m_u、m_v、u₀、v₀、κ₁、κ₂和 

带入式(24)得到关于θ的一元三次方程，求解后去除不合理的解到太阳入射角θ；相应k时刻的太阳入射角及太阳方向角分别记为 

3.根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的月球垂直陀螺方法模块方法中需适应月球重力常数、月球半径环境特点，通过重力反馈进行四元数修正的方法实现月球垂直陀螺的俯仰角与滚动角解算；

根据捷联导航方程即式(25-26)

其中q_(k)＝{λ₀，λ₁，λ₂，λ₃}^T，是四元数表示月球车姿态；

运用基于加速度反馈的捷联导航四元数迭代的修正方程

a_(k)＝Q_(k)a_s(k)，Δω_(k)＝a_(k)×g/‖g_m‖²                     (27)

ω_(k)＝ω_s(k)+kQΔω_(k)                                       (28)

解算探测月球车相对地平坐标系的俯仰角与横滚角，由三轴加速度a_s(k)及角速度ω_s(k)，通过式(26-28)计算 

得俯仰角α_(k)与横滚角β_(k)；前式(25-26)与式(27-28)构成了捷联式的垂直陀螺系统方程。

4.根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的月球车初始位姿校准方法是，通过日出时间计算太阳月球车经度，利用高精度太阳罗盘导航方法模块(5)确定纬度及太阳方位角；或者使用基于太阳连测的经纬校准方法模块(9)直接对太阳连续观测确定月球车经纬度。

5.根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的太阳罗盘位姿补偿方法是：依据现有天文知识，对太阳罗盘经纬度进行大气折射补偿、绕日运动补偿、光行差补偿、周日视差补偿、观测视差补偿、月球及地球的岁差及章动补偿，从而保证高精度太阳罗盘导航方法的高精度。 

Images