一种月面巡视探测器的三维定姿与局部定位方法技术

一种月面巡视探测器的三维定姿与局部定位方法如下：（１）探测器处于静态时，利用三轴加速度计敏感确定滚动和俯仰角；（２）利用太阳敏感器确定偏航角姿态；（３）三轴姿态和陀螺偏差作为状态量，三轴加速度计确定的滚动、俯仰角和由太阳敏感器确定的偏航角及三个陀螺输出作为测量信息，建立状态方程和测量方程，利用扩展卡尔曼滤波估计三轴姿态和陀螺偏差；（４）探测器运动时，利用估计得到的陀螺偏差对陀螺输出进行补偿后，计算探测器的姿态变化，完成陀螺姿态预估，实现姿态更新；（５）采集探测器各驱动轮转速、转向轮转角、左右摇臂转角信息，利用正运动学关系获得探测器在本体坐标系中的位置增量。本发明专利技术的定姿和定位精度高，计算简单，工程实现容易。

Three dimensional pose determination and local positioning method for Lunar Rover

As a three-dimensional posture and the local positioning of Lunar Rover: (1) the detector is static, using three axis acceleration sensitive scrolling and pitch angle; (2) to determine the yaw attitude by sun sensor; (3) three axis attitude and gyro bias as state variables, rolling and pitching axis three the accelerometer to determine the angle and yaw angle is determined by the sun sensor and gyro output three as measurement information, the establishment of the state equation and measurement equation, using the extended Calman filter to estimate three axis attitude and gyro bias; (4) probe motion, by estimating gyro deviation obtained to compensate the gyro output, attitude change calculation the detector, complete gyro attitude estimation, realize attitude updating; (5) acquisition detector of each driving wheel speed, steering wheel angle, Zuo Youyao Arm angle information, the position increment of the detector in the body coordinate system is obtained by using the positive kinematics relation. The invention has the advantages of high positioning and positioning accuracy, simple calculation and easy engineering realization.

【技术实现步骤摘要】

本专利技术涉及一种月面巡视探测器三维定姿与局部定位方法，适用于月 面、火星表面等复杂未知地形中探测器的自主定姿定位，或地面野外车辆的 自主导4元。
技术介绍
月面巡视探测器是一类在月面实施巡视勘查的探测器，也称月球探测 车、月球车等，也可将月面巡视探测器、行星车统称为探测车。广义上讲， 月面巡视4果测器是一种能够在月球表面移动，完成探测、采样、运载等任务 的航天器。狭义上讲，月面巡视探测器是能适应月球环境，携带科学探测仪 器在月面进行巡视探测的航天器。因此，月面巡视探测器是一类特殊的航天 器，不同于传统的卫星、飞船，在着陆月面之前，月面巡视探测器是着陆器 的有效载荷，着陆后是独立的、完整的移动探测器。月面巡视探测器的定位包括全局定位和局部定位。全局定位即确定月球 探测车在月表坐标系下的绝对位置。局部定位即确定月球探测车相对于导航 坐标系(坐标系原点通常取着陆器着陆所在点)的位置描述。由于月面环境 的特殊性，地面移动机器人和自主导航车辆采用的导航方法不适用于月面巡视探测器。如月面上无法检测到GPS信号，因此GPS无法用于月面巡视探 测器的定^f立；利用星图识别进行天文导航的精度很低(几百米 几公里)； 涉及已知环境地图和外部信息(例如雷达信标和超声波导航)的地图匹配定 位和导航信标等方法也不可用，因为这些方法需要充分的环境先验知识，不 适用于在未知环境中移动的月面巡视探测器。迄今为止，国外成功发射的月球车有美国的LRV和前苏联的 Lunokhod1、 Lunokhod2。美国的LRV是有人驾驶的月球车，前苏联的Lunokhod1和Lunokhod2采用遥操作方式。国外成功的火星车是美国1997 年的Sojourner和2004年的MER (包括Spirit和Opportunity) 。 jt匕夕卜国 内外也进行了地面原理样机的研制，比较典型的有FIDO、 Rocky 7、 Marsokhod、 Nomad等，其中除Nomad采用测距仪、倾斜仪、陀螺、惯量 计和GPS等比较传统的仪器结合起来进行定位外，其他都采用了IMU、太 阳敏感器和轮系编码器三类敏感器作为主要的定姿定位信息获得手段，但系 统配置、定姿定位算法方面有所不同。除以上已得到应用的技术外，近年来公开发表的文献中对月球及火星表 面巡视探测器定姿定位方法的研究也比较多。但上述已有方法存在的缺点主要体现在(1 )利用两轴加速度计输出 求解倾斜角度，这在倾斜角度较大，或当地引力加速度较常规值发生较大变 化时，产生的误差较大；(2)利用偏航轴陀螺测量偏航角，没有考虑由地 形起伏变化导致的滚动与俯仰角度；(3)利用水平两维位置信息，或利用 左右两轮运动几何关系建立与偏航姿态的关系，利用太阳敏感器输出对位置 进行修正；这种方法虽然简单，适合于平坦地形，但模型误差大，不适合于 复杂起伏地形；(4)为了提高定姿定位精度，选取比较多的状态量作为待 估计值，如位置误差、姿态误差和惯性敏感器偏差等，建立的状态方程和测 量方程比较复杂，虽然一定程度上提高了定姿和局部定位精度，但由于计算 的复杂性，工程应用实现难度较大；(5)常用的航位推算方法只利用左右 轮的转速计算行驶里程，没有考虑地形高度变化带来的影响，该方法在复杂 地形下会导致较大误差。
技术实现思路
本专利技术的技术解决问题是克服现有技术的不足之处，提供一种月面巡 视探测器三维定姿与局部定位方法，该方法能够获得高的定姿和定位精度， 同时计算简单，工程实现容易。本专利技术的技术解决方案是 一种月面巡视探测器的三维定姿与局部定位方法，其特点在于步骤如下(1 )当月面巡视探测器处于静态时，利用三轴加速度计敏感确定滚动和俯仰角；(2) 利用太阳敏感器的输出及上述步骤(1)获得的滚动和俯仰角确定 偏航角姿态；(3) 以三轴姿态和陀螺偏差作为状态量，三轴加速度计确定的滚动、俯 仰角和由太阳敏感器确定的偏航角，及三个陀螺输出作为测量信息，建立状 态方程和测量方程，利用扩展卡尔曼滤波估计三轴姿态和陀螺偏差；(4) 当月面巡视探测器运动时，利用由步骤(3)估计得到的陀螺偏差 对陀螺输出进行补偿后，计算月面巡视探测器的姿态变化，完成陀螺姿态预 估，实现姿态更新，获得由导航坐标系到本体坐标系的姿态矩阵；(5) 采集月面巡视探测器各驱动轮转速、转向轮转角、左右摇臂转角信 息，利用移动子系统的正运动学关系获得探测器在本体坐标系中的位置增 量；(6) 将步骤(5)获得探测器在本体坐标系中的位置增量转变导航坐标系中的位置增量，求解巡视探测器在导航坐标系三个方向上的速度和位置信 自本专利技术与现有技术相比的优点在于(1 )本专利技术利用静态时建立简单的状态方程，并利用惯性敏感器测量输 出与当时姿态之间的关系建立陀螺和加速度计静态测量方程，再通过扩展 Kalman滤波技术，克服了太阳敏感器和惯性敏感器的随机噪声，从而得到 较高精度的姿态估计值，提高了定姿精度；在姿态确定的同时还完成了对陀 螺偏差的估计。动态时利用陀螺输出进行姿态预估，此时上一次的陀螺偏差 估计值用于对陀螺输出进行补偿，提高了动态时陀螺姿态预估的精度。(2)利用探测器的全运动学关系和姿态信息进行局部位置确定。 与仅采用单轮或左右两轮转速的定位方法相比，本专利技术利用基于全部驱动轮转速、转向轮转角和摇臂转角的运动学关系求解探测器在本体坐标系中的速度，降低了轮子滑移所造成的误差；此外，姿态信息的引入实现了起伏地形下的三维定位，能够获得高度信息，与一般的忽略高度变化的平面简化 处理相比，定位精度提高。(3) 本专利技术得到的位置信息可作为进一步基于视觉信息处理的地面平差 处理的基础。用于地面车辆的自主导航时，可采用电解质型或力平衡伺服型 等高精度倾角传感器得到滚动和俯仰两个水平倾角，联合高精度绝对航向敏 感器(如太阳敏感器或电子罗盘等)获得偏航角，这样可增加测量信息，且 降低了测量函数的复杂程度，便于在线应用。(4) 位置确定时采用了基于六轮转速、四轮转角以及主副摇臂转角的全 运动学关系，并引入了姿态信息，这种方法有以下三个优点(1)减小了 单独采用某一个或几个轮子转速进行定位时的滑移误差；(2)减小了滑移 的影响；(3)克服了地形高度变化带来的影响。总之，本专利技术已在月面巡视探测器原理样机中通过内场和国内首次沙漠 外场试验验证，方法可行，工程技术易实现，因此具有实用性。 附图说明图1为本专利技术月面巡视探测器三维定姿与局部定位流程图2为采用本专利技术方法获得的滚动角的滤波估计值；图3为未采用本专利技术直接测量的滚动角值；图4为采用本专利技术方法获得的俯仰角的滤波估计值；图5为未采用本专利技术直接测量的俯仰角值；图6为采用本专利技术方法获得的偏航角的滤波估计值；图7为未采用本专利技术直接测量的偏航角值；图8为采用本专利技术方法获得的滚动轴陀螺偏差估计值；图9为采用本专利技术方法获得的俯仰轴陀螺偏差估计值；图10为采用本专利技术方法获得的偏航轴陀螺偏差估计值。 具体实施例方式实施例1:定义"东-北-天"月理坐标系为导航坐标系，以3-2-1 (即先绕Z轴，再绕 Y轴，然后X轴)转序为例，定义巡视探测器本体坐标系相对导航坐标系的 欧拉角p、 A W为三轴姿态角，相对导航坐标系原点的位置为局部位置。月 面巡视探测器以六轮摇臂式为例，其中六轮驱动，本文档来自技高网...

【技术保护点】
一种月面巡视探测器的三维定姿与局部定位方法，其特征在于步骤如下：（１）当月面巡视探测器处于静态时，利用三轴加速度计敏感确定滚动和俯仰角；（２）利用太阳敏感器的输出及上述步骤（１）获得的滚动和俯仰角确定偏航角姿态；（３）以三轴姿态和陀螺偏差作为状态量，三轴加速度计确定的滚动、俯仰角和由太阳敏感器确定的偏航角，及三个陀螺输出作为测量信息，建立状态方程和测量方程，利用扩展卡尔曼滤波估计三轴姿态和陀螺偏差；（４）当月面巡视探测器运动时，利用由步骤（３）估计得到的陀螺偏差对陀螺输出进行补偿后，计算月面巡视探测器的姿态变化，完成陀螺姿态预估，实现姿态更新，获得由导航坐标系到本体坐标系的姿态矩阵；（５）采集月面巡视探测器各驱动轮转速、转向轮转角、左右摇臂转角信息，利用移动子系统的正运动学关系获得探测器在本体坐标系中的位置增量；（６）将步骤（５）获得月面探测器在本体坐标系中的位置增量转换成导航坐标系中的位置增量，求解月面巡视探测器在导航坐标系三个方向上的速度和位置信息。

【技术特征摘要】

【专利技术属性】
技术研发人员：刑琰，刘云，张志，
申请(专利权)人：北京控制工程研究所，
类型：发明
国别省市：11[中国|北京]