java uwb定位算法_JAVA：角度前方交会算法GUI实现

编译环境：

操作系统：Win8.1  64位

IDE平台：Visual Studio 2013 Ultimate

一、原理与步骤

二、代码实现

角度前方交会.java

package text;

import java.awt.*;

import java.awt.event.*;

import java.math.*;

import javax.swing.*;

import javax.swing.border.Border;

public class 角度前方交会 extends JFrame

{

public static final int TEXTAREA_ROWS = 20;

public static final int TEXTAREA_COLUMNS = 40;

public 角度前方交会()

{

final JTextField A_X = new JTextField("37477");

final JTextField A_Y = new JTextField("16307");

final JTextField B_X = new JTextField("37327");

final JTextField B_Y = new JTextField("16078");

final JTextField C_X = new JTextField("37163");

final JTextField C_Y = new JTextField("16046");

final JTextField α1 = new JTextField("40°41'");

final JTextField β1 = new JTextField("75°19'");

final JTextField α2 = new JTextField("58°11'");

final JTextField β2 = new JTextField("69°6'");

JPanel northPanel = new JPanel();

final JTextField DAP = new JTextField();

final JTextField DBP = new JTextField();

JPanel WestPanel = new JPanel();

JLabel pic= new JLabel();

pic.setIcon(new ImageIcon("G:/workplace/角度前方交会/1.png"));

WestPanel.add(pic);

getContentPane().add(WestPanel, BorderLayout.WEST);

//角度前方交会

northPanel.setLayout(new GridLayout(2, 10));

northPanel.add(new JLabel("A_X: ", SwingConstants.CENTER));

northPanel.add(A_X);

northPanel.add(new JLabel("A_Y: ", SwingConstants.CENTER));

northPanel.add(A_Y);

northPanel.add(new JLabel("B_X: ", SwingConstants.CENTER));

northPanel.add(B_X);

northPanel.add(new JLabel("B_Y: ", SwingConstants.CENTER));

northPanel.add(B_Y);

northPanel.add(new JLabel("C_X: ", SwingConstants.CENTER));

northPanel.add(C_X);

northPanel.add(new JLabel("C_Y: ", SwingConstants.CENTER));

northPanel.add(C_Y);

northPanel.add(new JLabel("α1: ", SwingConstants.CENTER));

northPanel.add(α1);

northPanel.add(new JLabel("β1: ", SwingConstants.CENTER));

northPanel.add(β1);

northPanel.add(new JLabel("α2: ", SwingConstants.CENTER));

northPanel.add(α2);

northPanel.add(new JLabel("β2: ", SwingConstants.CENTER));

northPanel.add(β2);

Border etched = BorderFactory.createEtchedBorder();

Border titled = BorderFactory.createTitledBorder(etched, "角度前方交会");

northPanel.setBorder(titled);

getContentPane().add(northPanel, BorderLayout.NORTH);

final JTextArea textArea = new JTextArea(20,40);

JScrollPane scrollPane = new JScrollPane(textArea);

getContentPane().add(scrollPane, BorderLayout.CENTER);

// add button to append text into the text area

JPanel southPanel = new JPanel();

JButton insertButton = new JButton("计算P点坐标");

insertButton.addActionListener(new ActionListener()

{

public void actionPerformed(ActionEvent event)

{

//坐标初始化

Double X1=Double.valueOf(A_X.getText()).doubleValue();

Double Y1=Double.valueOf(A_Y.getText()).doubleValue();

Double X2=Double.valueOf(B_X.getText()).doubleValue();

Double Y2=Double.valueOf(B_Y.getText()).doubleValue();

Double X3=Double.valueOf(C_X.getText()).doubleValue();

Double Y3=Double.valueOf(C_Y.getText()).doubleValue();

Double α_1=convert(α1.getText());

Double β_1=convert(β1.getText());

Double α_2=convert(α2.getText());

Double β_2=convert(β2.getText());

//A、B、P

Double D_AB=Math.sqrt( Math.pow((X1-X2),2)+Math.pow((Y1-Y2),2));

Double D_AP=D_AB*Math.sin(β_1)/Math.sin(α_1+β_1);

Double α_AB=90-Math.atan(Math.abs(Y2-Y1)/Math.abs(X2-X1));

Double α_AP=α_AB-α_1;

Double XP1=X1+D_AP*Math.cos(α_AP);

Double YP1=Y1+D_AP*Math.sin(α_AP);

//B、C、P

Double D_BC=Math.sqrt( Math.pow((X3-X2),2)+Math.pow((Y3-Y2),2));

Double D_BP=D_BC*Math.sin(β_2)/Math.sin(α_2+β_2);

Double α_BC=Math.atan(Math.abs(Y2-Y3)/Math.abs(X2-X3));

Double α_BP=α_BC-α_2;

Double XP2=X2+D_BP*Math.cos(α_BP);

Double YP2=Y1+D_BP*Math.sin(α_BP);

textArea.append("=====角度前方交会===="+"\n"+"参考值1:"+ "( " +XP1+","+YP1+")"

+  "\n"+"参考值2:"+"( " +XP2+","+YP2+")"

+  "\n"+"误差距离约:"+Math.round(miss(XP1,YP1,XP2,YP2))+"米");

//textArea.append("测试："+β_2);

}

public double miss(double XP1,double YP1,double XP2,double YP2){

return Math.sqrt(Math.pow((XP1-XP2),2)+Math.pow((YP1-YP2),2));

}

public double convert(String input){

Double a=Double.valueOf(input.split("°")[0]).doubleValue();

Double b=Double.valueOf(input.split("°")[1].split("'")[0]).doubleValue();

return a+b/60;

}

});

add(insertButton, BorderLayout.SOUTH);

pack();

}

}

Test.java

package text;

import java.awt.*;

import javax.swing.*;

public class Test

{

public static void main(String[] args)

{

EventQueue.invokeLater(new Runnable()

{

public void run()

{

JFrame frame = new 角度前方交会();

frame.setTitle("角度前方交会");

frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);

frame.setVisible(true);

}

});

}

}

三、运行结果