主权项 |
一种多智能体分布式协同定位方法,其特征在于:该定位方法包括以下步骤:1)每个智能体建立用于位置估计的状态变量,状态变量包含该智能体的本体位置矢量以及与它邻近的普通智能体的他体位置矢量,每个智能体初始化状态变量的估计值及其对应的误差协方差矩阵,每个智能体初始化预测过程中的系统噪声矩阵和测量噪声矩阵,包括以下具体步骤:1‑1)构造状态变量:设智能体i建立状态变量X<sub>i</sub>,为<img file="FDA0000890925990000011.GIF" wi="559" he="135" />其中,r<sub>i</sub>代表智能体i的本体位置矢量,上标<sup>T</sup>代表转置,<img file="FDA0000890925990000012.GIF" wi="105" he="68" />代表第n个与智能体i邻近的普通智能体的他体位置矢量,<img file="FDA0000890925990000013.GIF" wi="68" he="78" />代表所有与智能体i邻近的普通智能体的数量,i、n、<img file="FDA0000890925990000014.GIF" wi="61" he="77" />均为正整数,<img file="FDA0000890925990000015.GIF" wi="598" he="95" />表示邻近智能体编号;1‑2)初始化定位变量:设智能体i状态变量X<sub>i</sub>的估计值<img file="FDA0000890925990000016.GIF" wi="109" he="78" />为<img file="FDA0000890925990000017.GIF" wi="718" he="133" />其中,<img file="FDA0000890925990000018.GIF" wi="85" he="71" />代表智能体i对本智能体i位置矢量的估计值,<img file="FDA0000890925990000019.GIF" wi="145" he="78" />代表智能体i对与其邻近的第n个普通智能体的他体位置矢量的估计值;初始化状态变量在0时刻的估计值<img file="FDA00008909259900000110.GIF" wi="118" he="102" />其中符号|<sub>0</sub>用于指示竖线左侧变量在t=0时刻的值;智能体i根据状态变量估计值的初始误差,初始化其误差协方差矩阵P<sub>i</sub>,得到P<sub>i</sub>|<sub>0</sub>;智能体i根据状态变量X<sub>i</sub>的维度,初始化系统噪声矩阵Q<sub>i</sub>;2)在第t=k时刻,对每个智能体的状态变量采用实时迭代方法进行实时估计,每一次迭代过程依次包括:对该状态变量进行预测、对邻近智能体进行相对距离的测量、对状态变量预测值进行本地优化、对状态变量中他体位置矢量估计值进行邻域索求与响应、对状态变量估计值进行邻域优化、对状态变量中本体位置矢量估计值进行邻域交换、对状态变量估计值进行重建,具体步骤为:2‑1)对状态变量进行预测:每个智能体根据上一时刻即k‑1时刻状态变量估计值,对k时刻的状态变量进行预测;设智能体i在k时刻状态变量的预测值<img file="FDA00008909259900000111.GIF" wi="175" he="101" />误差协方差矩阵预测值P<sub>i</sub>|<sub>k/k‑1</sub>,分别为:<maths num="0001"><math><![CDATA[<mrow><msub><mover><mi>X</mi><mo>^</mo></mover><mi>i</mi></msub><msub><mo>|</mo><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>=</mo><msub><mover><mi>X</mi><mo>^</mo></mover><mi>i</mi></msub><msub><mo>|</mo><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000890925990000021.GIF" wi="1061" he="102" /></maths>P<sub>i</sub>|<sub>k/k‑1</sub>=P<sub>i</sub>|<sub>k‑1</sub>+Q<sub>i</sub> (2)其中,<img file="FDA0000890925990000022.GIF" wi="128" he="102" />为k‑1时刻的状态变量估计值,P<sub>i</sub>|<sub>k‑1</sub>为对应的误差协方差矩阵;2‑2)对邻近智能体进行相对距离的测量:根据每个智能体i采集包含有它与邻近智能体之间相对位置信息的测量距离<img file="FDA0000890925990000023.GIF" wi="111" he="94" />按式(3)得到k时刻的邻近智能体相对距离测量变量<img file="FDA0000890925990000024.GIF" wi="86" he="94" /><img file="FDA0000890925990000025.GIF" wi="1230" he="134" />其中,<img file="FDA0000890925990000026.GIF" wi="124" he="95" />为智能体i与邻近的第n个普通智能体间相对距离测量值,<img file="FDA0000890925990000027.GIF" wi="128" he="95" />为智能体i与邻近的第n个锚点间相对距离测量值,锚点指的是具有绝对定位能力的位置参考实体;2‑3)对状态变量预测值进行本地优化:具体包括以下步骤:2‑3‑1)预测邻近相对距离,每个智能体i根据状态变量的预测值<img file="FDA0000890925990000028.GIF" wi="167" he="103" />进一步预测邻近相对距离<img file="FDA0000890925990000029.GIF" wi="559" he="91" />组成邻近测量变量的预测值<img file="FDA00008909259900000210.GIF" wi="133" he="103" /><img file="FDA00008909259900000211.GIF" wi="1148" he="135" />其中,<img file="FDA00008909259900000212.GIF" wi="133" he="93" />为智能体i与邻近的第n个普通智能体间相对距离预测值,<img file="FDA00008909259900000213.GIF" wi="135" he="93" />为智能体i与邻近的第n个锚点间相对距离预测值;2‑3‑2)根据邻近测量变量预测值的维度、大小及测量条件,适应性设定测量噪声矩阵R<sub>i</sub>;2‑3‑3)以扩展卡尔曼滤波的最优化策略,对状态变量及其协方差矩阵进行优化,得到本地优化后的状态变量<img file="FDA00008909259900000214.GIF" wi="152" he="101" />及其误差协方差矩阵P<sub>i</sub>|<sub>k‑0.5</sub>,它们分别为<maths num="0002"><math><![CDATA[<mrow><msub><mover><mi>X</mi><mo>^</mo></mover><mi>i</mi></msub><msub><mo>|</mo><mrow><mi>k</mi><mo>-</mo><mn>0.5</mn></mrow></msub><mo>=</mo><msub><mover><mi>X</mi><mo>^</mo></mover><mi>i</mi></msub><msub><mo>|</mo><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>K</mi><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><mrow><mo>(</mo><msub><mover><mi>Z</mi><mo>~</mo></mover><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><mo>-</mo><msub><mover><mi>Z</mi><mo>^</mo></mover><mi>i</mi></msub><msub><mo>|</mo><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>5</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA00008909259900000215.GIF" wi="968" he="111" /></maths><maths num="0003"><math><![CDATA[<mrow><msub><mi>P</mi><mi>i</mi></msub><msub><mo>|</mo><mrow><mi>k</mi><mo>-</mo><mn>0.5</mn></mrow></msub><mo>=</mo><mrow><mo>(</mo><mi>I</mi><mo>-</mo><msub><mi>K</mi><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><msub><mi>H</mi><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><mo>)</mo></mrow><msub><mi>P</mi><mi>i</mi></msub><msub><mo>|</mo><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msup><mrow><mo>(</mo><mi>I</mi><mo>-</mo><msub><mi>K</mi><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><msub><mi>H</mi><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><mo>)</mo></mrow><mi>T</mi></msup><mo>+</mo><msub><mi>K</mi><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><msub><mi>R</mi><mi>i</mi></msub><msub><mi>K</mi><mi>i</mi></msub><msubsup><mo>|</mo><mi>k</mi><mi>T</mi></msubsup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>6</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA00008909259900000216.GIF" wi="1317" he="101" /></maths>式(5)中增益修正矩阵K<sub>i</sub>|<sub>k</sub>、式(6)中线性化观测矩阵H<sub>i</sub>|<sub>k</sub>分别为<maths num="0004"><math><![CDATA[<mrow><msub><mi>K</mi><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><mo>=</mo><msub><mi>P</mi><mi>i</mi></msub><msub><mo>|</mo><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msub><mi>H</mi><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><msup><mrow><mo>(</mo><msub><mi>H</mi><mi>i</mi></msub><msub><mo>|</mo><mi>k</mi></msub><msub><mi>P</mi><mi>i</mi></msub><msub><mo>|</mo><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msub><mi>H</mi><mi>i</mi></msub><msubsup><mo>|</mo><mi>k</mi><mi>T</mi></msubsup><mo>+</mo><msub><mi>R</mi><mi>i</mi></msub><mo>)</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>7</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA00008909259900000217.GIF" wi="1229" he="112" /></maths><img file="FDA0000890925990000031.GIF" wi="1500" he="870" />2‑4)对状态变量中他体位置矢量估计值进行邻域索求与响应:每个智能体索求邻近智能体对该主动索求智能体位置矢量的最优估计,并响应其邻近智能体的同类索求;具体为:每个智能体i本地优化后的状态变量<img file="FDA0000890925990000032.GIF" wi="167" he="103" />中均包含有对其邻近智能体的他体位置矢量的估计;每个智能体i,向其邻近智能体<img file="FDA0000890925990000033.GIF" wi="176" he="71" />索求<img file="FDA0000890925990000034.GIF" wi="158" he="102" />中包含的位置估计量<img file="FDA0000890925990000035.GIF" wi="174" he="93" />及对应的包含于P<sub>j</sub>|<sub>k‑0.5</sub>中的误差协方差矩阵块P<sub>j</sub><i>|<sub>k‑0.5</sub>;同时,每个智能体i也向其邻近智能体<img file="FDA0000890925990000036.GIF" wi="177" he="77" />提供<img file="FDA0000890925990000037.GIF" wi="150" he="102" />中包含的位置估计量<img file="FDA0000890925990000038.GIF" wi="166" he="103" />及对应的包含于P<sub>i</sub>|<sub>k‑0.5</sub>中的误差协方差矩阵块P<sub>i</sub><j>|<sub>k‑0.5</sub>;2‑5)对状态变量估计值进行邻域优化:首先计算智能体i的邻近智能体个数<img file="FDA0000890925990000039.GIF" wi="86" he="76" />并计算系数ω<sub>i</sub>,<img file="FDA00008909259900000310.GIF" wi="245" he="143" />随后按式(9)式(10)对本体位置矢量的估计值及其误差协方差矩阵进行邻域优化,优化后的位置矢量估计值<img file="FDA00008909259900000311.GIF" wi="184" he="94" />及其误差协方差矩阵P<sub>i</sub><i>|<sub>k,block</sub>分别为<img file="FDA00008909259900000312.GIF" wi="1229" he="143" /><maths num="0005"><math><![CDATA[<mrow><msup><mrow><mo>(</mo><msub><mi>P</mi><mi>i</mi></msub><mo><</mo><mi>i</mi><mo>></mo><msub><mo>|</mo><mrow><mi>k</mi><mo>,</mo><mi>b</mi><mi>l</mi><mi>o</mi><mi>c</mi><mi>k</mi></mrow></msub><mo>)</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>=</mo><munder><mo>Σ</mo><mrow><mi>j</mi><mo>∈</mo><mo>{</mo><mi>i</mi><mo>}</mo><mo>∪</mo><msub><mi>N</mi><mi>i</mi></msub></mrow></munder><mo>[</mo><msub><mi>ω</mi><mi>i</mi></msub><msup><mrow><mo>(</mo><msub><mi>P</mi><mi>j</mi></msub><mo><</mo><mi>i</mi><mo>></mo><msub><mo>|</mo><mrow><mi>k</mi><mo>-</mo><mn>0.5</mn></mrow></msub><mo>)</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>]</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA00008909259900000313.GIF" wi="1086" he="143" /></maths>2‑6)对状态变量中本体位置矢量估计值进行邻域交换:智能体i邻域优化后的本体位置估计值<img file="FDA00008909259900000314.GIF" wi="193" he="86" />及P<sub>i</sub><i>|<sub>k,block</sub>,与其邻近普通智能体<img file="FDA00008909259900000315.GIF" wi="173" he="71" />的邻域优化后的本体位置估计值<img file="FDA0000890925990000041.GIF" wi="200" he="102" />及P<sub>j</sub><j>|<sub>k,block</sub>,进行彼此交换;2‑7)重建状态变量估计值及其协方差:根据智能体i邻域交换的本体位置估计值,按式(11)式(12)重建智能体i的状态变量的估计值及误差协方差,用于下一时刻的迭代过程;<img file="FDA0000890925990000042.GIF" wi="1437" he="159" /><img file="FDA0000890925990000043.GIF" wi="1517" he="151" />3)每当新时刻来临,即t=k+1时刻,依次重复步骤2‑1)至2‑7)进行新时刻的实时定位估计。 |