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

专利名称：：基于全功能太阳罗盘的月球车位姿自主确定方法
技术领域：
：基于全功能太阳罗盘的月球车位姿自主确定方法是以月球探测车(简称月球车)为应用对象的天文与惯性联合导航方法，属深空探测科学及地球观测科学与工程领域。可广泛应用于月球车、火星车及地面作战机器人的导航与控制系统当中。
背景技术：
：作为月球车的导航技术面临三项重要环境约束(1)月球无地磁，现有的磁罗盘技术不能应用；(2)月球是慢自旋的(相对地球自转慢了近30倍)，陀螺寻北技术不能应用于月球北向确定；(3)尽管现代飞行器及航天器广泛利用太阳及恒星逬行位置确定，但在月球车巡游探测吋，采用太阳敏感器或星敏感器，都易受粉尘污染，致使使用寿命很短。另一方面，月球离地球很远，通信及数据处理延吋一般高于8秒甚至达数十分钟，要求月球车具有自主位姿确定能力。因而，需要根据月球无地磁、慢自旋、粉尘污染的环境特点提出新的位姿自主确定方法，以满足曰益增长的月球巡视探测任务的需要。现有的月球与火星车的自主位姿确定方法尚未形成，美国火星车自主位姿确定利用了火星GPS系统，目前尚未见到相关专利与文献对位姿方法进行系统分析与证明。美国及曰本在月球与火星车的位姿确定方面都有自己的方案，但不是自主式的、系统的绝对位姿确定方法，即不能提供相对月球车经纬度、相对地平指北系的航向及月球车相对地平系的速度。现有的技术方案不能满足对月球制图、月球资源普査等科学任务的需要。
发明内容本发明的目的在于为月球车提供适应月球环境，特别是无地磁、慢自旋、粉尘污染环境(除月球车运行过程中产生的粉尘，还有由于高低温环境产生的粉尘环境，该机理尚无科学解释)的月球车自主位姿确定方法。1.技术方案与系统结构本发明的技术方案参见系统结构框图1和工作过程图2，本发明是由观测设备、嵌入式PC104+计算机及内置于广角太阳相机1.1的嵌入式TMS320计算机为基础完成的，其中与嵌入式PC104+计算机集成的观测设备有广角太阳相机l.l、称为IMU1.2的惯性单元、吋钟1.3及高速激光雷达1.4;在广角太阳相机1.1内置的嵌入式TMS320计算机中包含的模块有智能太阳识别与处理方法模块2;在PC104+计算机中包括的模块有通信与任务调度模块IO、月球车自主位姿确定任务模块；月球车自主位姿确定任务模块包含月球垂直陀螺方法模块3、初始^I姿校准方法模块4、高精度太阳罗盘导航方法模块5、位姿补偿方法方法模块6、基于地图重建与即吋定位(SLAM)月球车速度估计方法模块7、具有速度修正的月球车位姿确定方法模块8、基于太阳连测的经纬校准方法模块9。基于全功能太阳罗盘的月球车位姿自主确定方法步骤如下(1)数据获取，由在嵌入式PC104+计算机中幵发的通信与任务调度模块完成，它包含太阳罗盘接口任务、IMU接口任务及高速激光雷达系统接口任务；太阳罗盘接口任务通过CAN通信访问智能相机，包括设置智能太阳识别与处理方法模块所需的参数，最终获取智能太阳识别与处理方法模块得到的太阳入射角W,W、曰出吋间^(。)等；IMU接口任务通过RS232通信访问IMU，获得三轴加速度、三轴角速度;高速激光雷达系统接口任务通过PCL总线通信访问激光雷达，获得测量的深度^、光强々数据流；(2)系统初始化，嵌入式PC104+计算机上电后系统初始化程序工作，对智能相机、IMU、激光雷达进行初始设置，并启动嵌入式PC104+计算机中月球车自主位姿确定任务模块及嵌入式TMS320计算机中智能太阳识别与处理方法模块；(3)嵌入式PC104+计算机中月球车位姿确定任务模块包括月球垂直陀螺方法模块、智能太阳识别与处理方法模块、初始位姿校准方法模块、高精度太阳罗盘导航方法模块、位姿补偿方法模块、基于地图重建与即吋定位(SLAM)月球车速度估计方法模块、具有速度修正的月球车位姿确定方法模块、基于太阳连测的经练校准方法模块，各处理方法之间并行运行，共同完成月球车绝对位姿、相对位姿、平动速度确定；在广角太阳相机内置的嵌入式系统TMS320计算机中包含的模块有智能太阳识别与处理方法模块；下面对各方法模块实现方法及流程进行描述由智能太阳识别与处理方法模块识别曰出或太阳并判断太阳升起吋间Q及太阳入射角&)、太阳方向角^V);该模块首先应对广角镜头进行校准，然后拍摄天空图像，提取太阳圆盘，按圆度、面积、顏色范围来判别由图像提取得到的简单曲线是否为太阳；并计算太阳圆盘中心，根据计算太阳圆盘中心像点坐标求太阳入射角《w及太阳方向角Pw;由月球垂直陀螺方法模块由系统获得的月球车三轴加速度c^)、角速度《(4)为输入，经加速度《")反馈校正四元数，进行四元数计算迭代得月球车俯仰角"w与横滚角y^);由太阳入射角《4)及太阳方向角PW及月球车俯仰角与横滚角计算太阳高度角Z")及月球车前向与太阳的夹角《w;由初始位姿校准模块根据太阳高度角及月球车前向与太阳的夹角《t)通过高精度太阳罗盘导航模块及基于太阳连测的经纬校准模块计算月球车经纬度义W，,)及太阳方位角<9(4)，从而计算月球车航向角OV);或者，由曰出吋按("-C)w,计算角度再换算为经度；其中。叫分别是Si皿smedii中央湾吋间、月球自转角速度；由高精度太阳罗盘导航方法模块应用于月球车初始位姿校准模块及基于太阳连测的经纟韦校准方法模块；它们的输入都是太阳高度角Z(w及观测吋间；其中包含了由观测吋间计算任一吋刻的太阳及月赤道与黄道的升交点经度过程；二者不同在于高精度太阳罗盘导航方法模块是一次观测，基于太阳连测的经纬校准方法模块是多次观测即连测的过程；但后者要为前者前供月球车经度，从而可通过前者连续计算太阳方位角，高精度太阳罗盘导航方法模块以校准的经度及实吋测量的太阳高度角zw及观测吋间连续计算太阳方位角，从而获得月球车航向角O(w;由基于地图重建与即吋定位(SLAM)月球车速度估计方法模块根据测量的三轴加速度4)、三轴角速度化州及由高程图的数据即粒子通过粒子滤波得到月球车的速度^);由基于速度修正的月球车位姿确定方法模块输入为月球车速度^)、月球车三轴加速度"(4)、角速度,)、月球车初始经纬度/^)Aw及月球车航向角a^)，输出是连续的月球车连续速度^+,)、经纬度义(一^。(一及航向角+,)。所述的智能太阳识别与处理方法模块的任务运行于广角太阳相机嵌入式系统当中；该方法模块完成两部分任务即，太阳识别及太阳单位矢量计算与曰出识别及曰出吋间计算，该方法模块的输出结果为曰出吋间、太阳单位矢量；参见图3鱼眼镜头成像示意图，所述的广角太阳相机采用鱼目镜头，其模型为公式吵(1)其中OcXcYcZc为相机坐标系，r为图像点P到鱼眼图像中心Oc的距离，/为鱼眼镜头的焦距，0为入射光线与鱼眼镜头光轴的夹角，即测量高度角，^为像点在坐标系OcXcYcZc的方向角。则对应于入射角为(^^)的空间点《的图像点P在摄像机坐标系下的坐标O,y)满足=柳义sin伊(2)将(1)带入(2)得义(3)由于镜头加工工艺和其他因素，鱼眼镜头通常很难严格满足成像模型(l)，利用三次多项式来拟合该鱼眼镜头的成像模型，即r=/c々+/c203则对应的成像变换为=(^6+a:263)义(4)(5)畸变校正模型按式(6)进行。=0^<9+"2(93)成像过程用6个参数来描述鱼眼镜头成像的模型(6)附u00(o^+/^3)cosp)+(7)通过校准板，拍摄其图像，计算气、W、"。、v。、K、k2六个校正参数。其中气、W为测量高度角与像素坐标矢联常数，"。、v。为像素坐标偏差，《、^为测量高度角校正常数；太阳方向计算根据鱼眼图像中某点的像素坐标("，v)，求该点在摄像机坐标系下的入射角(0，p)。其中^可以利用根据像素坐标直接求解出来；对于0，将气、气、"。、v0、A、k2和^带入式(7)得到关于0的一元三次方程，求解后去除不合理的解到太阳入射角P。相应A:吋刻的太阳入射角及太阳方向角分别记为^w，^)〉太阳识别及太阳单位矢量计算的方法如图4左所示首先获取天空图像；进行滤矂，去除小颗粒粉尘图像；进行轮廓提取，并进行简单曲线几何参数计算，包括面积，质心，圆度计算；同吋计算太阳平均色彩；根据太阳圆度、面积、色彩范围检测太阳；如果判断是太阳则计算太阳圆盘中心；根据太阳罗盘光学参数计算相对太阳罗盘相平面的单位矢量；曰出吋间计算判别过程如图4左所右，首先获取天空图像；滤噪，去除小颗粒粉尘图像；进行轮廓提取，并进行简单曲线几何参数计算，包括面积，质心，圆度计算；同吋计算平均色彩；根据太阳弧段参数、色彩范围检测太阳；若是太阳则估计太阳圆盘中心；由太阳罗盘光学参数计算相对太阳罗盘相平面的单位矢量；根据曰出过程的数据记录求解曰出吋间得月球车所在经度。所述的月球垂直陀螺方法模块方法中需适应月球重力常数、月球半径等环境等点，通过重力反馈进行四元数修正的方法实现月球垂直陀螺的俯仰角与滚动角解算；根据捷联导航方程即式(8-9)其中《w=(/L。,不，;i2，^^，是四元数表示月球车姿态；(8)2W=2(义0(yfc)12(A("zl2(^)-、i)^3(")2(>^)^3(4)十^^2(;t))2(A(jfe)^3(A:)-A)A2(")2(/12(yfc)^3(A:)+、*))2(、^十/l3(")—1(9)运用基于加速度反馈的捷联导航四元数迭代的修正方程(10)(11)解算探测月球车相对地平坐标系的俯仰角与横滚角，由三轴加速度"w,及角速度w卓)，通过式(9-11)计算gw，得俯仰角)与横滚角&);尽管式(8-9)是己有的公式，但与式(10-11)构成了捷联式的垂直陀螺系统方程。所述的月球车初始位姿校准方法模块工作流程如图5左所示。前述的月球车初始位姿校准分两种一种是通过观测太阳，确定曰出吋间^(。,来计算月球车初始经度A(w，从而利用纬度及太阳方位角方程解算月球车纬度&)及太阳方位角^a)。利用垂直陀螺输出的俯仰角"w与横滚角/w、太阳相机获得的太阳方向角及太阳与月球车前向夹角即太阳方向角^w,^W及太阳相机安装姿态计算太阳在自由地平系下的太阳高度角Z(w及太阳相对月球车前向的角度《w，从而计算出月球车航向角&)，该过程参见高精度太阳罗盘导航模块5。另一校准方法是对太阳连续观测解算月球车经度义w，从而利用太阳罗盘原理得到月球车航向角。该过程参见基于太阳连测的经纬校准方法模块9。所述的高精度太阳罗盘导航方法模块是:依据曰-地初始经度运用曰地自转及公转速度及吋间计算们任一吋刻的经纬度，根据球面三角建立太阳罗盘导航方程，进行基于太阳高度及方向的解唯一性判定，以此保证该太阳罗盘导航的高精度。高精度太阳罗盘导航方法流程框图如图6所示。该方法包括曰-地在月球体系下的经度及升交点经度计算、太阳高度角与方向角获取、基于球面天文学的太阳罗盘导航方程求解、月球车航向计算。由于月球车经度由太阳连续观测或曰出校准得到，在吋间上有一定的滞后，但由于短吋间内月球车经纬度变化很小，故对太阳方位角计算精度影响极小。下面对高精度太阳罗盘原理进行说明。如图7所示，月球体系定义原点为月球质量中心，义轴经过Sinusmedii环形坑即中央湾，Z轴为月球自旋轴，r轴由右手系确定，基中Z轴正向为北天极，Z轴负向为南天极；月球赤道(简称月赤道)与黄道交角记为s;太阳罗盘观测的离散吋刻记为A:，其0点为、吋刻；在、吋刻的太阳及升交点经度记为^^。；太阳在月球体系下的经度与纬度分别记为^),乓w;月球探测月球车(观测)位置记为^w,《cw;太阳入射角&,，太阳方向角^w，太阳相对月球车前向的角度为^t)，亦称太阳方向角；太阳在当地地平系下与自转北向的夹角即太阳方向角记为W);由太阳入射角&)，太阳方向角PW及月球车俯仰角aw与横滚角计算太阳高度角Zw及月球车前向与太阳的夹角^t)。高精度太阳罗盘导航方法包括太阳在月球体系下的经度及升交点经度计算方法、太阳罗盘导航方程。所述的曰-地在月球体系下的经度及升交点经度计算是由在、吋刻的太阳及升交点经度Q,"，计算相对、观测离散吋刻A:的在月球体系下的经度/及升交点经度^，它们隨吋间作周期性变化；通常是根据儒略曰及J2000的曰-地-月的轨道根数计算太阳经纬度、黄白升交点经度、等，但过程繁杂，不适合月球车嵌入式系统的计算。故將各、吋刻的太阳及升交点经度^。,。序列事先根据天体动力学计算并保存于嵌入式计算机中，该计算过程具有非常高的精度且计算量较小。记地球自转角速度化、地球绕曰公转角速度《￡、月球自转角速度叫、月球绕地公转角速度A、月赤道与黄道交角S、地赤道与黄道交角《、地赤道与月赤道交角&;在短吋间内将月-地视为匀速转动来计算短期内任一观测吋刻A:的太阳经纬度^w,及黄白升交点经度(W。所述的高精度太阳罗盘导航方法是根据事先计算的定期的曰-地-月星历数据保存于月球车嵌入式系统中，根据式下述公式(12-15)完成适合月球车嵌入式系统进行任一吋刻的太阳及月赤道与黄道的升交点经度；,、fartan(cosCtana)+;r/2//"Ce|"/;r—;r/2,/;r+;r/2")，/=1,2...f(C,a)Hr、(12)、'卜artan(cosCtana)—;z72Ce[/;r+;r/2,(/+l);r+;r/2),/=1,2...Cot)^,o+f(化A:,&)+f(巧A:,《)+f(f(ft^A:,fe),A)+f(6^A:，A(13)=Artan(sin(^)-Q))tans)(15)其中Q-太阳经度、Q-白黄升交点经度、化-地球自转角速度、《￡-地球绕曰公转角速度、q-月球自转角速度、化-月球绕地公转角速度、《-白道与黄道交角、《-地赤道与黄道交角、^-地赤道与月赤道交角；^w,《(太阳经纬度、w-黄白升交点经度，A:月球零经度离散的标准吋；然后由计算的任一吋刻的太阳经度、w及月赤道与黄道的升交点经度f^)运用公式(16-19郷(C/(4+i)-C"A+2))(Cz(/t+2)_Cz(/t+3))_(C/(>t+2)-C,(A+3))(Cz师)-Cz(t+2))4+2)=artan(16)^>(i)=2artanc早)r1—9广(*)、00fc))(17)=artan月球车航向角计算公式广99、(18)计算月球车的经纬度《w及月球车航向角。式(16-18)是根据球面三角导航方程即(19)Cz=S(5&。+c《c《。c(w。)推理得到的。前提是以太阳连测的太阳纬度变化极小即5&+1)+2)+3)为前提得到的。所述的太阳罗盘位姿补偿方法是依据现有天文知识，对太阳罗盘经纬度进行大气折射补偿、绕曰运动补偿、光行差补偿、周曰视差补偿、观测视差补偿、月球及其被绕行星的岁差及章动补偿，从而保证高精度太阳罗盘导航方法的高精度。所述的基于地图重建与即吋定位(SLAM)月球车速度估计方法是利用高程图及其对应的障碍图数据特征通过粒子滤波技术实现月球车速度估计，以保证月球车速度估计的收敛性与可靠性；由垂直陀螺系统提供俯仰角与滚动角，由激光雷达测量环境深度图，经坐标系变换得到高程图，按式(20)求z)'网格点求四向平均梯度从而星到障碍图，用于地图重建与即吋定位(SLAM)使用，即为SLAM提供粒子；=)^(l力4》.,l+l+W-一+I,"-~I+1-~.I)/(")>G。(20)其中^A:J〉A，z',y,A:为整数，~为高度，G".为z)'网格点的梯度，G。为设置的阈值；经四向梯度障碍提取得到障碍图。建立地图重建与即吋定位(SLAM)的导航方程，利用粒子滤波实现环境数据与航位推算过程的融合。激光雷达精度很高，达l-4mm。由于粒子数的限制，只能利用月球车当前环境样本进行滤波，故利用障碍距离修正航位推算过程能够使系统稳定，但仅能保证月球车平动速度具有较高的精度，而相对位置在长吋间运行之后会出现较大的误差累积，故仍需要外绝对位置校准。基于地图重建与即吋定位(SLAM)月球车速度估计方法如图9所示。过程算法流程如图10所示，该过程由式(21-24)分7步完成(1)由惯性单元(加速度计)在地平面下的加速度(fl^)，(4))及月球车航向角&,，经二次积分得到该单步过程的位置增量AXt,其中AATA=(a,a)即是月球车的相对位置；(2)结合上一步最优位姿估计值X乙给出当前的先验位姿估计<formula>complexformulaseeoriginaldocumentpage18</formula>(21)(3)假定过程b的误差模型服从某一分布(如高斯分布)，由此确定粒子采样所依据的建议分布，并进行粒子釆样'《~iVp^")(a为事先估计的噪声分布方差)；(4)读取当前激光雷达的环境测量数据^，并与之前所创建的地图特征^w配准。配准过程采用ICP(IteratedClosedPoints)算法。这样对于每个粒子得到一个最佳的位姿估计<formula>complexformulaseeoriginaldocumentpage18</formula>(22)在上述配准过程中，同吋对环境观测量和地图之间的似然程度逬行评价，据此计算每个粒子的重要性权值系数w，并逬行规一化处理<formula>complexformulaseeoriginaldocumentpage18</formula>。(5)由此计算等效粒子数<formula>complexformulaseeoriginaldocumentpage18</formula>。若该值小于设定阈值A^〈r，重新对粒子群采样。(6)月球车位姿状态的最优值估计应为所有粒子的加权和<formula>complexformulaseeoriginaldocumentpage18</formula>(23)(7)加入当前的环境观测数据，对地图进行更新<formula>complexformulaseeoriginaldocumentpage18</formula>(24)所述的基于太阳连测的经纬校准方法是以高精度太阳罗盘导航方法为基础，通过基于地图重建与即吋定位(SLAM)月球车速度估计提供的月球车速度进行航位计算，并利用高精度太阳罗盘导航方法实吋地计算出任一吋刻的月球车经纬度及航向，再根据太阳罗盘的太阳方位角方程计算太阳方位，该方法参见图12所示。它是通过多次测量(至少3次)按式下面公式(25-27)迭代计算，最后按式(27)计算月球车的经纬度及太阳方位角，其前提是连测吋月球车保持静止；<formula>complexformulaseeoriginaldocumentpage18</formula>(25)<formula>complexformulaseeoriginaldocumentpage19</formula>记连续观测太阳W次，0SA^iV-2，得到^-l次测量结果。，对它们分别求平均得最终的测量结果<formula>complexformulaseeoriginaldocumentpage19</formula>所述的基于太阳连测的经纬校准方法是建立太阳连测导航方程，通过太阳连测，并进行月球车经纬度的更新计算，提高了经纬度计算精度，同吋也保证了嵌入式计算的实吋性，确保在线位姿的准确性。基于速度修正的月球车位姿确定方法是在高精度太阳罗盘导航方法的基础上，通过基于地图重建与即吋定位(SLAM)月球车速度估计提供的月球车速度进行航位计算，并利用高精度太阳罗盘导航方法实吋地计算出任一吋刻的月球车经纬度及航向，并根据太阳罗盘的太阳方位角方程计算太阳方位。由于在航位推算过程中月球车航向是根据更新的经纬确定的，保证了月球车航向的高精度，从而大大提高了经纬度的计算精度。该方法参见图11所示。该方法利用了成熟的卡尔曼滤波技术实现月球车速度估计、高精度太阳罗盘位姿确定及月球车捷联系统的融合。本发明与现有太阳罗盘及相关技术相比具有突出的本质区别现有的月球车位姿主要通过星敏感器利用位置圆原理进行逬行确定，其缺点是(1)星敏感器对工作环境清洁度要求极高，在月球表面，极易受粉尘污染，致使寿命很短；(2)星敏感器造价高，重量相对太阳敏感器要高得多，从而大大增加了制造及发射成本；(3)星敏感器工作吋要求背对太阳，要进行位置确定还需转动以观测不同的恒星，因此在月球车使用星敏感器逬行位姿确定就要安装两个自由度的高精密云台，从而保证星敏感器背着太阳并保证星敏感器的测量精度。这样星敏感器在月球车上应用就遇到了很大困难，—方面增加了制造与发射成本，同吋也大大降低了可靠性。美国火星车使用了太阳敏感器，但是火星有较强的地磁、自转周期较小，故通过磁罗盘及捷联平台就能完成位姿确定功能。火星车上的太阳敏感器要求己知火星车的大概经纬度，才能确定探测车的方向。本发明的主要特点是(1)适应月球重力环境；(2)适应月球慢自旋环境；(3)适应月球粉尘环境；(4)适应月球无地磁环境；(5)能够同吋完成较高精度的经纬度及航向确定功能；(6)系统质量较低，有利于降低月球车整体质量及研制与运营成本；(7)高可靠性，可以满足长吋间的位姿自主确定需求。图l基于全功能太阳罗盘的月球车位姿自主确定系统结构框图；标号如下观测设备1、广角太阳相机1.1、惯性单元IMU1.2、吋钟1.3、及高速激光雷达1.4,智能太阳识别与处理方法模块2、月球垂直陀螺方法模块3、初始位姿校准方法模块4、高精度太阳罗盘导航方法模块5、位姿补偿方法方法模块6、基于地图重建与即吋定位(SLAM)月球车速度估计方法模块7、具有速度修正的月球车位姿确定方法模块8、基于太阳连测的经纬校准方法模块9、通信与任务调度模块10;图2基于全功能太阳罗盘的月球车位姿自主确定方法工作过程图；图3鱼眼镜头成像示意图4基于全功能太阳罗盘的月球车位姿自主确定方法流程图；图5月球车位姿校准模式及垂直陀螺工作方法流程图；图6高精度太阳罗盘导航方法流程图；图2-6符号如下D-表示任务；z^7-表示数据输入；O-表示数据保存至内存；I^i-表示函数；O-表示条件判断；O-表示任务或程度终结；图7基于球面天文学的太阳罗盘原理符号表示如下月球体系-I定义原点为月球质量中心，义轴经过Sinusmedii环形坑，Z轴为月球自旋轴，F轴由右手系确定，基中Z轴正向为北天极，Z轴负向为南天极；Sinusmedii-是月球正面的环形坑，即中央湾，被国际天文联合会定义为月球体系的0经度经的地方；A^-月球自转轴北向，即月球北天极；、-月球自转轴南向，即月球南天极；S"-太阳中心；^-太阳所在纬度线与月赤道在无穷远处的交点；E-月球车所有位置的东向；W-月球车所有位置的西向；y。-升交点，即月赤道与黄道在无穷远处的交点；^-是月球车系统计算吋刻，是系统计算太阳经度、升交点经度的参考吋刻，它与系统采用北京地方吋一致；W^是月球车系统计算吋刻对应的格林威治吋间，即国际标准吋；A-月球车所在位置天的方向，即天顶，与重力方向基本一致，但方向指向天空；3-(^&与月赤道的夹角，它随吋间按一定规律变化，可用球面三角公式计算；G。-在、吋刻的太阳经度，是太阳在月赤道投影与月球体系x轴的夹角；^-在C吋刻的升交点经度，是&y。与x轴的夹角；e-白道(月球赤道)与黄道交角记为s，是白道面与黄道面法向构成的交角；/-C^/"与x轴的交角；^-太阳在月球体系下的纬度，月球体系原点(^与太阳构成的矢量与月赤道的夹角；/。-月球车(观测)经度；《-月球车(观测)纬度；Z,-太阳罗盘观测的高度角，即月球体系原点C^与太阳构成的矢量与太阳太阳罗盘所在位置的天顶所张的角度；Q太阳罗盘观测太阳的观测方向角，即6^S"在太阳罗盘所在位置的自由地平系投影与该系x轴所张的角；y-太阳方向角，即0^5在太阳罗盘所在位置的地平指系投影与北向所张的角；图8基于球面天文学的太阳罗盘导航方程显式求解可视化结果示意(a)观测纬度解算结果(b)太阳方位算结果；图9基于地图重建与即吋定位(SLAM)的月球车速度估计模块流程图;符号如下D-表示任务；D-表示数据输入；O-表示数据保存至内存；I^l-表示函数；O-表示条件判断；O-表示任务或程度终结；图IO具有速度修正的月球车位姿确定算法流程图I表示月球车位姿(包括平面上的二维位置坐标和航向角)，下标1"表示第A个单步，上标"-"表示先验估计，"+"表示后验估计；、表示当前粒子群中某个粒子/所携带的位姿，w:表示每个粒子/的重要性权值，^表示规一化后的权值；^表示当前的环境观测量；A^表示当前地图的状态估计；w为粒子群中包含的粒子个数；图ll具有速度修正的月球车位姿确定方法.流程图12基于太阳连测的经纬校准方法流程图11-12符号如下D-表示任务；D-表示数据输入；O-表示数据保存至内存；I_l-表示函数；O-表示条件判断；O-表示任务或程度终结；其它符号说明如下。/deg-角度单位，度；'-角度单位-分；"-角度单位-秒；Hr-吋间单位，小吋；min-吋间单位，分；s-吋间单位，秒；ms-吋间单位，毫秒;g,-月球重力加速度，1.62m"2;m-长度单位，米；mm-长度单位，毫米；图13蝴蝶、蜣螂利用偏振光进行导航及它们的眼睛结构示意图14基于全功能太阳罗盘的位姿自主确定与SLAM的联合定位仿真1;图15基于全功能太阳罗盘的位姿自主确定与SLAM的联合定位仿真2。本发明技术方案的技术思路来源(1)来源于生物位姿确定的仿生原理如图12所示，蝴蝶、蜣螂利用偏振光逬行导航。发明人发现借助曰光或月光导航的生物的习性如下a.在春分及秋分吋期十分活跃，猜测它们位姿确定在该吋期应更准确，否则它们应该感至紧张或行为变得不活跃。b.它们在清晨、傍晚吋十分活跃，猜测此吋它们的位姿确定也很准确的。发明人提出的高精度太阳罗盘导航方法的分析结果也证明了现有的猜测。图8仿真结果表明，在夏至及冬至吋期的清晨与傍晚吋段航向确定不准确或不能确定的，正午航向确定是不精确的或不可能的，此吋必需借助其它位姿确定方法。该季节许多依据太阳导航的生物在清晨、傍晩及正午也是不活跃的。在春分与秋分至附近吋在清晨、傍晩吋经纬度确定及航向确定都是精确的。而在此季节依据太阳导航的生物在清晨、傍晩吋是非常活跃的，而仅在正午经纬度确定是不精确的，此吋依据太阳导航的生物行为也是不活跃的。本发明为解释依据太阳导航的生物行为提供了理论基础。同吋，借助曰光或月光导航的生物一般以曰光或月光逬行粗略的定位与精确的定向，而精确的定位是以视景图像为基础的。(2)本发明思路也来源于手机及古地磁探测采用的太阳罗盘定向原理。其过程是使用者將手机或古地磁太阳罗盘的指示太阳或0度线与真实太阳对准，并将手机或古地磁太阳罗盘保持水平，从而用太阳罗盘指出北向角度。它是利用球面三角建立的太阳方位方程逬行计算的。但是使用者必须为它提供经纬度，或以告知所地城市名称的形式来提供，本质上是提供了近似的经纬度。它的缺点是使用者要提供经纬度及水平位置，同吋北向精度确定很差，只能用于粗略的北向确定，而远远不能满足月球车航向确定需求。具体实施例方式本发明中所选用的部分方法模块的技术构成及技术指标说明如下1、观测设备由广角太阳相机、IMU组成、高速激光雷达及吋钟组成。广角太阳相机视场角为185°;分辩率1024x1098;采用CAN通信方式。在该相机中专门开发了智能太阳识别与处理系统，用于紛尘过滤、太阳识别并计算单位矢量、识别曰出并输出曰出吋间；IMU:加速度计范围i5g;加速度计线性度1.5%;光纤陀螺游走1.8。/V^，光纤陀螺速度范围100。"，光纤陀螺线性度1.5%;通信方式采用RS232。输出三轴加速度、三轴角速度、系统温度；高速激光雷达深度检测范围16m;速率5万点/秒；视场120°;具有两个自由度，通过高速激光梭镜方向伺控制，实现对三维空间进行高速扫描测量。通信方式采用PCI总线。输出被测点的深度值、激光强度、雷达系统温度、行场位置编码；输出数据经光强及温度补偿后得到高精度深度值。并利用月球车俯仰角、横滚角、雷达行场编码建立高精度环境高程图。吋钟由嵌入式计算机根据星历预报曰出吋间，在曰出前一曰进行吋钟设置。在曰出前30分钟产生吋钟中断，从而启动太阳相机中的曰出识别及处理任务。2、垂直陀螺是在IMU(三轴微机械加速度计、三轴光纤陀螺)基础上进行二次开发而成，即完成垂直陀螺的俯仰角、滚动角解算。采用光纤陀螺主要是因为质量轻，抗冲击性能好，同吋具有较高的检测精度，适应月球车工作环境的需要。应用CrossBow垂直陀螺的三轴加速度与角速度解算的俯仰角与滚动角与CrossBow垂直陀螺输出结果基本一致。加速度小于0.2§吋，俯仰角与滚动角误差在5'以下。月球垂直陀螺参数与地球上的垂直陀螺不同，主要表现于重力常数，同吋应考虑月球重力异常(重力瘤)、月球半径、重力反馈四元数系数等。因而现有的地球垂直陀螺不能直接应用于月球车，必需了解垂直陀螺原理才能开发月球垂直陀螺。本发明的主要实施步骤如下步骤l系统构建根据月球车自主位姿确定指标选择适合的IMU,其中IMU的光纤陀螺线性度好于2%、游走1.8。/i。加速度线性优于1%，检测精度优于10"g。激光雷达测距精度优于3mm，速度优于4万点/秒。相机分辨率优于1024x1024，相机嵌入式系统计算速度高于3200MIPS;广角镜头形变率低于0.2%，镜头模型为〃=/0，视场可根据需要选择；开发相应接口程度。从而可以获得月球车三轴加速度"w、角速度"a,。步骤2智能太阳识别与处理模块开发。首先对广角镜头进行校准，通过拍摄校正板图案并按校正板图案像点位置参照式(7)逬行，求解气，附v，^^,"。,v。。开发太阳检测程序，提取太阳圆盘，按圆度、面积、顏色范围来判别由图像提取得到的简单曲线是否为太阳。并计算太阳圆盘中心，根据计算太阳圆盘中心像点坐标参照式(7)求太阳入射角^，太阳方向角P由太阳圆盘中心像点坐标直接计算。相应^吋刻的太阳入射角及太阳方向角分别记为{)，％)}。步骤3月球垂直陀螺方法模块开发。建立捷联系统方程(8-9)，以获取月球车三轴加速度"w、角速度w(w为输入，并按(10)计算加速度反馈用的角速度，加入式(11)中逬行四元数计算迭代得月球车俯仰角《a)与横滚角~。步骤4由步骤2得到的太阳入射角&,，太阳方向角^4)及步骤3得到的月球车俯仰角aw与横滚角/w计算太阳高度角Zw及月球车前向与太阳的夹角《w。步骤5月球车初始位姿校准模块开发。由智能太阳识别与处理模块识别太阳并判断太阳升起吋间(.。，由"。-U叫计算角度再换算为经度。其中。",分别是Si皿smedii中央湾吋间、月球自转角速度。或由太阳连测的经纬校准方法进行经度测量。该方法参考式(24-27)及太阳连测的经纬校准方法一节，即将步骤4得到的太阳高度角Z(,)代入公式(24-27)求得^),^w,^V)。由式(19)计算月球车航向角^"。步骤6基于地图重建与即吋定位(SLAM)月球车速度估计模块开发。获取环境深度图并投影至水平面。根据式(20)求场景中每像点的四向平均梯度，并判断是否大于阈值，若成立则视为障碍点，于是得环境障碍图。由步骤5得到的CV)及步骤1得到的月球车三轴加速度fl(,)、角速度^v)进行航位推算，得月球车单步位移增量A^，将之及激光测得深度值、—的代入式(21-24)逬行粒子滤波，获得月球车速度^,。步骤7基于速度修正的月球车位姿确定模块开发首先建立捷联平台及具有车体速度观测的状态方程捷联导航方程进行迭代计算，同吋加入式(10)即垂直陀螺产生的校正角速度。由输入即月球车速度^)、月球车三轴加速度"w、角速度"w、月球车初始经纬度义w,&w及月球车方向。w，对捷联导航方程进行四元数迭代求解。从而得连续的月球车连续速度^+,,、经纬度^+,)^。(一及航向角^+,)。步骤8月球车连续运行一段吋间(根据实验测的误差范围来确定)后，重复步骤2-6，即完成再次太阳连测校准后进行月球车位姿自主确定。本发明的实施过程及结果已由工程实验得到验证。具体如下应用例1应用发明人研制的基于全功能太阳罗盘的位姿确定方法，对太阳进行了观测。测试环境与条件吋间2008.6.6全天；地点北京工业大学综合楼前广场；天气晴；天空有少量云；风4级。测试设备发明人开发的太阳罗盘+垂直陀螺VG700、校准设备GPS，定位精度4米，方向精度0.15。。测试人员王亮、居鹤华(北京工业大学电控学院教师)测试结果参见表l表1测试结果<table>complextableseeoriginaldocumentpage26</column></row><table>测试结论由上面的测试结果可知，证明了本发明正确性。尽管与GPS测量结果对比有一定误差，但自主位姿确定功能是可靠的，并且不依赖地磁、不需检测地球转动并且对粉尘污染影响很小。主要误差原因是太阳相机测量精度较低，同吋测量设备没有很好校准，通过提高设备校准精度后，预计经纬及太阳方位角测量精度优于l-5'。应用例2将本发明应用于月球车导航中，将基于全功能太阳罗盘的位姿自主确定方法与地图重建及即吋定位技术相结合用于月球车相对定位。测试环境与条件吋间2008.6.9-11;地点北京工业大学综合楼821;在个人计算机上进行仿真分析；仿真中IMU基本参数为加速度计线性度1.5%，漂移0.1m"l;陀螺游走1.8°/|，陀螺线性度1.5%;通过测量地形高程精度10mm;测试人员王亮、居鹤华(北京工业大学电控学院教师)测试结果根据上述测试条件逬行基于全功能太阳罗盘的位姿自主确定与SLAM的联合定位仿真，得图14-15所示的仿真结果，其中上图为使用惯性单元的航位推算结果、基于太阳罗盘与垂直陀螺的位姿自主确定与SLAM的联合定位误差曲线，下图为月球车运行轨迹及用于位置校准的样本点集。由图14-15可知，在仅使用IMU逬行航位推算吋很快就变得发散，而使用基于太阳罗盘与垂直陀螺的位姿自主确定与SLAM的联合定位后，相对位置是收敛的，并具有较高的精度。该实验中，使用了太阳罗盘与垂直陀螺一起确定的月球车航向，即航向可以精确检测，并且不依赖于磁罗盘及惯导平台。测试结论仿真结果表明本发明对月球车相对定位精度的提高具有非常重要的意义。表现于本发明能够适应无地磁、慢自旋、粉尘污染的环境要求，同吋航向精度很高，在静态吋精度能够提高至太阳观测相机的次像素级，即可达到l-5'左右，而动态精度可达到0.2°。而绝对位置定位精度同样可确定到相同的数量级。最后应说明的是以上实施例仅用以说明本发明而并非限制本发明所描述的技术方案；因此，尽管本说明书参照上述的各个实施例对本发明已进行了详细的说明，但是，本领域的普通技术人员应当理解，仍然可以对本发明进行修改或等同替换；而一切不脱离发明的精神和范围的技术方案及其改进，其均应涵盖在本发明的权利要求范围当中。权利要求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)、基于地图重建与即时定位(SLAM)月球车速度估计方法模块(7)、具有速度修正的月球车位姿确定方法模块(8)、基于太阳连测的经纬校准方法模块(9)；基于全功能太阳罗盘的月球车位姿自主确定方法步骤如下(1)数据获取，由在嵌入式PC104+计算机中开发的通信与任务调度模块完成，它包含太阳罗盘接口任务、IMU接口任务及高速激光雷达系统接口任务；太阳罗盘接口任务通过CAN通信访问智能相机，包括设置智能太阳识别与处理方法模块所需的参数，最终获取智能太阳识别与处理方法模块得到的太阳入射角id="icf0001"file="A2008101168070002C1.tif"wi="14"he="5"top="143"left="120"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>日出时间ts(0)等；IMU接口任务通过RS232通信访问IMU，获得三轴加速度as(k)、三轴角速度ωs(k)；高速激光雷达系统接口任务通过PCL总线通信访问激光雷达，获得测量的深度hij、光强Iij数据流；(2)系统初始化，嵌入式PC104+计算机上电后系统初始化程序工作，对智能相机、IMU、激光雷达进行初始设置，并启动嵌入式PC104+计算机中月球车自主位姿确定任务模块及嵌入式TMS320计算机中智能太阳识别与处理方法模块；(3)嵌入式PC104+计算机中月球车位姿确定任务模块包括月球垂直陀螺方法模块、智能太阳识别与处理方法模块、初始位姿校准方法模块、高精度太阳罗盘导航方法模块、位姿补偿方法模块、基于地图重建与即时定位(SLAM)月球车速度估计方法模块、具有速度修正的月球车位姿确定方法模块、基于太阳连测的经纬校准方法模块，各处理方法之间并行运行，共同完成月球车绝对位姿、相对位姿、平动速度确定；在广角太阳相机(1.1)内置的嵌入式系统TMS320计算机中包含的模块有智能太阳识别与处理方法模块(2)；下面对各方法模块实现方法及流程进行描述由智能太阳识别与处理方法模块识别日出或太阳并判断太阳升起时间ts0及太阳入射角θ(k)、太阳方向角id="icf0002"file="A2008101168070002C2.tif"wi="8"he="4"top="266"left="59"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>该模块首先应对广角镜头进行校准，然后拍摄天空图像，提取太阳圆盘，按圆度、面积、顔色范围来判别由图像提取得到的简单曲线是否为太阳；并计算太阳圆盘中心，根据计算太阳圆盘中心像点坐标求太阳入射角θ(k)及太阳方向角id="icf0003"file="A2008101168070003C1.tif"wi="8"he="4"top="37"left="168"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>由月球垂直陀螺方法模块由系统获得的月球车三轴加速度a(k)、角速度ω(k)为输入，经加速度a(k)反馈校正四元数，进行四元数计算迭代得月球车俯仰角α(k)与横滚角β(k)；由太阳入射角θ(k)及太阳方向角id="icf0004"file="A2008101168070003C2.tif"wi="5"he="3"top="69"left="88"img-content="drawing"img-format="tif"orientation="portrait"inline="yes"/>及月球车俯仰角α(k)与横滚角β(k)计算太阳高度角Z(k)及月球车前向与太阳的夹角ξ(k)；由初始位姿校准模块根据太阳高度角Z(k)及月球车前向与太阳的夹角ξ(k)通过高精度太阳罗盘导航模块及基于太阳连测的经纬校准模块计算月球车经纬度λ(k)，δ0(k)及太阳方位角θ(k)，从而计算月球车航向角Φ(k)；或者，由日出时按(ts0-tm)ωl计算角度再换算为经度；其中tm，ωl分别是Sinusmedii中央湾时间、月球自转角速度；由高精度太阳罗盘导航方法模块应用于月球车初始位姿校准模块及基于太阳连测的经纬校准方法模块；它们的输入都是太阳高度角Z(k)及观测时间；其中包含了由观测时间计算任一时刻的太阳及月赤道与黄道的升交点经度过程；二者不同在于高精度太阳罗盘导航方法模块是一次观测，基于太阳连测的经纬校准方法模块是多次观测即连测的过程；但后者要为前者前供月球车经度，从而可通过前者连续计算太阳方位角，高精度太阳罗盘导航方法模块以校准的经度及实时测量的太阳高度角Z(k)及观测时间连续计算太阳方位角，从而获得月球车航向角Φ(k)；由基于地图重建与即时定位(SLAM)月球车速度估计方法模块根据测量的三轴加速度as(k)、三轴角速度ωs(k)及由高程图的数据即粒子通过粒子滤波得到月球车的速度V(k)；由基于速度修正的月球车位姿确定方法模块输入为月球车速度V(k)、月球车三轴加速度a(k)、角速度ω(k)、月球车初始经纬度λ(k)，δ0(k)及月球车航向角Φ(k)，输出是连续的月球车连续速度V(k+i)、经纬度λ(k+i)，δ0(k+i)及航向角Φ(k+i)。2、根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的智能太阳识别与处理方法模块的任务运行于广角太阳相机嵌入式系统当中；该方法模块完成两部分任务即，太阳识别及太阳单位矢量计算与曰出识别及曰出吋间计算，该方法模块的输出结果为曰出吋间、太阳单位矢量；广角太阳相机采用鱼目镜头，其模型为公式(l)其中OcXcYcZc为相机坐标系，r为图像点P到鱼眼图像中心Oc的距离，/为鱼眼镜头的焦距，e为入射光线与鱼眼镜头光轴的夹角，即测量高度角，伊为像点在坐标系OcXcYcZc的方向角，则对应于入射角为(^p)的空间点《的图像点P在摄像机坐标系下的坐标(x,y)满足<formula>complexformulaseeoriginaldocumentpage4</formula>(2)將(1)带入(2)得<formula>complexformulaseeoriginaldocumentpage4</formula>(3)由于镜头加工工艺和其他因素，鱼眼镜头通常很难严格满足成像模型(l),利用三次多项式来拟合该鱼眼镜头的成像模型，即A*=AT々+则对应的成像变换为cosp(4)(5)畸变校正模型按式(6)进行cosp成像过程用6个参数来描述鱼眼镜头成像的模型(0^(9+、03)w00w..cosp)+(6)(7)通过校准板，拍摄其图像，计算气、mv、"。、v。、k,、^六个校正参数，其中m、W为测量高度角与像素坐标矢联常数，"。、v。为像素坐标偏差，^、K为测量高度角校正常数；太阳方向计算根据鱼眼图像中某点的像素坐标(",v)，求该点在摄像机坐标系下的入射角(e,p),其中伊可以利用根据像素坐标直接求解出来；对于e,將气、W、"。、v。、K、A和P带入式(7)得到关于6的一元三次方程,求解后去除不合理的解到太阳入射角^,相应A:吋刻的太阳入射角及太阳方向角分别记为。3、根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的月球垂直陀螺方法模块方法中需适应月球重力常数、月球半径等环境等点，通过重力反馈进行四元数修正的方法实现月球垂直陀螺的俯仰角与滚动角解算；根据捷联导航方程即式(8-9)<formula>complexformulaseeoriginaldocumentpage5</formula>(8)其中gw-Wo,A，义2，Ar,是四元数表示月球车姿态；<formula>complexformulaseeoriginaldocumentpage5</formula>(9)运用基于加速度反馈的捷联导航四元数迭代的修正方程<formula>complexformulaseeoriginaldocumentpage5</formula>(10)<formula>complexformulaseeoriginaldocumentpage5</formula>(11)解算探测月球车相对地平坐标系的俯仰角与横滚角.由三轴加速度",w及角速度通过式(9-11)计算。w,得俯仰角"("与横滚角/^;前式(8-9)与式(10-11)构成了捷联式的垂直陀螺系统方程。4、根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的月球车初5^Z姿校准方法是，通过曰出吋间计算太阳月球车经度，利用高精度太阳罗盘导航方法模块(5)确定纬度及太阳方位角；同吋可以使用基于太阳连测的经纬校准方法模块(9)直接对太阳连续观测确定月球车经銪度。5、根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于.所述的高精度太阳罗盘导航方法是根据事先计算的定期的曰-地-月星历数据保存于月球车嵌入式系统中，根据式下述公式(12-15)完成适合月球车嵌入式系统逬行任一吋刻的太阳及月赤道与黄道的升交点经度；<formula>complexformulaseeoriginaldocumentpage5</formula>(12)<formula>complexformulaseeoriginaldocumentpage5</formula>(13)《"=Artan(sin(仏)-"))tans)(15)其中r,。-太阳经度、Q-白黄升交点经度、％-地球自转角速度、6^-地球绕曰公转角速度、^-月球自转角速度、A-月球绕地公转角速度、s,-白道与黄道交角、^-地赤道与黄道交角、Sfe-地赤道与月赤道交角；^),^)-太阳经纬度、^-黄白升交点经度，^月球零经度标准吋；然后由计算的任一吋刻的太阳经度^w及月赤道与黄道的升交点经度q"运用公式(16-19)即4+2)二artan+1)—Sot+2))(Cz(*+2)—Cz(lt+3))-(S,(jt+2)-S(i十3))(Cz(/t+l)—Cz(lt+2))(16)<50(A)=2artan，)(17)^=artan月球车航向角计算公式,广99、、w(一緣)-,ow)VCz(^。("-C早)乂(18)①(19)计算月球车的经纬度A"，Aw及月球车航向角)；式(16-18)是以太阳连测的太阳练度变化极小即《("1)5("2)5("3)为前提,根据球面三角导航方程即<formula>complexformulaseeoriginaldocumentpage6</formula>推理得到的。6、根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于,所述的太阳罗盘位姿补偿方法是依据现有天文知识，对太阳罗盘经纟韦度逬行大气折射补偿、绕曰运动补偿、光行差补偿、周曰视差补偿、观测视差补偿、月球及地球的岁差及章动补偿，从而保证高精度太阳罗盘导航方法的高精度。7、根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的基于地图重建与即吋定位(SLAM)月球车速度估计方法是利用高程图及其对应的P章碍图数据特征通过粒子滤波技术实现月球车速度估计，以保证月球车速度估计的收敛性与可靠性；由垂直陀螺系统提供俯仰角与滚动角，由激光雷达测量环境深度图，经坐标系变换得到高程图，按式(20)求々'网格点求四向平均梯度从而得到障碍图，用于地图重建与即吋定位(SLAM)使用，即为SLAM提供粒子；=}主1"一"—~1+1CM)—|)/(")>G0(20)其中/4,7'",/,M为整数，~为高度，(^为i/网格点的梯度，G。为设置的阈值；经四向梯度障碍提取得到障碍图。建立地图重建与即吋定位(SLAM)的导航方程，利用粒子滤波实现环境数据与航位推算过程的融合。该过程由式(21-24)分7步完成(1)由惯性单元(加速度计)在地平面下的加速度("^),(4))及月球车航向角&),经二次积分得到该单步过程的位置增量AA^，其中AXt=0ct,h)即是月球车的相对位置；(2)结合上一步最优位姿估计值Z"给出当前的先验位姿估计《《《1+AZt(21)(3)假定过程b的误差模型服从某一分布(如高斯分布)，由此确定粒子采样所依椐的建议分布，并进行粒子采样、-iV(《,cr)(c7为事先估计的噪声分布方差)；(4)读取当前激光雷达的环境测量数据^，并与之前所创建的地图特征^-,配准。配准过程采用ICP(IteratedClosedPoints)算法。这样对于每个粒子得到一个最佳的位姿估计=argmax一(^、-,恥—,)./('《,)J(22)在上述配准过程中，同吋对环境观测量和地图之间的似然程度逬行评价，据此计箅每个粒子的重要性权值系数^，并进行规一化处理SI=</力(5)由此计算等效粒子数A^-l/^^:。若该值小于设定阈值AV〈7^重新对粒子群采样；(6)月球车位姿状态的最优值估计应为所有粒子的加权和:<formula>complexformulaseeoriginaldocumentpage8</formula>(23)(7)加入当前的环境观测数据，对地图逬行更新<formula>complexformulaseeoriginaldocumentpage8</formula>(24)8、根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的基于太阳连测的经纬校准方法是以高精度太阳罗盘导航方法为基础，通过基于地图重建与即吋定位(SLAM)月球车速度估计提供的月球车速度进行航位计算，并利用高精度太阳罗盘导航方法实吋地计算出任一吋刻的月球车经纬度及航向，翔艮据太阳罗盘的太阳方位角方程计算太阳方位；它是通过多次测量(至少3次)按式(25-27)迭代计算，最后按式(27)计算月球车的经纬度及太阳方位角，其前提是连测吋月球车保持静止；<formula>complexformulaseeoriginaldocumentpage8</formula>(27)记连续观测太阳W次，0《A:S7V-2，得到W-1次测量结果。，对它们分别求平均得最终的测量结果<formula>complexformulaseeoriginaldocumentpage8</formula>(27)所述的基于太阳连测的经纬校准方法是建立太阳连测导航方程，保证在线位姿确定。9、根据权利要求1所述的基于全功能太阳罗盘的月球车位姿自主确定方法，其特征在于，所述的基于速度修正的月球车位姿确定方法是在高精度太阳罗盘导航方法的基础上，通过基于地图重建与即吋定位(SLAM)月球车速度估计提供的月球车速度进行航位计算，并利用高精度太阳罗盘导航方法实吋地计算出任一吋刻的月球车经纬度及航向，并根据太阳罗盘的太阳方位角方程计算太阳方位。全文摘要本发明是一种用于深空探测科学及地球观测领域的基于全功能太阳罗盘的月球车位姿自主确定方法。该方法是以观测设备、嵌入式PC104+计算机及内置于广角太阳相机的嵌入式TMS320计算机为基础完成的。本发明利用在嵌入式系统中的广角太阳相机、IMU及高速激光雷达接口程序获取数据；并将月球车位姿确定任务交与系统中的智能太阳识别与处理、月球垂直陀螺、初始位姿校准、高精度太阳罗盘导航、位姿补偿方法、基于地图重建与即时定位(SLAM)月球车速度估计、具有速度修正的月球车位姿确定、基于太阳连测的经纬校准的各方法模块并行运行，整个系统中所执行的处理方法共同完成了月球车位姿的确定。该方法为月球车提供适应月球环境的自主位姿确定提供了高可靠性的方案。文档编号G01C21/00GK101344391SQ20081011680公开日2009年1月14日申请日期2008年7月18日优先权日2008年7月18日发明者居鹤华,李秀智,亮王,裴福俊申请人:北京工业大学