## 3 System Modeling
### Coordinate Conversion
This mark can roughly show the position of the motor shaft at the moment a 3D scan is started, that is, the position of the axis \(Y\)' of coordinate frame \(O\)'-\(X\)'\(Y\)'\(Z\)'. We can find that for each 3D scan, the position of this mark relative to the 3D point cloud is not constant. For more details, see the experiment section in Section 5. After introducing the definitions of the above three Cartesian coordinate frames, the conversion among them should be studied out. Our goal is to get 3D point cloud relative to coordinate frame \(O\) accurately, both in shape and attitude. Our raw data includes ranging data \(r\) and azimuth angle \(\theta\), which is obtained from the 2D lidar, and the rotation angle \(\varphi_{\text{motor}}\) of the motor shaft. Among them, \(r\) can be obtained directly, while \(\theta\) and \(\varphi_{\text{motor}}\) can be obtained by linear interpolation. First, we calculate a 2D point \(\mathbf{p}_{L}\) relative to coordinate frame \(L\) according to the ranging data \(r\) and azimuth angle \(\theta\)  
\[\mathbf{p}_{L}=r[ \begin{array}{cc}0&s(\theta)&c(\theta)\end{array} ]^{T} \tag{4}\]  
where \(c(\cdot)\) and \(s(\cdot)\) are cos and sin, respectively. Second, we convert point \(\mathbf{p}_{L}\) in coordinate frame \(L\) to the corresponding point \(\mathbf{p}_{O^{\prime}}\) in coordinate frame \(O\)'  
\[\mathbf{p}_{O^{\prime}}=\mathbf{R}_{M}(R_{L}^{O^{\prime}}\mathbf{p}_{L}+t_{L}^{O^{\prime}}) \tag{5}\]  
where \(\mathbf{R}_{L}^{O^{\prime}}\) and \(t_{L}^{O^{\prime}}\) are the rotation matrix and the translation vector from coordinate frame \(L\) to coordinate frame \(O\)' at the moment when a 3D scan is started, respectively. At this moment, axis \(X_{L}\) coincides with axis \(X\)', axis \(Y_{L}\) and axis \(Y\)' are parallel, axis \(Z_{L}\) and axis \(Z\)' are parallel, and the distance between point \(L\) and point \(O\)' is 13.9 mm. Therefore, We can know that \(\mathbf{R}_{L}^{O^{\prime}}=\mathbf{I}\), \(\mathbf{t}_{L}^{O^{\prime}}=\) (13.9 mm 0 0)\({}^{T}\). \(\mathbf{R}_{\text{M}}\) is the rotation matrix calculated according to the rotation angle \(\varphi_{\text{motor}}\) of the motor shaft, which shows the attitude change of coordinate frame \(L\) relative to its initial position after being rotated by the motor shaft. Since the direction of the rotation is along the axis \(Z\)', there is no rotation component in other directions, \(\mathbf{R}_{\text{M}}\) can be calculated as:  
\[\mathbf{R}_{M}=\left[ \begin{array}{ccc}c(\varphi_{\text{motor}})&-s(\varphi_{ \text{motor}})&0\\ s(\varphi_{\text{motor}})&c(\varphi_{\text{motor}})&0\\ 0&0&1\end{array} \right] \tag{6}\]  
Figure 5: The scanning sector of the 2D lidar does not coincide with the rotation axis of the motor shaft. They are parallel to each other, and the distance between them is 13.9 mm. This design can collect the 3D point cloud with a mark, which can roughly show the position of the motor shaft at the moment a 3D scan is started, that is, the position of the \(Y\)’-axis of coordinate frame \(O\)’-\(X\)’\(Y\)’\(Z\)’. where \(\varphi_{\rm motor}\) is calculated linearly according to the serial number of the corresponding point in all points. The total number of points in the point cloud obtained by a 3D scan of a rotating 2D lidar is denoted as K, the rotation angle of the motor shaft at the \(k\)-th point is:  
\[\varphi_{\rm motor}^{k}=\frac{k}{\rm K}\times 180^{\circ} \tag{7}\]  
Until now, we can get the 3D point cloud relative to coordinate frame \(O^{\prime}\), which has an uncertain attitude error, but no shape error. The calibration of this uncertain attitude error is the next step. Third, we convert point \(\boldsymbol{p}_{O^{\prime}}\) in coordinate frame \(O^{\prime}\) to the corresponding point \(\boldsymbol{p}_{O}\) in coordinate frame \(O\)  
\[\boldsymbol{p}_{O}=\boldsymbol{R}_{O^{\prime}}^{O}\boldsymbol{p}_{O^{\prime}} \tag{8}\]  
where \(R_{O^{\prime}}^{O}\) is the rotation matrix from coordinate frame \(O^{\prime}\) to coordinate frame \(O\).