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

三角测量与坐标更新算法

最新推荐文章于 2025-10-15 16:20:32 发布

原创最新推荐文章于 2025-10-15 16:20:32 发布 · 245 阅读

0 ·

CC 4.0 BY-SA版权

本文介绍了一种基于三角测量原理的坐标更新算法，通过计算角度、距离等参数，实现站点间坐标转换与高程调整。该算法适用于测绘、地理信息系统等领域。


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);//将成员变量的值赋给控件

核心公式：

[img]http://dl.iteye.com/upload/attachment/0080/3175/cc31b826-e7dd-3851-8b50-0ef81dbebd27.gif[/img]

运行结果：

[img]http://dl.iteye.com/upload/attachment/0080/3192/0fbdb609-2171-3a0b-bc66-6a8f29bfc3d2.jpg[/img]