-
随着深空探测技术的发展，越来越多的行星探测任务要求探测器能够在复杂未知地形环境下安全着陆，这就要求探测器具有在未知环境下进行自主相对导航的能力[1-2]。
传统的深空探测软着陆任务中主要采用惯性导航为主的自主导航方式，但惯性导航误差会随时间累积，因此需要利用其它敏感器测量对惯性导航进行修正。“嫦娥三号”（Chang'E-3，CE-3）和“嫦娥四号”（Chang'E-4，CE-4）软着陆采用了基于惯性导航配以测距测速修正的导航方法[3-4]。尽管测距测速敏感器可对惯性导航的高度和速度进行修正，但是其不能抑制导航系统水平位置误差的发散，可能导致着陆器偏离目标着陆点。
针对惯性导航配以测距测速修正的导航方法无法估计水平位置的问题，一种最为可行的解决方法是引入行星表面陆标图像信息进行自主导航。目前采用的陆标图像信息的地外天体着陆自主导航方法的研究大都是在已有着陆区域参考地形图的假设下进行的，通过在下降的过程中，将图像中提取的陆标与包含陆标位置信息的参考地形图进行匹配获得下降图像中导航陆标的位置信息，进而利用这些陆标的位置信息结合其图像测量信息得到探测器的导航信息[5]。然而，当事先没有着陆区域参考地形图或由于参考地形图与下降图像的分辨率差异过大导致无法进行特征匹配时，下降序列图像中观测和跟踪到的陆标位置信息将无法获取，需要解决在未知环境下利用未知位置信息的陆标进行导航的问题。
针对未知环境下的行星着陆导航问题，美国国家航空航天局（National Aeronautics and Space Administration，NASA）为“火星探测漫游者”（Mars Exploration Rover，MER）任务研究了下降图像运动估计子系统（Descent Image Motion Estimation System，DIMES），该系统通过跟踪3幅下降序列图像中的未知位置信息的特征点结合高度计测高信息来估计探测器相对着陆面的水平速度，通过实际的火星着陆任务验证了该系统的有效性[6-7]。然而，DIMES系统仅能估计探测器相对着陆面的水平速度信息。文献[8~9]利用同时定位与地图创建方法（Simultaneous Localization and Mapping，SLAM），将陆标位置作为系统状态进行估计从而实现飞行器在未知环境下的自主导航，但是SLAM方法计算量大不利于实时计算；文献[10~11]给出一种多状态约束的未知环境下视觉辅助惯性导航的自主导航方法；文献[12]给出一种利用未知陆标在连续2幅下降图像中的测量建立隐式测量模型对着陆器状态进行估计的方法，但该方法只适用于着陆面近似为平面的情况。上述方法虽然都避免了需要陆标位置的先验信息，但这些导航方法系统状态都不完全可观，因而探测器某些状态在着陆过程中并不能得到精确估计。
针对未知环境下行星表面导航问题，本文提出一种利用未知陆标序列图像测量的行星表面相对导航方法。该方法可有效估计探测器相对行星表面的速度，较为精确地估计探测器相对于着陆点或陆标的位置，从而可满足精确避障着陆的导航要求。以火星着陆为例的数学仿真表明该方法是有效可行的。
-
以初始时刻星下点天南东系为着陆参考坐标系，假设惯性敏感器安装和零偏等偏差已经通过标定消除，探测器相对着陆坐标系的姿态可通过陀螺和星敏等敏感器精确确定，则利用加速度计测量建立探测器着陆运动学方程[12]
$$\left\{ \begin{array}{l} {}^{{L}}{\dot{{r}}} = {}^{{L}}{{v}} \\ {}^{{L}}{\dot{{v}}} = {{{C}}^{\rm T}}{{{a}}_{imu}} - 2\left\lfloor {{}^{{L}}{{{\omega }}_G} \times } \right\rfloor {}^{{L}}{{v}} - {\left\lfloor {{}^{{L}}{{{\omega }}_G} \times } \right\rfloor ^2}{}^{{L}}{{r}} + {}^{{L}}{{g}} + {{{n}}_g} \end{array} \right.$$ (1) 其中：
${}^{{L}}{{r}}$ 和${}^{{L}}{{v}}$ 分别为探测器在着陆坐标系下的位置和速度；${{C}}$ 为着陆坐标系到探测器本体系转换的方向余弦阵；${}^{{L}}{{{\omega }}_G}$ 为目标天体转动角速度在着陆坐标系下的表示；${}^{{L}}{{g}}$ 为当地引力加速度，对于大天体设为常值，${{{n}}_g}$ 为引力加速度扰动；对于任一角速度${{\omega }} = {\left[ {\begin{array}{*{20}{c}} {{\omega _x}}\!\!\!&\!\!\!\!{{\omega _y}}\!\!\!&\!\!\!\!{{\omega _z}} \end{array}} \right]^{\rm{T}}}$ ，$\left\lfloor {{ \cdot ^ \times }} \right\rfloor $ 定义为$$\left\lfloor {{{{\omega }}^ \times }} \right\rfloor = \left[ {\begin{array}{*{20}{c}} 0&{ - {\omega _z}}&{{\omega _y}} \\ {{\omega _z}}&0&{ - {\omega _x}} \\ { - {\omega _y}}&{{\omega _x}}&0 \end{array}} \right]$$ 加速度计测量模型为
$${{{a}}_{imu}} = {{C}}({}^{{L}}{{a}} - {}^{{L}}{{g}} + 2\left\lfloor {{}^{{L}}{{{\omega }}_G} \times } \right\rfloor {}^{{L}}{{v}} + {\left\lfloor {{}^{{L}}{{{\omega }}_G} \times } \right\rfloor ^2}{}^{{L}}{{r}}) + {{{n}}_a}$$ (2) 其中：
${}^{{L}}{{a}}$ 为除引力外所有作用于探测器上力的合力产生的加速度；${{n}}_a^{}$ 为加速度计测量噪声。 -
隐式测量模型利用陆标序列图像测量信息结合探测器估计位姿构建隐式测量方程，该测量模型不需要陆标位置先验信息。
假设陆标
${p_j}$ 在连续3幅下降图像中均被观测到，如图1所示。${{y}}_i^j$ 与$l_i^j$ 分别表示${t_i}$ 时刻相机坐标系下陆标${p_j}$ 的视线矢量和比例参数，其中$\left\| {l_i^j{{y}}_i^j} \right\|$ 表示${t_i}$ 时刻相机到陆标${p_j}$ 的距离，则陆标${p_j}$ 在着陆坐标系下的位置可表示为
图 1 未知陆标序列图像测量示意
Figure 1. Schematic diagram of a unknown landmark observed in three sequential images
$$\begin{aligned}[b] {}^{{L}}{{{p}}_j} =& {}^{{L}}{{{r}}_i} + l_i^j{{C}}_i^{\rm T}{{y}}_i^j =\\ \;\;\;\;& {}^{{L}}{{{r}}_{i + 1}} + l_{i + 1}^j{{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j \end{aligned}$$ (3) 令
$$\left[ {\begin{array}{*{20}{c}} {a_i^j} \\ {b_i^j} \\ {c_i^j} \end{array}} \right] = {{C}}_i^{\rm{T}}{{y}}_i^j, \;\;\;\;\; \left[ {\begin{array}{*{20}{c}} {a_{i + 1}^j} \\ {b_{i + 1}^j} \\ {c_{i + 1}^j} \end{array}} \right] = {{C}}_{i + 1}^{\rm{T}}{{y}}_{i + 1}^j, \;\;\;\;\; {{M}}_{i,j}^{} = \left[ {\begin{array}{*{20}{c}} {a_i^j}&{ - a_{i + 1}^j} \\ {b_i^j}&{ - b_{i + 1}^j} \\ {c_i^j}&{ - c_{i + 1}^j} \end{array}} \right]$$ 则有
$${{M}}_{i,j}^{}\left[ {\begin{array}{*{20}{c}} {l_i^j} \\ {l_{i + 1}^j} \end{array}} \right] = {}^{{L}}{{{r}}_{i + 1}} - {}^{{L}}{{{r}}_i}$$ (4) 由式（4）可求解得到
${t_i}$ 和${t_j}$ 时刻相机到陆标${p_j}$ 的距离$$\begin{array}{l} \left[ {\begin{array}{*{20}{c}} {l_i^j} \\ {l_{i + 1}^j} \end{array}} \right] = {({{M}}_{i,j}^{\rm T}{{M}}_{i,j}^{})^{ - 1}}{{M}}_{i,j}^{\rm T}({}^{{L}}{{{r}}_{i + 1}} - {}^{{L}}{{{r}}_i}) = \\ \dfrac{1}{{1 - {{[{{({{C}}_i^{\rm T}{{y}}_i^j)}^{\rm T}}{{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j]}^2}}} \times \\ \left[ {\begin{array}{*{20}{l}} {{{({{C}}_i^{\rm T}{{y}}_i^j)}^{\rm T}} - [{{({{C}}_i^{\rm T}{{y}}_i^j)}^{\rm T}}{{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j]{{({{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j)}^{\rm T}}} \\ {{{({{C}}_i^T{{y}}_i^j)}^{\rm T}}[{{({{C}}_i^{\rm T}{{y}}_i^j)}^{\rm T}}{{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j] - {{({{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j)}^{\rm T}}} \end{array}} \right]({}^{{L}}{{{r}}_{i + 1}} - {}^{{L}}{{{r}}_i}) \end{array} $$ (5) 根据式（3）和式（5）可计算
${t_{i + 1}}$ 时刻陆标${p_j}$ 的位置为$$\begin{aligned}[b] &{}^{{L}}{{{p}}_j} = {}^{{L}}{{{r}}_{i + 1}} +\\ &\frac{{[{{({{C}}_i^{\rm T}{{y}}_i^j)}^{\rm T}}[{{({{C}}_i^{\rm T}{{y}}_i^j)}^{\rm T}}{{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j] - {{({{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j)}^{\rm T}}]({}^{{L}}{{{r}}_{i + 1}} - {}^{{L}}{{{r}}_i})}}{{1 - {{[{{({{C}}_i^{\rm T}{{y}}_i^j)}^{\rm T}}{{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j]}^2}}}{{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j \end{aligned}$$ (6) 则
${t_{i + 2}}$ 时刻陆标${p_j}$ 在相机系下的视线矢量为$${{y}}_{i + 2}^j = \frac{{{{z}}_{i + 2}^j}}{{\left\| {{{z}}_{i + 2}^j} \right\|}} + {{n}}_{i + 2}^j$$ (7) 其中：
${{n}}_{i + 2}^j$ 为观测噪声，设其协方差阵为${{{R}}_n}$ ；${{z}}_{i + 2}^j$ 为相机系下相机到陆标的矢量，计算如下$$\begin{aligned}[b] &{{z}}_{i + 2}^j = \left[ {\begin{array}{*{20}{c}} {x_{i + 2}^j} \\ {y_{i + 2}^j} \\ {z_{i + 2}^j} \end{array}} \right] ={{{C}}_{i + 2}}\Bigg\{ {}^{{L}}{{{r}}_{i + 1}} + \\ & \frac{{\Bigg\{ {{({{C}}_i^{\rm T}{{y}}_i^j )}^{\rm T}}\Bigg[{{({{C}}_i^{\rm T}{{y}}_i^j)}^{\rm T}}{{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j \Bigg] - {{({{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j)}^{\rm T}} \Bigg\} ({}^{{L}}{{{r}}_{i + 1}} - {}^{{L}}{{{r}}_i})}} {{1 - {{[{{({{C}}_i^{\rm T}{{y}}_i^j)}^{\rm T}}{{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j ]}^2}}} \times \\ & {{C}}_{i + 1}^{\rm T}{{y}}_{i + 1}^j - {}^{{L}}{{{r}}_{i + 2}} \Bigg\}\end{aligned}$$ (8) 由式（7）和式（8）可看出，对任意位置未知的陆标，只要在3幅下降图像中观测到该陆标，即可建立该陆标的隐式测量方程用于探测器的状态估计。
-
由（7）和式（8）可知，未知陆标的隐式测量模型同时与探测器在3个时刻的状态相关，故基于未知陆标图像隐式测量模型的自主导航系统状态向量取为
$${{x}} = {\left[ {\begin{array}{*{20}{c}} {{}^{{L}}{{{r}}^{\rm T}}}&{{}^{{L}}{{{v}}^{\rm T}}}&{{}^{{L}}{{r}}_{i + 1}^{\rm T}}&{{}^{{L}}{{r}}_i^{\rm T}} \end{array}} \right]^{\rm T}}$$ (9) 其中：
${}^{{L}}{{r}}_{i + 1}^{\rm T}$ 和${}^{{L}}{{r}}_i^{\rm T}$ 分别为前两次成像${t_{i + 1}}$ 和${t_i}$ 时刻着陆器的位置，其不随时间变化。当相机拍摄一幅新图像后系统状态按如下方法确定
$${{{x}}_{i{\rm{ + 2}}}} = {{M}}{{{x}}_{i{\rm{ + 1}}}}$$ (10) 其中：
${{{x}}_{i{\rm{ + 1}}}}$ 和${{{x}}_{i{\rm{ + 2}}}}$ 分别为成像时刻${t_{i + 1}}$ 和${t_{i{\rm{ + 2}}}}$ 之后的系统状态，M为$${{M}} = \left[ {\begin{array}{*{20}{c}} {{{{I}}_3}}&{{{{0}}_3}}&{{{{0}}_3}}&{{{{0}}_3}} \\ {{{{0}}_3}}&{{{{I}}_3}}&{{{{0}}_3}}&{{{{0}}_3}} \\ {{{{I}}_3}}&{{{{0}}_3}}&{{{{0}}_3}}&{{{{0}}_3}} \\ {{{{0}}_3}}&{{{{0}}_3}}&{{{{I}}_3}}&{{{{0}}_3}} \end{array}} \right]$$ 根据式（9）定义的探测器状态，可得探测器误差状态为
$$\tilde{ {{x}}} = \left[ {\begin{array}{*{20}{c}} {{}^{{L}}{{\tilde{ {{r}}}}^{{T}}}}& {{}^{{L}}{{{\tilde {{v}}}}^{\rm{T}}}}& {{}^{{L}} \tilde{ {{r}}}_{i + 1}^{\rm{T}}}& {{}^{{L}} \tilde{ {{r}}}_i^T{\rm{}}} \end{array}} \right]$$ (11) 其中：对于位置、速度、惯性测量和加速度变化偏差有
${}^{{L}}{\tilde{{r}}} = {}^{{L}}{{r}} - {}^{{L}}{\hat{{r}}}$ ，${}^{{L}}{\tilde{{v}}} = {}^{{L}}{{v}} - {}^{{L}}{\hat{{v}}}$ 。线性化式（1）得误差状态方程
$${\dot {\tilde{{ x}}}} = {{F}}{\tilde{{x}}} + {{Gn}}$$ (12) 其中：
${{n}} = {[\begin{array}{*{20}{c}} {{{n}}_a^{\rm T}}&{{{n}}_g^{\rm T}} \end{array}]^T}$ 为系统噪声，假设系统噪声各分量为互不相关的零均值高斯白噪声，设协方差为${{Q}}$ ；${{F}}$ 、${{G}}$ 为系统状态雅克比矩阵$$\begin{array}{l} {{F}} = \left[ {\begin{array}{*{20}{c}} {{{{0}}_{3 \times 3}}}&{{{{I}}_3}}&{{{{0}}_{3 \times 6}}} \\ { - {{\left\lfloor {{{{\omega }}_L} \times } \right\rfloor }^2}}&{ - 2\left\lfloor {{{{\omega }}_L} \times } \right\rfloor }&{{{{0}}_{3 \times 6}}} \\ {{{{0}}_{6 \times 6}}}&{{{{0}}_{6 \times 6}}}&{{{{0}}_{6 \times 6}}} \end{array}} \right] \\ {{G}} = \left[ {\begin{array}{*{20}{c}} {{{{0}}_{3 \times 3}}}&{{{{0}}_{3 \times 3}}} \\ { - {{{C}}^T}}&{{{{I}}_3}} \\ {{{{0}}_{6 \times 3}}}&{{{{0}}_{6 \times 3}}} \end{array}} \right] \end{array} $$ -
由第2部分可得，
${t_{i + 2}}$ 时刻导航陆标${p_j}$ 估计观测量为$${\hat{{y}}}_{i + 2}^j = \frac{{{\hat{{z}}}_{i + 2}^j}}{{\left\| {{\hat{{z}}}_{i + 2}^j} \right\|}}$$ (13) 其中
$$\begin{aligned} & {\hat{{z}}}_{i + 2}^j = {{{C}}_{i + 2}}\Bigg\{ {}^{{L}}{{\hat{{r}}}_{i + 1}} +\\ & \frac{{\Bigg\{ {{({{C}}_i^{\rm {\rm T}}{\hat{{y}}}_i^j)}^{\rm T}} \Bigg[{{({{C}}_i^{\rm T}{\hat{{y}}}_i^j)}^{\rm T}}{{C}}_{i + 1}^{\rm T}{\hat{{y}}}_{i + 1}^j\Bigg] - {{({{C}}_{i + 1}^{\rm {\rm T}}{\hat{{y}}}_{i + 1}^j)}^{\rm T}}\Bigg\}({}^{{L}}{{{\hat{{r}}}}_{i + 1}} - {}^{{L}}{{{\hat{{r}}}}_i})}}{{1 - {{({{({{C}}_i^{\rm T}{\hat{{y}}}_i^j)}^{\rm T}}{{C}}_{i + 1}^{\rm T}{\hat{{y}}}_{i + 1}^j)}^2}}}\times \\ & {{C}}_{i + 1}^{\rm T}{\hat{{y}}}_{i + 1}^j - {}^{{L}}{{\hat{{r}}}_{i + 2}} \Bigg\} \end{aligned}$$ 则
${t_{i{\rm{ + 2}}}}$ 时刻观测量残差$$\begin{aligned}[b] & \Delta {{y}}_{i + 2}^j = {{y}}_{i + 2}^j - {\hat{{y}}}_{i + 2}^j \simeq\\ & {{H}}_{{r_{i + 2}}}^j{}^{{L}}{{{\tilde{{r}}}}_{i{\rm{ + 2}}}}{\rm{ + }}{{H}}_{{r_{i + 1}}}^j{}^{{L}}{{{\tilde{{r}}}}_{i{\rm{ + }}1}}{\rm{ + }}{{H}}_{{r_i}}^j{}^{{L}}{{{\tilde{{r}}}}_i}{\rm{ + }}{{H}}_{{y_{i + 1}}}^j{{n}}_{i + 1}^j{\rm{ + }}{{H}}_{{y_i}}^j{{n}}_i^j + {{n}}_{i + 2}^j \end{aligned}$$ (14) 其中
$$\begin{array}{l} {{{H}}_C} = \dfrac{1}{{\left\| {{\hat{{z}}}_{i + 2}^j} \right\|}}{{{I}}_3} - \dfrac{{{\hat{{z}}}_{i + 2}^j{{({\hat{{z}}}_{i + 2}^j)}^{\rm{T}}}}}{{{{\left\| {{\hat{{z}}}_{i + 2}^j} \right\|}^3}}} \\ {{H}}_{{r_{i + 2}}}^j = - {{{H}}_C}{{{C}}_{i + 2}} \\ {{H}}_{{r_{i + 1}}}^j = {{{H}}_C}{{{C}}_{i + 2}} \Bigg\{ {{I}} + \dfrac{{{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j\Bigg\{ {{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}} \Bigg[{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg] - {{({{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j)}^{\rm{T}}} \Bigg\}}}{{1 - {{[{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j]}^2}}} \Bigg\} \\ {{H}}_{{r_i}}^j = - {{{H}}_C}{{{C}}_{i + 2}} \dfrac{{{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg\{ {{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}\Bigg[{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg] - {{({{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j)}^{\rm{T}}} \Bigg\} }}{{1 - {{\Bigg[ {{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg]}^2}}} \\ {{H}}_{{y_{i + 1}}}^j = {{{H}}_C}{{{C}}_{i + 2}} \Bigg\{ \Bigg( {\dfrac{{{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}({}^{{L}}{{{\hat{{r}}}}_{i + 1}} - {}^{{L}}{{{\hat{{r}}}}_i}) \Bigg\{ 1 + {{\Bigg[{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg]}^2} \Bigg\} - 2 \Bigg[{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg]{{({{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j)}^{\rm{T}}}({}^{{L}}{{{\hat{{r}}}}_{i + 1}} - {}^{{L}}{{{\hat{{r}}}}_i})}}{{{{(1 - {{({{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j)}^2})}^2}}}} \Bigg){{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)^{\rm{T}}} + \\ \Bigg( {\dfrac{{{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}({}^{{L}}{{{\hat{{r}}}}_{i + 1}} - {}^{{L}}{{{\hat{{r}}}}_i})\Bigg[ {{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg] - {{({{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j)}^{\rm{T}}}({}^{{L}}{{{\hat{{r}}}}_{i + 1}} - {}^{{L}}{{{\hat{{r}}}}_i})}}{{1 - {{\Bigg[ {{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg]}^2}}}} \Bigg){{{I}}_3} - \\ \Bigg( {\dfrac{1}{{1 - {{ \Bigg[ {{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg]}^2}}}} \Bigg){{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j{({}^{{L}}{{{\hat{{r}}}}_{i + 1}} - {}^{{L}}{{{\hat{{r}}}}_i})^{\rm{T}}} \Bigg\} \\ {{H}}_{{y_i}}^j = \dfrac{{{{{H}}_C}{{{C}}_{i + 2}}}}{{{{\Bigg\{1 - {{\Bigg[{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg]}^2}\Bigg\}}^2}}} \cdot \\ \Bigg\{ \Bigg\{ {{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}({}^{{L}}{{{\hat{{r}}}}_{i + 1}} - {}^{{L}}{{{\hat{{r}}}}_i}) \Bigg\{1 + {{ \Bigg[{{({{C}}_i^T{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg]}^2} \Bigg\} - 2 \Bigg[{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg] {{({{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j)}^{\rm{T}}}({}^{{L}}{{{\hat{{r}}}}_{i + 1}} - {}^{{L}}{{{\hat{{r}}}}_i})} \Bigg\} {{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j{({{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j)^{\rm{T}}} + \\ \Bigg\{ {{{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg\{ 1 - {{ \Bigg[ {{({{C}}_i^{\rm{T}}{\hat{{y}}}_i^j)}^{\rm{T}}}{{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j \Bigg] }^2}\Bigg\} } \Bigg\} {{C}}_{i + 1}^{\rm{T}}{\hat{{y}}}_{i + 1}^j{({}^{{L}}{{{\hat{{r}}}}_{i + 1}} - {}^{{L}}{{{\hat{{r}}}}_i})^{\rm{T}}} \Bigg\} \end{array} $$ 根据状态误差定义，式（18）可写为
$$\Delta {{y}}_{i + 2}^j = {{H}}_{i + 2}^j{{\tilde{{x}}}_{i + 2}} + {{\eta }}_{i + 2}^j$$ (15) 其中
$${{H}}_{i + 2}^j = \left[ {\begin{array}{*{20}{c}} {{{H}}_{{r_{i + 2}}}^j}&{{{{0}}_{3 \times 3}}}&{{{H}}_{{r_{i + 1}}}^j}&{{{H}}_{{r_i}}^j} \end{array}} \right]$$ (16) $${{\eta }}_{i + 2}^j = {{H}}_{{y_{i + 1}}}^j{{n}}_{i + 1}^j + {{H}}_{{y_i}}^j{{n}}_i^j + {{n}}_{i + 2}^j$$ (17) 设陆标方向矢量测量噪声为互不相关的高斯白噪声，协方差为
${{{R}}_n}$ ，则陆标隐式测量模型误差${{\eta }}_{i + 2}^j$ 均值和协方差为$$\begin{array}{l} \left\{ {\begin{array}{*{20}{l}} {{\rm E}[{{\eta }}_{i + 2}^j] = {{{0}}_{3 \times 1}}} \\ {{R}}_{i + 2}^j = {\rm{E}}\left[ {{{\eta }}_{i + 2}^j{{({{\eta }}_{i + 2}^j)}^{\rm T}}} \right] = {{H}}_{{y_{i + 1}}}^j{{{R}}_{{n}}}{{({{H}}_{{y_{i + 1}}}^j)}^{\rm T}} + \\ {{H}}_{{y_i}}^j{{{R}}_{{n}}}{{({{H}}_{{y_i}}^j)}^{\rm T}} + {{{R}}_{{n}}} \end{array}} \right. \end{array} $$ (18) -
采用EKF作为滤波算法对探测器状态进行估计。当惯性敏感器得到加速度测量数据时，系统状态可按式（1）进行递推，同时系统状态误差协方差可由下式积分得到
$${\dot{{ P}}} = {{FP}} + {{P}}{{{F}}^{\rm T}} + {{GQG}}$$ (19) 由于导航系统每获得一幅图像时均对系统状态按式（10）重置，故系统状态误差协方差
${{P}}$ 在每次成像时刻时按式（20）更新$${{P}} = {{MP}}{{{M}}^{\rm{T}}}$$ (20) 假设有N个未知陆标同时在
${t_i}$ 、${t_{i + 1}}$ 和${t_{i + 2}}$ 时刻拍摄的下降图像中被检测到，则可得观测残差向量$$\Delta {{y}}_{i + 2}^{} = {{H}}_{i + 2}^{}{{\tilde{{x}}}_{i + 2}} + {{\eta }}_{i + 2}^{}$$ 其中：
$\Delta {{y}}_{i + 2}^{}$ ，${{H}}_{i + 2}^{}$ 和${{\eta }}_{i + 2}^{}$ 分别由$\Delta {{y}}_{i + 2}^j$ ，${{H}}_{i + 2}^j$ ，和${{\eta }}_{i + 2}^j$ 组成。假设测量误差项${{\eta }}_{i + 2}^{}$ 为互不相关的零均值白噪声，其协方差为${{{R}}_{i + 2}} = \rm blkdiag({{R}}_{i + 2}^1, \cdots ,{{R}}_{i + 2}^N)$ 。卡尔曼滤波增益计算为
$${{K}} = {{{P}}_{i + 2|i + 1}}{{H}}_{i + 2}^{\rm{T}}{({{{H}}_{i + 2}}{{{P}}_{i + 2|i + 1}}{{H}}_{i + 2}^{\rm{T}} + {{{R}}_{i + 2}})^{ - 1}}$$ (21) 导航系统状态和协方差更新为
$${{\hat{{ x}}}_{i + 2}} = {{\hat{{ x}}}_{i + 2\left| {i + 1} \right.}} + {{K}}\Delta {{y}}_{i + 2}^{}$$ (22) $${{{P}}_{i + 2}} = ({{I}} - {{K}}{{{H}}_{i + 2}}){{{P}}_{i + 2|i + 1}}{({{I}} - {{K}}{{{H}}_{i + 2}})^{\rm T}} + {{K}}{{{R}}_{i + 2}}{{{K}}^{\rm T}}$$ (23) -
为了验证本文提出的导航方案，以火星着陆为例进行仿真，参考“好奇号”（Curiosity）火星着陆探测器的轨道数据[13]以及降落伞和主发动机的减速能力，设置仿真初始条件：探测器在初始时刻星下点天南东系下位置和速度如表1所示。
表 1 探测器仿真初始状态
Table 1. Initial state parameters
初始状态参数 值 位置/m 2 000, 0, 0 速度/（m·s–1） –92, –15, 5 假设探测器在着陆过程中始终保持相机指向着陆点方向。仿真初始条件为位置各方向存在1 000 m的随机误差，速度各方向存在10 m/s随机误差。设陆标方向矢量测量噪声为0.1＇，当没有陆标图像测量可用于导航更新时探测器状态根据惯性测量单元（Inertial Measurement Unit，IMU）测量积分得到。仿真采用模拟陆标进行，并在着陆过程拍摄的第一幅下降图像中选择安全着陆点，为了在下降图像中持续跟踪安全着陆点，需要在着陆点附近选取4个特征点且4个特征点中任意3个均不共线，从而根据射影几何交比不变定理利用选定的4个特征点在前后图像中的位置可实现对安全着陆点的连续跟踪，因此仿真中以选定的4个特征点及安全着陆点作为导航陆标进行仿真。需要注意的是，除着陆点外，每连续成像3次后根据相机视场重新选取4个特征点作为导航陆标，以保证可以在下降序列图像中存在可用的导航陆标。
图2和图3给出了探测器着陆轨迹的位置和速度变化，图4和图5给出了探测器着陆过程初始时刻星下点天南东系下位置速度的估计误差。从图4和图5中可看出，采用所提出的导航方法可以对探测器的速度进行精确估计，精度可优于0.1 m/s，但无法对位置信息进行修正，只能将位置误差维持在初始误差附近。值得注意的是，在未知环境着陆过程中，通常更关注探测器相对于着陆点的位置及速度信息。由于着陆点或障碍物在行星表面静止不动，因此探测器相对于着陆点的速度与着陆参考系下的速度相同，由图5可知其可被精确估计。探测器相对于着陆点的位置可通过估计的探测器位置以及着陆点在连续3幅下降图像中的视线矢量测量计算得到，如图6和图7所示，可见利用所提出的导航方法可有效估计探测相对于着陆点的位置，精度可优于1.5 m，满足精确避障着陆的要求。
图 2 探测器位置
Figure 2. The position in up-south-east frame during landing
图 3 探测器速度
Figure 3. The velocity in up-south-east frame during landing
图 4 探测器位置估计误差
Figure 4. The position estimation errors in up-south-east frame during landing
图 5 探测器速度估计误差
Figure 5. The velocity estimation errors in up-south-east frame during landing
图 6 探测器相对于着陆点位置
Figure 6. The relative position from the lander to the target landing site in up-south-east frame during landing
图 7 探测器相对于着陆点位置估计误差
Figure 7. The relative position estimation errors from the lander to the target landing site.in up-south-east frame during landing
-
本文研究了一种利用未知陆标序列图像测量的行星表面相对导航方法。该方法首先利用未知陆标在连续3幅下降图像中测量得到的视线方向矢量结合探测器成像时刻位置建立了未知陆标的隐式测量模型，然后利用建立的未知陆标隐式测量模型通过卡尔曼滤波对探测器的位置、速度进行估计，最后利用估计的探测器位置结合着陆点的视线方向测量计算着陆点位置，进而获得探测器相对于着陆点的位置估计。仿真结果表明，提出的方法可有效估计探测器相对行星表面的速度，虽然无法直接估计探测器的位置，但可较为精确的估计出探测器相对于着陆点或陆标的位置，从而可满足精确避障着陆的导航要求。
Autonomous Relative Navigation Based on Sequential Image Measurements of Unknown Landmarks
-
摘要: 针对未知环境下行星表面导航问题，提出一种利用未知陆标序列图像测量的行星表面相对导航方法。该方法以初始时刻星下点天南东系为着陆参考坐标系，利用行星表面未知陆标在连续3幅下降图像中的观测信息及探测器状态估计建立未知陆标的隐式测量模型，通过扩展卡尔曼滤波（Extended Kalman Filter，EKF）所建立的未知陆标隐式测量模型和基于惯性测量单元（Inertial Measurement Unit，IMU）测量建立的运动学模型对探测器的相对运动状态进行估计。以火星着陆为例进行数学仿真，仿真结果表明提出的方法可有效估计探测器相对行星表面的速度以及相对于着陆点的位置，能够满足行星安全着陆自主相对导航的要求。Abstract: An autonomous relative navigation method based on sequential image measurements of unknown landmarks for planetary landing in unknown environment is presented. In the proposed method, the line-of-sight vectors of landmarks with unknown 3D positions tracked in three sequence images and position information derived from inertial measurement unit at the time instances of the three images are used to develop implicit measurement constraints. An implicit extended Kalman filter tightly integrates the constraints with the measurements from IMU to estimate the lander's position and velocity relative to the landing site. Numerical simulations of Mars landing indicate the proposed navigation method is efficient and meets the relative navigation requirement of safe planetary landing.Highlights
● An autonomous relative navigation method in unknown environment is presented. ● The proposed method only needs an IMU and a monocular camera to estimate the position and velocity relative to the landing site without using rangefinder and Doppler velocimetry. ● An implicit measurement mode based on optical measurements of landmarks with unknown 3D positions tracked in three sequence images is established，and it can be applied to non-flat terrain. -
图 1 未知陆标序列图像测量示意
Fig. 1 Schematic diagram of a unknown landmark observed in three sequential images
图 4 探测器位置估计误差
Fig. 4 The position estimation errors in up-south-east frame during landing
图 5 探测器速度估计误差
Fig. 5 The velocity estimation errors in up-south-east frame during landing
图 6 探测器相对于着陆点位置
Fig. 6 The relative position from the lander to the target landing site in up-south-east frame during landing
图 7 探测器相对于着陆点位置估计误差
Fig. 7 The relative position estimation errors from the lander to the target landing site.in up-south-east frame during landing
表 1 探测器仿真初始状态
Table 1 Initial state parameters
初始状态参数 值 位置/m 2 000, 0, 0 速度/（m·s–1） –92, –15, 5 
下载: 导出CSV
-
[1] 崔平远,秦同,朱圣英. 火星动力下降自主导航与制导技术研究进展[J]. 宇航学报,2020,41(1):1-9.CUI P Y,QIN T,ZHU S Y. Progress in autonomous navigation and guidance technology for mars powered descent landing[J]. Journal of Astronautics,2020,41(1):1-9. [2] 崔平远,高艾,于正湜. 火星着陆自主导航方案研究进展[J]. 深空探测学报(中英文),2014,1(1):18-27.CUI P Y,GAO A,YU Z S. Research progress of autonomous navigation scheme for Mars landing[J]. Journal of Deep Space Exploration,2014,1(1):18-27. [3] 王大铁,李骥,黄翔宇,等. 月球软着陆过程高精度自主导航避障方法[J]. 深空探测学报(中英文),2014,1(1):44-51.WANG D Y,LI J,HUANG X Y,et al. A pinpoint autonomous for navigation and hazard avoidance method lunar soft landing[J]. Journal of Deep Space Exploration,2014,1(1):44-51. [4] 张洪华,关轶峰,程铭,等. 嫦娥四号着陆器制导导航与控制系统[J]. 中国科学:技术科学,2019,49(12):1418-1428.ZHANG H H,GUAN Y F,CHENG M,et al. Guidance navigation and control for Chang’E-4 lander[J]. Scientia Sinica Technologica,2019,49(12):1418-1428. [5] 徐超,王大轶,黄翔宇. 基于陆标图像的火星精确着陆自主导航方法研究[J]. 深空探测学报(中英文),2016,3(2):150-155.XU C,WANG D Y,HUANG X Y. Autonomous navigation for Mars pin-point landing based on landmark image[J]. Journal of Deep Space Exploration,2016,3(2):150-155. [6] JOHNSON A, WILLSON R, CHENG Y, et al. Design through operation of an image-based velocity estimation system for Mars landing[J]. International Journal of Computer Vision. 2007, 74（3）: 319–341. [7] JOHNSON A, WILLSON R, CHENG Y, et al. Field testing of the Mars exploration rovers descent image motion estimation system[C]//IEEE International Conference on Robotics and Automation. Barcelona, Spain: IEEE, 2005. [8] KIM J, SUKKARIEH S. Autonomous airborne navigation in unknown terrain environments[J]. IEEE Transactions on Aerospace and Electronic Systems, 40（3）: 1031-1045, 2004. [9] CERIANI S,MARZORATI D,MATTEUCCI M,et al. Single and multi camera simultaneous localization and mapping using the extended Kalman filter on the different parameterizations for 3D point features[J]. Journal of Mathematical Modelling and Algorithm in Operations Research,2014,13(1):23-57. doi: 10.1007/s10852-013-9219-7 [10] MOURIKIS A I, ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation[C]//IEEE International Conference on Robotics and Automation. Roma, Italy: IEEE, 2007. [11] MOURIKIS A I,TRAWNY N,ROUMELIOTIS S I,et al. Vision-aided inertial navigation for spacecraft entry,descent,and landing[J]. IEEE Transactions on Robotics,2009,25(2):264-280. doi: 10.1109/TRO.2009.2012342 [12] XU C,WANG D Y,HUANG X Y. Autonomous navigation based on sequential images for planetary landing in unknown environments[J]. Journal of Guidance,Control,and Dynamics,2017,40(10):2587-2602. [13] SAN M M,MENDECK G F,BRUGAROLAS P B,et al. In-flight experience of the Mars science laboratory guidance,navigation,and control system for entry,descent,and landing[J]. CEAS Space Journal,2015,7(2):119-142. doi: 10.1007/s12567-015-0091-3 -
点击查看大图
计量
- 文章访问数: 582
- HTML全文浏览量: 187
- PDF下载量: 54
- 被引次数: 0
