用后方交会法求测量机器人换站后三维坐标

this->UpdateData(TRUE);//将控件的值赋给成员变量

/**
 * 计算中间变量γ1，γ2
 */
double r1_angle = ((this->m_b_hz - this->m_a_hz)/PI) * 180;
double r2_angle = ((this->m_c_hz - this->m_b_hz)/PI) * 180;
double r1_rad = (double)(this->m_b_hz - this->m_a_hz);
this->m_r1.Format("%lf",r1_rad);

double r2_rad = (double)(this->m_c_hz - this->m_b_hz);
this->m_r2.Format("%lf",r2_rad);
/**
 * 计算中间变量AB,BC,AC
 * 其中Math.pow((a.e - b.e), 2)表示(a的e坐标-b的e坐标)的2次方
 * Math.sqrt(x)表示对x开方
 */
double AB = sqrt(pow((this->m_a_e - this->m_b_e), 2) + pow((this->m_a_n - this->m_b_n), 2));
double BC = sqrt(pow((this->m_c_e - this->m_b_e), 2) + pow((this->m_c_n - this->m_b_n), 2));
double AC = sqrt(pow((this->m_a_e - this->m_c_e), 2) + pow((this->m_a_n - this->m_c_n), 2));
this->m_ab.Format("%lf",AB);
this->m_bc.Format("%lf",BC);
this->m_ac.Format("%lf",AC);
/**
 * 计算终中间变量∠ABC
 * 先要计算cos∠ABC
 */
double cosABC = (pow(BC, 2) + pow(AB, 2) - pow(AC, 2))/(2 * BC * AB);
double abc = ( acos(cosABC) * 180 )/ PI;//利用arcos求出∠ABC
this->m_cosjabc.Format("%lf",cosABC);
this->m_jabc.Format("%lf",abc);

/**
 * 计算中间变量φ，ω
 */
double fi = (AB * sin(r2_rad))/(BC * sin(r1_rad));
double wmd_angle = 360 - (r1_angle + r2_angle + abc);
double wmd_rad = ( wmd_angle * PI )/180;
this->m_wmd.Format("%lf",wmd_rad);
this->m_fi.Format("%lf",fi);
/**
 * 求出中间变量cotα1,cotβ1
 */
//Double ctanβ2 = (Math.tan(φ) * Math.cos(ω) + 1)/(Math.tan(φ) * Math.sin(ω));
double ctanalpha1 = (tan(fi) + cos(wmd_rad))/sin(wmd_rad);
double alpha1_rad = atan(1/ctanalpha1);
double alpha1_angle = (alpha1_rad * 180 )/PI;
double beta1 = (360 - r1_angle - alpha1_angle) * PI / 180;
double cotalpha1 = ctanalpha1;
double cotbeta1 = 1/tan(beta1);
this->m_alpha.Format("%lf",cotalpha1);
this->m_beta.Format("%lf",cotbeta1);
/**
 * 求出仪器的新坐标中的E,N
 */
this->m_e.Format("%lf",(this->m_a_e * cotbeta1 + this->m_b_e * cotalpha1 + (this->m_b_n - this->m_a_n)) / (cotalpha1 + cotbeta1));
this->m_n.Format("%lf",(this->m_a_n * cotbeta1 + this->m_b_n * cotalpha1 - (this->m_b_e - this->m_a_e)) / (cotalpha1 + cotbeta1));
/**
 * 处理换站后的高程，其中
 * a.h代表基准点a首次测的坐标H，
 * a.newH,代表换站后，基准点a的新坐标H
 */
double z0 = this->m_a_h - this->m_a_nh;
double z1 = this->m_b_h - this->m_b_nh;
double z2 = this->m_c_h - this->m_c_nh;
this->m_h.Format("%lf",(z0+z1+z2)/3);//求平均值
this->UpdateData(FALSE);//将成员变量的值赋给控件

核心公式：



运行结果：