坐标计算公式

大家好，欢迎来到IT知识分享网。

直线：

$$x=x_0+(L-K)\cdot cos{a}_0+D\cdot cos(a_0\pm b)$$
$$y=y_0+(L-K)\cdot sin{a}_0+D\cdot sin(a_0\pm b)$$

$$(x_0,y_0)——起点坐标，K,L——起终点桩号$$
$$(x_0,y_0)——起点坐标，K,L——起终点桩号$$
$$a_0——起点方位角，b——偏移角度，D——偏距$$

圆曲线：

$$a=a_0\pm \frac{180\cdot (L-K)}{\pi R}（单位是角度）$$
$$x=x_0+R\cdot [cos(a_0\pm 90)+cos(a\pm 90)]+D\cdot cos(a\pm b)$$
$$y=y_0+R\cdot [sin(a_0\pm 90)+sin(a\pm 90)]+D\cdot sin(a\pm b)$$

$$(x_0,y_0):起点坐标;K,L:起终点桩号;R:半径;D:偏距（左减右加）$$
$$a_0:起点方位角;a:待求点方位角;b:偏移角度$$
$$第一个\pm 是左减右加，第二个\pm 是左加右减$$

缓和曲线：

$$A=\sqrt{R\cdot L_h}$$
$$△x=(L-K)-\frac{(L-K{)}^5}{40A^4}+\frac{(L-K{)}^9}{3456A^8}$$
$$△y=\frac{(L-K{)}^3}{6A^2}-\frac{(L-K{)}^7}{336A^6}+\frac{(L-K{)}^{11}}{42240A^{10}}$$
$$△L=\sqrt{△x^2+△y^2}$$
$$△a=arctan(\frac{△y}{△x})$$
$$a=a_0\pm \frac{90\cdot (L-K{)}^2}{\pi A^2}（单位是角度）$$
$$x=x_0+△L\cdot cos(a_0\pm △a)+D\cdot cos(a\pm b)$$
$$y=y_0+△L\cdot sin(a_0\pm △a)+D\cdot sin(a\pm b)$$

$$A:缓和曲线参数;R:半径;L_h:缓和曲线长度;△x△y:坐标增量$$
$$K,L:起终点桩号;△L:距离;△a:切线夹角（左减右加）;(x_0,y_0):起点坐标$$
$$a_0:起点方位角;a:待求点方位角;b:偏移角度$$

$$n——项数序号，！——阶乘，R——圆曲线半径，L_s——缓和曲线长，A——缓和曲线参数$$
$$PS：一般取公式前6项满足小半径的缓和曲线计算精度$$

卵曲线：

$$解决卵曲线的的问题，重点在于将其转换为完整的缓和曲线;$$
$$找出真正的起点坐标，起点方位角，以及起点桩号，然后代入缓和曲线公式进行计算;$$
$$残缺段为半径\infty 的卵曲线计算方法：$$

$$C=A^2=\frac{l{R}_{大}{R}_{小}}{R_{大}-R_{小}}$$
$$l_{弦}=\frac{C}{R_{大}}-\frac{C^3}{90\cdot R_{大}^5}$$
$$a_{弦}=a\pm \frac{C}{3R_{大}^2}（弧度）$$
$$a_0=a\pm \frac{C}{2R_{大}^2}（弧度）$$
$$x_0=x+l_{弦}\cdot cos(a_{弦}+180)$$
$$y_0=y+l_{弦}\cdot sin(a_{弦}+180)$$
$$K=L-\frac{C}{R_{小}}$$

$$A:缓和曲线参数，l：非完整缓和曲线长度，R_{大},R_{小}：非完整缓和曲线两端的半径$$
$$l_{弦}：残缺段缓和曲线的弦长，a_{弦}：残缺段缓和曲线弦的方位角，a_0：起点方位角$$
$$x_0,y_0:起点坐标，K,L:起终点桩号$$

源码：

# -*- coding: utf-8 -*- # @Author:  # @Date: 2019-10-30 18:23:29 # @Last Modified by:  # @Last Modified time: 2019-11-06 16:37:29 # data = [x0,y0,a0,k,l,r0,r1,t,L,d,b]  继承  # (x0,y0) 起点坐标(data[0],data[1]) # a0 起点方位角(data[2]) # k 起点里程(data[3])  提供  # l 终点里程(data[4]) # r0 起点半径(data[5]) # r1 终点半径(data[6]) # t 转向(data[7])  固定  # L 待求里程(data[8]) # d 偏距(data[9]) # b 旋转角度(data[10])  待求  # (x,y) 待求点坐标 # a 待求点方位角  输出  # (x1,y1) 终点坐标 # a1 终点方位角 # l 终点里程 import math as m import numpy as np def data(name): if name == '松岗主线': # 起点坐标(x,y)，起点方位角，起点里程 data_start = [42814.2898, 91516.6697, dfm_to_x(119, 13, 28.1), 7715.405] # 曲线类型，终点里程，起点半径，终点半径，偏向（无穷大和没有都视为零） data = [['直线', 8384.071, 0, 0, 0], ['缓和曲线', 8614.071, 0, 1560, -1], ['圆曲线', 9134.734, 1560, 1560, -1], ['缓和曲线', 9364.734, 1560, 0, -1], ['直线', 10017.163, 0, 0, 0], ['缓和曲线', 10197.163, 0, 1290, 1], ['圆曲线', 11248.951, 1290, 1290, 1], ['缓和曲线', 11428.951, 1290, 0, 1], ['直线', 11648.042, 0, 0, 0], ['缓和曲线', 11828.042, 0, 1290, -1], ['圆曲线', 12372.275, 1290, 1290, -1], ['缓和曲线', 12552.275, 1290, 0, -1], ['直线', 13355.558, 0, 0, 0], ['缓和曲线', 13555.558, 0, 1550, 1], ['圆曲线', 14699.663, 1550, 1550, 1], ['缓和曲线', 14899.663, 1550, 0, 1]] return data_start, data def dfm_to_x(x, y, z): # 度分秒-弧度 x = x + y / 60 + z / 3600 return m.radians(x) def z(data): # x0,y0,a0,k,l,r0,r1,t,L,d,b if data[8] <= data[3]: # 待求点里程小于等于起点里程 return data[:3] + [data[4]] # 直接返回初始值 else: if data[8] > data[4]: # 待求点里程大于终点里程 L = data[4] # 取终点里程 data[9] = 0 # 偏距归零 else: L = data[8] # 取待求点里程 x = data[0] + (L - data[3]) * m.cos(data[2]) + data[9] * m.cos(data[2] + m.radians(data[10])) y = data[1] + (L - data[3]) * m.sin(data[2]) + data[9] * m.sin(data[2] + m.radians(data[10])) return [x, y, data[2], data[4]] def y(data): # x0,y0,a0,k,l,r0,r1,t,L,d,b if data[8] <= data[3]: # 待求点里程小于等于起点里程 return data[:3] + [data[4]] # 直接返回初始值 else: if data[8] > data[4]: # 待求点里程大于终点里程 L = data[4] # 取终点里程 data[9] = 0 # 偏距归零 else: L = data[8] # 取待求点里程 a = data[2] + np.sign(data[7]) * (L - data[3]) / data[5] x = data[0] + data[5] * (m.cos(data[2] + np.sign(data[7]) * 0.5 * m.pi) + m.cos(a - np.sign(data[7]) * 0.5 * m.pi)) + data[9] * m.cos(a + m.radians(data[10])) y = data[1] + data[5] * (m.sin(data[2] + np.sign(data[7]) * 0.5 * m.pi) + m.sin(a - np.sign(data[7]) * 0.5 * m.pi)) + data[9] * m.sin(a + m.radians(data[10])) return [x, y, a, data[4]] def h(data): # x0,y0,a0,k,l,r0,r1,t,L,d,b if data[8] <= data[3]: # 待求点里程小于等于起点里程 return data[:3] + [data[4]] # 直接返回初始值 else: if data[5] == 0: # 入缓和曲线 if data[8] > data[4]: # 待求点里程大于终点里程 L = data[4] # 取终点里程 data[9] = 0 # 偏距归零 else: L = data[8] # 取待求点里程 A = m.pow(data[6] * (data[4] - data[3]), 0.5) dx = dy = 0 for i in range(1, 7): dx = dx + m.pow(-1, i + 1) * m.pow(L - data[3], 4 * i - 3) / (m.factorial(2 * i - 2) * m.pow(2, 2 * i - 2) * (4 * i - 3) * m.pow(A, 4 * i - 4)) dy = dy + m.pow(-1, i + 1) * m.pow(L - data[3], 4 * i - 1) / (m.factorial(2 * i - 1) * m.pow(2, 2 * i - 1) * (4 * i - 1) * m.pow(A, 4 * i - 2)) dl = m.hypot(dx, dy) dx = dx + m.pow(10, -100) da = m.atan(dy / dx) a = data[2] + np.sign(data[7]) * m.pow(L - data[3], 2) / (2 * A * A) x = data[0] + dl * m.cos(data[2] + np.sign(data[7]) * da) + data[9] * m.cos(a + m.radians(data[10])) y = data[1] + dl * m.sin(data[2] + np.sign(data[7]) * da) + data[9] * m.sin(a + m.radians(data[10])) return [x, y, a, data[4]] if data[6] == 0: # 出缓和曲线 A = m.pow(data[5] * (data[4] - data[3]), 0.5) dx = dy = 0 for i in range(1, 7): dx = dx + m.pow(-1, i + 1) * m.pow(data[4] - data[3], 4 * i - 3) / (m.factorial(2 * i - 2) * m.pow(2, 2 * i - 2) * (4 * i - 3) * m.pow(A, 4 * i - 4)) dy = dy + m.pow(-1, i + 1) * m.pow(data[4] - data[3], 4 * i - 1) / (m.factorial(2 * i - 1) * m.pow(2, 2 * i - 1) * (4 * i - 1) * m.pow(A, 4 * i - 2)) dl = m.hypot(dx, dy) dx = dx + m.pow(10, -100) da = m.atan(dy / dx) a0 = data[2] + np.sign(data[7]) * m.pow(data[4] - data[3], 2) / (2 * A * A) x = data[0] - dl * m.cos(a0 + m.pi - np.sign(data[7]) * da) y = data[1] - dl * m.sin(a0 + m.pi - np.sign(data[7]) * da) if data[8] > data[4]: # 待求点里程大于终点里程 L = data[4] # 取终点里程 data[9] = 0 # 偏距归零 else: L = data[8] # 取待求点里程 dx = dy = 0 for i in range(1, 7): dx = dx + m.pow(-1, i + 1) * m.pow(data[4] - L, 4 * i - 3) / (m.factorial(2 * i - 2) * m.pow(2, 2 * i - 2) * (4 * i - 3) * m.pow(A, 4 * i - 4)) dy = dy + m.pow(-1, i + 1) * m.pow(data[4] - L, 4 * i - 1) / (m.factorial(2 * i - 1) * m.pow(2, 2 * i - 1) * (4 * i - 1) * m.pow(A, 4 * i - 2)) dl = m.hypot(dx, dy) dx = dx + m.pow(10, -100) da = m.atan(dy / dx) a = a0 - np.sign(data[7]) * m.pow(data[4] - L, 2) / (2 * A * A) x = x + dl * m.cos(a0 + m.pi - np.sign(data[7]) * da) + data[9] * m.cos(a + m.radians(data[10])) y = y + dl * m.sin(a0 + m.pi - np.sign(data[7]) * da) + data[9] * m.sin(a + m.radians(data[10])) return [x, y, a, data[4]] # 卵曲线 # x0,y0,a0,k,l,r0,r1,t,L,d,b if data[5] > data[6] > 0: # 入卵曲线r0>r1 dL = data[4] - data[3] # 卵曲线全长 c = dL * data[5] * data[6] / (data[5] - data[6]) l_xuan = c / data[5] - m.pow(c, 3) / (90 * m.pow(data[5], 5)) a_xuan = data[2] - np.sign(data[7]) * c / 3 / m.pow(data[5], 2) a = data[2] - np.sign(data[7]) * c / 2 / m.pow(data[5], 2) x = data[0] + l_xuan * m.cos(a_xuan + m.pi) y = data[1] + l_xuan * m.sin(a_xuan + m.pi) L_start = data[4] - c / data[6] return h([x, y, a, L_start, data[4], 0] + data[6:]) if data[6] > data[5] > 0: # 出卵曲线r0<r1 if data[8] > data[4]: # 待求点里程大于终点里程 data[8] = data[4] # 取终点里程 data[9] = 0 # 偏距归零 # x0,y0,a0,k,l,r0,r1,t,L,d,b dL = data[4] - data[3] # 卵曲线全长 c = dL * data[5] * data[6] / (data[6] - data[5]) l = c / data[5] return h(data[:4] + [data[3] + l] + [data[5], 0] + data[7:])[:3] + [data[4]] def main(data_start, data, data_dq): for i in range(len(data)): if data[i][0] == '直线': data_start = z(data_start + data[i][1:] + data_dq) if data[i][0] == '缓和曲线': data_start = h(data_start + data[i][1:] + data_dq) if data[i][0] == '圆曲线': data_start = y(data_start + data[i][1:] + data_dq) # 格式化输出结果 data_start[0] = round(data_start[0], 4) data_start[1] = round(data_start[1], 4) data_start[2] = m.degrees(data_start[2]) return data_start if __name__ == '__main__': name = '松岗主线' data_dq = [10000, 0, 90] print(main(data(name)[0], data(name)[1], data_dq))