CN106125026B - 一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法 - Google Patents

Abstract

本发明提供的是一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法。利用地磁场的模量不变性，由不同姿态下的地磁场矢量测量数据对三轴磁强计的全误差参数进行辨识，以此构造误差校正器，利用构建的误差校正器校正三轴磁强计的全误差，获得校正后的被测磁场。解决现有技术在没有地磁场场量的情况下无法辨识三轴磁强计全误差参数，进而不能进行三轴磁强计全误差校正的问题；同时本发明不需要附加的地磁场测量设备，也无需附加的姿态测量设备提供准确的外部姿态信息。该发明一步辨识出全部的三轴磁强计误差，辨识精度高，不需要测量地磁场及姿态的辅助设备，简化了三轴磁强计的校正装置，降低了磁场测量系统的成本。

Description

一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校 正方法

技术领域

本发明涉及的是一种地磁场测量方法，具体地说是一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法。

背景技术

地磁场是地球的固有物理场，与人类生活息息相关，在地球科学、航空航天、资源勘探及地震预报等领域有着重要的应用。三轴磁强计是测量地磁场三分量及其模量的重要仪器。由于制造技术和安装工艺的不完善等原因，三轴磁强计存在三个测量轴的非正交、轴间灵敏度偏差及三个测量轴的零偏误差；这些仪器误差不仅影响被测磁场三分量的测量精度，也会影响被测磁场模量的计算。为了精确地测量磁场三分量及其模量，一般要估计出磁强计的上述误差参数。

Time 提出了基于最小二乘椭球拟合的三轴磁场传感器自动标定方案，标定算法能自适应地更新标定参数，但在没有外部地磁场信息时，最小二乘椭球拟合无法辨识出三轴传感器的仪器误差(Time Automatic and adaptivecalibration of 3D field sensors，Applied Mathematical Modelling，2008，32(4)：575–287)。John L.Crassidis等人提出了一种基于卡尔曼滤波框架的三轴磁强计在线标定方法，这种方法使用的外部观测量是地磁场标量，具有不依赖于航行器姿态的特点，但需要外部设备提供导航信息。卢兆兴等人设计了一种基于自适性遗传算法的磁强计误差校正方法，但该方法只能够校正磁强计的正交误差(卢兆兴，王仕成，张金生，乔玉坤,蔡欣华.一种基于自适性遗传算法的三轴磁强计误差校正技术.中国专利，2014.03.28，专利申请号：CN201410120249.6)。樊鹏辉等将外界磁干扰误差和三轴磁强计机械误差统一进行椭球拟合计算，以辨识误差参数，实现三轴磁强计误差校正。该方法虽能校正三轴磁强计的全误差，但也需要精确测量地磁场强度才能实现(樊鹏辉，魏雅川，倪书豪，刘伟，龚晶，苗楠，梁宇恒.一种应用于微小型无人机的整机磁强计校准方法，中国专利，2015.02.03，专利申请号：CN 201510056387.7)。

目前的磁强计误差辨识与校正方法主要存在两大局限：(1)只校正磁强计误差中的一种或两种，不能实现磁强计的全误差参数辨识与校正；(2)即使能够辨识与校正磁强计的全误差参数，也必须配备外部辅助测量设备，如测量当地磁场场值的标量磁强计及测姿设备等，这就增加了辨识与校正系统的使用成本，失去了辨识与校正方法的经济性。

发明内容

本发明的目的在于提供一种辨识精度高，不需要测量地磁场及姿态的辅助设备，成本低的不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法。

本发明的目的是这样实现的：

步骤1、将磁强计放置在无磁转台上，磁强计分别绕三个轴旋转获取不同姿态下的地磁场测量数据构成向量序列，所述数据个数为2N，将该向量序列任意分成数目相同的两组；

步骤2、将步骤1中的两组向量序列代入目标函数f的表达式，得到待优化的目标函数，利用下面的最优化关系式进行误差参数辨识，获得x的估计值

式中，R⁸表示8维实数域，下同；x_i、x_ui和x_li分别为x、x_u和x_l的第i个元素，向量x＝[q₁,q₂,q₃,q₄,q₅,b_x,b_y,b_z]，x_u＝[2,1,1,2,1,1,1,1]，x_l＝[0,-1,-1,0,-1,-1,-1,-1]，且

式(2)中的角度θ，ψ用于描述传感器坐标系的三轴空间非正交关系；δk_x和δk_y分别表示磁强计x和y轴的灵敏度相对于z轴的偏差，称之为轴间灵敏度偏差；磁强计三个轴的灵敏度分别为k_x＝1-δk_x、k_y＝1-δk_y和1，b₁，b₂和b₃分别为磁强计x，y和z轴的零偏误差；

利用二次规划算法(SQP)求解式(1)其具体求解步骤如下：

步1)令k＝0，设定k_max和j_max，给定初始点x_k＝x₀∈R⁸，对称正定矩阵C_k＝C₀∈R^8×8。计算选择参数η∈(0,0.5)，ρ∈(0,1)，容许误差0≤ω₁,ω₂＜＜1。

步2)j＝0，选取ξ∈(0,1)，ε_j＞0，d_kj∈R⁸，λ_kj∈R¹⁶。

步3)置z_kj＝(ε_j,d_kj,λ_kj)， i∈I＝{1,2,…,16}，(λ_kj)_i为λ_kj的第i个分量，表示矩阵的第i行，D₁＝diag(a₁,a₂,…,a₁₆)，D₂＝diag(b₁,b₂,…,b₁₆)，选取γ∈(0,1)，使γ||H(z_kj)||＜1。

步4)计算计算得到Φ(ε_j,d_kj,λ_kj)＝(φ₁(ε_j,d_kj,λ_kj),φ₂(ε_j,d_kj,λ_kj),…,φ₁₆(ε_j,d_kj,λ_kj))^T，i∈I＝{1,2,…,16}；计算其中v＝▽_εΦ(ε_j,d_kj,λ_kj)＝(v₁,…,v₁₆)^T，如果||H(z_kj)||＝0或j≥j_max，得到d_k＝d_kj和λ_k＝λ_kj并转步8)；否则，计算β_kj＝β(z_kj)＝γ||H(z_kj)||min{1,||H(z_kj)||}，并转步3)。

步5)求解方程组得Δz_kj＝(Δε_kj,Δd_kj,Δλ_kj)。

步6)求解的最小非负整数m_j。

步7)由和对z_kj、d_kj和λ_kj进行更新；j:＝j+1，并转步3)。

步8)计算||d_k||₁和||g(x_k)_{_}||₁，其中g(x_k)_-＝max{0,-g_i(x_k)}，i∈I。若||d_k||₁≤ω₁且||g(x_k)_{_}||₁≤ω₂，转步14)；否则，转步9)。

步9)令δ＞0，罚参数σ_k按式(3)进行修正，得到σ′_k。

式中，τ＝max{||λ_k||}。

步10)计算不等式φ(x_k+ρ^md_k,σ_k)-φ(x_k,σ_k)≤ηρ^mφ′(x_k,σ_k,d_k)成立的最小非负整数m_k，其中φ(x,y)＝f(x)+y^-1||g(x)_-||₁；由计算得到x′_k。

步11)计算及最小二乘乘子

步12)计算y_k＝▽_xL(x′_k,λ_k)-▽_xL(x_k,λ_k)，

计算z_k＝θ_ky_k+(1-θ_k)C_ks_k，按式(4)计算得到C_k′。

步13)令k＝k+1，如果k＜k_max，转步2)；否则转步14)。

步14)算法结束，得到

步骤3、由步骤2得到的计算q_m的最优估计值(m＝1,2,…,5)和b_n的最优估计值(n＝1,2,3)。

步骤4、将步骤3得到的代入式(5)，计算三轴非正交误差角及轴间灵敏度偏差的估计值步骤3得到的就是三轴磁强计零偏误差的估计值。

同时，按如下过程构建三轴磁强计全误差校正器。

由步骤3得到的和分别按式(6)和式(7)计算矩阵Ω的估计和三轴磁强计零偏误差的估计

步骤5、将步骤4得到的和代入三轴磁强计全误差校正公式，得到经误差校正后的地磁场向量估计值。

本发明提供了一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法，该方法能全面校正磁强计因制作工艺和安装技术等因素引起的仪器误差；解决现有技术在没有地磁场场量的情况下无法完整辨识三轴磁强计全误差及其校正的问题，全误差辨识及其校正方法也无需准确的外部姿态信息；本发明一步辨识出全部的三轴磁强计误差，辨识精度高，校正操作简便，不需要附加的地磁场及姿态测量设备，简化了三轴磁强计校正装置，降低了磁场测量系统的成本。

本发明提出一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法，利用地磁场的模量不变性，由不同姿态下的地磁场矢量测量数据对三轴磁强计的全误差参数进行辨识，以此构造误差校正器，利用构建的误差校正器校正三轴磁强计的全误差，获得校正后的被测磁场。解决现有技术在没有地磁场场量的情况下无法辨识三轴磁强计全误差参数，进而不能进行三轴磁强计全误差校正的问题；同时本发明不需要附加的地磁场测量设备，也无需附加的姿态测量设备提供准确的外部姿态信息。该发明一步辨识出全部的三轴磁强计误差，辨识精度高，不需要测量地磁场及姿态的辅助设备，简化了三轴磁强计的校正装置，降低了磁场测量系统的成本。

本发明与现有技术比较具有以下优点：提出的一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法具有低成本及操作简便等特点，解决在没有地磁场场量的情况下无法完整辨识三轴磁强计全误差及其校正问题；同时本发明也无需准确的姿态信息，不需要附加的地磁场及姿态测量设备，可实现三轴磁强计校正装置的简单化，校正操作简便，提高了磁场测量系统的经济性。

附图说明

图1是磁强计轴间非正交示意图；

图2是磁强计对外磁场测量与校正的方框图；

图3是三轴磁强计全误差校正方法的流程图；

图4是不同测量噪声下J_P与N之间的关系曲线；

图5是不同测量噪声下J_B与N之间的关系曲线。

具体实施方式

下面结合具体实施例对本发明进行详细说明。

步骤1、建立三轴磁强计全误差磁测模型：

一般地说，磁强计所在的传感器坐标系Ox′y′z′的三个轴之间是两两非正交的。在传感器坐标系的基础上建立空间正交坐标系Oxyz，如图1所示，其中Oz轴与Oz′轴重合，坐标面yOz与y′Oz′共面，Oy′轴与Oy′轴的夹角为ψ，Ox轴由Oy轴和Oz轴来确定。Ox′轴的方向可以由角θ和来表示，其中θ为Ox′轴与xOy平面之间的夹角，为Ox′轴在xOy面的投影与Ox轴的夹角，即用θ，ψ描述传感器坐标系的三轴空间非正交关系。

以z轴灵敏度为单位灵敏度，对磁强计另外两个轴的灵敏度进行归一化，系数δk_x和δk_y分别表示磁强计x和y轴的灵敏度相对于z轴的偏差，称之为轴间灵敏度偏差，则磁强计三个轴的灵敏度分别为k_x＝1-δk_x、k_y＝1-δk_y和1。设被测磁场B在Oxyz坐标系下各轴分量分别为B_x、B_y和B_z，在传感器坐标下各轴分量分别为B′_x、B′_y和B′_z，则有

B′＝KAB+B₀ (1)

其中，为零偏向量，而b₁，b₂和b₃分别为磁强计三个轴的零偏误差。

令磁场测量关系矩阵则由式(1)得被测磁场B的表达式

B＝Ω(B′-B₀) (2)

其中，Ω＝Γ^-1，式(2)就是三轴磁强计全误差校正公式。

定义

则式(2)中Ω可表示为

由于θ，和ψ都是小角度，且k_x和k_y接近于1，故q_m(m＝1,2,…,5)为一阶小量，即矩阵Ω中的非零元素没有二阶小量，因而能精确地辨识矩阵Ω中的每个非零元素。

由式(2)和(3)可知，对轴间非正交误差角与轴间灵敏度偏差(δk_x，δk_y)的辨识已转化为对q₁、q₂、q₃、q₄和q₅的最优估计。若能利用B′求得矩阵Ω的估计和向量B₀的估计，则可根据式(2)由磁场测量数据获得磁场矢量的估计值。

步骤2、磁强计分别绕其三个轴旋转获取不同姿态下的地磁场测量数据，数据个数为2N。三轴磁强计输出的地磁场向量测量序列为将该向量序列任意分成向量数相同的两组和

在不考虑测量误差的情况下，磁强计的最优误差参数应使两组测量向量序列B′_ef和B′_es中的元素范数差的平方和最小，因此目标函数f可设定为

设的三个分量分别为和的三个分量分别为和令

其中，

将式(5)展开得到

式中，W_hp(h＝1,2,…,6，p＝1,2,…,N)是b₁、b₂和b₃的函数。

步骤3、将步骤2得到的磁场测量数据代入下式

式中，x_i、x_ui和x_li分别为x、x_u和x_l的第i个元素。向量x＝[q₁,q₂,q₃,q₄,q₅,b_x,b_y,b_z]，x_u＝[2,1,1,2,1,1,1,1]，x_l＝[0,-1,-1,0,-1,-1,-1,-1]。式(13)所示的一般约束优化问题求解步骤如下：

步1)令k＝0。设定k_max和j_max，给定初始点x_k＝x₀∈R⁸，对称正定矩阵C_k＝C₀∈R^8×8。计算选择参数η∈(0,0.5)，ρ∈(0,1)，容许误差0≤ω₁,ω₂＜＜1。

步2)令j＝0，选取ξ∈(0,1)，ε_j＞0，d_kj∈R⁸，λ_kj∈R¹⁶。

步3)置z_kj＝(ε_j,d_kj,λ_kj)， i∈I＝{1,2,…,16}，(λ_kj)_i为λ_kj的第i个分量，表示矩阵的第i行，D₁＝diag(a₁,a₂,…,a₁₆)，D₂＝diag(b₁,b₂,…,b₁₆)，选取γ∈(0,1)，使γ||H(z_kj)||＜1。

步4)计算计算得到Φ(ε_j,d_kj,λ_kj)＝(φ₁(ε_j,d_kj,λ_kj),φ₂(ε_j,d_kj,λ_kj),…,φ₁₆(ε_j,d_kj,λ_kj))^T，i∈I＝{1,2,…,16}；计算其中v＝▽_εΦ(ε_j,d_kj,λ_kj)＝(v₁,…,v₁₆)^T，如果||H(z_kj)||＝0或j≥j_max，得到d_k＝d_kj和λ_k＝λ_kj并转步8)；否则，计算β_kj＝β(z_kj)＝γ||H(z_kj)||min{1,||H(z_kj)||}，并转步3)。

步5)求解方程组得Δz_kj＝(Δε_kj,Δd_kj,Δλ_kj)。

步6)求解的最小非负整数m_j。

步7)由和对z_kj、d_kj和λ_kj进行更新；j:＝j+1，并转步3)。

步8)计算||d_k||₁和||g(x_k)_{_}||₁，其中g(x_k)_-＝max{0,-g_i(x_k)}，i∈I。若||d_k||₁≤ω₁且||g(x_k)_{_}||₁≤ω₂，转步14)；否则，转步9)。

步9)令δ＞0，罚参数σ_k按式(14)进行修正，得到σ′_k。

式中，τ＝max{||λ_k||}。

步10)计算不等式φ(x_k+ρ^md_k,σ_k)-φ(x_k,σ_k)≤ηρ^mφ′(x_k,σ_k,d_k)成立的最小非负整数m_k，其中φ(x,y)＝f(x)+y^-1||g(x)_{_}||₁；由计算得到x′_k。

步11)计算及最小二乘乘子

步12)计算y_k＝▽_xL(x′_k,λ_k)-▽_xL(x_k,λ_k)，

计算z_k＝θ_ky_k+(1-θ_k)C_ks_k，按式(15)计算得到C′_k。

步13)令k＝k+1，如果k＜k_max，转步2)；否则转步14)。

步14)算法结束，得到

步骤4、由步骤3得到的计算q_m的最优估计值(m＝1,2,…,5)和b_n的最优估计值(n＝1,2,3)。

步骤5、根据步骤4得到的代入式(16)，计算三轴非正交误差角及轴间灵敏度偏差这些三轴磁强计误差参数的估计值而(n＝x,y,z)就是三轴磁强计零偏误差的估计值。

同时，按如下过程构建三轴磁强计全误差校正器。

由步骤3得到的和分别按式(17)和式(18)计算矩阵Ω的估计和三轴磁强计零偏误差的估计

步骤6、将步骤5得到的和代入步骤1中的三轴磁强计全误差校正公式(2)，得到经误差校正后的地磁场向量估计值。

利用地磁场B_e对三轴磁强计进行误差校正的方框图如图2所示，三轴磁强计测得的地磁场矢量为B′_e，经误差校正后的地磁场矢量为具体的误差校正流程图如图3所示。经校正的三轴磁强计能精确地测量外磁场矢量。

为直接反映仪器误差参数辨识与误差校正算法对这些误差参数的辨识及对测量磁场的校正效果，分别定义一个无量纲的精度指标J_P和J_B，如式(19)和(20)所示。

其中，仪器误差参数辨识的精度指标J_P表征了三轴磁强计误差参数辨识的精度，其值越小，参数辨识精度越高；磁测误差校正的精度指标J_B表征了三轴磁强计磁测误差的校正效果，其值越小，表明经误差参数辨识与误差校正后对三轴磁强计磁测精度越高，校正效果越好。

将当地地磁场矢量规范化为无单位的值，其中x方向分量B_ex＝1、y方向分量B_ey＝1和z方向分量其他仿真参数为：三轴磁强计的误差参数分别为θ＝0.03rad，ψ＝-0.02rad，δk_x＝0.05、δk_y＝-0.04、b_x＝-0.01、b_y＝0.02和b_z＝0.01；磁强计在三个测量轴方向上均匀采样，且假设每个轴的噪声为相互独立的高斯过程，其均值为0，方差为σ。

在不同磁强计测量噪声下，精度指标J_P与采样数据点个数N之间的关系如图4所示，N从1依次增1直到180，为清楚的显示，N从1依次增10。图4中五条曲线的线型分别为 和这5条曲线依次代表σ＝0.0001、σ＝0.0002、σ＝0.0003、σ＝0.0004和σ＝0.0005时J_P与N之间的变化关系。由图4可知，J_P随N的增大呈减小趋势，随σ的增大呈增加趋势；这表明三轴磁强计误差参数辨识的误差随N的增大而呈减小趋势，也说明当N足够大时，算法能较精确地辨识出三轴磁强计的全误差参数。在不同磁强计测量噪声下，计算40≤N≤180范围内J_P的平均值得到 和 和分别表示σ＝0.0001、σ＝0.0002、σ＝0.0003、σ＝0.0004和σ＝0.0005时在40≤N≤180范围内J_P的平均值。

在不同磁强计测量噪声下，精度指标J_B与采样数据点个数N之间的关系如图5所示，N从1依次增1直到180，但为清楚的显示，N从1依次增10。图5中的5条曲线线型分别为 和这5条曲线依次代表σ＝0.0001、σ＝0.0002、σ＝0.0003、σ＝0.0004和σ＝0.0005时J_B与N之间的变化关系。

由图5可知，J_B随N的增大呈减小趋势，随σ的增大呈增加趋势；这表明三轴磁强计对外磁场的测量误差随N的增大而呈减小趋势，当N越大，三轴磁强计的误差参数被辨识的精度越高，误差校正效果也越好。在不同磁力计测量噪声下，计算40≤N≤180范围内J_B的平均值得到和 和分别表示σ＝0.0001、σ＝0.0002、σ＝0.0003、σ＝0.0004和σ＝0.0005时在40≤N≤180范围内J_B的平均值。

根据三轴磁强计全误差参数辨识与校正的结果，定义了误差参数辨识与误差校正的精度指标，给出了不同信噪比条件下误差校正精度与采样点个数的关系。比较分析结果表明本发明能在不需要附加的地磁场测量设备情况下实现三轴磁强计的全误差参数辨识与校正，解决现有技术在没有地磁场场量的情况下无法完整地辨识三轴磁强计全误差及其校正的问题；同时本发明也无需附加的姿态测量设备提供准确的外部姿态信息，简化了三轴磁强计校正装置，降低了磁场测量系统的成本，提高了校正试验的操作性。

Claims (2)

1.一种不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法，其特征是：

步骤1、将磁强计放置在无磁转台上，磁强计分别绕三个轴旋转获取不同姿态下的地磁场测量数据构成向量序列，所述数据个数为2N，将该向量序列任意分成数目相同的两组；

步骤2、将步骤1中的两组向量序列代入目标函数f的表达式，得到待优化的目标函数，利用下面的最优化关系式进行误差参数辨识，获得x的估计值

式中，R⁸表示8维实数域，下同；x_i、x_ui和x_li分别为x、x_u和x_l的第i个元素，向量x＝[q₁,q₂,q₃,q₄,q₅,b₁,b₂,b₃]，x_u＝[2,1,1,2,1,1,1,1]，x_l＝[0,-1,-1,0,-1,-1,-1,-1]，且

式(2)中的角度θ，ψ用于描述传感器坐标系的三轴空间非正交关系；δk_x和δk_y分别表示磁强计x和y轴的灵敏度相对于z轴的偏差，称之为轴间灵敏度偏差；磁强计三个轴的灵敏度分别为k_x＝1-δk_x、k_y＝1-δk_y和1，b₁，b₂和b₃分别为磁强计x，y和z轴的零偏误差；

利用二次规划算法求解式(1)；

步骤3、由步骤2得到的计算q_m的最优估计值m＝1,2,…,5和b_n的最优估计值n＝1,2,3；

步骤4、将步骤3得到的代入式(5)，计算三轴非正交误差角及轴间灵敏度偏差的估计值步骤3得到的就是三轴磁强计零偏误差的估计值，

同时，按如下过程构建三轴磁强计全误差校正器，

由步骤3得到的和分别按式(6)和式(7)计算矩阵Ω的估计值和三轴磁强计零偏误差的估计值

步骤5、将步骤4得到的和代入三轴磁强计全误差校正公式得到经误差校正后的地磁场向量估计值B′为地磁场向量的测量值。

2.根据权利要求1所述的不依赖于地磁场场量的三轴磁强计全误差参数辨识与校正方法，其特征是所述利用二次规划算法求解式(1)的具体求解步骤包括：

步1)令k＝0，设定k_max和j_max，给定初始点x_k＝x₀∈R⁸，对称正定矩阵C_k＝C₀∈R^8×8，计算选择参数η∈(0,0.5)，ρ∈(0,1)，容许误差0≤ω₁,ω2＜＜1；

步2)j＝0，选取ξ∈(0,1)，ε_j＞0，d_kj∈R⁸，λ_kj∈R¹⁶；

步3)置z_kj＝(ε_j,d_kj,λ_kj)， i∈I＝{1,2,…,16}，(λ_kj)_i为λ_kj的第i个分量，表示矩阵的第i行，D₁＝diag(a₁,a2,…,a₁₆)，D₂＝diag(b₁,b₂,…,b₁₆)，选取γ∈(0,1)，使γ||H(z_kj)||＜1；

步4)计算计算得到Φ(ε_j,d_kj,λ_kj)＝(φ₁(ε_j,d_kj,λ_kj),φ₂(ε_j,d_kj,λ_kj),…,φ₁₆(ε_j,d_kj,λ_kj))^T，i∈I＝{1,2,…,16}；计算其中v＝▽_εΦ(ε_j,d_kj,λ_kj)＝(v₁,…,v₁₆)^T，如果||H(z_kj)||＝0或j≥j_max，得到d_k＝d_kj和λ_k＝λ_kj并转步8)；否则，计算β_kj＝β(z_kj)＝γ||H(z_kj)||min{1,||H(z_kj)||}，并转步3)；

步5)求解方程组得Δz_kj＝(Δε_kj,Δd_kj,Δλ_kj)；

步6)求解的最小非负整数m_j；

步7)由和对z_kj、d_kj和λ_kj进行更新；j:＝j+1，并转步3)；

步8)计算||d_k||₁和||g(x_k)_{_}||₁，其中g(x_k)_-＝max{0,-g_i(x_k)}，i∈I，若||d_k||₁≤ω₁且||g(x_k)_{_}||₁≤ω₂，转步14)；否则，转步9)；

步9)令δ＞0，罚参数σ_k按式(3)进行修正，得到σ′_k；

式中，τ＝max{||λ_k||}；

步10)计算不等式φ(x_k+ρ^md_k,σ_k)-φ(x_k,σ_k)≤ηρ^mφ′(x_k,σ_k,d_k)成立的最小非负整数m_k，其中φ(x,y)＝f(x)+y^-1||g(x)_-||₁；由计算得到x′_k；

步11)计算及最小二乘乘子

步12)计算y_k＝▽_xL(x′_k,λ_k)-▽_xL(x_k,λ_k)，计算z_k＝θ_ky_k+(1-θ_k)C_ks_k，按式(4)计算得到C′_k；

步13)令k＝k+1，如果k＜k_max，转步2)；否则转步14)；

步14)算法结束，得到