[发明专利]基于全功能太阳罗盘的月球车位姿自主确定方法

权利要求书

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)。